體為:
[0101]1、利用無人機搭載的加速度傳感器獲取加速度信息，并通過旋轉(zhuǎn)矩陣將其轉(zhuǎn)換到機體坐標系中，若機體存在多個健康的加速度傳感器，則將多個加速度傳感器的數(shù)據(jù)進行融合后作為最終加速度信息A;
[0102]2、根據(jù)所述數(shù)據(jù)判斷GPS狀態(tài)，若沒有GPS、GPS鎖定目標不滿足預(yù)設(shè)條件或GPS收到的衛(wèi)星數(shù)少于預(yù)設(shè)值，則GPS狀態(tài)為不可用，反之，則GPS狀態(tài)為可用;若GPS狀態(tài)為不可用時，通過空速傳感器獲取無人機的空速，再通過DCM旋轉(zhuǎn)矩陣將其轉(zhuǎn)換到大地坐標系中，在考慮風速影響后得到無人機沿大地坐標系中x、y和z方向上的速度;若GPS狀態(tài)為可用時，根據(jù)所述數(shù)據(jù)中GPS返回的數(shù)據(jù)獲得無人機沿大地坐標系中x、y和z方向上的速度；
[0103]3、獲得速度信息后，根據(jù)固定時間間隔內(nèi)飛行器速度的變化量計算出無人機x、y和z軸三個方向上的加速度信息，并通過疊加重力方向的加速度信息并經(jīng)過歸一化處理得到無人機在機體坐標系下的實際加速度信息，對所述實際加速度信息進行歸一化處理后生成加速度信息B;
[0104]4、根據(jù)加速度信息A與加速度信息B進行向量叉乘，獲得的結(jié)果為陀螺儀的漂移誤差，通過PI (Proport1nal Integral，比例/積分)控制器計算控制量的輸出對陀螺儀進行修正，從而控制無人機的正常飛行。
[0105]在本實施例中，通過使用獲取無人機速度信息計算無人機加速度信息，并通過與由加速度傳感器獲得的加速度信息進行計算獲得漂移誤差，最后對陀螺儀進行修正，由此保證無人機在陀螺儀發(fā)生漂移后，及時對其進行修正，使得飛控系統(tǒng)依然能夠獲得正確數(shù)據(jù)，從而控制無人機正常飛行。
[0106]參照圖4，為本發(fā)明控制無人機穩(wěn)定飛行的裝置的第一實施例，所述控制無人機穩(wěn)定飛行的裝置包括:
[0107]數(shù)據(jù)收集模塊100，用于在無人機飛行過程中，飛控系統(tǒng)收集無人機上所搭載的監(jiān)測裝置所返回的監(jiān)測數(shù)據(jù)，所述監(jiān)測裝置至少包括:GPS、加速度傳感器、陀螺儀和磁力計；
[0108]—種可選的實施例中，為提高所述收集無人機搭載的監(jiān)測裝置返回的監(jiān)測數(shù)據(jù)的可靠性，采用多次對監(jiān)測裝置數(shù)據(jù)進行采集得到采集數(shù)據(jù)，最后對所述采集數(shù)據(jù)進行平均及加權(quán)處理以生成所述無人機搭載的監(jiān)測裝置返回的監(jiān)測數(shù)據(jù)。
[0109]飛行控制模塊200，用于根據(jù)所述監(jiān)測裝置返回的監(jiān)測數(shù)據(jù)，至少對所述無人機的飛行航向和平穩(wěn)性中的一項進行控制；
[0110]根據(jù)所述GPS返回的監(jiān)測數(shù)據(jù)，獲得所述無人機的在大地坐標系x、y和z方向上的三個速度，以及無人機當前位置的經(jīng)度和瑋度信息，對所述無人機的偏航方向進行控制以保證無人機的飛行航向，并通過所述傳感器中包括的陀螺儀返回的監(jiān)測數(shù)據(jù)，結(jié)合其他傳感器返回的加速度監(jiān)測信息，對所述無人機的橫滾角進行控制，以對所述無人機的平穩(wěn)性進行控制。
[0111]本市實施例中，一種可選實施方式的具體實施步驟如下:
[0112]1、利用磁力計返回的監(jiān)測數(shù)據(jù)計算無人機在x、y和z軸上的磁場信息，由于地磁場以大地坐標系為參考，因此需將所述磁場信息從機體坐標系轉(zhuǎn)到大地坐標系；
[0113]2、通過GPS返回的監(jiān)測數(shù)據(jù)獲取無人機當前位置的經(jīng)度和瑋度信息，并根據(jù)所述經(jīng)度和瑋度信息計算出磁偏角，通過磁偏角計算出無人機在X和y方向的磁場信息，計算公式如下:
[0114]_mag_earth.X = Cosf (_last_decl inat 1n);//根據(jù)所述經(jīng)度和瑋度信息通過余弦函數(shù)計算出無人機X方向的磁場；
[0115]_mag_earth.y = sinf (_last_declinat1n);//根據(jù)所述經(jīng)度和瑋度信息通過正弦函數(shù)計算出無人機y方向的磁場；
[0116]3、對所述通過所述步驟I獲得的磁場信息與所述步驟2獲得的磁場信息進行向量叉乘計算獲得計算結(jié)果，通過所述計算結(jié)果獲得磁場誤差數(shù)據(jù)，由于無人機的偏航方向只與X，y方向的磁場有關(guān)，與z軸方向的磁場無關(guān)，因此所述通過所述計算結(jié)果獲得磁場誤差數(shù)據(jù)與z軸方向上的數(shù)據(jù)無關(guān)；
[0117]4、將所述誤差數(shù)據(jù)送入到DCM姿態(tài)控制算法，完成對無人機偏航方向的修正，達到控制無人機飛行航向的作用。
[0118]在本實施例中，通過GPS和磁力計傳感器返回的監(jiān)測數(shù)據(jù)在無人機飛行過程中對無人機進行控制，使得飛控系統(tǒng)通過GPS和磁力計傳感器返回的監(jiān)測數(shù)據(jù)控制無人機偏航角，保證無人機能夠按照正確的航向飛行。
[0119]參照圖5，為本發(fā)明控制無人機穩(wěn)定飛行的裝置的第二實施例，基于圖4所示的實施例，所述飛行控制模塊200包括:
[0120]判斷單元201，用于根據(jù)所述GPS和磁力計返回的監(jiān)測數(shù)據(jù)判斷磁力計是否失效；
[0121]通過收集到的所述數(shù)據(jù)對根據(jù)磁力計獲得的無人機偏航方向誤差判斷磁力計是否失效，磁力計失效的原因可能由電磁干擾和/或無人機在強磁場環(huán)境下工作引起。
[0122]誤差獲取單元202，用于當判斷到磁力計已失效時，根據(jù)GPS返回的監(jiān)測數(shù)據(jù)計算無人機偏航角，獲得無人機的偏航角誤差；
[0123]當判斷到磁力計已失效時，誤差獲取單元202通過獲取GPS返回的信息計算出無人機偏航角，并通過所述偏航角更進一步的獲得無人機的偏航角誤差。
[0124]方向控制單元203，用于根據(jù)所述偏航角誤差控制所述無人機的偏航方向；
[0125]方向控制單元203通過所述偏航角誤差通過DCM姿態(tài)控制算法從而完成對偏航方向的控制，使得無人機能夠保持正確的偏航方向。
[0126]具體實施時，本實施例所述步驟具體如下:
[0127]1、在無人機飛行過程中，數(shù)據(jù)收集模塊100通過磁力計獲得的數(shù)據(jù)計算出無人機機體x、y和z軸的磁場向量，由于地磁場以大地坐標系為參考，因此需要將所述磁場從機體坐標系轉(zhuǎn)換到大地坐標系中；
[0128]2、判斷單元201通過GPS返回的數(shù)據(jù)獲取無人機當前位置的經(jīng)度和瑋度信息，并通過所述經(jīng)度和瑋度信息計算出磁偏角，進一步的通過所述磁偏角計算出無人機在X和y方向上的磁場向量；
[0129]3、根據(jù)步驟I所述獲得的磁場向量與步驟2所獲得的磁場向量進行叉乘計算后獲得偏航角誤差，根據(jù)所述偏航角誤差控制無人機偏航方向；
[0130]4、根據(jù)GPS返回的數(shù)據(jù)獲得無人機沿大地坐標系X和y方向的速度，并根據(jù)所述沿大地坐標系X和y方向的速度進行反正切函數(shù)計算得到無人機偏航角，較佳的實施方式可以通過如下代碼計算得到無人機偏航角:
[0131]atan2f(istate.velocity.y,istate.velocity.x)；
[0132]5、將所述無人機偏航角與步驟3中通過磁力計返回的數(shù)據(jù)計算獲得的無人機偏航角進行差值運算，獲得差值并求取所述差值的絕對值，判斷所述絕對值是超過預(yù)設(shè)閥值，若超過則判斷磁力計已失效；
[0133]6、當磁力計失效時根據(jù)所述步驟4獲得的無人機偏航角和上一時刻無人機偏航角獲得無人機的偏航角誤差，即以上一時刻無人機偏航角為基準，計算獲得無人機的偏航角誤差，將所述無人機的偏航角誤差送入到DCM姿態(tài)控制算法，從而對無人機的偏航方向進行控制。
[0134]在本實施例中，通過上述方案使用GPS數(shù)據(jù)在磁力計受到電磁干擾時對無人機進行偏航角控制，使得無人機具有更為廣泛的應(yīng)用范圍，并且保證無人機在強磁場環(huán)境下依然能夠正常飛行，避免了磁力計在收到電磁干擾時，返回數(shù)據(jù)不準確導(dǎo)致無人機無法正確控制偏航方向最后可能帶來無人機墜機等災(zāi)難性的問題。
[0135]進一步的，參照圖6，為本發(fā)明控制無人機穩(wěn)定飛行的裝置的第三實施例，基于圖4所述的實施例，所述飛行控制模塊200包括:
[0136]直接獲取加速度單元211，用于將所述加速度傳感器返回的監(jiān)測數(shù)據(jù)通過旋轉(zhuǎn)矩陣獲得無人機機體x、y和z軸三個方向上的加速度信息A;
[0137]通過加速度傳感器直接獲取加速度數(shù)據(jù)，并通過旋轉(zhuǎn)矩陣將其轉(zhuǎn)換到機體坐標系下，由此得到無人機機體x、y和z軸三個方向上的加速度信息A，其中較佳的實施方式為，若無人機搭載多套健康的加速度傳感器，則將所述多套健康的傳感器返回的監(jiān)測數(shù)據(jù)進行融合后用于計算加速度信息A;
[0138]計算獲取加速度單元212，用于根據(jù)所述GPS返回的監(jiān)測數(shù)據(jù)獲得無人機沿大地坐標系中X、y和Z三個方向上的速度，并根據(jù)所述x、y和z三個方向上的速度獲得無人機機體X、y和z軸三個方向上的加速度信息B;
[0139]根據(jù)所述數(shù)據(jù)獲得無人機沿大地坐標系中x、y和z方向上的速度信息后，根據(jù)固定時間間隔內(nèi)飛行器速度的變化量計算出無人機x、y和z軸三個方向上的加速度信息，并通過疊加重力方向的加速度信息得到無人機在機體坐標系下的實際加速度信息，對所述實際加速度信息進行歸一化處理后生成