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

自車座標系

自車座標系，也叫車身座標系，通常有一下兩種常用定義：

座標系–右-前-天座標（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.