一种无人机飞行控制器姿态解算和控制方法技术

本发明专利技术公开了一种无人机飞行控制器姿态解算和控制方法，姿态解算方法使用由加速度计与磁力计融合得到的第一姿态矩阵，陀螺仪和上个控制周期的第三姿态矩阵融合得到的第二姿态矩阵，第一姿态矩阵和第二姿态矩阵融合得到包含高精度姿态信息的第三姿态矩阵。姿态控制方法以姿态空间矢量为控制对象，根据第三姿态矩阵得到各个电机的控制量，实现对无人机飞行器的姿态控制。本发明专利技术能适用于无人机全姿态的控制，实时性高，姿态解算和控制稳定。

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