坐標系

1.判定坐標系：大拇指指向 Z 軸，看四指環繞方向，如果是 X 指向 Y，就是右手系。
 右手大拇指指向z軸方向，其余四指由x軸握向y軸方向，如果成功，那么判定為右手系。
 左手大拇指指向z軸方向，其余四指由x軸握向y軸方向，如果成功，那么判定為左手系。
 
 左手系無法通過三軸旋轉變成右手系，只能通過換軸的方法轉換
 
2.旋轉正方向的判斷

歐拉角是一種表示姿態（物體相對于另一個坐標系的方向和方位）的三個角度的集合，
   而姿態則描述了物體在空間中的方向，可以通過歐拉角、方向余弦矩陣、四元數等多種方式表示，
   其中歐拉角是其中一種直觀但存在奇異點（例如在俯仰角接近90度時）的表示方法

坐標系-Coordinate

時間和空間
1.map坐標系 ：
     絕對坐標表示車體相對于地球或者相對于地圖的絕對位置和姿態（位姿）
	 世界坐標系 經緯度，  緯度、 經度和高度
	 橫軸墨卡托（The Universal Transverse Mercator， UTM） 坐標 通過投影將坐標位置投影至地面， 通常稱之為局部坐標系或者大地坐標系
	    投影后采取地面上一點作為參考點， 即局部坐標系原點。 
		通常采用東北天（East-North-Up, ENU） 表達坐標系方向， 即 X 軸在當地水平面內分別指向東， Y 軸指向北， Z 軸指向天
	    東北天局部坐標系采用三維直角坐標系來描述地球表面
	 
2.車體坐標系  vehicle  car  ego 
  智能網聯汽車主要關注車輛前方， 通常的定義中都是以車輛前方為正方向
    車輛坐標原點:是以汽車簧載質量上的點為原點 Origin
    X 軸平行于汽車縱向對稱平面且其正方向水平向前，
	Y 軸垂直于汽車縱向對稱平面且其正方向指向汽車左側，
	Z軸豎直朝上
  	
	GB/T 12673-2019 車輛后方為 X 軸， Y 軸向右， Z 軸朝上
3.傳感器坐標系
    車載傳感器相對于車體的位姿以及目標物相對車體的位姿WYT@ca1c293 
	   激光雷達-毫米波雷達-相機-超聲波
	   IMU及組合導航
	   V2X
    相機：相機坐標系， 還有圖像坐標系， 圖像坐標是 2D 的坐標， 而相機坐標是 3D 坐標， 由相機坐標映射到圖像坐標的關系通常被稱為相機內參
	V2X： 加速度其中方向定義為“前/右/下” 
	
NED（n系北東地 – b系前右下）、ENU（n系東北天 – b系右前上）的姿態角（歐拉角）與姿態旋轉矩陣的關	
  導航方位角（Bearing）：從北方向開始，順時針旋轉為正（即北偏東為正）。例如，北為0°，東為90°，南為180°，西為270°。
  
  
  車體坐標系中，yaw角向左橫擺為正。注意，這里車體坐標系的yaw角是繞車體Z軸旋轉，向左轉為正，即逆時針方向

坐標系標準

ISO 8855: 2011
平臺慣性制導系統
捷聯慣性制導系統 
平臺慣性制導系統的精度要高于捷聯慣性制導系統     
捷聯慣導系統（Strapdown Inertial Navigation System，SINS）是一種自主式導航系統，由慣性測量單元和導航計算機構成   
   載體系在導航系中的姿態
   捷聯制導將陀螺儀直接連接載體，利用姿態變化測量載體運動，而平臺制導通過反向力矩保持穩定
   
捷聯式慣導的核心差異在于?取消了實體機電平臺?，通過數學算法實現慣性測量單元（IMU）與載體坐標系的實時解耦   
機電式陀螺儀和光電類陀螺儀兩大類  MEMS陀螺儀


IMU 坐標系（body frame，簡稱 b 系），
導航坐標系（navigation frame，簡稱 n 系）
載體坐標系（vechile frame，簡稱 v 系），


ROS 坐標系  Apollo 坐標系系統
   ENU 表達 East, North, Up, 中文通常表達為東北天。 ENU 表示東向為 x 軸， 北向為 y 軸， 天向為 z 軸
組合導航的重要功能是輸出車輛在世界坐標系下的位置和姿態
車體坐標系常見定義
名稱 	車輛前方 	車輛左側 	車輛上方 	參考系統
前左上 	x 軸正方向 	y 軸正方向 	z 軸正方向 	ROS，GB/T 40429-2021
右前上 	y 軸正方向 	x 軸負方向 	z 軸正方向 	Apollo	

華測 HCG-630

  組合導航設備輸出數據要求
  【強制】GPSWeek： 自 1980/1/6 至當前星期數(格林尼治時間)；
  【強制】GPSTime：本周日 0:00:00 至當前的秒數；
  【強制】Latitude：緯度（-90° 至 90°）；
  【強制】Longitude：經度（-180° 至 180°）；
  【強制】Altitude：高度，單位（米）；
  【強制】Heading（Yaw）： 航向角，0 至 359.99，北偏東順時針為正（如果不是需自行轉換）（IMU 原始值和校正值至少輸出一個）；
  【強制】Pitch：俯仰角（-90 至 90)，車頭上揚為正 （IMU 原始值和校正值至少輸出一個）；
  【強制】Roll：橫滾角（-180 至 180），車身右傾為正（IMU 原始值和校正值至少輸出一個）；
  【強制】Gyro X/Y/Z：陀螺儀 X 、Y、Z 軸，單位 °/s；
  【強制】Acc X/Y/Z：加速度 X、Y、Z 軸 （IMU 原始值和校正值至少輸出一個）；
  【強制】Ve、Vn、Vu：東向、北向、天向速度，單位 m/s；
  【強制】SolutionStatus: 系統狀態，參考 Solution Status 定義，至少能區分是否是組合導航；
  【強制】PositionType：衛星狀態，參考 Position or Velocity Type， 至少包含不定向、RTK浮點解、穩定解；
IMU全稱inertial measurement unit，即慣性測量單元		
航向角， 角度范圍   0° ～360°  
俯仰角， 角度范圍   -90° ～90° ；  故在俯仰角較大的場合下不適合采用歐拉角法
橫滾角， 角度范圍  -180° ～180°

   華測坐標系定義為 X軸向右、Y軸向前、Z軸向上
Novatel坐標系定義為 X軸向右、Y軸向前、Z軸向上
   parser 中負責各種廠商 gnss 數據解析，新慣導接入需要新增一個 parser 類型
 
全局位姿變換關系，這也是定位模塊localization主要的任務

旋轉

 描述坐標系{B}相對于參考坐標系{A}的姿態有兩種方式。繞固定坐標系旋轉 繞自身坐標軸旋轉
一種是 繞固定（參考）坐標軸旋轉：
   假設開始兩個坐標系重合，先將{B}繞{A}的X軸旋轉γ，然后繞{A}的Y軸旋轉β，最后繞{A}的Z軸旋轉α，就能旋轉到當前姿態。
   可以稱其為X-Y-Z fixed angles 或RPY角(Roll, Pitch, Yaw)	
一種是 繞自身坐標軸旋轉
  假設開始兩個坐標系重合，先將{B}繞自身的Z軸旋轉α，然后繞Y軸旋轉β，最后繞X軸旋轉γ，就能旋轉到當前姿態。
  稱其為Z-Y-X歐拉角，由于是繞自身坐標軸進行旋轉

enum RotSeq{zyx, zyz, zxy, zxz, yxz, yxy, yzx, yzy, xyz, xyx, xzy,xzx};
  如ADAMS軟件里就默認 Body 3-1-3次序，即Z-X-Z歐拉角，而VREP中則按照X-Y-Z歐拉角旋轉。


  
靜態坐標變換是指兩坐標系之間的變換關系是固定的一種坐標變換，
   常用于表示汽車上兩位置固定坐標系之間的變換關系，例如各個傳感器之間的坐標變換。  

數據

src/novatel_oem7_msgs/msg/INSPVA.msg 
    float64 roll
    float64 pitch
    float64 azimuth  heading yaw 

?諾瓦泰 Novatel
? 和芯?通 Unicorecomm
? 天寶 Trimble
? 萊卡 Leica

RTK+IMU 
   RTK 接收機的成本從小幾萬到了兩三百 +  RTK 的服務費
         RTK-天線 小口徑的航空天線
      	 IMU 對于震動\溫度 敏感
高精度地圖			 

代碼

tf::Transform transform;//這里transform為一個類  (in radians)
transform.setRPY(roll,pitch,yaw)
z90_deg_rotaion.setRPY(0.0, 0.0, pi/2);//setRPY是類Quaternion的一個成員函數 (in radians)

setRPY (const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
    Set the quaternion using fixed axis RPY.
	
tf::Transform ros_ori = z90_deg_rotaion * transform	

ROS 中關于歐拉角旋轉的函數:setRPY、setEuler、setEulerZYX(
  歐拉角可以分為繞定軸和繞動軸的變換方式，
  當沒有指定Euler時，    默認是按定軸轉動，矩陣依次左乘。
  若函數的名字中有Euler，則表示為繞動軸轉動的方式，矩陣依次右乘
 setRPY();
    這個函數采用固定軸的旋轉方式，先繞定軸x旋轉（橫滾），然后再繞定軸y（俯仰），最后繞定軸z（偏航）
	 R = R(z)R(y)R(x)的順
  setEuler
     這種方式是繞著動軸轉動，先繞Y軸，在繞變換后的X軸，再繞變換后的Z軸旋轉	這是繞定軸YXZ矩陣依次右乘，
	 即：R = R(y)R(x)R(z) 的順序
  setEulerZYX
    與2相同，此種旋轉變換也是繞動軸旋轉，只不過次序為ZYX，矩陣也是右乘，即R = R(z) R(y)R(x)

代碼二

from scipy.spatial.transform import Rotation as R
Rotation.from_euler()時需特別注意歐拉角的旋轉順序和內旋/外旋的區別：

坐標系定義

? ENU（東北天）?：

X軸指向東，Y軸指向北，Z軸指向天
繞X軸旋轉為pitch角，繞Y軸為roll角，繞Z軸為yaw角
歐拉角旋轉順序：Z-X-Y ?

? NED（北東地）?：

X軸指向北，Y軸指向東，Z軸指向地
繞X軸旋轉為roll角，繞Y軸為pitch角，繞Z軸為yaw角
歐拉角旋轉順序：Z-Y-X ?
Rotation.from_euler()參數說明

? seq?：指定旋轉軸順序，取值范圍為'X'/'Y'/'Z'（內旋）或'x'/'y'/'z'（外旋）
? angles?：旋轉角度，需與旋轉順序匹配（如Z-X-Y對應yaw、pitch、roll）

developer.apollo.auto/Apollo-Homepage-Document/Apollo_Doc_CN_6_0/數據格式/定位模塊/apollo_localization_pose

MSF定位模塊  多傳感器融合 定位模塊利用來自 GPS,LiDAR 和 IMU 設備的信息來評估自動駕駛車輛的位置 fusion_status	
RTK 定位

 ENU坐標系下的x軸姿態值（四元數）
 euler_angles	ENU坐標系下的姿態（歐拉角）

 https://developer.apollo.auto/Apollo-Homepage-Document/Apollo_Doc_CN_6_0/%E6%95%B0%E6%8D%AE%E6%A0%BC%E5%BC%8F/%E5%AE%9A%E4%BD%8D%E6%A8%A1%E5%9D%97/apollo_localization_pose/	 
 將 GPS 和 IMU 數據組合成一個完整的定位消息：
     整合來自 GPS 和 IMU 的定位信息，構建出一個包含位置信息、方向、線速度、線加速度、角速度和歐拉角的完整定位消息	
https://github.com/ApolloAuto/apollo/blob/master/docs/05_Localization/coordination_cn.md	

參考

https://docs.ros.org/en/melodic/api/novatel_msgs/html/msg/INSPVAX.html
https://github.com/novatel/novatel_oem7_driver/blob/humble/src/novatel_oem7_msgs/msg/INSPVA.msg