Prescan之車輛座標系的計算

simulink得到的感測器資料都是基於prescan座標系的，而實際場景中的感測器資料都是基於車輛座標系的，故此處主要對prescan座標系與車輛座標系進行簡單描述，並給出兩個座標系之間的關係。

prescan座標系相當於大地座標系，所有資料資訊(包括車輛)都是基於此座標系輸出的，各座標軸指向：x = right( east ), y = back( north ), z = up；

車輛座標系是以車輛後軸中心為原點，以車輛行駛方向為基準，給出各座標軸指向：x = front(執行方向)，y = left, z = up.

注：車輛的cog offset引數要設定為(0, 0, 0)。

圖1 兩個座標系的軸向解釋

車輛在prescan中的方向性，主要有兩個：(1) 車輛繞prescan座標系的x,y,z軸的旋轉； (2) 車輛的指南針航向。兩者的具體解釋可見self port埠介紹。車輛在2d平面的旋轉主要與prescan座標系的z軸相關，故此處給出rotz與heading的座標角度：

圖2 rotz與heading的座標角度

prescan座標系與車輛座標系之間的對映矩陣，是車輛座標系繞x,y,z軸的旋轉與平移，且兩個座標系之間的旋轉不止是2d平面而是3d空間，故應該選用方式(1)才能實現prescan座標系-->車輛座標系的轉換。

第2部分已經解釋車輛在prescan座標系的轉向及角度關係，此處直接給出兩者在2d平面的對映關係。

圖3 車輛原點在車輛座標系中的位置

將以上公式用向量表示，並得到繞x,y軸的旋轉矩陣，即可計算車輛原點在車輛座標系的幾何位置。

車輛在prescan座標系的旋轉順序為z,y,x; 故兩個座標系的旋轉矩陣r=rotx*roty*rotz。

通過以上步驟即可得到車輛原點從prescan座標系-->車輛座標系的位置。