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

有效

• 申請?zhí)枺?/td>: 202110402442.9
• 申請日: 2021-04-14
• 公開（公告）號: CN113252321B
• 公開（公告）日: 2022-01-11
• 發(fā)明（設(shè)計(jì)）人: 劉劍;許一航;薛磊;鄭昊
• 申請（專利權(quán)）人: 南京云智控產(chǎn)業(yè)技術(shù)研究院有限公司
• 主分類號: G01M13/00
• 分類號: G01M13/00;G01P3/54;G01S11/02
• 代理公司: 南京眾聯(lián)專利代理有限公司 32206
• 代理人: 許小莉

本發(fā)明公開了一種涉及動(dòng)力輪打滑修正的多傳感器融合載具定位方法，該方法包括如下步驟：S1.前饋控制：使用前饋PID算法對每個(gè)電機(jī)的轉(zhuǎn)速做閉環(huán)控制，基于編碼器數(shù)據(jù)獲得三個(gè)動(dòng)力輪的轉(zhuǎn)速，根據(jù)3個(gè)動(dòng)力輪的尺寸得到相應(yīng)的動(dòng)力輪的相對移速；S2.超寬帶定位：基于UWB定位系統(tǒng)獲得全向輪載具的絕對位置坐標(biāo)；S3.打滑修正：根據(jù)電機(jī)負(fù)載狀態(tài)估計(jì)出每個(gè)動(dòng)力輪的相對移速修正值，基于模型解算得到位置，基于狀態(tài)方程解出x_car、y_car、θ_car；S4.數(shù)據(jù)融合：使用卡爾曼濾波對上述兩種位置數(shù)據(jù)進(jìn)行融合，得到融合結(jié)果x_KF、y_KF、θ_KF。本發(fā)明可有效提高定位數(shù)據(jù)融合的精度。

技術(shù)領(lǐng)域

本發(fā)明屬于定位算法領(lǐng)域，具體涉及一種涉及動(dòng)力輪打滑修正的多傳感器融合載具定位方法。

背景技術(shù)

移動(dòng)載具的定位技術(shù)是無人車、無人機(jī)以及復(fù)雜系統(tǒng)編隊(duì)控制中的關(guān)鍵技術(shù)。移動(dòng)載具實(shí)現(xiàn)軌跡跟蹤、路徑規(guī)劃、編隊(duì)控制等應(yīng)用需要依賴載具的準(zhǔn)確位置信息。

基于電機(jī)編碼器和陀螺儀數(shù)據(jù)可以通過輪胎轉(zhuǎn)速和載具加速度估計(jì)出全向輪載具的當(dāng)前的速度與方向，通過積分可以得到載具的位移，短時(shí)定位精度可達(dá)到毫米級。但是由于分辨率的限制和量測噪聲的存在，位移數(shù)據(jù)在長時(shí)間的計(jì)算中會(huì)產(chǎn)生漂移誤差，影響定位的準(zhǔn)確度。

基于超寬帶(UWB)的定位方案通過測量脈沖信號的傳輸時(shí)間來獲取標(biāo)簽與基站的直線距離。通過一系列幾何計(jì)算獲得標(biāo)簽相對基站的位置。這種方法具有定位數(shù)據(jù)不漂移的優(yōu)點(diǎn)。現(xiàn)有技術(shù)可以達(dá)到1kHz的一對一收發(fā)測距，但一般的定位往往存在多個(gè)基站與標(biāo)簽。這會(huì)導(dǎo)致數(shù)據(jù)更新速度的下降，實(shí)測使用Decawave公司的DWM1000開發(fā)的4基站4標(biāo)簽的定位系統(tǒng)的數(shù)據(jù)刷新速度為10Hz。此外，UWB定位的精度為分米級，在進(jìn)行高速、精密的集群定位時(shí)，若完全依賴則精度難以保證。

發(fā)明內(nèi)容

發(fā)明目的：針對上述存在的問題，本發(fā)明提出了一種涉及動(dòng)力輪打滑修正的多傳感器融合載具定位方法，將上述兩種測量方式有效融合，提升總體定位精度，在數(shù)據(jù)融合的同時(shí)完成了對載具打滑擾動(dòng)的估計(jì)，從而補(bǔ)償打滑對定位數(shù)據(jù)的擾動(dòng)。有效提高定位數(shù)據(jù)融合的精度。

上述的目的通過以下技術(shù)方案實(shí)現(xiàn)：

一種涉及動(dòng)力輪打滑修正的多傳感器融合載具定位方法，該方法包括如下步驟：

S1.前饋控制：使用前饋PID算法對每個(gè)電機(jī)的轉(zhuǎn)速做閉環(huán)控制，基于編碼器數(shù)據(jù)獲得三個(gè)動(dòng)力輪的轉(zhuǎn)速ω₁、ω₂、ω₃，根據(jù)3個(gè)動(dòng)力輪的尺寸得到相應(yīng)的動(dòng)力輪的相對移速；

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

S3.打滑修正：根據(jù)電機(jī)負(fù)載狀態(tài)估計(jì)出每個(gè)動(dòng)力輪的相對移速修正值ν_{X_corr}，(X＝1,2,3)，基于模型解算得到位置大地坐標(biāo)系下載具的橫縱移速以及航向轉(zhuǎn)速v_{x_car}、v_{y_car}、ω_car基于狀態(tài)方程解出大地坐標(biāo)系下載具的橫縱坐標(biāo)以及航向角度x_car、y_car、θ_car；

S4.數(shù)據(jù)融合：使用卡爾曼濾波對上述兩種位置數(shù)據(jù)進(jìn)行融合，得到融合結(jié)果x_KF、y_KF、θ_KF。

所述的涉及動(dòng)力輪打滑修正的多傳感器融合載具定位方法，步驟S1中所述前饋控制的具體方法是：

S11.在全向輪載具的三個(gè)動(dòng)力輪的電機(jī)軸上安裝徑向磁鐵，使用正交霍爾傳感器檢測徑向磁鐵產(chǎn)生的脈沖，根據(jù)該脈沖頻率計(jì)算徑向磁鐵轉(zhuǎn)速，也即是電機(jī)轉(zhuǎn)速；