CN105929836B - 用于四旋翼飞行器的控制方法 - Google Patents
用于四旋翼飞行器的控制方法 Download PDFInfo
- Publication number
- CN105929836B CN105929836B CN201610244268.9A CN201610244268A CN105929836B CN 105929836 B CN105929836 B CN 105929836B CN 201610244268 A CN201610244268 A CN 201610244268A CN 105929836 B CN105929836 B CN 105929836B
- Authority
- CN
- China
- Prior art keywords
- inertial navigation
- rotor aircraft
- navigation system
- ins inertial
- ins
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 30
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 claims abstract description 13
- 238000004422 calculation algorithm Methods 0.000 claims description 38
- 239000011159 matrix material Substances 0.000 claims description 33
- 230000001133 acceleration Effects 0.000 claims description 9
- 230000005484 gravity Effects 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000008569 process Effects 0.000 claims description 4
- 238000005259 measurement Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 claims description 3
- 230000009897 systematic effect Effects 0.000 claims description 3
- 230000009466 transformation Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000005516 engineering process Methods 0.000 abstract description 5
- 230000000903 blocking effect Effects 0.000 abstract 1
- 230000002452 interceptive effect Effects 0.000 abstract 1
- 230000033001 locomotion Effects 0.000 description 11
- 230000007547 defect Effects 0.000 description 10
- 238000010586 diagram Methods 0.000 description 10
- 230000000694 effects Effects 0.000 description 7
- 238000004088 simulation Methods 0.000 description 6
- 230000008901 benefit Effects 0.000 description 4
- 238000013178 mathematical model Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 230000001186 cumulative effect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0808—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for aircraft
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/10—Simultaneous control of position or course in three dimensions
- G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种对四旋翼飞行器的姿态角和位置均能有效、快速地控制的用于四旋翼飞行器的控制方法。该用于四旋翼飞行器的控制方法通过利用INS惯性导航和GPS导航组合导航的方法解决了单一的GPS导航技术易受干扰和遮挡，短时定位精度不高，输出频率有限并且输出不连续的缺点；可以获得四旋翼飞行器较为准确的导航信息参数，接着利用准确的导航信息参数计算得到四旋翼飞行器的控制输入量；进而将其输入四旋翼飞行器的控制***进行飞行控制，该控制方法对导航信息参数进行优化，可以得到较为准确的导航信息参数，同时对控制算法进行优化，可以使四旋翼飞行器的姿态角和位置均能有效、快速地控制。适合在飞行控制领域推广应用。
Description
技术领域
本发明涉及飞行控制领域，尤其是一种用于四旋翼飞行器的控制方法。
背景技术
由于四旋翼具有好的机动性能、简洁的结构和稳定的飞行能力，使得它在许多的场合，如环境保护、质地勘测、农业管理，电力巡线和电影工业等领域发挥了重要作用。然而，四旋翼飞行器是一个非线性、欠驱动、高耦合的控制***，因此要获得准确的数学模型和好的控制算法并不容易。国内外的许多研究人员对四旋翼的数学模型，导航技术和控制方法进行了研究，并取得了许多重要成果。
在飞行控制算法方面，由于四旋翼是一个非线性***，因此对四旋翼的控制有两个大的方向，一种是直接将***按非线性处理，得到非线性控制算法；另一种是将非线性***线性化得到线性的***表达式,然后运用线性***理论的相关知识来获得控制算法。非线性控制算法计算比较复杂，但控制效果准确。
四旋翼飞行器是由两个相互垂直的刚性结构体构成十字型的机架，在四个角上分别安装上直流电机。四旋翼在空间有六个自由度，可以实现六种基本的运动：沿着z轴的上下运动，沿着x轴的前后运动，沿着y轴的左右运动；绕着x轴的滚转运动，绕着y轴的俯仰运动，绕着z轴的偏航运动。同时增加或减少四个电机的推力，可以实现上下运动；通过不同的推力控制策略可以实现前后、左右和偏航运动。四旋翼在三维空间内的任意一种复杂运动都可以看作是这六种基本运动的组合。因此，对四旋翼的控制实际上就是对这六个自由度的控制，其数学模型也是以这六个自由度为参数的。
对四旋翼的飞行控制可以分为姿态控制和位置控制。姿态控制可以保证四旋翼在飞行时能实时控制飞机的姿态以防止坠机；位置控制可以保证飞机可以向任何方向飞行。但四旋翼强耦合、非线性、欠驱动、干扰敏感的特性，使得飞行控制***的设计变得非常困难。此外，控制***的性能还受到模型准确性和传感器精度的影响。虽然四旋翼直升机控制***的设计面临着种种困难，但其独特的性能优势和广泛的应用领域吸引着越来越多的学者对其进行研究。
四旋翼的飞行控制的基础在于导航信息参数的准确性，获得的导航信息参数越准确，其飞行控制越准确。在导航技术方面，目前应用得最多，最成熟的导航方式有INS惯性导航和卫星导航。GPS卫星导航的优点是具有全球性、全天候、长时间定位精度高的特点，但缺点是信号易受干扰和遮挡，在强电磁环境下和有高楼遮挡时，信号质量变差，并且其输出频率有限，一般为1—10Hz，输出不连续，在需要快速更新信息的场合，如机动性和实时性要求较高的无人机***上，GPS卫星导航的缺点便凸显出来。而INS惯性导航***是一种全自主式的导航方式，因此具有很强的隐蔽性和抗干扰的能力，并且输出信息连续，短时间内定位精度高。但由于MEMS-INS器件自身的特点，陀螺仪和加速度计有初始零偏、随机漂移等误差，随着时间的累计作用，其误差越来越大，长时间定位精度较差，最终无法准确反映无人机的姿态和位置信息。
发明内容
本发明所要解决的技术问题是提供一种对四旋翼飞行器的姿态角和位置均能有效、快速地控制的用于四旋翼飞行器的控制方法。
本发明解决其技术问题所采用的技术方案为：该用于四旋翼飞行器的控制方法，包括以下步骤：
S1、利用INS惯性导航与GPS导航以及磁力计的导航算法计算得到四旋翼飞行器的导航信息参数；
S2、将步骤S1得到的导航信息参数代入如下公式得到四旋翼飞行器的控制输入量U1、U2、U3、U4；
其中，x1＝γ，x3＝θ，θ是俯仰角，γ是滚转角，是偏航角，Ωr为转速扰动，m为四旋翼飞行器的质量，g为重力加速度，α1、α2、α3、α4、α5、α6、α7、α8为大于零的正数， l表示四旋翼飞行器的机臂长度，Ixx,Iyy,Izz分别是四旋翼飞行器绕x轴，y轴，z轴的转动惯量，Jr为四旋翼飞行器的电机转子的转动惯量，z1是俯仰角的目标值和通过步骤S1计算得到实际值之差，z2是俯仰角速度目标值和通过步骤S1计算得到实际值之差；z3是滚转角的目标值和通过步骤S1计算得到实际值之差，z4是滚转角速度目标值和通过步骤S1计算得到实际值之差；z5是偏航角的目标值和通过步骤S1计算得到实际值之差，z6是偏航角速度目标值和通过步骤S1计算得到实际值之差；z7是Z轴高度的目标值和通过步骤S1计算得到实际值之差，z8是Z轴高度方向上速度目标值和通过步骤S1计算得到实际值之差；
S3、将步骤S2计算得到的四旋翼飞行器的控制输入量U1、U2、U3、U4输入四旋翼飞行器的控制***进行飞行控制。
进一步的是，在步骤S1中，所述INS惯性导航与GPS导航以及磁力计的导航算法的具体计算方法如下所述：
A、将INS惯性导航***的陀螺仪测得的四旋翼飞行器角速度参数代入四元数微分方程求解得到四元数q0，q1，q2，q3；其中为陀螺仪在四旋翼飞行器自身坐标系下的测得的三个轴的角速度信息；
所述四元数微分方程为：
B、将步骤A中求解的q0，q1，q2，q3代入下式求解得到姿态矩阵
根据下述与方向余弦的关系式
计算得出四旋翼飞行器的INS惯性导航模块姿态角θ、γ、
C、利用磁力计测得的偏航角替换步骤B计算得到的偏航角
D、将INS惯性导航***的加速度计测得的加速度参数fb和步骤B中求解得到姿态矩阵代入下述微分方程中求解得到四旋翼飞行器在INS惯性导航坐标系下的东、北、天三个方向上的速度信息vN vE vU，所述微分方程为：
其中，vn＝[vN vE vU]'为四旋翼飞行器在INS惯性导航坐标系下的东、北、天三个方向上的速度；为在INS惯性导航坐标系下地球自转角速度，为四旋翼飞行器在INS惯性导航坐标系下绕INS惯性导航坐标系各轴向的转动角速率，gn为在INS惯性导航坐标系下重力加速度；
E、将步骤D计算得出的vN vE vU分别代入下式求解得出四旋翼飞行器在INS惯性导航***中的位置信息，其中L为纬度，λ为经度，h为高度，
h＝h(0)+∫vUdt，其中L(0)表示四旋翼飞行器初始位置的纬度值，λ(0)表示四旋翼飞行器初始位置的经度值，h(0)表示四旋翼飞行器距离地球表面的初始高度。RM表示地球子午圈上的曲率半径，RN表示纬度圈上的曲率半径；
F、建立状态方程和观测方程Z(t)＝H(t)XI(t)+V(t)，
XI(t)表示INS惯性导航***在t时刻的误差状态，它是一个15维的向量，如下所示：
δvx,δvy,δvz为INS惯性导航***沿东、北、天方向上的速度误差；φx,φy,φz为四旋翼飞行器的姿态角误差；δL,δλ,δh分别代表四旋翼飞行器所在纬度、经度和高度误差；εx,εy,εz分别代表陀螺仪的随机漂移；分别为加速度计的随机漂移，其中是一个15×15的矩阵；其中FN(t)对应于δvx,δvy,δvz，φx,φy,φz，δL,δλ,δh这9个参数的INS惯性导航***矩阵，其非零元素如下：
F(1,5)＝-fz F(1,6)＝fy
F(2,4)＝fz F(2,6)＝-fx
F(3,4)＝-fy F(3,5)＝fx F(3,7)＝-2ωievx sinL
F(5,7)＝-ωie sinL
F(9,3)＝1
FS(t)为δvx,δvy,δvz，φx,φy,φz，δL,δλ,δh这9个参数与陀螺仪及加速度计漂移之间的变换矩阵，其维数是9×6，
FM(t)为εx,εy,εz，与陀螺仪及加速度计漂移对应的INS惯性导航***矩阵，是一个维数为6×6的对角线矩阵，表示如下：
FM(t)＝diag[-1/Tgx -1/Tgy -1/Tgz -1/Tax -1/Tay -1/Taz]；其中，Tgx表示陀螺仪x轴的误差模型的时间常数，Tgy表示陀螺仪y轴的误差模型的时间常数，Tgz表示陀螺仪z轴的误差模型的时间常数，Tax表示加速度计x轴误差模型的时间常数，Tay表示加速度计y轴误差模型的时间常数，Taz表示加速度计z轴误差模型的时间常数；
GI(t)＝diag[1 1 1......1 1]15×15；
WI(t)是一个15维的向量，如下所示：
WI(t)＝[a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15]，
a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15表示***过程噪声序列；
Z(t)为四旋翼飞行器在INS惯性导航中的位置速度信息与四旋翼飞行器在GPS导航***中的位置速度信息的差值，是一个6维向量，
Z(t)＝[δvx+Nvx δvy+Nvy δvz+Nvz (RM+h)δL+Ny (RM+h)cosLδλ+Nx δh+Nh]T，其中，Nvx表示GPS导航***在x方向上的速度误差，Nvy表示GPS导航***在y方向上的速度误差，Nvz表示GPS导航***在z方向上的速度误差，Nx表示GPS导航***在x方向上的位置误差，Ny表示GPS导航***在y方向上的位置误差，Nh表示GPS导航***在z方向上的位置误差；
其中
Vv(t)＝[Nvx Nvy Nvz]T
Vp(t)＝[Nx Ny Nz]T
G、将上述得到的连续状态方程离散化后得到Xk＝Φk,k-1Xk-1+Wk-1，其中
将上述得到的连续观测方程Z(t)＝H(t)XI(t)+V(t)离散化后得到Zk＝HkXk+Vk；
其中I是单位矩阵，F是INS惯性导航***的状态转移矩阵，Δt是离散化后INS惯性导航***的采样时间；
H、将四旋翼飞行器在INS惯性导航***中的位置速度信息与四旋翼飞行器在GPS导航***中的位置速度信息作差得到Z(t)在k时刻的观测信息z；
I、计算k时刻INS惯性导航***状态的最优估计值 其中， 为在k-1时刻INS惯性导航***状态的最优估计值， Qk-1是INS惯性导航***的噪声矩阵，其大小是由INS惯性导航元件的性能决定， Rk是***测量噪声的方差阵，其大小是由GPS接收机的性能决定；
J、将计算得到的值与四旋翼飞行器在INS惯性导航***中的位置速度信息作差得到最优的导航参数；
K、重复步骤H-J，得到连续的四旋翼飞行器的导航信息参数。
本发明的有益效果：该用于四旋翼飞行器的控制方法通过利用INS惯性导航和GPS导航组合导航的方法解决了单一的GPS导航技术易受干扰和遮挡，短时定位精度不高，输出频率有限并且输出不连续的缺点；同时也解决了单一的INS惯性导航参数累计误差越来越大，长时间定位精度发散的缺点，利用GPS导航长时间具有高的定位精度的优点来弥补INS惯性导航累计误差随时间的增加而发散的缺点；利用INS惯性导航不受外界干扰、输出的导航信息连续的特点弥补GPS易受干扰和输出频率有限的缺点，并且为了解决由惯性导航计算出的偏航角无法找到真北，以及漂移较大的情况，本***利用磁力计计算出的偏航角来校正，获得地理真北方向和稳定的偏航角，可以获得四旋翼飞行器较为准确的导航信息参数，接着利用准确的导航信息参数计算得到四旋翼飞行器的控制输入量；进而将其输入四旋翼飞行器的控制***进行飞行控制，该控制方法对导航信息参数进行优化，可以得到较为准确的导航信息参数，同时对控制算法进行优化，可以使四旋翼飞行器的姿态角和位置均能有效、快速地控制。
附图说明
图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导航以及磁力计的导航算法计算得出的天向速度误差方差值；
图13为滚转角的仿真效果图；
图14为俯仰角的仿真效果图；
图15为偏航角的仿真效果图；
图16为四旋翼飞行器在x轴，y轴和z轴的位置仿真图；
图17为四旋翼飞行器在三维空间中的轨迹图；
图18为四旋翼飞行器的飞行轨迹图。
具体实施方式
本发明所述的用于四旋翼飞行器的控制方法，包括以下步骤：
S1、利用INS惯性导航与GPS导航以及磁力计的导航算法计算得到四旋翼飞行器的导航信息参数；
S2、将步骤S1得到的导航信息参数代入如下公式得到四旋翼飞行器的控制输入量U1、U2、U3、U4；
其中，x1＝γ，x3＝θ，θ是俯仰角，γ是滚转角，是偏航角，Ωr为转速扰动，m为四旋翼飞行器的质量，g为重力加速度，α1、α2、α3、α4、α5、α6、α7、α8为大于零的正数， l表示四旋翼飞行器的机臂长度，Ixx,Iyy,Izz分别是四旋翼飞行器绕x轴，y轴，z轴的转动惯量，Jr为四旋翼飞行器的电机转子的转动惯量，z1是俯仰角的目标值和通过步骤S1计算得到实际值之差，z2是俯仰角速度目标值和通过步骤S1计算得到实际值之差；z3是滚转角的目标值和通过步骤S1计算得到实际值之差，z4是滚转角速度目标值和通过步骤S1计算得到实际值之差；z5是偏航角的目标值和通过步骤S1计算得到实际值之差，z6是偏航角速度目标值和通过步骤S1计算得到实际值之差；z7是Z轴高度的目标值和通过步骤S1计算得到实际值之差，z8是Z轴高度方向上速度目标值和通过步骤S1计算得到实际值之差；
S3、将步骤S2计算得到的四旋翼飞行器的控制输入量U1、U2、U3、U4输入四旋翼飞行器的控制***进行飞行控制。
该用于四旋翼飞行器的控制方法通过利用INS惯性导航和GPS导航组合导航的方法解决了单一的GPS导航技术易受干扰和遮挡，短时定位精度不高，输出频率有限并且输出不连续的缺点；同时也解决了单一的INS惯性导航参数累计误差越来越大，长时间定位精度发散的缺点，利用GPS导航长时间具有高的定位精度的优点来弥补INS惯性导航累计误差随时间的增加而发散的缺点；利用INS惯性导航不受外界干扰、输出的导航信息连续的特点弥补GPS易受干扰和输出频率有限的缺点，并且为了解决由惯性导航计算出的偏航角无法找到真北，以及漂移较大的情况，本***利用磁力计计算出的偏航角来校正，获得地理真北方向和稳定的偏航角，可以获得四旋翼飞行器较为准确的导航信息参数，接着利用准确的导航信息参数计算得到四旋翼飞行器的控制输入量；进而将其输入四旋翼飞行器的控制***进行飞行控制，该控制方法对导航信息参数进行优化，可以得到较为准确的导航信息参数，同时对控制算法进行优化，可以使四旋翼飞行器的姿态角和位置均能有效、快速地控制。
所述控制中心采用如下方法计算得到最终的导航信息，其具体计算方法如下所述：
在步骤S1中，所述INS惯性导航与GPS导航以及磁力计的导航算法的具体计算方法如下所述：
A、将INS惯性导航***的陀螺仪测得的四旋翼飞行器角速度参数代入四元数微分方程求解得到四元数q0，q1，q2，q3；其中为陀螺仪在四旋翼飞行器自身坐标系下的测得的三个轴的角速度信息；
所述四元数微分方程为：
B、将步骤A中求解的q0，q1，q2，q3代入下式求解得到姿态矩阵
根据下述与方向余弦的关系式
计算得出四旋翼飞行器的INS惯性导航模块姿态角θ、γ、
C、利用磁力计测得的偏航角替换步骤B计算得到的偏航角
D、将INS惯性导航***的加速度计测得的加速度参数fb和步骤B中求解得到姿态矩阵代入下述微分方程中求解得到四旋翼飞行器在INS惯性导航坐标系下的东、北、天三个方向上的速度信息vN vE vU，所述微分方程为：
其中，vn＝[vN vE vU]'为四旋翼飞行器在INS惯性导航坐标系下的东、北、天三个方向上的速度；为在INS惯性导航坐标系下地球自转角速度，为四旋翼飞行器在INS惯性导航坐标系下绕INS惯性导航坐标系各轴向的转动角速率，gn为在INS惯性导航坐标系下重力加速度；
E、将步骤D计算得出的vN vE vU分别代入下式求解得出四旋翼飞行器在INS惯性导航***中的位置信息，其中L为纬度，λ为经度，h为高度，
h＝h(0)+∫vUdt，其中L(0)表示四旋翼飞行器初始位置的纬度值，λ(0)表示四旋翼飞行器初始位置的经度值，h(0)表示四旋翼飞行器距离地球表面的初始高度。RM表示地球子午圈上的曲率半径，RN表示纬度圈上的曲率半径；
F、建立状态方程和观测方程Z(t)＝H(t)XI(t)+V(t)，
XI(t)表示INS惯性导航***在t时刻的误差状态，它是一个15维的向量，如下所示：
δvx,δvy,δvz为INS惯性导航***沿东、北、天方向上的速度误差；φx,φy,φz为四旋翼飞行器的姿态角误差；δL,δλ,δh分别代表四旋翼飞行器所在纬度、经度和高度误差；εx,εy,εz分别代表陀螺仪的随机漂移；分别为加速度计的随机漂移，其中是一个15×15的矩阵；其中FN(t)对应于δvx,δvy,δvz，φx,φy,φz，δL,δλ,δh这9个参数的INS惯性导航***矩阵，其非零元素如下：
F(2,4)＝fz F(2,6)＝-fx
F(3,4)＝-fy F(3,5)＝fx F(3,7)＝-2ωievx sinL
F(9,3)＝1
FS(t)为δvx,δvy,δvz，φx,φy,φz，δL,δλ,δh这9个参数与陀螺仪及加速度计漂移之间的变换矩阵，其维数是9×6，
FM(t)为εx,εy,εz，与陀螺仪及加速度计漂移对应的INS惯性导航***矩阵，是一个维数为6×6的对角线矩阵，表示如下：
FM(t)＝diag[-1/Tgx -1/Tgy -1/Tgz -1/Tax -1/Tay -1/Taz]；其中，Tgx表示陀螺仪x轴的误差模型的时间常数，Tgy表示陀螺仪y轴的误差模型的时间常数，Tgz表示陀螺仪z轴的误差模型的时间常数，Tax表示加速度计x轴误差模型的时间常数，Tay表示加速度计y轴误差模型的时间常数，Taz表示加速度计z轴误差模型的时间常数；
GI(t)＝diag[1 1 1......1 1]15×15；
WI(t)是一个15维的向量，如下所示：
WI(t)＝[a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15]，
a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15表示***过程噪声序列；
Z(t)为四旋翼飞行器在INS惯性导航中的位置速度信息与四旋翼飞行器在GPS导航***中的位置速度信息的差值，是一个6维向量，
Z(t)＝[δvx+Nvx δvy+Nvy δvz+Nvz (RM+h)δL+Ny (RM+h)cosLδλ+Nx δh+Nh]T，其中，Nvx表示GPS导航***在x方向上的速度误差，Nvy表示GPS导航***在y方向上的速度误差，Nvz表示GPS导航***在z方向上的速度误差，Nx表示GPS导航***在x方向上的位置误差，Ny表示GPS导航***在y方向上的位置误差，Nh表示GPS导航***在z方向上的位置误差；
其中
Vv(t)＝[Nvx Nvy Nvz]T
Vp(t)＝[Nx Ny Nz]T
G、将上述得到的连续状态方程离散化后得到Xk＝Φk,k-1Xk-1+Wk-1，其中
将上述得到的连续观测方程Z(t)＝H(t)XI(t)+V(t)离散化后得到Zk＝HkXk+Vk；
其中I是单位矩阵，F是INS惯性导航***的状态转移矩阵，Δt是离散化后INS惯性导航***的采样时间；
H、将四旋翼飞行器在INS惯性导航***中的位置速度信息与四旋翼飞行器在GPS导航***中的位置速度信息作差得到Z(t)在k时刻的观测信息z；
I、计算k时刻INS惯性导航***状态的最优估计值 其中， 为在k-1时刻INS惯性导航***状态的最优估计值，Qk-1是INS惯性导航***的噪声矩阵，其大小是由INS惯性导航元件的性能决定， Rk是***测量噪声的方差阵，其大小是由GPS接收机的性能决定；
J、将计算得到的值与四旋翼飞行器在INS惯性导航***中的位置速度信息作差得到最优的导航参数；
K、重复步骤H-J，得到连续的四旋翼飞行器的导航信息参数。
利用上述方法计算得出的导航信息，能够准确反映四旋翼飞行器的姿态和位置信息，可以实现较好的定位导航精度，图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导航以及磁力计的导航算法得出的经度、纬度、高度的误差方差均能快速收敛至比较小的数值；对位置、速度等导航信息也能实现滤平滑作用，不会产生大的跳变，***的稳定性较强。
实施例一
使用MATLAB对本发明所述用于四旋翼飞行器的控制方法进行仿真，设置滚转、俯仰和偏航角的初始值为某一非零角度，目标值均为零度，即飞机姿态保持水平，通过本发明所述用于四旋翼飞行器的控制方法进行控制，在经过一段时间后，三个姿态角均能趋近于零度，使姿态保持水平。图13为滚转角的仿真效果图；图14为俯仰角的仿真效果图；图15为偏航角的仿真效果图；
实施例二
在三维坐标中，设置四旋翼飞行器的初始位置在(0,0,0,)，目标期望的位置坐标为(1,0,1)，经过反步法的控制，使飞机逼近坐标点(1,0,1)。图16为四旋翼飞行器在x轴，y轴和z轴的位置仿真图；图17为四旋翼飞行器在三维空间中的轨迹图。
实施例三
设置4个航点(0,1,1),(1,1,1),(1,0,1),(0,0,1)，用本发明所述用于四旋翼飞行器的控制方法控制四旋翼飞行器依次经过这些航点，图18为四旋翼飞行器的飞行轨迹图。
实验表明，用本发明所述用于四旋翼飞行器的控制方法对四旋翼飞行器的姿态角和位置均能有效、快速地控制其收敛于目标值。
Claims (1)
1.用于四旋翼飞行器的控制方法，其特征在于包括以下步骤：
S1、利用INS惯性导航与GPS导航以及磁力计的导航算法计算得到四旋翼飞行器的导航信息参数；所述INS惯性导航与GPS导航以及磁力计的导航算法的具体计算方法如下所述：
A、将INS惯性导航***的陀螺仪测得的四旋翼飞行器角速度参数代入四元数微分方程求解得到四元数q0，q1，q2，q3；其中为陀螺仪在四旋翼飞行器自身坐标系下的测得的三个轴的角速度信息；
所述四元数微分方程为：
B、将步骤A中求解的q0，q1，q2，q3代入下式求解得到姿态矩阵
根据下述与方向余弦的关系式
计算得出四旋翼飞行器的INS惯性导航模块姿态角θ、γ、
C、利用磁力计测得的偏航角替换步骤B计算得到的偏航角
D、将INS惯性导航***的加速度计测得的加速度参数fb和步骤B中求解得到姿态矩阵代入下述微分方程中求解得到四旋翼飞行器在INS惯性导航坐标系下的东、北、天三个方向上的速度信息vN vE vU，所述微分方程为：
其中，vn＝[vN vE vU]'为四旋翼飞行器在INS惯性导航坐标系下的东、北、天三个方向上的速度；为在INS惯性导航坐标系下地球自转角速度，为四旋翼飞行器在INS惯性导航坐标系下绕INS惯性导航坐标系各轴向的转动角速率，gn为在INS惯性导航坐标系下重力加速度；
E、将步骤D计算得出的vN vE vU分别代入下式求解得出四旋翼飞行器在INS惯性导航***中的位置信息，其中L为纬度，λ为经度，h为高度，
h＝h(0)+∫vUdt，其中L(0)表示四旋翼飞行器初始位置的纬度值，λ(0)表示四旋翼飞行器初始位置的经度值，h(0)表示四旋翼飞行器距离地球表面的初始高度，RM表示地球子午圈上的曲率半径，RN表示纬度圈上的曲率半径；
F、建立状态方程和观测方程Z(t)＝H(t)XI(t)+V(t)，
XI(t)表示INS惯性导航***在t时刻的误差状态，它是一个15维的向量，如下所示：
XI(t)＝[δvx δvy δvz φx φy φz δL δλ δh εx εy εz ▽x ▽y ▽z]，δvx,δvy,δvz为INS惯性导航***沿东、北、天方向上的速度误差；φx,φy,φz为四旋翼飞行器的姿态角误差；δL,δλ,δh分别代表四旋翼飞行器所在纬度、经度和高度误差；εx,εy,εz分别代表陀螺仪的随机漂移；▽x,▽y,▽z分别为加速度计的随机漂移，其中是一个15×15的矩阵；其中FN(t)对应于δvx,δvy,δvz，φx,φy,φz，δL,δλ,δh这9个参数的INS惯性导航***矩阵，其非零元素如下：
F(1,5)＝-fz F(1,6)＝fy
F(2,4)＝fz F(2,6)＝-fx
F(3,4)＝-fy F(3,5)＝fx F(3,7)＝-2ωievxsinL
F(5,7)＝-ωiesinL
F(9,3)＝1
FS(t)为δvx,δvy,δvz，φx,φy,φz，δL,δλ,δh这9个参数与陀螺仪及加速度计漂移之间的变换矩阵，其维数是9×6，
FM(t)为εx,εy,εz，▽x,▽y,▽z与陀螺仪及加速度计漂移对应的INS惯性导航***矩阵，是一个维数为6×6的对角线矩阵，表示如下：
FM(t)＝diag[-1/Tgx -1/Tgy -1/Tgz -1/Tax -1/Tay -1/Taz]；其中，Tgx表示陀螺仪x轴的误差模型的时间常数，Tgy表示陀螺仪y轴的误差模型的时间常数，Tgz表示陀螺仪z轴的误差模型的时间常数，Tax表示加速度计x轴误差模型的时间常数，Tay表示加速度计y轴误差模型的时间常数，Taz表示加速度计z轴误差模型的时间常数；
GI(t)＝diag[1 1 1......1 1]15×15；
WI(t)是一个15维的向量，如下所示：
WI(t)＝[a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15]，
a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15表示***过程噪声序列；
Z(t)为四旋翼飞行器在INS惯性导航中的位置速度信息与四旋翼飞行器在GPS导航***中的位置速度信息的差值，是一个6维向量，
Z(t)＝[δvx+Nvx δvy+Nvy δvz+Nvz (RM+h)δL+Ny (RM+h)cosLδλ+Nx δh+Nh]T，其中，Nvx表示GPS导航***在x方向上的速度误差，Nvy表示GPS导航***在y方向上的速度误差，Nvz表示GPS导航***在z方向上的速度误差，Nx表示GPS导航***在x方向上的位置误差，Ny表示GPS导航***在y方向上的位置误差，Nh表示GPS导航***在z方向上的位置误差；
其中
Vv(t)＝[Nvx Nvy Nvz]T
Vp(t)＝[Nx Ny Nz]T
G、将上述得到的连续状态方程离散化后得到Xk＝Φk,k-1Xk-1+Wk-1，其中
将上述得到的连续观测方程Z(t)＝H(t)XI(t)+V(t)离散化后得到Zk＝HkXk+Vk；
其中I是单位矩阵，F是INS惯性导航***的状态转移矩阵，Δt是离散化后INS惯性导航***的采样时间；
H、将四旋翼飞行器在INS惯性导航***中的位置速度信息与四旋翼飞行器在GPS导航***中的位置速度信息作差得到Z(t)在k时刻的观测信息z；
I、计算k时刻INS惯性导航***状态的最优估计值 其中， 为在k-1时刻INS惯性导航***状态的最优估计值，Qk-1是INS惯性导航***的噪声矩阵，其大小是由INS惯性导航元件的性能决定， Rk是***测量噪声的方差阵，其大小是由GPS接收机的性能决定；
J、将计算得到的值与四旋翼飞行器在INS惯性导航***中的位置速度信息作差得到最优的导航参数；
K、重复步骤H-J，得到连续的四旋翼飞行器的导航信息参数；
S2、将步骤S1得到的导航信息参数代入如下公式得到四旋翼飞行器的控制输入量U1、U2、U3、U4；
其中，x1＝γ，x3＝θ，θ是俯仰角，γ是滚转角，是偏航角，Ωr为转速扰动，m为四旋翼飞行器的质量，g为重力加速度，α1、α2、α3、α4、α5、α6、α7、α8为大于零的正数， l表示四旋翼飞行器的机臂长度，Ixx,Iyy,Izz分别是四旋翼飞行器绕x轴，y轴，z轴的转动惯量，Jr为四旋翼飞行器的电机转子的转动惯量，z1是俯仰角的目标值和通过步骤S1计算得到实际值之差，z2是俯仰角速度目标值和通过步骤S1计算得到实际值之差；z3是滚转角的目标值和通过步骤S1计算得到实际值之差，z4是滚转角速度目标值和通过步骤S1计算得到实际值之差；z5是偏航角的目标值和通过步骤S1计算得到实际值之差，z6是偏航角速度目标值和通过步骤S1计算得到实际值之差；z7是Z轴高度的目标值和通过步骤S1计算得到实际值之差，z8是Z轴高度方向上速度目标值和通过步骤S1计算得到实际值之差；
S3、将步骤S2计算得到的四旋翼飞行器的控制输入量U1、U2、U3、U4输入四旋翼飞行器的控制***进行飞行控制。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610244268.9A CN105929836B (zh) | 2016-04-19 | 2016-04-19 | 用于四旋翼飞行器的控制方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610244268.9A CN105929836B (zh) | 2016-04-19 | 2016-04-19 | 用于四旋翼飞行器的控制方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105929836A CN105929836A (zh) | 2016-09-07 |
| CN105929836B true CN105929836B (zh) | 2019-07-02 |
Family
ID=56838329
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610244268.9A Active CN105929836B (zh) | 2016-04-19 | 2016-04-19 | 用于四旋翼飞行器的控制方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105929836B (zh) |
Families Citing this family (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106568433B (zh) * | 2016-10-28 | 2020-11-03 | 易瓦特科技股份公司 | 一种设置飞行轨迹的方法和装置 |
| CN106788676B (zh) * | 2016-12-09 | 2020-02-21 | 清华大学 | 基于调频数据广播的无人机管理方法、无人机、监控终端和管理中心 |
| CN106681336B (zh) * | 2016-12-29 | 2019-07-26 | 湖北三江航天红峰控制有限公司 | 基于容错处理的无人飞行器高度测量控制***及控制方法 |
| CN106979779A (zh) * | 2017-05-22 | 2017-07-25 | 深圳市靖洲科技有限公司 | 一种无人车实时姿态测量方法 |
| CN107272733A (zh) * | 2017-06-13 | 2017-10-20 | 深圳市伊特利网络科技有限公司 | 终端定位的无人机控制方法及*** |
| CN107992072A (zh) * | 2017-12-06 | 2018-05-04 | 陕西土豆数据科技有限公司 | 一种多旋翼飞行器的姿态控制误差获取方法 |
| CN108827302A (zh) * | 2018-04-24 | 2018-11-16 | 大连理工大学 | 基于旋翼转速测量的多旋翼飞行器导航方法 |
| CN113759706B (zh) * | 2021-11-05 | 2022-02-08 | 普宙科技(深圳)有限公司 | 基于轴角矢量的多旋翼姿态自抗扰控制方法及*** |
| CN117902043B (zh) * | 2024-03-19 | 2024-05-14 | 成都翼比特自动化设备有限公司 | 一种无人机挂载登塔防坠装置及控制方法 |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105021183A (zh) * | 2015-07-05 | 2015-11-04 | 电子科技大学 | 多旋翼飞行器gps和ins低成本组合导航*** |
-
2016
- 2016-04-19 CN CN201610244268.9A patent/CN105929836B/zh active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105021183A (zh) * | 2015-07-05 | 2015-11-04 | 电子科技大学 | 多旋翼飞行器gps和ins低成本组合导航*** |
Non-Patent Citations (2)
| Title |
|---|
| 四旋翼无人飞行器导航控制***研究;谷永晟 等;《遥测遥控》;20120531;第33卷(第3期);68-72页 |
| 四旋翼飞行器建模与PID控制器设计;江杰 等;《电子设计工程》;20131231;第21卷(第23期);147-149页 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105929836A (zh) | 2016-09-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105929836B (zh) | 用于四旋翼飞行器的控制方法 | |
| CN109813311B (zh) | 一种无人机编队协同导航方法 | |
| Redding et al. | Vision-based target localization from a fixed-wing miniature air vehicle | |
| CN106643737B (zh) | 风力干扰环境下四旋翼飞行器姿态解算方法 | |
| CN111351482B (zh) | 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 | |
| CN102809377B (zh) | 飞行器惯性/气动模型组合导航方法 | |
| CN104374388B (zh) | 一种基于偏振光传感器的航姿测定方法 | |
| CN104764467B (zh) | 空天飞行器惯性传感器误差在线自适应标定方法 | |
| CN103837151B (zh) | 一种四旋翼飞行器的气动模型辅助导航方法 | |
| CN105928515B (zh) | 一种无人机导航*** | |
| CN202939489U (zh) | 一种多旋翼自动平衡飞行控制器 | |
| CN111207745B (zh) | 一种适用于大机动无人机垂直陀螺仪的惯性测量方法 | |
| CN105865452B (zh) | 一种基于间接卡尔曼滤波的移动平台位姿估计方法 | |
| CN109084760B (zh) | 一种楼宇间导航*** | |
| CN110440830A (zh) | 动基座下车载捷联惯导***自对准方法 | |
| CN109708663B (zh) | 基于空天飞机sins辅助的星敏感器在线标定方法 | |
| CN105841698A (zh) | 一种无需调零的auv舵角精确实时测量*** | |
| CN108759814B (zh) | 一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法 | |
| CN105115505A (zh) | 一种四轴惯性稳定平台***的二阶动态干扰力矩补偿方法 | |
| CN112556724A (zh) | 动态环境下的微型飞行器低成本导航***初始粗对准方法 | |
| CN113821044A (zh) | 基于强化学习的桥梁检测无人机自主导航与稳定控制方法 | |
| CN103033197B (zh) | 一种mems陀螺零位漂移的校正方法 | |
| CN105928519B (zh) | 基于ins惯性导航与gps导航以及磁力计的导航算法 | |
| CN115727842B (zh) | 一种无人机快速对准方法、***、计算机设备及存储介质 | |
| CN105739542A (zh) | 一种基于两轴机载雷达伺服平台的兴趣点跟踪控制方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |