[發(fā)明專利]一種基于視覺和慣性導(dǎo)航組合的移動機(jī)器人定位方法

在審

• 申請?zhí)枺?/td>: 202010475627.8
• 申請日: 2020-05-29
• 公開（公告）號: CN111637892A
• 公開（公告）日: 2020-09-08
• 發(fā)明（設(shè)計）人: 于惠鈞;龔星宇;劉曉燕
• 申請（專利權(quán)）人: 湖南工業(yè)大學(xué)
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16;G01C21/18
• 代理公司: 廣州粵高專利商標(biāo)代理有限公司 44102
• 代理人: 楊千尋;馮振寧

1.一種基于視覺和慣性導(dǎo)航組合的移動機(jī)器人定位方法，其特征在于，包含以下步驟：

S1：利用視覺傳感器獲取移動機(jī)器人所在的位置：建立外參數(shù)模型，外參數(shù)將相機(jī)坐標(biāo)系與世界坐標(biāo)系對應(yīng)；

S2：建立慣性導(dǎo)航系統(tǒng)，利用視覺傳感器進(jìn)行速度、方向和重力測量及報告；

S2.1：利用互補(bǔ)濾波法在慣性導(dǎo)航系統(tǒng)中進(jìn)行姿態(tài)角解算：對慣性導(dǎo)航坐標(biāo)體系與世界坐標(biāo)系構(gòu)建方向余弦矩陣，然后求導(dǎo)并對微分方程進(jìn)行計算，最終得到姿態(tài)角的值；

S2.2：慣性導(dǎo)航航位推算：將慣性導(dǎo)航坐標(biāo)系下的加速度轉(zhuǎn)換到世界坐標(biāo)系，將轉(zhuǎn)換后的加速度扣除重力加速度后單次積分得到速度，二次積分得到位移；

S2.3：對測量的加速度和角速度分別進(jìn)行慣性導(dǎo)航誤差分析處理；

S3：利用視覺傳感器采集的信息與SLAM系統(tǒng)構(gòu)建的云地圖轉(zhuǎn)換為柵格地圖后進(jìn)行路徑規(guī)劃。

2.根據(jù)權(quán)利要求1所述的移動機(jī)器人定位方法，其特征在于，所述外參數(shù)模型為：

式中，(x_c,y_c,z_c)代表相機(jī)坐標(biāo)，(x_w,y_w,z_w)代表世界坐標(biāo)，R代表旋轉(zhuǎn)矩陣，t代表平移矩陣，m代表外參數(shù)的矩陣。

3.根據(jù)權(quán)利要求1所述的移動機(jī)器人定位方法，其特征在于，所述慣性導(dǎo)航系統(tǒng)采用MPU6050傳感器來測量坐標(biāo)系三軸上的角速度與加速度，所述MPU6050傳感器包含三軸陀螺儀與三軸加速度計。

4.根據(jù)權(quán)利要求1所述的移動機(jī)器人定位方法，其特征在于，所述慣性導(dǎo)航航位推算中慣性導(dǎo)航的加速度a_b(t)＝(a_bx(t),a_by(t),a_bz(t))^T，世界坐標(biāo)系下的加速度a_n(t)＝c_bna_b(t)，其中，a_bx(t)、a_by(t)、a_bz(t)分別表示的是加速度a_b(t)在慣性坐標(biāo)系x、y、z軸上的分量，C_bn表示的是慣性坐標(biāo)系至世界坐標(biāo)系的旋轉(zhuǎn)矩陣。

5.根據(jù)權(quán)利要求4所述的移動機(jī)器人定位方法，其特征在于，所述世界坐標(biāo)系下的加速度a_n(t)扣除重力加速度后一次積分，計算出速度v_g(t)，即其中，v_g(0)、g_g(0)分別代表初始速度、重力加速度。

6.根據(jù)權(quán)利要求5所述的移動機(jī)器人定位方法，其特征在于，所述速度v_g(t)進(jìn)行再次積分，得到位移s_g(t)，即其中，s_g(0)為初始位移。

7.根據(jù)權(quán)利要求1所述的移動機(jī)器人定位方法，其特征在于，所述S2.3中測量的加速度以三軸加速度計為誤差分析對象，對其建模為a_m＝a+g+δb+w_a，其中，a、w_a、a_m、b、δ、g分別表示實際加速度、隨機(jī)噪聲、測量加速度、加速度偏移量、加速度偏移量系數(shù)、重力加速度。

8.根據(jù)權(quán)利要求1所述的移動機(jī)器人定位方法，其特征在于，所述S2.3中測量的角速度為誤差分析對象，對其建模為w_m＝w+b_c+b_w+n，其中，b_c、b_w、w、n、w_m分別表示的是靜態(tài)偏移量、動態(tài)偏移量、角速度真實值、隨機(jī)噪聲、角速度測量值。

9.根據(jù)權(quán)利要求1所述的移動機(jī)器人定位方法，其特征在于，S3中所述云地圖向柵格地圖轉(zhuǎn)換，借助機(jī)器人操作系統(tǒng)向外發(fā)布構(gòu)建的地圖階段與定位節(jié)點。

10.根據(jù)權(quán)利要求9所述的移動機(jī)器人定位方法，其特征在于，所述柵格地圖在move-base框架內(nèi)完成定位節(jié)點與地圖的訂閱操作，實現(xiàn)機(jī)器人的局部與全局路徑規(guī)劃。