目标跟踪中的坐标变换问题以及四元数

最新推荐文章于 2025-05-18 13:47:48 发布

shaozhenghan

最新推荐文章于 2025-05-18 13:47:48 发布

阅读量2.2k

点赞数 1

CC 4.0 BY-SA版权

分类专栏：目标跟踪卡尔曼滤波

本文链接：https://blog.csdn.net/shaozhenghan/article/details/88088192

在自动驾驶的目标跟踪问题中，自车的坐标系随着自车运动，包括yaw angle。因此，在跟踪时，需要这样做：车身偏航角的变化率是可以得到的yaw_rate，在每次触发跟踪算法时根据两帧时间戳的差值对yaw_rate做积分，就有这个时间段内产生的偏航角变化，把这个值算到卡尔曼的估计中，再和测量值做加权。也就是说，卡尔曼的预测过程=基于目标运动模型+自车坐标系变化引起的坐标变换（旋转）.

可以这样理解，就是自车偏转时的跟踪，相当于自车不动，而将目标状态根据yaw angle 进行坐标变换。这样目标跟踪就始终处于自车坐标系下了。

在多目标的跟踪中，分为三个步骤，1. 预测（包括自车yaw angle 的补偿），2. 匹配，即多个测量点和对象列表中的多个ID一一对应（Apollo中用的是匈牙利算法）。3. 测量更新

由于欧拉角有万向节死锁的缺点，自动驾驶中常用 四元数，有关四元数的简洁讲解在下面的链接中：
链接：http://www.zhihu.com/question/23005815/answer/33971127

附上原文：

作者：Yang Eninala
链接：https://www.zhihu.com/question/23005815/answer/33971127
来源：知乎
著作权归作者所有。商业转载请联系作者获得授权，非商业转载请注明出处。

根据我的理解，大多数人用汉密尔顿四元数就只是做三维空间的旋转变换（我反正没见过其他用法）。那么你不用学群论，甚至不用复习线性代数，看我下面的几张图就可以了。

首先，定义一个你需要做的旋转。旋转轴为向量 v=(vx,vy,vz) ，旋转角度为 $\theta$ （右手法则的旋转）。如下图所示：
此图中 $v=(\frac{1}{\sqrt{14} } ,\frac{2}{\sqrt{14} } ,\frac{3}{\sqrt{14} })$ , $\theta =\frac{\pi }{3}$