|
雙超聲波數(shù)據(jù)融合的兩輪機(jī)器人平衡行進(jìn)策略研究簡(jiǎn)要研究了動(dòng)輪自平衡機(jī)在分娩和分娩中應(yīng)用中存在的問(wèn)題，以及其結(jié)構(gòu)點(diǎn)和姿態(tài)調(diào)平的控制策略。提出了一種基于雙超聲測(cè)距數(shù)據(jù)融合的平衡運(yùn)行策略，用于故障保護(hù)機(jī)的平衡與調(diào)諧操作。通過(guò)將雙超聲數(shù)據(jù)與平衡計(jì)算方法相結(jié)合，在Arduino主控制器上實(shí)現(xiàn)自平衡姿態(tài)控制，認(rèn)為對(duì)雙超聲傳感器數(shù)量進(jìn)行了處理。通過(guò)以下方法保證機(jī)器的自平衡運(yùn)行。同時(shí)在機(jī)器人的操作過(guò)程中加入PID控制算法，調(diào)節(jié)機(jī)器人的操作狀態(tài)，加快機(jī)器人信號(hào)誤差的處理速度。試驗(yàn)和仿真結(jié)果表明，該策略是有效可行的，雙超聲自流平機(jī)能保持較好的平衡姿態(tài)，其進(jìn)度控制系統(tǒng)和基本方向控制系統(tǒng)也是有效可靠的。具有較好的穩(wěn)定性和平衡協(xié)調(diào)性。在平衡方法的控制下，雙超聲波機(jī)不僅實(shí)現(xiàn)了調(diào)平前進(jìn)，還實(shí)現(xiàn)了運(yùn)行策略下的調(diào)平后退和方陣方向控制下的調(diào)平轉(zhuǎn)彎。有一個(gè)廣泛的可用的前視圖。 0 引言 在復(fù)雜、狹窄的大轉(zhuǎn)角工作環(huán)境下，兩輪自平衡移動(dòng)式機(jī)器人可以靈活、快捷地執(zhí)行任務(wù)。這種移動(dòng)式機(jī)器人兩輪共軸，機(jī)身重心置于輪軸重心上方，可憑借前后運(yùn)動(dòng)保持機(jī)身的相對(duì)平衡，實(shí)現(xiàn)動(dòng)態(tài)平衡的直立行走。由于具有體積小、運(yùn)動(dòng)靈活、適應(yīng)地形變化能力強(qiáng)等優(yōu)點(diǎn)，兩輪自平衡移動(dòng)式機(jī)器人可完成一些狹小環(huán)境內(nèi)的任務(wù)[1,2]。在復(fù)雜環(huán)境下能對(duì)環(huán)境高效感知和及時(shí)調(diào)整自身姿態(tài)，是自平衡機(jī)器人的必備功能。由于單一的傳感器（比如超聲波傳感器）感知能力有限 ，因此需要由多個(gè)傳感器交互實(shí)現(xiàn)有效協(xié)調(diào)，提高工作效率，同時(shí)在不同的應(yīng)用空間與任務(wù)空間中實(shí)現(xiàn)多傳感器數(shù)據(jù)融合[3,4]。 為改進(jìn)兩輪自平衡移動(dòng)式機(jī)器人的性能，提出了兩輪自平衡機(jī)器人姿勢(shì)平衡和行進(jìn)策略，利用雙超聲波測(cè)距檢測(cè)平衡與行進(jìn)特征，通過(guò)不斷檢測(cè)收集超聲波傳感器與地面的距離，進(jìn)行數(shù)據(jù)濾波融合，以提高兩輪自平衡機(jī)器人的控制效果[6_7]。雙超聲波檢測(cè)提高了控制精度和集成度，在保證良好的平衡姿態(tài)和移動(dòng)控制效果的前提下，有效降低了整體生產(chǎn)成本，并運(yùn)用配套的數(shù)據(jù)融合處理算法，實(shí)現(xiàn)了對(duì)平衡姿態(tài)與行進(jìn)姿態(tài)的調(diào)控。 1 雙超聲波姿態(tài)平衡動(dòng)力學(xué)模型 兩輪自平衡移動(dòng)式機(jī)器人的基本平衡控制原理為：當(dāng)檢測(cè)到車體產(chǎn)生傾斜時(shí)，對(duì)主控系統(tǒng)通過(guò)檢測(cè)得到的數(shù)據(jù)進(jìn)行融合、得出矯 正 的 角 度 值 與 輸 出 的占空比。 自平衡機(jī)器人主要通過(guò)驅(qū)動(dòng)2個(gè)輪子的電機(jī)所產(chǎn)生的轉(zhuǎn)動(dòng)力矩來(lái)控制機(jī)身傾斜方向的速度〜通過(guò)控制電機(jī)控制機(jī)身平衡姿態(tài)的調(diào)整，讓兩個(gè)輪子朝機(jī)身頃斜倒下的方向運(yùn)動(dòng)，進(jìn)而保持機(jī)身的動(dòng)態(tài)平衡 。平衡控制過(guò)程中，直接控制量為輪子的轉(zhuǎn)動(dòng)力矩[8]。 若雙超聲波機(jī)器人作直線運(yùn)動(dòng)為輪軸半徑，沒(méi)為偏離Z軸的角度大小，θ為機(jī)器人運(yùn)動(dòng)的位移， V=θwr 為速度。假設(shè)不變?chǔ)�，則速度在Z、F軸上可表示為： 由式（1)得速度模型：
自平衡機(jī)器人受力分析圖如圖1 所示。圖 1 中，〇為輪軸中心，P 為機(jī)身重心，機(jī) 體 質(zhì) 量 為 〜 重 力 加 速度 為 g ，機(jī)軸傾角為a ，超 聲 波 A 直射到地面的距離為A ，超聲 波B 直射到地面的距離為L(zhǎng)b，重心距機(jī)身下的平臺(tái)的距離為L(zhǎng) 。設(shè)機(jī)體以加速度a 向一邊傾斜，則受到水平方向的最優(yōu)慣性合力為：
圖 1 自平衡機(jī)器人受力分析圖 2 雙超聲波測(cè)距控制方案 2 .1 超聲波測(cè)距原理與誤差調(diào)整 采用時(shí)間差測(cè)距的方式來(lái)完成距離測(cè)量，超聲波傳感器發(fā)射端向探頭正方向發(fā)射特定頻率的超聲波，內(nèi)部電路的定時(shí)器在發(fā)射超聲波時(shí)開(kāi)始計(jì)時(shí)。超聲波發(fā)射與回響檢測(cè)時(shí)序圖如圖2 所示。 發(fā)射電路會(huì)根據(jù)所給的脈沖發(fā)出特定頻率的超聲波 ，超聲波碰到被測(cè)物界面則會(huì)被反射回接收端。文獻(xiàn) [9]采用了超聲波傳感器接收探頭接收到反射波后定時(shí)器停止計(jì)時(shí)的方法，進(jìn)而得出超聲波發(fā)射和接收回波的時(shí)間差L 超聲波在傳播介質(zhì)中的傳播速度為c(在空氣中為340 m/s)。通過(guò)計(jì)算，得出 從TXD端到被測(cè)界面之間的距離s 為 ：
由于超聲波頻率較高，可滿足多種用途的測(cè)量需要 。但在低維度空間中，超聲波測(cè)量會(huì)受到周邊溫度、自身響應(yīng)冗余、回波遲緩、傳播介質(zhì)等因素的影響。超聲波在固體中傳播速度最快，液體次之，在氣體中傳播速度最慢。聲 速 c 的表征速度與傳播介質(zhì)的溫度有關(guān) 。為了實(shí)現(xiàn)超聲波傳感器的精確測(cè)量，文獻(xiàn)[9]提出在硬件中設(shè)置溫度補(bǔ)償電路，以補(bǔ)償因溫度變化而帶來(lái)的聲速誤差。如果環(huán)境溫度變化不明顯，則無(wú)需設(shè)置溫度補(bǔ)償。 對(duì)于超聲波發(fā)出速度有：
式中T。=273.16℃ ，溫度傳感器正常工作的溫度范圍為（ -40~80℃） 在本系統(tǒng)中，假設(shè)恒定不變，V0=331.45 m/s。由于在態(tài)中各個(gè)接收器波頭滯后誤差與硬件電路延遲比較一致，故渡越時(shí)間用相同變量可表示為：
由于超聲波的測(cè)量精度的影響文獻(xiàn)[9]、文獻(xiàn)[10]提出，定時(shí)器真正測(cè)量的不是從TXD發(fā) 射 到 RXD接收到回波的時(shí)間，而是超聲波模塊已進(jìn)行溫度補(bǔ)償后的一個(gè)上升沿觸發(fā)電平的輸出時(shí)間。如果超聲波模塊A 的觸發(fā)點(diǎn)沒(méi)有被主控控制觸發(fā)，而是模 塊 B 的觸發(fā)點(diǎn)被觸發(fā)后發(fā)射超聲波，并 且 被 模塊A接收到返回信號(hào)，或者超聲波模塊A和超聲波模塊B同時(shí)觸發(fā)，但都接收到彼此的信號(hào)，都會(huì)造成信號(hào)干擾 ，使下個(gè)周期內(nèi)TXD端口在某個(gè)不確定的時(shí)段發(fā)出一個(gè)高電平脈沖輸出，從而增大測(cè)量誤差[10]。 2 . 2 超聲波數(shù)據(jù)融合平衡算法
式中：L為機(jī)身前后的2個(gè)超聲波模塊A 、B 間的直線距離;A 為平衡點(diǎn)尚度。 在自平衡機(jī)器人基于雙超聲波的檢測(cè)中，假設(shè)采樣周期為100ms，在 A:時(shí)刻獲取超聲波實(shí)測(cè)值a、6,并結(jié)合(K - 1) 時(shí)刻的距離預(yù)測(cè)值。根據(jù)測(cè)量的預(yù)測(cè)協(xié)方差S對(duì)&時(shí)刻的測(cè)量值進(jìn)行濾波估計(jì)修正，進(jìn)而對(duì)(K+1) 時(shí)刻的距離進(jìn)行預(yù)測(cè)。 測(cè)距獲取的平衡角度模型如圖3 所示。
圖 3 測(cè)距獲取的平衡角度模型 在估計(jì)平衡姿態(tài)機(jī)器人的運(yùn)動(dòng)狀態(tài)時(shí)，機(jī)器人的超聲波傳感器坐標(biāo)采用極坐標(biāo)系。設(shè)系統(tǒng)的狀態(tài)向量E (k )為:
系統(tǒng)平衡的狀態(tài)表達(dá)式為：
由于超聲波測(cè)距的采樣周期￡為300 ms，而機(jī)器人保持平衡的運(yùn)動(dòng)速度較為平緩，所以在局部時(shí)間段內(nèi)設(shè)定調(diào)整過(guò)程為近似勻速運(yùn)動(dòng)，x方向的運(yùn)動(dòng)方程為 ：
取前一個(gè)采樣周期內(nèi)速度的均值或者前兩個(gè)狀態(tài)周期內(nèi)的速度均值，得 ：
由式（14)可得平衡系統(tǒng)的狀態(tài)方程為：
經(jīng)過(guò)PID數(shù)據(jù)整定后的PWM輸出方程為：
基于Arduino的雙超聲波機(jī)器人平衡驅(qū)動(dòng)系統(tǒng)經(jīng)過(guò)PID調(diào)整后，輸出實(shí)現(xiàn)姿態(tài)的平衡。具體的超聲波雙向測(cè)距定位平衡模型如圖4 所示。由模型圖可得測(cè)量值Z(k)為 ：
圖 4 雙向測(cè)距定位平衡模型 由于各狀態(tài)變量的誤差符合均值為〇的高斯分布 ，故平衡策略下的測(cè)量方程為:
式中： C 為測(cè)量系統(tǒng)的表征參數(shù)，當(dāng)系統(tǒng)為多測(cè)量系統(tǒng)時(shí) ，C 為矩陣;F0 為測(cè)量系統(tǒng)在&狀態(tài)下的噪聲誤差。由平衡系統(tǒng)在極坐標(biāo)下的雙超聲波測(cè)距徑向距離和融合后偏差方向角的測(cè)量數(shù)據(jù)可得：
測(cè)速碼盤對(duì)電機(jī)編碼器產(chǎn)生的PWM脈沖進(jìn)行記數(shù) ，監(jiān)測(cè)機(jī)身的位移與輪子的速度。根據(jù)超聲波傳感器和姿態(tài)換算監(jiān)測(cè)車體運(yùn)動(dòng)姿態(tài)信號(hào)，經(jīng)過(guò)雙超聲波數(shù)據(jù)融合流程，文獻(xiàn)[11 ]通過(guò)相關(guān)的控制算法計(jì)算出輸出控制電壓的數(shù)值大小，對(duì)雙超聲波的2 個(gè)驅(qū)動(dòng)電機(jī)進(jìn)行控制 ，進(jìn)而調(diào)整車體平臺(tái)的行進(jìn)姿態(tài)與平衡位置，從而使機(jī)身保持良好的運(yùn)動(dòng)協(xié)調(diào)性和平衡狀態(tài)[11,12]。 2.3 控制方案與行進(jìn)策略 Arduino如 今已被運(yùn)用到各個(gè)領(lǐng)域 ，是一款使用Atmel Atmega328微處理控制器的開(kāi)源控制板。其開(kāi)發(fā)操作界面和搭建外設(shè)環(huán)境簡(jiǎn)易，有 0~13個(gè)數(shù)字模擬I/O、0~5 個(gè) Analog I/O,控制效果顯著。 自平衡機(jī)器 人 采 用 超 聲 波 電 路Arduino為核心控制模塊，電機(jī)驅(qū) 動(dòng)模塊為L(zhǎng)298N ，電機(jī)為TT馬達(dá)，并以2個(gè)HC-SR0 4 作為超聲波測(cè)距模塊，采 用 7.4 V/2200 mAh鋰電池供電。電位器用于平衡點(diǎn)設(shè)置與PID中的調(diào)試，方便數(shù)據(jù)的更新與調(diào)試，有效減少了開(kāi)發(fā)時(shí)間與生產(chǎn)成本。 雙超聲波自平衡機(jī)器人的行進(jìn)控制主要通過(guò)PWM占空比輸出控制電機(jī)轉(zhuǎn)速大小，進(jìn)而調(diào)整角度值的偏差，使得角度值趨近系統(tǒng)平衡零點(diǎn)。 控制部分由無(wú)線藍(lán)牙遙控的代碼實(shí)現(xiàn)，功能實(shí)現(xiàn)為“前進(jìn)狀態(tài)"、“后退狀態(tài) ”、“左轉(zhuǎn)狀態(tài) "、“右轉(zhuǎn)狀態(tài)”。從程序初始化參數(shù)設(shè)置可以看出，雙超聲波自平衡機(jī)器人的行進(jìn)控制方案是先調(diào)整平衡點(diǎn)和PID設(shè)定系數(shù)，然后調(diào)整左右電機(jī)的PWM輸出 ，從而實(shí)現(xiàn)行進(jìn)角度的控制與電機(jī)控制[13]。 3 試驗(yàn)與仿真 雙超聲波機(jī)器人運(yùn)用了主控Arduino,以簡(jiǎn)單、快捷的編程實(shí)現(xiàn)與I/O 接口連接，通過(guò)雙超聲波傳感器進(jìn)行環(huán)境測(cè)距定位，并利用控制算法進(jìn)行機(jī)器人姿態(tài)平衡控制。在試驗(yàn)驗(yàn)證方面，通過(guò)仿真顯示超聲波采集到的回聲檢測(cè)數(shù)據(jù)。由超聲波接收端接收到的回聲檢測(cè)波形圖可知，平衡系統(tǒng)的姿態(tài)搖擺幅度不大，基本處于平衡零點(diǎn)上下，具備基本穩(wěn)定性與數(shù)據(jù)的可計(jì)算性。 假設(shè)2 個(gè)超聲波傳感器經(jīng)過(guò)互補(bǔ)濾波后，換算出的角度數(shù)據(jù)為，超聲波采集數(shù)據(jù)為s，超聲波傳感器測(cè)距換算出的偏差角度值相對(duì)加速度的因子為L(zhǎng) 前一次超聲波傳感器經(jīng)過(guò)濾波后的角度數(shù)據(jù)為M2，增益選擇固定值為g 。濾波后 的角度數(shù)據(jù)為realM，最后得出融合出的角度數(shù)據(jù)。 試驗(yàn)結(jié)果證明，雙超聲波自平衡機(jī)器人能有效保持平衡，在平衡算法控制下能平衡行進(jìn);可實(shí)現(xiàn)行進(jìn)策略下的平衡后退和方向控制下的平衡轉(zhuǎn)彎，穩(wěn)定效果好。 4 結(jié)束語(yǔ) 參考文獻(xiàn)： 班寧產(chǎn)品匯總 |
