1.一種基于速度矢量合成的航跡規(guī)劃算法,其特征在于,包括如下步驟:
步驟1:用戶通過遙控裝置控制飛行器本體進入空中懸停模式;
步驟2:遙控裝置上的風速風向傳感器采集風速的大小v1和風速的方向θ1發(fā)送給飛行器,其中,v1為采集的風速的大小,θ1為風的方向角;
步驟3:飛行器本體接收遙控裝置傳來的風速的大小v1和風的方向角θ1后,飛行器本體把接收的風速的大小v1與原設定的風速初始值進行比較;
步驟4:飛行器本體根據(jù)飛行器氣壓計采集回來的氣壓算出飛行器本體相對地面的高度df,遙控裝置根據(jù)遙控氣壓計采集回來的氣壓算出遙控裝置相對地面的高度dy,從而可以算出飛行器本體相對遙控裝置豎直上的高度為dj=df+dy,其中dy為飛行器本體相對地面的高度,dy為遙控裝置相對地面的高度,dj為飛行器本體相對遙控裝置豎直上的高度;
步驟5:遙控裝置通過遙控GPS模塊采集遙控裝置的經緯度數(shù)據(jù)(lati1,long1),飛行器本體通過飛行器GPS模塊采集飛行器本體的經緯度數(shù)據(jù)(lati2,long2),其中,lati1為遙控裝置的經度數(shù)據(jù),long1為遙控裝置的維度數(shù)據(jù),lati2為飛行器本體的經度數(shù)據(jù),long2為飛行器本體的維度數(shù)據(jù);
步驟6:根據(jù)遙控裝置緯度數(shù)據(jù)和飛行器本體經緯度數(shù)據(jù)解算出飛行器本體與遙控裝置的相對距離dx和相對方向角θx,其中,dx為飛行器本體與遙控裝置的相對距離,θx為飛行器本體與遙控裝置的相對方向角;
步驟7:根據(jù)飛行器本體與遙控裝置的相對距離dx和飛行器本體相對遙控裝置豎直上的高度dj算出飛行器本體與遙控裝置的絕對距離其中,dfs為飛行器本體與遙控裝置的絕對距離;
步驟8:根據(jù)目標點與遙控裝置的絕對距離dfs和飛行器本體與遙控裝置的絕對距離dfj算出飛行器本體與目標點的距離s;
步驟9:當步驟3中風速的大小v1小于等于原設定的風速初始值時,用戶通過遙控器根據(jù)飛行器本體與遙控裝置的相對方向角θx來確定飛行器本體的飛行方向,當風速的大小v1大于原設定的風速初始值時,進入步驟12;
步驟10:飛行器本體運用PID算法來調控飛行器的輸出速度,輸出速度大小為v2,v2為飛行器本體的輸出速度;
步驟11:相隔設定的時間后,返回步驟4,直到飛行到達目標點;
步驟12:根據(jù)風速的大小v1和風速的方向θ1,既風速為再由設定的飛行器本體的輸出速度從而算出飛行器本體的實際飛行速度其中,為風速的矢量,為飛行器本體的輸出速度矢量,為飛行器本體實際飛行速度矢量;
步驟13:飛行器本體運用PID算法來調控飛行器的輸出速度,輸出速度大小為v2,v2為飛行器本體的輸出速度;
步驟14:相隔設定的時間后,返回步驟4,直到飛行到達目標點。
2.根據(jù)權利要求1所述的一種基于速度矢量合成的航跡規(guī)劃算法,其特征在于:所述步驟4中算出飛行器本體相對地面的高度df和遙控裝置相對地面的高度dy的過程均為通過氣壓計采集其空間位置的氣壓,把采集的氣壓減去大地表層的氣壓,再根據(jù)氣壓與高度的比例關系求出高度。
3.根據(jù)權利要求1所述的一種基于速度矢量合成的航跡規(guī)劃算法,其特征在于:所述步驟6算出飛行器本體與遙控裝置的相對距離dx和相對方向角θx的過程包括如下步驟,
步驟6.1:根據(jù)步驟5采集的經緯度算出飛行器本體與遙控裝置兩點的經度弧度長度:long=(long1-long2)×π/180,其中,π為圓周率,long1為遙控裝置點的經度,long2為飛行器本體經度,long為飛行器本體與遙控裝置兩點的經度弧度長度差;
步驟6.2:根據(jù)步驟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為地球半徑,π為圓周率。
4.根據(jù)權利要求1所述的一種基于速度矢量合成的航跡規(guī)劃算法,其特征在于:所述步驟8算出飛行器本體與目標點的距離s的公式為:
其中π為圓周率,dy為飛行器本體相對地面的高度,dy為遙控裝置相對地面的高度,dj為飛行器本體相對遙控裝置豎直上的高度,dfs為飛行器本體與遙控裝置的絕對距離,飛行器本體與遙控裝置的絕對距離dfj。
5.根據(jù)權利要求1所述的一種基于速度矢量合成的航跡規(guī)劃算法,其特征在于:所述步驟10中的PID算法為:
其中,u(t)為控制飛行器本體的輸出速度v2;e(t)為控制器的輸入(是初始飛行器與目標點的距離s與當前飛行器與目標點的距離ss之差,即e(t)=S-Ss),Kp為控制器的比例放大系數(shù),Ti為控制器的積分時間,Td為控制器的微分時間,t為時間。
6.根據(jù)權利要求1所述的一種基于速度矢量合成的航跡規(guī)劃算法,其特征在于:所述步驟12中算出飛行器本體實際飛行速度矢量的過程為:先算出飛行器本體飛行的方向:θ2為飛行器本體飛行的方向角,飛行器本體實際飛行速度大小為:
其中,v1為風速大小,v2為飛行器本體的輸出速度大小,v為飛行器本體實際飛行速度,θ2為飛行器本體飛行的方向角度,為飛行器本體實際飛行的方向與飛行器本體與目標點連線構成的角。
7.根據(jù)權利要求1所述的一種基于速度矢量合成的航跡規(guī)劃算法,其特征在于:所述步驟13中的PID算法為:
其中,u(t)為控制飛行器本體的輸出速度v2;e(t)為控制器的輸入(即e(t)=v-v2),Kp為控制器的比例放大系數(shù),Ti為控制器的積分時間,Td為控制器的微分時間,t為時間。
8.根據(jù)上述權利要求1-7任一項所述的一種基于速度矢量合成的航跡規(guī)劃算法的飛行器,其特征在于:包括飛行器本體和遙控裝置,遙控裝置與飛行器本體無線連接;
所述飛行器本體包括機架和控制電路單元,控制電路單元包括電機、電機驅動模塊、激光模塊、GPS定位模塊、氣壓計、控制器模塊、磁力計、顯示模塊、陀螺儀和控制無線模塊;
所述激光模塊、GPS定位模塊、氣壓計、磁力計和陀螺儀的輸出端與控制器模塊連接;所述GPS定位模塊用于獲取飛行器本體的經度和維度數(shù)據(jù);所述氣壓計用以檢測飛行器本體外的氣壓大小從而算出飛行器本體與地相距高度;所述陀螺儀用于檢測飛行器本體的平衡狀態(tài);
所述顯示模塊的輸入端與控制器模塊連接;所述控制器模塊的輸出端經電機驅動模塊與電機連接;所述控制器模塊與控制無線模塊連接;所述控制無線模塊與遙控裝置無線連接;所述顯示模塊用于顯示飛行器本體的飛行數(shù)據(jù)和檢測的氣壓、經緯度數(shù)據(jù);所述電機驅動模塊接收控制器模塊的控制信號控制電機轉動使得飛行器本體進行飛行;
所述遙控裝置包括遙控裝置包括遙控無線模塊、遙控氣壓計、風速風向傳感器、遙控顯示模塊、遙控控制器模塊、按鍵和搖桿;
所述遙控氣壓計、風速風向傳感器、按鍵和搖桿的輸出端與遙控控制器模塊連接;所述遙控氣壓計用以檢測遙控裝置外的氣壓大小從而算出遙控裝置與地相距高度;所述風速風向傳感器用于檢測飛速的大小和方向;所述按鍵和搖桿用于用戶輸入控制飛行器本體的飛行;
所述遙控顯示模塊的輸入端與遙控控制器模塊連接;所述遙控無線模塊與遙控控制器模塊連接;所述遙控無線模塊與控制無線模塊連接;所述遙控顯示模塊用于顯示飛行器本體的飛行的速度、與目標點的距離數(shù)據(jù);所述遙控無線模塊實現(xiàn)與飛行器本體無線通信。
9.根據(jù)上述權利要求8所述的一種基于速度矢量合成的航跡規(guī)劃算法的飛行器,其特征在于:飛行器本體和遙控裝置上均設置有供能裝置,供能裝置為電池。
10.根據(jù)上述權利要求8所述的一種基于速度矢量合成的航跡規(guī)劃算法的飛行器,其特征在于:所述陀螺儀使用型號為MCU6050的陀螺儀。