自車坐标系下的物體相對和絕對位置和速度計算

自車坐标系與絕對坐标系的速度位置換算原理

• 自車坐标系
• • 坐标系--右-前-天坐标（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.