|
什么是航跡推算 What 航跡推算（Dead Reckoning/DR），也被稱為航位推算，是一種通過已知的初始位置、運動速度、運動方向和時間來推算物體當前位置的方法。 自動駕駛車輛上，常用于航跡推算的傳感器有：輪速編碼器、方向盤、imu(6軸即陀螺儀+加速度計）等； 根據(jù)功能場景/算力需求等，常見的航跡推算方案如下：
為什么用航跡推算 1 提供連續(xù)的位置估計  通過航跡推算，不必依賴外界輸入，保證即使在衛(wèi)星導航信號暫時丟失或不準確的情況下，也能為車輛提供連續(xù)的位置估計，保證自動駕駛系統(tǒng)的正常運行。 2 預測車輛運動趨勢 根據(jù)當前的運動狀態(tài)和歷史數(shù)據(jù)，航跡推算可以預測車輛未來的位置和姿態(tài)，幫助自動駕駛系統(tǒng)進行路徑規(guī)劃、決策制定和避障等操作 3 方便計算 基于全局坐標系下的計算（涉及經(jīng)緯度或大地坐標系）通常數(shù)值巨大或者不易計算，將車輛歷史幀和當前幀轉(zhuǎn)換至dr坐標系通常是更優(yōu)的選擇； 航跡推算方案-1 1、坐標系轉(zhuǎn)換 以t1時刻坐標系為世界系，計算任意時刻相對世界系的姿態(tài)，即可確定任意時刻的里程計； 1、已知初始時刻t1姿態(tài)為單位矩陣I； 2、根據(jù)輪速編碼器，可以確定任意相鄰兩幀之間的姿態(tài)變化Ti,i+1 3、因此可知，Ti+1 = Ti * Ti,i+1 4、最后可知，Ti+1 = T1 * T12 * T23 …… * Ti-1,i * Ti,i+1 由于輪速脈沖編碼器的頻率較高， 通常為50hz或100hz，可以進行離散積分。  2、計算相鄰時刻姿態(tài) 假設(shè)n-1時刻和n時刻，后輪走過的位置如圖所示，則 Δs 即為左右輪的距離差, L為兩個后輪中心線之間的距離；  則可知，n-1時刻到n時刻，Δyaw = ΔS/L；ΔS= 兩個后輪平均移動距離； 以上是近似計算公式：當Δyaw趨近于0時，sin(Δyaw)≈Δyaw 因此幀間位移 delta_X = ΔS * sin(Δyaw)/Δyaw; 因此幀間位移 delta_Y = ΔS *(1-cos(Δyaw))/Δyaw; // 左后右后輪相鄰時刻編碼器計數(shù)變化量uint64_t delta_pulse_rear_left = 4;uint64_t delta_pulse_rear_right = 1;// 單個編碼器計數(shù)對應的距離，使用前需要標定double coeff_per_pulse = 0.022;double delta_S = ( delta_pulse_rear_right + delta_pulse_rear_left)*coeff_per_pulse/2.0// 輪中心間距double wheelBase = 1.62;double delta_yaw = (delta_pulse_rear_left - delta_pulse_rear_right)/wheelbase;/// 計算相鄰兩個時刻旋轉(zhuǎn)變換；roll/pitch視為0；Eigen::Quaterniond q = Eigen::Quaterniond(1, 0.0, 0.0, delta_yaw/2);/// 計算相鄰兩幀平移；double delta_X = delta_S * sin(delta_yaw)/delta_yaw;double delta_Y = delta_S *(1-cos(delta_yaw))/delta_yaw;Eigen::Vector3d delta_trans = Eigen::Vector3d(delta_X, delta_Y, 0);    建議  以上方法不依賴IMU傳感器（比如角速度的獲取）、方向盤角度不參與計算，所以不受轉(zhuǎn)向系統(tǒng)間隙的影響。 因此，最大優(yōu)勢在于易實現(xiàn)，不需要與其他傳感器做時間同步，算力消耗小，適于短距離使用，通常作為后續(xù)方案的觀測使用； 劣勢也同樣明顯，算法默認兩個輪子走過的距離是相同的，且為定值，所以胎壓不均/不足/過足的時候，對于theta的推算有影響，通常在使用輪速計時最好進行在線標定； 更多文章請關(guān)注：SLAM之路 |
|
|
