本發(fā)明涉及無人飛行器領(lǐng)域，具體涉及一種四旋翼無人飛行器的狀態(tài)受限控制方法和系統(tǒng)。

背景技術(shù)：

四旋翼無人飛行器是一種由四個電機驅(qū)動旋翼飛行的小型無人飛行器，具有機動性強、結(jié)構(gòu)緊湊、可以垂直起降以及空中懸停等優(yōu)點，近年來在諸多領(lǐng)域得到了廣泛的應用。同時，由于其系統(tǒng)內(nèi)部所具有的欠驅(qū)動特性：控制輸入具有四個而被控量具有六個，使得對四旋翼無人飛行器控制系統(tǒng)的設(shè)計分析具有一定的難度，因此在理論研究方面，四旋翼無人飛行器的控制器設(shè)計與分析也是一個研究熱點。

現(xiàn)有的四旋翼無人飛行器的模型大多采用歐拉角來對姿態(tài)進行描述，會發(fā)生歐拉奇異現(xiàn)象，使控制律失效；同時，歐拉角進行姿態(tài)描述會引入三角函數(shù)運算，使得系統(tǒng)的分析更為復雜；其次，現(xiàn)有的控制方法僅能夠保證誤差的收斂性能，而對動態(tài)過程的誤差變化情況沒有進行考慮，雖然系統(tǒng)穩(wěn)定，但是誤差在跟蹤過程時可能已經(jīng)超出了容許范圍，不能滿足控制要求，使得控制方案失效；此外，在常規(guī)的控制技術(shù)中，對系統(tǒng)的模型進行了簡化，不僅引入了建模誤差，而且僅僅保證了位置和姿態(tài)的各自的穩(wěn)定性，并沒有對位置與姿態(tài)之間的耦合進行分析，閉環(huán)系統(tǒng)的穩(wěn)定性沒有得到證明。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的在于克服目前四旋翼無人飛行器的控制方法存在的上述缺陷，考慮到單位四元數(shù)具有計算效率高，便于系統(tǒng)的設(shè)計與分析等優(yōu)點，本發(fā)明的方法采用單位四元數(shù)對飛行器姿態(tài)進行描述，避免了歐拉奇異現(xiàn)象，在設(shè)計控制律時引入了barrier約束函數(shù)，使得系統(tǒng)的誤差被限制在指定的范圍內(nèi)，保證了軌跡跟蹤的精度。

為實現(xiàn)上述目的，本發(fā)明提出了一種四旋翼無人飛行器的狀態(tài)受限控制方法，所述方法包括：

步驟1)獲取t時刻飛行器的位置信息、速度信息、姿態(tài)信息和角速度信息；

步驟2)根據(jù)理想軌跡計算t時刻的位置跟蹤誤差和速度跟蹤誤差，并由此設(shè)計t+1時刻位置中間控制律f；

步驟3)根據(jù)t+1時刻的位置中間控制率f，設(shè)計t+1時刻的控制升力t；

步驟4)根據(jù)t+1時刻的控制升力t，計算飛行器t+1時刻的理想姿態(tài)和理想角速度；

步驟5)根據(jù)理想姿態(tài)和理想角速度計算姿態(tài)誤差和角速度誤差；

步驟6)設(shè)計t+1時刻的姿態(tài)中間控制律β，并由此設(shè)計t+1時刻的控制力矩γ，使飛行器在t+1時刻達到理想姿態(tài)和理想角速度。

上述技術(shù)方案中，所述步驟2)具體為：

設(shè)t時刻的飛行器的位置信息為p，速度信息為v，理想軌跡信息分別為pd與vd，定義位置跟蹤誤差為速度跟蹤誤差為

則t+1時刻位置中間控制律f為：

其中kb1,kb2＞0，為位置跟蹤誤差與速度跟蹤誤差所容許的最大設(shè)定值，k^z＞0為控制增益。

上述技術(shù)方案中，所述步驟3)具體為：

將f投影到慣性坐標系中表示為：f＝(fx,fy,fz)^t，則控制升力t為：

其中，m為飛行器的質(zhì)量。

上述技術(shù)方案中，所述步驟4)具體為：

定義單位四元數(shù)為四旋翼飛行器的理想姿態(tài)，ηd為qd的標量部分，ωd為飛行器的理想角速度；

則ηd為：

理想角速度為ωd為：

其中，i3為三階的單位對角矩陣，s(qd)為3×3斜對稱矩陣：

上述技術(shù)方案中，所述步驟5)具體為：

設(shè)t時刻的飛行器的姿態(tài)信息為單位四元數(shù)q＝(q^t,η)^t＝(q1,q2,q3,η)^t，q＝(q1,q2,q3)^t為q的矢量部分，η為q的標量部分；角速度信息為ω＝(ωx,ωy,ωz)^t；

以飛行器的姿態(tài)q建立的坐標系為飛行器的本體坐標系，以飛行器的理想姿態(tài)qd建立的坐標系為飛行器的理想坐標系，則飛行器的本體坐標系與理想坐標系之間的姿態(tài)誤差以及角速度誤差為：

其中，為理想坐標系到機體坐標系的旋轉(zhuǎn)矩陣，其中：

r(q)＝(η²-||q||²)i3+2qq^t-2ηs(q)

r(qd)＝(ηd²-||qd||²)i3+2qdqd^t-2ηds(qd)

q＝(q^t,η)^t＝(q1,q2,q3,η)^t，q＝(q1,q2,q3)^t；ω＝(ωx,ωy,ωz)^t。

上述技術(shù)方案中，所述步驟6)具體為：

定義矩陣為：

if為慣性矩陣，

設(shè)計的姿態(tài)中間控制律為：

其中，k^β＞0為控制增益，

列向量

設(shè)計控制力矩γ為：

其中，中間變量ω：k^ω＞0為控制增益，

m1為對角矩陣：

設(shè)t＝0時，的前三項分別為：則kb31、kb32、kb33都為大于零的常數(shù)，且滿足

一種四旋翼無人飛行器的狀態(tài)受限控制系統(tǒng)，包括存儲器、處理器和存儲在存儲器上的并可在處理器上運行的計算機程序，其特征在于，所述處理器執(zhí)行所述程序時實現(xiàn)上述方法的步驟。

本發(fā)明的優(yōu)勢在于：

1、本發(fā)明的方法利用單位四元數(shù)對飛行器姿態(tài)進行描述，提高了計算效率，避免了奇異現(xiàn)象；

2、根據(jù)在某些狹窄環(huán)境下(如山洞、狹小的室內(nèi)空間)對四旋翼無人機軌跡跟蹤任務(wù)的精度要求，此時需要限制無人機的速度以及位置，保證軌跡跟蹤的精度，本發(fā)明的方法通過引入barrier約束函數(shù)，設(shè)計控制器(分為位置控制器以及姿態(tài)控制器)，保證位置追蹤誤差以及速度追蹤誤差的精度要求，從而間接避免了碰撞的發(fā)生。

附圖說明

圖1為四旋翼飛行器結(jié)構(gòu)圖；

圖2a為仿真實例的x軸位置誤差跟蹤圖；

圖2b為仿真實例的y軸位置誤差跟蹤圖；

圖2c為仿真實例的z軸位置誤差跟蹤圖；

圖3a為仿真實例的x軸速度誤差跟蹤圖；

圖3b為仿真實例的y軸速度誤差跟蹤圖；

圖3c為仿真實例的z軸速度誤差跟蹤圖；

圖4為仿真實例的空間三維軌跡跟蹤圖；

圖5a為仿真實例的四元數(shù)qd1跟蹤圖；

圖5b為仿真實例的四元數(shù)qd2跟蹤圖；

圖5c為仿真實例的四元數(shù)qd3跟蹤圖；

圖5d為仿真實例的四元數(shù)ηd跟蹤圖。

具體實施方式

下面結(jié)合附圖和具體實施例對本發(fā)明進行詳細的說明。

一種四旋翼無人飛行器的狀態(tài)受限控制方法，所述方法包括：

步驟1)建立四旋翼無人機飛行器的動力學方程；

如圖1所示，定義方向為北、東、地的坐標系e＝{e1,e2,e3}為慣性參考系；定義原點在四旋翼幾何中心方向為前、右、下的坐標系b＝{b1,b2,b3}為本體坐標系，均滿足右手定則。為了避免歐拉奇異現(xiàn)象且提高運算效率，使用單位四元數(shù)描述飛行器的姿態(tài)。定義單位四元數(shù)為其中稱為單位四元數(shù)矢量部分，稱為單位四元數(shù)標量部分，單位四元數(shù)的各個分量滿足q^tq+η²＝1。為從慣性坐標系到本體坐標系的旋轉(zhuǎn)矩陣，定義為：

r(q)＝(η²-||q||²)i3+2qq^t-2ηs(q)(1)

其中i3為三階的單位對角矩陣，||·||為向量歐式范數(shù)。s(q)為3×3斜對稱矩陣：

四旋翼飛行器的結(jié)構(gòu)如圖1所示，其運動學以及動力學微分方程表示為：

稱式(2)為位置子系統(tǒng)，式(3)為姿態(tài)子系統(tǒng)。其中，m為飛行器質(zhì)量，g為重力加速度，為慣性矩陣，為機體角速度。為待設(shè)計的四旋翼飛行器輸入升力，為待設(shè)計的輸入轉(zhuǎn)矩。

觀察式(2)可知，控制輸入為t，而系統(tǒng)狀態(tài)變量為p，控制量的維數(shù)小于系統(tǒng)狀態(tài)空間的維數(shù)，因此位置子系統(tǒng)是一個欠驅(qū)動系統(tǒng)。觀察式(3)可知，控制輸入為γ，飛行器姿態(tài)變量為單位四元數(shù)q＝(q^t,η)^t＝(q1,q2,q3,η)^t，q＝(q1,q2,q3)^t為q的矢量部分，η為q的標量部分；角速度信息為ω＝(ωx,ωy,ωz)^t；，該系統(tǒng)是全驅(qū)動的。為了使系統(tǒng)能夠跟蹤一個三維的理想軌跡一個合理的控制目標為保證位置p＝(px,py,pz)^t以及某一個姿態(tài)角對指令的跟蹤，其余的兩個姿態(tài)角保持鎮(zhèn)定或者隨動。

定義為四旋翼飛行器的理想姿態(tài)，ωd為機體的理想角速度。本體坐標系與理想坐標系之間的姿態(tài)誤差以及角速度誤差為：

其中，為理想坐標系到機體坐標系的旋轉(zhuǎn)矩陣，其導數(shù)關(guān)系滿足其中：

r(q)＝(η²-||q||²)i3+2qq^t-2ηs(q)

r(qd)＝(ηd²-||qd||²)i3+2qdqd^t-2ηds(qd)

四元數(shù)q＝(q^t,η)^t＝(q1,q2,q3,η)^t，q＝(q1,q2,q3)^t；角速度信息為ω＝(ωx,ωy,ωz)^t。

步驟2)設(shè)計中間控制律；

由位置子系統(tǒng)的微分方程以及圖2可知，t的方向總是與本體坐標系的豎軸b3共線，僅僅通過t無法同時跟蹤三維的理想軌跡。因此，在特定時刻四旋翼飛行器要具有某些特定的姿態(tài)，從而利用理想姿態(tài)qd將升力t投影到慣性坐標系的各個軸上以產(chǎn)生分量。

將升力t結(jié)合該時刻所需要的理想姿態(tài)qd，可以合成一個三維的控制力為

定義位置跟蹤誤差為速度跟蹤誤差為設(shè)計位置中間控制律f為

其中kb1,kb2＞0，為位置誤差與速度誤差所容許的最大設(shè)定值，k^z＞0為控制增益。利用中間控制律(6)，可以使得系統(tǒng)誤差最終滿足

其中，下標“j”為向量在慣性坐標系下x，y，z方向的分量。

步驟3)控制升力t的求解

式(6)表示了f實際值的變化，在獲取了位置信息p與速度信息v后，與理想軌跡信息pd與vd分別做差可以得到位置誤差與從而可知f在各個時刻的取值。但是f僅僅是一個虛擬的中間控制力，是由t與qd通過式(5)的運算關(guān)系合成得到，因此若想將式(6)中的f作用于系統(tǒng)，必須要通過式(5)反解出實際的控制升力t的大小，并由電機驅(qū)動產(chǎn)生升力t實現(xiàn)對系統(tǒng)的控制。

為了解出t，要用到f在慣性坐標系各坐標軸上的分量，投影到慣性坐標系中表示為：f＝(fx,fy,fz)^t。將式(5)展開為

將位置中間控制律f代入式(2)可得：

其中：

表示當前四旋翼無人機的姿態(tài)q與理想姿態(tài)qd對位置子系統(tǒng)造成的影響，對旋轉(zhuǎn)矩陣造成的誤差為

對于已知的(fx,fy,fz)^t，要通過上式來解出目標姿態(tài)以及實際升力t，共五個未知量。又因為qd滿足單位向量的約束，因此上述方程組實際具有四個未知量，沒有唯一解。為了得到一組解，可以固定qd中的某個分量，不失一般性，令qd3＝0，則：

將上式前兩行的平方相加，可得：

通過配方法解方程式(12)，可以得到一個解為：

由于單位四元數(shù)的模值為1，代入式(11)可得：

將式(13)代入式(14)可得：

因此，在完成由式(6)所描述f的設(shè)計后，可以解出實際的控制升力t為：

步驟4)理想姿態(tài)的求解

將式(16)代入式(13)，解出ηd為：

結(jié)合式(11)的前兩個方程，可以解出：

根據(jù)單位四元數(shù)運動學方程，可以得到理想角速度為ωd為：

步驟5)控制力矩γ的設(shè)計；

下面設(shè)計控制力矩γ來跟蹤步驟4)所得到的理想姿態(tài)。

根據(jù)式(3)中的四元數(shù)運動學方程及式(4)中姿態(tài)誤差的定義，可以建立模型的姿態(tài)誤差系統(tǒng)如下：

其中，矩陣定義為：

if為慣性矩陣；

引入姿態(tài)中間控制律β，令

求導可得

則

設(shè)計姿態(tài)中間控制律為：

其中，k^β＞0為控制增益，列向量

最終設(shè)計控制力矩γ為：

其中，k^ω＞0為控制增益，對角矩陣

設(shè)t＝0時，的前三項分別為：則kb31、kb32、kb33都為大于零的常數(shù)，且滿足

穩(wěn)定性分析：

由式(2)與式(3)所描述的四旋翼無人飛行器模型，控制輸入按式(6)、式(16)以及式(25)、(26)給定，當初始誤差以及滿足

可以證明閉環(huán)系統(tǒng)所有信號均有界，漸進收斂到零點，且

證明如下：

構(gòu)造lyapunov函數(shù)

對式(30)求導得

其中，kb31，kb32，kb33為大于零的常數(shù)。

將控制律式(6)、式(16)以及式(25)、(26)代入式(31)，得

又因為m1正定對角矩陣，因此又因為k^z、α、k^β、k^ω均大于零，當時，有ω≡0，因此將結(jié)果代入方程(26)代入姿態(tài)子系統(tǒng)誤差方程(22)可知將結(jié)果代入位置子系統(tǒng)誤差方程可知

因此，閉環(huán)系統(tǒng)的平衡點為r(q)＝i3。根據(jù)lasalle不變集原理可知，當t→∞時，

引理1：針對誤差動態(tài)系統(tǒng)式(34)：

其中存在連續(xù)可微并正定的函數(shù)v1和v2，kbi＞0,i＝1,2。例如設(shè)位置為x1，速度為x2，定義位置誤差e1＝x1-yd，速度誤差滿足ei→-kbi或ei→kbi時，有vi(zi)→∞。假設(shè)|e1(0)|＜kbi，取如果滿足式(35)

則|ei(t)|＜kbi,

根據(jù)引理1，式(29)可以得到證明。

仿真驗證：

取質(zhì)量為m＝0.5kg；重力加速度為g＝9.8m/s²；慣性矩陣為：

if＝diag(0.039,0.039,0.012)kg·m²?？刂圃鲆鏋椋簁^z＝5，kb1＝0.6，kb2＝0.3，kb31＝0.6，kb32＝0.6，kb33＝0.6，k^β＝20，k^ω＝10。系統(tǒng)變量初始狀態(tài)為：

p(0)＝(0,0,0)^tm，v(0)＝(0,0,0)^tm/s，

參考軌跡為：pd＝(0.5cos(t),0.5sin(t),t/10)^tm。仿真結(jié)果如下圖2a、圖2b、圖2c、圖3a、圖3b、圖3c、圖4、圖5a、圖5b、圖5c和圖5d。

以上所述的具體實施方式，對本發(fā)明的目的、技術(shù)方案和有益效果進行了進一步詳細說明，所應理解的是，以上所述僅為本發(fā)明的具體實施方式而已，并不用于限定本發(fā)明的保護范圍，凡在本發(fā)明的精神和原則之內(nèi)，所做的任何修改、等同替換、改進等，均應包含在本發(fā)明的保護范圍之內(nèi)。