本發(fā)明涉及四旋翼飛行器姿態(tài)與位置的解耦控制領(lǐng)域，特別涉及一種基于dic-pid的四旋翼飛行器姿態(tài)與位置的混合控制方法。
背景技術(shù)：
：隨著航空技術(shù)、傳感器技術(shù)及通信技術(shù)的發(fā)展，無(wú)人機(jī)因其巨大的應(yīng)用前景而受到人們的廣泛關(guān)注。在所有類型的無(wú)人機(jī)中，四旋翼飛行器機(jī)械結(jié)構(gòu)簡(jiǎn)單，易于拆裝，具有垂直起降、定點(diǎn)懸停的功能，因此它也是國(guó)內(nèi)外學(xué)者熱衷研究的對(duì)象。然而，四旋翼是一個(gè)非線性、強(qiáng)耦合的欠驅(qū)動(dòng)系統(tǒng)，其動(dòng)力學(xué)方程并不滿足疊加性與齊次性，故要實(shí)現(xiàn)其高精度的姿態(tài)與位置控制難度頗大。針對(duì)上述問題，國(guó)內(nèi)外的學(xué)者們對(duì)四旋翼飛行器的控制策略進(jìn)行了一系列相關(guān)研究，包括反步法控制、自抗擾控制、自適應(yīng)滑?？刂频?。近年來(lái)的研究表明基于動(dòng)態(tài)逆控制(dynamicinversioncontrol，簡(jiǎn)稱dic)來(lái)設(shè)計(jì)四旋翼的控制器是一種有效途徑。dic的基本原理是通過構(gòu)造系統(tǒng)的全局狀態(tài)反饋來(lái)消除系統(tǒng)的非線性，使原系統(tǒng)轉(zhuǎn)變?yōu)樾碌木€性系統(tǒng)。例如，利用dic理論分別設(shè)計(jì)了四旋翼的內(nèi)外回路控制，實(shí)現(xiàn)了姿態(tài)與位置的解耦控制。但是，這種單純的dic控制策略會(huì)因外界擾動(dòng)與建模不確定性而導(dǎo)致控制效果不佳。因此，考慮多種控制策略結(jié)合的方法受到學(xué)者們的青睞。例如：將dic理論與定量反饋理論相機(jī)和設(shè)計(jì)了四旋翼姿態(tài)回路的魯棒控制器，并在氣動(dòng)參數(shù)攝動(dòng)的情況下驗(yàn)證了控制器的有效性。在系統(tǒng)回路中引入模型參考自適應(yīng)與dic結(jié)合的方法設(shè)計(jì)了px4四旋翼的姿態(tài)與位置控制律，仿真結(jié)果表明該控制器能較快適應(yīng)外界擾動(dòng)與負(fù)載變化。技術(shù)實(shí)現(xiàn)要素：為了克服現(xiàn)有技術(shù)中的不足，本發(fā)明提供一種基于dic-pid的四旋翼飛行器姿態(tài)與位置的混合控制方法，該控制方法對(duì)外界擾動(dòng)與未建模動(dòng)態(tài)有較好的抑制能力，能夠滿足四旋翼飛行器姿態(tài)控制與位置控制的需求。為了達(dá)到上述發(fā)明目的，解決其技術(shù)問題所采用的技術(shù)方案如下：一種基于dic-pid的四旋翼飛行器姿態(tài)與位置的混合控制方法，包括以下步驟：步驟1：將四旋翼飛行器當(dāng)作單剛體，通過牛頓-歐拉方程獲得其非線性動(dòng)力學(xué)模型，確立系統(tǒng)的輸入輸出關(guān)系；步驟2：基于dic動(dòng)態(tài)逆控制理論針對(duì)四旋翼飛行器的內(nèi)回路進(jìn)行姿態(tài)控制，形成內(nèi)環(huán)姿態(tài)控制器；步驟3：基于pid理論針對(duì)四旋翼飛行器的外回路進(jìn)行位置控制，形成外環(huán)位置控制器，實(shí)現(xiàn)姿態(tài)與位置通道的解耦控制；步驟4：在仿真環(huán)境下，通過階躍響應(yīng)和跟蹤上升螺旋線檢驗(yàn)所提出的控制方法。進(jìn)一步的，在步驟1中具體包括：在機(jī)體坐標(biāo)系obxbybzb下，四旋翼飛行器產(chǎn)生的升力f＝[f1f2f3f4]t正向obzb方向；當(dāng)f1＝f2＝f3＝f4，飛行器做垂直起降或懸停運(yùn)動(dòng)；當(dāng)f2＝f4且f1≠f3，飛行器做俯仰運(yùn)動(dòng)；當(dāng)f1＝f3且f≠f4，飛行器做橫滾運(yùn)動(dòng)；當(dāng)f1＝f3≠f2＝f4時(shí)，飛行器做偏航運(yùn)動(dòng)；基于上述驅(qū)動(dòng)原理，可得到四旋翼控制輸入的表達(dá)形式：其中，l為旋翼中心線各槳葉法向線的距離，u＝[u1,u2,u3,u4]t為俯仰、橫滾、偏航與總距輸入信號(hào)，m＝[m1,m2,m3,m4]t為各旋翼的力矩；根據(jù)牛頓-歐拉方程，可以推導(dǎo)出四旋翼飛行器的動(dòng)力學(xué)方程：其中，m為四旋翼總質(zhì)量，為大地坐標(biāo)系oixiyizi到機(jī)體坐標(biāo)系的轉(zhuǎn)換矩陣，g為重力加速度，fd為空氣阻力，i＝[ixxiyyizz]t為轉(zhuǎn)動(dòng)慣量，q＝[φθψ]t為姿態(tài)矩陣，d為姿態(tài)角到角速度的映射關(guān)系，s(·)為旋量矩陣，φ、θ和ψ分別為橫滾角、俯仰角和偏航角。進(jìn)一步的，若不考慮空氣阻力的影響，在低速飛行或懸停狀態(tài)下，有φ≈0、θ≈0，另外，由四旋翼結(jié)構(gòu)對(duì)稱性可得ixx≈iyy，同時(shí)，由于不需要頻繁控制偏航角，有因此，公式(2)可近似為：進(jìn)一步的，在步驟2中具體包括：設(shè)定非線性系統(tǒng)為：為了跟蹤期望軌跡r(t)，定義跟蹤誤差為：e(t)＝r(t)-y(t)(5)若對(duì)式(4)中系統(tǒng)輸出進(jìn)行微分，則有：當(dāng)g(x)可逆時(shí)，定義動(dòng)態(tài)逆控制器為：其中，v為逆動(dòng)態(tài)模型的控制量，將公式(6)代入公式(5)中，可得誤差動(dòng)態(tài)系統(tǒng)為：利用線性控制技術(shù)配置v使系統(tǒng)極點(diǎn)位于原點(diǎn)處，令：v＝ke(9)其中，k為整定矩陣，因此，系統(tǒng)完整的動(dòng)態(tài)逆控制器為：進(jìn)一步的，在步驟2中，內(nèi)回路的被控量為三軸姿態(tài)角，內(nèi)回路的輸出向量為y1c＝[φθψ]t，逆動(dòng)態(tài)控制量為：因此，內(nèi)回路的控制律為：進(jìn)一步的，在步驟3中，外回路的被控量為三軸位置與線速度，公式(3)給出了四旋翼相對(duì)于大地坐標(biāo)系的線加速度，位置變量y1w＝[xyz]t可通過期望輸入信號(hào)與姿態(tài)角獲得，外回路中的線加速度為：采用pid控制技術(shù)可得到線加速度為：其中，kp、ki和kd分別為比例系數(shù)、積分系數(shù)與微分系數(shù)；ew為位置誤差；ev為線速度誤差。本發(fā)明由于采用以上技術(shù)方案，使之與現(xiàn)有技術(shù)相比，具有以下的優(yōu)點(diǎn)和積極效果：(1)本發(fā)明采用dic-pid控制策略設(shè)計(jì)了四旋翼飛行器姿態(tài)與位置的控制器，實(shí)現(xiàn)了其在模型不確定和外界擾動(dòng)下的魯棒控制。(2)相比于基于pid的四旋翼飛行器控制系統(tǒng)，本發(fā)明提出的控制系統(tǒng)控制效果更好，魯棒性更強(qiáng)，穩(wěn)定性更優(yōu)，能夠解決四旋翼易受未建模動(dòng)態(tài)與外界擾動(dòng)影響的問題。(3)實(shí)現(xiàn)了四旋翼飛行器姿態(tài)與位置的解耦控制，仿真測(cè)試與飛行實(shí)驗(yàn)驗(yàn)證了本發(fā)明所設(shè)計(jì)控制策略的有效性。附圖說明為了更清楚地說明本發(fā)明實(shí)施例的技術(shù)方案，下面將對(duì)實(shí)施例描述中所需要使用的附圖作簡(jiǎn)單的介紹。顯而易見，下面描述中的附圖僅僅是本發(fā)明的一些實(shí)施例，對(duì)于本領(lǐng)域技術(shù)人員來(lái)講，在不付出創(chuàng)造性勞動(dòng)的前提下，還可以根據(jù)這些附圖獲得其他的附圖。附圖中：圖1是本發(fā)明一種基于dic-pid的四旋翼飛行器姿態(tài)與位置的混合控制方法的流程示意圖；圖2是本發(fā)明中四旋翼飛行器受力圖；圖3是本發(fā)明中四旋翼飛行器的控制系統(tǒng)圖；圖4是本發(fā)明中四旋翼飛行器的內(nèi)回路控制系統(tǒng)圖；圖5是本發(fā)明中四旋翼飛行器在陣風(fēng)擾動(dòng)下的位置控制軌跡圖；圖6是本發(fā)明中四旋翼飛行器的三維跟蹤軌跡圖；圖7是本發(fā)明中四旋翼飛行器姿態(tài)角的變化示意圖；圖8是本發(fā)明中四旋翼飛行器實(shí)驗(yàn)平臺(tái)飛行試驗(yàn)效果圖。具體實(shí)施方式以下將結(jié)合本發(fā)明的附圖，對(duì)本發(fā)明實(shí)施例中的技術(shù)方案進(jìn)行清楚、完整的描述和討論，顯然，這里所描述的僅僅是本發(fā)明的一部分實(shí)例，并不是全部的實(shí)例，基于本發(fā)明中的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒有做出創(chuàng)造性勞動(dòng)的前提下所獲得的所有其他實(shí)施例，都屬于本發(fā)明的保護(hù)范圍。如圖1所示，本發(fā)明公開了一種基于dic-pid的四旋翼飛行器姿態(tài)與位置的混合控制方法，包括以下步驟：步驟1：將四旋翼飛行器當(dāng)作單剛體，通過牛頓-歐拉方程獲得其非線性動(dòng)力學(xué)模型，確立系統(tǒng)的輸入輸出關(guān)系；步驟2：基于dic動(dòng)態(tài)逆控制理論針對(duì)四旋翼飛行器的內(nèi)回路進(jìn)行姿態(tài)控制，形成內(nèi)環(huán)姿態(tài)控制器；步驟3：基于pid理論針對(duì)四旋翼飛行器的外回路進(jìn)行位置控制，形成外環(huán)位置控制器，實(shí)現(xiàn)姿態(tài)與位置通道的解耦控制；步驟4：在仿真環(huán)境下，通過階躍響應(yīng)和跟蹤上升螺旋線檢驗(yàn)所提出的控制方法。國(guó)際上通常將四旋翼飛行器當(dāng)作六自由度剛體建模。如圖2所示，其在沿著自身法向升力的作用下在三維空間內(nèi)自由運(yùn)動(dòng)，則在步驟1中具體包括：在機(jī)體坐標(biāo)系obxbybzb下，四旋翼飛行器產(chǎn)生的升力f＝[f1f2f3f4]t正向obzb方向；當(dāng)f1＝f2＝f3＝f4，飛行器做垂直起降或懸停運(yùn)動(dòng)；當(dāng)f2＝f4且f1≠f3，飛行器做俯仰運(yùn)動(dòng)；當(dāng)f1＝f3且f≠f4，飛行器做橫滾運(yùn)動(dòng)；當(dāng)f1＝f3≠f2＝f4時(shí)，飛行器做偏航運(yùn)動(dòng)；基于上述驅(qū)動(dòng)原理，可得到四旋翼控制輸入的表達(dá)形式：其中，l為旋翼中心線各槳葉法向線的距離，u＝[u1,u2,u3,u4]t為俯仰、橫滾、偏航與總距輸入信號(hào)，m＝[m1,m2,m3,m4]t為各旋翼的力矩；根據(jù)牛頓-歐拉方程，可以推導(dǎo)出四旋翼飛行器的動(dòng)力學(xué)方程：其中，m為四旋翼總質(zhì)量，為大地坐標(biāo)系oixiyizi到機(jī)體坐標(biāo)系的轉(zhuǎn)換矩陣，g為重力加速度，fd為空氣阻力，i＝[ixxiyyizz]t為轉(zhuǎn)動(dòng)慣量，q＝[φθψ]t為姿態(tài)矩陣，d為姿態(tài)角到角速度的映射關(guān)系，s(·)為旋量矩陣，φ、θ和ψ分別為橫滾角、俯仰角和偏航角。進(jìn)一步的，若不考慮空氣阻力的影響，在低速飛行或懸停狀態(tài)下，有φ≈0、θ≈0，另外，由四旋翼結(jié)構(gòu)對(duì)稱性可得ixx≈iyy，同時(shí)，由于不需要頻繁控制偏航角，有因此，公式(2)可近似為：進(jìn)一步的，在步驟2中具體包括：設(shè)定非線性系統(tǒng)為：為了跟蹤期望軌跡r(t)，定義跟蹤誤差為：e(t)＝r(t)-y(t)(5)若對(duì)式(4)中系統(tǒng)輸出進(jìn)行微分，則有：當(dāng)g(x)可逆時(shí)，定義動(dòng)態(tài)逆控制器為：其中，v為逆動(dòng)態(tài)模型的控制量，將公式(6)代入公式(5)中，可得誤差動(dòng)態(tài)系統(tǒng)為：利用線性控制技術(shù)配置v使系統(tǒng)極點(diǎn)位于原點(diǎn)處，令：v＝ke(9)其中，k為整定矩陣，因此，系統(tǒng)完整的動(dòng)態(tài)逆控制器為：根據(jù)時(shí)間尺度分離原理，由內(nèi)向外依次設(shè)計(jì)四旋翼飛行器的控制器，如圖3所示。其中，內(nèi)回路進(jìn)行姿態(tài)控制，外回路進(jìn)行位置控制。內(nèi)回路的設(shè)計(jì)是為了鎮(zhèn)定四旋翼的姿態(tài)，被控量為三軸姿態(tài)角。如圖4所示，在步驟2中，內(nèi)回路的被控量為三軸姿態(tài)角，內(nèi)回路的輸出向量為y1c＝[φθψ]t，逆動(dòng)態(tài)控制量為：因此，內(nèi)回路的控制律為：進(jìn)一步的，外回路的設(shè)計(jì)是為了鎮(zhèn)定四旋翼的位置，在步驟3中，外回路的被控量為三軸位置與線速度，公式(3)給出了四旋翼相對(duì)于大地坐標(biāo)系的線加速度，位置變量y1w＝[xyz]t可通過期望輸入信號(hào)與姿態(tài)角獲得，外回路中的線加速度為：采用pid控制技術(shù)可得到線加速度為：其中，kp、ki和kd分別為比例系數(shù)、積分系數(shù)與微分系數(shù)；ew為位置誤差；ev為線速度誤差。為了驗(yàn)證所提混合控制方法的有效性，在matlab2012b的仿真環(huán)境下對(duì)四旋翼飛行器的控制性能進(jìn)行驗(yàn)證，仿真中所選四旋翼飛行器的參數(shù)如表1所示：參數(shù)l(m)m(kg)ixx(kg.m2)iyy(kg.m2)izz(kg.m2)數(shù)值0.2500.6120.00450.00450.0053表1四旋翼飛行器的物理參數(shù)case1：給定參考階躍信號(hào)為(1.5m,1m,0.5m)，分別采用本發(fā)明提出的混合控制方法與現(xiàn)有技術(shù)中的pid控制策略去跟蹤此信號(hào)。為了更好地模擬實(shí)際飛行，在輸入端引入幅值為0.05米的陣風(fēng)擾動(dòng)信號(hào)，仿真結(jié)果如圖5所示。從圖中可以看出，兩種控制器均呈現(xiàn)出大的超調(diào)量。相對(duì)而言，dic-pid控制器的響應(yīng)速度明顯要快于pid控制器。同時(shí)，dic-pid的穩(wěn)定時(shí)間小于5s，且穩(wěn)態(tài)誤差不大于2％。這說明在陣風(fēng)擾動(dòng)的情況下，本文所提控制算法依然可以提供相對(duì)穩(wěn)定的響應(yīng)。case2：給定三維參考軌跡為一個(gè)上升螺旋線，軌跡方程如下所示：為驗(yàn)證dic-pid的抗干擾能力，在仿真中引入一個(gè)0.1m/s2的外界干擾，整個(gè)仿真持續(xù)30s，結(jié)果如圖6所示。從圖中可以看出，在初始階段，參考軌跡與仿真軌跡存在較大的誤差，這里需指明該誤差是由于四旋翼跟蹤偏差所導(dǎo)致。但整體上講，dic-pid控制器響應(yīng)速度很快，能夠快速精確地跟蹤上參考軌跡。另外，圖7給出了四旋翼飛行器姿態(tài)角的變化情況，4.3s時(shí)內(nèi)環(huán)便趨于穩(wěn)定。同時(shí)，圖7也給出了四個(gè)通道的控制信號(hào)。飛行實(shí)驗(yàn)建立在上一節(jié)測(cè)試環(huán)境的基礎(chǔ)上，實(shí)驗(yàn)中的四旋翼飛行器經(jīng)過改裝，裝載有g(shù)ps、飛控、減振板、數(shù)傳等。設(shè)定四旋翼沿這樣的軌跡飛行：起始點(diǎn)-a-b-c-d-e-著陸點(diǎn)。先在仿真環(huán)境下確定dic-pid控制器參數(shù)，再通過matlabde的代碼轉(zhuǎn)化功能將控制算法轉(zhuǎn)換成c代碼寫入到飛控中進(jìn)行試驗(yàn)驗(yàn)證。這里需指出，由于實(shí)驗(yàn)室缺少測(cè)量噪聲干擾的設(shè)備，故在仿真中添加不同的噪聲信號(hào)來(lái)模擬實(shí)際擾動(dòng)，最后選擇一組最優(yōu)或次優(yōu)的控制器參數(shù)。圖8給出了飛行實(shí)驗(yàn)的結(jié)果，可以看出根據(jù)本發(fā)明設(shè)計(jì)的控制器四旋翼飛行器基本能夠跟蹤上期望軌跡，可實(shí)現(xiàn)對(duì)四旋翼姿態(tài)與位置進(jìn)行有效控制。以上所述，僅為本發(fā)明較佳的具體實(shí)施方式，但本發(fā)明的保護(hù)范圍并不局限于此，任何熟悉本
技術(shù)領(lǐng)域：
的技術(shù)人員在本發(fā)明揭露的技術(shù)范圍內(nèi)，可輕易想到的變化或替換，都應(yīng)涵蓋在本發(fā)明的保護(hù)范圍之內(nèi)。因此，本發(fā)明的保護(hù)范圍應(yīng)該以權(quán)利要求的保護(hù)范圍為準(zhǔn)。當(dāng)前第1頁(yè)12