用于四旋翼飞行器的控制方法
【专利摘要】本发明公开了一种对四旋翼飞行器的姿态角和位置均能有效、快速地控制的用于四旋翼飞行器的控制方法。该用于四旋翼飞行器的控制方法通过利用INS惯性导航和GPS导航组合导航的方法解决了单一的GPS导航技术易受干扰和遮挡，短时定位精度不高，输出频率有限并且输出不连续的缺点；可以获得四旋翼飞行器较为准确的导航信息参数，接着利用准确的导航信息参数计算得到四旋翼飞行器的控制输入量；进而将其输入四旋翼飞行器的控制系统进行飞行控制，该控制方法对导航信息参数进行优化，可以得到较为准确的导航信息参数，同时对控制算法进行优化，可以使四旋翼飞行器的姿态角和位置均能有效、快速地控制。适合在飞行控制领域推广应用。
【专利说明】
用于四旋翼飞行器的控制方法
技术领域
[0001] 本发明涉及飞行控制领域，尤其是一种用于四旋翼飞行器的控制方法。
【背景技术】
[0002] 由于四旋翼具有好的机动性能、简洁的结构和稳定的飞行能力，使得它在许多的 场合，如环境保护、质地勘测、农业管理，电力巡线和电影工业等领域发挥了重要作用。然 而，四旋翼飞行器是一个非线性、欠驱动、高耦合的控制系统，因此要获得准确的数学模型 和好的控制算法并不容易。国内外的许多研究人员对四旋翼的数学模型，导航技术和控制 方法进行了研究，并取得了许多重要成果。
[0003] 在飞行控制算法方面，由于四旋翼是一个非线性系统，因此对四旋翼的控制有两 个大的方向，一种是直接将系统按非线性处理，得到非线性控制算法;另一种是将非线性系 统线性化得到线性的系统表达式，然后运用线性系统理论的相关知识来获得控制算法。非 线性控制算法计算比较复杂，但控制效果准确。
[0004] 四旋翼飞行器是由两个相互垂直的刚性结构体构成十字型的机架，在四个角上分 别安装上直流电机。四旋翼在空间有六个自由度，可以实现六种基本的运动:沿着z轴的上 下运动，沿着x轴的前后运动，沿着y轴的左右运动;绕着x轴的滚转运动，绕着y轴的俯仰运 动，绕着z轴的偏航运动。同时增加或减少四个电机的推力，可以实现上下运动;通过不同的 推力控制策略可以实现前后、左右和偏航运动。四旋翼在三维空间内的任意一种复杂运动 都可以看作是这六种基本运动的组合。因此，对四旋翼的控制实际上就是对这六个自由度 的控制，其数学模型也是以这六个自由度为参数的。
[0005] 对四旋翼的飞行控制可以分为姿态控制和位置控制。姿态控制可以保证四旋翼在 飞行时能实时控制飞机的姿态以防止坠机;位置控制可以保证飞机可以向任何方向飞行。 但四旋翼强耦合、非线性、欠驱动、干扰敏感的特性，使得飞行控制系统的设计变得非常困 难。此外，控制系统的性能还受到模型准确性和传感器精度的影响。虽然四旋翼直升机控制 系统的设计面临着种种困难，但其独特的性能优势和广泛的应用领域吸引着越来越多的学 者对其进行研究。
[0006] 四旋翼的飞行控制的基础在于导航信息参数的准确性，获得的导航信息参数越准 确，其飞行控制越准确。在导航技术方面，目前应用得最多，最成熟的导航方式有INS惯性导 航和卫星导航。GPS卫星导航的优点是具有全球性、全天候、长时间定位精度高的特点，但缺 点是信号易受干扰和遮挡，在强电磁环境下和有高楼遮挡时，信号质量变差，并且其输出频 率有限，一般为1 一 10Hz，输出不连续，在需要快速更新信息的场合，如机动性和实时性要求 较高的无人机系统上，GPS卫星导航的缺点便凸显出来。而INS惯性导航系统是一种全自主 式的导航方式，因此具有很强的隐蔽性和抗干扰的能力，并且输出信息连续，短时间内定位 精度高。但由于MEMS-INS器件自身的特点，陀螺仪和加速度计有初始零偏、随机漂移等误 差，随着时间的累计作用，其误差越来越大，长时间定位精度较差，最终无法准确反映无人 机的姿态和位置信息。
【发明内容】
[0007] 本发明所要解决的技术问题是提供一种对四旋翼飞行器的姿态角和位置均能有 效、快速地控制的用于四旋翼飞行器的控制方法。
[0008] 本发明解决其技术问题所采用的技术方案为:该用于四旋翼飞行器的控制方法， 包括以下步骤：
[0009] S1、利用INS惯性导航与GPS导航以及磁力计的导航算法计算得到四旋翼飞行器的 导航信息参数；
[0010] S2、将步骤S1得到的导航信息参数代入如下公式得到四旋翼飞行器的控制输入量 UhUhUhUh
[0015]其中，叉1=丫，4=；?> =為，叉3 = 0，.'4=6 = 乂.;，.》5=浐％6..=_分=、.9是俯仰角，丫是滚 转角，供是偏航角，?为转速扰动，m为四旋翼飞行器的质量，g为重力加速度，ahahahazt、 a5、a6、a7、a8为大于零的正数
，1表示四旋翼飞行器的机臂长度，Ixx，Iyy，Izz 分别是四旋翼飞行器绕x轴，y轴，z轴的转动惯量，Jr为四旋翼飞行器的电机转子的转动惯 量，21是俯仰角的目标值和通过步骤S1计算得到实际值之差，z2是俯仰角速度目标值和通过 步骤S1计算得到实际值之差; Z3是滚转角的目标值和通过步骤S1计算得到实际值之差，Z4是 滚转角速度目标值和通过步骤S1计算得到实际值之差; 25是偏航角的目标值和通过步骤S1 计算得到实际值之差，Z6是偏航角速度目标值和通过步骤S1计算得到实际值之差;27是冗轴 高度的目标值和通过步骤S1计算得到实际值之差，Z8是Z轴高度方向上速度目标值和通过 步骤S1计算得到实际值之差；
[0016] S3、将步骤S2计算得到的四旋翼飞行器的控制输入量^、^、^、山输入四旋翼飞行 器的控制系统进行飞行控制。
[0017] 进一步的是，在步骤S1中，所述INS惯性导航与GPS导航以及磁力计的导航算法的 具体计算方法如下所述：
[0018] A、将INS惯性导航系统的陀螺仪测得的四旋翼飞行器角速度参数 代入四元数微分方程求解得到四元数9〇^142^3;其中64：*?4.，< 6,为陀螺仪在四旋翼 飞行器自身坐标系下的测得的三个轴的角速度信息；
[0019]所述四元数微分方程为：
[0021] B、将步骤A中求解的(^，(^，(^，(^代入下式求解得到姿态矩阵^^， %+9i+%+% 2(^2+^) 2(q^-q{)q2)
[0022] liq^-cf^q,) q,-(f,+(f2-qs 2(q1q,+(f i)q]) _2(^<7,+a0a,) 2(n 糾丨）an + %_
[0023] 根据下述与方向余弦的关系式 coi^a^sr -sin^;a^/+cosr/;sin^sin7 .-cos你 in;/
[0024] = sin^cos^ ccstpcosO sin 沒 cos^?sin,_sin妒sin沒oosf -sin^sinf-cosf?sin^cosf cm沒cos,
[0025] 计算得出四旋翼飞行器的INS惯性导航模块姿态角0、Y、f&
[0026] C、利用磁力计测得的偏航角#替换步骤B计算得到的偏航角
[0027] D、将INS惯性导航系统的加速度计测得的加速度参数护和步骤B中求解得到姿态 矩阵g代入下述微分方程中求解得到四旋翼飞行器在INS惯性导航坐标系下的东、北、天三 个方向上的速度信息VN VE VU，所述微分方程为：
[0028] =： df ~ (2< + < ) X V" +
[0029] 其中，vn=[VN vE vu]'分别为INS惯性导航坐标系中东、北、天方向上的速度，fiC为 地球自转角速度，为四旋翼飞行器绕INS惯性导航坐标系各轴向的转动角速率，g n为重力 加速度；
[0030] E、将步骤D计算得出的VN VE VU分别代入下式求解得出四旋翼飞行器在INS惯性导 航系统中的位置信息，其中L为炜度，A为经度，h为高度，
h = h(0)+/vudt,其中L(0)表示四旋翼飞行 7 器初始位置的炜度值A(0)表示四旋翼飞行器初始位置的经度值，h(0)表示四旋翼飞行器 距离地球表面的初始高度。Rm表示地球子午圈上的曲率半径，Rn表示炜度圈上的曲率半径； [0032] F、建立状态方程夂（/) =巧（/) .Y, (/) + G, (、/)%(/)和观测方程Z(t) ⑴，
[0033] X〗（t)表示INS惯性导航系统在t时刻的误差状态，它是一个15维的向量，如下所 示：
[0034] X(V)=[加 T 5vr 私戎為.於况況激《6 ▽r]，5vx，5Vy， 5VzS INS惯性导航系统沿东、北、天方向上的速度误差；(K，(i>y，(K为四旋翼飞行器的姿态角误 差;SL，SA，Sh分别代表四旋翼飞行器所在炜度、经度和高度误差；^，^，^分别代表陀螺仪 的随机漂移；u,.，v;分别为加速度计的随机漂移，其中匕⑴=，是一 L 0 fm (0J15x15 个15X15的矩阵;其中FN(t)对应于5%5%，^，(}^，<^，4)2，乩，从，511这9个参数的1吧惯 性导航系统矩阵，其非零元素如下：
[0048]卩5(1:)为5~，^，^，（^，<^，伞2,81，5入，5]1这9个参数与陀螺仪及加速度计漂移 _u_ 之间的变换矩阵，其维数是9X6,巧(〇= C； 03x3 ; _〇3x3 〇3x3 _
[0049] FM(t)为￡\6，￡2，^入与陀螺仪及加速度计漂移对应的1剌贯性导航系统矩 阵，是一个维数为6 X 6的对角线矩阵，表示如下：
[0050] FM(t) = diag[_l/Tgx _1/Tgy _1/Tgz _1/Tax _1/Tay _1/Taz];其中，Tgx表示陀螺仪x 轴的误差模型的时间常数，Tgy表示陀螺仪y轴的误差模型的时间常数，Tgz表示陀螺仪z轴的 误差模型的时间常数，T ax表示加速度计x轴误差模型的时间常数，Tay表示加速度计y轴误差 模型的时间常数，T az表示加速度计z轴误差模型的时间常数；
[0051] Gi(t) = diag[l 1 1......11] 15X15 ；
[0052] Wi(t)是一个15维的向量，如下所示：
[0053] Wi(t) = [ai a2 a3 a4 as a6 a7 as ag aio an ai2 ai3 ai4 ai5]，
[0054] ai a2 a3 a4 a5 a6 a7 as ag ai。aii ai2 ai3 ai4 ai5表不系统过程噪声序列；
[0055]
[0056] Z(t)为四旋翼飞行器在INS惯性导航中的位置速度信息与四旋翼飞行器在GPS导 航系统中的位置速度信息的差值，是一个6维向量，
[0057] Z(t) = [5vx+Nvx 5vy+NVy 5vz+Nvz (RM+h)5L+Ny (RM+h)cosL5人+NX 5h+Nh]T，其中，Nvx 表示GPS导航系统在x方向上的速度误差，Nvy表示GPS导航系统在y方向上的速度误差，Nvz表 示GPS导航系统在z方向上的速度误差，N x表示GPS导航系统在x方向上的位置误差，Ny表示 GPS导航系统在y方向上的位置误差，Nh表示GPS导航系统在z方向上的位置误差；
[Goss]
[0059] IIt(l) = \diag[\ 1 l]：〇；xl:j
[0060] g[(i?A/+/?) (/?'+/?)cosI
[0061] Vv(t) = [Nvx Nvy Nvz]t
[0062] VP(t) = [Nx Ny Nz]t
[0063] G、将上述得到的连续状态方程尤（/) = A G) + G/ G)离散化后得到知= ① k, k-iXk-i+Wk-1，其中
[0064] 将上述得到的连续观测方程ZUhmOXKtHVU)离散化后得到Zk = HkXk+Vk;
[0065] 其中I是单位矩阵，F是INS惯性导航系统的状态转移矩阵，A t是离散化后INS惯性 导航系统的采样时间；
[0066] H、将四旋翼飞行器在INS惯性导航系统中的位置速度信息与四旋翼飞行器在GPS 导航系统中的位置速度信息作差得到Z(t)在k时刻的观测信息z;
[0067] I、计算k时刻INS惯性导航系统状态的最优估计值之降.，童心.=_氧|卜.丨+.& (z -名.卜j), 其中 尤Hi为在k -1时刻IN S惯性导航系统状态的最优估计值， A 对(44-此+41，心―,+这-tA-1是INS惯性导航系统的噪声矩阵，其 ~ H 1 大小是由INS惯性导航元件的性能决定，A =好，& = \1 一KkH-KkHJ十[々篇， Rk是系统测量噪声的方差阵，其大小是由GPS接收机的性能决定；
[0068] J、将计算得到的值与四旋翼飞行器在INS惯性导航系统中的位置速度信息作 差得到最优的导航参数；
[0069] K、重复步骤H-J，得到连续的四旋翼飞行器的导航信息参数。
[0070] 本发明的有益效果:该用于四旋翼飞行器的控制方法通过利用INS惯性导航和GPS 导航组合导航的方法解决了单一的GPS导航技术易受干扰和遮挡，短时定位精度不高，输出 频率有限并且输出不连续的缺点；同时也解决了单一的INS惯性导航参数累计误差越来越 大，长时间定位精度发散的缺点，利用GPS导航长时间具有高的定位精度的优点来弥补INS 惯性导航累计误差随时间的增加而发散的缺点；利用INS惯性导航不受外界干扰、输出的导 航信息连续的特点弥补GPS易受干扰和输出频率有限的缺点，并且为了解决由惯性导航计 算出的偏航角无法找到真北，以及漂移较大的情况，本系统利用磁力计计算出的偏航角来 校正，获得地理真北方向和稳定的偏航角，可以获得四旋翼飞行器较为准确的导航信息参 数，接着利用准确的导航信息参数计算得到四旋翼飞行器的控制输入量;进而将其输入四 旋翼飞行器的控制系统进行飞行控制，该控制方法对导航信息参数进行优化，可以得到较 为准确的导航信息参数，同时对控制算法进行优化，可以使四旋翼飞行器的姿态角和位置 均能有效、快速地控制。
【附图说明】
[0071] 图1为本发明所述INS惯性导航与GPS导航以及磁力计的导航算法计算得出的炜度 误差值；
[0072]图2为本发明所述INS惯性导航与GPS导航以及磁力计的导航算法计算得出的炜度 误差方差值；
[0073]图3为本发明所述INS惯性导航与GPS导航以及磁力计的导航算法计算得出的经度 误差值；
[0074]图4为本发明所述INS惯性导航与GPS导航以及磁力计的导航算法计算得出的经度 误差方差值；
[0075]图5为本发明所述INS惯性导航与GPS导航以及磁力计的导航算法计算得出的高度 误差值；
[0076]图6为本发明所述INS惯性导航与GPS导航以及磁力计的导航算法计算得出的高度 误差方差值；
[0077]图7为本发明所述INS惯性导航与GPS导航以及磁力计的导航算法计算得出的东向 速度误差值；
[0078]图8为本发明所述INS惯性导航与GPS导航以及磁力计的导航算法计算得出的东向 速度误差方差值；
[0079]图9为本发明所述INS惯性导航与GPS导航以及磁力计的导航算法计算得出的北向 速度误差值；
[0080] 图10为本发明所述INS惯性导航与GPS导航以及磁力计的导航算法计算得出的北 向速度误差方差值；
[0081] 图11为本发明所述INS惯性导航与GPS导航以及磁力计的导航算法计算得出的天 向速度误差值；
[0082]图12为本发明所述INS惯性导航与GPS导航以及磁力计的导航算法计算得出的天 向速度误差方差值；
[0083]图13为滚转角的仿真效果图；
[0084]图14为俯仰角的仿真效果图；
[0085]图15为偏航角的仿真效果图；
[0086]图16为四旋翼飞行器在x轴，y轴和z轴的位置仿真图；
[0087]图17为四旋翼飞行器在三维空间中的轨迹图；
[0088] 图18为四旋翼飞行器的飞行轨迹图。
【具体实施方式】
[0089] 本发明所述的用于四旋翼飞行器的控制方法，包括以下步骤：
[0090] S1、利用INS惯性导航与GPS导航以及磁力计的导航算法计算得到四旋翼飞行器的 导航信息参数；
[0091] S2、将步骤S1得到的导航信息参数代入如下公式得到四旋翼飞行器的控制输入量 UhUhUhUh
[0096] 其中，xi= y，x2.二户=夫?叉3 = 0，；《4 =6.= i3.，=庐 x6 =分'=毛..，.0 是俯仰角，y 是滚 转角，口是偏航角，为转速扰动，m为四旋翼飞行器的质量，g为重力加速度，(^、(^、(^、(^、 a5、a6、a7、a8为大于零的正数：
，1表示四旋翼飞行器的机臂长度，Ixx，Iyy，Izz 分别是四旋翼飞行器绕x轴，y轴，z轴的转动惯量，Jr为四旋翼飞行器的电机转子的转动惯 量，21是俯仰角的目标值和通过步骤S1计算得到实际值之差，z2是俯仰角速度目标值和通过 步骤S1计算得到实际值之差; Z3是滚转角的目标值和通过步骤S1计算得到实际值之差，Z4是 滚转角速度目标值和通过步骤S1计算得到实际值之差; 25是偏航角的目标值和通过步骤S1 计算得到实际值之差，Z6是偏航角速度目标值和通过步骤S1计算得到实际值之差;27是冗轴 高度的目标值和通过步骤S1计算得到实际值之差，Z8是Z轴高度方向上速度目标值和通过 步骤S1计算得到实际值之差；
[0097] S3、将步骤S2计算得到的四旋翼飞行器的控制输入量^、^、^、山输入四旋翼飞行 器的控制系统进行飞行控制。
[0098] 该用于四旋翼飞行器的控制方法通过利用INS惯性导航和GPS导航组合导航的方 法解决了单一的GPS导航技术易受干扰和遮挡，短时定位精度不高，输出频率有限并且输出 不连续的缺点；同时也解决了单一的INS惯性导航参数累计误差越来越大，长时间定位精度 发散的缺点，利用GPS导航长时间具有高的定位精度的优点来弥补INS惯性导航累计误差随 时间的增加而发散的缺点;利用INS惯性导航不受外界干扰、输出的导航信息连续的特点弥 补GPS易受干扰和输出频率有限的缺点，并且为了解决由惯性导航计算出的偏航角无法找 到真北，以及漂移较大的情况，本系统利用磁力计计算出的偏航角来校正，获得地理真北方 向和稳定的偏航角，可以获得四旋翼飞行器较为准确的导航信息参数，接着利用准确的导 航信息参数计算得到四旋翼飞行器的控制输入量;进而将其输入四旋翼飞行器的控制系统 进行飞行控制，该控制方法对导航信息参数进行优化，可以得到较为准确的导航信息参数， 同时对控制算法进行优化，可以使四旋翼飞行器的姿态角和位置均能有效、快速地控制。
[0099] 所述控制中心采用如下方法计算得到最终的导航信息，其具体计算方法如下所 述：
[0100] 在步骤S1中，所述INS惯性导航与GPS导航以及磁力计的导航算法的具体计算方法 如下所述：
[0101] A、将INS惯性导航系统的陀螺仪测得的四旋翼飞行器角速度参数乂fa， 代入四元数微分方程求解得到四元数9〇^142^3;其中?^，64/?4 2为陀螺仪在四旋翼 飞行器自身坐标系下的测得的三个轴的角速度信息；
[0102] 所述四元数微分方程为：
[0104] B、将步骤A中求解的(^，(^(^，阳代入下式求解得到姿态矩阵^， 2(秘 +祕）2(_-跡）
[0105] Cl = 2(qlq2 -qQq3) 免 2(q2q3 +q0ql) _2(q]q,+q{)q2) 2(c/nq') q0-q, -q2 +q,_
[0106] 根据下述Cf与方向余弦的关系式 cos (poos y -sin (pccBy+cos c^sin ^sin/ -a?6^sin/
[oi07] sirupccBO ccs(pc(B〇 %mO _cos^sin;/-sin^sin^oosf -siripsiiif-cos穸sin汐cos/ cos^GOSf _
[0108]计算得出四旋翼飞行器的INS惯性导航模块姿态角0、y、P;
[0109] C、利用磁力计测得的偏航角※替换步骤B计算得到的偏航角 [011 0] D、将INS惯性导航系统的加速度计测得的加速度参数沪和步骤B中求解得到姿态 矩阵g代入下述微分方程中求解得到四旋翼飞行器在INS惯性导航坐标系下的东、北、天三 个方向上的速度信息VN VE VU，所述微分方程为：
[cm] v?=C；//7-(2< + <)xvw+gH
[0112] 其中，vn=[VN VE VU]'分别为INS惯性导航坐标系中东、北、天方向上的速度，<为 地球自转角速度，< 为四旋翼飞行器绕INS惯性导航坐标系各轴向的转动角速率，gn为重力 加速度；
[0113] E、将步骤D计算得出的VN VE vu分别代入下式求解得出四旋翼飞行器在INS惯性导 航系统中的位置信息，其中L为炜度，A为经度，h为高度，
h = h(0)+/vudt,其中L(0)表示四旋翼飞行 '， 器初始位置的炜度值A(0)表示四旋翼飞行器初始位置的经度值，h(0)表示四旋翼飞行器 距离地球表面的初始高度。Rm表示地球子午圈上的曲率半径，Rn表示炜度圈上的曲率半径；
[0115] F、建立状态方程夂（/) = Z7, (/)丨（/) + 6 (/) % (/)和观测方程Z (t) = H( t )X〗（t) +V (t)，
[0116] XKt)表示INS惯性导航系统在t时刻的误差状态，它是一个15维的向量，如下所 示：
[0117] 本⑷=[火况，，與我為為级说廉q $ & % Vz]，SVx，SVy，SVz* INS惯性导航系统沿东、北、天方向上的速度误差；(K，(i>y，为四旋翼飞行器的姿态角误 差;SL，SA，Sh分别代表四旋翼飞行器所在炜度、经度和高度误差；^，^，^分别代表陀螺仪 的随机漂移^乂^分别为加速度计的随机漂移^中朽^7^^* ，^ ，是一 L 0 & (，)L 个15X15的矩阵;其中FN(t)对应于5%5%，^，(}^，<^，4)2，乩，从，511这9个参数的1吧惯 性导航系统矩阵，其非零元素如下：
[0131] 卩5(1:)为5~，5%，^，(^，<^，巾2,81，5人，5]1这9个参数与陀螺仪及加速度计漂移 & 之间的变换矩阵，其维数是9X6，K(〇= q 03x3 ; _〇3x3 〇.3.x3
[0132] Fm( t)为ex，ey，ez，▽、，▽,.▽:与陀螺仪及加速度计漂移对应的INS惯性导航系统矩 阵，是一个维数为6 X 6的对角线矩阵，表示如下：
[0133] FM(t) = diag[_l/Tgx _1/Tgy _1/Tgz _1/Tax _1/Tay _1/Taz];其中，Tgx表示陀螺仪x 轴的误差模型的时间常数，Tgy表示陀螺仪y轴的误差模型的时间常数，Tgz表示陀螺仪z轴的 误差模型的时间常数，T ax表示加速度计x轴误差模型的时间常数，Tay表示加速度计y轴误差 模型的时间常数，T az表示加速度计z轴误差模型的时间常数；
[0134] Gi(t) = diag[l 1 1......1 I]i5xi5；
[0135] Wi(t)是一个15维的向量，如下所示：
[0136] Wi(t) = [ai a2 a3 a4 as a6 a7 as ag aio an ai2 ai3 ai4 ai5]，
[0137] ai a2 a3 a4 a5 a6 a7 as ag ai。aii ai2 ai3 ai4 ai5表不系统过程噪声序列；
[0138] Z(t)为四旋翼飞行器在INS惯性导航中的位置速度信息与四旋翼飞行器在GPS导 航系统中的位置速度信息的差值，是一个6维向量，
[0139] Z(t) = [5vx+Nvx 5vy+NVy 5vz+Nvz (RM+h)5L+Ny (RM+h)cosL5入+NX 5h+Nh]T，其中，Nvx 表示GPS导航系统在x方向上的速度误差，Nvy表示GPS导航系统在y方向上的速度误差，Nvz表 示GPS导航系统在z方向上的速度误差，N x表示GPS导航系统在x方向上的位置误差，Ny表示 GPS导航系统在y方向上的位置误差，Nh表示GPS导航系统在z方向上的位置误差； _]剛m '其中
[0141] ll,U)^\diag[\ 1 1 ]!〇,,,：}
[0142] //J/) = [(/?,,, +/;) (R, +/;)cosL l]:03x6}
[0143] Vv(t) = [Nvx Nvy Nvz]t
[0144] VP(t) = [Nx Ny Nz]t
[0145] G、将上述得到的连续状态方程= + 离散化后得到Xk = ① k, k-lXk-l+Wk-1，其中
[0146] 将上述得到的连续观测方程Z(t)=H(t)XI(t)+V(t)离散化后得到Zk = HkXk+Vk;
[0147] 其中I是单位矩阵，F是INS惯性导航系统的状态转移矩阵，A t是离散化后INS惯性 导航系统的采样时间；
[0148] H、将四旋翼飞行器在INS惯性导航系统中的位置速度信息与四旋翼飞行器在GPS 导航系统中的位置速度信息作差得到Z(t)在k时刻的观测信息z;
[0149] I、计算k时刻INS惯性导航系统状态的最优估计值之力，名^-=之|卜丨-小 其中，矣h d为在k _ 1时刻IN S惯性导航系统状态的最优估计值， A = VXbVM+&r，1是顶S惯性导航系统的噪声矩阵， 其大小是由INS惯性导航元件的性能决定，札=^Pk = [l-KLHk]PLkA [l-KkHkf +K,RkK[, Rk是系统测量噪声的方差阵，其大小是由GPS接收机的性能决定；
[0150] J、将计算得到的之丨值与四旋翼飞行器在INS惯性导航系统中的位置速度信息作 差得到最优的导航参数；
[0151] K、重复步骤H-J，得到连续的四旋翼飞行器的导航信息参数。
[0152] 利用上述方法计算得出的导航信息，能够准确反映四旋翼飞行器的姿态和位置信 息，可以实现较好的定位导航精度，图1为本发明所述INS惯性导航与GPS导航以及磁力计的 导航算法计算得出的炜度误差值；图2为本发明所述INS惯性导航与GPS导航以及磁力计的 导航算法计算得出的炜度误差方差值；图3为本发明所述INS惯性导航与GPS导航以及磁力 计的导航算法计算得出的经度误差值；图4为本发明所述INS惯性导航与GPS导航以及磁力 计的导航算法计算得出的经度误差方差值；图5为本发明所述INS惯性导航与GPS导航以及 磁力计的导航算法计算得出的高度误差值；图6为本发明所述INS惯性导航与GPS导航以及 磁力计的导航算法计算得出的高度误差方差值；图7为本发明所述INS惯性导航与GPS导航 以及磁力计的导航算法计算得出的东向速度误差值；图8为本发明所述INS惯性导航与GPS 导航以及磁力计的导航算法计算得出的东向速度误差方差值;图9为本发明所述INS惯性导 航与GPS导航以及磁力计的导航算法计算得出的北向速度误差值；图10为本发明所述INS惯 性导航与GPS导航以及磁力计的导航算法计算得出的北向速度误差方差值；图11为本发明 所述INS惯性导航与GPS导航以及磁力计的导航算法计算得出的天向速度误差值；图12为本 发明所述INS惯性导航与GPS导航以及磁力计的导航算法计算得出的天向速度误差方差值； 从上述测试结果图可以看出，本发明所述INS惯性导航与GPS导航以及磁力计的导航算法得 出的经度、炜度、高度的误差方差均能快速收敛至比较小的数值;对位置、速度等导航信息 也能实现滤平滑作用，不会产生大的跳变，系统的稳定性较强。
[0153] 实施例一
[0154] 使用MATLAB对本发明所述用于四旋翼飞行器的控制方法进行仿真，设置滚转、俯 仰和偏航角的初始值为某一非零角度，目标值均为零度，即飞机姿态保持水平，通过本发明 所述用于四旋翼飞行器的控制方法进行控制，在经过一段时间后，三个姿态角均能趋近于 零度，使姿态保持水平。图13为滚转角的仿真效果图；图14为俯仰角的仿真效果图；图15为 偏航角的仿真效果图；
[0155] 实施例二
[0156] 在三维坐标中，设置四旋翼飞行器的初始位置在(0,0,0，），目标期望的位置坐标 为（1，〇，1)，经过反步法的控制，使飞机逼近坐标点（1，〇，1)。图16为四旋翼飞行器在x轴，y 轴和z轴的位置仿真图；图17为四旋翼飞行器在三维空间中的轨迹图。
[0157] 实施例三
[0158] 设置4个航点（0，1，1)，（1，1，1)，（1，0，1)，（0,0，1)，用本发明所述用于四旋翼飞行 器的控制方法控制四旋翼飞行器依次经过这些航点，图18为四旋翼飞行器的飞行轨迹图。 [0159]实验表明，用本发明所述用于四旋翼飞行器的控制方法对四旋翼飞行器的姿态角 和位置均能有效、快速地控制其收敛于目标值。
【主权项】
1. 用于四旋翼飞行器的控制方法，其特征在于包括以下步骤： 51、 利用INS惯性导航与GPS导航以及磁力计的导航算法计算得到四旋翼飞行器的导航 信息参数； 52、 将步骤S1得到的导航信息参数代入如下公式得到四旋翼飞行器的控制输入量山、 U2、U3、U4;其中，Χ1= γ ,? =.f =?.，Χ3 = Θ，x4:=汐= ir3，.? =砂 =:多=?，θ是俯仰角，γ 是滚转角，庐 是偏航角，Ωρ为转速扰动，m为四旋翼飞行器的质量，g为重力加速度，(^、(^(^、(^、(^、(^、(^、(^为大1表示四旋翼飞行器的机臂长度，Ιχχ，Iyy，Izz分别是四旋翼飞行器绕X轴，y轴，z轴的转动 惯量，Jr为四旋翼飞行器的电机转子的转动惯量，21是俯仰角的目标值和通过步骤S1计算得 到实际值之差， Z2是俯仰角速度目标值和通过步骤S1计算得到实际值之差;Z3是滚转角的目 标值和通过步骤S1计算得到实际值之差， Z4是滚转角速度目标值和通过步骤S1计算得到实 际值之差;Z5是偏航角的目标值和通过步骤S1计算得到实际值之差，Z6是偏航角速度目标值 和通过步骤S1计算得到实际值之差； 27是2轴高度的目标值和通过步骤S1计算得到实际值 之差，Z8是Z轴高度方向上速度目标值和通过步骤S1计算得到实际值之差； 53、 将步骤S2计算得到的四旋翼飞行器的控制输入量^、^、^、山输入四旋翼飞行器的 控制系统进行飞行控制。2. 如权利要求1所述的用于四旋翼飞行器的控制方法，其特征在于:在步骤S1中，所述 INS惯性导航与GPS导航以及磁力计的导航算法的具体计算方法如下所述： A、将INS惯性导航系统的陀螺仪测得的四旋翼飞行器角速度参数6么,，代入 四元数微分方程求解得到四元数9〇^1，屯，阳；其中64,64,64为陀螺仪在四旋翼飞行 器自身坐标系下的测得的三个轴的角速度信息； 所述四元数微分方程为：B、 将步骤A中求解的qo，qi，q2，q3代入下式求解得到姿态矩阵0，根据下述与方向余弦的关系式计算得出四旋翼飞行器的INS惯性导航模块姿态角θ、γ、P; C、 利用磁力计测得的偏航角P替换步骤B计算得到的偏航角 D、 将INS惯性导航系统的加速度计测得的加速度参数户和步骤B中求解得到姿态矩阵0 代入下述微分方程中求解得到四旋翼飞行器在INS惯性导航坐标系下的东、北、天三个方向 上的速度信息VN VE VU，所述微分方程为：其中，Vn=[VN VE VU]'分别为INS惯性导航坐标系中东、北、天方向上的速度，为地球 自转角速度，< 为四旋翼飞行器绕INS惯性导航坐标系各轴向的转动角速率，gn为重力加速 度； E、 将步骤D计算得出的VN VE Vu分别代入下式求解得出四旋翼飞行器在INS惯性导航系 统中的位置信息，其中L为炜度，λ为经度，h为高度，h = h(0)+JVudt，其中L(0)表示四旋翼飞行器初 '， 始位置的炜度值，λ(ο)表示四旋翼飞行器初始位置的经度值，h(0)表示四旋翼飞行器距离 地球表面的初始高度。Rm表示地球子午圈上的曲率半径，Rn表示炜度圈上的曲率半径； F、 建立状态方程< ⑴=6⑴⑴十G, (/)%⑴和观测方程Z(t) =H(t)X〗(t)+V(t)， XKt)表示INS惯性导航系统在t时刻的误差状态，它是一个15维的向量，如下所示： 9vy ：Svz φχ ^ φζ 81 ^ Μ εχ c Vr ν^,δγχ,5Vy,5Vz^INS 惯性导航系统沿东、北、天方向上的速度误差；Φχ，ΦΥ，Φζ为四旋翼飞行器的姿态角误差;S ?，δλ，δ?!分别代表四旋翼飞行器所在炜度、经度和高度误差；εχ，εγ，εζ分别代表陀螺仪的随 机漂移；1，1，1分别为加速度计的随机漂移，是一个15 \15的矩阵；其中？〃(〇对应于61，6%，61，(^，(^，(})^1^\411这9个参数的1吣惯性导 航系统矩阵，其非零元素如下：Fs(t)为3"，5%，5￥2，(^，(^，(|)2况，3入，5]1这9个参数与陀螺仪及加速度计漂移之间的 变换矩阵，FM⑴为^士士^^^与陀螺仪及加速度计漂移对应的預对贯性导航系统矩阵^ 一个维数为6 X 6的对角线矩阵，表示如下： FM(t)=diag[-l/Tgx -1/Tgy -1/Tgz -1/Tax -1/Tay -1/Taz];其中，Tgx表示陀螺仪X轴的 误差模型的时间常数，Tgy表示陀螺仪y轴的误差模型的时间常数，Tgz表示陀螺仪z轴的误差 模型的时间常数，T ax表示加速度计X轴误差模型的时间常数，Tay表示加速度计y轴误差模型 的时间常数，T az表示加速度计z轴误差模型的时间常数； Gi(t)=diag[l 11......1 1]?5χ?5; Wi(t)是一个15维的向量，如下所示： Wi(t) = [ai a2 a3 a4 as a6 a7 as ag aio aii ai2 ai3 ai4 ai5]， ai a2 a3 a4 as a6 a7 as ag ai。aii ai2 ai3 ai4 ai5表不系统过程噪声序列； Z(t)为四旋翼飞行器在INS惯性导航中的位置速度信息与四旋翼飞行器在GPS导航系 统中的位置速度信息的差值，是一个6维向量， Ζ(?) = [δνχ+Ννχ δνγ+Ννγ δνζ+Ννζ (RM+h)3L+Ny (RM+h)cosL3A+Nx 3h+Nh]T，其中，Ννχ表不 GPS导航系统在x方向上的速度误差，Nvy表示GPS导航系统在y方向上的速度误差，Nvz表示 GPS导航系统在ζ方向上的速度误差，Νχ表示GPS导航系统在X方向上的位置误差，Ny表示GPS 导航系统在y方向上的位置误差，Nh表示GPS导航系统在z方向上的位置误差； G、 将上述得到的连续状态方利Λ,(〇二dR, (/) + (?,(/)%⑴离散化后得到Xk = Φ??,??-iXk-ι+Wk-1，将上述得到的连续观测方程Ζ (t) =H( t )X: (t) +V( t)离散化后得到Zk=HkXk+Vk; 其中I是单位矩阵，F是INS惯性导航系统的状态转移矩阵，△ t是离散化后INS惯性导航 系统的采样时间； H、 将四旋翼飞行器在INS惯性导航系统中的位置速度信息与四旋翼飞行器在GPS导航 系统中的位置速度信息作差得到Z(t)在k时刻的观测信息ζ; I、 计算k时亥[|INS惯性导航系统状态的最优估计值之，之=之(ζ-J , 其中，之Η = Φ，之为在k -1时刻IN S惯性导航系统状态的最优估计值， 尸i =Φ…尸A-^INS惯性导航系统的噪声矩阵， 其大小是由INS惯性导航元件的性能决定，+心/?，/ , Rk是系统测量噪声的方差阵，其大小是由GPS接收机的性能决定； J、 将计算得到的值与四旋翼飞行器在INS惯性导航系统中的位置速度信息作差得 到最优的导航参数； K、 重复步骤H-J，得到连续的四旋翼飞行器的导航信息参数。
【文档编号】G05D1/08GK105929836SQ201610244268
【公开日】2016年9月7日
【申请日】2016年4月19日
【发明人】张瑜, 李诗扬
【申请人】成都翼比特自动化设备有限公司