全向底盤機器人智能定位和姿態(tài)檢測系統(tǒng)

趙晉秀 劉文杰

摘? ?要： 基于正交編碼器、陀螺儀等傳感裝置，為全向底盤機器人設(shè)計一套智能定位和姿態(tài)檢測系統(tǒng)。將陀螺儀安裝于全向底盤轉(zhuǎn)動中心處，通過對角速度進(jìn)行積分運算，得到行駛軌跡偏離原坐標(biāo)系的角度;將編碼器正交布置，作為機器人測速、計路程傳感器;使用定位算法，確認(rèn)機器人所處位置的坐標(biāo)和姿態(tài)信息，由主控芯片作出判斷，控制機器人到達(dá)目標(biāo)坐標(biāo)點和姿態(tài)。制作了實物模型，進(jìn)行了系統(tǒng)應(yīng)用，通過改變PID權(quán)重初步解決了PID算法造成全向底盤震蕩的問題。系統(tǒng)誤差僅為5 mm/m左右，可滿足機器人競賽和室內(nèi)物料搬運等應(yīng)用場景的精度要求。

關(guān)鍵詞： 全向底盤機器人;正交編碼器;陀螺儀;智能定位;姿態(tài)檢測;PID權(quán)重

中圖分類號：TP242? ? 文獻(xiàn)標(biāo)識碼：A? ? 文章編號：2095-8412 （2020） 05-033-05

工業(yè)技術(shù)創(chuàng)新 URL： http：//gyjs.cbpt.cnki.net? ? DOI： 10.14103/j.issn.2095-8412.2020.05.007

引言

21世紀(jì)以來，智能機器人技術(shù)發(fā)展迅速，輪式機器人以結(jié)構(gòu)簡單、載重量大、性能穩(wěn)定、轉(zhuǎn)向靈活等特點，成為一種應(yīng)用廣泛的機器人[1]。全向底盤機器人是一種典型的輪式機器人，能夠通過狹窄空間，實現(xiàn)邊走邊自轉(zhuǎn)、弧線運動等高難度動作，以高度的機動性在各大機器人賽事、服務(wù)型機器人設(shè)計、搬運機器人應(yīng)用中備受青睞[2]。

全向底盤在提高機動性的同時，也給坐標(biāo)定位和姿態(tài)檢測帶來了困難[3]。使用攝像機、激光雷達(dá)、微波雷達(dá)、超聲波、紅外線等傳感器對機器人進(jìn)行定位，易受工作環(huán)境等外部因素的干擾，具有一定的局限性[4-5]。

據(jù)此，本文基于正交編碼器、陀螺儀等傳感裝置，為全向底盤機器人設(shè)計一套智能定位和姿態(tài)檢測系統(tǒng)。系統(tǒng)采用STM32F407芯片作為坐標(biāo)姿態(tài)獲取和驅(qū)動控制芯片，對正交編碼器和陀螺儀信號進(jìn)行分析，結(jié)合使用定位算法，得到機器人坐標(biāo)和姿態(tài);使用SPI串口向其他芯片傳送全向底盤數(shù)據(jù)，同時對當(dāng)前坐標(biāo)和姿態(tài)與目標(biāo)坐標(biāo)和姿態(tài)進(jìn)行對比，計算得到全向底盤上每個直流電機的占空比，實現(xiàn)全向底盤運行，直至到達(dá)目標(biāo)坐標(biāo)和姿態(tài)。

1? 系統(tǒng)總體設(shè)計

1.1? 全向底盤

全向底盤機器人使用3個或3個以上全向輪或4個麥克納姆輪實現(xiàn)平面內(nèi)的全向移動。全向輪如圖1所示，由輪轂和從動輪等構(gòu)成。輪轂的外圓周處均勻開設(shè)3個或3個以上輪轂齒，每兩個輪轂齒之間裝設(shè)有一從動輪，各從動輪的徑向方向與輪轂外圓周的切線方向垂直。全向底盤結(jié)構(gòu)如圖2所示，為保障全向底盤有足夠的驅(qū)動力和機動性，本方案在全向底盤上安裝四個獨立直流電機，分別通過聯(lián)軸器與全向輪連接，各個直流電機和全向輪采用中心對稱式結(jié)構(gòu)。如此，全向底盤可以輕松實現(xiàn)平面全向移動和原地自轉(zhuǎn)。

1.2? 陀螺儀

陀螺儀最早應(yīng)用于航海領(lǐng)域，后來在航空航天領(lǐng)域也有較多應(yīng)用，非常適合導(dǎo)航、定位等系統(tǒng)。本方案采用的MPU6050陀螺儀通過對角速度進(jìn)行積分運算，得到行駛軌跡偏離原坐標(biāo)系的角度，多用于機器人、無人機等裝備的姿態(tài)檢測。主控使用yaw（航向角）作為全向底盤姿態(tài)數(shù)據(jù)。出于姿態(tài)調(diào)整的實際需要，全向底盤會發(fā)生自轉(zhuǎn)，故陀螺儀安裝在全向底盤轉(zhuǎn)動中心處為佳。在全向底盤上做一標(biāo)記，給直流電機一定速度，使全向底盤勻速轉(zhuǎn)動，當(dāng)目視標(biāo)記處于轉(zhuǎn)動中心處時，即可確定陀螺儀安放位置。

1.3? 正交編碼器

編碼器又稱作碼盤，是目前廣泛使用的測速測位移傳感器。傳統(tǒng)三輪全向底盤使用直流電機自帶的編碼器測速、計路程。此時編碼器和直流電機本體作為整體，當(dāng)動力輪打滑或懸空時，編碼器仍在計數(shù)，其帶來的系統(tǒng)誤差較大，會對定位精度產(chǎn)生嚴(yán)重影響。鑒于此，本方案使用2個獨立的歐姆龍E6A2-CW5C編碼器作為機器人測速、計路程傳感器。每個編碼器內(nèi)部有兩對光電耦合器，輸出相位差為90°的兩路脈沖序列。正轉(zhuǎn)和反轉(zhuǎn)時，兩路脈沖的超前、滯后關(guān)系剛好相反。由圖3可知，在B相脈沖的上升沿，正轉(zhuǎn)和反轉(zhuǎn)對應(yīng)的A相脈沖的電平高低剛好相反，因此使用AB相編碼器，可以很容易地識別出轉(zhuǎn)軸旋轉(zhuǎn)的方向，并據(jù)此對坐標(biāo)調(diào)整進(jìn)行判斷。

所使用歐姆龍E6A2-CW5C編碼器為增量式編碼器，規(guī)格是500P/R，即轉(zhuǎn)一圈可產(chǎn)生500個脈沖，將脈沖傳向主控即可得知編碼器所轉(zhuǎn)圈數(shù)。在編碼器上連接小全向輪，正交后可記錄全向底盤在X和Y方向的行程。為使正交編碼器輪子與地面緊密貼合，減少因全向輪打滑和空轉(zhuǎn)對精度產(chǎn)生的影響，正交編碼器與全向底盤使用彈簧減震器連接，結(jié)構(gòu)如圖4所示（其中彈簧已省略，小全向輪從易）。

1.4? 主控

主控是機器人的核心部件，負(fù)責(zé)接收各個傳感器的信號、對信號進(jìn)行相應(yīng)的處理、對處理結(jié)果作出反應(yīng)等。本方案采用STM32F407芯片作為全向底盤主控芯片，負(fù)責(zé)接收產(chǎn)生于編碼器的脈沖信號和陀螺儀角速度信息，將角速度進(jìn)行積分，即可得到當(dāng)前姿態(tài)角度。STM32F407芯片將定時器設(shè)置為編碼器模式，通過訪問對應(yīng)寄存器，可實時監(jiān)測編碼器所轉(zhuǎn)過的圈數(shù)和接收到的脈沖數(shù)，從而得到編碼器走過路程為

其中，為全向輪半徑。根據(jù)路程和姿態(tài)角度，按照定位算法，計算出機器人當(dāng)前坐標(biāo)，并按照目標(biāo)坐標(biāo)，給直流電機相應(yīng)信號，驅(qū)使機器人運動。通過SPI串口，其他芯片可以給定目標(biāo)坐標(biāo)和姿態(tài)，也可將全向底盤數(shù)據(jù)傳給其他芯片。

2? 機器人運動坐標(biāo)系建立

為適應(yīng)小型機器人結(jié)構(gòu)需要和安裝便捷需求，將正交編碼器安裝于直流電機間隙處，如圖2所示全向底盤結(jié)構(gòu)。坐標(biāo)系建立如圖5所示，正交編碼器法線交點即為坐標(biāo)零點。啟動機器人時，陀螺儀初始化，約定逆時針轉(zhuǎn)動為正，順時針轉(zhuǎn)動為負(fù)。機器人正前所指方向即為0°方向，設(shè)此0°方向為軸正方向，順時針旋轉(zhuǎn)90°為軸正方向。陀螺儀偏轉(zhuǎn)方向即為機器人相對于坐標(biāo)系的偏轉(zhuǎn)方向。

3? 機器人定位算法

STM32F407芯片僅供全向底盤使用，執(zhí)行功能較少。芯片約5 ms記錄一次數(shù)據(jù)，因此可將機器人路線離散成無數(shù)個5 ms路程。由于時間間隔較小，陀螺儀角度變化微小，所以每一小段路程均可以看作直線運動。

取一小段路程，基于直線運動假設(shè)，陀螺儀讀數(shù)為一定值。設(shè)當(dāng)前陀螺儀讀數(shù)為，即機器人與坐標(biāo)系角度為。

得到剩余路程和角度。將、方向剩余路程差值和航向角差值分別用PID處理并疊加后，傳送給直流電機，直流電機據(jù)此運轉(zhuǎn)，即可到達(dá)指定坐標(biāo)和姿態(tài)。

4? 應(yīng)用與討論

制作了實物模型，進(jìn)行了系統(tǒng)應(yīng)用。上電后，系統(tǒng)程序和傳感器初始化完成，建立以編碼器法線交點為坐標(biāo)原點，機器人指向方向為軸正方向，順時針旋轉(zhuǎn)90°方向為軸正方向的直角坐標(biāo)系。由全向底盤主控程序或其他芯片通過串口給出目標(biāo)坐標(biāo)和姿態(tài)，芯片將根據(jù)定位算法驅(qū)動直流電機，使全向底盤逐漸逼近目標(biāo)坐標(biāo)和姿態(tài)。機器人工作流程如圖7所示。

使用傳統(tǒng)PID算法進(jìn)行坐標(biāo)矯正總會出現(xiàn)超調(diào)和在目標(biāo)點附近震蕩的情況，這是PID算法的固有缺陷。解決方法如下：

（1）使用分級PID。根據(jù)距離目標(biāo)點遠(yuǎn)近改變PID值。使用多套PID，根據(jù)距離選擇合適的PID值進(jìn)行調(diào)節(jié)。

（2）改變PID權(quán)重。根據(jù)距離目標(biāo)點遠(yuǎn)近改變PID影響直流電機驅(qū)動的權(quán)重。將距離變量與PID值關(guān)聯(lián)，越靠近目標(biāo)點，P權(quán)重越小，I、D權(quán)重越大，并且一段時間清零一次累計誤差。

經(jīng)實驗，分級PID方法簡單，但是調(diào)試流程繁瑣。最終決定采用改變PID權(quán)重方法。

可通過程序調(diào)節(jié)定點精度，隨著定點精度的提高，機器人在目標(biāo)點的調(diào)整時間也在改變。如圖8所示，精度越高，在定點附近微調(diào)時間越長。

當(dāng)誤差低于5 mm/m時，程序?qū)㈦y以跳出循環(huán)，全向底盤會在定點附近震蕩，原因如下：

（1）由于全向底盤自重和載重限制，直流電機速度低于一定值時，全向底盤不會發(fā)生移動。

（2）由于I、D均依靠累計誤差進(jìn)行速度調(diào)節(jié)，因此當(dāng)直流電機速度達(dá)到了可以使全向底盤移動的速度時，累計誤差已經(jīng)較大，必將出現(xiàn)超調(diào)現(xiàn)象。

筆者對原因（2）進(jìn)行了優(yōu)化，每經(jīng)過一段時間便將累計誤差清零，略微提高了精度，但未予徹底解決。

5? 結(jié)束語

本文設(shè)計了一種基于正交編碼器和陀螺儀的全向底盤機器人智能定位和姿態(tài)檢測系統(tǒng)，可以直接通過主控程序給出目標(biāo)坐標(biāo)，也可由其他芯片通過SPI串口給出目標(biāo)坐標(biāo)，機器人將智能移動到達(dá)指定坐標(biāo)位置。這種模塊化控制方法更加靈活，使得機器人開發(fā)更加簡便?？刂瞥绦蚴褂昧薖ID算法，且PID的值隨距離目標(biāo)坐標(biāo)遠(yuǎn)近而調(diào)節(jié)，通過實時計算坐標(biāo)、實時檢測機器人姿態(tài)，并與目標(biāo)坐標(biāo)和姿態(tài)進(jìn)行對比，最大程度地保障了運行平穩(wěn)度和精度。經(jīng)實驗，這種全向底盤機器人誤差約為5 mm/m，滿足機器人競賽和室內(nèi)物料搬運等應(yīng)用場景的精度要求。

參考文獻(xiàn)

[1] 徐明釗， 于海濤， 楊春， 等. 基于陀螺儀和碼盤的自主定位機器人系統(tǒng)[J]. 兵工自動化， 2016， 35（10）： 60-63.

[2] 趙偉雄. 全方位移動機器人控制系統(tǒng)研究與實現(xiàn)[D]. 邯鄲：河北工程大學(xué)， 2016.

[3] 許倫輝， 朱群強， 胡發(fā)煥. 基于Euler算法的全向輪機器人位姿檢測[J]. 計算機測量與控制， 2015， 23（7）： 2295-2297， 2301.

[4] 熊家新， 劉麗， 徐鶴， 等.全向輪式移動機器人的定位導(dǎo)航算法[J]. 長春理工大學(xué)學(xué)報（自然科學(xué)版），2014（4）： 83-88.

[5] 李潔， 李果， 黃鴻， 等.全向輪機器人基于雙全向輪： 陀螺儀模式坐標(biāo)定位[J].硅谷， 2010（8）： 34.

作者簡介：

趙晉秀（2000—），通信作者，太原工業(yè)學(xué)院本科在讀，2019年華北五省機器人大賽山西賽區(qū)裁判助理，參加2020屆中國工程機器人大賽備賽，對全向底盤機器人深感興趣。

E-mail： 375400964@qq.com

（收稿日期：2020-07-29）