技術(shù)領(lǐng)域
[0001] 本
發(fā)明涉及無人飛機系統(tǒng)（Unmanned Aerial Vehicle，簡稱UAV）自主導(dǎo)航的非線性導(dǎo)航尋跡控制方法，屬于
飛行器導(dǎo)航控制方法領(lǐng)域，適用于無人飛機系統(tǒng)自主導(dǎo)航的控制方法和應(yīng)用技術(shù)研究。
背景技術(shù)
[0002] 無人駕駛飛機簡稱“無人機”，是利用無線電遙控遙測設(shè)備和自備的程序控制裝置操縱的不載人飛機。機上安裝有導(dǎo)航
飛行控制系統(tǒng)、程序控制裝置以及動
力和電源等設(shè)備。地面遙控遙測站人員通過數(shù)據(jù)鏈等設(shè)備，對其進行
跟蹤、
定位、遙控、遙測和進行實時數(shù)據(jù)傳輸。與載人飛機相比，它具有適應(yīng)多種飛行環(huán)境要求的特點，特別是可以承擔(dān)人力所不及的長航時飛行或高
風(fēng)險飛行，飛行航線和
姿態(tài)控制
精度高，可廣泛用于航空遙感、氣象研究、農(nóng)業(yè)飛播和
病蟲害防治中；在戰(zhàn)爭中更具有特殊的優(yōu)勢，可廣泛用于空中偵察、監(jiān)視、通信、反潛、
電子干擾和武器打擊等。
[0003] 無人機的飛行
控制器主要包括導(dǎo)航級和控制級，其中無人機導(dǎo)航級的基本任務(wù)是精確確定無人機載
水平空間中的
位置，解決飛機如何以預(yù)定
空速飛行在預(yù)定高度的問題，以及如何轉(zhuǎn)彎飛往目標(biāo)問題，通過
算法給出飛機需要的
俯仰角、
油門和
橫滾角，并使其能按預(yù)定航線飛行，然后交給控制級進行控制解算。線性的導(dǎo)航控制算法以被廣泛地應(yīng)用于無人機飛行器中，例如：采用傳統(tǒng)的PID控制方法，根據(jù)
偏航距（當(dāng)前無人機的地理位置與預(yù)定的航跡路線的距離）計算飛機的導(dǎo)航角。在PID控制方法中，如果當(dāng)前無人機繼續(xù)偏離預(yù)設(shè)的航線飛行時，導(dǎo)航角的比例項與
積分項符號相同，增大積分項的運算，使得無人機更快地向預(yù)設(shè)的航線飛行；如果當(dāng)前無人機靠近預(yù)設(shè)的航線飛行時，導(dǎo)航角的比例項與積分項的符號相反，阻礙了積分項的運算，減慢或停止無人機繼續(xù)向靠近預(yù)設(shè)航線的方向飛行，避免了積分超調(diào)或者無人機在預(yù)設(shè)的航線上振蕩的現(xiàn)象。
[0004] 但是，傳統(tǒng)的PID算法仍具有以下不足之處：（1）在復(fù)雜的規(guī)劃航跡任務(wù)中由于偏航距的計算誤差較大，傳統(tǒng)的PID算法導(dǎo)航效果較差，壓航線效果較差，準(zhǔn)確度不高。（2）由于PID算法是線性的，PID參數(shù)整定的過程中必然會遇到系統(tǒng)
穩(wěn)定性與準(zhǔn)確性之間的矛盾，往往是取比例、積分和微分三部分控制作用的折衷，難于收到最佳的效果。并且，PID參數(shù)不能動態(tài)地進行調(diào)整，準(zhǔn)確度不高。
發(fā)明內(nèi)容
[0005] 針對
現(xiàn)有技術(shù)中存在的上述不足，本發(fā)明的目的在于提供一種無人機的非線性導(dǎo)航尋跡控制方法，用以提高對無人機導(dǎo)航控制的準(zhǔn)確度，使得無人機能夠更好地實現(xiàn)壓航線飛行，解決現(xiàn)有技術(shù)中對無人機航線控制準(zhǔn)確度不高的問題。
[0006] 為解決上述技術(shù)問題，實現(xiàn)發(fā)明目的，本發(fā)明采用的技術(shù)方案如下：
[0007] 一種無人機的非線性導(dǎo)航尋跡控制方法，其特征在于，通過GPS定位實時地獲取當(dāng)前無人機的地理位置和飛行方向，并以固定的導(dǎo)航控制周期執(zhí)行導(dǎo)航尋跡控制；每當(dāng)導(dǎo)航控制周期到達時，執(zhí)行導(dǎo)航尋跡控制的步驟包括：
[0008] （1）判斷當(dāng)前的飛行模式，若當(dāng)前飛行模式為盤旋式的飛行模式，執(zhí)行步驟8；若當(dāng)前飛行模式為直線式的飛行模式，執(zhí)行步驟2；
[0009] （2）確定目標(biāo)距離LAC，單次飛行距離L1_dist和航線偏離系數(shù)X；所述目標(biāo)距離LAC是指當(dāng)前無人機的地理位置到預(yù)設(shè)的航線終點的地理位置之間的距離；所述單次飛行距離L1_dist是指無人機在一個導(dǎo)航控制周期內(nèi)飛行的距離；所述航線偏離系數(shù)X是指目標(biāo)距離LAC在預(yù)設(shè)的直線式航線上的投影和目標(biāo)距離LAC之間的比值；
[0010] 所述單次飛行距離L1_dist按下式確定：
[0011]
[0012] 其中，L1_period表示一個導(dǎo)航控制周期，L1_damping表示阻尼系數(shù)，v表示無人機飛行的速度；
[0013] 所述航線偏離系數(shù)X按下式確定：
[0014] X=l/LAC，
[0015] 其中，l表示目標(biāo)距離LAC在預(yù)設(shè)的直線式航線上的投影，l按下式確定：
[0016] 表示當(dāng)前無人機的地理位置到預(yù)設(shè)的航線終點的距離向量；用來表示預(yù)設(shè)的直線式航線，具體是指預(yù)設(shè)的航線起始點到預(yù)設(shè)的航線終點的距離向量；
表示預(yù)設(shè)的航線起始點到預(yù)設(shè)的航線終點之間的距離；
[0017] （3）判斷單次飛行距離L1_dist是否小于目標(biāo)距離LAC，如果是，執(zhí)行步驟4；否則執(zhí)行步驟6；
[0018] （4）判斷航線偏離系數(shù)X是否大于0.7071，如果是，執(zhí)行步驟5，否則，執(zhí)行步驟6；
[0019] （5）確定直線式飛行夾角ηs，并執(zhí)行步驟7；所述直線式飛行夾角ηs是指從當(dāng)前無人機的飛行方向到距離向量 所在方向之間的夾角；
[0020] 直線式飛行夾角ηs按下式確定：
[0021]
[0022] 其中，α0取值為1或-1；當(dāng)從當(dāng)前無人機的飛行方向到距離向量 所在的方向為順
時針方向，α0為1；當(dāng)從當(dāng)前無人機的飛行方向到距離向量 所在的方向為逆時針方向，α0為-1；vl1表示速度v在距離向量 所在方向上的分向量的模，vx1表示速度v在距離向量 和距離向量 所在平面內(nèi)與距離向量 方向垂直的方向上的分向量的模；vl1和vx1分別按照下式確定：
[0023]
[0024] （6）按下式確定直線式飛行夾角ηs；并執(zhí)行步驟7；
[0025] ηs=α1*η1+α2*η2，
[0026] 其中 ds表示直線式偏航距，所述直線式偏航距ds是指當(dāng)前無人機的地理位置到預(yù)設(shè)的直線式航線 之間的距離， α1取值為1或-1；
當(dāng)從距離向量 所在方向到預(yù)設(shè)的直線式航線 所在的方向為順時針方向，α1為1；
當(dāng)從距離向量 所在方向到預(yù)設(shè)的直線式航線 所在的方向為逆時針方向，α1為-1；
α2取值為1或-1；當(dāng)從當(dāng)前無人機的飛行方向到預(yù)設(shè)的直線式航線
所在的方向為順時針方向，α2為1；當(dāng)從當(dāng)前無人機的飛行方向到預(yù)設(shè)的直線式航線 所在的方向為逆時針方向，α2為-1；vl2表示速度v在距離向量 所在方向上的分向量的模，vx2表示速度v在距離向量 和距離向量 所在平面內(nèi)與距離向量 方向垂直的方向上的分向量的模；vx2和vl2分別按照下式確定：
[0027]
[0028] （7）按下式確定無人機的橫向
加速度acmds，并執(zhí)行步驟12；
[0029] acmds=KLS*v2/L1_dist*sinηs；
[0030] 其中KLS表示直線式飛行系數(shù)，KLS=4.0*L1_damping*L1_damping；
[0031] （8）判斷盤旋距離 是否小于預(yù)設(shè)的盤旋半徑R，所述盤旋距離 是指當(dāng)前無人機的地理位置到預(yù)設(shè)的航線盤旋中心點之間的距離；如果是，執(zhí)行步驟9，否則，執(zhí)行步驟10；
[0032] （9）確定盤旋式飛行夾角ηr和無人機的
向心加速度acmdr，執(zhí)行步驟11；所述盤旋式飛行夾角ηr是指從當(dāng)前無人機的飛行方向到距離向量 所在方向之間的夾角；所述距離向量 是指當(dāng)前無人機的地理位置到預(yù)設(shè)的航線盤旋中心點的距離向量；
[0033] 所述盤旋式飛行夾角ηr按下式確定：
[0034]
[0035] 其中，α3取值為1或-1；當(dāng)從當(dāng)前無人機的飛行方向到距離向量 所在的方向為順時針方向，α3為1；當(dāng)從當(dāng)前無人機的飛行方向到距離向量 所在的方向為逆時針方向，α3為-1；
[0036] vx3和vl3分別按下式確定：
[0037]
[0038] vl3表示速度v在距離向量 所在方向上的速度分量，vx3表示速度v在與距離向量 所在方向上垂直的方向上的速度分量；
[0039] 所述無人機的向心加速度acmdr按下式確定為：
[0040] acmdr=KLR*v2/L1_dist*sinηr；
[0041] 其中 L1_period表示一個導(dǎo)航控制周期，L1_damping表示阻尼系數(shù)，KLR表示盤旋式飛行系數(shù)，KLS=4.0*L1_damping*L1_damping；
[0042] （10）按下式確定無人機的向心加速度acmdr，執(zhí)行步驟11；
[0043]
[0044] 其中，kd表示盤旋式飛行第一系數(shù)，kd=2*L1_damping*L1_damping；kp表示盤旋式飛行第二系數(shù)，kp=(2π/L1_period)2；dr表示盤旋式偏航距，所述盤旋式偏航距dr是指當(dāng)前無人機的地理位置到預(yù)設(shè)的盤旋式航線之間的最短距離， 表示盤旋式偏航距dr的變化率， 按下式確定：
[0045]
[0046] 其中，dr_last表示此前一個導(dǎo)航控制周期到達時執(zhí)行導(dǎo)航尋跡控制的盤旋式偏航距；dr_curr表示當(dāng)前的盤旋式偏航距；
[0047] （11）將無人機的向心加速度acmdr帶入下式確定此次導(dǎo)航尋跡控制的導(dǎo)航角θ；執(zhí)行步驟13；
[0048] θ=arctan(acmdr/g)；
[0050] （12）將無人機的橫向加速度acmds帶入下式確定此次導(dǎo)航尋跡控制的導(dǎo)航角θ；執(zhí)行步驟13；
[0051] θ=arctan(acmds/g)；
[0052] 其中的g是重力加速度的值；
[0053] （13）根據(jù)導(dǎo)航角θ控制無人機的飛行方向。
[0054] 作為優(yōu)化，所述導(dǎo)航控制周期L1_period的取值范圍為10～50ms；所述阻尼系數(shù)L1_damping的取值范圍為0.6～1。
[0055] 作為優(yōu)化，當(dāng)所述導(dǎo)航控制周期L1_period每改變1ms，所述阻尼系數(shù)L1_damping的值相應(yīng)地改變0.05。
[0056] 相比于現(xiàn)有技術(shù)，本發(fā)明具有如下優(yōu)點：
[0057] 1、本發(fā)明無人機的非線性導(dǎo)航尋跡控制方法中，采用固定的導(dǎo)航控制周期，根據(jù)無人機的四種不同的飛行狀態(tài)及其偏離情況，得出導(dǎo)航角，從而根據(jù)導(dǎo)航角對無人機執(zhí)行非線性的導(dǎo)航尋跡控制。利用這種非線性導(dǎo)航尋跡控制方法，確保了在執(zhí)行不同的航線飛行任務(wù)時，無人機都能根據(jù)不同航線的特點和當(dāng)前偏離航線的程度，得到合適的導(dǎo)航角，使得無人機能夠更好地實現(xiàn)壓航線飛行，提高了對無人機導(dǎo)航控制的準(zhǔn)確度。
[0058] 2、本發(fā)明無人機的非線性導(dǎo)航尋跡控制方法中，無人機根據(jù)不同飛行速度下的橫向加速度或者向心加速度實時地調(diào)整導(dǎo)航角，當(dāng)無人機距離航線比較近時，只產(chǎn)生比較小的導(dǎo)航角，當(dāng)無人機距離航線比較遠時，產(chǎn)生比較大的導(dǎo)航角，從而提高導(dǎo)航角控制的精確性，使得無人機能夠更好地實現(xiàn)壓航線飛行。
[0059] 3、本發(fā)明無人機的非線性導(dǎo)航尋跡控制方法中，參數(shù)L1_damping和L1_period可以根據(jù)無人機的飛行速度v進行動態(tài)調(diào)整，克服了現(xiàn)有技術(shù)中PID參數(shù)不能動態(tài)地進行調(diào)整的問題，提高了對無人機進行導(dǎo)航尋跡控制的靈活性。
附圖說明
[0060] 圖1為本發(fā)明無人機的非線性導(dǎo)航尋跡控制方法中飛行模式為直線式的一種情況。
[0061] 圖2為本發(fā)明無人機的非線性導(dǎo)航尋跡控制方法中飛行模式為直線式的另一種情況。
[0062] 圖3為本發(fā)明無人機的非線性導(dǎo)航尋跡控制方法中飛行模式為盤旋式的一種情況。
[0063] 圖4為本發(fā)明無人機的非線性導(dǎo)航尋跡控制方法中飛行模式為盤旋式的另一種情況。
具體實施方式
[0064] 本發(fā)明無人機的非線性導(dǎo)航尋跡控制方法，通過獲取當(dāng)前無人機的地理位置和飛行方向，結(jié)合預(yù)設(shè)的航線，采用非線性的導(dǎo)航尋跡控制實時計算無人機的橫向加速度或向心加速度，并求出無人機的導(dǎo)航角，確保無人機在執(zhí)行設(shè)定的航線飛行任務(wù)時，能夠更好地實現(xiàn)壓航線飛行。解決現(xiàn)有技術(shù)中采用傳統(tǒng)的PID算法進行導(dǎo)航尋跡控制時，PID參數(shù)往往是取比例、積分和微分三部分控制作用的折衷，使得計算出的導(dǎo)航角呈線性變化。因此在復(fù)雜的航線飛行任務(wù)中，傳統(tǒng)的PID算法求得的導(dǎo)航角不能根據(jù)具體航線飛行任務(wù)而做出相應(yīng)的調(diào)整，準(zhǔn)確度不高。
[0065] 本發(fā)明無人機的非線性導(dǎo)航尋跡控制方法中，在執(zhí)行導(dǎo)航尋跡控制求導(dǎo)航角的過程中，主要根據(jù)四種不同的飛行狀態(tài)求導(dǎo)航角。第一種，無人機在飛行模式為直線式的飛行模式，且無人機當(dāng)前靠近預(yù)設(shè)航線飛行。第二種，無人機在飛行模式為直線式的飛行模式，但無人機當(dāng)前遠離預(yù)設(shè)航線飛行。第三種，無人機在飛行模式為盤旋式的飛行模式，且無人機在盤旋半徑內(nèi)飛行。第四種，無人機在飛行模式為盤旋式的飛行模式，且無人機在盤旋半徑外飛行。本發(fā)明將無人機飛行的航線分為了四種不同的航線，并按照不同的航線及其偏離情況，得出導(dǎo)航角。本發(fā)明通過監(jiān)測無人機當(dāng)前屬于上述四種飛行狀態(tài)的任一種情況，從而建立相應(yīng)的模型來求得導(dǎo)航角。使得盡管在復(fù)雜的航線飛行任務(wù)中，無人機也能根據(jù)該飛行任務(wù)中具體的飛行狀態(tài)調(diào)整導(dǎo)航角的計算，使得無人機更好地壓航線飛行，提高對無人機導(dǎo)航控制的準(zhǔn)確度。
[0066] 下面結(jié)合附圖和
實施例對本發(fā)明的技術(shù)方案進一步說明。
[0067] 無人機的非線性導(dǎo)航尋跡控制方法，通過GPS（Global Positioning System，全球定位系統(tǒng)）定位實時地獲取當(dāng)前無人機的地理位置和飛行方向，并以固定的導(dǎo)航控制周期，按如下步驟執(zhí)行導(dǎo)航尋跡控制：
[0068] （1）判斷當(dāng)前的飛行模式，若當(dāng)前飛行模式為盤旋式的飛行模式，執(zhí)行步驟8；若當(dāng)前飛行模式為直線式的飛行模式，執(zhí)行步驟2。
[0069] （2）確定目標(biāo)距離LAC，單次飛行距離L1_dist和航線偏離系數(shù)X；所述目標(biāo)距離LAC是指當(dāng)前無人機的地理位置到預(yù)設(shè)的航線終點的地理位置之間的距離；所述單次飛行距離L1_dist是指無人機在一個導(dǎo)航控制周期內(nèi)飛行的距離；所述航線偏離系數(shù)X是指目標(biāo)距離LAC在預(yù)設(shè)的直線式航線上的投影和目標(biāo)距離LAC之間的比值。
[0070] 所述單次飛行距離L1_dist按下式確定：
[0071]
[0072] 其中，L1_period表示一個導(dǎo)航控制周期，L1_damping表示阻尼系數(shù)，v表示無人機飛行的速度；
[0073] 所述航線偏離系數(shù)X按下式確定：
[0074] X=l/LAC，
[0075] 其中，l表示目標(biāo)距離LAC在預(yù)設(shè)的直線式航線上的投影，l按下式確定：
[0076] 表示當(dāng)前無人機的地理位置到預(yù)設(shè)的航線終點的距離向量；用來表示預(yù)設(shè)的直線式航線，具體是指預(yù)設(shè)的航線起始點到預(yù)設(shè)的航線終點的距離向量；
表示預(yù)設(shè)的航線起始點到預(yù)設(shè)的航線終點之間的距離。
[0077] 假設(shè)通過GPS定位獲取到的當(dāng)前無人機的地理位置用C點表示，C點坐標(biāo)表示為（c1，c2）；在無人機當(dāng)前飛行模式為直線式的飛行模式的前提下，設(shè)置直線式航線的起始點和終點；航線起始點用B點表示，B點的坐標(biāo)表示為（b1，b2）；航線終點用A點表示，A點的坐標(biāo)表示為（a1，a2）；所述目標(biāo)距離LAC是指當(dāng)前無人機的地理位置到設(shè)的航線終點的地理位置之間的距離，則LAC的計算公式為 假設(shè)無人機是勻速飛行，則單次飛行距離L1_dist為常量，根據(jù)預(yù)設(shè)的導(dǎo)航控制周期L1_period、阻尼系數(shù)L1_damping和無人機的速度確定。航線偏離系數(shù)X表示的數(shù)學(xué)含義是無人機當(dāng)前飛行方向和距離向量的夾角的余弦值，在計算航線偏離系數(shù)X時， 的坐標(biāo)為（（a1-c1），（a2-c2））， 的坐標(biāo)為（（a1-b1），（a2-b2）），其中
[0078] （3）判斷單次飛行距離L1_dist是否小于目標(biāo)距離LAC，如果是，執(zhí)行步驟4；否則執(zhí)行步驟6。
[0079] （4）判斷航線偏離系數(shù)X是否大于0.7071，如果是，執(zhí)行步驟5，否則，執(zhí)行步驟6。
[0080] 步驟3和步驟4用來判斷無人機當(dāng)前是否是靠近預(yù)設(shè)航線飛行。如果單次飛行距離L1_dist小于目標(biāo)距離LAC，說明無人機在下一個導(dǎo)航控制周期便能到達終點，如果航線偏離系數(shù)X大于0.7071時，說明無人機當(dāng)前飛行方向和距離向量 的夾角小于45°，當(dāng)單次飛行距離L1_dist小于目標(biāo)距離LAC且無人機當(dāng)前飛行方向和距離向量 的夾角小于45°時，則認為當(dāng)前無人機是在靠近預(yù)設(shè)航線飛行，其飛行情況如圖1所示，此時根據(jù)步驟5中的方法求直線式飛行夾角ηs。否則認為當(dāng)前無人機在遠離預(yù)設(shè)航線飛行，其飛行情況如圖2所示，此時根據(jù)步驟6中的方法求直線式飛行夾角ηs。
[0081] （5）確定直線式飛行夾角ηs，并執(zhí)行步驟7；所述直線式飛行夾角ηs是指從當(dāng)前無人機的飛行方向到距離向量 所在方向之間的夾角。
[0082] 直線式飛行夾角ηs按下式確定：
[0083]
[0084] 其中，α0取值為1或-1；當(dāng)從當(dāng)前無人機的飛行方向到距離向量 所在的方向為順時針方向，α0為1；當(dāng)從當(dāng)前無人機的飛行方向到距離向量 所在的方向為逆時針方向，α0為-1；vl1表示速度v在距離向量 所在方向上的分向量的模，vx1表示速度v在距離向量 和距離向量 所在平面內(nèi)與距離向量 方向垂直的方向上的分向量的模；vl1和vx1分別按照下式確定：
[0085]
[0086] 步驟5是針對直線式飛行模式下，當(dāng)前無人機在靠近預(yù)設(shè)航線飛行時，求直線式飛行夾角ηs的情況。由于無人機距離航線比較近，所以無人機只需要一個比較小的導(dǎo)航角來調(diào)整飛行方向即可。步驟5將速度v分解為vl1和vx1兩個分量時，建立的數(shù)學(xué)模型是：以當(dāng)前無人機的地理位置為坐標(biāo)原點，以距離向量 所在的方向為X軸，以距離向量和距離向量 所在平面內(nèi)與距離向量 方向垂直的方向為Y軸，將速度v分解為X軸上的分量vl1和Y軸上的分量vx1。由此得出從當(dāng)前無人機的飛行方向到距離向量 所在方向之間的夾角
[0087] （6）按下式確定直線式飛行夾角ηs；并執(zhí)行步驟7。
[0088] ηs=α1*η1+α2*η2，
[0089] 其中 ds表示直線式偏航距，所述直線式偏航距ds是指當(dāng)前無人機的地理位置到預(yù)設(shè)的直線式航線 之間的距離， α1取值為1或-1；
當(dāng)從距離向量 所在方向到預(yù)設(shè)的直線式航線 所在的方向為順時針方向，α1為1；
當(dāng)從距離向量 所在方向到預(yù)設(shè)的直線式航線 所在的方向為逆時針方向，α1為-1；
α2取值為1或-1；當(dāng)從當(dāng)前無人機的飛行方向到預(yù)設(shè)的直線式航線
所在的方向為順時針方向，α2為1；當(dāng)從當(dāng)前無人機的飛行方向到預(yù)設(shè)的直線式航線 所在的方向為逆時針方向，α2為-1；vl2表示速度v在距離向量 所在方向上的分向量的模，vx2表示速度v在距離向量 和距離向量 所在平面內(nèi)與距離向量 方向垂直的方向上的分向量的模；vx2和vl2分別按照下式確定：
[0090]
[0091] 步驟6是針對直線式飛行模式下，當(dāng)前無人機在遠離預(yù)設(shè)航線飛行時，求直線式飛行夾角ηs的情況。由于無人機距離航線比較遠，即偏航距較大，此時無人機需要一個大的偏航角度來使無人機更好的壓航線。因此步驟6將直線式飛行夾角ηs分成η1和η2進行計算，其建立的數(shù)學(xué)模型是：直線式飛行夾角ηs通過角分線分為η1和η2，其中角分線與距離向量 平行，可以看出，η1在由直線式偏航距ds，單次飛行距離L1_dist和距離向組成的三角形中，由反三角函數(shù)可得 η2在計算的過程中，建立的數(shù)學(xué)模型是：以當(dāng)前無人機的地理位置為坐標(biāo)原點，以距離向量 所在的方向為X軸，以距離向量 和距離向量 所在平面內(nèi)與距離向量 方向垂直的方向為Y軸，將速度v分解為X軸上的分量vl2和Y軸上的分量vx2。由此得出
[0092] （7）按下式確定無人機的橫向加速度acmds，并執(zhí)行步驟12。
[0093] acmds=KLS*v2/L1_dist*sinηs；
[0094] 其中KLS表示直線式飛行系數(shù)，KLS=4.0*L1_damping*L1_damping。
[0095] （8）判斷盤旋距離 是否小于預(yù)設(shè)的盤旋半徑R，所述盤旋距離 是指當(dāng)前無人機的地理位置到預(yù)設(shè)的航線盤旋中心點之間的距離；如果是，執(zhí)行步驟9，否則，執(zhí)行步驟10。
[0096] 步驟8用來判斷無人機當(dāng)前是否在預(yù)設(shè)盤旋航線的半徑之內(nèi)飛行。如果盤旋距離是否小于預(yù)設(shè)的盤旋半徑R，說明無人機當(dāng)前在預(yù)設(shè)盤旋航線的半徑之內(nèi)飛行，其飛行情況如圖3所示，此時根據(jù)步驟9中的方法求盤旋式飛行夾角ηr。否則，說明無人機當(dāng)前在預(yù)設(shè)盤旋航線的半徑之外飛行，其飛行情況如圖4所示，此時根據(jù)步驟10中的方法求盤旋式飛行夾角ηr。
[0097] （9）確定盤旋式飛行夾角ηr和無人機的向心加速度acmdr，執(zhí)行步驟11；所述盤旋式飛行夾角ηr是指從當(dāng)前無人機的飛行方向到距離向量 所在方向之間的夾角；所述距離向量 是指當(dāng)前無人機的地理位置到預(yù)設(shè)的航線盤旋中心點的距離向量。
[0098] 所述盤旋式飛行夾角ηr按下式確定：
[0099]
[0100] 其中，α3取值為1或-1；當(dāng)從當(dāng)前無人機的飛行方向到距離向量 所在的方向為順時針方向，α3為1；當(dāng)從當(dāng)前無人機的飛行方向到距離向量 所在的方向為逆時針方向，α3為-1。
[0101] vx3和vl3分別按下式確定：
[0102]
[0103] vl3表示速度v在距離向量 所在方向上的速度分量，vx3表示速度v在與距離向量 所在方向上垂直的方向上的速度分量。
[0104] 所述無人機的向心加速度acmdr按下式確定為：
[0105] acmdr=KLR*v2/L1_dist*sinηr；
[0106] 其中 L1_period表示一個導(dǎo)航控制周期，L1_damping表示阻尼系數(shù)，KLR表示盤旋式飛行系數(shù)，KLS=4.0*L1_damping*L1_damping。
[0107] 步驟9是針對盤旋式飛行模式下，當(dāng)前無人機在盤旋半徑內(nèi)飛行時，計算盤旋式飛行夾角ηr的情況。在計算盤旋式飛行夾角ηr時，建立的數(shù)學(xué)模型是：以當(dāng)前無人機的地理位置為坐標(biāo)原點，以距離向量 所在的方向為X軸，以盤旋航線所在平面內(nèi)與距離向量方向垂直的方向為Y軸，將速度v分解為X軸上的分量vl3和Y軸上的分量vx3。由此可得出 假設(shè)通過GPS定位獲取到的當(dāng)前無人機的地理位置用C點表示，C點坐標(biāo)表示為（c1，c2）；在無人機當(dāng)前飛行模式為盤旋式的飛行模式的前提下，設(shè)置盤旋式航線的盤旋中心點和盤旋半徑R；航線中心點用D點表示，D點的坐標(biāo)表示為（d1，d2）；則的坐標(biāo)為（（d1-c1），（d2-c2））， 無人機的向心加速度acmdr的
方向為當(dāng)前無人機的地理位置指向航線中心點的方向。
[0108] （10）按下式確定無人機的向心加速度acmdr，執(zhí)行步驟11。
[0109]
[0110] 其中，kd表示盤旋式飛行第一系數(shù)，kd=2*L1_damping*L1_damping；kp表示盤旋式飛行第二2
系數(shù)，kp=(2π/L1_period) ；dr表示盤旋式偏航距，所述盤旋式偏航距dr是指當(dāng)前無人機的地理位置到預(yù)設(shè)的盤旋式航線之間的最短距離， 表示盤旋式偏航距dr的變
化率， 按下式確定：
[0111]
[0112] 其中，dr_last表示此前一個導(dǎo)航控制周期到達時執(zhí)行導(dǎo)航尋跡控制的盤旋式偏航距；dr_curr表示當(dāng)前的盤旋式偏航距。
[0113] 步驟10是針對盤旋式飛行模式下，當(dāng)前無人機在盤旋半徑外飛行時，計算盤旋式飛行夾角ηr的情況。其中當(dāng)前的盤旋式偏航距dr_curr等于當(dāng)前dr的取值。假設(shè)通過GPS定位獲取到的當(dāng)前無人機的地理位置用C點表示，C點坐標(biāo)表示為（c1，c2）；在無人機當(dāng)前飛行模式為盤旋式的飛行模式的前提下，設(shè)置盤旋式航線的盤旋中心點和盤旋半徑R；航線中心點用D點表示，D點的坐標(biāo)表示為（d1，d2）；則
[0114] （11）將無人機的向心加速度acmdr帶入下式確定此次導(dǎo)航尋跡控制的導(dǎo)航角θ；執(zhí)行步驟13。
[0115] θ=arctan(acmdr/g)；
[0116] 其中的g是重力加速度的值。
[0117] （12）將無人機的橫向加速度acmds帶入下式確定此次導(dǎo)航尋跡控制的導(dǎo)航角θ；執(zhí)行步驟13。
[0118] θ=arctan(acmds/g)；
[0119] 其中的g是重力加速度的值。
[0120] （13）根據(jù)導(dǎo)航角θ控制無人機的飛行方向。
[0121] 可以看出，本實施例的無人機的非線性導(dǎo)航尋跡控制方法，將導(dǎo)航角θ的計算分為了四種情況。針對不同的情況，通過設(shè)定的航線和當(dāng)前無人機的地理位置算出導(dǎo)航角θ，當(dāng)無人機遠離設(shè)定的航線時，盤旋式飛行夾角ηr或直線式飛行夾角ηs角度比較大，導(dǎo)致對應(yīng)的向心加速度acmdr或橫向加速度acmds的值比較大，于是就會產(chǎn)生一個較大的導(dǎo)航角，使無人機快速逼近航線。反之，無人機將以較小的導(dǎo)航角去逼近航線。利用該非線性導(dǎo)航尋跡控制方法，使得無人機即使在執(zhí)行復(fù)雜的飛行任務(wù)時，也能根據(jù)具體的飛行狀態(tài)改變導(dǎo)航角θ的計算方法，使得得到的導(dǎo)航角θ更準(zhǔn)確。同時，本實施例通過設(shè)定航線和無人機當(dāng)前的地理位置算出合適導(dǎo)航角θ，使無人機能更好地完成壓航線的飛行任務(wù)。
[0122] 具體應(yīng)用時，參數(shù)L1_damping和L1_period可以根據(jù)無人機的飛行速度v動態(tài)地進行調(diào)整，克服PID參數(shù)不能動態(tài)地進行調(diào)整的問題。導(dǎo)航控制周期L1_period的取值范圍為10～50ms，阻尼系數(shù)L1_damping的取值范圍為0.6～1。另外，為了使無人機達到最好的性能，當(dāng)所述執(zhí)行導(dǎo)航尋跡控制的間隔時間L1_period每改變1ms，所述阻尼系數(shù)L1_damping的值相應(yīng)地改變
0.05。當(dāng)無人機在轉(zhuǎn)彎時，如果轉(zhuǎn)彎太慢的話，可將L1_period的值減少5ms；如果無人機轉(zhuǎn)彎后是在飛行航線上振蕩的，那么將L1_period增加1ms或者是2ms。
[0123] 綜上所述，本發(fā)明的無人機的非線性導(dǎo)航尋跡控制方法，通過獲取當(dāng)前無人機的地理位置和飛行方向，結(jié)合預(yù)設(shè)的航線，采用非線性的導(dǎo)航尋跡控制實時計算無人機的橫向加速度或向心加速度，并求出無人機的導(dǎo)航角，確保無人機在執(zhí)行設(shè)定的航線飛行任務(wù)時，能夠更好地實現(xiàn)壓航線飛行。同時將導(dǎo)航角θ的計算分為了四種情況，針對不同的情況，通過不同的計算方法得出導(dǎo)航角θ。使得盡管在復(fù)雜的航線飛行任務(wù)中，無人機也能根據(jù)該飛行任務(wù)中具體的飛行狀態(tài)調(diào)整導(dǎo)航角的計算，保證了導(dǎo)航角θ的準(zhǔn)確性。同時，參數(shù)L1_damping和L1_period可以根據(jù)無人機的飛行速度v動態(tài)地進行調(diào)整，克服PID參數(shù)不能動態(tài)地進行調(diào)整的問題，提高了對無人機進行導(dǎo)航尋跡控制的靈活性。
[0124] 最后說明的是，以上實施例僅用以說明本發(fā)明的技術(shù)方案而非限制，盡管參照較佳實施例對本發(fā)明進行了詳細說明，本領(lǐng)域的普通技術(shù)人員應(yīng)當(dāng)理解，可以對本發(fā)明的技術(shù)方案進行
修改或者等同替換，而不脫離本發(fā)明技術(shù)方案的宗旨和范圍，其均應(yīng)涵蓋在本發(fā)明的
權(quán)利要求范圍當(dāng)中。
