趙晉秀 劉文杰

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

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

中圖分類(lèi)號(hào)：TP242? ? 文獻(xiàn)標(biāo)識(shí)碼：A? ? 文章編號(hào)：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ì)以來(lái)，智能機(jī)器人技術(shù)發(fā)展迅速，輪式機(jī)器人以結(jié)構(gòu)簡(jiǎn)單、載重量大、性能穩(wěn)定、轉(zhuǎn)向靈活等特點(diǎn)，成為一種應(yīng)用廣泛的機(jī)器人[1]。全向底盤(pán)機(jī)器人是一種典型的輪式機(jī)器人，能夠通過(guò)狹窄空間，實(shí)現(xiàn)邊走邊自轉(zhuǎn)、弧線(xiàn)運(yùn)動(dòng)等高難度動(dòng)作，以高度的機(jī)動(dòng)性在各大機(jī)器人賽事、服務(wù)型機(jī)器人設(shè)計(jì)、搬運(yùn)機(jī)器人應(yīng)用中備受青睞[2]。

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

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

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

1.1? 全向底盤(pán)

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

1.2? 陀螺儀

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

1.3? 正交編碼器

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

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

1.4? 主控

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

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

2? 機(jī)器人運(yùn)動(dòng)坐標(biāo)系建立

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

3? 機(jī)器人定位算法

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

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

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

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

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

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

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

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

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

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

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

（1）由于全向底盤(pán)自重和載重限制，直流電機(jī)速度低于一定值時(shí)，全向底盤(pán)不會(huì)發(fā)生移動(dòng)。

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

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

5? 結(jié)束語(yǔ)

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

參考文獻(xiàn)

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

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

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

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

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

作者簡(jiǎn)介：

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

E-mail： 375400964@qq.com

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