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

自车坐标系与绝对坐标系的速度位置换算原理

• 自车坐标系
• • 坐标系--右-前-天坐标（RFU）
  • 坐标系--前-左-上（FLU)
• 位置换算
• 速度换算
• • 示意图
  • 情形一
  • 情形二
• 小结
• 参考

自车坐标系

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

坐标系–右-前-天坐标（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 ) P(X_a,Y_a) = P(X_r,Y_r) P(Xa​,Ya​)=P(Xr​,Yr​)

速度换算

示意图

情形一

不考虑自车偏航角速度，不考自车虑横向速度，不考虑他车旋转

此种情形较简单，已知他车或障碍物的在自车坐标系下的相对速度为

( V r x , V r y ) (V_rx,V_ry) (Vr​x,Vr​y)

由于自车只考虑纵向速度，不考虑横向速度和旋转，自车在大地坐标系（注意与自车坐标系重合）的速度为

( V e x , 0 ) (V_ex,0) (Ve​x,0)

因此他车在大地坐标系的速度为

( V a x , V a y ) = ( V e x + V r x , V r y ) (V_ax,V_ay) = (V_ex+V_rx,V_ry) (Va​x,Va​y)=(Ve​x+Vr​x,Vr​y)

情形二

考虑自车偏航角速度、不考虑他车偏航角速度

已知他车或障碍物的在自车坐标系下的坐标为

P ( x a , y a ) = P ( x r , y r ) P(x_a,y_a) = P(x_r,y_r) P(xa​,ya​)=P(xr​,yr​)

已知他车或障碍物的在自车坐标系下的相对速度为

( V r x , V r y ) (V_rx,V_ry) (Vr​x,Vr​y)

自车在大地坐标系（注意与自车坐标系重合）的速度为

( V e x , 0 ) (V_ex,0) (Ve​x,0)

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

w w w

根据旋转坐标系转换原理，则他车在大地坐标系的速度为

( V a x , V a y ) = ( V e x + V r x − w Y r , V r y + w X r ) (V_ax,V_ay) = (V_ex+V_rx-wY_r,V_ry+wX_r ) (Va​x,Va​y)=(Ve​x+Vr​x−wYr​,Vr​y+wXr​)

本情形较为实际，作为计算他车或者障碍物的绝对速度的常用形式。

小结

本文总结了车身坐标系中两种常见的坐标系参考形式，并根据根据两种情形总结了在他车在本车坐标系下相对位置与速度和在绝对坐标系下的位置和速度的关系，给出了计算公式。

如果想进一步了解旋转坐标系和静止坐标系中的位置、速度和加速度关系，可以阅读博文

旋转坐标系解读——基于Wikipedia

参考

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