[發(fā)明專利]機(jī)器人自主導(dǎo)航定位方法及系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 201810572810.2
• 申請(qǐng)日: 2018-06-05
• 公開（公告）號(hào): CN108731673B
• 公開（公告）日: 2021-07-27
• 發(fā)明（設(shè)計(jì)）人: 曹天揚(yáng);蔡浩原;舒心;劉昶
• 申請(qǐng)（專利權(quán)）人: 中國科學(xué)院電子學(xué)研究所
• 主分類號(hào): G01C21/16
• 分類號(hào): G01C21/16;G01C21/18;G01C21/00
• 代理公司: 中科專利商標(biāo)代理有限責(zé)任公司 11021
• 代理人: 任巖

一種機(jī)器人自主導(dǎo)航定位方法及系統(tǒng)，其中，該機(jī)器人上布置有視覺傳感器、單軸慣性傳感器和里程計(jì)，視覺傳感器的主光軸和單軸慣性傳感器的測(cè)量軸均沿機(jī)器人轉(zhuǎn)動(dòng)軸向設(shè)置，利用單軸慣性傳感器和里程計(jì)對(duì)機(jī)器人進(jìn)行定位運(yùn)算，并結(jié)合視覺測(cè)量消除運(yùn)算過程中的航向誤差和里程誤差。本公開充分利用了慣導(dǎo)、視覺、和里程計(jì)的信息互補(bǔ)性，具有運(yùn)算量非常小、硬件簡(jiǎn)潔的優(yōu)點(diǎn)。

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

本公開涉及導(dǎo)航定位技術(shù)領(lǐng)域，尤其涉及一種機(jī)器人自主導(dǎo)航定位方法及系統(tǒng)。

背景技術(shù)

對(duì)于在室內(nèi)地面上運(yùn)動(dòng)的機(jī)器人，能夠自主導(dǎo)航是一項(xiàng)非常重要的功能。目前常用的方法非常消耗運(yùn)算資源，難以應(yīng)用到目前機(jī)器人的嵌入式平臺(tái)上。

例如，目前已有報(bào)道采用視覺、慣導(dǎo)和里程計(jì)進(jìn)行導(dǎo)航定位，如SLAM等，但主要是通過視覺特征點(diǎn)解算機(jī)器人的運(yùn)動(dòng)，非常復(fù)雜、要進(jìn)行大量的運(yùn)算、對(duì)計(jì)算機(jī)硬件平臺(tái)的要求很高，目前難以運(yùn)用到掃地機(jī)器人等運(yùn)算量有限的嵌入式平臺(tái)上。

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

(一)要解決的技術(shù)問題

本公開提供了一種機(jī)器人自主導(dǎo)航定位方法及系統(tǒng)，以至少部分解決以上所提出的技術(shù)問題。

(二)技術(shù)方案

根據(jù)本公開的一個(gè)方面，提供了一種機(jī)器人自主導(dǎo)航定位方法，該機(jī)器人上布置有視覺傳感器、單軸慣性傳感器和里程計(jì)，所述視覺傳感器的主光軸和單軸慣性傳感器的測(cè)量軸均沿機(jī)器人轉(zhuǎn)動(dòng)軸向設(shè)置，該機(jī)器人自主導(dǎo)航定位方法包括以下步驟中的至少之一：

對(duì)機(jī)器人轉(zhuǎn)彎運(yùn)動(dòng)的航向誤差進(jìn)行校正的步驟，具體包括：當(dāng)機(jī)器人作出轉(zhuǎn)彎運(yùn)動(dòng)時(shí)進(jìn)行一次視覺測(cè)量，利用視覺傳感器分別拍攝對(duì)應(yīng)于開始轉(zhuǎn)彎運(yùn)動(dòng)和結(jié)束轉(zhuǎn)彎運(yùn)動(dòng)時(shí)機(jī)器人所處環(huán)境的兩幀第一圖像，從該兩幀第一圖像中獲取機(jī)器人視覺的第一航向變化量，結(jié)合單軸慣性傳感器的測(cè)量數(shù)據(jù)，獲取機(jī)器人轉(zhuǎn)彎運(yùn)動(dòng)的航向誤差并進(jìn)行校正；

對(duì)機(jī)器人直線運(yùn)動(dòng)的航向誤差進(jìn)行校正的步驟，具體包括：當(dāng)機(jī)器人作出直線運(yùn)動(dòng)時(shí)每隔設(shè)定距離時(shí)進(jìn)行一次視覺測(cè)量，利用視覺傳感器分別拍攝對(duì)應(yīng)于第m個(gè)設(shè)定距離的起點(diǎn)和終點(diǎn)處機(jī)器人所處環(huán)境的兩幀第二圖像，該設(shè)定距離根據(jù)里程計(jì)測(cè)量得到，從該兩幀第二圖像中獲取機(jī)器人視覺的第二航向變化量，結(jié)合單軸慣性傳感器的測(cè)量數(shù)據(jù)，獲取在該第m個(gè)設(shè)定距離內(nèi)機(jī)器人直線運(yùn)動(dòng)的航向誤差并進(jìn)行校正，其中m≥1；

對(duì)機(jī)器人直線運(yùn)動(dòng)的里程誤差進(jìn)行校正的步驟，具體包括：利用所述兩幀第二圖像獲取第m個(gè)設(shè)定距離內(nèi)機(jī)器人視覺的直線運(yùn)動(dòng)量，結(jié)合里程計(jì)的測(cè)量結(jié)果，獲取該第m個(gè)設(shè)定距離內(nèi)機(jī)器人直線運(yùn)動(dòng)的里程誤差并進(jìn)行校正。

在本公開的一些實(shí)施例中，所述機(jī)器人視覺的第一航向變化量通過以下步驟得到：

確定所述兩幀第一圖像中的N組相同特征點(diǎn)，N≥1；

將該N組相同特征點(diǎn)的圖像坐標(biāo)代入式1和式2中，通過最小二乘法解算出機(jī)器人視覺的第一航向變化量：

其中，(x_1，n，y_1，n)和(x_2，n，y_2，n)分別為第n組相同特征點(diǎn)在兩幀第一圖像中的坐標(biāo)，n＝1、2...、N，f為視覺傳感器的焦距，dx和dy均為像素尺寸，Z_c為視覺傳感器與被拍攝物體之間的距離，H_vis，R為機(jī)器人視覺的第一航向變化量，(T_x，T_y)為機(jī)器人視覺的相對(duì)位移量。

在本公開的一些實(shí)施例中，所述獲取機(jī)器人轉(zhuǎn)彎運(yùn)動(dòng)的航向誤差進(jìn)行校正的步驟包括：

利用式3計(jì)算機(jī)器人轉(zhuǎn)彎運(yùn)動(dòng)的航向誤差：