自车座标系下的物体相对和绝对位置和速度计算

自车座标系

自车座标系，也叫车身座标系，通常有一下两种常用定义：

座标系–右-前-天座标（RFU）

车辆座标系的参数定义如下：

• X轴：面向车辆前方，右手所指方向

• Y轴：车辆前进方向

• Z轴：与地面垂直，指向车顶方向

车辆参考点原可以为之心中心或者后轴中心，通常取后轴中心。 百度的Apollo项目采用了以上车身座标系。

座标系–前-左-上（FLU)

与上述座标系对应的还有“前-左-上（FLU)：

定义如下

• X轴：车辆前进方向
• Y轴：面向车辆前方，左手所指方向
• Z轴：与地面垂直，指向车顶方向

该座标系为SAEJ182的定义，也是国标（GB/T 19234 ）的定义，因此常被传统车厂采用。

位置换算

这里以自车座标系为原点，自车的x、y、z轴作为大地座标系即绝对座标系的x、y、z轴，则自车座标系与大地座标系重叠，因此他车或者障碍物的绝对座标即为在自车座标系中的相对座标（x，y）。
即
$P(X_a,Y_a) = P(X_r,Y_r)$

速度换算

示意图

情形一

不考虑自车偏航角速度，不考自车虑横向速度，不考虑他车旋转
此种情形较简单，已知他车或障碍物的在自车座标系下的相对速度为
$(V_rx,V_ry)$

由于自车只考虑纵向速度，不考虑横向速度和旋转，自车在大地座标系（注意与自车座标系重合）的速度为
$(V_ex,0)$

因此他车在大地座标系的速度为
$(V_ax,V_ay) = (V_ex+V_rx,V_ry)$

情形二

考虑自车偏航角速度、不考虑他车偏航角速度
已知他车或障碍物的在自车座标系下的座标为
$P(x_a,y_a) = P(x_r,y_r)$

已知他车或障碍物的在自车座标系下的相对速度为
$(V_rx,V_ry)$

自车在大地座标系（注意与自车座标系重合）的速度为
$(V_ex,0)$

偏航角速度(不考虑加速度)为
$w$

根据旋转座标系转换原理，则他车在大地座标系的速度为
$(V_ax,V_ay) = (V_ex+V_rx-wY_r,V_ry+wX_r )$
本情形较为实际，作为计算他车或者障碍物的绝对速度的常用形式。

小结

本文总结了车身座标系中两种常见的座标系参考形式，并根据根据两种情形总结了在他车在本车座标系下相对位置与速度和在绝对座标系下的位置和速度的关系，给出了计算公式。
如果想进一步了解旋转座标系和静止座标系中的位置、速度和加速度关系，可以阅读博文
旋转座标系解读——基于Wikipedia

参考

https://zhuanlan.zhihu.com/p/59743409.