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