孟祥冬 何玉慶張宏達(dá)楊麗英谷 豐韓建達(dá)

(1.中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所機(jī)器人學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室,遼寧沈陽(yáng) 110016;2.中國(guó)科學(xué)院機(jī)器人與智能制造創(chuàng)新研究院,遼寧沈陽(yáng) 110016;3.中國(guó)科學(xué)院大學(xué),北京 100049;4.廣州中國(guó)科學(xué)院沈陽(yáng)自動(dòng)化研究所分所,廣東廣州 511458)

1 引言

旋翼無(wú)人機(jī)(rotor unmanned aerial vehicle,RUAV),又稱旋翼飛行機(jī)器人,在過(guò)去幾十年間得到了快速發(fā)展,使得這類飛行器已經(jīng)具備垂直起降、定點(diǎn)懸停和超低空飛行等一系列優(yōu)點(diǎn),并且廣泛應(yīng)用于軍事和民用領(lǐng)域,例如反恐防暴[1]、情報(bào)偵查[2]、災(zāi)難救援[3]和精準(zhǔn)農(nóng)業(yè)[4]等.然而,現(xiàn)有關(guān)于旋翼無(wú)人機(jī)的應(yīng)用通常是將其當(dāng)作一個(gè)搭載傳感器的感知和運(yùn)動(dòng)平臺(tái),在飛行過(guò)程中不會(huì)與外部環(huán)境發(fā)生任何接觸.

在最近幾年,由旋翼無(wú)人機(jī)和機(jī)器人操作臂構(gòu)成的飛行機(jī)械臂系統(tǒng)(aerial manipulator system),已經(jīng)逐漸成為機(jī)器人領(lǐng)域中一個(gè)新的研究熱點(diǎn).相比于傳統(tǒng)的飛行機(jī)器人,飛行機(jī)械臂系統(tǒng)可以與外部環(huán)境發(fā)生交互作用,并能進(jìn)行一些主動(dòng)作業(yè)任務(wù).現(xiàn)有研究大都集中在目標(biāo)物體的抓取、碼放和運(yùn)送方面.文獻(xiàn)[5]使用一個(gè)五自由度飛行機(jī)械臂系統(tǒng)實(shí)現(xiàn)了對(duì)圓柱形物體的抓取.文獻(xiàn)[6]設(shè)計(jì)出一個(gè)仿人的雙臂飛行機(jī)械臂來(lái)進(jìn)行抓取操作.文獻(xiàn)[7]研究使用兩個(gè)六旋翼飛行機(jī)械臂系統(tǒng)協(xié)作搬運(yùn)一個(gè)輕質(zhì)長(zhǎng)桿.文獻(xiàn)[8]在六旋翼飛行平臺(tái)上安裝七自由度操作臂來(lái)對(duì)運(yùn)動(dòng)目標(biāo)進(jìn)行抓取.文獻(xiàn)[9]則設(shè)計(jì)出一個(gè)集成了飛行機(jī)械臂和地面移動(dòng)平臺(tái)的運(yùn)輸協(xié)作系統(tǒng),來(lái)合作運(yùn)送重物.以上這些研究的飛行機(jī)械臂交互操作都是在無(wú)人機(jī)平臺(tái)處于自由飛行條件下進(jìn)行.盡管交互過(guò)程中會(huì)產(chǎn)生接觸力,但是通常忽略接觸力的存在,不考慮所產(chǎn)生的影響.

最新的研究工作開始對(duì)飛行機(jī)械臂在接觸作業(yè)過(guò)程中產(chǎn)生接觸力及其控制的問(wèn)題展開了探索.文獻(xiàn)[10]為小型四旋翼無(wú)人機(jī)設(shè)計(jì)了一個(gè)復(fù)雜的兩自由度機(jī)械臂,用于將接觸環(huán)境時(shí)所產(chǎn)生的動(dòng)能轉(zhuǎn)化成彈性機(jī)構(gòu)的勢(shì)能,以保證接觸環(huán)境過(guò)程中無(wú)人機(jī)平臺(tái)的穩(wěn)定飛行.文獻(xiàn)[11]使用了線性二次型調(diào)節(jié)器優(yōu)化狀態(tài)反饋的控制方法來(lái)控制一個(gè)持續(xù)作用在墻面上的接觸力,并且所提出的方法在四旋翼平臺(tái)上得到驗(yàn)證.文獻(xiàn)[12]設(shè)計(jì)了一個(gè)八旋翼飛行機(jī)械臂系統(tǒng)攜帶探傷設(shè)備用于橋梁損傷檢測(cè)的任務(wù),并將測(cè)量力反饋加入到無(wú)人機(jī)位置控制環(huán)路中來(lái)實(shí)現(xiàn)接觸操作中接觸力控制.文獻(xiàn)[13]設(shè)計(jì)了一個(gè)六旋翼單自由度飛行機(jī)械臂系統(tǒng)進(jìn)行按壓急停開關(guān)關(guān)閉電燈的操作.以上研究及實(shí)驗(yàn)結(jié)果表明,現(xiàn)階段飛行機(jī)械臂系統(tǒng)在與環(huán)境交互時(shí)的接觸力控制依舊面臨許多亟待解決的問(wèn)題.其中飛行機(jī)械臂的恒定接觸力控制就是一個(gè)具有挑戰(zhàn)性的問(wèn)題,這同時(shí)也是本文重點(diǎn)研究的內(nèi)容.

一般來(lái)講,機(jī)器人的接觸力控制需要將力傳感器信號(hào)作為反饋來(lái)設(shè)計(jì)力控制環(huán)路[14].而力信號(hào)通常又是通過(guò)傳感器應(yīng)變?cè)氖芰π巫儊?lái)獲得.然而,無(wú)人機(jī)在飛行時(shí),由于高速旋轉(zhuǎn)螺旋槳的氣動(dòng)作用，機(jī)體存在一定的振動(dòng)[15].因此,機(jī)載使用力傳感器測(cè)量環(huán)境接觸力,就會(huì)出現(xiàn)較大噪聲,導(dǎo)致測(cè)量誤差變大,最終降低飛行機(jī)械臂系統(tǒng)的接觸力控制精度.為避免直接使用力傳感器所帶來(lái)的缺點(diǎn),本文依靠無(wú)人機(jī)接觸環(huán)境時(shí)的力平衡關(guān)系來(lái)間接獲得接觸力信息.

本文的研究工作旨在提出一種有效的接觸力控制方法,用于保證飛行機(jī)械臂接觸外部環(huán)境時(shí)能夠可靠地進(jìn)行作業(yè).因此,本文研究了在外部力作用下的無(wú)人機(jī)位置響應(yīng),并從理論上首次證明閉環(huán)無(wú)人機(jī)系統(tǒng)具有與彈簧-質(zhì)量-阻尼系統(tǒng)相同的動(dòng)力學(xué)特性[16].同時(shí),基于阻抗控制的思想[17],創(chuàng)新地提出了飛行機(jī)械臂系統(tǒng)的接觸力控制方法.由于旋翼無(wú)人機(jī)系統(tǒng)加裝了機(jī)器人操作臂,導(dǎo)致這類新型飛行器動(dòng)力學(xué)與常規(guī)系統(tǒng)模型存在一定的差異:一是本體動(dòng)力學(xué)模型更加復(fù)雜,二是整體模型建模困難.同時(shí)存在的接觸力對(duì)無(wú)人機(jī)位置和姿態(tài)角控制也帶來(lái)兩方面區(qū)別:一是接觸模態(tài)下的本體安全、動(dòng)態(tài)穩(wěn)定控制更加重要;二是需要解決接觸模態(tài)下力和位置混合控制的問(wèn)題.

本文的其他部分內(nèi)容安排如下:第2部分研究了無(wú)人機(jī)系統(tǒng)的位置響應(yīng)與外部施加作用力之間的關(guān)系,并且從理論上證明了閉環(huán)無(wú)人機(jī)系統(tǒng)具有與質(zhì)量-彈簧-阻尼系統(tǒng)一致的動(dòng)態(tài)特性.第3部分基于阻抗控制思想,提出了飛行機(jī)械臂系統(tǒng)的接觸力控制方法,并進(jìn)行了穩(wěn)定性分析.第4部分介紹了所開發(fā)的新型飛行機(jī)械臂系統(tǒng),并通過(guò)實(shí)際的飛行實(shí)驗(yàn)證明所提出方法的有效性.最后一部分對(duì)本文所做的研究工作進(jìn)行了總結(jié).

2 閉環(huán)無(wú)人機(jī)系統(tǒng)的模型推導(dǎo)

本部分首先對(duì)常規(guī)的旋翼無(wú)人機(jī)系統(tǒng)設(shè)計(jì)一般的姿態(tài)和位置控制器.然后基于控制器結(jié)構(gòu),對(duì)閉環(huán)無(wú)人機(jī)系統(tǒng)進(jìn)行再建模,研究無(wú)人機(jī)位置響應(yīng)與所受到外部作用力之間的關(guān)系.最后證明位置閉環(huán)無(wú)人機(jī)系統(tǒng)具有類似于彈簧-質(zhì)量-阻尼系統(tǒng)的動(dòng)態(tài)特性.

2.1 常規(guī)無(wú)人機(jī)系統(tǒng)動(dòng)力學(xué)模型

多旋翼無(wú)人機(jī)是一種由多個(gè)旋翼對(duì)稱分布而構(gòu)成的飛行器系統(tǒng).為不失一般性,本文選擇普通四旋翼無(wú)人機(jī)建立動(dòng)力學(xué)模型,坐標(biāo)系的定義如圖1所示.

圖1 旋翼無(wú)人機(jī)系統(tǒng)的坐標(biāo)系定義Fig.1 Coordinate system definition for RUAV system

圖中:慣性坐標(biāo)系{I}采用北東地(north-eastdown,NED)坐標(biāo)系;本體固定坐標(biāo)系{B}的原點(diǎn)位于無(wú)人機(jī)系統(tǒng)幾何中心并與質(zhì)心重合,坐標(biāo)軸xB,yB,zB方向?qū)?yīng)選取前右下指向;{B}在{I}系中的位置向量為P=[x y z]T∈R3;歐拉角Φ=[? θ ψ]T∈R3分別對(duì)應(yīng)表示滾轉(zhuǎn)、俯仰和偏航姿態(tài)角,本文核心內(nèi)容是研究飛行機(jī)械臂的接觸力控制問(wèn)題,而不要求無(wú)人機(jī)做大姿態(tài)角度的機(jī)動(dòng)飛行，因此實(shí)際姿態(tài)角范圍始終能夠滿足條件?

可以得到無(wú)人機(jī)系統(tǒng)在慣性系下的平移動(dòng)力學(xué)和姿態(tài)動(dòng)力學(xué)模型[18],即為

其中:系統(tǒng)質(zhì)量m∈R;g是重力加速度9.81 ms?2;轉(zhuǎn)動(dòng)慣量I=diag{Ix,Iy,Iz}∈R3×3;l是從機(jī)體中心到電機(jī)軸的距離;ai和bi都是常數(shù),定義為a2=;u1是無(wú)人機(jī)系統(tǒng)升力,u2,u3和u4分別對(duì)應(yīng)到各軸上的旋轉(zhuǎn)力矩.

2.2 無(wú)人機(jī)系統(tǒng)常見控制器結(jié)構(gòu)

無(wú)人機(jī)控制器通常使用內(nèi)-外環(huán)結(jié)構(gòu),即姿態(tài)子系統(tǒng)作為內(nèi)環(huán),位置子系統(tǒng)作為外環(huán).在本節(jié)內(nèi)容中,首先為無(wú)人機(jī)設(shè)計(jì)姿態(tài)和高度控制器來(lái)實(shí)現(xiàn)平穩(wěn)的空中飛行,進(jìn)而設(shè)計(jì)位置控制器來(lái)實(shí)現(xiàn)無(wú)人機(jī)在三維空間定點(diǎn)懸停.整個(gè)系統(tǒng)對(duì)應(yīng)的控制框架如圖2所示.

圖2 無(wú)人機(jī)系統(tǒng)控制框圖Fig.2 Control block diagram of UAV system

2.2.1 姿態(tài)控制器

首先對(duì)無(wú)人機(jī)的姿態(tài)動(dòng)力學(xué)式(1b)使用反饋線性化方法轉(zhuǎn)化成一個(gè)線性系統(tǒng),此時(shí),系統(tǒng)的虛擬控制量與原來(lái)控制量u2,u3,u4的關(guān)系為

針對(duì)所得到的系統(tǒng),設(shè)計(jì)如下狀態(tài)反饋控制器:

2.2.2 高度控制器

無(wú)人機(jī)的三維位置控制通常分成高度控制和水平位置控制.對(duì)于式(1a)中的高度動(dòng)力學(xué),使用變量代換

對(duì)所得到的模型,設(shè)計(jì)如下狀態(tài)反饋控制器:

2.2.3 水平位置控制器

基于常用的內(nèi)-外環(huán)控制,內(nèi)環(huán)姿態(tài)角度的期望值是由外環(huán)位置控制的輸出計(jì)算得到,由式(1a)可得

其中:ux和uy是水平位置控制器的輸出;是位置控制器的增益系數(shù),即

無(wú)人機(jī)在控制器作用下可以實(shí)現(xiàn)三維空間中穩(wěn)定懸停,此飛行狀態(tài)下的姿態(tài)角度變化較小.因此,將式(9)代入到式(8)中,再經(jīng)整理可以得到

2.3 閉環(huán)無(wú)人機(jī)系統(tǒng)的再建模

其中zd選取坐標(biāo)原點(diǎn)作為期望懸停高度,由此可得無(wú)人機(jī)高度在外力作用下的動(dòng)態(tài)響應(yīng)關(guān)系為

當(dāng)研究無(wú)人機(jī)水平位置受力特性時(shí),可以將航向角度設(shè)為0,從而有sin ψ=0, cos ψ=1.同時(shí)也選取坐標(biāo)系原點(diǎn)作為期望的懸停位置.那么,式(1a)對(duì)應(yīng)的系統(tǒng)水平運(yùn)動(dòng)動(dòng)力學(xué)可以寫為

對(duì)式(14)進(jìn)行拉普拉斯變換,將式(4)和式(10)代入后,可以得到無(wú)人機(jī)系統(tǒng)水平位置x和y對(duì)應(yīng)在外部力作用下的動(dòng)態(tài)響應(yīng)關(guān)系為式(15):

繼而再對(duì)式(15)分式中的分子分母分別除以各自的分子.注意到此時(shí)無(wú)人機(jī)已經(jīng)處于位置閉環(huán)控制狀態(tài),當(dāng)以此為平臺(tái)的飛行機(jī)械臂系統(tǒng)接觸外部環(huán)境時(shí),所產(chǎn)生接觸力或的變化頻率相比較于無(wú)人機(jī)系統(tǒng)控制頻率來(lái)講比較緩慢.外部力作用在閉環(huán)系統(tǒng)上,其位置響應(yīng)的頻率較低.同時(shí),無(wú)人機(jī)物理結(jié)構(gòu)由輕質(zhì)材料構(gòu)成并且所有部件設(shè)計(jì)緊湊靠近幾何中心,所以這類小型無(wú)人機(jī)實(shí)際轉(zhuǎn)動(dòng)慣量Ix和Iy的數(shù)值一般在10?2～10?3kg·m2這個(gè)量級(jí)[18],是比較小的數(shù)值,使得因此,可以很合理地得到近似關(guān)系進(jìn)而將式(15)可以寫成

由式(12)和式(16)可知,閉環(huán)無(wú)人機(jī)系統(tǒng)在外部力作用下的位置響應(yīng)(包括豎直高度和水平位置),類似于普通彈簧-質(zhì)量-阻尼系統(tǒng)的動(dòng)力學(xué)特性,如圖3所示.

圖3 閉環(huán)無(wú)人機(jī)等效為彈簧-質(zhì)量-阻尼系統(tǒng)Fig.3 Closed-loop UAV system behaves as a spring-mass-damper system

2.4 水平接觸力關(guān)系分析

當(dāng)飛行機(jī)械臂系統(tǒng)在前向接觸外部環(huán)境并施加某一水平接觸力Fc時(shí),根據(jù)作用力與反作用力的原理,環(huán)境也會(huì)給飛行機(jī)械臂系統(tǒng)施加反作用力?Fc.假設(shè)旋翼無(wú)人機(jī)在接觸環(huán)境時(shí)依舊保持懸停飛行,并且保證接觸力能夠始終垂直于環(huán)境接觸面.根據(jù)式(1a),可以得到在穩(wěn)定接觸狀態(tài)下的平衡關(guān)系為

將航向設(shè)為固定值,即令ψ=0.因此可以根據(jù)式(17),推導(dǎo)出接觸力Fc的表達(dá)為

可見,接觸力Fc可以由飛行機(jī)械臂系統(tǒng)的重力mg和俯仰角度θ來(lái)共同確定,如圖4所示.此時(shí)的無(wú)人機(jī)系統(tǒng)就相當(dāng)于是一個(gè)可測(cè)量接觸力的傳感器.

圖4 飛行機(jī)械臂系統(tǒng)接觸力關(guān)系圖Fig.4 Force relationships for aerial manipulator system

由圖4可知,當(dāng)接觸力Fc出現(xiàn)時(shí),無(wú)人機(jī)總會(huì)有一個(gè)俯仰角度θ與其時(shí)刻對(duì)應(yīng).若此時(shí)的接觸力為期望控制力Fd,那么所對(duì)應(yīng)俯仰角就是系統(tǒng)期望達(dá)到的俯仰角度θd.進(jìn)而可以控制無(wú)人機(jī)跟蹤這個(gè)期望的俯仰角θd來(lái)實(shí)現(xiàn)對(duì)Fd的控制.因此可由式(18)得到θd的值為

3 飛行機(jī)械臂接觸力控制

本部分主要研究飛行機(jī)械臂的接觸力控制.首先,簡(jiǎn)單的介紹阻抗控制原理;然后根據(jù)阻抗控制思想提出飛行機(jī)械臂系統(tǒng)的接觸力控制方法.之后分析系統(tǒng)的穩(wěn)定性問(wèn)題.

3.1 阻抗控制原理

阻抗控制是由Hogan提出,用于解決機(jī)器人接觸環(huán)境時(shí)的力控制問(wèn)題[17],是通過(guò)調(diào)節(jié)位置和力之間的期望阻抗關(guān)系來(lái)實(shí)現(xiàn)機(jī)器人與環(huán)境之間的作用力控制.基于位置的阻抗控制原理是將環(huán)境剛度和機(jī)器人位置共同確定的軌跡輸入到系統(tǒng)進(jìn)行跟蹤,從而間接實(shí)現(xiàn)力控制[19].這里提到的“間接”就是阻抗控制的核心思想,即通過(guò)控制機(jī)器人跟蹤期望位置軌跡來(lái)實(shí)現(xiàn)最終的力控制.

3.2 飛行機(jī)械臂接觸力控制

飛行機(jī)械臂系統(tǒng)在接觸環(huán)境進(jìn)行作業(yè)任務(wù)時(shí),面臨兩個(gè)控制問(wèn)題:一是要保證無(wú)人機(jī)平臺(tái)的穩(wěn)定飛行,二是接觸過(guò)程中要控制接觸力.現(xiàn)有關(guān)于無(wú)人機(jī)穩(wěn)定性的研究工作已經(jīng)實(shí)現(xiàn)第1個(gè)目標(biāo).對(duì)于第2個(gè)控制問(wèn)題,本部分將使用第3.1節(jié)中提到的間接實(shí)現(xiàn)力控制思想加以解決,即根據(jù)力偏差計(jì)算出飛行器期望的位置軌跡并輸入系統(tǒng)中來(lái)實(shí)現(xiàn)接觸力控制.

由閉環(huán)無(wú)人機(jī)系統(tǒng)的特性可知,若期望位置和實(shí)際位置之間存在誤差,系統(tǒng)就會(huì)產(chǎn)生相應(yīng)的驅(qū)動(dòng)力矢量μ=[μxμyμz]T∈R3.同時(shí)還會(huì)產(chǎn)生與之對(duì)應(yīng)的期望姿態(tài)角(滾轉(zhuǎn)角?d和俯仰角θd),以最終產(chǎn)生實(shí)現(xiàn)平移運(yùn)動(dòng)的分力[20].通常情況下,油門升力u1、驅(qū)動(dòng)力矢量μ以及期望姿態(tài)角度?d和θd的關(guān)系如下:

因此,如果根據(jù)期望控制的接觸力,將計(jì)算所得的期望位置和對(duì)應(yīng)期望姿態(tài)角度輸入到無(wú)人機(jī)系統(tǒng)中,那么最終會(huì)產(chǎn)生期望的旋翼控制量.于是無(wú)人機(jī)將會(huì)在所接觸環(huán)境上施加一定的作用力.上述就是阻抗控制的“間接”思想在飛行機(jī)械臂接觸力控制問(wèn)題上的應(yīng)用,整體控制實(shí)現(xiàn)框圖如圖5所示.定義無(wú)人機(jī)的位置誤差為?x,系統(tǒng)期望位置的設(shè)定值為xd,由環(huán)境位置確定的飛行機(jī)械臂參考位置為xr,并有以下關(guān)系:

并且由已推導(dǎo)出的彈簧-質(zhì)量-阻尼系統(tǒng)模型(16a)可以得到系統(tǒng)位置誤差?x和接觸作用力Fe之間的關(guān)系為

其中定義Fe=Fd?Fc,并且Fc由式(18)得到,進(jìn)而有xd=xr+?x.此外,本文所提出的接觸力控制方法旨在控制水平接觸力,不論實(shí)際所接觸環(huán)境表面特性具體怎樣,只要在接觸過(guò)程中保證牢固靜態(tài)不滑動(dòng)接觸即可.因此,需要控制飛行機(jī)械臂末端姿態(tài)來(lái)補(bǔ)償無(wú)人機(jī)俯仰角度θ,以保證是正交的接觸力.

圖5 飛行機(jī)械臂系統(tǒng)的接觸力控制Fig.5 Contact force control of aerial manipulator system

3.3 動(dòng)力學(xué)模型

接觸狀態(tài)下飛行機(jī)械臂系統(tǒng)動(dòng)力學(xué)模型含有3個(gè)部分:無(wú)人機(jī)位置、無(wú)人機(jī)姿態(tài)和機(jī)械臂模型.這里以圖4中所示單自由度旋轉(zhuǎn)關(guān)節(jié)機(jī)械臂為例,機(jī)械臂基座安裝在整體系統(tǒng)質(zhì)心位置且保持正交接觸,即為

根據(jù)上述公式可知,無(wú)人機(jī)的位置動(dòng)力學(xué)式(24a)要受到環(huán)境接觸力?Fc∈R3的影響,其中Fc為

由于機(jī)械臂是垂直于接觸面并且安裝在系統(tǒng)質(zhì)心位置，所以無(wú)人機(jī)的姿態(tài)動(dòng)力學(xué)式(24b)僅會(huì)受到機(jī)械臂關(guān)節(jié)電機(jī)反扭矩τm∈R3的影響.其中:τm=[0 τm0]T是關(guān)節(jié)電機(jī)驅(qū)動(dòng)扭矩τm的矢量形式,W=diag{l,l, 1}∈R3×3是系數(shù)矩陣,τ=[u2u3u4]T∈R3,e3=[0 0 1]T∈R3,轉(zhuǎn)換矩陣RΦ(?,θ,0)∈R3×3具體形式為

其余變量符號(hào)的定義同第2.1節(jié)中所示.

假設(shè)接觸面不是絕對(duì)光滑且能夠提供足夠的摩擦力,對(duì)應(yīng)的連桿重力全部由懸停的無(wú)人機(jī)平臺(tái)和所接觸的環(huán)境來(lái)分擔(dān).因此,常規(guī)的機(jī)械臂動(dòng)力學(xué)模型此時(shí)就退化為關(guān)節(jié)電機(jī)模型式(24c).其中:θmrad是機(jī)械臂的關(guān)節(jié)角度,H kg·m2是關(guān)于電機(jī)軸的轉(zhuǎn)動(dòng)慣量,τr=u3N·m是無(wú)人機(jī)姿態(tài)控制過(guò)程中對(duì)機(jī)械臂旋轉(zhuǎn)關(guān)節(jié)的反作用力矩.

3.4 穩(wěn)定性分析

由上述分析可知,接觸操作狀態(tài)下飛行機(jī)械臂系統(tǒng)的穩(wěn)定性與環(huán)境的剛度沒有關(guān)系.只要保證各個(gè)系統(tǒng)的穩(wěn)定性,那么整個(gè)系統(tǒng)穩(wěn)定性就可以隨之確定.因此,本節(jié)首先為3個(gè)子系統(tǒng)設(shè)計(jì)控制器,并分析穩(wěn)定性.

3.4.1 控制器設(shè)計(jì)與穩(wěn)定性

針對(duì)第3.3節(jié)中式(24)對(duì)應(yīng)的各個(gè)系統(tǒng)分別設(shè)計(jì)控制器.機(jī)械臂的控制器用于控制關(guān)節(jié)角度和速度來(lái)跟蹤期望的軌跡,同時(shí)要考慮無(wú)人機(jī)姿態(tài)動(dòng)力學(xué)的耦合影響.無(wú)人機(jī)位置控制器的設(shè)計(jì)在保證位置穩(wěn)定的同時(shí),要補(bǔ)償環(huán)境接觸力作用.無(wú)人機(jī)姿態(tài)控制器的設(shè)計(jì)要在穩(wěn)定姿態(tài)跟蹤的同時(shí),還要補(bǔ)償機(jī)械臂關(guān)節(jié)控制反扭矩的影響.

本文研究的核心問(wèn)題是接觸作業(yè)型飛行機(jī)械臂系統(tǒng)的接觸力控制方法與實(shí)現(xiàn),旨在提出一個(gè)新的框架來(lái)解決飛行機(jī)械臂接觸力控制問(wèn)題.同時(shí)還希望那些已在飛行器上驗(yàn)證可用的控制算法經(jīng)修正依舊能夠滿足新問(wèn)題的需求,以便達(dá)到易于系統(tǒng)實(shí)現(xiàn)的目的.因此,首先對(duì)飛行器動(dòng)力學(xué)進(jìn)行反饋線性化,然后設(shè)計(jì)滑?？刂破鱽?lái)分析系統(tǒng)穩(wěn)定性[21].

針對(duì)式(24a)子系統(tǒng)定義新的輸入量μ1,其具體形式如下:

對(duì)應(yīng)的位置動(dòng)力學(xué)式(24a)可以寫成

對(duì)于式(24b)依舊采取式(2)中所述的反饋線性化方法,并令對(duì)角矩陣Wτ=I?1W∈R3×3.為了統(tǒng)一形式,定義新的控制量

所以姿態(tài)動(dòng)力學(xué)式(24b)可以寫成

因此,飛行機(jī)械臂各子系統(tǒng)對(duì)應(yīng)模型都可以寫成統(tǒng)一的形式

針對(duì)系統(tǒng)(32)設(shè)計(jì)滑?？刂破?定義狀態(tài)誤差eξ為eξ=(ξd?ξ),滑模面定義為

其中矩陣C為系數(shù)對(duì)角正定陣.依據(jù)滑模理論,可以得到系統(tǒng)控制率為

其中:λ和γ是正系數(shù);γ是有界擾動(dòng)的上邊界;sat(·)是具有邊界層厚度δ的飽和函數(shù),常用于削弱模型滑動(dòng)過(guò)程中的抖振問(wèn)題,其定義為

根據(jù)Lyapunov穩(wěn)定性理論可知,系統(tǒng)(32)是漸近穩(wěn)定的,系統(tǒng)狀態(tài)ξ能夠漸近收斂到期望值ξd.因此,當(dāng)飛行機(jī)械臂系統(tǒng)進(jìn)行空中接觸操作任務(wù)時(shí),只要對(duì)應(yīng)各個(gè)子系統(tǒng)能夠保證穩(wěn)定性,那么整個(gè)系統(tǒng)的穩(wěn)定性也就可以確定.

4 飛行機(jī)械臂系統(tǒng)與飛行接觸實(shí)驗(yàn)

4.1 飛行機(jī)械臂系統(tǒng)介紹

如圖6所示的飛行機(jī)械臂系統(tǒng),機(jī)體采用輕質(zhì)材料并且機(jī)械結(jié)構(gòu)設(shè)計(jì)緊湊.飛行平臺(tái)是一個(gè)普通的四旋翼無(wú)人機(jī)，所使用的飛行控制器是一款開源飛控[22].機(jī)械臂連桿由系統(tǒng)中心位置向前伸出,關(guān)節(jié)驅(qū)動(dòng)采用一個(gè)大扭矩舵機(jī).為了測(cè)量操作時(shí)的接觸力信息,使用了一個(gè)單軸力傳感器,并將其安裝在機(jī)械臂連桿末端.力傳感器可以通過(guò)數(shù)據(jù)采集板與飛控通信,測(cè)量力的信息可以由飛控實(shí)時(shí)記錄.連桿末端安裝了一個(gè)小型接觸板用于保證接觸的穩(wěn)固性.關(guān)于系統(tǒng)的一些重要的物理結(jié)構(gòu)參數(shù),如表1所示.

圖6 飛行機(jī)械臂系統(tǒng)Fig.6 Aerial manipulator system introduction

表1 飛行機(jī)械臂系統(tǒng)參數(shù)表Table 1 Parameters of aerial manipulator system

此外,為實(shí)現(xiàn)本文所提出的接觸力控制方法,單自由度飛行機(jī)械臂的關(guān)節(jié)舵機(jī)需要實(shí)現(xiàn)自動(dòng)控制,來(lái)補(bǔ)償接觸操作飛行平臺(tái)的姿態(tài)角,以保證機(jī)械臂連桿正交于接觸面.

4.2 飛行接觸實(shí)驗(yàn)

本部分使用第4.1節(jié)所介紹的飛行機(jī)械臂系統(tǒng),進(jìn)行接觸環(huán)境的力控制實(shí)驗(yàn).首先介紹實(shí)驗(yàn)的環(huán)境和條件,然后進(jìn)行斜坡力跟蹤和恒定力控制實(shí)驗(yàn)來(lái)驗(yàn)證所提出接觸力控制方法的有效性.

在室內(nèi)環(huán)境下,使用NaturalPoint Optitrack動(dòng)作捕捉系統(tǒng)來(lái)提供飛行器的位置和速度信息[23].而無(wú)人機(jī)的姿態(tài)運(yùn)動(dòng)信息則由機(jī)載慣性測(cè)量單元IMU來(lái)獲得.自由飛行狀態(tài)下旋翼無(wú)人機(jī)的位置控制精度為±0.03 m,其姿態(tài)控制精度為±0.03 rad.在下面的實(shí)驗(yàn)中,將無(wú)人機(jī)的航向設(shè)定為0,并且所接觸環(huán)境的位置是[0.45 m,?0.92 m,?1.48 m].

本文的控制方法不需要接觸力信號(hào)的直接測(cè)量,而是采用姿態(tài)反饋的方法間接實(shí)現(xiàn).盡管是這樣,筆者在實(shí)驗(yàn)過(guò)程中依舊加載了力傳感器,其主要目的是獲取真實(shí)的接觸力信息,并與實(shí)際控制結(jié)果進(jìn)行對(duì)比,以驗(yàn)證筆者方法的準(zhǔn)確性,如圖6中展示的物理系統(tǒng)所示.正如文章引言部分提到,飛行狀態(tài)下機(jī)體的振動(dòng)不可避免,并且是高頻振動(dòng),會(huì)引起測(cè)量噪聲.在使用力傳感器測(cè)量時(shí),一方面從數(shù)據(jù)層面進(jìn)行低通濾波來(lái)減小高頻噪聲,另一方面是從結(jié)構(gòu)層面解決,在連桿末端接觸板上加裝能夠減小接觸振動(dòng)的軟質(zhì)墊層來(lái)進(jìn)行緩沖.

飛行機(jī)械臂接觸外部環(huán)境進(jìn)行操作任務(wù),其過(guò)程主要分為4個(gè)步驟:接近目標(biāo)、接觸過(guò)渡、接觸力控制和返回,即如圖7所示.本實(shí)驗(yàn)重點(diǎn)研究飛行機(jī)械臂接觸環(huán)境時(shí)的力控制效果.

圖7 飛行機(jī)械臂接觸操作的主要步驟Fig.7 Main steps of aerial contact operation

將一個(gè)斜率為0.5 N/s終值為5 N的斜坡函數(shù)F(t)=0.5 N/s·t(0 ≥t ≥10)輸入系統(tǒng)作為期望控制的接觸力.當(dāng)飛行機(jī)械臂系統(tǒng)接觸環(huán)境時(shí),就會(huì)在控制器的作用下跟蹤這個(gè)期望力.若期望力數(shù)值不再改變時(shí),還可實(shí)現(xiàn)恒定接觸力的保持.相應(yīng)的實(shí)驗(yàn)結(jié)果及分析在下面給出.

斜坡力跟蹤與恒定力保持的結(jié)果在圖8所示,其中Fdes是系統(tǒng)期望的接觸力,Fmes是使用機(jī)載力傳感器實(shí)際測(cè)量的接觸力,Fcal是根據(jù)姿態(tài)角由式(18)關(guān)系計(jì)算得到的接觸力.分析圖中0～10 s的曲線可知,系統(tǒng)對(duì)斜坡力的跟蹤響應(yīng)較快,在跟蹤過(guò)程中存在的數(shù)值波動(dòng)則是由飛行平臺(tái)振動(dòng)引起的;10～30 s的曲線顯示系統(tǒng)可以在一定精度范圍內(nèi)保持期望的恒定力,且沒有穩(wěn)態(tài)誤差.

圖8 斜坡力跟蹤與恒定力保持Fig.8 Ramp force tracking and constant force control

將圖8所示的接觸力Fcal與Fmes分別與期望接觸力Fdes求偏差,結(jié)果顯示在圖9中.求出接觸力誤差?Fmes=Fmes?Fdes和?Fcal=Fcal?Fdes的均值與方差,如表2中所示.對(duì)比分析可知,?Fcal的均值為0.0353 小于?Fmes的均值0.1328;且?Fcal的方差為0.0438小于?Fmes的方差0.0867.這是由于力傳感器中的感壓應(yīng)變?cè)诠ぷ鬟^(guò)程中存在低帶寬、大振動(dòng)噪聲以及存有遲滯誤差等缺點(diǎn),從而導(dǎo)致其測(cè)量力結(jié)果存在較大偏差.而飛行器姿態(tài)是由IMU和磁羅盤等多種高帶寬低延時(shí)的運(yùn)動(dòng)傳感器測(cè)量并經(jīng)數(shù)據(jù)融合得到的相對(duì)精確的姿態(tài)信息,并且由式(18)對(duì)應(yīng)的力關(guān)系直接換算得到,所以從數(shù)據(jù)來(lái)看獲得接觸力的結(jié)果更可靠一些.

圖9 接觸力的實(shí)時(shí)誤差Fig.9 The real time contact force error

表2 接觸力的誤差分析Table 2 The error of contact force

由圖8和9顯示的實(shí)驗(yàn)數(shù)據(jù)結(jié)果來(lái)看,恒定力控制仍然存在一定的誤差,以下對(duì)其原因進(jìn)行深入的分析.對(duì)于旋翼飛行器系統(tǒng)來(lái)講,本身是依靠多個(gè)螺旋槳高速旋轉(zhuǎn)壓縮空氣產(chǎn)生動(dòng)力,由于周圍氣流不穩(wěn)、各個(gè)電機(jī)和螺旋槳不能保證性能和物理參數(shù)絕對(duì)的一致以及電壓不恒定等因素,使得實(shí)際飛行器存在無(wú)法避免的振動(dòng),導(dǎo)致自身定位精度相對(duì)較低,即使是在動(dòng)作捕捉系統(tǒng)中的位置控制精度最高可達(dá)±2 cm.自由懸停飛行狀態(tài)下的每個(gè)電機(jī)轉(zhuǎn)速也都不是恒定值(通常有大范圍的動(dòng)態(tài)變化).當(dāng)在飛行器上加載機(jī)械臂進(jìn)行接近環(huán)境的接觸作業(yè)時(shí),氣流擾動(dòng)等因素更加明顯.對(duì)于一般的工業(yè)機(jī)器人,由于具有基座固定、伺服系統(tǒng)高帶寬、優(yōu)化的結(jié)構(gòu)設(shè)計(jì)等優(yōu)勢(shì),其末端位置控制精度可以達(dá)到亞毫米量級(jí).而飛行器位置控制精度與之差2個(gè)量級(jí),進(jìn)而基于飛行器位置來(lái)控制接觸力的效果,確實(shí)不能與工業(yè)機(jī)械臂力控制效果相比較,因此在力控制實(shí)驗(yàn)結(jié)果上就自然顯示出相對(duì)較大的波動(dòng)和誤差.

根據(jù)本文的方法,飛行器x方向的期望位置會(huì)根據(jù)期望接觸力做出相應(yīng)調(diào)整,其曲線如圖10 所示.可以發(fā)現(xiàn),當(dāng)機(jī)械臂接觸環(huán)境位置為xreal時(shí),只要在x方向上期望力Fd不為0,對(duì)應(yīng)x位置就會(huì)存在相應(yīng)的偏差,為xdesd?xreal.由于在y與z方向上沒有進(jìn)行接觸力的控制,所以對(duì)應(yīng)這兩個(gè)維度的實(shí)際位置yreal和zreal一直保持在期望值ydesd和zdesd附近,其位置控制精度與自由懸停飛行時(shí)精度一樣都是±0.03 m.

圖10 飛行器位置變化曲線Fig.10 The real time position variation

在接觸力控制過(guò)程中,期望俯仰角θd是由式(19)計(jì)算得到,并且將其輸入到姿態(tài)子系統(tǒng)中進(jìn)行跟蹤控制.斜坡力對(duì)應(yīng)的俯仰角曲線在圖11顯示,其中:實(shí)際俯仰角是θreal(對(duì)應(yīng)圖中pitchreal),期望俯仰角是θdesd(對(duì)應(yīng)圖中pitchdesd),且恒定力對(duì)應(yīng)俯仰角是?0.218 rad.而滾轉(zhuǎn)和航向角度值一直在零值附近波動(dòng),這是因?yàn)橹皇窃跓o(wú)人機(jī)前向方向上有接觸力控制,其他方向上沒有接觸力,所以無(wú)人機(jī)的滾轉(zhuǎn)(對(duì)應(yīng)圖中roll)和航向(對(duì)應(yīng)圖中yaw)是自由飛行狀態(tài)下的表現(xiàn),且控制精度依舊為±0.03 rad.

圖11 飛行器姿態(tài)變化曲線Fig.11 The real time attitude variation

對(duì)于飛行機(jī)械臂進(jìn)行的接觸作業(yè),接觸力最終還是由無(wú)人機(jī)平臺(tái)來(lái)提供.對(duì)于斜坡力跟蹤和恒定力保持的過(guò)程中,無(wú)人機(jī)總油門升力的變化也展現(xiàn)出與之對(duì)應(yīng)的曲線,如圖12所示.油門曲線常使用0～1之間的數(shù)值來(lái)表示,圖中0～10 s的曲線對(duì)應(yīng)是斜坡力跟蹤時(shí)候的油門變化,10～30 s的曲線對(duì)應(yīng)是恒定力保持時(shí)候的油門變化,可見此時(shí)油門保持在0.565值附近.

圖12 飛行器油門量變化曲線Fig.12 The total thrust curve of UAV

本文所附帶的視頻展示了所設(shè)計(jì)的單自由度飛行機(jī)械臂系統(tǒng)接觸墻壁的實(shí)驗(yàn),展示了飛行機(jī)械臂接近環(huán)境、接觸力控制和返回3個(gè)場(chǎng)景,其中力控制實(shí)驗(yàn)包含斜坡接觸力跟蹤和恒定接觸力保持2部分內(nèi)容.

5 結(jié)論

本文首先從理論上證明了基于狀態(tài)反饋控制器的閉環(huán)無(wú)人機(jī)系統(tǒng)表現(xiàn)出與彈簧-質(zhì)量-阻尼系統(tǒng)一致的動(dòng)態(tài)特性.并且基于推導(dǎo)的模型和阻抗控制思想,詳細(xì)介紹了飛行機(jī)械臂系統(tǒng)不使用力傳感器的接觸力控制方法,同時(shí)分析了系統(tǒng)穩(wěn)定性.繼而開發(fā)了單自由度飛行機(jī)械臂系統(tǒng)用于方法的驗(yàn)證.最后的飛行接觸力實(shí)驗(yàn)結(jié)果證明了本文所提出接觸力控制方法的有效性以及所開發(fā)系統(tǒng)平臺(tái)的可靠性.

下一步的研究工作將集中在多自由度飛行機(jī)械臂系統(tǒng)的接觸操作方面.考慮到非結(jié)構(gòu)化環(huán)境中的飛行操作,因此環(huán)境感知的問(wèn)題也是未來(lái)工作的關(guān)注點(diǎn).