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

> 原文链接：https://robotics.hellobike.com/pitfalls/2016-02-28-robotics-se-9267
> 频道：pitfalls | 更新日期：2016-02-28 | 标签：auto, stackexchange, algorithm, geometry
> 核心摘要：PixHawk 的 yaw 不可靠；先用 roll/pitch 预旋转激光，再由两束激光的墙面方程求 yaw。

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

::: v-pre
> 本文由 AI 根据公开内容整理，可能有误，请以原文为准：[6D localization with 6 lasers](https://robotics.stackexchange.com/questions/9267/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 授权。
:::

## 原始参考来源 (Sources)

- [Robotics Stack Exchange #9267：6D localization with 6 lasers](https://robotics.stackexchange.com/questions/9267/6d-localization-with-6-lasers)