[發明專利]一種涉及動力輪打滑修正的多傳感器融合載具定位方法

有效

• 申請號: 202110402442.9
• 申請日: 2021-04-14
• 公開（公告）號: CN113252321B
• 公開（公告）日: 2022-01-11
• 發明（設計）人: 劉劍;許一航;薛磊;鄭昊
• 申請（專利權）人: 南京云智控產業技術研究院有限公司
• 主分類號: G01M13/00
• 分類號: G01M13/00;G01P3/54;G01S11/02
• 代理公司: 南京眾聯專利代理有限公司 32206
• 代理人: 許小莉

1.一種涉及動力輪打滑修正的多傳感器融合載具定位方法，其特征在于，該方法包括如下步驟：

S1.前饋控制：使用前饋PID算法對每個電機的轉速做閉環控制，基于編碼器數據獲得三個動力輪的轉速ω₁、ω₂、ω₃，根據3個動力輪的尺寸得到相應的動力輪的相對移速；

S2.超寬帶定位：基于UWB定位系統獲得全向輪載具的絕對位置坐標(x_UWB,y_UWB)；

S3.打滑修正：根據電機負載狀態估計出每個動力輪的相對移速修正值ν_{X_corr}，X＝1,2,3，基于模型解算得到位置大地坐標系下載具的橫縱移速以及航向轉速v_{x_car}、v_{y_car}、ω_car基于狀態方程解出大地坐標系下載具的橫縱坐標以及航向角度x_car、y_car、θ_car；

S4.數據融合：使用卡爾曼濾波對上述兩種位置數據進行融合，得到融合結果；

步驟S1中所述前饋控制的具體方法是：

S11.在全向輪載具的三個動力輪的電機軸上安裝徑向磁鐵，使用正交霍爾傳感器檢測徑向磁鐵產生的脈沖，根據該脈沖頻率計算徑向磁鐵轉速，也即是電機轉速；

S22.根據步驟S11中得到的電機轉速得到動力輪外廓行進的速度，具體公式如下：

v_X＝k_rDω_X，X＝1,2,3 (1)

其中v_X，X＝1,2,3為動力輪的相對移速，ω_X，X＝1,2,3為動力輪電機轉速，k_r為電機減速比，D為動力輪的直徑；

步驟S3中所述打滑修正的具體方法是：

S31.計算轉速誤差：設計前饋加PID電機轉速控制算法，得到在前饋控制下控制的轉速誤差ω_err，具體公式如下：

ω_err＝ω_exp-ω_out (2)

其中ω_exp為期望轉速，ω_out為實際轉速，ω_err為在前饋控制下控制轉速的誤差；

S32.根據步驟S31得到的轉速誤差計算轉矩誤差T_err：

轉矩誤差T_err與轉速誤差ω_err近似線性相關，將轉速誤差ω_err經過低通濾波后得到的ω_{err_LPF}消除了噪聲影響，此時通過近似線性公式計算出轉矩誤差T_err，具體公式如下：

T_err＝k_wω_{err_LPF} (3)

其中k_w為線性化系數，轉矩誤差T_err即為電機負載偏離正常情況的數值；

S33.根據步驟S32中得到的轉矩誤差T_err，對電機的運行狀態做分類并進行修正：

當差值的絕對值小于等于正常波動閾值T_thres時，記電機處于狀態1；

當差值的絕對值大于正常波動閾值閾值T_thres且差值為正時，記電機處于狀態2；

當差值的絕對值大于正常波動閾值閾值T_thres且差值為負時，記電機處于狀態3；

對于狀態1，電機工作正常，未出現打滑現象，編碼器反饋的轉動距離與電機行駛距離一致，此時直接給出修正值：

ω_{X_corr}＝ω_X，X＝1,2,3 (4)

對于狀態2，電機負載小于額定值，意味著電機與地面接觸不良，可能出現了打滑，此時將電機轉速設為ω_X和0經過式(1)得到輪子外廓相對移速后分別代入式(5)計算估計下次的坐標得到兩組坐標(x_ω,y_ω)、(x₀,y₀)：

其中θ為機體坐標系與大地坐標系的偏角，x_t、y_t為當前時刻的橫、縱坐標，x_t+1、y_t+1為下一時刻的橫、縱坐標，t_Δ為兩次計算時間的間隔，將電機轉速設為ω_X代入計算得到的結果x_t+1、y_t+1分別記為x_ω、y_ω，將電機轉速設為0代入計算得到的結果x_t+1、y_t+1分別記為x₀、y₀，

與下時刻超寬帶定位返回的絕對位置坐標(x_UWB，y_UWB)分別計算其歐氏距離d_ω、d₀，公式如下：

此時修正值為

對于狀態3，電機負載大于額定值，意味著電機重載，此時可能行駛到阻力較大區域，也可能是碰到障礙而無法繼續行進，此時將電機轉速設為ω_X和0經過式(1)得到輪子外廓相對移速后分別帶入式(5)計算估計下次的坐標得到(x_ω,y_ω)、(x₀,y₀)，與下時刻超寬帶定位返回的位置分別計算其歐氏距離d_ω、d₀，此時修正值參照式(7)得到；

步驟S4中所述數據融合的具體方法是：

S41.使用步驟S3中獲得的電機轉速修正值ω_{X_corr}，X＝1,2,3，根據式(1)計算動力輪外廓行進速度修正值v_{X_corr}，X＝1,2,3,得到三個動力輪外廓行進速度后，對其進行變換，得到機體直角坐標系下的速度分量：

其中[v₁ v₂ v₃]^T表示三個動力輪外廓行進速度修正值，r為載具動力輪到載具中心的距離，[v_x v_y ω_z]^T中三個元素分別為載具機體直角坐標系下的橫、縱速度分量以及航向軸的轉速；

S42.再通過步驟S41得到的機體直角坐標系下的速度分量獲得大地坐標系下的速度分量：

其中θ為機體坐標系與大地坐標系的偏角，也即是載具的航向角，[v'_x v'_y ω'_z]^T中三個元素分別為大地直角坐標系下的橫、縱速度分量以及航向軸的轉速，其中M^T表示矩陣M的轉置；

由式(8)、式(9)可得：

S43.使用狀態方程實現對當前狀態的計算，即式(11)：

其中為t-1時刻卡爾曼濾波后的位置；

S44.載具的平面坐標使用卡爾曼濾波方式融合UWB定位數據和編碼器數據，迭代計算公式如下：

式(12.1)為基于過去時刻狀態及控制量對當前時刻的狀態進行估計，其中

為t時刻的預測值，為t-1時刻的卡爾曼估計值；

式(12.2)計算出式(12.1)先驗估計的協方差矩陣，其中為t時刻估計值的協方差矩陣，P_t-1為t-1時刻濾波輸出值的協方差矩陣，Q為式(12.1)計算引入誤差的協方差矩陣，A^T表示矩陣A的轉置；

式(12.3)計算出卡爾曼增益K_t，H為UWB定位數據轉換到編碼器數據轉換矩陣，R為UWB定位數據誤差的協方差矩陣，H^T表示UWB定位數據轉換到編碼器數據轉換矩陣H的轉置；

式(12.4)根據卡爾曼增益計算位置的最優估計數值，Z_t為t時刻UWB定位數據，為為t時刻的卡爾曼估計值；

式(12.5)計算得到式(12.4)t時刻濾波輸出值的協方差矩陣P_t；

根據全向輪載具實際狀態，針對控制器代碼優化，其迭代格式可簡化表示為

其中為t時刻卡爾曼濾波的位置估計結果。

2.根據權利要求1所述的涉及動力輪打滑修正的多傳感器融合載具定位方法，其特征在于，步驟S2中所述超寬帶定位的具體方法是：在全向輪載具上掛載的超寬帶標簽與周邊的基站通訊并測距，通過最小二乘估計獲得全向輪載具的絕對位置坐標(x_UWB,y_UWB)。