自动驾驶坐标系与旋转矩阵的确定（位置补偿）

最新推荐文章于 2025-04-29 22:44:29 发布

假装自己很懂

最新推荐文章于 2025-04-29 22:44:29 发布

阅读量810

点赞数 8

文章标签：自动驾驶矩阵人工智能

本文链接：https://blog.csdn.net/m0_46398174/article/details/136257873

版权

本文讲述了在处理新模块时遇到的坐标系统问题，通过确定东北天局部坐标系与车辆坐标系的关系，以及利用旋转矩阵进行坐标转换和位置补偿的过程。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

最近新接受的模块需要做位置补偿，按说是比较简单的。但是因为坐标系的问题导致了诸多麻烦出现，事后复盘如下：

一个未知的坐标系如何确定方向：
此处出现了转换WGS坐标系转东北天，且东北天第一帧初始化局部坐标系。

此时我不清楚到底东北天和所取XYZ有什么关系。选择某个已知的heading角度，最好是正东方向，然后观察坐标变化，我发现正东方向前进时，Y增加，因此Y轴正方向确定，同理可找正北向，发现X增加。此时可以确定程序坐标系为


/*          N(y)
          ^
          |
          |
 W <------O------> E(x)
          |
          |
          S

*/

这时候你还需要去确定你的heading变化方向是顺时针还是逆时针，选择一个中间角度，观察后发现heading角为与正北方夹角得顺时针变化，此时可以得到转换关系。

车辆坐标系下的位置关系转到东北天局部坐标系可以如下计算：
已知局部坐标系下，GPS坐标（x,y）且已知车辆坐标系下GPS与IMU位置差距（-0.4，-1）求IMU（x',y'）.

则可得如下方程:将局部坐标系转车辆坐标系即可有位置关系等式：

R逆（（x,y）-(x',y')）=(-0.4,-1)

(x',y')=(x,y)-R顺（-0.4，-1）

其中R逆转置后变成R顺

转换函数轮子如下: