簡要研究了動輪自平衡機在分娩和分娩中應用中存在的問題，以及其結構點和姿態(tài)調平的控制策略。提出了一種基于雙超聲測距數(shù)據(jù)融合的平衡運行策略，用于故障保護機的平衡與調諧操作。通過將雙超聲數(shù)據(jù)與平衡計算方法相結合，在Arduino主控制器上實現(xiàn)自平衡姿態(tài)控制，認為對雙超聲傳感器數(shù)量進行了處理。通過以下方法保證機器的自平衡運行。同時在機器人的操作過程中加入PID控制算法，調節(jié)機器人的操作狀態(tài)，加快機器人信號誤差的處理速度。試驗和仿真結果表明，該策略是有效可行的，雙超聲自流平機能保持較好的平衡姿態(tài)，其進度控制系統(tǒng)和基本方向控制系統(tǒng)也是有效可靠的。具有較好的穩(wěn)定性和平衡協(xié)調性。在平衡方法的控制下，雙超聲波機不僅實現(xiàn)了調平前進，還實現(xiàn)了運行策略下的調平后退和方陣方向控制下的調平轉彎。有一個廣泛的可用的前視圖。

0 引言

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

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

1 雙超聲波姿態(tài)平衡動力學模型

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

若雙超聲波機器人作直線運動為輪軸半徑，沒為偏離Z軸的角度大小，θ為機器人運動的位移， V=θwr 為速度。假設不變θ，則速度在Z、F軸上可表示為：

由式（1)得速度模型：

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

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

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

2 .1 超聲波測距原理與誤差調整

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

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

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

對于超聲波發(fā)出速度有：

式中T。=273.16℃ ，溫度傳感器正常工作的溫度范圍為（ -40~80℃）在本系統(tǒng)中，假設恒定不變，V0=331.45 m/s。由于在態(tài)中各個接收器波頭滯后誤差與硬件電路延遲比較一致，故渡越時間用相同變量可表示為：

由于超聲波的測量精度的影響文獻[9]、文獻[10]提出，定時器真正測量的不是從TXD發(fā) 射到 RXD接收到回波的時間，而是超聲波模塊已進行溫度補償后的一個上升沿觸發(fā)電平的輸出時間。如果超聲波模塊A 的觸發(fā)點沒有被主控控制觸發(fā)，而是模塊 B 的觸發(fā)點被觸發(fā)后發(fā)射超聲波，并且被模塊A接收到返回信號，或者超聲波模塊A和超聲波模塊B同時觸發(fā)，但都接收到彼此的信號，都會造成信號干擾，使下個周期內TXD端口在某個不確定的時段發(fā)出一個高電平脈沖輸出，從而增大測量誤差[10]。
針對這一誤差，在滿足超聲波傳感器的主要電氣參數(shù)的條件下，本系統(tǒng)選用了具有回聲保護的HC-SR0 4超聲波模塊，并且盡可能地把超聲波模塊A、B隔離開。兩輪結構也在一定程度上阻擋了超聲波的互相干擾。

2 . 2 超聲波數(shù)據(jù)融合平衡算法
通過超聲波測距獲取機器人在極坐標平面內角度0的變化，以實現(xiàn)機身的平衡。當兩輪機器人平衡時，需滿足以下角度值換算公式：

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

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

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

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

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

系統(tǒng)平衡的狀態(tài)表達式為：

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

取前一個采樣周期內速度的均值或者前兩個狀態(tài)周期內的速度均值，得：

由式（14)可得平衡系統(tǒng)的狀態(tài)方程為：

經過PID數(shù)據(jù)整定后的PWM輸出方程為：

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

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

由于各狀態(tài)變量的誤差符合均值為〇的高斯分布，故平衡策略下的測量方程為:

式中： C 為測量系統(tǒng)的表征參數(shù)，當系統(tǒng)為多測量系統(tǒng)時，C 為矩陣;F0 為測量系統(tǒng)在&狀態(tài)下的噪聲誤差。由平衡系統(tǒng)在極坐標下的雙超聲波測距徑向距離和融合后偏差方向角的測量數(shù)據(jù)可得：

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

2.3 控制方案與行進策略

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

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

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

3 試驗與仿真

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

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

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

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

參考文獻：
[1] 劉斌.兩輪自平衡小車軟硬件研發(fā)與基于模糊線性化模型的變結構控制研究[ D] . 西安 :西安電子科技大學，2009.
[2] 陳偉，延文杰，周超英，等兩輪自平衡機器人控制系統(tǒng)的設計 [ J ]. 傳感器與微系統(tǒng)，2008，27 (4) : 117 - 120.
[3] 閆飛，莊嚴，王偉.移動機器人基于多傳感器信息融合的室外場景理解[ J ]. 控制理論與應用，2011，28 ( 8 ) : 1093 -1098.
[4] 蘇志毅，趙偉，黃松嶺.多傳感器信息融合技術在現(xiàn)代測量領域的地位和重要作用[*1].電測與儀表，2013,50(3):1 -5 .
[5] 王洪波. 物聯(lián)網信息融合技術及存在的問題研究[J].計算機應用研究，2013，30( 8) :2252 - 2255，2261 •
[6] 陳輝，鄧記才，吳曉輝，等.多傳感器信息融合在輪式機器人運動控制中的應用[ J ]. 傳感技術學報，2011，24(6) :915 -918.
[7] 張偉民，段曉明，趙艷花. 兩輪自平衡小車控制研究[J ] . 自動化技術與應用，2011，30(4):1 0-13 .
[8] 孫建勤，何雅靜，孟祥忠.兩輪不平衡小車的優(yōu)化模糊控制研究 [J] •計算機仿真，2011，28(2):184-186,406.
[9] 張劍，王維，何俊峰，等基于雙超聲波模塊的新型定位方法 [J ]. 傳感器與微系統(tǒng)，2012,31 (9) :22 -24,31.
[10] 蔣林，閆繼宏，臧希喆，等. 一種新的超聲波絕對定位方法[ J].吉林大學學報( 工學版），2〇〇9,39(1) :188 -193.
[11] 蔡建羨，阮曉鋼. 0CPA仿生自主學習系統(tǒng)及在機器人姿態(tài)平衡控制上的應用[ J ] . 模式識別與人工智能，2011，2 4 ( 1 ) ，138-146.
[12] 朱效洲，李宇波，盧惠民，等.基于視覺的移動機器人可通行區(qū)域識另帳究綜述[>!].計算驢用研究，2012,28(6):1135-1137.
[13]阮曉鋼，蔡建羨，陳靜.強化學習規(guī)則的兩輪機器人自平衡控制 [ J ] . 計算機測量與控制，2009， 17 (2 ) : 321 - 323.