本發(fā)明涉及一種利用慣性系統(tǒng)進(jìn)行定位的技術(shù)領(lǐng)域，尤其涉及無(wú)GPS信號(hào)區(qū)域慣性導(dǎo)航系統(tǒng)的技術(shù)領(lǐng)域。

背景技術(shù)：

慣性技術(shù)的民用領(lǐng)域主要是精密測(cè)量和定位。在GPS信號(hào)不能進(jìn)入的水下、冰下、原始森林、隧道以及城市建筑物密集地區(qū)，慣性技術(shù)在精密導(dǎo)航、測(cè)量以及定位方面仍較GPS具有優(yōu)勢(shì)。此外，采用高精度的航海和航空重力儀用于陸地和海洋資源的物理勘探，以及采用慣性系統(tǒng)測(cè)量勘探深埋地下的各種管道的曲率半徑等，都是慣性系統(tǒng)具有優(yōu)勢(shì)的應(yīng)用領(lǐng)域。

目前國(guó)內(nèi)外對(duì)慣性導(dǎo)航的理論研究比較多，導(dǎo)航方法主要通過(guò)慣性測(cè)量組件（IMU）測(cè)量載體相對(duì)慣性空間的角速率和加速度信息，利用牛頓運(yùn)動(dòng)定律自動(dòng)推算物體的瞬時(shí)速度和位置信息，近幾年也取得了一系列好的結(jié)果，國(guó)外主要用于AGV小車，而且也有相應(yīng)的產(chǎn)品問(wèn)世，但是價(jià)格昂貴。一般用于港口物流和倉(cāng)儲(chǔ)等領(lǐng)域。國(guó)內(nèi)這方面理論研究比較多，但真正投入使用的國(guó)產(chǎn)慣性導(dǎo)航小型機(jī)器人卻很少，高校中目前有浙江大學(xué)、湖南大學(xué)、南昌大學(xué)等在進(jìn)行研究。市場(chǎng)應(yīng)用目前主要依賴進(jìn)口，價(jià)格普遍較高。

申請(qǐng)?zhí)枮?01210173646.0的中國(guó)專利申請(qǐng)公開(kāi)了一種磁導(dǎo)航輔助GPS/INS 組合導(dǎo)航定位系統(tǒng)及其控制方法，屬于機(jī)動(dòng)車輛導(dǎo)航定位領(lǐng)域。組合導(dǎo)航定位系統(tǒng)包括GPS 接收機(jī)、INS 及INS 處理單元、鋪設(shè)在道路上的磁釘、車載磁傳感器及中央處理模塊。中央處理單元包括解碼器和卡爾曼濾波模塊，接收GPS 接收機(jī)、INS 處理單元和磁傳感器的導(dǎo)航信息，通過(guò)卡爾曼濾波計(jì)算，修正INS 處理模塊的參數(shù)，得到最終的組合導(dǎo)航信息數(shù)據(jù)。該方法有效地解決了GPS 導(dǎo)航系統(tǒng)易受環(huán)境影響以及INS 的導(dǎo)航定位誤差隨時(shí)間延續(xù)不斷增大的問(wèn)題，提高了導(dǎo)航定位精度，實(shí)現(xiàn)了不間斷的車輛高精度導(dǎo)航定位功能。然而GPS不適用于室內(nèi)的導(dǎo)航系統(tǒng)。

申請(qǐng)?zhí)枮?01510590866.7的中國(guó)專利申請(qǐng)公開(kāi)了一種適合倉(cāng)儲(chǔ)AGV 的慣性導(dǎo)航方法，它包括如下子步驟：S1 ：導(dǎo)航系統(tǒng)搭建，S2 ：讀取磁釘數(shù)據(jù)，S3 ：獲取小車偏移數(shù)據(jù)，S4 ：規(guī)劃路徑，S5 ：獲取陀螺儀角度，S6 ：角度校準(zhǔn)，S7 ：讀取RFID，S8 ：確定運(yùn)行模式，S9 ：運(yùn)動(dòng)控制，S10 ：重復(fù)步驟S2-S9。該方法采用磁釘代替磁條和二維碼，有效地節(jié)約成本的同時(shí)能解決二維碼受多塵環(huán)境影響較嚴(yán)重的缺陷，實(shí)現(xiàn)無(wú)軌導(dǎo)航方式，該系統(tǒng)測(cè)量位置準(zhǔn)確，位置精度可達(dá)±5mm，測(cè)量角度準(zhǔn)確，角度精度為±0.1 度，其導(dǎo)航精度高，導(dǎo)航精度為±10mm，導(dǎo)航速度可達(dá)1m/s，有效的節(jié)約資源。然而其獲取航向信息的算法比較復(fù)雜，需要導(dǎo)航系統(tǒng)占用更多的時(shí)間和資源進(jìn)行計(jì)算，制約了導(dǎo)航速度。

申請(qǐng)?zhí)枮?01410572116.2的中國(guó)專利申請(qǐng)公開(kāi)了一種新型叉車型AGV 小車，包括車體, 車體的前部設(shè)置有升降系統(tǒng)，升降系統(tǒng)傳動(dòng)連接有物料叉，其特征在于：所述車體上設(shè)置有多功能一體控制裝置，所述車體的中部設(shè)置有雙輪差速驅(qū)動(dòng)裝置，雙輪差速驅(qū)動(dòng)裝置連接多功能一體控制裝置；所述車體的四周設(shè)置有防護(hù)裝置；所述車體的底部設(shè)置有自適應(yīng)移動(dòng)機(jī)構(gòu)。該發(fā)明實(shí)現(xiàn)了在叉車的性能加載了AGV 的原地打轉(zhuǎn)，精準(zhǔn)控制，自動(dòng)運(yùn)行的特點(diǎn)，同時(shí)能夠保證本發(fā)明轉(zhuǎn)向更靈活，轉(zhuǎn)彎半徑變小。然而該發(fā)明需要較多的傳感器同時(shí)確定航向信息，磁導(dǎo)航裝置、慣性導(dǎo)航裝置、激光導(dǎo)航裝置、無(wú)發(fā)射板激光導(dǎo)航裝置、光帶導(dǎo)航裝置，并且磁條的使用使導(dǎo)航精度受到了制約。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明提供了一種面向室內(nèi)移動(dòng)機(jī)器人的高精度組合導(dǎo)航系統(tǒng)及方法，在該系統(tǒng)中加入自主設(shè)計(jì)的慣性導(dǎo)航控制算法模塊，并且獨(dú)創(chuàng)了新的航向角計(jì)算方法，其有益效果在于：1）采用信息融合的方式，利用多種傳感器融合來(lái)實(shí)現(xiàn)慣性導(dǎo)航；2）自主設(shè)計(jì)慣性導(dǎo)航控制算法模塊，總結(jié)出更簡(jiǎn)單的算法及航向角算法公式；3）導(dǎo)航精度高；4）不需要依賴GPS；5）適合復(fù)雜環(huán)境導(dǎo)航；6) 面向室內(nèi)機(jī)器人領(lǐng)域。

本發(fā)明提供了一種面向室內(nèi)移動(dòng)機(jī)器人的高精度組合導(dǎo)航系統(tǒng)，包括傳感器數(shù)據(jù)采集模塊、微處理器和PC端以及慣性導(dǎo)航控制算法模塊、磁釘和先驗(yàn)地圖，所述微處理器包括數(shù)據(jù)融合模塊、控制模塊、無(wú)線模塊，所述數(shù)據(jù)融合模塊對(duì)所述傳感器數(shù)據(jù)采集模塊獲得的信號(hào)進(jìn)行數(shù)據(jù)融合，所述慣性導(dǎo)航控制算法模塊依據(jù)數(shù)據(jù)融合得到的信息及所述磁釘?shù)奈恢?，結(jié)合所述先驗(yàn)地圖經(jīng)過(guò)計(jì)算確定航向信息。

優(yōu)選的是，所述航向信息通過(guò)航向角算法獲取，具體公式為：

da為機(jī)器人檢測(cè)到磁釘A并調(diào)整航向后，所述磁釘A距離機(jī)器人中軸線的距離；db為檢測(cè)到磁釘B時(shí)，磁釘B距離中軸線的距離；L為機(jī)器人從節(jié)點(diǎn)A到達(dá)節(jié)點(diǎn)B行駛的里程；（xa,ya）是磁釘A的全局坐標(biāo)；（xb,yb）是磁釘B的全局坐標(biāo)；angle為磁釘A、B直接的路徑航向，根據(jù)angle的正負(fù)判斷機(jī)器人左偏或右偏。

上述任一方案中優(yōu)選的是，所述傳感器數(shù)據(jù)采集模塊包括慣性導(dǎo)航傳感器、磁導(dǎo)航傳感器和光電編碼器。

上述任一方案中優(yōu)選的是，所述慣性導(dǎo)航傳感器為9自由度慣性導(dǎo)航傳感器。

上述任一方案中優(yōu)選的是，所述9自由度慣性導(dǎo)航傳感器包括三軸陀螺儀、三軸加速器和三軸電子羅盤。

上述任一方案中優(yōu)選的是，所述三軸陀螺儀為ITG3205。

上述任一方案中優(yōu)選的是，所述三軸加速器為ADXL345。

上述任一方案中優(yōu)選的是，所述三軸電子羅盤為HMC5883L。

上述任一方案中優(yōu)選的是，所述磁導(dǎo)航傳感器為16位磁導(dǎo)航傳感器。

上述任一方案中優(yōu)選的是，所述16位磁導(dǎo)航傳感器為XGS16N型16位磁導(dǎo)航傳感器。

上述任一方案中優(yōu)選的是，所述光電編碼器為歐姆龍E6B2-CWZ6C編碼器。

上述任一方案中優(yōu)選的是，所述微處理器包括主控芯片，所述主控芯片為STM32-F103。

上述任一方案中優(yōu)選的是，所述數(shù)據(jù)融合模塊包括移動(dòng)平臺(tái)自定位模塊和航向信息修正模塊，所述移動(dòng)平臺(tái)自定位模塊通過(guò)卡爾曼濾波算法進(jìn)行輸入融合。

上述任一方案中優(yōu)選的是，所述PC端包括上位機(jī)模塊。

上述任一方案中優(yōu)選的是，所述PC端包括對(duì)控制模式的選擇模塊，所述控制模式包括遙控模式和自循跡模式。

上述任一方案中優(yōu)選的是，所述無(wú)線模塊采用ESP-12F 無(wú)線WIFI模塊。

本發(fā)明還提供了一種面向室內(nèi)移動(dòng)機(jī)器人的高精度組合導(dǎo)航的方法，包括以下步驟：

步驟一：通過(guò)終端獲取傳感器的值，

步驟二：進(jìn)行數(shù)據(jù)融合計(jì)算出下一步的航向值，

步驟三：發(fā)送給上位機(jī)，

步驟四：傳送給機(jī)器人導(dǎo)航系統(tǒng)進(jìn)行決策，具體步驟為，

通過(guò)主控芯片采集慣性導(dǎo)航傳感器、磁導(dǎo)航傳感器和光電編碼器的信號(hào)；對(duì)所述慣性導(dǎo)航傳感器、磁導(dǎo)航傳感器和光電編碼器信息進(jìn)行融合，獲取控制量，依據(jù)所述磁導(dǎo)航傳感器以及磁釘?shù)奈恢茫Y(jié)合先驗(yàn)地圖，通過(guò)計(jì)算獲得航向角；實(shí)時(shí)輸出控制量信號(hào)到驅(qū)動(dòng)模塊，將計(jì)算結(jié)果通過(guò)無(wú)線模塊傳送到PC上位機(jī)模塊；PC上位機(jī)模塊顯示定位結(jié)果和地圖。

優(yōu)選的是，所述控制量的獲取步驟為：

所述慣性導(dǎo)航傳感器返回位姿信息，所述光電編碼器測(cè)量機(jī)器人當(dāng)前的速度，通過(guò)所述慣性導(dǎo)航傳感器和光電編碼器的信息融合推算出所述機(jī)器人的相對(duì)位姿變化；

通過(guò)所述磁釘?shù)奈恢脤?duì)所述機(jī)器人航向信息進(jìn)行修正；

慣性導(dǎo)航控制算法模塊依據(jù)所述數(shù)據(jù)融合得到的信息及所述磁釘?shù)奈恢?，結(jié)合所述先驗(yàn)地圖經(jīng)過(guò)計(jì)算確定所述航向信息，數(shù)據(jù)融合模塊采用卡爾曼濾波算法進(jìn)行輸入融合，傳送給控制模塊修正航向信息。

上述任一方案中優(yōu)選的是，通過(guò)航向角算法計(jì)算得到所述航向信息，基體公式如下：

da為機(jī)器人檢測(cè)到磁釘A并調(diào)整航向后，磁釘A距離機(jī)器人中軸線的距離；db為檢測(cè)到磁釘B時(shí)，磁釘B距離中軸線的距離；L為機(jī)器人從節(jié)點(diǎn)A到達(dá)節(jié)點(diǎn)B行駛的里程；（xa,ya）是磁釘A的全局坐標(biāo)；（xb,yb）是磁釘B的全局坐標(biāo)；angle為磁釘A、B直接的路徑航向，根據(jù)angle的正負(fù)判斷機(jī)器人左偏或右偏。

上述任一方案中優(yōu)選的是，通過(guò)所述慣性導(dǎo)航傳感器測(cè)量機(jī)器人當(dāng)前的加速度、角加速度和方位角。

上述任一方案中優(yōu)選的是，通過(guò)所述磁導(dǎo)航傳感器檢測(cè)磁釘?shù)奈恢茫龃艑?dǎo)航傳感器包含有數(shù)據(jù)口，通過(guò)所述數(shù)據(jù)口向微控制器發(fā)送數(shù)據(jù)。

上述任一方案中優(yōu)選的是，通過(guò)所述光電編碼器檢測(cè)機(jī)器人的速度，所述光電編碼器包含有光電開(kāi)關(guān)，通過(guò)所述光電開(kāi)關(guān)檢測(cè)光柵轉(zhuǎn)動(dòng)的次數(shù)獲取機(jī)器人的速度。

上述任一方案中優(yōu)選的是，所述數(shù)據(jù)融合模塊包括移動(dòng)平臺(tái)自定位模塊和航向信息修正模塊，所述移動(dòng)平臺(tái)自定位模塊進(jìn)行移動(dòng)平臺(tái)自定位，通過(guò)卡爾曼濾波得到精確的角度信息，結(jié)合所述光電編碼器算出里程；航向信息修正模塊進(jìn)行航向信息的修正，根據(jù)所述移動(dòng)平臺(tái)自定位模塊得到的里程信息得到累計(jì)誤差，通過(guò)磁導(dǎo)航傳感器來(lái)修正所述累計(jì)誤差，所述數(shù)據(jù)融合模塊通過(guò)磁導(dǎo)航傳感器檢測(cè)到磁釘時(shí)，從保存的路徑信息中獲取所述磁釘?shù)娜肿鴺?biāo)，以及所述磁釘和下一個(gè)磁釘之間的航向，根據(jù)所述磁釘對(duì)應(yīng)的路徑信息，實(shí)時(shí)修正機(jī)器人的導(dǎo)航信息，以正確的姿態(tài)駛?cè)胂乱粋€(gè)節(jié)點(diǎn)。本發(fā)明中所述下一個(gè)磁釘指的是行進(jìn)軌跡上的下一個(gè)磁釘，當(dāng)行進(jìn)軌跡經(jīng)過(guò)兩個(gè)相鄰磁釘時(shí)，所述當(dāng)前磁釘和下一個(gè)磁釘為相鄰磁釘。

上述任一方案中優(yōu)選的是，微控制器通過(guò)無(wú)線模塊與PC端進(jìn)行雙向通訊。

附圖說(shuō)明

為了更清楚地說(shuō)明本發(fā)明實(shí)施例中的技術(shù)方案，下面將對(duì)實(shí)施例中所需要使用的附圖作一簡(jiǎn)單地介紹，顯而易見(jiàn)地，下面描述中的附圖是本發(fā)明的一些實(shí)施例，對(duì)于本領(lǐng)域普通技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)性的前提下，還可以根據(jù)這些附圖獲得其他的附圖。

圖1 是作為按照本發(fā)明的一優(yōu)選實(shí)施例的面向室內(nèi)移動(dòng)機(jī)器人的高精度組合導(dǎo)航系統(tǒng)的總體框圖

圖2是作為按照本發(fā)明的一優(yōu)選實(shí)施例的面向室內(nèi)移動(dòng)機(jī)器人的高精度組合導(dǎo)航系統(tǒng)的慣性導(dǎo)航電路設(shè)計(jì)圖

圖3是作為按照本發(fā)明的一優(yōu)選實(shí)施例的面向室內(nèi)移動(dòng)機(jī)器人的高精度組合導(dǎo)航系統(tǒng)的磁導(dǎo)航電路設(shè)計(jì)圖

圖4是作為按照本發(fā)明的一優(yōu)選實(shí)施例的一種面向室內(nèi)移動(dòng)機(jī)器人的高精度組合導(dǎo)航系統(tǒng)的光電編碼器電路設(shè)計(jì)圖

圖5 是作為按照本發(fā)明的一優(yōu)選實(shí)施例的面向室內(nèi)移動(dòng)機(jī)器人的高精度組合導(dǎo)航系統(tǒng)的WIFI無(wú)線發(fā)射接口電路設(shè)計(jì)圖

圖6 是作為按照本發(fā)明的一優(yōu)選實(shí)施例的面向室內(nèi)移動(dòng)機(jī)器人的高精度組合導(dǎo)航方法的主程序流程圖

圖7 是作為按照本發(fā)明的一優(yōu)選實(shí)施例的面向室內(nèi)移動(dòng)機(jī)器人的高精度組合導(dǎo)航系統(tǒng)及方法的航向角算法示意圖。

具體實(shí)施方式

下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步詳細(xì)描述，有必要在此指出的是，以下具體實(shí)施方式只用于對(duì)本發(fā)明進(jìn)行進(jìn)一步的說(shuō)明，不能理解為對(duì)本發(fā)明保護(hù)范圍的限制，該領(lǐng)域的技術(shù)人員可以根據(jù)上述發(fā)明內(nèi)容對(duì)本發(fā)明作出一些非本質(zhì)的改進(jìn)和調(diào)整。

本發(fā)明利用高精度磁導(dǎo)航傳感器去檢測(cè)磁釘，在磁釘位置，機(jī)器人根據(jù)在此位置保存的先驗(yàn)知識(shí)去校準(zhǔn)慣性導(dǎo)航航向誤差和機(jī)器人位置誤差，并調(diào)整機(jī)器人航向。本發(fā)明利用輪式機(jī)器人作為實(shí)驗(yàn)平臺(tái)，搭載高精度磁導(dǎo)航傳感器并結(jié)合先驗(yàn)地圖，理論上可以將橫向誤差控制在厘米級(jí)。這種方案?jìng)鞲衅鞒杀据^低，導(dǎo)航算法簡(jiǎn)單易行，在工程上容易實(shí)現(xiàn)，可以極大的提高機(jī)器人導(dǎo)航的實(shí)時(shí)性和定位精度。

圖1為整個(gè)系統(tǒng)的總體設(shè)計(jì)框圖，慣性導(dǎo)航傳感器返回位姿信息，光電編碼器測(cè)量當(dāng)前的速度，通過(guò)兩種傳感器的信息融合推算出系統(tǒng)的相對(duì)位姿變化。然后通過(guò)磁釘?shù)奈恢脤?duì)機(jī)器人航向信息進(jìn)行修正。數(shù)據(jù)融合模塊采用卡爾曼濾波算法進(jìn)行輸入融合，然后傳送給控制模塊進(jìn)行系統(tǒng)的修正?？柭鼮V波是線性系統(tǒng)中最優(yōu)估計(jì)理論，它僅需要存儲(chǔ)上一時(shí)刻的歷史數(shù)據(jù)，數(shù)據(jù)量小，非常適合于處理器實(shí)時(shí)運(yùn)算。微控制器通過(guò)WIFI模塊與PC端進(jìn)行雙向通訊。

本實(shí)施例提供了一種優(yōu)選的面向室內(nèi)移動(dòng)機(jī)器人的高精度組合導(dǎo)航系統(tǒng)，包括傳感器數(shù)據(jù)采集模塊、微處理器和PC端，以及慣性導(dǎo)航控制算法模塊、磁釘和先驗(yàn)地圖，所述微處理器包括數(shù)據(jù)融合模塊、控制模塊、無(wú)線模塊，所述數(shù)據(jù)融合模塊對(duì)所述傳感器數(shù)據(jù)采集模塊獲得的信號(hào)進(jìn)行數(shù)據(jù)融合，所述慣性導(dǎo)航控制算法模塊依據(jù)所述數(shù)據(jù)融合得到的信息及所述磁釘?shù)奈恢?，結(jié)合所述先驗(yàn)地圖經(jīng)過(guò)計(jì)算確定航向信息。

所述航向信息通過(guò)航向角算法獲取，具體公式為：

如圖7所示，da為機(jī)器人檢測(cè)到磁釘A并調(diào)整航向后，所述磁釘A距離機(jī)器人中軸線的距離；db為檢測(cè)到磁釘B時(shí)，磁釘B距離中軸線的距離；L為機(jī)器人從節(jié)點(diǎn)A到達(dá)節(jié)點(diǎn)B行駛的里程；（xa,ya）是磁釘A的全局坐標(biāo)；（xb,yb）是磁釘B的全局坐標(biāo)；angle為磁釘A、B直接的路徑航向，根據(jù)angle的正負(fù)判斷機(jī)器人左偏或右偏。

在本實(shí)施例中，所述傳感器數(shù)據(jù)采集模塊包括慣性導(dǎo)航傳感器、磁導(dǎo)航傳感器和光電編碼器。

在本實(shí)施例中，所述慣性導(dǎo)航傳感器優(yōu)選為9自由度慣性導(dǎo)航傳感器。

在本實(shí)施例中，所述9自由度慣性導(dǎo)航傳感器包括三軸陀螺儀、三軸加速器和三軸電子羅盤。相對(duì)于單一自由度的慣性導(dǎo)航傳感器，9自由度慣性導(dǎo)航傳感器從更多的維度對(duì)導(dǎo)航系統(tǒng)進(jìn)行檢測(cè)，包括對(duì)角度、加速度及方位角的多自由度檢測(cè)，使所得數(shù)據(jù)的精度更高，導(dǎo)航系統(tǒng)的定位更加精準(zhǔn)，操控更加精確。

本實(shí)施例中，所述磁導(dǎo)航傳感器優(yōu)選為16位磁導(dǎo)航傳感器，所述16位磁導(dǎo)航傳感器是兩排的，一排可以出8位數(shù)據(jù)，兩排組合成16位的數(shù)據(jù)。圖3中僅展示出一排的電路設(shè)計(jì)。16位磁導(dǎo)航傳感器為高精度磁導(dǎo)航傳感器，其有益的效果在于對(duì)磁釘信號(hào)的檢測(cè)更加精確即便是磁釘微小的信號(hào)變化也能夠檢測(cè)到，使所得到的數(shù)據(jù)更加準(zhǔn)確，使導(dǎo)航系統(tǒng)的操控更加精確。

本實(shí)施例中，優(yōu)選使用光電編碼器代替?zhèn)鹘y(tǒng)的編碼器，其有益效果在于光電編碼器體積小，更加精密，光電編碼器本身分辨度很高，并且無(wú)接觸無(wú)磨損；同時(shí)，光電編碼器既可檢測(cè)角度位移，又可在機(jī)械轉(zhuǎn)換裝置幫助下檢測(cè)直線位移。

本實(shí)施例中，所述微處理器包括主控芯片，所述主控芯片為STM32-F103。

進(jìn)一步在本實(shí)施例中，在主控端設(shè)置有先驗(yàn)地圖，可以輔助機(jī)器人導(dǎo)航系統(tǒng)航向信息的矯正。

在本實(shí)施例中，所述數(shù)據(jù)融合模塊包括移動(dòng)平臺(tái)自定位模塊和航向信息修正模塊。所述移動(dòng)平臺(tái)自定位模塊進(jìn)行移動(dòng)平臺(tái)的自定位和所述航向信息修正模塊進(jìn)行航向信息的修正。移動(dòng)平臺(tái)自定位需要通過(guò)卡爾曼濾波得到精確的角度信息然后結(jié)合光電編碼器算出里程。航向信息的修正，通過(guò)以上得到的里程信息的累計(jì)誤差，然后通過(guò)磁導(dǎo)航傳感器來(lái)修正所述累計(jì)誤差。所述數(shù)據(jù)融合模塊通過(guò)磁導(dǎo)航傳感器檢測(cè)到某個(gè)磁釘時(shí)，從保存的路徑信息中獲取該磁釘?shù)娜肿鴺?biāo)，以及當(dāng)前磁釘和下一個(gè)磁釘之間的航向。根據(jù)磁釘對(duì)應(yīng)的路徑信息，實(shí)時(shí)修正機(jī)器人的導(dǎo)航信息，繼而以正確的姿態(tài)駛?cè)胂乱粋€(gè)節(jié)點(diǎn)。

在本實(shí)施例中，所述PC端包括上位機(jī)模塊。本實(shí)施例的導(dǎo)航系統(tǒng)的工作原理為通過(guò)主控芯片采集慣性導(dǎo)航傳感器、磁導(dǎo)航傳感器和光電編碼器的信號(hào)；對(duì)所述慣性導(dǎo)航傳感器、磁導(dǎo)航傳感器和光電編碼器信息進(jìn)行融合，獲取控制量，依據(jù)所述磁導(dǎo)航傳感器以及磁釘?shù)奈恢?，結(jié)合先驗(yàn)地圖，通過(guò)計(jì)算獲得航向角；實(shí)時(shí)輸出控制量信號(hào)到驅(qū)動(dòng)模塊，將計(jì)算結(jié)果通過(guò)無(wú)線模塊傳送到PC端上位機(jī)模塊；PC端上位機(jī)模塊顯示定位結(jié)果和地圖。

在本實(shí)施例中，所述PC端包括控制模式選擇模塊，所述控制模式包括遙控模式和自循跡模式。所述遙控模式指操作者可以通過(guò)鍵盤發(fā)送控制命令遙控小車前進(jìn)或后退；所述自循跡模式指操作者通過(guò)預(yù)先加載地圖的方式將地圖存放在主控芯片中，這樣機(jī)器人就可以根據(jù)當(dāng)前軌跡匹配離線地圖里面的軌跡信息來(lái)獲取下一個(gè)節(jié)點(diǎn)的信息。

本實(shí)施例中主要硬件選擇為：

1. 處理器的選擇 ：STM32-F103作為信息處理的核心部分

2. 慣性導(dǎo)航傳感器的選擇：采用9自由度慣性導(dǎo)航傳感器，包括ITG3205 、ADXL345 和HMC5883L，其中ITG3205為三軸陀螺儀 、ADXL345為三周加速器和HMC5883L為三周電子羅盤

3. 磁導(dǎo)航傳感器的選擇：XGS16N型16位磁導(dǎo)航傳感器

4. 光電編碼器的選擇：歐姆龍E6B2-CWZ6C編碼器。

5. WIFI無(wú)線發(fā)射模塊的選擇：采用ESP-12F 無(wú)線WIFI模塊。

如圖2所示為本實(shí)施例中慣性導(dǎo)航電路設(shè)計(jì)圖，慣性導(dǎo)航傳感器可以測(cè)量當(dāng)前系統(tǒng)的加速度，角加速度以及方位角。

如圖3所示為本實(shí)施例中磁導(dǎo)航電路，磁導(dǎo)航傳感器選用XGS16N型16位磁導(dǎo)航傳感器，其中8個(gè)數(shù)據(jù)口與微控制器相連，檢測(cè)到磁釘?shù)奈恢?，通過(guò)數(shù)據(jù)口向微控制器發(fā)送數(shù)據(jù)。

如圖4所示為本實(shí)施例中光電編碼器電路，光電編碼器可以通過(guò)光電開(kāi)關(guān)檢測(cè)光柵轉(zhuǎn)動(dòng)的次數(shù)從而檢測(cè)系統(tǒng)的速度。

如圖5所示為WIFI無(wú)線發(fā)射接口電路設(shè)計(jì)圖，本實(shí)施例中WIFI模塊的數(shù)據(jù)傳輸接口為標(biāo)準(zhǔn)異步通信串口，分別連接微控制器A12、A13管腳。微控制器通過(guò)串口中斷向WIFI模塊發(fā)送信息。

本實(shí)施例的導(dǎo)航系統(tǒng)軟件設(shè)計(jì)中采用STM32-F103作為主控芯片，通過(guò)主控芯片采集各MEMS慣性導(dǎo)航系統(tǒng)信號(hào)、磁導(dǎo)航傳感器信號(hào)以及光電編碼器信號(hào)；對(duì)各傳感器信息進(jìn)行融合，獲取控制量，并實(shí)時(shí)輸出控制量信號(hào)到驅(qū)動(dòng)模塊以及將計(jì)算結(jié)果通過(guò)WIFI模塊傳送到PC端上位機(jī)模塊；PC端上位機(jī)模塊負(fù)責(zé)定位結(jié)果顯示和地圖顯示。當(dāng)然，PC端在初始階段可以選擇控制模式，可以選擇遙控模式和自循跡模式。遙控模式指操作者可以通過(guò)鍵盤發(fā)送控制命令遙控小車前進(jìn)或后退；自循跡模式指操作者通過(guò)預(yù)先加載地圖的方式將地圖存放在主控芯片中，這樣機(jī)器人就可以根據(jù)當(dāng)前軌跡匹配離線地圖里面的軌跡信息來(lái)獲取下一個(gè)節(jié)點(diǎn)的信息。

通過(guò)本實(shí)施例提供的導(dǎo)航系統(tǒng)可按照以下方法進(jìn)行室內(nèi)移動(dòng)機(jī)器人的高精度組合導(dǎo)航，所述導(dǎo)航系統(tǒng)程序步驟：

步驟一：通過(guò)終端獲取傳感器的值；

步驟二：進(jìn)行數(shù)據(jù)融合計(jì)算出下一步的航向值；

步驟三：發(fā)送給上位機(jī)；

步驟四：傳送給系統(tǒng)進(jìn)行決策。

具體步驟為，

通過(guò)主控芯片采集慣性導(dǎo)航傳感器、磁導(dǎo)航傳感器和光電編碼器的信號(hào)；對(duì)所述慣性導(dǎo)航傳感器、磁導(dǎo)航傳感器和光電編碼器信息進(jìn)行融合，獲取控制量，依據(jù)所述磁導(dǎo)航傳感器以及磁釘?shù)奈恢?，結(jié)合先驗(yàn)地圖，通過(guò)計(jì)算獲得航向角；實(shí)時(shí)輸出控制量信號(hào)到驅(qū)動(dòng)模塊，將計(jì)算結(jié)果通過(guò)無(wú)線模塊傳送到PC上位機(jī)模塊；PC上位機(jī)模塊顯示定位結(jié)果和地圖。

在本實(shí)施例中，所述控制量的獲取步驟為：

所述慣性導(dǎo)航傳感器返回位姿信息，所述光電編碼器測(cè)量機(jī)器人當(dāng)前的速度，通過(guò)所述慣性導(dǎo)航傳感器和光電編碼器的信息融合推算出所述機(jī)器人的相對(duì)位姿變化；

通過(guò)所述磁釘?shù)奈恢脤?duì)所述機(jī)器人航向信息進(jìn)行修正；

慣性導(dǎo)航控制算法模塊依據(jù)所述數(shù)據(jù)融合得到的信息及所述磁釘?shù)奈恢?，結(jié)合所述先驗(yàn)地圖經(jīng)過(guò)計(jì)算確定所述航向信息，數(shù)據(jù)融合模塊采用卡爾曼濾波算法進(jìn)行輸入融合，傳送給控制模塊修正航向信息。

如圖6所示為主程序流程圖，導(dǎo)航開(kāi)始后傳感器數(shù)據(jù)采集模塊進(jìn)行信號(hào)的采集，然后進(jìn)行數(shù)據(jù)的預(yù)處理，再經(jīng)過(guò)數(shù)據(jù)融合模塊對(duì)數(shù)據(jù)進(jìn)行融合處理，獲取的結(jié)果傳輸?shù)綀?zhí)行機(jī)構(gòu)模塊和/或經(jīng)過(guò)無(wú)線網(wǎng)進(jìn)行數(shù)據(jù)傳輸?shù)缴衔粰C(jī)模塊，上位機(jī)模塊顯示定位結(jié)果和地圖，然后再重新回到傳感器數(shù)據(jù)采集模塊進(jìn)行下一個(gè)步驟的循環(huán)，完成機(jī)器人系統(tǒng)的定位導(dǎo)航。

本實(shí)施例中系統(tǒng)的實(shí)現(xiàn)方法主要通過(guò)兩大主要的處理模塊完成，包括傳感器采集模塊和數(shù)據(jù)融合模塊。

本實(shí)施例中傳感器采集模塊的作用原理為，主控芯片通過(guò)本身自帶的AD模塊和SCI模塊獲取低精度慣性導(dǎo)航系統(tǒng)的信號(hào)，通過(guò)脈沖累計(jì)器模塊獲取光電編碼器采集的輪速信號(hào)，通過(guò)XGS16N型16位磁導(dǎo)航傳感器獲取磁釘偏離磁導(dǎo)航傳感器的距離信號(hào)。

本實(shí)施例中數(shù)據(jù)融合模塊包括兩部分，一個(gè)是移動(dòng)平臺(tái)自定位，需要通過(guò)卡爾曼濾波得到精確的角度信息然后結(jié)合光電編碼器算出里程。另一個(gè)是航向信息的修正，以上得到的里程信息中得到累計(jì)誤差，然后通過(guò)磁導(dǎo)航傳感器來(lái)修正這個(gè)累計(jì)誤差。此模塊通過(guò)磁導(dǎo)航傳感器檢測(cè)到某個(gè)磁釘時(shí)，從保存的路徑信息中獲取該磁釘?shù)娜肿鴺?biāo)，以及當(dāng)前磁釘和下一個(gè)磁釘之間的航向。根據(jù)磁釘對(duì)應(yīng)的路徑信息，實(shí)時(shí)修正機(jī)器人的導(dǎo)航信息，繼而以正確的姿態(tài)駛?cè)胂乱粋€(gè)節(jié)點(diǎn)。下面介紹下航向角算法，如圖7所示，da為機(jī)器人檢測(cè)到磁釘A并調(diào)整航向后，磁釘A距離機(jī)器人中軸線的距離；db為檢測(cè)到磁釘B時(shí)，磁釘B距離中軸線的距離；L為機(jī)器人從節(jié)點(diǎn)A到達(dá)節(jié)點(diǎn)B行駛的里程，（xa,ya）是磁釘A的全局坐標(biāo)，（xb,yb）是磁釘B的全局坐標(biāo)；angle為磁釘A、B直接的路徑航向。由圖我們可以算出angle的值，根據(jù)angle的正負(fù)我們可以判斷機(jī)器人左偏還是右偏。

本發(fā)明的創(chuàng)新之處在于：

1. 采用信息融合的方式，利用多種傳感器融合來(lái)實(shí)現(xiàn)慣性導(dǎo)航；

2. 自主設(shè)計(jì)慣性導(dǎo)航控制算法模塊，與現(xiàn)有技術(shù)中通過(guò)傳感器直接測(cè)得航向角的方法不同的是，本發(fā)明主要是依據(jù)磁導(dǎo)航傳感器以及磁釘?shù)奈恢?，結(jié)合之前保存的先驗(yàn)地圖，通過(guò)計(jì)算獲得的航向角。在本發(fā)明的這一過(guò)程中需要進(jìn)行磁釘?shù)亩ㄎ灰约跋鄳?yīng)地圖的采集，主要作用是提高了導(dǎo)航的精度，而且使導(dǎo)航系統(tǒng)受環(huán)境影響小。本發(fā)明所述的導(dǎo)航系統(tǒng)含有慣性導(dǎo)航控制算法模塊，所述慣性導(dǎo)航控制算法模塊通過(guò)航向角算法計(jì)算得到航向信息及矯正信息，其公式，為利用本發(fā)明所述導(dǎo)航系統(tǒng)獲得的數(shù)據(jù)所得到，并且為本發(fā)明所述的方法所特有的，其算法簡(jiǎn)單，使得數(shù)據(jù)處理速度更快，導(dǎo)航系統(tǒng)更加精確快速；

3. 導(dǎo)航精度高，位置精度±5mm，角度精度±0.1度，導(dǎo)航精度±10mm，導(dǎo)航速度2m/s；

4. 不需要依賴GPS，導(dǎo)航系統(tǒng)中不包括GPS裝置；

5. 適合復(fù)雜環(huán)境導(dǎo)航；

6. 使用磁釘而不是磁條，這是由于磁條只能夠?qū)B續(xù)的位置進(jìn)行檢測(cè)，而磁釘用于對(duì)單獨(dú)的點(diǎn)進(jìn)行檢測(cè)，因此檢測(cè)更加靈活，定位精度更高，成本也更低、易維護(hù)；

7.面向室內(nèi)機(jī)器人領(lǐng)域。