[發明專利]一種基于曲率補償的橫側向曲線航跡跟蹤方法

有效

• 申請號: 201310059533.2
• 申請日: 2013-02-26
• 公開（公告）號: CN103149937A
• 公開（公告）日: 2013-06-12
• 發明（設計）人: 王宏倫;許敬剛;呂文濤;蓋文東
• 申請（專利權）人: 北京航空航天大學
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 北京永創新實專利事務所 11121
• 代理人: 姜榮麗

1.一種基于曲率補償的橫側向曲線航跡跟蹤方法，其特征在于，包括如下步驟：

步驟一：通過航跡規劃模塊得到參考航跡的離散航路點數據；

步驟二：通過拉格朗日分段多項式插值方法，對步驟一所得離散航路點數據進行分段插值擬合處理，得到曲線航跡；

步驟三：計算無人機當前時刻側偏距，以及參考航跡上距飛機最近點坐標；

步驟四：通過曲率計算公式，計算步驟三所得參考航跡上距飛機最近點處曲率值；

步驟五：重復步驟二～步驟四，并將步驟三所得側偏距和步驟四所得曲率值作為無人機橫側向控制器的輸入量，完成整個曲線航跡跟蹤過程。

2.根據權利要求1所述的一種基于曲率補償的橫側向曲線航跡跟蹤方法，其特征在于：所述的分段多項式插值方法，處理過程具體為：

(1)每當無人機到達一個航路點，設該航路點為P_count；

(2)如果count≤n-2，則利用P_count,P_count+1,P_count+2三個航路點數據進行二次多項式插值；否則

結束，完成整條航跡的跟蹤；

(3)如果是count=n-2，則繪制P_count-P_count+2段擬合航路；否則繪制P_count-P_count+1航路段曲線航

跡，并將該曲線航跡作為參考航跡；

(4)令count=count+1，返回步驟(2)。

3.根據權利要求1所述的一種基于曲率補償的橫側向曲線航跡跟蹤方法，其特征在于：所述的步驟三所得參考航路段上距離飛機最近點處曲率值K表達式：

K=L2′′(x)(1+L2′(x)2)32=2p1[1+(2p1x+p2)2]32---(3)]]>

將拉格朗日二次多項式方程表示為L₂(x)＝p₁x²+p₂x+p₃，p₁，p₂，p₃分別是多項式的各項系數，對其求導得到L₂'(x)＝2p₁x+p₂，L₂″(x)＝2p₁。