本發(fā)明屬于無人機導(dǎo)航制導(dǎo)與控制技術(shù)領(lǐng)域，具體涉及一種基于逆動力學(xué)的無人機三維軌跡制導(dǎo)方法。

背景技術(shù)：

無人機又稱無人駕駛飛行器，廣泛應(yīng)用于軍用和民用領(lǐng)域；無人機制導(dǎo)是指通過指令程序，使無人機沿給定的軌跡飛行。隨著無人機的執(zhí)行任務(wù)日趨多樣，人們對無人機機動性能的要求也日益提高，傳統(tǒng)的二維軌跡制導(dǎo)律已無法滿足需求，實現(xiàn)三維軌跡的高精度跟蹤控制，能讓無人機完成地形回避，編隊飛行以及自主空中加油等特殊任務(wù)，具有重要的意義。

在無人機的軌跡跟蹤控制中，實現(xiàn)無人機直接沿給定航路點飛行具有重要的實用價值，目前很多無人機的參考軌跡設(shè)計方法是將各航路點以空間三維曲線進行擬合，或?qū)④壽E拆分成若干段分別進行擬合，這類方法雖然可得到較為光滑的參考軌跡，但算法計算量較大，特別是面對復(fù)雜參考軌跡時，其擬合難度將直線上升，這對于機載計算機設(shè)備性能較差的低成本無人機來說是非常不利的。

另外，從無人機的飛行動力學(xué)角度來看，制導(dǎo)回路所屬的各控制量之間存在高度耦合的關(guān)系。其中，無人機的位置方程為

式中，(x,y,h)為無人機的三維坐標；

地速及航跡角狀態(tài)方程為

(V_g,χ,γ)分別為無人機的地速、無人機的航跡偏角和無人機的航跡傾角；(α,β,μ)分別為無人機的迎角、無人機的側(cè)滑角和無人機的航跡滾轉(zhuǎn)角；(T,F_D,F_Y,F_L)分別為無人機的發(fā)動機推力、無人機的阻力、無人機的側(cè)力和無人機的升力；m和g分別為無人機質(zhì)量和重力加速度。

由式(1)(2)可見，(V_g,χ,γ)和(T,α,β,μ)均為相互耦合的控制量，若采用傳統(tǒng)PID控制，控制效果必然會因耦合作用而受到嚴重影響。

基于上述分析，所設(shè)計的三維軌跡制導(dǎo)方法應(yīng)滿足如下要求：

(1)能夠在飛行包線約束條件下，同時控制無人機的地速和位置，精確跟蹤三維軌跡，并實現(xiàn)無人機直接沿給定航路點飛行任務(wù)；

(2)期望飛行軌跡便于線生成和調(diào)整，且所需的時間和計算成本較??；

(3)制導(dǎo)方法物理意義明確，形式簡潔，參數(shù)整定方便，易于工程實現(xiàn)。

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

本發(fā)明的目的是為了解決上述問題，并滿足上述要求，提出一種基于逆動力學(xué)的無人機三維軌跡制導(dǎo)方法，具體包括如下步驟：

步驟一、針對某個無人機，根據(jù)期望軌跡將無人機飛行經(jīng)過的航路點設(shè)定坐標，并劃分軌跡段；

期望軌跡由若干航路點聯(lián)結(jié)成的航路段構(gòu)成；航路段共有n(n≥1)段，對應(yīng)有n+1個航路點。

步驟二、針對每個航路點，根據(jù)無人機的機動性能和所設(shè)定好的航路情況，設(shè)定該航路點對應(yīng)的轉(zhuǎn)換半徑為d；

步驟三、針對第k段軌跡段，利用無人機的期望軌跡分別計算該軌跡段的實際速度誤差和實際位移誤差，采用PID控制律加和構(gòu)成該軌跡段期望的橫側(cè)向位移狀態(tài)的微分項和縱向位移狀態(tài)的微分項

初始k＝1；

首先，無人機的實際速度誤差包括橫側(cè)向速度誤差和縱向速度誤差計算如下：

是無人機在第k段軌跡段上設(shè)定的指令跟蹤橫側(cè)向速度；是無人機當(dāng)前在第k段軌跡段上實際的橫側(cè)向速度；是無人機在第k段軌跡段上設(shè)定的指令跟蹤縱向速度；是無人機當(dāng)前在第k段軌跡段上實際的縱向速度；V_gk是第k段軌跡段上為無人機設(shè)定的指令地速；γ_k是第k個軌跡段所指向方向的航跡傾角；χ_k是第k個軌跡段所指向方向的航跡偏角；V_g是無人機當(dāng)前在第k段軌跡段上實際的地速；γ是無人機當(dāng)前在第k段軌跡段上實際的航跡傾角；χ是無人機當(dāng)前在第k段軌跡段上實際的航跡偏角。

然后，無人機的實際位移誤差包括橫側(cè)向位移誤差(y_line-y)和縱向位移誤差(h_line-h)；計算如下：

從無人機當(dāng)前實際位置處作此時對應(yīng)直線軌跡的空間垂線，得到垂足坐標(x_line,y_line,h_line)，該垂線段即為無人機的位置誤差總和，分解為橫側(cè)向位移誤差(y_line-y)和縱向位移誤差(h_line-h)。

最后，PID控制律加和具體為：對無人機的位移誤差(y_line-y)和(h_line-h)采用PI控制，將速度誤差和作為微分項，并將PI控制與微分項求和，PID的參數(shù)即為速度誤差和位移誤差在飛行制導(dǎo)律中分別所占的權(quán)重，得到期望的橫側(cè)向位移狀態(tài)的微分項和縱向位移狀態(tài)的微分項

步驟四、將期望的微分項和微分項分別帶入橫側(cè)向位移狀態(tài)和縱向位移狀態(tài)的微分方程中，采用解析法和數(shù)值迭代法相結(jié)合的方法對微分方程進行第一級逆動力學(xué)解算，輸出指令航跡偏角χ_c和航跡傾角γ_c；

首先，將期望的微分項帶入縱向位移狀態(tài)的微分方程中，采用解析法解算出指令航跡傾角γ_c，計算如下：

然后，將期望的微分項和指令航跡傾角γ_c帶入到橫側(cè)向位移狀態(tài)的微分方程中，利用數(shù)值迭代法解算出指令航跡偏角χ_c，計算如下：

步驟五、將指令航跡偏角χ_c和指令航跡傾角γ_c以及無人機在該航路段上的指令地速V_gk作為下一級逆動力學(xué)解算的輸入指令值，采用解析法和數(shù)值迭代法相結(jié)合的方法對無人機地速、航跡傾角和航跡偏角的狀態(tài)方程進行第二級逆動力學(xué)解算，輸出指令推力T_c、指令迎角α_c和指令航跡滾轉(zhuǎn)角μ_c；

具體步驟如下：

步驟501、將指令航跡偏角χ_c，指令航跡傾角γ_c和指令地速V_gk分別與當(dāng)前無人機實際的航跡偏角χ，航跡傾角γ和地速V_g作差，并采用PD控制，分別得到期望的航跡偏角微分值期望的航跡傾角微分值和期望的地速微分值

步驟502、利用當(dāng)前無人機的實際地速V_g和航跡傾角γ，以及期望的航跡偏角微分值和期望的航跡傾角微分值計算指令航跡滾轉(zhuǎn)角μ_c的解析解；

步驟503、利用指令航跡滾轉(zhuǎn)角μ_c的解析解，結(jié)合期望地速狀態(tài)方程，代入期望航跡傾角狀態(tài)方程中，得到關(guān)于指令迎角α_c的非線性一元方程；

步驟504、用數(shù)值迭代法對關(guān)于指令迎角α_c的非線性一元方程進行解算，得到指令迎角α_c。

步驟505、將指令迎角α_c代入期望地速狀態(tài)方程中，解算出指令推力T_c；

期望地速狀態(tài)方程如下：

步驟六、將輸出的指令推力T_c、指令迎角α_c、側(cè)滑角0°和航跡滾轉(zhuǎn)角μ_c作為無人機姿態(tài)控制回路的輸入，即可實現(xiàn)對無人機三維軌跡的跟蹤控制；

步驟七、當(dāng)無人機飛行至第k段軌跡段對應(yīng)的轉(zhuǎn)換半徑d的范圍內(nèi)時，則無人機飛到所對應(yīng)的航路點位置，繼續(xù)將跟蹤下一段軌跡，返回至步驟三，直至追蹤至最后一個航路點。

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

(1)一種基于逆動力學(xué)的無人機三維軌跡制導(dǎo)方法，可同時精確的控制無人機的地速和三維位置。

(2)一種基于逆動力學(xué)的無人機三維軌跡制導(dǎo)方法，可在線調(diào)整無人機的期望飛行軌跡，且控制器計算成本低。

(3)一種基于逆動力學(xué)的無人機三維軌跡制導(dǎo)方法，控制結(jié)構(gòu)簡單，各部分物理意義明確，便于參數(shù)整定。

附圖說明

圖1是本發(fā)明整個制導(dǎo)控制器的結(jié)構(gòu)框圖；

圖2是本發(fā)明一種基于逆動力學(xué)的無人機三維軌跡制導(dǎo)方法流程圖；

圖3是本發(fā)明采用的三維制導(dǎo)方法中存在的幾何關(guān)系示意圖；

圖4是本發(fā)明采用解析法和數(shù)值迭代法相結(jié)合進行第二級逆動力學(xué)解算的流程圖；

圖5a是無人機實際位置與對應(yīng)直線航路段之間的距離誤差圖；

圖5b是無人機地速誤差圖；

圖5c是無人機XY平面軌跡跟蹤效果圖；

圖5d是無人機XZ平面軌跡跟蹤效果圖；

圖5e是無人機YZ平面軌跡跟蹤效果圖。

具體實施方式

下面將結(jié)合附圖和實施例對本發(fā)明作進一步的詳細說明。

本發(fā)明一種基于逆動力學(xué)的無人機三維軌跡制導(dǎo)方法，通過將解析法和數(shù)值迭代法相結(jié)合，依次對無人機的位置狀態(tài)方程進行一級逆動力學(xué)解算，以及對無人機的地速和航跡角狀態(tài)方程進行二級逆動力學(xué)解算，得到無人機的指令推力、指令迎角和指令航跡滾轉(zhuǎn)角，再將這幾個指令量輸入已設(shè)計好的姿態(tài)控制回路，即可控制無人機的實際地速和位置，同時采用PID控制，使無人機的實際位置收斂于航路點之間的參考軌跡上。

整個制導(dǎo)控制器的結(jié)構(gòu)框圖如圖1，其整體工作流程為：

針對某個無人機，首先設(shè)定好各航路點的坐標，根據(jù)指令航路點坐標，根據(jù)無人機當(dāng)前位置(x,y,h)獲取計算出期望航跡角，并設(shè)定好無人機在各航路段上的指令地速；根據(jù)指令地速和期望航跡角，可計算出無人機在橫側(cè)向與縱向的指令跟蹤地速分量和令其分別于無人機實際的地速分量和分別作差，獲取無人機的實際速度誤差和再乘以系數(shù)K_D，作為PID控制器的微分項；

同時根據(jù)指令航路點坐標，生成對應(yīng)的各直線軌跡段，從無人機的當(dāng)前位置向當(dāng)前對應(yīng)的直線航路段作垂線，垂足坐標為(x_line,y_line,h_line)，令y_line和h_line分別與無人機的實際橫側(cè)向坐標y與縱向高度h作差，計算無人機在各直線軌跡段上的實際位移誤差(y_line-y)和(h_line-h)；并并采用PI控制律，分別乘以系數(shù)K_P和K_I；

接下來要采用PID控制，構(gòu)成期望的橫側(cè)向和縱向位移狀態(tài)的微分值和作為第一級逆動力學(xué)解算的輸入，將PI控制項與微分項分別對應(yīng)加和構(gòu)成和作為第一級逆動力學(xué)解算的輸入，采用解析法與數(shù)值迭代法相結(jié)合的逆動力學(xué)方法解算出指令航跡角，作為第二級逆動力學(xué)解算的輸入，解算出指令推力T_c、指令迎角α_c和指令航跡滾轉(zhuǎn)角μ_c。將T_c輸入至無人機動力控制模塊，將α_c和μ_c輸入至無人機的姿態(tài)控制回路中，進一步生成無人機的相應(yīng)舵角指令，從而同時控制無人機的姿態(tài)與地速。

如圖2所示，具體包括如下步驟：

步驟一、針對某個無人機，根據(jù)期望軌跡將無人機飛行經(jīng)過的航路點設(shè)定坐標，并劃分軌跡段；

根據(jù)設(shè)定的無人機飛行所需要經(jīng)過的航路點坐標，構(gòu)成無人機的期望軌跡，并將軌跡劃分為若干軌跡段；無人機的期望軌跡由若干航路點所聯(lián)結(jié)成的航路段構(gòu)成，設(shè)航路點按序號可聯(lián)結(jié)為n(n≥1)段直線航路，對應(yīng)有n+1個航路點。

步驟二、針對每個航路點，根據(jù)無人機的機動性能和所設(shè)定好的航路情況，設(shè)定該航路點對應(yīng)的轉(zhuǎn)換半徑為d；

本實施例中將每個航路點的轉(zhuǎn)換半徑均設(shè)定為d。

步驟三、針對第k段軌跡段，利用無人機的期望軌跡分別計算該軌跡段的實際速度誤差和實際位移誤差，采用PID控制律加和構(gòu)成該軌跡段期望的橫側(cè)向位移狀態(tài)的微分項和縱向位移狀態(tài)的微分項

將無人機基于航路點的期望軌跡跟蹤問題劃分為速度誤差收斂問題和位置誤差收斂問題，分別設(shè)計相應(yīng)的PID控制策略，再將所設(shè)計的PID控制律加和構(gòu)成期望橫側(cè)向位移狀態(tài)和縱向位移狀態(tài)的微分項；

具體為：

(1)無人機速度誤差收斂問題

如圖3所示，當(dāng)無人機UAV收斂于第k(1≤k≤n)段航路上時，無人機UAV的三維坐標為(x,y,h)，第k-1個航路點WP_k-1(x_k-1,y_k-1,h_k-1)與第k個航路點WP_k(x_k,y_k,h_k)之間的航跡偏角χ_k∈[0,2π)和航跡傾角γ_k∈(-π/2,π/2)即為此時無人機的期望航跡偏角和航跡傾角，其表達式為：

其中y_k-1和y_k起到了指令橫側(cè)向位移的作用，h_k-1和h_k起到了指令縱向位移的作用，χ_k和γ_k即為所要跟蹤的虛擬航跡角；χ_k是第k個軌跡段所指向方向的航跡偏角；γ_k是第k個軌跡段所指向方向的航跡傾角；

注意：當(dāng)k＝1時，第k-1個航路點為(x_k-1,y_k-1,h_k-1)＝(x₀,y₀,h₀)，該點被定義為飛行的起點。

將第k個軌跡段所要跟蹤的虛擬航跡角χ_k和γ_k以及無人機此時設(shè)定的指令地速V_gk代入式(1)，得到此時的指令跟蹤橫側(cè)向速度和縱向速度其與實際速度的誤差分別為和表達式為

(2)無人機位置誤差收斂問題

如圖3所示，從無人機當(dāng)前實際位置處作此時所對應(yīng)第k段航路的垂線，可得垂足坐標FP(x_line,y_line,h_line)，該垂線段即為無人機的位置誤差總和，可分解為橫側(cè)向位移誤差(y_line-y)和縱向位移誤差(h_line-h)。其中，y_line起著指令橫側(cè)向位移的作用，h_line起著指令縱向位移的作用。

也就是說，y_k-1，h_k-1，y_k，h_k，y_line和h_line均起到了指令位置的作用，其中，y_k和h_k是驅(qū)使無人機收斂到指令橫側(cè)向和縱向速度的位置指令，y_line和h_line是驅(qū)使無人機收斂到指令軌跡的位置指令，制導(dǎo)方法中存在的幾何關(guān)系如圖1所示，此時需要對位置指令進行加權(quán)求和處理，本發(fā)明引入了PID控制的思路來解決這一問題。

對于指令軌跡的收斂問題，其位移誤差(y_line-y)和(h_line-h)采用PI控制，從而使無人機能夠無差收斂到指令軌跡上；對于指令速度的收斂問題，通過式(3)和(4)已經(jīng)求得了橫側(cè)向和縱向速度誤差和該項可直接作為微分項。將PI控制的結(jié)構(gòu)與微分項求和，其中PID的參數(shù)即為速度誤差收斂問題和位置誤差收斂問題分別在飛行制導(dǎo)律中所占的權(quán)重，可得期望的橫側(cè)向位移狀態(tài)的微分項和縱向位移狀態(tài)的微分項

步驟四：將步驟三中所得的期望狀態(tài)的微分項和分別帶入橫側(cè)向位移狀態(tài)的微分方程和縱向位移狀態(tài)的微分方程中，采用解析法和數(shù)值迭代法相結(jié)合的方法對無人機的橫側(cè)向位移和縱向位移狀態(tài)方程進行第一級逆動力學(xué)解算，輸出指令航跡偏角χ_c和航跡傾角γ_c。

用期望橫側(cè)向位移狀態(tài)的微分項和縱向位移狀態(tài)的微分項替代式(1)中的對應(yīng)的微分項和

首先，針對期望縱向位移的狀態(tài)方程，通過解析法解算出指令航跡傾角γ_c的解析解：

然后，將解算出的γ_c帶入到期望橫側(cè)向位移的微分狀態(tài)方程中，可得僅包含未知數(shù)χ_c的一元方程：

利用數(shù)值方法，例如牛頓迭代法解算出指令航跡偏角χ_c。

步驟五：將步驟四解算出的航跡偏角χ_c和航跡傾角γ_c以及設(shè)定的無人機指令地速V_gk作為下一級逆動力學(xué)解算的輸入，采用解析法和數(shù)值迭代法相結(jié)合的方法對無人機地速、航跡傾角和航跡偏角的狀態(tài)方程進行第二級逆動力學(xué)解算，輸出指令推力T_c、指令迎角α_c和指令航跡滾轉(zhuǎn)角μ_c。

如圖4所示，具體步驟如下：

步驟501、將指令航跡偏角χ_c，指令航跡傾角γ_c和指令地速V_gk分別與當(dāng)前無人機實際的航跡偏角χ，航跡傾角γ和地速V_g作差，并采用PD控制，分別得到期望的航跡偏角微分值期望的航跡傾角微分值和期望的地速微分值

步驟502、利用當(dāng)前無人機的實際地速V_g和航跡傾角γ，以及期望的航跡偏角微分值和期望的航跡傾角微分值計算指令航跡滾轉(zhuǎn)角μ_c的解析解；

由于無人機采用BTT方式轉(zhuǎn)彎，側(cè)滑角β約等于0，此時側(cè)力F_Y也約等于0，因此式(2)的三個方程可化簡為

上式中的微分值和被其期望值和所替換，聯(lián)立式(8)和(9)，可得指令航跡滾轉(zhuǎn)角的解析解：

步驟503、利用指令航跡滾轉(zhuǎn)角μ_c的解析解，結(jié)合期望地速狀態(tài)方程，代入期望航跡傾角狀態(tài)方程中，得到關(guān)于指令迎角α_c的非線性一元方程；

將解算出的指令航跡滾轉(zhuǎn)角μ_c和式(7)代入式(9)中，得只含未知數(shù)指令迎角α_c的非線性一元方程：

注：阻力F_D和升力F_L在此時均可視為變量α_c的函數(shù)；

步驟504、用數(shù)值迭代法對關(guān)于指令迎角α_c的非線性一元方程進行解算，得到指令迎角α_c。

步驟505、將指令迎角α_c代入期望地速狀態(tài)方程中，解算出指令推力T_c；

期望地速狀態(tài)方程如下：

將α_c代入式(7)，即可解算出指令推力T_c。

步驟六、將輸出的指令推力T_c、指令迎角α_c、側(cè)滑角0°和航跡滾轉(zhuǎn)角μ_c作為無人機姿態(tài)控制回路的輸入，即可實現(xiàn)對無人機三維軌跡的跟蹤控制；

步驟七：當(dāng)無人機飛行至距離目前所跟蹤軌跡段的對應(yīng)航路點d的位置時，即可認為無人機已飛到所對應(yīng)的航路點位置，此時無人機將跟蹤下一段軌跡，整個制導(dǎo)算法循環(huán)至步驟三，直至追蹤至最后一個航路點。

本實施例中，設(shè)定無人機以200m/s的地速飛行，初始高度為7010m；采用本發(fā)明方法的風(fēng)擾條件下，獲取的無人機三維軌跡跟蹤效果圖；

如圖5a所示，是無人機實際位置與對應(yīng)直線航路段之間的距離誤差圖，可以看出無人機在進行恒速爬升及轉(zhuǎn)彎動作時，位置不可避免的存在一定程度的超調(diào)，但很快位置誤差就能收斂到接近0的程度；

如圖5b所示，是無人機地速誤差圖，可以看出無人機在進行恒速爬升及轉(zhuǎn)彎動作時，會不可避免的存在一定的速度損失，但很快速度誤差就能收斂到接近0的程度，且速度損失不大；

如圖5c，5d和5e所示，分別是XY平面軌跡跟蹤效果圖，XZ平面軌跡跟蹤效果圖和YZ平面軌跡跟蹤效果圖，這三個圖直觀的體現(xiàn)了無人機的實際飛行過程，可以看出：在爬升-拉平階段，縱向高度上一開始存在約100m左右的超調(diào)，但是很快無人機就恢復(fù)到指令軌跡上了；在45°大角度水平轉(zhuǎn)彎時，高度變化僅在10m-20m左右，橫側(cè)向位置超調(diào)達到約400m后，能較快收斂到指令位置，相對于無人機200m/s的高速，該位置超調(diào)是較為可觀的。