踩坑/Robotics Stack Exchange·2016-02-28

PixHawk 偏航不可靠时如何用 2 个激光求 yaw

PixHawk 的 yaw 不可靠；先用 roll/pitch 预旋转激光，再由两束激光的墙面方程求 yaw。

AI 整理

• 硬件零部件
• 感知与导航

本文由 AI 根据公开内容整理，可能有误，请以原文为准：6D localization with 6 lasers（Robotics Stack Exchange #9267）

状态：已解决

问题

多旋翼在 10m×10m 房间内用 6 个固定激光定位，PixHawk IMU 给出的 yaw 不可靠；已知 roll/pitch、激光原始位置与方向、测距值，需从激光测量恢复 yaw。

环境

10m×10m方形房间；多旋翼；6个固定激光，每轴2个；PixHawk IMU四元数；Python、ROS geometry library、numpy。

解决办法

1. 从 IMU 四元数用 euler_from_quaternion(q, axes='sxyz') 取 roll、pitch，令 yaw 为 0，用 quaternion_from_euler(roll, pitch, 0, axes='sxyz') 和 quaternion_matrix 预旋转激光位置与方向。
2. 将绕 Z 轴的 yaw 作为未知量，写出预旋转后的激光位置和方向关于 yaw 的代数表达式。
3. 用测距外推墙面点：令 k=measure/length(orientation)，PointOnWall = k * orientation + position。
4. 对两束激光建立同一墙面的方程，计算 numerator = -k2*orientation2[0] + k1*orientation1[0] + position1[0] - position2[0]，`denominator = -k2*orientation2[1]…

原问题作者：Alexis Paques；采纳答案作者：Alexis Paques。原内容依 CC BY-SA 4.0 授权，本页摘要同样依 CC BY-SA 4.0 授权。

来源

本页事实对应的公开页面，以原文为准。

1. Robotics Stack Exchange #9267：6D localization with 6 lasers访问 2026-10-22