專利名稱：機(jī)器人自主定位系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明屬于機(jī)器人智能控制裝置，具體涉及一種基于慣性傳感器和 三維數(shù)字羅盤的組合自主定位系統(tǒng)，用于煤礦、隧道等地下環(huán)境智能機(jī) 器人的自主定位。
背景技術(shù)：
機(jī)器人定位技術(shù)是用于確定機(jī)器人在環(huán)境中的實(shí)時(shí)位姿、引導(dǎo)機(jī)器 人到達(dá)目標(biāo)點(diǎn)的關(guān)鍵技術(shù)。在實(shí)際工作中，當(dāng)機(jī)器人要自主實(shí)現(xiàn)某一預(yù) 期功能時(shí)，首要的是要對(duì)自身位姿進(jìn)行實(shí)時(shí)監(jiān)測，然后才能根據(jù)當(dāng)前和 預(yù)期位姿之間的差異來規(guī)劃機(jī)器人下一步的運(yùn)動(dòng)軌跡，并實(shí)時(shí)更新自己 的位姿。
機(jī)器人的定位技術(shù)經(jīng)歷了由單定位方式向組合定位方式轉(zhuǎn)化的過程。當(dāng)前應(yīng)用較多的定位方式有GPS、圖像匹配、慣導(dǎo)等，這些方式各 有所長。自主性和定位精度一直是評(píng)價(jià)定位系統(tǒng)的兩個(gè)重要指標(biāo)，目前 還沒有一種定位方式能同時(shí)具有這兩個(gè)優(yōu)點(diǎn)。整合多種定位的優(yōu)勢，設(shè) 計(jì)新型組合定位系統(tǒng)是提高定位自主性和精度的主要途徑。
智能機(jī)器人應(yīng)用己經(jīng)從傳統(tǒng)的工業(yè)應(yīng)用向礦井地圖繪制、地下隧道 測量與施工、人員救援探測和定位等領(lǐng)域擴(kuò)展，對(duì)定位系統(tǒng)提出了更高 要求，如自主性，實(shí)時(shí)性等?，F(xiàn)有的定位理論和技術(shù)在人工環(huán)境下能提 供可靠而準(zhǔn)確的機(jī)器人位姿信息，但對(duì)于存在大量未知信息的地下環(huán)境， 現(xiàn)有定位方法存在很多局限性，甚至無法工作。
地下環(huán)境作業(yè)智能機(jī)器人對(duì)其定位系統(tǒng)的自主性提出了明確要求。 在地下非結(jié)構(gòu)化環(huán)境中，無法接收到GPS信號(hào)，只能利用機(jī)器人自身內(nèi)
部傳感器來獲取運(yùn)動(dòng)狀態(tài)，再通過數(shù)據(jù)處理獲取自身位姿。慣性傳感器 不需要環(huán)境信息即可感知、測量機(jī)器人運(yùn)動(dòng)參數(shù)，完全自主、可全天候 工作、更新速度快、瞬時(shí)精度高。但是，慣性傳感器存在誤差隨時(shí)間積 累快、使用前需初始對(duì)準(zhǔn)的缺點(diǎn)，這限制了慣性傳感器在需要長時(shí)間高 精度定位場合的應(yīng)用。三維數(shù)字羅盤是一種根據(jù)地球磁場測量機(jī)器人三 維姿態(tài)的傳感器，但它不能提供機(jī)器人運(yùn)動(dòng)加速度信息、信號(hào)輸出頻率 低、容易受環(huán)境干擾磁場影響，這些致命的缺點(diǎn)限制了三維數(shù)字羅盤的 廣泛應(yīng)用。
為解決機(jī)器人的高精度自主定位問題，傳統(tǒng)上主要采用慣導(dǎo)與GPS 結(jié)合的定位方法。這種方法具有自主性強(qiáng)、定位精度高的特點(diǎn)。在實(shí)際
使用中，當(dāng)GPS信號(hào)有效時(shí)，其定位效果良好。然而，在無GPS信號(hào)的 環(huán)境中，利用慣導(dǎo)雖然能實(shí)現(xiàn)自主定位，但其定位誤差會(huì)隨時(shí)間急劇累 積。圖像匹配方法也是智能機(jī)器人定位中常用的方法，但這種方式需要 預(yù)先存儲(chǔ)匹配圖像或地圖，然后才能通過圖像匹配來確定機(jī)器人當(dāng)前位 置。這些定位方式不適用于在地下非結(jié)構(gòu)化環(huán)境內(nèi)工作的機(jī)器人定位。 因此，研制針對(duì)地下環(huán)境的機(jī)器人自主定位系統(tǒng)是當(dāng)前研究的一個(gè)難點(diǎn) 問題，也是智能機(jī)器人技術(shù)的一個(gè)重要發(fā)展方向。
自主定位技術(shù)是智能機(jī)器人能否順利完成預(yù)期任務(wù)需具備關(guān)鍵核心 技術(shù)之一，現(xiàn)有的定位技術(shù)存在自主性和精度之間的矛盾，不能滿足地 下環(huán)境作業(yè)要求。因此，針對(duì)地下環(huán)境的特點(diǎn)，結(jié)合智能控制和機(jī)器人 技術(shù)發(fā)展的最新進(jìn)展，研制具有自主性的定位系統(tǒng)是機(jī)器人定位導(dǎo)航技 術(shù)發(fā)展的一個(gè)重要方向，也是提高機(jī)器人智能水平的一個(gè)重要手段。

發(fā)明內(nèi)容
本發(fā)明提供一種機(jī)器人自主定位系統(tǒng)，解決機(jī)器人在地下環(huán)境作業(yè) 的高精度自主定位問題。本發(fā)明的一種機(jī)器人自主定位系統(tǒng)，包括安裝平臺(tái)、設(shè)置于安裝平 臺(tái)上的傳感器子系統(tǒng)、數(shù)據(jù)處理子系統(tǒng)以及電壓轉(zhuǎn)換模塊，安裝平臺(tái)同 時(shí)為傳感器子系統(tǒng)標(biāo)定提供所需的采樣點(diǎn)；傳感器子系統(tǒng)實(shí)時(shí)感測機(jī)器 人運(yùn)動(dòng)狀態(tài)信息，經(jīng)數(shù)據(jù)處理子系統(tǒng)進(jìn)行去噪、濾波、融合和計(jì)算，得 出機(jī)器人的位姿和速度信息；電壓轉(zhuǎn)換模塊將車載直流電源轉(zhuǎn)換為所需 的工作電壓；其特征在于
所述安裝平臺(tái)上部為承載傳感器子系統(tǒng)的轉(zhuǎn)臺(tái)，轉(zhuǎn)臺(tái)運(yùn)動(dòng)范圍為 ±360°，由電機(jī)驅(qū)動(dòng)模塊驅(qū)動(dòng)，運(yùn)動(dòng)誤差控制在土0.1。以內(nèi)；安裝平臺(tái)下部 的固定基座為密封腔式結(jié)構(gòu)，內(nèi)部安裝數(shù)據(jù)處理子系統(tǒng)和電機(jī)驅(qū)動(dòng)模塊；
所述安裝平臺(tái)具有鎖緊裝置，鎖緊裝置的一端與轉(zhuǎn)臺(tái)底部固定連接， 鎖緊裝置另一端通過可分離螺桿與安裝平臺(tái)下部的固定基座鎖緊；在安 裝平臺(tái)上轉(zhuǎn)臺(tái)需要運(yùn)動(dòng)時(shí)，螺桿與安裝平臺(tái)下部固定基座分離，轉(zhuǎn)臺(tái)旋 轉(zhuǎn)，傳感器子系統(tǒng)標(biāo)定結(jié)束后，螺桿與安裝平臺(tái)下部固定基座鎖緊；
所述傳感器子系統(tǒng)包括慣性傳感器模塊和三維數(shù)字羅盤，慣性傳感 器模塊由三維加速度計(jì)及固聯(lián)在其三個(gè)敏感軸上相互正交的光纖陀螺儀 組成，測量慣性傳感器模塊參考坐標(biāo)系相對(duì)于地球慣性坐標(biāo)系運(yùn)動(dòng)角速 度和加速度；三維數(shù)字羅盤測量三維數(shù)字羅盤參考坐標(biāo)系相對(duì)地理坐標(biāo) 系的三維姿態(tài)；
所述數(shù)據(jù)處理子系統(tǒng)包括中央控制器及裝載于其內(nèi)部的數(shù)據(jù)預(yù)處理 模塊、數(shù)據(jù)融合模塊和航跡推算模塊；中央控制器具有多路RS-232串口 和以太網(wǎng)網(wǎng)絡(luò)接口 ，利用這些接口分別與傳感器子系統(tǒng)和外部設(shè)備進(jìn)行 通信；數(shù)據(jù)預(yù)處理模塊采用小波變換方法分別對(duì)光纖陀螺儀、三維加速 度計(jì)和三維數(shù)字羅盤的數(shù)據(jù)預(yù)處理，降低數(shù)據(jù)噪聲、隔離振動(dòng)與沖擊； 數(shù)據(jù)融合模塊利用卡爾曼算法融合光纖陀螺儀和三維數(shù)字羅盤經(jīng)過預(yù)處 理的數(shù)據(jù)，獲取安裝平臺(tái)的三維姿態(tài)信息；航跡推算模塊根據(jù)安裝平臺(tái)
所在地的經(jīng)緯度和引力場向量、初始速度向量或各個(gè)時(shí)刻的速度和位姿 向量，采用相對(duì)定位方式計(jì)算并更新安裝平臺(tái)實(shí)時(shí)位姿和速度信息。
所述的機(jī)器人自主定位系統(tǒng)，其特征在于，所述數(shù)據(jù)預(yù)處理模塊的 處理步驟為(1)對(duì)光纖陀螺儀、三維加速度計(jì)和三維數(shù)字羅盤的數(shù)據(jù) 進(jìn)行多通道多分辨小波分解；(2)對(duì)前述分解數(shù)據(jù)中的高頻分量，采取 強(qiáng)制消噪處理；(3)提取前述分解數(shù)據(jù)中的低頻分量進(jìn)行多通道多分辨 小波重構(gòu)；(4)輸出經(jīng)預(yù)處理后的數(shù)據(jù)。
所述的機(jī)器人自主定位系統(tǒng)，其特征在于，所述數(shù)據(jù)融合模塊的處 理步驟為(1)對(duì)光纖陀螺儀和三維數(shù)字羅盤的數(shù)據(jù)計(jì)算統(tǒng)計(jì)學(xué)參數(shù)； (2)實(shí)時(shí)更新卡爾曼濾波器參數(shù)；(3)自適應(yīng)卡爾曼濾波；(4)計(jì)算自 主定位系統(tǒng)姿態(tài)參數(shù)，將其返回步驟(2)。
所述的機(jī)器人自主定位系統(tǒng)，其特征在于，所述航跡推算模塊的處 理步驟為(1)初始化系統(tǒng)參數(shù)，包括初始點(diǎn)地理坐標(biāo)系，地理坐標(biāo)系 下機(jī)器人初始角速度向量、加速度向量、線速度向量、位置向量、當(dāng)?shù)?經(jīng)緯度以及引力場；
(2) 建立機(jī)器人自主定位系統(tǒng)坐標(biāo)系；
(3) 根據(jù)步驟(1)、 (2)，對(duì)機(jī)器人自主定位系統(tǒng)進(jìn)行初始對(duì)準(zhǔn)；
(4) 計(jì)算方向變換矩陣；
(5) 根據(jù)前一時(shí)刻機(jī)器人位姿和傳感器數(shù)據(jù)預(yù)處理與融合更新，計(jì) 算當(dāng)前時(shí)刻地理坐標(biāo)系下機(jī)器人運(yùn)動(dòng)狀態(tài)參數(shù)，包括地理坐標(biāo)系下線加 速度向量、相對(duì)于地理坐標(biāo)系的姿態(tài)向量、速度向量和位移向量；
(6) 機(jī)器人自主定位數(shù)據(jù)存儲(chǔ)和更新，定位數(shù)據(jù)通過通信接口傳輸 到外部設(shè)備，更新后的速度向量、位移向量、姿態(tài)向量進(jìn)入步驟(4)進(jìn)行循環(huán)。
所述的機(jī)器人自主定位系統(tǒng)，其特征在于所述電機(jī)驅(qū)動(dòng)模塊由固 定在支座上的附編碼盤的直流電機(jī)構(gòu)成，直流電機(jī)通過傳動(dòng)軸與轉(zhuǎn)臺(tái)下 部固定聯(lián)接；
所述的機(jī)器人自主定位系統(tǒng)，其特征在于所述安裝平臺(tái)主體材質(zhì) 為鋁合金，聯(lián)接螺栓為銅質(zhì)螺栓，除三維數(shù)字羅盤外安裝平臺(tái)的整體采 用磁性屏蔽材料包覆，以隔離電磁場對(duì)三維數(shù)字羅盤的干擾。
本發(fā)明利用慣性傳感器和三維數(shù)字羅盤組合方式實(shí)現(xiàn)自主定位，主 要原理和過程如下 一方面，利用光纖陀螺儀測量機(jī)器人相對(duì)于地球慣 性坐標(biāo)系的角速度向量，經(jīng)方向矩陣變換和剔除地球自轉(zhuǎn)角速度向量， 計(jì)算機(jī)器人相對(duì)于地理坐標(biāo)系的角速度向量，然后對(duì)時(shí)間積分一次即可 獲取機(jī)器人姿態(tài)向量；三維數(shù)字羅盤直接測量機(jī)器人在地理坐標(biāo)系下的 姿態(tài)向量；然后采用數(shù)據(jù)融合方法計(jì)算機(jī)器人姿態(tài)向量。另一方面，三
維加速度計(jì)測量機(jī)器人相對(duì)于地球慣性坐標(biāo)系的線加速度向量，經(jīng)方向 矩陣變換和剔除有害加速度向量后得到機(jī)器人在地理坐標(biāo)系下的線加速
度向量；根據(jù)前一時(shí)刻機(jī)器人的位姿、速度，對(duì)時(shí)間積分兩次計(jì)算機(jī)器 人當(dāng)前時(shí)刻的位置；不斷更新傳感器數(shù)據(jù)和重復(fù)計(jì)算過程循環(huán)，自主定 位系統(tǒng)即可實(shí)時(shí)獲取機(jī)器人位姿。
傳感器數(shù)據(jù)處理過程中，由于傳感器測量的不確定性、對(duì)機(jī)器人運(yùn) 動(dòng)過程缺乏精確的描述、環(huán)境信息描述的缺失等原因，傳感器數(shù)據(jù)被大 量的噪聲污染；傳感器數(shù)據(jù)中含有的誤差和噪聲在進(jìn)行時(shí)間積分時(shí)會(huì)被 放大，機(jī)器人運(yùn)動(dòng)時(shí)產(chǎn)生的振動(dòng)和沖擊為傳感器輸出數(shù)據(jù)的無效分量， 這些因素降低了測量數(shù)據(jù)的可信度。三維數(shù)字羅盤在工作過程中，環(huán)境
中鐵磁性材料和干擾電磁場都對(duì)三維數(shù)字羅盤測量的準(zhǔn)確性產(chǎn)生嚴(yán)重影 響，尤其是電磁場的干擾對(duì)三維數(shù)字羅盤的干擾很難徹底消除。因此， 必須對(duì)傳感器數(shù)據(jù)進(jìn)行各種處理。數(shù)據(jù)處理子系統(tǒng)采用小波變換和卡爾 曼濾波組合的方法對(duì)傳感器數(shù)據(jù)預(yù)處理和傳感器數(shù)據(jù)融合后，經(jīng)航跡推 算計(jì)算出機(jī)器人位姿，并根據(jù)不斷更新的傳感器數(shù)據(jù)循環(huán)上述計(jì)算過程，
即可實(shí)時(shí)獲取機(jī)器人位姿信息；電壓轉(zhuǎn)換模塊提供符合各子系統(tǒng)和模塊
的工作電壓。
本發(fā)明的自主定位系統(tǒng)主要針對(duì)應(yīng)用于地下環(huán)境作業(yè)機(jī)器人自主定 位，可用于煤礦、隧道、溶洞等地下環(huán)境工作智能機(jī)器人(如機(jī)器人，盾 構(gòu)機(jī)，地鐵機(jī)車等)自主定位，為智能機(jī)器人在地下環(huán)境完成勘測、探險(xiǎn) 和搜索等任務(wù)提供有效的位姿信息，解決了智能機(jī)器人在地下環(huán)境作業(yè) 的自主定位問題，為智能機(jī)器人完成預(yù)期任務(wù)提供了決策依據(jù)。


圖1本發(fā)明總體框圖2本發(fā)明安裝平臺(tái)結(jié)構(gòu)示意圖3數(shù)據(jù)處理子系統(tǒng)流程示意圖4 (a)數(shù)據(jù)處理子系統(tǒng)數(shù)據(jù)預(yù)處理模塊處理流程示意圖； 圖4 (b)數(shù)據(jù)處理子系統(tǒng)數(shù)據(jù)融合模塊處理流程示意圖； 圖5航跡推算模塊處理流程示意圖； 圖6電壓轉(zhuǎn)換模塊原理圖。
具體實(shí)施例方式
圖1為本發(fā)明的總體框圖，本發(fā)明包括安裝平臺(tái)、設(shè)置于安裝平臺(tái) 上的傳感器子系統(tǒng)、數(shù)據(jù)處理子系統(tǒng)以及電壓轉(zhuǎn)換模塊，數(shù)據(jù)處理子系 統(tǒng)的中央控制器l具有RS-232接口和以太網(wǎng)接口，利用這些接口，自主 定位系統(tǒng)將機(jī)器人定位信息實(shí)時(shí)傳輸?shù)街付ㄔO(shè)備；中央控制器1內(nèi)部裝 載有數(shù)據(jù)預(yù)處理模塊8、數(shù)據(jù)融合模塊9和航跡推算模塊10。傳感器子 系統(tǒng)2包括慣性傳感器模塊3、三維數(shù)字羅盤4，慣性傳感器模塊3和三 維數(shù)字羅盤4內(nèi)部分別集成了內(nèi)部處理器，內(nèi)部處理器將傳感器測量數(shù) 據(jù)轉(zhuǎn)換為計(jì)算機(jī)能直接傳輸和處理的數(shù)字信號(hào)，并通過RS-232C接口輸 出到中央控制器1。慣性傳感器模塊參考坐標(biāo)系由光纖陀螺儀和三維加速 度計(jì)的敏感軸構(gòu)成，三維數(shù)字羅盤4的敏感軸構(gòu)成的參考坐標(biāo)系與慣性 傳感器模塊參考坐標(biāo)系三個(gè)敏感軸始終平行，將自主定位系統(tǒng)參考坐標(biāo) 系設(shè)置為與慣性傳感器模塊參考坐標(biāo)系重合。因此，光纖陀螺儀、三維 加速度計(jì)和三維數(shù)字羅盤測量信息是機(jī)器人在自主定位系統(tǒng)參考坐標(biāo)系 內(nèi)的角速度向量、線加速度向量和姿態(tài)向量，然后通過數(shù)據(jù)處理子系統(tǒng) 實(shí)現(xiàn)機(jī)器人的自主定位功能。安裝平臺(tái)的電機(jī)驅(qū)動(dòng)模塊5接收中央控制 器1的控制信號(hào)，并轉(zhuǎn)換為直流電機(jī)7的驅(qū)動(dòng)電壓，從而實(shí)現(xiàn)傳感器標(biāo) 定。電壓轉(zhuǎn)換模塊6提供定位系統(tǒng)所需的工作電壓。
本發(fā)明安裝平臺(tái)如圖2所示，包括具有鎖緊裝置的轉(zhuǎn)臺(tái)、固定基座 和電機(jī)驅(qū)動(dòng)模塊。轉(zhuǎn)臺(tái)由蓋板11和半封閉式鋁筒12構(gòu)成封閉空間，三 維數(shù)字羅盤4固定安裝在蓋板11上部。半封閉式鋁筒12內(nèi)部安裝慣性 傳感器模塊3，慣性傳感器模塊3和三維數(shù)字羅盤4兩種傳感器參考坐標(biāo) 系對(duì)應(yīng)軸之間的平行度誤差由制造和安裝控制在±0.5°。轉(zhuǎn)臺(tái)在電機(jī)驅(qū)動(dòng) 下能繞自身軸線作±360°范圍內(nèi)轉(zhuǎn)動(dòng)，轉(zhuǎn)動(dòng)誤差控制在±0.1°。固定基座為 基座蓋板13和底座14形成密封腔式結(jié)構(gòu)，內(nèi)部安裝有電機(jī)驅(qū)動(dòng)模塊5 和中央控制器l，底座14可與機(jī)器人本體通過螺栓固聯(lián)。電機(jī)驅(qū)動(dòng)模塊 5包括附編碼盤的直流電機(jī)7、電機(jī)固定支座15、傳動(dòng)軸16，然后通過 傳動(dòng)軸16與轉(zhuǎn)臺(tái)的半封閉式鋁筒12下部固定聯(lián)接；鎖緊裝置17與半封 閉式鋁筒12固聯(lián)，然后通過螺釘與固定基座實(shí)現(xiàn)鎖緊與解鎖。安裝平臺(tái)、 除直流電機(jī)7外的電機(jī)驅(qū)動(dòng)系統(tǒng)組件以及所有固聯(lián)螺栓材質(zhì)為非鐵磁性
材料。除三維數(shù)字羅盤外，安裝平臺(tái)的轉(zhuǎn)臺(tái)、固定基座以及直流電機(jī)等 組件外部均包覆磁性屏蔽材料，以隔離慣性傳感器、直流電機(jī)對(duì)三維數(shù) 字羅盤和中央控制器的電磁干擾。
如圖3所示，數(shù)據(jù)處理子系統(tǒng)的中央控制器是實(shí)現(xiàn)傳感器數(shù)據(jù)處理
的硬件基礎(chǔ)，其內(nèi)部裝載有數(shù)據(jù)預(yù)處理模塊8、數(shù)據(jù)融合模塊9和航跡推 算模塊10，通過對(duì)傳感器原始數(shù)據(jù)進(jìn)行預(yù)處理、融合以及航跡推算，實(shí) 時(shí)獲取機(jī)器人在運(yùn)動(dòng)過程中的位姿信息。
數(shù)據(jù)預(yù)處理模塊8的具體流程如圖4 (a)所示；數(shù)據(jù)預(yù)處理模塊的 目的是針對(duì)自主定位系統(tǒng)特點(diǎn)、使用環(huán)境和傳感器精度水平、誤差特點(diǎn)， 利用算法對(duì)傳感器數(shù)據(jù)預(yù)處理，去除或削弱干擾噪聲對(duì)傳感器精度的影 響，從而為提高機(jī)器人定位精度做好準(zhǔn)備。傳感器數(shù)據(jù)預(yù)處理的主要方 法是多通道多分辨小波分解與重構(gòu)，即采用多分辨小波分解，剔除傳感 器數(shù)據(jù)中的高頻分量，僅提取最終的低頻分量重構(gòu)傳感器數(shù)據(jù)。經(jīng)預(yù)處 理后，傳感器數(shù)據(jù)具有真實(shí)反應(yīng)環(huán)境和傳感器隨時(shí)間變化的特點(diǎn)，更能 反應(yīng)機(jī)器人的運(yùn)動(dòng)狀態(tài)。加速度計(jì)數(shù)據(jù)經(jīng)數(shù)據(jù)預(yù)處理模塊后，其高頻噪 聲已經(jīng)大大降低或剔除，環(huán)境造成的振動(dòng)和沖擊干擾能有效被隔離。光 纖陀螺儀和三維數(shù)字羅盤預(yù)處理數(shù)據(jù)經(jīng)數(shù)據(jù)融合模塊融合，獲得機(jī)器人 自主定位系統(tǒng)的姿態(tài)向量。
數(shù)據(jù)融合模塊9的具體流程如圖4 (b)所示；數(shù)據(jù)融合模塊的核心 算法是自適應(yīng)卡爾曼融合濾波算法，即根據(jù)傳感器預(yù)處理數(shù)據(jù)及其統(tǒng)計(jì) 特性，利用最優(yōu)估計(jì)的方法估算機(jī)器人姿態(tài)信息。經(jīng)傳感器數(shù)據(jù)預(yù)處理 和融合后，傳感器數(shù)據(jù)精度得到很好的改善，由此計(jì)算出的自主定位系 統(tǒng)的精度大大提高。
如圖5所示，機(jī)器人自主定位系統(tǒng)的航跡推算模塊10是一個(gè)實(shí)時(shí)更 新和循環(huán)計(jì)算模塊，其主要功能是根據(jù)預(yù)先設(shè)定的機(jī)器人初始位姿、地 理環(huán)境(包括經(jīng)度、緯度、引力場等)、上一時(shí)刻機(jī)器人速度和位姿信息以
及傳感器更新數(shù)據(jù)等，實(shí)時(shí)計(jì)算機(jī)器人相對(duì)于初始點(diǎn)的位姿信息，并不 斷更新機(jī)器人位姿。航跡推算模塊采用相對(duì)定位方式，輸出的是機(jī)器人 相對(duì)于初始點(diǎn)的位姿。航跡推算模塊的推算過程如下
(1) 初始化系統(tǒng)參數(shù)，包括初始點(diǎn)地理坐標(biāo)系，地理坐標(biāo)系下機(jī)器 人初始角速度向量、加速度向量、線速度向量、位置向量、當(dāng)?shù)亟?jīng)緯度 以及引力場等；
(2) 建立機(jī)器人自主定位系統(tǒng)坐標(biāo)系；
(3) 根據(jù)(1)、 (2)，對(duì)機(jī)器人自主定位系統(tǒng)進(jìn)行初始對(duì)準(zhǔn)；
(4) 計(jì)算方向變換矩陣；
(5) 根據(jù)前一時(shí)刻機(jī)器人位姿和傳感器數(shù)據(jù)預(yù)處理與融合更新，計(jì) 算當(dāng)前時(shí)刻地理坐標(biāo)系下機(jī)器人運(yùn)動(dòng)狀態(tài)參數(shù)，包括地理坐標(biāo)系下線加 速度向量、相對(duì)于地理坐標(biāo)系的姿態(tài)向量、速度向量和位移向量；
(6) 機(jī)器人自主定位數(shù)據(jù)存儲(chǔ)和更新，定位數(shù)據(jù)通過通信接口傳輸 到外部設(shè)備，更新后的速度向量、位移向量、姿態(tài)向量進(jìn)入步驟(4)進(jìn) 行循環(huán)。
如圖6所示，電壓轉(zhuǎn)換模塊為地下環(huán)境機(jī)器人自主定位系統(tǒng)提供滿 足要求的電壓。該自主定位系統(tǒng)可使用電壓范圍為+24V +35V的車載直 流電源，自主定位系統(tǒng)內(nèi)部各傳感器、中央控制器以及電機(jī)驅(qū)動(dòng)模塊等 所需的電源電壓為+5V、 +12V兩種。電壓轉(zhuǎn)換模塊通過標(biāo)準(zhǔn)聯(lián)接頭 CON2、開關(guān)S,與車載直流電源相連，經(jīng)7812芯片U,和7805芯片U2 提將車載直流電源分別轉(zhuǎn)換為+12V和+5V電壓，其中圖中GND為共地
上山頓。
本實(shí)例的具體實(shí)施例
機(jī)器人自主定位系統(tǒng)外形最大外形尺寸為250mmx250mmx320mm (長x寬x高)。
慣性傳感器模塊采用北京星網(wǎng)迅達(dá)科技發(fā)展有限公司研發(fā)的XW
VG7200。
三維數(shù)字羅盤美國Honeywell公司的HMR3000三維數(shù)字羅盤。 中央控制器采用研華科技股份有限公司的PCM-3341嵌入式系統(tǒng)，
該系統(tǒng)具備數(shù)據(jù)處理的軟硬件條件，且具有4個(gè)RS-232接口和1個(gè)以太
網(wǎng)接口。數(shù)字I/O模塊PC3730提供A/D轉(zhuǎn)換接口。
電壓轉(zhuǎn)換模塊:采用7805芯片和7812芯片為自主定位系統(tǒng)提供+5V、
+12V電壓。
電機(jī)驅(qū)動(dòng)模塊采用附帶旋轉(zhuǎn)編碼器的直流電機(jī)，直流電機(jī)的工作
電壓為+12V。電機(jī)的控制信號(hào)來自處理器的A/D接口，經(jīng)驅(qū)動(dòng)電路驅(qū)動(dòng) 電機(jī)。
數(shù)據(jù)處理子系統(tǒng)利用多分辨小波變換重構(gòu)傳感器數(shù)據(jù)低頻量的方
法，有效剔除噪聲和干擾。光纖陀螺儀和三維數(shù)字羅盤數(shù)據(jù)經(jīng)預(yù)處理后 由卡爾曼融合濾波算法融合，獲得機(jī)器人自主定位系統(tǒng)的姿態(tài)向量，并 作為下一時(shí)刻方向變換矩陣的姿態(tài)參數(shù)。加速度計(jì)數(shù)據(jù)經(jīng)預(yù)處理和方向 變換矩陣后進(jìn)入航跡推算模塊。而且，隨著傳感器數(shù)據(jù)的更新，數(shù)據(jù)處 理子系統(tǒng)不斷循環(huán)，實(shí)時(shí)計(jì)算機(jī)器人位姿。
權(quán)利要求
1.一種機(jī)器人自主定位系統(tǒng)，包括安裝平臺(tái)、設(shè)置于安裝平臺(tái)上的傳感器子系統(tǒng)、數(shù)據(jù)處理子系統(tǒng)以及電壓轉(zhuǎn)換模塊，安裝平臺(tái)同時(shí)為傳感器子系統(tǒng)標(biāo)定提供所需的采樣點(diǎn)；傳感器子系統(tǒng)實(shí)時(shí)感測機(jī)器人運(yùn)動(dòng)狀態(tài)信息，經(jīng)數(shù)據(jù)處理子系統(tǒng)進(jìn)行去噪、濾波、融合和計(jì)算，得出機(jī)器人的位姿和速度信息；電壓轉(zhuǎn)換模塊將車載直流電源轉(zhuǎn)換為所需的工作電壓；其特征在于所述安裝平臺(tái)上部為承載傳感器子系統(tǒng)的轉(zhuǎn)臺(tái)，轉(zhuǎn)臺(tái)運(yùn)動(dòng)范圍為±360°，由電機(jī)驅(qū)動(dòng)模塊驅(qū)動(dòng)，運(yùn)動(dòng)誤差控制在±0.1°以內(nèi)；安裝平臺(tái)下部的固定基座為密封腔式結(jié)構(gòu)，內(nèi)部安裝數(shù)據(jù)處理子系統(tǒng)和電機(jī)驅(qū)動(dòng)模塊；所述安裝平臺(tái)具有鎖緊裝置，鎖緊裝置的一端與轉(zhuǎn)臺(tái)底部固定連接，鎖緊裝置另一端通過可分離螺桿與安裝平臺(tái)下部的固定基座鎖緊；在安裝平臺(tái)上轉(zhuǎn)臺(tái)需要運(yùn)動(dòng)時(shí)，螺桿與安裝平臺(tái)下部固定基座分離，轉(zhuǎn)臺(tái)旋轉(zhuǎn)，傳感器子系統(tǒng)標(biāo)定結(jié)束后，螺桿與安裝平臺(tái)下部固定基座鎖緊；所述傳感器子系統(tǒng)包括慣性傳感器模塊和三維數(shù)字羅盤，慣性傳感器模塊由三維加速度計(jì)及固聯(lián)在其三個(gè)敏感軸上相互正交的光纖陀螺儀組成，測量慣性傳感器模塊參考坐標(biāo)系相對(duì)于地球慣性坐標(biāo)系運(yùn)動(dòng)角速度和加速度；三維數(shù)字羅盤測量三維數(shù)字羅盤參考坐標(biāo)系相對(duì)地理坐標(biāo)系的三維姿態(tài)；所述數(shù)據(jù)處理子系統(tǒng)包括中央控制器及裝載于其內(nèi)部的數(shù)據(jù)預(yù)處理模塊、數(shù)據(jù)融合模塊和航跡推算模塊；中央控制器具有多路RS-232串口和以太網(wǎng)網(wǎng)絡(luò)接口，利用這些接口分別與傳感器子系統(tǒng)和外部設(shè)備進(jìn)行通信；數(shù)據(jù)預(yù)處理模塊采用小波變換方法分別對(duì)光纖陀螺儀、三維加速度計(jì)和三維數(shù)字羅盤的數(shù)據(jù)預(yù)處理，降低數(shù)據(jù)噪聲、隔離振動(dòng)與沖擊；數(shù)據(jù)融合模塊利用卡爾曼算法融合光纖陀螺儀和三維數(shù)字羅盤經(jīng)過預(yù)處理的數(shù)據(jù)，獲取安裝平臺(tái)的三維姿態(tài)信息；航跡推算模塊根據(jù)安裝平臺(tái)所在地的經(jīng)緯度和引力場向量、初始速度向量或各個(gè)時(shí)刻的速度和位姿向量，采用相對(duì)定位方式計(jì)算并更新安裝平臺(tái)實(shí)時(shí)位姿和速度信息。
2. 如權(quán)利要求l所述的機(jī)器人自主定位系統(tǒng)，其特征在于，所述數(shù) 據(jù)預(yù)處理模塊的處理步驟為(1)對(duì)光纖陀螺儀、三維加速度計(jì)和三維 數(shù)字羅盤的數(shù)據(jù)進(jìn)行多通道多分辨小波分解；(2)對(duì)前述分解數(shù)據(jù)中的 高頻分量，采取強(qiáng)制消噪處理；(3)提取前述分解數(shù)據(jù)中的低頻分量進(jìn) 行多通道多分辨小波重構(gòu)；(4)輸出經(jīng)預(yù)處理后的數(shù)據(jù)。
3. 如權(quán)利要求l所述的機(jī)器人自主定位系統(tǒng)，其特征在于，所述數(shù) 據(jù)融合模塊的處理步驟為(1)對(duì)光纖陀螺儀和三維數(shù)字羅盤的數(shù)據(jù)計(jì) 算統(tǒng)計(jì)學(xué)參數(shù)；(2)實(shí)時(shí)更新卡爾曼濾波器參數(shù)；(3)自適應(yīng)卡爾曼濾 波；(4)計(jì)算自主定位系統(tǒng)姿態(tài)參數(shù)，將其返回步驟(2)。
4. 如權(quán)利要求l所述的機(jī)器人自主定位系統(tǒng)，其特征在于，所述航 跡推算模塊的處理步驟為(1)初始化系統(tǒng)參數(shù)，包括初始點(diǎn)地理坐標(biāo) 系，地理坐標(biāo)系下機(jī)器人初始角速度向量、加速度向量、線速度向量、 位置向量、當(dāng)?shù)亟?jīng)緯度以及引力場；(2) 建立機(jī)器人自主定位系統(tǒng)坐標(biāo)系；(3) 根據(jù)步驟(1)、 (2)，對(duì)機(jī)器人自主定位系統(tǒng)進(jìn)行初始對(duì)準(zhǔn)；(4) 計(jì)算方向變換矩陣；(5) 根據(jù)前一時(shí)刻機(jī)器人位姿和傳感器數(shù)據(jù)預(yù)處理與融合更新，計(jì) 算當(dāng)前時(shí)刻地理坐標(biāo)系下機(jī)器人運(yùn)動(dòng)狀態(tài)參數(shù)，包括地理坐標(biāo)系下線加 速度向量、相對(duì)于地理坐標(biāo)系的姿態(tài)向量、速度向量和位移向量；(6) 機(jī)器人自主定位數(shù)據(jù)存儲(chǔ)和更新，定位數(shù)據(jù)通過通信接口傳輸?shù)酵獠吭O(shè)備，更新后的速度向量、位移向量、姿態(tài)向量進(jìn)入步驟(4)進(jìn)行循環(huán)。
5. 如權(quán)利要求l、 2、 3或4所述的機(jī)器人自主定位系統(tǒng)，其特征在 于所述電機(jī)驅(qū)動(dòng)模塊由固定在支座上的附編碼盤的直流電機(jī)構(gòu)成，直 流電機(jī)通過傳動(dòng)軸與轉(zhuǎn)臺(tái)下部固定聯(lián)接；
6. 如權(quán)利要求5所述的機(jī)器人自主定位系統(tǒng)，其特征在于所述安 裝平臺(tái)主體材質(zhì)為鋁合金，聯(lián)接螺栓為銅質(zhì)螺栓，除三維數(shù)字羅盤外安 裝平臺(tái)的整體采用磁性屏蔽材料包覆，以隔離電磁場對(duì)三維數(shù)字羅盤的 干擾。
全文摘要
機(jī)器人自主定位系統(tǒng)，屬于機(jī)器人智能控制裝置，解決機(jī)器人在地下環(huán)境作業(yè)的高精度自主定位問題。本發(fā)明包括安裝平臺(tái)、設(shè)置于安裝平臺(tái)上的傳感器子系統(tǒng)、數(shù)據(jù)處理子系統(tǒng)以及電壓轉(zhuǎn)換模塊；安裝平臺(tái)上部為承載傳感器子系統(tǒng)的轉(zhuǎn)臺(tái)，安裝平臺(tái)下部的固定基座為密封腔式結(jié)構(gòu)，內(nèi)部安裝數(shù)據(jù)處理子系統(tǒng)和電機(jī)驅(qū)動(dòng)模塊；傳感器子系統(tǒng)包括慣性傳感器模塊和三維數(shù)字羅盤，數(shù)據(jù)處理子系統(tǒng)包括中央控制器及裝載于其內(nèi)部的數(shù)據(jù)預(yù)處理模塊、數(shù)據(jù)融合模塊和航跡推算模塊；本發(fā)明可用于在煤礦、隧道、溶洞等地下環(huán)境工作的智能機(jī)器人，如機(jī)器人，盾構(gòu)機(jī)，地鐵機(jī)車等的自主定位，為智能機(jī)器人在地下環(huán)境完成勘測、探險(xiǎn)和搜索等任務(wù)提供有效的位姿信息。
文檔編號(hào)G05D1/02GK101201626SQ200710168718
公開日2008年6月18日 申請(qǐng)日期2007年12月10日 優(yōu)先權(quán)日2007年12月10日
發(fā)明者尹周平, 熊有倫, 熊蔡華, 韓冬桂 申請(qǐng)人:華中科技大學(xué)