全向底盤機器人智能定位和姿態檢測系統

趙晉秀 劉文杰

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

關鍵詞： 全向底盤機器人;正交編碼器;陀螺儀;智能定位;姿態檢測;PID權重

中圖分類號：TP242? ? 文獻標識碼：A? ? 文章編號：2095-8412 （2020） 05-033-05

工業技術創新 URL： http：//gyjs.cbpt.cnki.net? ? DOI： 10.14103/j.issn.2095-8412.2020.05.007

引言

21世紀以來，智能機器人技術發展迅速，輪式機器人以結構簡單、載重量大、性能穩定、轉向靈活等特點，成為一種應用廣泛的機器人[1]。全向底盤機器人是一種典型的輪式機器人，能夠通過狹窄空間，實現邊走邊自轉、弧線運動等高難度動作，以高度的機動性在各大機器人賽事、服務型機器人設計、搬運機器人應用中備受青睞[2]。

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

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

1? 系統總體設計

1.1? 全向底盤

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

1.2? 陀螺儀

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

1.3? 正交編碼器

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

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

1.4? 主控

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

其中，為全向輪半徑。根據路程和姿態角度，按照定位算法，計算出機器人當前坐標，并按照目標坐標，給直流電機相應信號，驅使機器人運動。通過SPI串口，其他芯片可以給定目標坐標和姿態，也可將全向底盤數據傳給其他芯片。

2? 機器人運動坐標系建立

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

3? 機器人定位算法

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

取一小段路程，基于直線運動假設，陀螺儀讀數為一定值。設當前陀螺儀讀數為，即機器人與坐標系角度為。

得到剩余路程和角度。將、方向剩余路程差值和航向角差值分別用PID處理并疊加后，傳送給直流電機，直流電機據此運轉，即可到達指定坐標和姿態。

4? 應用與討論

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

使用傳統PID算法進行坐標矯正總會出現超調和在目標點附近震蕩的情況，這是PID算法的固有缺陷。解決方法如下：

（1）使用分級PID。根據距離目標點遠近改變PID值。使用多套PID，根據距離選擇合適的PID值進行調節。

（2）改變PID權重。根據距離目標點遠近改變PID影響直流電機驅動的權重。將距離變量與PID值關聯，越靠近目標點，P權重越小，I、D權重越大，并且一段時間清零一次累計誤差。

經實驗，分級PID方法簡單，但是調試流程繁瑣。最終決定采用改變PID權重方法。

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

當誤差低于5 mm/m時，程序將難以跳出循環，全向底盤會在定點附近震蕩，原因如下：

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

（2）由于I、D均依靠累計誤差進行速度調節，因此當直流電機速度達到了可以使全向底盤移動的速度時，累計誤差已經較大，必將出現超調現象。

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

5? 結束語

本文設計了一種基于正交編碼器和陀螺儀的全向底盤機器人智能定位和姿態檢測系統，可以直接通過主控程序給出目標坐標，也可由其他芯片通過SPI串口給出目標坐標，機器人將智能移動到達指定坐標位置。這種模塊化控制方法更加靈活，使得機器人開發更加簡便。控制程序使用了PID算法，且PID的值隨距離目標坐標遠近而調節，通過實時計算坐標、實時檢測機器人姿態，并與目標坐標和姿態進行對比，最大程度地保障了運行平穩度和精度。經實驗，這種全向底盤機器人誤差約為5 mm/m，滿足機器人競賽和室內物料搬運等應用場景的精度要求。

參考文獻

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

[2] 趙偉雄. 全方位移動機器人控制系統研究與實現[D]. 邯鄲：河北工程大學， 2016.

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

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

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

作者簡介：

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

E-mail： 375400964@qq.com

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