雙超聲波數據融合的兩輪機器人平衡行進策略研究

舒英利， 寇 磊， 苑全德， 皮玉珍

(1.長春工程學院電氣與信息工程學院，吉林 長春 130012；2.東北電力大學信息工程學院，吉林 吉林 132012)

雙超聲波數據融合的兩輪機器人平衡行進策略研究

舒英利1， 寇 磊2， 苑全德1， 皮玉珍1

(1.長春工程學院電氣與信息工程學院，吉林 長春 130012；2.東北電力大學信息工程學院，吉林 吉林 132012)

針對移動輪式自平衡機器人在生產應用中存在的問題，對其結構特點與姿態平衡的控制策略進行了研究。為了保障機器人的平衡與協調運動，提出了基于雙超聲波測距數據融合的平衡和行進策略。采用雙超聲波數據融合平衡算法，在Arduino主控的機器人上實現了自平衡姿態的控制，對雙超聲波傳感器數據信息進行處理，從而保證了機器人的自平衡運動；同時，在機器人的運動過程中加入了PID控制算法，以調整機器人的運動狀態及加快機器人對信號誤差處理的速度。試驗與仿真結果表明，該策略設計有效可行，雙超聲波自平衡機器人可以保持較好的平衡姿態，行進控制與基本方向控制也有效可靠，體現了較好的平衡穩定性和平衡協調性。在平衡算法控制下，雙超聲波機器人不僅實現了平衡行進，還實現了行進策略下的平衡后退以及方向控制下的平衡轉彎，具有廣闊的應用前景。

機器人; 傳感器； 超聲波； 數據； 控制； 平衡； 策略

0 引言

在復雜、狹窄的大轉角工作環境下，兩輪自平衡移動式機器人可以靈活、快捷地執行任務。這種移動式機器人兩輪共軸，機身重心置于輪軸重心上方，可憑借前后運動保持機身的相對平衡，實現動態平衡的直立行走。由于具有體積小、運動靈活、適應地形變化能力強等優點，兩輪自平衡移動式機器人可完成一些狹小環境內的任務[1-2]。在復雜環境下能對環境高效感知和及時調整自身姿態，是自平衡機器人的必備功能。由于單一的傳感器(比如超聲波傳感器)感知能力有限，因此需要由多個傳感器交互實現有效協調，提高工作效率，同時在不同的應用空間與任務空間中實現多傳感器數據融合[3-5]。

為改進兩輪自平衡移動式機器人的性能，提出了兩輪自平衡機器人姿勢平衡和行進策略，利用雙超聲波測距檢測平衡與行進特征，通過不斷檢測收集超聲波傳感器與地面的距離，進行數據濾波融合，以提高兩輪自平衡機器人的控制效果[6-7]。雙超聲波檢測提高了控制精度和集成度，在保證良好的平衡姿態和移動控制效果的前提下，有效降低了整體生產成本，并運用配套的數據融合處理算法，實現了對平衡姿態與行進姿態的調控。

1 雙超聲波姿態平衡動力學模型

兩輪自平衡移動式機器人的基本平衡控制原理為:當檢測到車體產生傾斜時，對主控系統通過檢測得到的數據進行融合、得出矯正的角度值與輸出的占空比。自平衡機器人主要通過驅動2個輪子的電機所產生的轉動力矩來控制機身傾斜方向的速度v，通過控制電機控制機身平衡姿態的調整，讓兩個輪子朝機身傾斜倒下的方向運動，進而保持機身的動態平衡。平衡控制過程中，直接控制量為輪子的轉動力矩[8]。

(1)

由式(1)得速度模型：

(2)

自平衡機器人受力分析圖如圖1所示。圖1中，O為輪軸中心，P為機身重心，機體質量為m，重力加速度為g，機軸傾角為α，超聲波A直射到地面的距離為La，超聲波B直射到地面的距離為Lb，重心距機身下的平臺的距離為L。設機體以加速度a向一邊傾斜，則受到水平方向的最優慣性合力為：

∑F*=Ma

(3)

若要使機身保持平衡狀態，則∑M=0(M為力矩)。以O點為轉軸中心，支持力N作用在轉軸上，力矩M=0，即:

mgLsinθ+F*=0

(4)

圖1 自平衡機器人受力分析圖

2 雙超聲波測距控制方案

本研究中的自平衡機器人應用2個超聲波傳感器模塊檢測距離數據。選用HC-SR04超聲波測距模塊，Arduino主控板通過一階濾波的數據融合換算后得出機身當前狀態的角度值α與角速度ω，再經過PID參數整定生成對應的PWM輸出值，形成一個“雙超聲波測距、定位高度比較計算、角度值偏差計算、矯正數據融合、偏差調整-輸出控制”的自平衡實現過程。

2.1 超聲波測距原理與誤差調整

采用時間差測距的方式來完成距離測量，超聲波傳感器發射端向探頭正方向發射特定頻率的超聲波，內部電路的定時器在發射超聲波時開始計時。超聲波發射與回響檢測時序圖如圖2所示。

圖2 超聲波發射頻率時序圖

發射電路會根據所給的脈沖發出特定頻率的超聲波，超聲波碰到被測物界面則會被反射回接收端。文獻[9]采用了超聲波傳感器接收探頭接收到反射波后定時器停止計時的方法，進而得出超聲波發射和接收回波的時間差t。超聲波在傳播介質中的傳播速度為c(在空氣中為340 m/s)。通過計算，得出從TXD端到被測界面之間的距離s為：

s=(ct)/2

(5)

超聲波應用中，可由式(5)得出超聲波發出速度vi與接收端接收信號所需來回越渡時間的關系為：

(6)

由于超聲波頻率較高，可滿足多種用途的測量需要。但在低維度空間中，超聲波測量會受到周邊溫度、自身響應冗余、回波遲緩、傳播介質等因素的影響。超聲波在固體中傳播速度最快，液體次之，在氣體中傳播速度最慢。聲速c的表征速度與傳播介質的溫度有關。為了實現超聲波傳感器的精確測量，文獻[9]提出在硬件中設置溫度補償電路，以補償因溫度變化而帶來的聲速誤差。如果環境溫度變化不明顯，則無需設置溫度補償。

(7)

(8)

針對這一誤差，在滿足超聲波傳感器的主要電氣參數的條件下，本系統選用了具有回聲保護的HC-SR04超聲波模塊，并且盡可能地把超聲波模塊A、B隔離開。兩輪結構也在一定程度上阻擋了超聲波的互相干擾。

2.2 超聲波數據融合平衡算法

通過超聲波測距獲取機器人在XOY極坐標平面內角度θ的變化，以實現機身的平衡。當兩輪機器人平衡時，需滿足以下角度值換算公式：

(9)

式中：L為機身前后的2個超聲波模塊A、B間的直線距離；λ為平衡點高度。

在自平衡機器人基于雙超聲波的檢測中，假設采樣周期為100ms，在k時刻獲取超聲波實測值a、b,并結合(k-1)時刻的距離預測值。根據測量的預測協方差S對k時刻的測量值進行濾波估計修正，進而對(k+1)時刻的距離進行預測。

測距獲取的平衡角度模型如圖3所示。

圖3 測距獲取的平衡角度模型

在估計平衡姿態機器人的運動狀態時，機器人的超聲波傳感器坐標采用極坐標系。設系統的狀態向量E(k)為：

(10)

系統平衡E(k)的狀態表達式為：

研究結果顯示，‘索邦’花青素苷合成轉錄因子LhsorMYB12編碼區序列長720 bp，核苷酸序列長863 bp(登錄號：MG593164)，共編碼239個氨基酸(圖2)。

E(k)=AE(k-1)+BU(k)+W(k-1)

(11)

式中：E(k)為第k次超聲波測量時的狀態向量;A為角度值的表征參數；W為角速度的表征參數；U(k)為平衡系統的速度控制量；W(k-1)為上一次檢測(k-1)狀態的角速度對當前k狀態的噪聲干擾。

由于超聲波測距的采樣周期t為300 ms，而機器人保持平衡的運動速度較為平緩，所以在局部時間段內設定調整過程為近似勻速運動，x方向的運動方程為：

(12)

式中:tk為第k次超聲波測量狀態時的采樣周期。

(13)

同理，可得y方向的運動方程為：

(14)

由式(14)可得平衡系統的狀態方程為：

E(k)=A×E(k-1)+W(k-1)

(15)

(16)

經過PID數據整定后的PWM輸出方程為:

PWM|out=kapA+kadW+ksp×S+ksi×P

(17)

基于Arduino的雙超聲波機器人平衡驅動系統經過PID調整后，輸出實現姿態的平衡。具體的超聲波雙向測距定位平衡模型如圖4所示。由模型圖可得測量值Z(k)為：

圖4 雙向測距定位平衡模型

由于各狀態變量的誤差符合均值為0的高斯分布，故平衡策略下的測量方程為：

Z(K)=CE(k)+V(k)

(18)

式中:C為測量系統的表征參數，當系統為多測量系統時，C為矩陣;V(k)為測量系統在k狀態下的噪聲誤差。由平衡系統在極坐標下的雙超聲波測距徑向距離和融合后偏差方向角的測量數據可得:

(19)

測速碼盤對電機編碼器產生的PWM脈沖進行記數，監測機身的位移與輪子的速度。根據超聲波傳感器和姿態換算監測車體運動姿態信號，經過雙超聲波數據融合流程，文獻[11]通過相關的控制算法計算出輸出控制電壓的數值大小，對雙超聲波的2個驅動電機進行控制，進而調整車體平臺的行進姿態與平衡位置，從而使機身保持良好的運動協調性和平衡狀態[11-12]。

數據優化融合前的PWM輸出近似波形與融合優化后的實測波形如圖5所示。從波形對比中可以看出，經過數據優化融合后的波形更為穩定、平緩，姿態控制效果更好。

圖5 數據優化融合前后PWM波形對比圖

2.3 控制方案與行進策略

Arduino如今已被運用到各個領域，是一款使用Atmel Atmega328微處理控制器的開源控制板。其開發操作界面和搭建外設環境簡易，有0～13個數字模擬I/O、0～5個Analog I/O，控制效果顯著。自平衡機器人采用超聲波電路Arduino為核心控制模塊，電機驅動模塊為L298N，電機為TT馬達，并以2個HC-SR04作為超聲波測距模塊，采用7.4 V/2 200 mAh鋰電池供電。電位器用于平衡點設置與PID中kp、ki和kd系數的調試，方便數據的更新與調試，有效減少了開發時間與生產成本。

雙超聲波自平衡機器人的行進控制主要通過PWM占空比輸出控制電機轉速大小，進而調整角度值的偏差，使得角度值趨近系統平衡零點。

控制部分由無線藍牙遙控的代碼實現，功能實現為“前進狀態”、“后退狀態”、“左轉狀態”、“右轉狀態”。從程序初始化參數設置可以看出，雙超聲波自平衡機器人的行進控制方案是先調整平衡點和PID設定系數，然后調整左右電機的PWM輸出，從而實現行進角度的控制與電機控制[13]。

3 試驗與仿真

雙超聲波機器人運用了主控Arduino，以簡單、快捷的編程實現與I/O接口連接，通過雙超聲波傳感器進行環境測距定位，并利用控制算法進行機器人姿態平衡控制。在試驗驗證方面，通過仿真顯示超聲波采集到的回聲檢測數據。由超聲波接收端接收到的回聲檢測波形圖可知，平衡系統的姿態搖擺幅度不大，基本處于平衡零點上下，具備基本穩定性與數據的可計算性。

假設2個超聲波傳感器經過互補濾波后，換算出的角度數據為M1，超聲波采集數據為s，超聲波傳感器測距換算出的偏差角度值相對加速度的因子為k，前一次超聲波傳感器經過濾波后的角度數據為M2，增益選擇固定值為g。濾波后的角度數據為realM，最后得出融合出的角度數據。

試驗結果證明，雙超聲波自平衡機器人能有效保持平衡，在平衡算法控制下能平衡行進；可實現行進策略下的平衡后退和方向控制下的平衡轉彎，穩定效果好。

4 結束語

為實現基于Arduino的雙超聲波機器人的姿態平衡與行進控制，提出了雙超聲波傳感器數據融合策略。試驗與仿真結果表明，該策略有效可行，雙超聲波自平衡機器人可以保持較好的平衡姿態，行進控制與基本方向控制有效可靠，體現出了較好的平衡穩定性和平衡協調性。該策略整體硬件開發周期相對較短，選用材料性價比高，能滿足在較好控制效果的前提下降低生產成本的要求。在接下來的工作中，將對多超聲波定位自平衡機器人在上下坡和不平整復雜行動界面上實現姿態平衡進行研究。

[1] 劉斌.兩輪自平衡小車軟硬件研發與基于模糊線性化模型的變結構控制研究[D].西安：西安電子科技大學，2009.

[2] 陳偉，延文杰，周超英，等.兩輪自平衡機器人控制系統的設計[J].傳感器與微系統，2008，27(4):117-120.

[3] 閆飛，莊嚴，王偉.移動機器人基于多傳感器信息融合的室外場景理解[J].控制理論與應用，2011，28(8):1093-1098.

[4] 蘇志毅，趙偉，黃松嶺.多傳感器信息融合技術在現代測量領域的地位和重要作用[J].電測與儀表，2013，50(3):1-5.

[5] 王洪波.物聯網信息融合技術及存在的問題研究[J].計算機應用研究，2013，30(8):2252-2255，2261.

[6] 陳輝，鄧記才，吳曉輝，等.多傳感器信息融合在輪式機器人運動控制中的應用[J].傳感技術學報，2011，24(6):915-918.

[7] 張偉民，段曉明，趙艷花.兩輪自平衡小車控制研究[J].自動化技術與應用，2011，30(4):10-13.

[8] 孫建勤，何雅靜，孟祥忠.兩輪不平衡小車的優化模糊控制研究[J].計算機仿真，2011，28(2):184-186，406.

[9] 張劍，王維，何俊峰，等.基于雙超聲波模塊的新型定位方法[J].傳感器與微系統，2012，31(9):22-24，31.

[10]蔣林，閆繼宏，臧希喆，等.一種新的超聲波絕對定位方法[J].吉林大學學報(工學版)，2009，39(1):188-193.

[11]蔡建羨，阮曉鋼.OCPA仿生自主學習系統及在機器人姿態平衡控制上的應用[J].模式識別與人工智能，2011，24(1),138-146.

[12]朱效洲，李宇波，盧惠民，等.基于視覺的移動機器人可通行區域識別研究綜述[J].計算機應用研究，2012，28(6):1135-1137.

[13]阮曉鋼，蔡建羨，陳靜.強化學習規則的兩輪機器人自平衡控制[J].計算機測量與控制，2009，17(2):321-323.

Research on the Balanced Traveling Strategy Based on Dual Ultrasonic Data Fusion for Two Wheeled Robot

SHU Yingli1,KOU Lei2,YUAN Quande1,PI Yuzhen1

(1.School of Electrical and Information Engineering， Changchun Institute of Technology， Changchun 130012，China; 2.School of Information Engineering，Northeast Dianli University， Jilin 132012，China)

Aiming at the problems in production and application of the mobile self-balanced robot,its structural features and control strategy of posture balancing are researched.In order to ensure balanced and coordinated movement of the robot,the balanced traveling strategy based on dual ultrasonic distance measurement of data fusion is proposed.By utilizing the dual ultrasonic data fusion balancing algorithm,the control of self-balanced pose can be achieved in Arduino-controlled robots.In order to ensure the self-balanced motion of the robot,the dual ultrasonic sensor data information is processed;at the same time,the PID control algorithm is added in robot motion process,to adjust the motion status of robot and the speed of error processing is greatly improved.The results of experiment and simulation show that the proposed method is effective and feasible.The dual ultrasonic self balancing robot can maintain a good posture balance,and the moving control and basic direction control are effective and reliable,which reflects the better balance stability and coordination.Under control of balancing algorithm,the dual ultrasonic robot is not only balanced traveling,but also implementing balanced retreat,and balanced turning around under marching strategy,this possesses wide application prospects.

Robot； Sensor； Ultrasonic； Data； Control; Balance; Strategy

吉林省教育廳“十二五”科學技術研究項目(2014324)

舒英利(1965—)，男，碩士，高級試驗師，主要從事電力電子應用等研究。E-mail：shuyl@scitimes.com。 皮玉珍(通信作者)，女，碩士，主要從事智能電網技術、物聯網技術等方向的研究和應用。E-mail：piyuzhen@gmail.com。

TH-3；TP27

A

10.16086/j.cnki.issn 1000-0380.201702010

修改稿收到日期:2016-06-22