基本結構
autoware.ai是一個日本開發的自動駕駛開源平臺，是個較爲全面的開放資源,是學習和二次開發的不錯選擇。
autoware的定位模塊ndt_matching,其中融合了gnss衛星信息，IMU慣導信息，odom里程計，lidar激光雷達點雲數據，進行了綜合的定位判斷。在我們的實驗中，只使用GNSS和激光雷達。主要有以下兩個topic的回調控制。
一、Lidar定位
- points_callback:這裏主要做的是各種數據的融合定位，首先會結合IMU和odom數據進行一個基本位置預測，然後就是根據，filtered_points消息傳過來的點雲數據進行矩陣變化，加入到ndt的算法內進行計算得到一個ndt的預測位置。得到了點雲匹配算法加入後的位置，與之前利用IMU和里程計計算的位置進行比較
predict_pose_error = sqrt((ndt_pose.x - predict_pose_for_ndt.x) * (ndt_pose.x - predict_pose_for_ndt.x) +
(ndt_pose.y - predict_pose_for_ndt.y) * (ndt_pose.y - predict_pose_for_ndt.y) +
(ndt_pose.z - predict_pose_for_ndt.z) * (ndt_pose.z - predict_pose_for_ndt.z));
if (predict_pose_error <= PREDICT_POSE_THRESHOLD)//默認值0.5
小於0.5說明和計算匹配值相近，則說明定位沒有跳躍，設當前位置爲點雲計算匹配值，否則使用慣導和IMU物理計算綜合值
點雲過濾voxel_grid_filter
上邊的lidar定位中filtered_points這個topic就是voxel_grid_filter這個節點將接收到的points_raw原始點雲信息，進行體素化處理得到的。
使用體素化網格方法實現下采樣，即減少點的數量，減少點雲數據，並同時保持點雲的形狀特徵，在提高配準、曲面重建、形狀識別等算法速度中非常實用。
PCL實現的VoxelGrid類通過輸入的點雲數據創建一個三維體素柵格（可把體素柵格想象爲微小的空間三維立方體的集合），然後在每個體素（即，三維立方體）內，用體素中所有點的重心來近似顯示體素中其他點，這樣該體素就內所有點就用一個重心點最終表示，對於所有體素處理後得到過濾後的點雲。這種方法比用體素中心來逼近的方法更慢，但它對於採樣點對應曲面的表示更爲準確。
其中可配置的voxel_leaf_size即爲設置濾波器處理時採用的體素大小。
二、GNSS定位
- gnss_callback:接收到gnss_pose消息，進行判斷rosparam參數是否設置使用_use_gnss，init_pos_set是否已經有初始化位置，fitness_score爲points_callback中ndt算法點雲匹配的結果值，
_gnss_reinit_fitness爲靜態值500.
(_use_gnss == 1 && init_pos_set == 0) || fitness_score >= _gnss_reinit_fitness
當滿足以上條件時，則對當前位置進行重新設置，判斷ndt可能存在失準，改用GNSS位置爲當前正確位置。這也是對激光雷達定位的一個輔助，這裏將會重新設置點雲的起始位置，本身點雲匹配就會有累計誤差出現的，加上gnss後使點雲匹配恢復準確度，接下來繼續使用以激光雷達定位爲主的定位方式。
處理GNSS消息
autoware中對gnss進行處理的包在gnss和gnss_localizer,訂閱的topic爲/nmea_sentence。
nmea爲國際標準的衛星定位傳輸信號，可以通過華測等定位設備直接讀取出來。如一個標準的信息爲：
$GPGGA,063201.60,3016.3898531,N,12004.0198533,E,4,19,0.7,6.795,M,7.038,M,1.6,1792*78 $GPRMC,063201.60,A,3016.3898531,N,12004.0198533,E,0.04,190.24,311219,5.6,W,D,C*46
這裏就發送了兩個標準信息GPGGA和GPRMC，其中會有經緯度，高度，衛星質量等數據，我們使用其中的經緯度即可。
在autoware中默認是爲日本的座標系統，demo中也是默認使用了plane爲7的初始經緯度，經過測試我們可以在geo_pos_conv.cpp這個類中。使用我們當前位置的經緯度即可。如
lon_deg = 30;
lon_min = 16;
lat_deg = 120;
lat_min = 4;
三、座標轉換
我們知道每個物體都有基於自己的座標系，如雷達必須是以車身爲基準的，但是車身位置又要在map中展示，而map的座標和gps的座標因爲初始點的不同又可能存在偏差，那麼座標轉換就是爲了將這多個物體統一顯示在一張圖中而做的。
- base_link->velodyne,也就是激光雷達距離車身後軸的相對位置如：
- map->base_link,這個是ndt算法內實時計算的，車在map座標系中的位置。
- map->gps,gps爲上邊gnss計算出來的實際位置，gps在map座標系中的位置。
- world->map。爲什麼加一個world到map呢，主要是因爲，底下有一個地圖pcd就是白色的點雲，它的位置要和gps位置配準，將兩個顯示在中間，這樣點雲計算的值才能和gps基本一致。而這裏設置的world到map的值爲gnss起始點計算出來的值的相反，因爲我們在製作底圖pcd的時候已經將其從原本的（0，0）點移到了gps計算出來的初始點。這樣在world座標系下底圖，點雲，gps將會有同一個起始點。
四、實際效果
從數據發出，到RVIZ中顯示，首先由GNSS信息確定一個初始點，然後使用點雲匹配得到精確位置，隨着車輛前進，不斷計算gnss的值和點雲匹配的值，若出現點雲不精確的情況，立即使用gnss重新定位初始點，一直重複。