四旋翼機(jī)器人運(yùn)動(dòng)控制與動(dòng)力學(xué)研究與設(shè)計(jì)

湯 穎 ，趙 彬

1 引言

在新型材料、電子元件和控制算法的飛速發(fā)展下，四旋翼機(jī)器人在最近幾年有了長(zhǎng)足的發(fā)展。并且在民用領(lǐng)域和軍事領(lǐng)域都有廣闊的應(yīng)用前景。四旋翼飛行器不僅可以垂直起降和懸停，而且可以實(shí)現(xiàn)前后、左右平移、俯仰和旋轉(zhuǎn)等。因此，飛行機(jī)器人具有在危險(xiǎn)場(chǎng)景，自然災(zāi)害等各個(gè)領(lǐng)域的強(qiáng)大的適應(yīng)能力。

縱觀當(dāng)今機(jī)器人行業(yè)發(fā)展現(xiàn)狀，科研院所、高校和機(jī)器人公司針對(duì)四旋翼機(jī)器人控制技術(shù)進(jìn)行了幾十年的探索，已經(jīng)總結(jié)出了一套技術(shù)基礎(chǔ)。例如：文獻(xiàn)[1]針對(duì)四旋翼機(jī)器人的模糊位置控制進(jìn)行了深入的研究。當(dāng)吹風(fēng)時(shí)，四旋翼機(jī)器人難以進(jìn)行位置控制。原因是模型的不確定性。在傳統(tǒng)的線性和非線性控制器方法的情況下，依賴于精確的動(dòng)力學(xué)模型，并不明確地解決擾動(dòng)。使用模糊控制器補(bǔ)償模型不確定性控制飛行機(jī)器人的仿真模型[1]。文獻(xiàn)[2]針對(duì)四旋翼機(jī)器人運(yùn)動(dòng)難以控制，因?yàn)槠淇刂葡到y(tǒng)是具有非線性，強(qiáng)耦合和對(duì)擾動(dòng)敏感的特征的多輸入多輸出。鑒于這個(gè)問(wèn)題文獻(xiàn)[2]采用基于向量基的遺傳控制算法和模糊控制算法進(jìn)行了智能系統(tǒng)控制方法對(duì)飛行器進(jìn)行控制。文獻(xiàn)[3]針對(duì)四旋翼直升機(jī)參數(shù)的動(dòng)態(tài)建模與參數(shù)辨識(shí)，通過(guò)模擬程序獲得了適合的模型。使用相同的控制器在閉環(huán)系統(tǒng)中進(jìn)行模擬和實(shí)際系統(tǒng)的比較[3]。文獻(xiàn)[4]針介紹了四旋翼機(jī)器人的運(yùn)動(dòng)規(guī)劃穩(wěn)定性研究。還提出了特定的結(jié)構(gòu)，四旋翼懸停的同時(shí)產(chǎn)生穩(wěn)定的軌跡[4]。四旋翼機(jī)器人本體的結(jié)構(gòu)組成包含：在機(jī)身中安裝超聲波傳感器以測(cè)量飛行的實(shí)際高度，同時(shí)攜帶加速度傳感器，陀螺儀傳感器和地磁傳感器以測(cè)量旋轉(zhuǎn)角度和角速度[5-6]。攜帶的GPS/GLONASS全球定位系統(tǒng)用于檢測(cè)當(dāng)前無(wú)人機(jī)的實(shí)際位姿。此外，將運(yùn)動(dòng)控制和動(dòng)力學(xué)算法應(yīng)用到四旋翼機(jī)器人可以實(shí)現(xiàn)穩(wěn)定的飛行。由于，很難根據(jù)周圍的動(dòng)態(tài)環(huán)境來(lái)創(chuàng)建模型。鑒于此，采用模糊自適應(yīng)控制和PID控制完了對(duì)四旋翼控制器的位姿控制，通常應(yīng)用基于模型的控制方法來(lái)執(zhí)行準(zhǔn)確的位置控制。

2 四旋翼基本運(yùn)動(dòng)狀態(tài)

如圖1所示，四旋翼機(jī)器人機(jī)體的機(jī)械結(jié)構(gòu)對(duì)稱，四個(gè)旋翼均在一個(gè)水平面內(nèi)，運(yùn)動(dòng)特性具有幾何對(duì)稱的規(guī)律[7-8]。四旋翼機(jī)器人只能依靠改變電機(jī)轉(zhuǎn)子的度來(lái)進(jìn)行懸停、俯仰、滾動(dòng)和偏轉(zhuǎn)，這里所研究的四旋翼機(jī)器人采用正交方式安裝，其重心和中心重合。

圖1 四旋翼飛行器Fig.1 Four-Rotor Aircraft

四個(gè)旋轉(zhuǎn)器在一個(gè)正方形的四個(gè)頂點(diǎn)上。將四旋翼機(jī)器人從逆時(shí)針依次編隊(duì)，前后位1旋翼和3旋翼，左右為2旋翼和4旋翼。同時(shí)根據(jù)四旋翼旋轉(zhuǎn)的方向分為逆時(shí)針和順時(shí)針。并且進(jìn)行運(yùn)動(dòng)控制時(shí)，處于對(duì)角線旋翼的旋轉(zhuǎn)方向是一致的，兩個(gè)轉(zhuǎn)子在對(duì)角線上。

表1 旋翼速度控制表Tab.1 Speed Control of Four Rotor Helicopter

2.1 垂直升降與懸停

垂直升降與懸停的控制方式：同時(shí)增加或減少四個(gè)旋翼轉(zhuǎn)子的輸出功率，使得旋翼轉(zhuǎn)子轉(zhuǎn)速改變?cè)黾踊驕p少，旋翼轉(zhuǎn)子的轉(zhuǎn)速增加使得無(wú)人機(jī)整體總拉力增大或減小。當(dāng)無(wú)人機(jī)總拉力大于飛機(jī)重力時(shí)，在升力的作用下四旋翼機(jī)器人垂直上升的運(yùn)動(dòng)；當(dāng)無(wú)人機(jī)總拉力等于重力時(shí)，四旋翼機(jī)器人處于力平衡的狀態(tài)可以實(shí)現(xiàn)懸停。

2.2 前后和側(cè)向

四旋翼機(jī)器人前后和左右平移控制方式：無(wú)人機(jī)在水平面進(jìn)行前后、左右的運(yùn)動(dòng)必須首先保證其他方向無(wú)側(cè)向力，而且在此基礎(chǔ)上對(duì)在水平面內(nèi)施加一定的力使得進(jìn)行前后和左右移動(dòng)。具體來(lái)說(shuō)增加后旋翼3轉(zhuǎn)子的轉(zhuǎn)速，使得旋翼3端的側(cè)向拉力上升，同時(shí)減少前旋翼1轉(zhuǎn)子的轉(zhuǎn)速，使得旋翼1的側(cè)向拉力減少，總的拉力合理為旋翼3方向的水平拉力，同時(shí)保證反扭矩平衡，四旋翼機(jī)器人在側(cè)向力的作用下進(jìn)行側(cè)向移動(dòng)。

2.3 水平旋轉(zhuǎn)

四旋翼機(jī)器人實(shí)現(xiàn)水平旋轉(zhuǎn)首先要保持水平方向無(wú)側(cè)向力，無(wú)人機(jī)水平旋轉(zhuǎn)控制方式：保持左右旋翼的轉(zhuǎn)子輸出功率相同，前后旋翼的轉(zhuǎn)子的輸出功率相同。前后旋翼（1和3）轉(zhuǎn)子升力增加，左旋翼2轉(zhuǎn)子和右旋翼4轉(zhuǎn)子的兩端升力保持一致，并且同時(shí)下降。前旋翼1轉(zhuǎn)子與后旋翼3轉(zhuǎn)子對(duì)四旋翼機(jī)身整體的反扭矩要大于左旋翼2與右旋翼4的反扭矩，產(chǎn)生了繞Z軸旋轉(zhuǎn)的力，實(shí)現(xiàn)無(wú)人機(jī)的水平旋轉(zhuǎn)。

3 四旋翼結(jié)構(gòu)動(dòng)力學(xué)模型

四旋翼機(jī)器人四個(gè)旋翼顯十字分布，因?yàn)槠浣Y(jié)構(gòu)對(duì)稱，比起傳統(tǒng)的無(wú)人機(jī)有獨(dú)特的優(yōu)點(diǎn)，所以分析起來(lái)更為容易。在設(shè)計(jì)四旋翼機(jī)器人時(shí)結(jié)構(gòu)參數(shù)時(shí)，需要加入動(dòng)力學(xué)有限元分析，有助于研究其飛行規(guī)律，避免其設(shè)計(jì)失誤。同時(shí)在四旋翼機(jī)器人飛行控制時(shí)，加入動(dòng)力學(xué)有助于四旋翼飛行器的平穩(wěn)飛行，對(duì)于機(jī)身的單旋翼的力和力矩控制更有利于分析。四旋翼機(jī)器人是一個(gè)成熟的學(xué)科，也是一個(gè)熱門的學(xué)科。在四旋翼機(jī)器人加入動(dòng)力學(xué)有效緩解強(qiáng)烈震動(dòng)，增加穩(wěn)定性。研究的是動(dòng)力學(xué)的簡(jiǎn)化模型，不考慮阻尼力、慣性力和空氣摩擦彈性力。

3.1 無(wú)人機(jī)坐標(biāo)系

物體運(yùn)動(dòng)都是在特定的參考坐標(biāo)系下來(lái)描述的，因此建立合適的參考坐標(biāo)系是建立四旋翼機(jī)器人非線性動(dòng)力學(xué)模型的必要條件，同時(shí)可以描述旋翼飛行器線、角運(yùn)動(dòng)也需依托特定的坐標(biāo)系。如圖2所示，建立四旋翼機(jī)器人機(jī)器人的分析參考坐標(biāo)系[9]。四旋翼機(jī)器人的航向角、俯仰角和翻滾角都與四旋翼機(jī)器人的這些坐標(biāo)系聯(lián)系在一起。所以要想正確平穩(wěn)的控制無(wú)人機(jī)，就要分析各個(gè)坐標(biāo)系之間的相互關(guān)系和轉(zhuǎn)換關(guān)系式。

圖2 操作坐標(biāo)系、機(jī)體坐標(biāo)系和地面坐標(biāo)系Fig.2 Operation Coordinate System，Body Coordinate System and Ground Coordinate System

圖中：TW—四旋翼機(jī)器人西旋翼1的扭矩；TS—南旋翼2扭矩；TE—東旋翼3扭矩；TN—北旋翼4扭矩；d—旋翼升力作用點(diǎn)到質(zhì)心的距離。一般來(lái)說(shuō)分析無(wú)人機(jī)姿態(tài)需要：偏航ψ、俯仰角θ和滾轉(zhuǎn)角φ，其姿態(tài)公式如下：

3.2 動(dòng)力學(xué)特性

四旋翼機(jī)器人其動(dòng)力學(xué)特性為非線性系統(tǒng)，動(dòng)力學(xué)結(jié)構(gòu)比較復(fù)雜，一般采取的方案是簡(jiǎn)化動(dòng)力學(xué)方程。四旋翼機(jī)器人簡(jiǎn)化動(dòng)力學(xué)約束條件為：（1）旋翼相互之間為對(duì)稱結(jié)構(gòu)；（2）旋翼重心和幾何重心重合；（3）旋翼與空氣摩擦力看為0。建立的四旋翼動(dòng)力學(xué)模型為：

式中：ψ—偏航角；θ—俯仰角；φ—滾轉(zhuǎn)角；Ix，Iy，Iz—機(jī)體 x 軸，y 軸和z軸的轉(zhuǎn)動(dòng)慣量，地面坐標(biāo)系下旋翼飛行器所受到的阻力Fr=［frxfryfyz］，Jr—單個(gè)電機(jī)和旋翼一起對(duì)電機(jī)旋轉(zhuǎn)軸的轉(zhuǎn)動(dòng)慣量；m—旋翼飛行器的質(zhì)量。旋翼飛行器的輸入為：

式中：b—旋翼的拉力系數(shù)；wi—第i個(gè)旋翼的轉(zhuǎn)速；l—機(jī)械臂長(zhǎng)；kr—反扭矩系數(shù)。

4 模糊自適應(yīng)PID控制器

四旋翼機(jī)器人整體系統(tǒng)組成，如圖3所示。其中四旋翼自動(dòng)駕駛儀為無(wú)人機(jī)的重要核心部件，作為姿態(tài)控制器保證飛機(jī)能夠平穩(wěn)的飛行。姿態(tài)控制器的其作用主要是保持飛機(jī)姿態(tài)和輔助操作人員操縱無(wú)人機(jī)。通過(guò)使用經(jīng)典方法，將控制對(duì)象簡(jiǎn)化為線性和小擾動(dòng)模型，然后把問(wèn)題的重點(diǎn)放在控制小擾動(dòng)模型。

圖3 無(wú)人機(jī)自動(dòng)駕駛儀Fig.3 Four-Rotor Aircraft Autopilot

圖4 傳統(tǒng)的PID控制器原理圖Fig.4 Traditional PID Controller Schematic

公式表達(dá)式為：

式中：u（t）—被控對(duì)象的控制量；e（t）—無(wú)人機(jī)控制對(duì)象的誤差輸入量；kP、kI、kD—比例、積分和微分控制參數(shù)。

姿態(tài)控制器的輸出為三個(gè)姿態(tài)角，假設(shè)飛行器是在小姿態(tài)角下飛行，由飛行器的姿態(tài)模型可以簡(jiǎn)化為:

令輸入目標(biāo)值為Ud，滾轉(zhuǎn)角、俯仰角、偏航角目標(biāo)值和測(cè)量值分別為 φd、θd、ψd、φ、θ、ψ，則對(duì)應(yīng)的 PID 控制器可表示為式（6）所示。

4.2 模糊自適應(yīng)PID控制器設(shè)計(jì)

基于傳統(tǒng)意義的PID模型思想下，加入了模糊自適應(yīng)的理念模型，可以實(shí)現(xiàn)動(dòng)態(tài)的PI參數(shù)自調(diào)整，以適應(yīng)周圍環(huán)境變化帶來(lái)的擾動(dòng)。如圖5所示，為模糊自適應(yīng)PID控制器結(jié)構(gòu)，該控制結(jié)構(gòu)具有雙速率回路。不僅深入分析了控制結(jié)構(gòu)的抗干擾能力，而且引入了自調(diào)整因子和自學(xué)習(xí)控制規(guī)則，提高了系統(tǒng)的快速響應(yīng)能力。變速積分PID控制用于確保系統(tǒng)的高精度和穩(wěn)定性。可變模糊自適應(yīng)PID控制方法具有更好的動(dòng)態(tài)響應(yīng)性能和適應(yīng)能力，具有較少的過(guò)沖，更短的調(diào)整時(shí)間和更高的跟蹤精度。

圖5 模糊自適應(yīng)PID控制器結(jié)構(gòu)Fig.5 Fuzzy Adaptive PID Controller Structure

4.1 PID控制器設(shè)計(jì)

PID控制器一直以來(lái)被廣泛使用，其設(shè)計(jì)結(jié)構(gòu)不復(fù)雜，控制參數(shù)少和實(shí)現(xiàn)容易。PID控制器長(zhǎng)久以來(lái)是工業(yè)領(lǐng)域使用最多的控制算法[9]。同樣在旋翼飛行器的控制器中也是最常用的控制算法，傳統(tǒng)的PID控制器原理圖，如圖4所示。

5 實(shí)驗(yàn)及結(jié)果分析

5.1 飛行狀態(tài)實(shí)驗(yàn)

四旋翼機(jī)器人是一個(gè)多變量非線性的強(qiáng)耦合系統(tǒng)。四旋翼機(jī)器人控制器主要涉及的姿態(tài)控制主要是為解決各種噪聲和干擾導(dǎo)致引起的系統(tǒng)不穩(wěn)定無(wú)人機(jī)很難保持正確的飛行軌跡的問(wèn)題而提出的。四旋翼機(jī)器人在飛行過(guò)程中會(huì)出現(xiàn)噪聲干擾和質(zhì)量的變化，同時(shí)導(dǎo)致旋翼飛行器的動(dòng)力學(xué)模型發(fā)生細(xì)小的變化，導(dǎo)致動(dòng)力學(xué)參數(shù)不穩(wěn)定，該四旋翼機(jī)器人控制器可以解決外界干擾導(dǎo)致重心變化，不需要修改控制參數(shù)和控制模型，也不必修改動(dòng)力學(xué)模型。如圖6所示，為四旋翼機(jī)器人的飛行試驗(yàn)效果圖，該無(wú)人機(jī)在飛行時(shí)候能夠保持平穩(wěn)飛行，同時(shí)實(shí)時(shí)的在模糊自適應(yīng)控制控制器調(diào)整調(diào)整PID參數(shù)，以適應(yīng)環(huán)境的變化。

圖6 四旋翼機(jī)器人飛行實(shí)驗(yàn)圖Fig.6 Four-Rotor Aircraft Flight Test

5.2 姿態(tài)控制實(shí)驗(yàn)

一般來(lái)說(shuō)電機(jī)伺服有3個(gè)環(huán)，分別為位置環(huán)、速度環(huán)、電流環(huán)，其中電流環(huán)為最內(nèi)環(huán)，反應(yīng)速度最快，位置環(huán)為最外環(huán)，響應(yīng)速度最慢。如果選擇位置模式方式，位置、速度、轉(zhuǎn)矩都能控制，此方式應(yīng)用最多；而選擇速度模式，只能控制速度和轉(zhuǎn)矩；而采用轉(zhuǎn)矩模式只能控制轉(zhuǎn)矩。將模糊自適應(yīng)控制控制器應(yīng)用到四旋翼機(jī)器人進(jìn)行實(shí)時(shí)調(diào)整PID參數(shù)時(shí)采集的姿態(tài)角度跟蹤效果圖，如圖7所示。

圖7 姿態(tài)角度跟蹤效果Fig.7 Attitude Angle Tracking Effect

測(cè)試最大跟蹤角速為200°/s。從上圖情況來(lái)看，反向目標(biāo)角速度跟蹤比較好，正向跟蹤較為緩慢。具體原因在下圖中給出了解釋。

圖8 正反向運(yùn)轉(zhuǎn)跟蹤效果Fig.8 Forward and Reverse Operation Tracking Effect

Yaw角速度調(diào)節(jié)扭矩進(jìn)行了飽和限幅處理，如圖8所示。正向運(yùn)轉(zhuǎn)跟蹤速度慢，是因?yàn)檫_(dá)到了人為設(shè)定的一個(gè)扭矩限制值。而反向運(yùn)轉(zhuǎn)未達(dá)到這個(gè)飽和限制值。這就是為什么正向運(yùn)轉(zhuǎn)跟蹤速度慢，而反向運(yùn)轉(zhuǎn)跟蹤良好。從實(shí)驗(yàn)中可以看出，四旋翼機(jī)器人的控制器不僅保證了無(wú)人機(jī)飛行的過(guò)程中保持了各種姿態(tài)下穩(wěn)定飛行，而且可以保持與設(shè)定軌跡基本一致。

6 結(jié)論

針對(duì)四旋翼機(jī)器人運(yùn)動(dòng)控制、動(dòng)力學(xué)和姿態(tài)控制器的設(shè)計(jì)方法進(jìn)行了深入的研究。首先根據(jù)四旋翼機(jī)器人的對(duì)稱式機(jī)械結(jié)構(gòu)分析了其機(jī)構(gòu)特征及主要的用途。其次，根據(jù)四旋翼正交安裝的結(jié)構(gòu)特征進(jìn)行了運(yùn)動(dòng)控制分析，實(shí)現(xiàn)了懸停、前后移動(dòng)、水平移動(dòng)、俯仰運(yùn)動(dòng)和翻轉(zhuǎn)運(yùn)動(dòng)等。再次，建立了四旋翼機(jī)器人的非線性動(dòng)力學(xué)數(shù)學(xué)模型，實(shí)現(xiàn)了無(wú)人機(jī)的實(shí)時(shí)的力矩控制。最后，針對(duì)無(wú)人機(jī)在室外搬移物體引起無(wú)人機(jī)質(zhì)量和重心的變化，甚至不可避免地會(huì)受到各種噪聲和干擾導(dǎo)致無(wú)人機(jī)很難保持正確的飛行軌跡，針對(duì)這一問(wèn)題采用了基于模糊自適應(yīng)PID方法加入到了四旋翼機(jī)器人的控制器。

通過(guò)實(shí)驗(yàn)證明了無(wú)人機(jī)運(yùn)動(dòng)控制算法、動(dòng)力學(xué)算法和模糊自適應(yīng)控制器的穩(wěn)定性、準(zhǔn)確性和魯棒性。該四旋翼機(jī)器人控制器可以解決外界干擾導(dǎo)致重心變化，只需將重心偏移量添加到控制輸入中，直接適應(yīng)小型四旋翼飛行器載體重心的變化，不需要修改控制參數(shù)和控制模型，也不必修改動(dòng)力學(xué)模型。實(shí)現(xiàn)了四旋翼機(jī)器人最最重要的問(wèn)題就是保證飛行過(guò)程中各種姿態(tài)下保持穩(wěn)定飛行，同時(shí)保持與設(shè)定軌跡一致。