四旋翼兩棲機器人姿態(tài)求解與控制

，，，，，，3，，

(1.中國科學院合肥物質(zhì)科學研究院先進制造技術(shù)研究所，江蘇 常州 213164；

2．電子科技大學自動化系，四川 成都 611731；3. 中國科學技術(shù)大學自動化系，安徽 合肥 230027)

Attitude Solving and Control of an Amphibious Robot Based on Four-rotor

LI Tao1，WEI Qiang2，F(xiàn)U Long1，LUO Minzhou1，CHEN Saixuan1，WANG Meiling1，3，LIU Xiao1，ZHUANG Xiaoming1

(1．Institute of Advanced Manufacturing Technology，Hefei Institute of Physical Science CAS，Changzhou 213164，China；

2．Department of Automation of University of Electronic Science and Technology of China，Chengdu 611731，China；

3. Department of Automation of University of Science and Technology of China，Hefei 230026，China)

四旋翼兩棲機器人姿態(tài)求解與控制

李濤1，魏強2，付龍1，駱敏舟1，陳賽旋1，王美玲1，3，劉效1，莊曉明1

(1.中國科學院合肥物質(zhì)科學研究院先進制造技術(shù)研究所，江蘇 常州 213164；

2．電子科技大學自動化系，四川 成都 611731；3. 中國科學技術(shù)大學自動化系，安徽 合肥 230027)

Attitude Solving and Control of an Amphibious Robot Based on Four-rotor

LI Tao1，WEI Qiang2，F(xiàn)U Long1，LUO Minzhou1，CHEN Saixuan1，WANG Meiling1，3，LIU Xiao1，ZHUANG Xiaoming1

(1．Institute of Advanced Manufacturing Technology，Hefei Institute of Physical Science CAS，Changzhou 213164，China；

2．Department of Automation of University of Electronic Science and Technology of China，Chengdu 611731，China；

3. Department of Automation of University of Science and Technology of China，Hefei 230026，China)

摘要：介紹了一種基于四旋翼驅(qū)動的兩棲移動機器人。首先簡要介紹了該機器人的機械結(jié)構(gòu)與控制及傳感系統(tǒng)，并介紹了機器人由四旋翼機構(gòu)提供動力，并通過對4個旋翼的轉(zhuǎn)動速度和方向進行配置，從而實現(xiàn)在空中飛行或在地面滾動的原理。然后，采用四元數(shù)方法對該兩棲機器人進行了姿態(tài)求解，在此基礎(chǔ)上，基于PID算法開發(fā)了機器人的飛行控制算法，并進行了相應的仿真。最后通過實驗驗證了該兩棲機器人能夠?qū)崿F(xiàn)預期的兩種運動模式，即空中飛行和地面滾動。該機器人提高了傳統(tǒng)只具有單一運動模式的移動機器人的環(huán)境適應能力。

關(guān)鍵詞：移動機器人；四旋翼機構(gòu)；兩棲機器人；姿態(tài)求解；運動控制

中圖分類號：TP242

文獻標識碼：A

文章編號：1001-2257(2015)10-0062-05

收稿日期：2015-06-16

基金項目：中國科學院合肥物質(zhì)科學研究院院長基金青年火花項目(YZJJ201323)

Abstract：This paper mainly introduces the attitude solving and control of an amphibious robot based on four-rotor mechanism. Firstly，it introduces the mechanism，control and sensor system，then it introduces the working principle that four-rotor works as actuator，and by configuring rotating velocity and direction of the four rotors，the robot can both fly in the air and rolling on the ground. Then，the attitude solving is carried out by using quaternion method，and then control algorithm is developed as based on PID algorithm，after then corresponding simulation is implemented. Finally，experiments are presented to verify that the robot can generate two motion modes，namely flying in the air and rolling on the ground，as expected. This amphibious robot enhances the environment adaptability of conventional mobile robots which just have a single motion mode.

作者簡介：李濤(1984-)，男，安徽霍邱人，助理研究員，博士，研究方向為服務機器人；魏強(1992-)，男，安徽霍邱人，碩士研究生，研究方向為服務機器人。

Key words：mobile robot；four-rotor mechanism；amphibious robot；attitude solving；motion control

0引言

現(xiàn)有的移動機器人移動方式各異，常見的有輪式、腿式、蠕動及飛行等方式[1-]5]，它們的運動性能各有優(yōu)點，但也存在著不足，主要表現(xiàn)在環(huán)境適應的局限性。受運動方式的限制，目前研制出的基于這些運動方式的移動機器人大多只適用于單一作業(yè)環(huán)境，即或地面、或水下、或空中。近年來也出現(xiàn)了少量的兩棲機器人，如水陸兩棲機器人[6-]7]，是以兩棲動物蛇為仿生對象開發(fā)的機器人。

介紹了一種基于四旋翼的兩棲機器人，通過創(chuàng)新機構(gòu)設計，將四旋翼機構(gòu)與柔性球殼機構(gòu)集成到一起，該機器人既可以實現(xiàn)空中飛行，又可以實現(xiàn)地面滾動，且兩種運動模式共用一套驅(qū)動系統(tǒng)。此外，當機器人在兩種運動模式間來回切換時，在柔性球殼機構(gòu)的保護下，可以有效地避免機器人因撞擊而導致的損壞。

1機器人系統(tǒng)設計

機器人機械結(jié)構(gòu)如圖1所示，主要由四旋翼機構(gòu)和柔性球殼機構(gòu)耦合而成。四旋翼機構(gòu)是機器人的動力來源，而球殼機構(gòu)的作用是當機器人在空中飛行時作為保護裝置，而在地面滾動時，球殼機構(gòu)即作為保護裝置又作為運動裝置。四旋翼機構(gòu)與球殼機構(gòu)之間通過軸承連接，從而使得旋翼機構(gòu)可以繞長軸相對于球殼機構(gòu)進行轉(zhuǎn)動。該四旋翼兩棲機器人可以實現(xiàn)空中飛行和地面滾動兩種運動方式，兩種運動方式均由四旋翼提供動力，通過配置各旋翼的轉(zhuǎn)速及轉(zhuǎn)向，可以實現(xiàn)機器人在空中的上下、前后、左右飛行、側(cè)向飛行及懸停等，也可以實現(xiàn)機器人在地面上的前后滾動及轉(zhuǎn)向。

圖1　機器人機械結(jié)構(gòu)

兩棲機器人是由旋翼提供的升力完成各項目標運動，依賴于對自身狀態(tài)及旋翼的快速響應保持機身系統(tǒng)的穩(wěn)定。兩棲機器人的硬件系統(tǒng)主要包括飛行控制系統(tǒng)、導航系統(tǒng)、電源及動力系統(tǒng)、遙控及無線通信模塊以及其他任務設備，兩棲機器人控制系統(tǒng)的硬件結(jié)構(gòu)如圖2所示。其中飛行控制系統(tǒng)是整個系統(tǒng)的核心，用于收集傳感器信息，解算飛行姿態(tài)輸出PWM波，接收遙控信號及返回飛行信息等，導航系統(tǒng)用于采集飛行信息，將飛行信息上傳到控制模塊中，電源及動力系統(tǒng)為整個系統(tǒng)供電及為機體的飛行提供升力，遙控通過接收機向控制系統(tǒng)發(fā)送PPM信號，用于控制機器人的升降及調(diào)整機器人的飛行姿態(tài)，無線通信模塊用于機器人返回飛行信息，監(jiān)測機器人的飛行狀態(tài)。其他任務設備可能含有攝像機等，用于機器人執(zhí)行其他需要的任務。

圖2　兩棲機器人控制系統(tǒng)的硬件結(jié)構(gòu)

2機器人姿態(tài)求解

在四旋翼的導航系統(tǒng)中，首先設導航坐標系為OnXnYnZn，為了確定飛行器的導航參數(shù)并對其的位姿信息進行數(shù)學描述，采用載體坐標系ObXbYbZb，即在四旋翼上機器人上建立坐標系，原點選在四旋翼的中心，X軸沿四旋翼縱軸向前，Y軸沿四旋翼橫軸向左，Z軸垂直于機體向上。設載體坐標系在初始時刻與導航坐標系重合，載體坐標系經(jīng)過偏航方向、俯仰方向、橫滾方向上3個角度的旋轉(zhuǎn)后得到新的載體坐標系Ob′Xb′Yb′Zb′。

2.1　姿態(tài)角

設在某一坐標系下存在一向量R，用范數(shù)為1的四元數(shù)Q描述該坐標系的旋轉(zhuǎn)R′，則有：

(1)

設有2個四元數(shù)A和B，其中A=a0+a1i+a2j+a3k，B=b0+b1i+b2j+b3k，設C=c0+c1i+c2j+c3k為A和B兩四元數(shù)的乘積，即C= A·B，則有：

(2)

設Q=q0+q1i+q2j+q3k，且設

(3)

可由四元數(shù)得出姿態(tài)矩陣：

(4)

則可得姿態(tài)角：

(5)

對于任意旋轉(zhuǎn)，可用一單位四元數(shù)Q=cos(θ/2)+ usin(θ/2)表示，其中u為單位向量。用Q描述導航坐標系到載體坐標系的旋轉(zhuǎn)，則w=wg=QwbQ*，其中wg，wb分別為角速度在導航坐標系和載體坐標系的表述。因為Q為單位四元數(shù)，即Q·Q*=Q*·Q=1，所以有：

(6)

將上面的微分方程離散化便可由初始時刻四元數(shù)的值及各時刻的三軸角速率獲得接下來各個時刻的四元數(shù)的值，便可由四元數(shù)矩陣獲得當前的載體姿態(tài)。

2.2　基于陀螺儀、加速度計、磁力計的姿態(tài)求解

兩棲機器人配備了陀螺儀、加速度計和磁力計。通過陀螺儀得出3個軸上的角速度wb=[wbxwbywbz]，使用式(6)的離散形式由初始四元數(shù)不斷以角速度矩陣更新得出實時的四元數(shù)，再由式(5)便可求出當前載體的姿態(tài)角。需要說明的是單純使用加速度計無法提取到偏航方向的角度，而磁力計的引入則能解出偏航方向的角度變化。由于單個磁力計在測量過程中存在著誤差，故使用多傳感器進行姿態(tài)融合，得到較為精確的姿態(tài)數(shù)據(jù)?；赑I調(diào)節(jié)的姿態(tài)融合的基本思想為以陀螺儀的數(shù)據(jù)不斷累加解出當前姿態(tài)矩陣為基礎(chǔ)，以加速度計與磁力計作為反饋，通過姿態(tài)矩陣得出理想輸出并與實際輸出的向量積作為誤差，通過PI調(diào)節(jié)加至陀螺儀的累加過程中進行數(shù)據(jù)融合不斷更新四元數(shù)。將導航坐標系的理論加速度向量[00g]經(jīng)過四元數(shù)旋轉(zhuǎn)矩陣轉(zhuǎn)換至載體坐標系上，利用載體上加速度計實際的輸出值與轉(zhuǎn)換后的理論值做向量積誤差輸入到PI控制器上，經(jīng)過PI控制器補償?shù)酵勇輧x上。

使用上述方法得出的解出的靜態(tài)漂移的姿態(tài)角如圖3所示。從圖中可以看出基于上述方法解算出的姿態(tài)角靜態(tài)精度為0.2°且解算速度可以達到2 ms一次，能夠滿足飛行器的飛行要求。四旋翼飛行器是一個強耦合、欠驅(qū)動、非線性的控制系統(tǒng)，對其進行有效準確且穩(wěn)定的控制是四旋翼飛行器設計中一個很大的難點。雖然四旋翼飛行器有4個電機輸入，但是卻要控制6個自由度的輸出，分別為平移的3個自由度和旋轉(zhuǎn)3個自由度，再加上四旋翼飛行過程中旋翼間存在相互影響和干擾就造成了四旋翼在控制上具有強耦合、非線性等的特點。本章將簡單介紹應用于四旋翼上的控制方法，并基于以上建立的簡化模型進行控制仿真和軟件設計。

圖3　靜態(tài)漂移的姿態(tài)角

3機器人姿態(tài)控制

控制模型的不確定性以及存在的外界干擾等都會增加姿態(tài)控制的復雜度。該機器人應用了經(jīng)典的PID控制策略，該種控制方法的控制結(jié)構(gòu)較為簡單，適應性強，魯棒性好，且不需要預先知道被控對象的確切模型，也能達到較好的控制效果。在以上建立的動力學模型及PID控制的基礎(chǔ)上，將四旋翼飛行器的控制算法分成兩個回路，即為姿態(tài)控制回路與位置控制回路，四旋翼控制模型如圖4所示。

圖4　四旋翼控制模型

姿態(tài)回路是四旋翼能夠穩(wěn)定飛行的基本回路，其是位置控制回路的控制前提，決定了飛行器的控制品質(zhì)，其具體作用為控制飛行器的飛行姿態(tài)，使各姿態(tài)角與設定的姿態(tài)角一致。姿態(tài)控制回路有3個控制量分別為俯仰角φ、橫滾角θ、偏航角ψ，忽略各個姿態(tài)角間的耦合關(guān)系，對3個角分別進行獨立的PID控制。

由上面推導得姿態(tài)的動力學模型為：

(7)

由動力學方程的推導得，u2，u3，u4與電機轉(zhuǎn)速的平方成正比，分別以u2，u3，u4為控制量，則其有如下的控制形式：

(8)

由控制方程與系統(tǒng)的動力學模型對姿態(tài)環(huán)進行Simulink仿真，仿真參數(shù)如下：Iφ=Iθ=0.004 9 kg·m2，Iψ=0.008 8 kg·m2。因為在四旋翼的穩(wěn)定飛行過程中不可能存在著很大的橫滾角和俯仰角的變化，因此該仿真是在機體小角度旋轉(zhuǎn)下進行的，橫滾角的仿真結(jié)果如圖5所示。此時通道的比例增益為3，積分增益為0.08，微分增益為2。仿真設計的階躍的穩(wěn)定值為0.1 rad，換成角度值約為6°，由圖中可以看出，仿真的超調(diào)量小于0.05 rad，轉(zhuǎn)換成角度值小于3°，滿足四旋翼小角度變化的要求。雖然仿真中在0.08 s左右才達到預定值0.1 rad，但是可以看出由于Y軸的變化范圍較小，在遠小于0.08 s的時間內(nèi)角度的調(diào)節(jié)就已經(jīng)達到了可接受的范圍內(nèi)，即可視為穩(wěn)定。

圖5　橫滾角的仿真結(jié)果

俯仰角的仿真結(jié)果如圖6所示。同橫滾通道一樣，俯仰通道的比例增益為3，積分增益為0.08，微分增益為2。得出的仿真結(jié)果與橫滾角的仿真結(jié)果類似，說明了四旋翼的結(jié)構(gòu)具有對稱性。

圖6　俯仰角的仿真結(jié)果

偏航角的仿真結(jié)果如圖7所示。仿真偏航通道時的比例增益為5.8，積分增益為0.05，微分增益為2.5。其角度的單位也為弧度制，超調(diào)量小于3°，調(diào)節(jié)時間小于0.05 s，滿足四旋翼的飛行要求。

圖7　偏航角的仿真結(jié)果

在做姿態(tài)環(huán)仿真的同時通過Z軸的動力學方程還對機體的高度進行了控制，仿真結(jié)果如圖8所示。圖中給定值為10 m，可以看出超調(diào)量小于2 m，調(diào)節(jié)時間約為2 s，仿真的結(jié)果在實際飛行中可以接受。

圖8　Z軸高度仿真結(jié)果

4實驗驗證

在理論分析與機器人系統(tǒng)設計的基礎(chǔ)上，加工制作了實驗樣機，并進行了實驗。實驗包括兩部分，一部分為機器人的飛行測試，主要針對四旋翼機器人在室外的平穩(wěn)飛行，自主起飛進行試驗，以證明機器人能夠?qū)崿F(xiàn)前文中所述的姿態(tài)控制，即懸停、上升、下降、前進、后退、左右側(cè)飛以及滾轉(zhuǎn)等；另一部分為機器人的地面滾動測試，主要是驗證機器人能夠?qū)崿F(xiàn)在指定的起始點與目標點間做可控的來回滾動以及轉(zhuǎn)向的試驗。通過以上兩部分的測試來驗證機器人分析和設計的有效性及合理性。

圖9為四旋翼在室外進行的飛行試驗，機器人可以保持較為平穩(wěn)的飛行姿態(tài)。試驗時發(fā)現(xiàn)當油門信號較小時，機體會存在些許振蕩，需要一定時間才能平穩(wěn)下來，當油門信號較大時，機體能夠在很短的時間內(nèi)穩(wěn)定下來。綜合飛行可能室外環(huán)境存在風力等的干擾，當機體油門信號較小時由于調(diào)節(jié)的力度不夠大使得機體穩(wěn)定需要較長的時間，當油門信號較大時由于調(diào)節(jié)力較大可以很快使得機體穩(wěn)定。此外，在室外飛行測試過程中還對四旋翼機器人外殼的保護作用進行了測試，測試過程中，在10 m左右的高度下人為使機器人失去控制信號從而模擬意外掉落，當機器人撞擊地面時，由于彈性球殼機構(gòu)的保護作用，機器人從地面小幅彈起幾次而并沒有損壞，證明了外殼的設計起到了預期的保護作用。

圖9　四旋翼室外飛行試驗

兩棲機器人滾動試驗如圖10所示。試驗中，事先給定機器人的運動的起點A、中轉(zhuǎn)點B和終點C，然后發(fā)送控制命令使機器人先從A點滾動到B點，再由B點滾動到C點。實現(xiàn)了該機器人在地面上朝著目標點的前進和后退滾動，同時實現(xiàn)了轉(zhuǎn)向運動，從而驗證了機器人結(jié)構(gòu)設計的合理性。

圖10　機器人的地面滾動試驗

5結(jié)束語

介紹了一種既可以實現(xiàn)空中飛行又能夠?qū)崿F(xiàn)地面滾動的兩棲移動機器人，通過將四旋翼機構(gòu)和柔性球殼機構(gòu)有效耦合，實現(xiàn)了通過四旋翼對兩種運動模式的驅(qū)動。在姿態(tài)角求解和姿態(tài)控制的基礎(chǔ)上，通過開發(fā)試驗樣機進行試驗，改進了設計中存在的不足，實現(xiàn)了對機器人的有效控制，實現(xiàn)了預計的功能。該兩棲機器人改進了傳統(tǒng)只具備單一運動模式的移動機器人的環(huán)境適應性。

參考文獻：

Ziv N，Lee Y，Ciaravella G. Inline skating motion generator with passive wheels for small size humanoid robots // 2010 IEEE/ASME International Conference on Advanced Intelligent Mechatronics Montréal，Canada，2010：1391-1395.

王揚威，王振龍，李健. 仿生機器魚研究進展及發(fā)展趨勢.機械設計與研究，2011(02)：22-25.

Hashimoto K，Hosobata K，Sugahara Y，et al. Realization by Biped Leg-wheeled Robot of Biped Walking and Wheel-driven Locomotion //International Conference on Robotics and Automation，Barcelona，Spain，2006：2970-22975.

Carbone G，Suciu M，Ceccarelli M,et al. Design and simulation of Cassino hexapode walking machine . International Journal of Mechanics and Control，2009，10(2)：27-34.

侯宇，方宗德，孔建益，等.仿生撲翼飛行微機器人研究現(xiàn)狀與關(guān)鍵技術(shù). 機械設計，2008，25(7)：1-4.

蔡舒文，饒進軍，肖輔龍.仿蜥蜴水面機器人腳掌設計.機械設計與研究，2012，28(1)：22-25.

Tesch M，Lipkin K，Brown I. Parameterized and scripted gaits for modular snake robots . Advanced Robotics，2009，23(9)：1131-1158.