本發明涉及控制領域，特別是涉及一種基于速度矢量合成的航跡規劃算法及其飛行器。

背景技術：

當前，無人機已經逐漸出現于市場，特別是在跟蹤和航拍上的應用，如跟隨航拍、電影拍攝、植物保護、規劃路線、自然災害探測等涉及到自動拍攝的場合。常用的gps四軸跟隨無人機主要通過跟隨目標的狀態信息，以及期望的相對距離計算得到無人機的期望位置狀態，并與當下狀態進行比較，根據算法得出修正量。這樣的算法架構可以避免將相對距離看做系統狀態所需要的數據轉換過程，也實現了一部分的跟隨功能。但是在很多情況下跟隨狀態卻受到環境因素的影響很大，在定位跟隨下不能為用戶取得比較優秀的數據。

另外，無人機不是僅僅只能會是一個被動的牽引分子，而應該是一個完整的自己的思想，能夠自我判斷取得較好的路徑去跟隨目標。

技術實現要素：

本發明提供一種基于速度矢量合成的航跡規劃算法及其飛行器，解決現有無人機處于跟隨狀態下被風影響大的問題。

本發明通過以下技術方案解決上述問題：

一種基于速度矢量合成的航跡規劃算法，包括如下步驟：

步驟1：用戶通過遙控裝置控制飛行器本體進入空中懸停模式；

步驟2：遙控裝置上的風速風向傳感器采集風速的大小v₁和風速的方向θ₁發送給飛行器，其中，v₁為采集的風速的大小，θ₁為風的方向角；

步驟3：飛行器本體接收遙控裝置傳來的風速的大小v₁和風的方向角θ₁后，飛行器本體把接收的風速的大小v₁與原設定的風速初始值進行比較；

步驟4：飛行器本體根據飛行器氣壓計采集回來的氣壓算出飛行器本體相對地面的高度d_f，遙控裝置根據遙控氣壓計采集回來的氣壓算出遙控裝置相對地面的高度d_y，從而可以算出飛行器本體相對遙控裝置豎直上的高度為d_j＝d_f+d_y，其中d_y為飛行器本體相對地面的高度，d_y為遙控裝置相對地面的高度，d_j為飛行器本體相對遙控裝置豎直上的高度；

步驟5：遙控裝置通過遙控GPS模塊采集遙控裝置的經緯度數據(lati1,long1)，飛行器本體通過飛行器GPS模塊采集飛行器本體的經緯度數據(lati2,long2)，其中，lati1為遙控裝置的經度數據，long1為遙控裝置的維度數據，lati2為飛行器本體的經度數據，long2為飛行器本體的維度數據；

步驟6：根據遙控裝置緯度數據和飛行器本體經緯度數據解算出飛行器本體與遙控裝置的相對距離d_x和相對方向角θ_x，其中，d_x為飛行器本體與遙控裝置的相對距離，θ_x為飛行器本體與遙控裝置的相對方向角；

步驟7：根據飛行器本體與遙控裝置的相對距離d_x和飛行器本體相對遙控裝置豎直上的高度d_j算出飛行器本體與遙控裝置的絕對距離其中，d_fs為飛行器本體與遙控裝置的絕對距離；

步驟8：根據目標點與遙控裝置的絕對距離d_fs和飛行器本體與遙控裝置的絕對距離d_fj算出飛行器本體與目標點的距離s；

步驟9：當步驟3中風速的大小v₁小于等于原設定的風速初始值時，用戶通過遙控器根據飛行器本體與遙控裝置的相對方向角θ_x來確定飛行器本體的飛行方向，當風速的大小v₁大于原設定的風速初始值時，進入步驟12；

步驟10：飛行器本體運用PID算法來調控飛行器的輸出速度，輸出速度大小為v₂，v₂為飛行器本體的輸出速度；

步驟11：相隔設定的時間后，返回步驟4，直到飛行到達目標點；

步驟12:根據風速的大小v₁和風速的方向θ₁，既風速為再由設定的飛行器本體的輸出速度從而算出飛行器本體的飛行速度其中，為風速的矢量，為飛行器本體的輸出速度矢量，為飛行器本體實際飛行速度矢量；

步驟13：飛行器本體運用PID算法來調控飛行器的速度，速度大小為v₂，v₂為飛行器本體的輸出速度；

步驟14：相隔設定的時間后，返回步驟4，直到飛行到達目標點。

上述方案中，優選的是步驟4中算出飛行器本體相對地面的高度d_f和遙控裝置相對地面的高度d_y的過程均為通過氣壓計采集其空間位置的氣壓，把采集的氣壓減去大地表層的氣壓，再根據氣壓與高度的比例關系求出高度。

上述方案中，優選的是步驟6算出飛行器本體與遙控裝置的相對距離d_x和相對方向角θ_x的過程包括如下步驟，

步驟6.1：根據步驟5采集的經緯度算出飛行器本體與遙控裝置兩點的經度弧度長度：long＝(long1-long2)×π/180，其中，π為圓周率，long1為遙控裝置點的經度，long2為飛行器本體經度，long為飛行器本體與遙控裝置兩點的經度弧度長度差；

步驟6.2：根據步驟5采集的經緯度算出飛行器本體與遙控裝置兩點的緯度弧度長度：lati＝(lati1-lati2)×π/180；π為圓周率，lati1為遙控裝置點的維度，lati2為飛行器本體維度，lati為飛行器本體與遙控裝置兩點的維度弧度長度差；

步驟6.3：算出飛行器本體與遙控裝置兩點的相對距離為：

，

其中，long飛行器本體與遙控裝置兩點的經度弧度長度差，lati為為飛行器本體與遙控裝置兩點的維度弧度長度差，lati1為為遙控裝置點的維度，lati2為飛行器本體維度，R為地球半徑，π為圓周率；

步驟6.4：算出飛行器本體與遙控裝置兩點的相對角度為：θ_x＝mod(arctan2(R×cos(lati1×(lon2-lon1))，R×(lati2-lati1))，2×π)，mod為坐標點表示式。

上述方案中，優選的是步驟8算出飛行器本體與目標點的距離s的公式為：

其中π為圓周率。

上述方案中，優選的是步驟10中的PID算法為：

其中，u(t)為控制飛行器本體的輸出速度v₂；e(t)為控制器的輸入(是初始飛行器與目標點的距離s與當前飛行器與目標點的距離s_s之差，即e(t)＝S-S_s)，Kp為控制器的比例放大系數，T_i為控制器的積分時間，T_d為控制器的微分時間，t為時間。

上述方案中，優選的是步驟12中算出飛行器本體實際飛行速度矢量的過程為：先算出飛行器本體飛行的方向：θ₂為飛行器本體飛行的方向角，飛行器本體實際飛行速度大小為：

上述方案中，優選的是步驟13中的PID算法為：

其中，u(t)為控制飛行器的輸出速度v₂；e(t)為控制器的輸入(即e(t)＝v-v₂)，Kp為控制器的比例放大系數，T_i為控制器的積分時間，T_d為控制器的微分時間，t為時間。

根據上述的一種基于速度矢量合成的航跡規劃算法的飛行器，包括飛行器本體和遙控裝置，遙控裝置與飛行器本體無線連接；

所述飛行器本體包括機架和控制電路單元，控制電路單元包括電機、電機驅動模塊、激光模塊、GPS定位模塊、氣壓計、控制器模塊、磁力計、顯示模塊、陀螺儀和控制無線模塊；

所述激光模塊、GPS定位模塊、氣壓計、磁力計和陀螺儀的輸出端與控制器模塊連接；所述GPS定位模塊用于獲取飛行器本體的經度和維度數據；所述氣壓計用以檢測飛行器本體外的氣壓大小從而算出飛行器本體與地相距高度；所述陀螺儀用于檢測飛行器本體的平衡狀態；

所述顯示模塊的輸入端與控制器模塊連接；所述控制器模塊的輸出端經電機驅動模塊與電機連接；所述控制器模塊與控制無線模塊連接；所述控制無線模塊與遙控裝置無線連接；所述顯示模塊用于顯示飛行器本體的飛行數據和檢測的氣壓、經緯度數據；所述電機驅動模塊接收控制器模塊的控制信號控制電機轉動使得飛行器本體進行飛行；

所述遙控裝置包括遙控裝置包括遙控無線模塊、遙控氣壓計、風速風向傳感器、遙控顯示模塊、遙控控制器模塊、按鍵和搖桿；

所述遙控氣壓計、風速風向傳感器、按鍵和搖桿的輸出端與遙控控制器模塊連接；所述遙控氣壓計用以檢測遙控裝置外的氣壓大小從而算出遙控裝置與地相距高度；所述風速風向傳感器用于檢測飛速的大小和方向；所述按鍵和搖桿用于用戶輸入控制飛行器本體的飛行；

所述遙控顯示模塊的輸入端與遙控控制器模塊連接；所述遙控無線模塊與遙控控制器模塊連接；所述遙控無線模塊與控制無線模塊連接；所述遙控顯示模塊用于顯示飛行器本體的飛行的速度、與目標點的距離數據；所述遙控無線模塊實現與飛行器本體無線通信。

上述方案中，優選的是飛行器本體和遙控裝置上均設置有供能裝置，供能裝置為電池。

上述方案中，優選的是陀螺儀使用型號為MCU6050的陀螺儀。

本發明的優點與效果是：

本發明通過采集飛行環境中的風的大小和方向，在根據遙控裝置設定的飛行器本體飛行的速度，飛行器本體飛行根據風的大小和方向與設定的飛行速度從而進一步算出飛行器本體的實際飛行的速度和方向，從而能更好根據人們的需要進行飛行器的目標搜索，提高目標尋找的速度和效率，從而能夠更好的解決跟隨狀態下被風影響大的問題。

附圖說明

圖1為本發明飛行結構示意圖。

圖2為本發明結構框圖。

具體實施方式

以下結合實施例對本發明作進一步說明。

一種基于速度矢量合成的航跡規劃算法及其飛行器，如圖1所示，包括如下步驟：

步驟1：用戶通過遙控裝置控制飛行器本體進入空中懸停模式，用于完成飛行前期的數據采集和為下一步跟隨狀態做準備工作。

步驟2：遙控裝置上的風速風向傳感器采集風速的大小v₁和風速的方向θ₁發送給飛行器，其中，v₁為采集的風速的大小，θ₁為風的方向角，由于飛行器本體處于飛行狀態，無法檢測風的大小和方向，所以只能使用遙控裝置進行檢測，遙控裝置主要是使用遙控無線模塊與飛行器本體的控制無線模塊進行無線連接，遙控無線模塊和控制無線模塊均使用2.4G無線模塊。

步驟3：飛行遙控裝置傳來的風速的大小v₁和風的方向角θ₁后，飛行器本體把接收的風速的大小v₁與原設定的風速初始值進行比較。原設定的風速初始值可以為零，也可以是用戶根據飛行器的大小，風速對飛行器的航線產生影響的范圍，用戶自行設定。

步驟4：飛行器本體根據飛行器氣壓計采集回來的氣壓算出飛行器本體相對地面的高度d_f，遙控裝置根據遙控氣壓計采集回來的氣壓算出遙控裝置相對地面的高度d_y，從而可以算出飛行器本體相對遙控裝置豎直上的高度為d_j＝d_f+d_y，其中d_y為飛行器本體相對地面的高度，d_y為遙控裝置相對地面的高度，d_j為飛行器本體相對遙控裝置豎直上的高度。算出飛行器本體相對地面的高度d_f和遙控裝置相對地面的高度d_y的過程均為通過氣壓計采集其空間位置的氣壓，把采集的氣壓減去大地表層的氣壓，再根據氣壓與高度的比例關系求出高度。

步驟5：遙控裝置通過遙控GPS模塊采集遙控裝置的經緯度數據(lati1,long1)，飛行器本體通過飛行器GPS模塊采集飛行器本體的經緯度數據(lati2,long2)，其中，lati1為遙控裝置的經度數據，long1為遙控裝置的維度數據，lati2為飛行器本體的經度數據，long2為飛行器本體的維度數據。GPS模塊采集定位經緯度已經為現有技術。

步驟6：根據遙控裝置緯度數據和飛行器本體經緯度數據解算出飛行器本體與遙控裝置的相對距離d_x和相對方向角θ_x，其中，d_x為飛行器本體與遙控裝置的相對距離，θ_x為飛行器本體與遙控裝置的相對方向角。

飛行器本體與遙控裝置的相對距離d_x和相對方向角θ_x的過程包括如下步驟，

步驟6.1：根據步驟5采集的經緯度算出飛行器本體與遙控裝置兩點的經度弧度長度：long＝(long1-long2)×π/180，其中，π為圓周率，long1為遙控裝置點的經度，long2為飛行器本體經度，long為飛行器本體與遙控裝置兩點的經度弧度長度差。

步驟6.2：根據步驟5采集的經緯度算出飛行器本體與遙控裝置兩點的緯度弧度長度：lati＝(lati1-lati2)×π/180；π為圓周率，lati1為遙控裝置點的維度，lati2為飛行器本體維度，lati為飛行器本體與遙控裝置兩點的維度弧度長度差。

步驟6.3：算出飛行器本體與遙控裝置兩點的相對距離為：

，

其中，long飛行器本體與遙控裝置兩點的經度弧度長度差，lati為飛行器本體與遙控裝置兩點的維度弧度長度差，lati1為遙控裝置點的維度，lati2為飛行器本體維度，R為地球半徑，π為圓周率。

步驟6.4：算出飛行器本體與遙控裝置兩點的相對角度為：θ_x＝mod(arctan2(R×cos(lati1×(lon2-lon1))，R×(lati2-lati1))，2×π)，mod為坐標點表示式，lati1為遙控裝置點的維度，lati2為飛行器本體維度，long1為遙控裝置點的經度，long2為飛行器本體經度，R為地球半徑，π為圓周率。

步驟7：如圖1所示，根據飛行器本體與遙控裝置的相對距離d_x和飛行器本體相對遙控裝置豎直上的高度d_j算出飛行器本體與遙控裝置的絕對距離其中，d_fs為飛行器本體與遙控裝置的絕對距離。

步驟8：根據目標點與遙控裝置的絕對距離d_fs和飛行器本體與遙控裝置的絕對距離d_fj算出飛行器本體與目標點的距離s。算出飛行器本體與目標點的距離s的公式為：

其中π為圓周率，d_y為飛行器本體相對地面的高度，d_y為遙控裝置相對地面的高度，d_j為飛行器本體相對遙控裝置豎直上的高度，d_fs為飛行器本體與遙控裝置的絕對距離，飛行器本體與遙控裝置的絕對距離d_fj。

步驟9：當步驟3中風速的大小v₁小于等于原設定的風速初始值時，用戶通過遙控器根據飛行器本體與遙控裝置的相對方向角θ_x來確定飛行器本體的飛行方向，當風速的大小v₁大于原設定的風速初始值時，進入步驟12。主要是選擇飛行的模式，更有采集的風速的大小與用戶設定的值進行來比較。

步驟10：飛行器本體運用PID算法來調控飛行器的速度，速度大小為v₂，v₂為飛行器的飛行速度。PID算法為：

其中，u(t)為控制飛行器本體的輸出速度v₂；e(t)為控制器的輸入(是初始飛行器與目標點的距離s與當前飛行器與目標點的距離s_s之差，即e(t)＝S-S_s)，Kp為控制器的比例放大系數，T_i為控制器的積分時間，T_d為控制器的微分時間，t為時間。

步驟11：相隔設定的時間后，返回步驟4，直到飛行到達目標點。設定的時間可以是一分鐘或幾十秒都可以，由用戶來設定，用戶可以根據尋找目標的距離長遠來進行比較。達到目的點后完成尋找任務。

步驟12：根據風速的大小v₁和風速的方向θ₁，既風速為再由設定的飛行器本體與目標點的相對飛行速度從而算出飛行器本體的飛行速度其中，為風速的矢量，為飛行器本體與目標點的相對飛行速度矢量，為飛行器本體實際飛行速度矢量。算出飛行器本體實際飛行速度矢量的過程為：先算出飛行器本體飛行的方向：θ₂為飛行器本體飛行的方向角，飛行器本體實際飛行速度大小為：其中，v₁為風速大小，v₂為飛行器本體的輸出速度大小，v為飛行器本體實際飛行速度，θ₂為飛行器本體飛行的方向角度，為飛行器本體實際飛行的方向與飛行器本體與目標點連線構成的角。

步驟13：飛行器本體運用PID算法來調控飛行器的速度，速度大小為v，v為飛行器的飛行速度。PID算法為：其中，u(t)為控制飛行器的速度v₂；e(t)為控制器的輸入(即e(t)＝v-v₂)，Kp為控制器的比例放大系數，T_i為控制器的積分時間，T_d為控制器的微分時間，t為時間。

步驟14：相隔設定的時間后，返回步驟4，直到飛行到達目標點。設定的時間可以是一分鐘或幾十秒都可以，由用戶來設定，用戶可以根據尋找目標的距離長遠來進行比較。

如圖2所示，根據上述一種基于速度矢量合成的航跡規劃算法的飛行器，包括飛行器本體和遙控裝置，遙控裝置與飛行器本體無線連接.

所述飛行器本體包括機架和控制電路單元，控制電路單元包括電機、電機驅動模塊、激光模塊、GPS定位模塊、氣壓計、控制器模塊、磁力計、顯示模塊、陀螺儀和控制無線模塊。

所述激光模塊、GPS定位模塊、氣壓計、磁力計和陀螺儀的輸出端與控制器模塊連接；所述GPS定位模塊用于獲取飛行器本體的經度和維度數據；所述氣壓計用以檢測飛行器本體外的氣壓大小從而算出飛行器本體與地相距高度；所述陀螺儀用于檢測飛行器本體的平衡狀態。

所述顯示模塊的輸入端與控制器模塊連接；所述控制器模塊的輸出端經電機驅動模塊與電機連接；所述控制器模塊與控制無線模塊連接；所述控制無線模塊與遙控裝置無線連接；所述顯示模塊用于顯示飛行器本體的飛行數據和檢測的氣壓、經緯度數據；所述電機驅動模塊接收控制器模塊的控制信號控制電機轉動使得飛行器本體進行飛行。

飛行器本體的控制器模塊的微處理器主要實現的功能有：1、提供電機驅動模塊所需要的方波脈沖控制信號，以控制電機的轉速。2、驅動激光模塊來檢測當前所處位置附近的障礙物以進行壁障。3、根據GPS定位模塊得到當前飛行器所處的經緯度。4、利用2.4G數傳模塊與遙控器通信。5、處理氣壓計得到的數據得到當前飛行器所處的高度。6、將當前經緯度以及高度等數據顯示在OLED顯示模塊上。7、采集陀螺儀的數據，完成姿態，速度等導航信息的濾波解算。8、得到遙控裝置的數據進行相應的處理。9、驅動磁力計，定位飛行器的飛行方向。

所述遙控裝置包括遙控裝置包括遙控無線模塊、遙控氣壓計、風速風向傳感器、遙控顯示模塊、遙控控制器模塊、按鍵和搖桿。

所述遙控氣壓計、風速風向傳感器、按鍵和搖桿的輸出端與遙控控制器模塊連接；所述遙控氣壓計用以檢測遙控裝置外的氣壓大小從而算出遙控裝置與地相距高度；所述風速風向傳感器用于檢測飛速的大小和方向；所述按鍵和搖桿用于用戶輸入控制飛行器本體的飛行。

所述遙控顯示模塊的輸入端與遙控控制器模塊連接；所述遙控無線模塊與遙控控制器模塊連接；所述遙控無線模塊與控制無線模塊連接；所述遙控顯示模塊用于顯示飛行器本體的飛行的速度、與目標點的距離數據；所述遙控無線模塊實現與飛行器本體無線通信。

遙控裝置的遙控控制器模塊內的微處理器主要實現的功能有：1、根據GPS定位模塊得到當前遙控器所處的經緯度2、按鍵控制得到當前飛行器的模式3、采集當前位置信息4、處理氣壓計得到的數據得到當前遙控所處的高度5、OLED顯示當前位置，模式等信息。

遙控裝置通過2.4G數傳模塊發送相應的控制指令控制飛行器，飛行器既可工作?？刂泼畎ǎ?、自穩/懸停/基本跟隨模式/有風跟隨模式四個模式；2、控制脈沖頻率，即電機的轉動速度；3、遙控飛行方向；4、GPS數據傳輸5、無人機狀態查詢，當電壓過低時，發送警報給遙控裝置6、當要飛行器處于跟隨模式時，先讓飛控處于懸停狀態模式，然后再處于跟隨模式，飛行器即進入跟隨模式自主飛行7、處于跟隨模式時無人機結合根據遙控裝置傳過來的風力、風向和飛行目標點做出飛行航向的決策。

工作時，操作者通過遙控裝置上的按鍵裝置啟動飛行器進入懸停模式再進入跟隨模式，飛行器根據遙控裝置傳回的GPS數據、風速風向數據、氣壓值判斷目標點的方向，進而規劃航線和飛行的方向。這種飛行方式有效的決解了飛行器在有風的情況下也不會導致偏離航跡飛行。

以上已對本發明創造的較佳實施例進行了具體說明，但本發明并不限于實施例，熟悉本領域的技術人員在不違背本發明創造精神的前提下還可作出種種的等同的變型或替換，這些等同的變型或替換均包含在本申請的范圍內。