一種無人機飛行控制器姿態解算和控制方法技術

本發明專利技術公開了一種無人機飛行控制器姿態解算和控制方法，姿態解算方法使用由加速度計與磁力計融合得到的第一姿態矩陣，陀螺儀和上個控制周期的第三姿態矩陣融合得到的第二姿態矩陣，第一姿態矩陣和第二姿態矩陣融合得到包含高精度姿態信息的第三姿態矩陣。姿態控制方法以姿態空間矢量為控制對象，根據第三姿態矩陣得到各個電機的控制量，實現對無人機飛行器的姿態控制。本發明專利技術能適用于無人機全姿態的控制，實時性高，姿態解算和控制穩定。

Attitude calculation and control method for UAV Flight Controller

The invention discloses a UAV and control method to solve the flight attitude controller, attitude calculation method using the first attitude matrix and magnetometer fusion obtained by acceleration meter, third gyro and attitude matrix control cycle on the integration of the second attitude matrix, the attitude matrix and second attitude matrix fusion attitude third the matrix contains high precision attitude information. The attitude control method takes the attitude space vector as the control object, and obtains the control of each motor according to the third attitude matrix. The invention can be applied to the control of UAV attitude, high real-time performance, attitude calculation and control stability.

【技術實現步驟摘要】
所屬
本專利技術涉及無人機飛行控制領域，具體涉及一種無人機飛行控制器姿態解算和控制方法。
技術介紹
目前主流的無人機飛行控制器的姿態解算方法的輸出量是是歐拉角，而飛行控制方法是以歐拉角作為內部控制對象對機體的姿態進行控制，歐拉角具有萬向鎖缺陷，不能適用于全姿態的控制，限制了無人機的運動幅度，同時由于包含三角函數運算，增加了處理器負擔，降低了控制的實時性。無人機飛行控制器是無人機的核心控制部件，其任務是接收來自內部傳感器(陀螺儀、加速度計、磁力計、氣壓計、溫度計、電壓計)和外部傳感器(GNSS定位裝置、外部磁力計)的數據，通過特定的飛行控制算法轉換成電子調速器所需的控制信號，從而改變和控制無人機的姿態(俯仰/橫滾/航向情況)、地理位置和高度。目前，大多數無人機飛行控制器通過外部供電模塊進行供電，外部供電模塊供電電壓噪聲和波動較大，同時由于地線與飛行控制器的距離較遠，容易對飛行控制器造成干擾；大多數無人機飛行控制器采用IMU(Inertialmeasurementunit，慣性測量單元)硬連接，由于無人機飛行時螺旋槳產生的震動，將導致IMU采集的數據容易受到干擾，從而造成姿態解算的不穩定性。
技術實現思路
本專利技術所解決的技術問題是，針對現有技術的不足，提供一種無人機飛行控制器姿態解算和控制方法，能適用于無人機全姿態的控制，實時性高，姿態解算和控制穩定。本專利技術解決技術問題所采用的技術方案是：一種無人機飛行控制器姿態解算方法，通過無人機飛行控制器內部的IMU模塊采集原始三軸加速度計數據、原始三軸磁力計數據和原始三軸陀螺儀數據；通過以下步驟進行姿態解算：1)對原始三軸加速度計數據進行誤差修正和規范化，得到修正后的三軸加速度計數據記為：aXN，aYN，aZN；2)對原始三軸磁力計數據進行誤差修正和規范化，得到修正后的三軸磁力計數據記為：mXN，mYN，mZN；3)對步驟1)和2)中的兩組修正后的數據進行融合，得到第一姿態矩陣；4)對原始三軸陀螺儀數據進行誤差修正，得到修正后的三軸陀螺儀數據記為gX、gY、gZ；將修正后的三軸陀螺儀數據與上一個控制周期得到的第三姿態矩陣進行融合，得到第二姿態矩陣；第三姿態矩陣的初始值等于第一個控制周期得到的第一姿態矩陣的值；5)將第一姿態矩陣和第二姿態矩陣進行融合，得到該控制周期的第三姿態矩陣，即融合后的方向余弦矩陣。所述步驟1)具體為：讀取原始三軸加速度計數據(aX、aY、aZ)；根據以下公式對原始三軸加速度計數據進行零偏修正和靈敏度誤差修正：aXZ＝aX-zoaXaYZ＝(aY-zoaY)*saYaZZ＝(aZ-zoaZ)*saZ其中，zoaX，zoaY和zoaZ分別為三軸加速度計偏移值常量，saY和saZ分別為Y軸和Z軸加速度計靈敏度修正系數與X軸靈敏度修正系數的相對值；偏移值常量和靈敏度修正系數是由該儀器生產過程中的誤差決定的；再對三軸加速度計數據進行水平-磁偏角校正：aXH＝h11*aXZ+h12*aY+h13*aZaYH＝h21*aXZ+h22*aY+h23*aZaZH＝h31*aXZ+h32*aY+h33*aZ其中，水平-磁偏角校正矩陣通過校準測量得到；對三軸加速度計數據進行規范化處理：所述步驟2)具體為：讀取原始三軸磁力計數據(magX、magY、magZ)；先根據以下公式對原始三軸磁力計數據進行零偏修正和靈敏度誤差修正：mXZ＝magX-zomXmYZ＝(magY-zomY)*smYmZZ＝(magZ-zomZ)*smZ其中，zomX，zomY和zomZ分別為三軸磁力計偏移值常量，smY和smZ分別為Y軸和Z軸磁力計靈敏度修正系數與X軸磁力計靈敏度修正系數的相對值；【再對三軸磁力計數據進行橢圓修正，即將三軸磁力計數據進行二次多項式擬合；二次多項式是預先校準磁力計時得到的，得到實際的三軸磁力計數據后，用二次多項式曲線上與之距離最近的點，替換三軸磁力計數據；】再對三軸磁力計數據進行水平-磁偏角校正：mXH＝h11*mXZ+h12*mYZ+h13*mZZmYH＝h21*mXZ+h22*mYZ+h23*mZZmZH＝h31*mXZ+h32*mYZ+h33*mZZ然后對三軸磁力計數據進行規范化處理；后面有規范化，本來這里可以不用，但是考慮到浮點運算過程中數量級接近的浮點數之間運算的結果精度更高，因此這里執行規范化：所述步驟3)中，第一姿態矩陣記為：其中，第三行元素為：a31＝aXN，a32＝aYN，a33＝aZN；第一行元素為：其中，v為垂直化因子，v＝mXN*aXN+mYN*aYN+mZN*aZN；第二行元素為：a21＝a13*a32-a12*a33a22＝a11*a33-a31*a13a23＝a31*a12-a11*a32上述第一姿態矩陣第一行元素與第二行元素計算原理為：對修正后的三軸磁力計數據mXN，mYN，mZN進行垂直化處理；由于磁力計不與加速度計垂直，需要垂直化處理，需要在磁力計添加一個K*加速度計矢量，再根據相互垂直的向量點積為0原理有：(mX-v*aX,mY-v*aY,mZ-v*aZ)*(aX,aY,aZ)＝0可得垂直化因子：對磁力計數據進行垂直化處理：mXV＝mXN-v*aXNmYV＝mYN-v*aYNmZV＝mZN-v*aZN再次規范化處理：得到第一姿態矩陣第一行元素為：a11＝mXVNa12＝mYVNa13＝mZVN由方向余弦矩陣正交特性求出第一姿態矩陣第二行元素為：a21＝a13*a32-a12*a33a22＝a11*a33-a31*a13a23＝a31*a12-a11*a32。所述步驟4)中，第二姿態矩陣記為：第三姿態矩陣記為：第二姿態矩陣的計算過程為：首先將修正后的三軸陀螺儀數據分別乘以更新周期T，得到三軸角矢量αX、αY、αZ：αX＝gX*TαY＝gY*TαZ＝gZ*T計算四元數更新因子(r0、r1、r2、r3)：r0＝1-sSquareSum/8其中，sSquareSum＝αX2+αY2+αZ2將上一控制周期得到的第三姿態矩陣轉換成四元數：若q0≠0，則有：若q0＝0，則有：規范化上述四元數：更新四元數：q0＝q0N*r0-q1N*r1-q2N*r2-q3N*r3q1＝q1N*r0+q0N*r1-q3N*r2+q2N*r3q2＝q2N*r0+q3N*r1+q0N*r2-q1N*r3q3＝q3N*r0-q2N*r1+q1N*r2+q0N*r3對上述更新的四元數進行規范化處理(將四元數的各個分量分別除以各個分量的平方和的開方)，得到新的q0N，q1N，q2N和q3N：將其轉換成第二姿態矩陣【陀螺儀方向余弦矩陣】：b11＝q0N2+q1N2-q2N2-q3N2b12＝2*q1N*q2N-2*q0N*q3Nb13＝2*q1N*q3N+2*q0N*q2Nb21＝2*q1N*q2N+2*q0N*q3Nb22＝q0N2-q1N2+q2N2-q3N2b23＝2*q2N*q3N-2*q0N*q1Nb31＝2*q1N*q3N-2*q0N*q2Nb32＝2*q2N*q3N+2*q0N*q1Nb33＝q0N2-q1N2-q2N2+q3N2。所述步驟5)中，該控制周期的第三姿態矩陣【融合后的方向余弦矩陣】計算方法為：計算修正后的三軸加速度計數據與第二姿態矩本文檔來自技高網...

【技術保護點】
一種無人機飛行控制器姿態解算方法，其特征在于，通過無人機飛行控制器內部的IMU模塊采集原始三軸加速度計數據、原始三軸磁力計數據和原始三軸陀螺儀數據；通過以下步驟進行姿態解算：1)對原始三軸加速度計數據進行誤差修正和規范化，得到修正后的三軸加速度計數據記為：aXN，aYN，aZN；2)對原始三軸磁力計數據進行誤差修正和規范化，得到修正后的三軸磁力計數據記為：mXN，mYN，mZN；3)對步驟1)和2)中的兩組修正后的數據進行融合，得到第一姿態矩陣；4)對原始三軸陀螺儀數據進行誤差修正，得到修正后的三軸陀螺儀數據記為gX、gY、gZ；將修正后的三軸陀螺儀數據與上一個控制周期得到的第三姿態矩陣進行融合，得到第二姿態矩陣；第三姿態矩陣的初始值等于第一個控制周期得到的第一姿態矩陣的值；5)將第一姿態矩陣和第二姿態矩陣進行融合，得到該控制周期的第三姿態矩陣，即融合后的方向余弦矩陣。

【技術特征摘要】
1.一種無人機飛行控制器姿態解算方法，其特征在于，通過無人機飛行控制器內部的IMU模塊采集原始三軸加速度計數據、原始三軸磁力計數據和原始三軸陀螺儀數據；通過以下步驟進行姿態解算：1)對原始三軸加速度計數據進行誤差修正和規范化，得到修正后的三軸加速度計數據記為：aXN，aYN，aZN；2)對原始三軸磁力計數據進行誤差修正和規范化，得到修正后的三軸磁力計數據記為：mXN，mYN，mZN；3)對步驟1)和2)中的兩組修正后的數據進行融合，得到第一姿態矩陣；4)對原始三軸陀螺儀數據進行誤差修正，得到修正后的三軸陀螺儀數據記為gX、gY、gZ；將修正后的三軸陀螺儀數據與上一個控制周期得到的第三姿態矩陣進行融合，得到第二姿態矩陣；第三姿態矩陣的初始值等于第一個控制周期得到的第一姿態矩陣的值；5)將第一姿態矩陣和第二姿態矩陣進行融合，得到該控制周期的第三姿態矩陣，即融合后的方向余弦矩陣。2.根據權利要求1所述的無人機飛行控制器姿態解算方法，其特征在于，所述步驟1)具體為：讀取原始三軸加速度計數據(aX、aY、aZ)；根據以下公式對原始三軸加速度計數據進行零偏修正和靈敏度誤差修正：aXZ＝aX-zoaXaYZ＝(aY-zoaY)*saYaZZ＝(aZ-zoaZ)*saZ其中，zoaX，zoaY和zoaZ分別為三軸加速度計偏移值常量，saY和saZ分別為Y軸和Z軸加速度計靈敏度修正系數與X軸靈敏度修正系數的相對值；偏移值常量和靈敏度修正系數是由該儀器生產過程中的誤差決定的；再對三軸加速度計數據進行水平-磁偏角校正：aXH＝h11*aXZ+h12*aY+h13*aZaYH＝h21*aXZ+h22*aY+h23*aZaZH＝h31*aXZ+h32*aY+h33*aZ其中，水平-磁偏角校正矩陣通過校準測量得到；對三軸加速度計數據進行規范化處理：aSquareRoot=aXH2+aYH2+aZH2]]>aXN=aXHaSquareRoot]]>aYN=aYHaSquareRoot]]>aZN=aZHaSquareRoot.]]>3.根據權利要求2所述的無人機飛行控制器姿態解算方法，其特征在于，所述步驟2)具體為：讀取原始三軸磁力計數據(magX、magY、magZ)；先根據以下公式對原始三軸磁力計數據進行零偏修正和靈敏度誤差修正：mXZ＝magX-zomXmYZ＝(magY-zomY)*smYmZZ＝(magZ-zomZ)*smZ其中，zomX，zomY和zomZ分別為三軸磁力計偏移值常量，smY和smZ分別為Y軸和Z軸磁力計靈敏度修正系數與X軸磁力計靈敏度修正系數的相對值；再對三軸磁力計數據進行水平-磁偏角校正：mXH＝h11*mXZ+h12*mYZ+h13*mZZmYH＝h21*mXZ+h22*mYZ+h23*mZZmZH＝h31*mXZ+h32*mYZ+h33*mZZ然后對三軸磁力計數據進行規范化處理：mSquareRoot=mXH2+mYH2+mZH2]]>mXN=mXHmSquareRoot]]>mYN=mYHmSquareRoot]]>mZN=mZHmSquareRoot.]]>4.根據權利要求3所述的無人機飛行控制器姿態解算方法，其特征在于，所述步驟3)中，第一姿態矩陣記為：a11a12a13a21a22a23a31a32a33;]]>其中，第三行元素為：a31＝aXN，a32＝aYN，a33＝aZN；第一行元素為：a11=mXN-v*aXN(mXN-v*aXN)2+(mYN-v*aYN)2+(mZN-v*aZN)2]]>a12=mYN-v*aYN(mXN-v*aXN)2+(mYN-v*aYN)2+(mZN-v*aZN)2]]>a13=mZN-v*aZN(mXN-v*aXN)2+(mYN-v*aYN)2+(mZN-v*aZN)2]]>其中，v為垂直化因子，v＝mXN*aXN+mYN*aYN+mZN*aZN；第二行元素為：a21＝a13*a32-a12*a33a22＝a11*a33-a31*a13a23＝a31*a12-a11*a32。5.根據權利要求4所述的無人機飛行控制器姿態解算方法，其特征在于，所述步驟4)中，第二姿態矩陣記為：b11b12b13b21b22b23b31b32b33;]]>第三姿態矩陣記為：c11c12c13c21c22c23c31c32c33]]>第二姿態矩陣的計算過程為：首先將修正后的三軸陀螺儀數據分別乘以更新周期T，得到三軸角矢量αX、αY、αZ：αX＝gX*TαY＝gY*TαZ＝gZ*T計算四元數更新因子(r0、r1、r2、r3)：r0＝1-sSquareSum/8r1=0.5*αX*(1-sSquareSum24)]]>r2=0.5*αY*(1-sSquareSum24)]]>r3=0.5*αZ*(1-sSquareSum24)]]>其中，sSquareSum＝αX2+αY2+αZ2將上一控制周期得到的第三姿態矩陣轉換成四元數：q0=0.5*(1+c11+c22+c33)]]>若q0≠0，則有：q1=0.25q0*(c32-c23)q2=0.25q0*(c13-c31)q3=0.25q0*(c21-c12)]]>若q0＝0，則有：q1=c32-c23q2=c13-c31q3=c21-c12]]>規范化上述四元數：qSquareRoot=q02+q12+q22+q32]]>q0N=q0qSquareRoot]]>q1N=q1qSquareRoot]]>q2N=q2qSquareRoot]]>q3N=q3qSquareRoot]]>更新四元數：q0＝q0N*r0-q1N*r1-q2N*r2-q3N*r3q1＝q1N*r0+q0N*r1-q3N*r2+q2N*r3q2＝q2N*r0+q3N*r1+q0N*r2-q1N*r3q3＝q3N*r0-q2N*r1+q1N*r2+q0N*r3對上述更新的四元數進行規范化處理，得到新的q0N，q1N，q2N和q3N，并將其轉換成第二姿態矩陣：b11＝q0N2+q1N2-q2N2-q3N2b12＝2*q1N*q2N-2*q0N*q3Nb13＝2*q1N*q3N+2*q0N*q2Nb21＝2*q1N*q2N+2*q0N*q3Nb22＝q0N2-q1N2+q2N2-q3N2b23＝2*q2N*q3N-2*q0N*q1Nb31＝2*q1N*q3N-2*q0N*q2Nb32＝2*q2N*q3N+2*q0N*q1Nb33＝q0N2-q1N2-q2N...

【專利技術屬性】
技術研發人員：弓箭，
申請(專利權)人：湖南綠野航空科技有限公司，
類型：發明
國別省市：湖南;43