CN109460052B - 一种可拼组飞行器的控制方法 - Google Patents

一种可拼组飞行器的控制方法 Download PDF

Info

Publication number
CN109460052B
CN109460052B CN201910017678.3A CN201910017678A CN109460052B CN 109460052 B CN109460052 B CN 109460052B CN 201910017678 A CN201910017678 A CN 201910017678A CN 109460052 B CN109460052 B CN 109460052B
Authority
CN
China
Prior art keywords
aircraft
expected
thrust
acceleration
attitude
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
Application number
CN201910017678.3A
Other languages
English (en)
Other versions
CN109460052A (zh
Inventor
翁雷
王刚毅
李宗成
贺凯
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Weng Lei
Original Assignee
Individual
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Individual filed Critical Individual
Priority to CN201910017678.3A priority Critical patent/CN109460052B/zh
Publication of CN109460052A publication Critical patent/CN109460052A/zh
Application granted granted Critical
Publication of CN109460052B publication Critical patent/CN109460052B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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

一种可拼组飞行器的控制方法，所述方法根据飞行器传感器采集的数据，计算出飞行器的姿态信息和位置信息，由水平位置控制器根据期望水平位置与当前水平位置计算出目标欧拉角，姿态控制器根据期望欧拉角和当前欧拉角计算出所需扭矩；高度控制器根据期望高度与当前高度计算出误差推力，并根据期望加速度与当前加速度计算出悬停推力，悬停推力与误差推力求和，得到高度控制的总推力，将扭矩和总推力输入到动力系统，实现对飞行器的控制。本发明采用误差推力与悬停推力相结合的方式来控制飞行器高度，使推力输出依据飞行器实际目标位置快速响应，实现高度自适应控制。该方法解决了飞行器结构、质量、重心变化引起的控制不稳定问题，能够满足教学需要。

Description

一种可拼组飞行器的控制方法
技术领域
本发明涉及一种通用性强的飞行器控制方法，适用于各种可拼组飞行器，属于控制技术领域。
背景技术
飞行器是由多种部件组合而成的，例如旋翼飞行器包括电池、电机、控制器、接收机、超声波模块、光流模块、陀螺仪、加速度计、磁罗盘等部件。传统飞行器的结构都是固定不变的，在学校的教学过程中，为了培养学生的动手能力，帮助学生掌握飞行技术，了解飞行器的结构和工作原理，需要让学生接触多种类型、多种形态的飞行器，因此需要配备多种飞行器，这样就增加了教学成本。采用可拼组飞行器(参看图2)可以很好地解决上述问题，可拼组飞行器是将飞行器的各种功能模块通过专用的连接件，以标准连接方式灵活地拼装在一起，通过增减功能模块或改变连接方式即可拼装成不同形态的飞行器，从而大大降低学校的教学成本。但现有的飞行器控制方法都是根据不可变形或不可改变结构的飞行器量身定做的，其通用性极差，不适用于不同结构不同形态的飞行器，主要原因是飞行器的结构和形状发生变化时，其质量、重心也会发生变化，一般的控制方法无法保证飞行器的稳定性。因此，目前急需一种适用范围广的飞行器控制方法，以满足飞行器教学的需要。
发明内容
本发明的目的在于针对现有技术之弊端，提供一种可拼组飞行器的控制方法，以提高控制方法的通用性，满足飞行器教学的需要。
本发明所述问题是以下述技术方案实现的：
一种可拼组飞行器的控制方法，所述方法根据飞行器传感器采集的数据，计算出飞行器的姿态信息和位置信息，由水平位置控制器根据飞行器期望水平位置与当前水平位置的差计算出目标欧拉角，姿态控制器根据期望欧拉角和当前欧拉角的差计算出所需扭矩；高度控制器根据期望高度与当前高度的差计算出误差推力，并根据期望加速度与当前加速度的差计算出悬停推力，悬停推力与误差推力求和，得到高度控制的总推力，将扭矩和总推力输入到动力系统，实现对飞行器水平位置和高度的控制。
上述可拼组飞行器的控制方法，所述水平位置控制器的计算方法如下：
从遥控器或上位机获取的飞行器期望水平位置与飞行器当前水平位置的差，经比例环节处理后得到期望水平速度，期望水平速度与当前水平速度的差由PID控制器计算出相应的比例、积分、微分输出量并求和，得到期望水平加速度，最后由姿态矩阵将期望水平加速度转换为目标欧拉角，即期望滚转角
Figure BDA0001939635540000021
，期望俯仰角θ和期望航向角ψ。
上述可拼组飞行器的控制方法，所述高度控制器的计算方法如下：
①计算误差推力：期望高度与当前高度的差通过比例环节形成期望垂直速度，期望垂直速度与飞行器当前垂直速度的差由PID控制器计算出相应的比例、积分、微分输出量并求和，形成误差推力；
②计算悬停推力：期望加速度(可以设定为0)与当前加速度的差由PID控制器计算出相应的比例、积分、微分输出量并求和，得到输出推力增量，推力增量与前一时刻的悬停推力相加，形成悬停推力；
③计算总推力：悬停推力与误差推力求和，得到高度控制的总推力，总推力输入到动力系统，实现对飞行高度的控制。
上述可拼组飞行器的控制方法，所述姿态控制器的计算方法如下：
位置控制器输出的期望欧拉角与当前欧拉角的差通过比例环节形成期望角速度，期望角速度与当前角速度的差由PID控制器计算出相应的比例、积分、微分输出量并求和，得到期望角加速度，期望角加速度与转动惯量相乘得到所需扭矩，从而控制飞行器转动达到期望角度，进而达到期望位置。
上述可拼组飞行器的控制方法，根据飞行器传感器采集的数据计算飞行器的姿态信息和位置信息的具体步骤如下：
a.在建立机体坐标系和导航坐标系的基础上确定核心融合计算算法：
所述核心融合计算算法包括状态空间模型建立和计算流程两部分：
I.建立状态空间模型
构建离散系统状态空间方程
xk＝Φk|k-1xk-1+Buk-1+Γk|k-1wk-1
zk＝Hkxk+vk
式中xk为k时刻系统状态，xk-1表示k-1时刻系统状态，Φk|k-1为系统状态空间转移矩阵，B为控制输入的系数矩阵，uk-1为k-1时刻的系统输入量，Γk|k-1为系统噪声系数，wk-1为系统噪声，zk表示由k时刻估计出来的观测量，Hk为观测矩阵，vk为观测噪声，vk为k时刻的观测噪声；
II.计算流程
包含以下五个公式：
①状态预测：
Figure BDA0001939635540000031
式中
Figure BDA0001939635540000032
为k-1时刻对k时刻估计的系统状态，
Figure BDA0001939635540000033
表示k-1时刻的系统状态最优估计；
②协方差预测：
Figure BDA0001939635540000034
式中Pk-1|k-1为k-1时刻系统状态协方差的最优估计，Pk|k-1为k-1时刻对k时刻的状态协方差的估计，
Figure BDA0001939635540000041
为Φk|k-1的转置，Qk-1为系统状态的系统噪声；
③卡尔曼滤波增益
Figure BDA0001939635540000042
式中Kk为卡尔曼增益矩阵，Rk为观测噪声；
④状态估计校正：
Figure BDA0001939635540000043
式中
Figure BDA0001939635540000044
为k时刻系统状态的最优估计，zk为观测量，
Figure BDA0001939635540000045
为k-1时刻由系统状态对观测量的估计；
⑤协方差估计校正：
Pk|k＝Pk|k-1-KkHkPk|k-1
式中Pk|k为k时刻协方差的最优估计；
b.姿态融合解算：
①陀螺仪数据处理
读取陀螺仪测量的机体角速度ωm＝[ωmx ωmy ωmz]T，式中三个元素分别表示飞行器绕机体坐标系的x，y，z轴旋转的角速度测量值，用ωb＝[ωbx ωby ωbz]T，表示陀螺仪的偏移量，式中三个元素表示绕x，y，z轴的角速度的偏移量，设真实角速度为ω＝[ωx ωyωz]T，三个元素表示绕机体坐标x，y，z轴的真实角速度，则陀螺仪测量模型为
ω＝ωm-ωb
即，
Figure BDA0001939635540000051
②读取加速度计测量的比力数据ab＝[ax ay az]T，式中三个元素分别表示机体坐标系下对应x，y，z方向的比力；
③读取磁罗盘测量的地球磁感应强度mb＝[mx my mz]T，三个元素分别表示机体坐标系下对应x，y，z磁感应强度，用mh＝[hx hy hz]T表示地磁坐标系下三个轴向的磁感应强度，则有：
Figure BDA0001939635540000052
其中
Figure BDA0001939635540000053
表示从n坐标系到b坐标系的旋转矩阵；
④建立姿态解算模型的状态空间方程：
x＝[q ωb]T＝[q0 q1 q2 q3 ωbx ωby ωbz]T
式中q＝[q0 q1 q2 q3]T为四元数，由此求出状态空间转移矩阵：
Figure BDA0001939635540000061
式中I7×7表示7行7列的单位矩阵；
系统输入参数矩阵为：
Figure BDA0001939635540000062
系统噪声系数矩阵为：
Γk|k-1＝diag{1，1，1，1，1，1，1}
系统噪声对应为四元数噪声，角速度偏移量的系统噪声，分别设定为10-7和10-11，
Qk-1＝diag{10-7，10-7，10-7，10-7，10-11，10-11，10-11}
观测量z＝[ax ay az mx my mz]T，观测噪声为：
Rk＝diag{0.032，0.032，0.032，0.04，0.04，0.04}
观测矩阵为：
Figure BDA0001939635540000071
其中bx，bz表示地磁向量在水平面的分量和垂直方向的分量：
Figure BDA0001939635540000072
将上述姿态解算模型的状态空间方程代入计算流程给出的五个公式，得到姿态四元数，再由下式计算出姿态角度(当前欧拉角)：
Figure BDA0001939635540000073
其中
Figure BDA0001939635540000074
表示滚转角度，θ表示俯仰角度，ψ表示航向角度，
Figure BDA0001939635540000075
表示
Figure BDA0001939635540000076
的第i行第j列元素，其中i，j∈1，2，3，从而得到飞行器的姿态信息；
c.位置融合解算
①光流的速度信息转化
光流传感器读取到的速度数据记为[VX VY]T，则飞行器速度在北向和东向的投影VN，VE为：
Figure BDA0001939635540000081
②GPS测量的大地坐标转化
设导航坐标系原点的GPS坐标为[Hlat Hlon Halt]T，三个元素分别表示原点的维度、经度、海拔高度，飞行器的GPS坐标为[Plat Plon Palt]T，三个元素分别表示飞行器GPS测到的维度、经度、海拔高度，则导航坐标系下飞行器的空间位置为
Figure BDA0001939635540000082
其中
Figure BDA0001939635540000083
c＝arcos(sin(Hlat)sin(Plat)+cos(Hlat)cos(Plat)cos(Plon-Hlon))，
R为地球半径；
③加速度坐标转化
利用下式将加速度计测量的比力数据ab＝[ax ay az]T转化为导航坐标系下的加速度：
Figure BDA0001939635540000091
Figure BDA0001939635540000092
Figure BDA0001939635540000093
式中A＝[AN AE AD]T导航坐标系下的加速度，其中的三个元素分别表示北、东、地方向的加速度，AB＝[ABN ABE ABD]T为导航坐标系下的加速度偏移量，g为重力加速度；
④位置融合解算建模
首先建立北向位置状态空间模型：
系统状态选择位置PN，速度VN，加速度偏移ABN为状态量，系统输入为加速度AN，观测量为GPS测量值转换的北向位置GPS_PN，光流测量转换的速度FLOW_VN，则北向位置状态空间模型如下：
x＝[PN VN ABN]T
Figure BDA0001939635540000101
Figure BDA0001939635540000102
uk-1＝AN
Γk，k-1＝I3×3
Qk-1＝diag{2.5×10-6，5×10-4，0.05}
Figure BDA0001939635540000103
zk＝[GPS_PN FLOW_VN]
将北向位置状态空间模型代入计算流程给出的五个公式，得到飞行器北向的实际位置、速度和加速度；
用同样的方法求得飞行器在东向和竖直向下方向的实际位置、速度和加速度，从而得到飞行器的所有位置信息。
本发明采用误差推力与悬停推力相结合的方式来控制飞行器高度，并对悬停推力采用了模糊估计处理，使推力输出依据飞行器实际目标位置快速响应，实现高度自适应控制。该方法解决了因飞行器结构、质量、重心变化引起的控制不稳定问题，能够满足飞行器教学的需要。
附图说明
下面结合附图对本发明作进一步详述。
图1是机体坐标系示意图；
图2是一种模块化四旋翼飞行器的结构示意图；
图3是本可拼组飞行器控制方法的流程图；
图4是Kalmanfilter(Kalman滤波)流程框图；
图5是水平位置控制器框图；
图6是高度控制器框图；
图7是姿态控制器框图。
图中和文中各符号为：
Figure BDA0001939635540000111
为n坐标系到b坐标系的旋转矩阵，
Figure BDA0001939635540000112
为期望滚转角，θ为期望俯仰角，ψ为期望航向角，xk为k时刻系统状态，xk-1表示k-1时刻系统状态，Φk|k-1为系统状态空间转移矩阵，B为控制输入的系数矩阵，uk-1为k-1时刻的系统输入量，Гk|k-1为系统噪声系数，wk-1为系统噪声，zk表示由k时刻估计出来的观测量，Hk为观测矩阵，vk为k时刻的观测噪声；
Figure BDA0001939635540000113
为k-1时刻对k时刻估计的系统状态，
Figure BDA0001939635540000114
表示k-1时刻的系统状态最优估计；Pk-1|k-1为k-1时刻系统状态协方差的最优估计，Pk|k-1为k-1时刻对k时刻的状态协方差的估计，
Figure BDA0001939635540000115
为Φk|k-1的转置，Qk-1为系统状态的系统噪声；Kk为卡尔曼增益矩阵，Rk为观测噪声；
Figure BDA0001939635540000116
为k时刻系统状态的最优估计，
Figure BDA0001939635540000117
为k-1时刻由系统状态对观测量的估计；Pk|k为k时刻协方差的最优估计；ωm为陀螺仪测量的机体角速度；ωb为陀螺仪的偏移量，ω为真实角速度，ab为加速度计测量的比力，mb为磁罗盘测量的地球磁感应强度，mh为地磁坐标系下的磁感应强度，I7×7表示7行7列的单位矩阵；bx，bz表示地磁向量在水平面的分量和垂直方向的分量，
Figure BDA0001939635540000121
表示
Figure BDA0001939635540000122
的第i行第j列元素，VN，VE表示飞行器速度在北向和东向的投影，[Hlat Hlon Halt]T表示导航坐标系原点的GPS坐标，[Plat Plon Palt]T表示飞行器的GPS坐标，R为地球半径；A为当前加速度，AB＝[ABN ABE ABD]T为导航坐标系下的加速度偏移量，g为重力加速度；GPS_PN为GPS测量值转换的北向位置，FLOW_VN为光流测量转换的速度，q表示四元数，Ki/s为积分环节，Kds为微分环节，Kp为比例环节，Pd为期望水平位置，P为当前水平位置，Pe为误差位置，V为当前水平速度，Ad为期望水平加速度，Φ*为期望滚转角，θ*为期望俯仰角，ψ*为期望航向角，Pd*为期望高度，P*为当前高度，Pe*为误差高度，Vd*为期望垂直速度，V*为当前垂直速度，Δthrt_err为误差推力，Ad*为期望垂直加速度，Δthrt为推力增量，thrt为悬停推力，Thrt为总推力，ηd为期望欧拉角，n为当前欧拉角，r_d为期望角速度，r为当前角速度，r_a为期望角加速度，T为扭矩。
具体实施方式
术语解释：
1.机体坐标系，也称载体坐标系，以飞行器重心为原点O，飞行器重心指向前方为x轴，重心指向右方向为y轴，与x轴呈90度角，从重心指向下方为z轴，与x，y构成右手直角坐标系(参看图1)。
2.导航坐标系，也称当地位置坐标系，是一种地理坐标系，即选择地球表面一点作为原点，指向北方向为X轴，指向东方向为Y轴，竖直向下指向地球中心为Z方向，构成右手直角坐标系，即NED坐标系。
3.IMU，惯性测量单元，包括加速度计、陀螺仪、等传感器，用于测量物体的三轴姿态角、加速度、角速度的装置。
4.四元数(quaternion)，是一种简单的超复数，可以表示姿态。基本思路：一个坐标系到另一个坐标系的变换可以绕一个定义在参考坐标系中的矢量μ的单次转动来实现。四元数用符号q表示，它是一个具有四个元素的矢量，这些元素是该矢量方向和转动方向的函数
Figure BDA0001939635540000131
式中：μx、μy、μz是角矢量μ的分量，μ是其大小。
定义μ的大小和方向是使导航坐标系绕μ转动一个角度μ，就能与载体坐标系重合。
四元数也可以用其分量q0，q1，q2，q3表示成一个具有4个参数的复数形式，q0为其实部，q1，q2，q3为其虚部。
q＝q0+iq1+jq2+kq3
其中，i，j，k满足i·i＝-1，j·j＝-1，k·k＝-1，为四元数的虚部。
【注】：姿态，表示一个坐标到另一个坐标的转换，姿态的表示方式一般有欧拉角表示，四元数表示，方向余弦矩阵表示三种，方向余弦矩阵(DCM)，描述了导航坐标系和机体坐标系的旋转关系，即从DCM矩阵可以求出姿态角度，姿态角度也可以计算出DCM矩阵。
本发明从高度控制方法上改进了现有控制方法，即采用高度误差推力+悬停推力相结合的方式，对悬停推力估算采用了模糊估计的处理，进而使得推力输出依据飞行器实际目标位置快速响应，实现高度自适应控制。解决了因飞行器结构变化、质量、重心变化引起的控制不稳定的问题。
本发明包括五个步骤：1).核心融合计算算法。2)姿态信息融合计算过程。3)位置信息融合计算过程。4)位置控制器计算方法。5)姿态控制器计算方法。简单来讲飞行控制分两大部分：第一，飞行器的位置、姿态的计算过程；第二，利用位置姿态信息进行飞行器的控制输出过程，即计算哪个电机应该转多快输出多大扭矩的过程。其中1)作为底层计算方法，2)和3)都是依据1)进行展开计算。飞行器的控制分为外环路(位置控制环路)和内环路(姿态控制环路)，两者成级联关系，即期望位置作为外环路输入，经过计算输出期望姿态，再传递给内环路，经过姿态控制器计算输出控制力矩，实现飞行器对姿态角的跟踪，从而实现了飞行器对期望位置的跟踪，使飞行器跟踪飞行指令，完成人工控制或者自动控制的过程。
指令信息，即为期望信息或目标信息，例如目标位置，目标角度等，意思是控制指令要求飞行器达到某一位置或某个角度。
1)、核心融合计算算法。
融合计算算法是采用扩展卡尔曼滤波器实现多传感器融合进行状态估算的算法，分为①状态空间模型建立和②计算流程两部分进行说明：
①状态空间模型
构建离散系统状态空间方程
xk＝Φk|k-1xk-1+Buk-1+Γk|k-1wk-1 (2-1)
zk＝Hkxk+vk (2-2)
公式2-1至2-7中，下标k，k-1分别表示k时刻，k-1时刻的变量。
xk为k时刻系统状态，xk-1表示k-1时刻系统状态，Φk|k-1为系统状态空间转移矩阵，B为控制输入的系数矩阵，uk-1为k-1时刻的系统输入量，Гk|k-1为系统噪声系数，wk-1为系统噪声，zk表示由k时刻估计出来的观测量，Hk为观测矩阵，vk为观测噪声。vk为k时刻的观测噪声。
②计算流程步骤
计算流程主要有以下五个步骤(公式)。该流程可参看Kalman filter(Kalman滤波)流程框图(图4)。公式与框图表示一致
A.状态预测，根据上一时刻状态估算未来状态。
Figure BDA0001939635540000151
Figure BDA0001939635540000152
为k-1时刻对k时刻估计的系统状态。
Figure BDA0001939635540000153
表示k-1时刻的系统状态最优估计。该公式旨在通过系统输入量和当前状态对下一时刻的状态估计。【例如：已知当前时刻的位置和速度，估计一下时刻的位置，即未来位置＝当前位置+速度*时间。】
B.协方差预测，估算状态量的协方差。
Figure BDA0001939635540000154
Pk-1|k-1为k-1时刻系统状态协方差的最优估计，Pk|k-1为k-1时刻对k时刻的状态协方差的估计。
Figure BDA0001939635540000155
为Φk|k-1的转置，Qk-1为系统状态的系统噪声。
C.卡尔曼滤波增益，根据协方差计算kalmanfilter增益。
Figure BDA0001939635540000156
Kk为卡尔曼增益矩阵，Rk为观测噪声。该公式是通过协方差计算卡尔曼增益。
D.状态估计校正，利用增益修正估计值。
Figure BDA0001939635540000161
Figure BDA0001939635540000162
为k时刻系统状态的最优估计，zk为观测量，
Figure BDA0001939635540000163
为k-1时刻由系统状态对观测量的估计。该公式是用于将观测量和观测量估计值进行比较，并通过卡尔曼增益Kk补偿给系统状态，实现系统状态的最优估计。
E.协方差估计校正，利用增益修正状态协方差。
Pk|k＝Pk|k-1-KkHkPk|k-1 (2-7)
Pk|k为k时刻协方差的最优估计，该公式是将观测量的噪声补偿给系统噪声，实现对系统噪声的最优估计。
2)、姿态融合解算
姿态解算是cpu利用采集的传感器数据(包括陀螺仪、加速度计、磁罗盘)经过融合，计算出动态响应高、噪声小、精度高的姿态信息的过程，该过程是将传感器数据进行预处理，融合计算，最终输出姿态信息(包括四元数、方向余弦矩阵和、欧拉角)。下面分4个要点进行说明，①陀螺仪数据处理，②加速度计数据处理，③磁罗盘数据处理，④姿态解算模型建立与计算步骤。
①陀螺仪测量值为机体角速度，即ωm＝[ωmx ωmy ωmz]T，式中三个元素分别表示飞行器绕机体坐标系的x，y，z轴旋转的角速度测量值。
ωb＝[ωbx ωby ωbz]T，表示陀螺仪的偏移量，三个元素表示绕x，y，z轴的角速度的偏移量。设真实角速度为ω＝[ωx ωy ωz]T，三个元素表示绕机体坐标x，y，z轴的真实角速度。则陀螺仪测量模型为
ω＝ωm-ωb (3-1)
即，
Figure BDA0001939635540000171
②加速度计测量数据为比力，记ab＝[ax ay az]T，三个元素分别表示机体坐标系下对应x，y，z方向的比力。【比力，物体所受的非引力的合力产生的加速度】。
③磁罗盘测量地球磁感应强度，记mb＝[mx my mz]T，三个元素分别表示机体坐标系下对应x，y，z磁感应强度。mh＝[hx hy hz]T地磁坐标系下三个轴向的磁感应强度
Figure BDA0001939635540000172
其中
Figure BDA0001939635540000181
表示从n坐标系到b坐标系的旋转矩阵，
Figure BDA0001939635540000182
表示
Figure BDA0001939635540000183
的第i行第j列元素，
其中i，j∈1，2，3.
④建立姿态解算模型的状态空间方程：
x＝[q ωb]T＝[q0 q1 q2 q3 ωbx ωby ωbz]T (3-5)
系统状态是由四元数和角速度偏移量构成的7维状态量
Figure BDA0001939635540000184
根据构建模型，求出状态空间转移矩阵如公式3-6。I7×7表示7行7列的单位矩阵
Figure BDA0001939635540000185
角速度数据作为系统输入，故系统输入参数矩阵如公式3-7
Γk|k-1＝diag{1，1，1，1，1，1，1} (3-8)
系统噪声系数矩阵为7X7的单位矩阵
Qk-1＝diag{10-7，10-7，10-7，10-7，10-11，10-11，10-11} (3-9)
系统噪声对应为四元数噪声，角速度偏移量的系统噪声，分别设定为10-7和10-11.
Rk＝diag{0.032，0.032，0.032，0.04，0.04，0.04} (3-10)
观测量z＝[ax ay az mx my mz]T，即加速度数据和磁罗盘数据。观测噪声见公式3-10.
Figure BDA0001939635540000191
观测矩阵见公式3-11.其中bx，bz表示地磁向量在水平面的分量和垂直方向的分量，见公式3-12.
Figure BDA0001939635540000192
至此姿态融合的状态方程建立完毕，将3-5至3-11带入卡尔曼计算流程公式(2-3至2-7)即可完成姿态解算。姿态解算结果输出姿态四元数，由3-4和3-13可以计算得到姿态角度。其中
Figure BDA0001939635540000193
表示滚转角度，θ表示俯仰角度，ψ表示航向角度。
Figure BDA0001939635540000201
3)、位置融合解算
位置融合计算是将GPS信息、光流速度信息经过与加速度的融合计算得出精度较高、噪声较小、响应更快的实际位置、速度、加速度的过程。位置信息的融合计算需要在导航坐标系下进行，所以相关变量都要转换到导航坐标系下才能进行。由于位置融合的三个维度计算法一样，我们以北向作为叙述对象展开分析。下面分4个方面进行说明：①光流的速度信息转化。②GPS测量的大地坐标转化。③加速度坐标转化。④位置融合解算建模过程。
①光流传感器读取到速度数据[VX VY]T。光流测量的速度是平面坐标，即X方向指向飞行器前方，与水平面平行，Y轴为指向右方，与水平面平行。所以，要再次旋转到导航坐标系下对应的北向和东向速度，依据公式4-4.式中VN，VE表示飞行器速度在北向和东向的投影，ψ表示航向角度，由3-13计算而来。
Figure BDA0001939635540000202
②GPS可以测到大地坐标系下的维度、经度、海拨高度等信息。通过转换也可以得到导航坐标系下的位置，即先选取一点作为原点，从该点向北、向东、竖直向下分别为N，E，D的位置数据[PN PE PD]T。构成右手系空间直角坐标系。X，Y，Z分别由N，E，D表示。设选取点GPS坐标为[Hlat Hlon Halt]T，三个元素分别表示选定点维度、经度、海拔高度，观测点的GPS坐标为[Plat Plon Palt]T，三个元素分别表示测量点维度、经度、海拔高度，则导航坐标系下的空间位置为
Figure BDA0001939635540000211
其中c＝arcos(sin(Hlat)sin(Plat)+cos(Hlat)cos(Plat)cos(Plon-Hlon)) (4-3)
Figure BDA0001939635540000212
R为地球半径。k由4-3、4-4公式联合推倒得出。
③机体坐标系下的加速度比力通过姿态矩阵可以旋转到导航坐标系下，参考公式4-5，其中A表示导航坐标系下的加速度，[AN AE AD]分别表示北东地方向的加速度。AB为导航坐标系下的加速度偏移量。g为重力加速度
Figure BDA0001939635540000213
Figure BDA0001939635540000221
④选取北向方向为例说明系统状态空间模型：
系统状态选择位置PN，速度VN，加速度偏移ABN为状态量。系统输入为加速度AN，观测量为GPS测量值转换的北向位置GPS_PN，光流测量转换的速度FLOW_VN。由此可以得出状态转移矩阵Φk|k-1，输入参数矩阵B。状态量噪声Qk-1如公式4-12.系统噪声矩阵为3×3维单位矩阵。
x＝[PN VN ABN]T (4-7)
Figure BDA0001939635540000222
Figure BDA0001939635540000223
uk-1＝AN (4-10)
Γk，k-1＝I3×3 (4-11)
Qk-1＝diag{2.5×10-6，5×10-4，0.05} (4-12)
Figure BDA0001939635540000231
Zk＝[GPS-PN FLOW_VN] (4-14)
综上，北向位置状态空间模型建立完毕，带入卡尔曼计算步骤流程(2-3至2-7)则可以完成位置融合解算。
同理，可以求得东向，竖直向下方向的位置信息。至此位置融合过程分析完毕。
4)、位置控制器
位置控制器包括水平位置控制器和高度控制器
①水平位置控制器
图5是水平位置控制器框图，图中各参数标识的意义参看表1。
表1.水平位置控制器各参数标识
参数标识 说明 参数标识 说明
Pd 期望水平位置 Ki/s 积分环节
P 当前水平位置 Kds 微分环节
Pe 误差位置 Ad 期望水平加速度
KP 比例环节 φ* 期望滚转角
Vd 期望水平速度 θ* 期望俯仰角
V 当前水平速度 ψ* 期望航向角
Kp 比例环节
获取目标位置信息，即从遥控器或上位机获取飞行器的目标位置(期望水平位置)。与飞行器当前水平位置比较，做比例控制输出作为期望水平速度，再与当前水平速度比较，使用PID控制器计算出相应的比例、积分、微分输出量，求和得出期望水平加速度。根据姿态矩阵(即n坐标系到b坐标系的旋转矩阵
Figure BDA0001939635540000241
)转换为目标欧拉角(或称期望欧拉角)，即期望滚转角
Figure BDA0001939635540000242
期望俯仰角θ*，期望航向角ψ*。
②高度控制器
本发明实现了飞行器高度自适应控制算法。图6是高度控制器框图，图中各参数标识的意义参看表2。
表2.高度控制器各参数标识
参数标识 说明 参数标识 说明
Pd* 期望高度 Kds 微分环节
P* 当前高度 Δthrt_err 误差推力
Pe* 误差高度 Ad* 期望垂直加速度
KP 比例环节 A 当前加速度
Vd* 期望垂直速度 Δthrt 推力增量
V* 当前垂直速度 T(k-1) 悬停推力延时一个周期
Kp 比例环节 thrt 悬停推力
Ki/s 积分环节 Thrt 总推力
高度控制推力包括误差推力(位置误差控制输出的推力)和悬停推力(悬停时克服重力输出的推力)
其中误差推力的计算过程为：期望高度Pd*与当前高度P*做差，通过比例环节输出期望垂直速度，并获取飞行器当前垂直速度V*，得出速度误差，并通过PID控制器的比例、积分、微分环节，输出误差推力，即位置误差产生的推力；
悬停推力的计算过程为：获取期望加速度，或者设定为0，并获取当前加速度，做差得到加速度误差，通过PID控制器的比例、积分、微分环节，输出推力增量，并叠加前一时刻的悬停推力输出，形成悬停推力；
悬停推力与误差推力求和，最后输出总推力给动力系统，通过电子调速器控制电机旋转，由螺旋桨产生相应的推力，从而达到控制高度的目的。
5)、姿态控制器
图7是姿态控制器框图。图中各参数标识的意义参看表3。
表3.姿态控制器各参数标识
参数标识 说明 参数标识 说明
nd 期望欧拉角，即φ*，θ*，ψ* Kp 比例环节
n 当前欧拉角，即φ，θ，ψ Ki/s 积分环节
ηe 误差欧拉角 Kds 微分环节
KP 比例环节 r_a 期望角加速度
r_d 期望角速度 T 扭矩
r 当前角速度
从位置控制器输出获取期望欧拉角，与当前欧拉角比较，做比例控制输出，即为期望角速度。期望角速度与当前角速度做差，使用PID控制器运算输出即为期望角加速度，与转动惯量相乘便可得到扭矩输出，从而控制飞机转动达到期望角度，进而达到期望位置。

Claims (4)

1.一种可拼组飞行器的控制方法，其特征是，所述方法根据飞行器传感器采集的数据，计算出飞行器的姿态信息和位置信息，由水平位置控制器根据飞行器期望水平位置与当前水平位置的差计算出目标欧拉角，姿态控制器根据期望欧拉角和当前欧拉角的差计算出所需扭矩；高度控制器根据期望高度与当前高度的差计算出误差推力，并根据期望加速度与当前加速度的差计算出悬停推力，悬停推力与误差推力求和，得到高度控制的总推力，将扭矩和总推力输入到动力系统，实现对飞行器水平位置和高度的控制；
所述高度控制器的计算方法如下：
①计算误差推力：期望高度与当前高度的差通过比例环节形成期望垂直速度，期望垂直速度与飞行器当前垂直速度的差由PID控制器计算出相应的比例、积分、微分输出量并求和，形成误差推力；
②计算悬停推力：期望加速度与当前加速度的差由PID控制器计算出相应的比例、积分、微分输出量并求和，得到输出推力增量，推力增量与前一时刻的悬停推力相加，形成悬停推力；期望加速度可以设定为0；
③计算总推力：悬停推力与误差推力求和，得到高度控制的总推力，总推力输入到动力系统，实现对飞行高度的控制。
2.根据权利要求1所述的一种可拼组飞行器的控制方法，其特征是，所述水平位置控制器的计算方法如下：
从遥控器或上位机获取的飞行器期望水平位置与飞行器当前水平位置的差，经比例环节处理后得到期望水平速度，期望水平速度与当前水平速度的差由PID控制器计算出相应的比例、积分、微分输出量并求和，得到期望水平加速度，最后由姿态矩阵将期望水平加速度转换为目标欧拉角，即期望滚转角φ，期望俯仰角θ和期望航向角ψ。
3.根据权利要求1所述的一种可拼组飞行器的控制方法，其特征是，所述姿态控制器的计算方法如下：
位置控制器输出的期望欧拉角与当前欧拉角的差通过比例环节形成期望角速度，期望角速度与当前角速度的差由PID控制器计算出相应的比例、积分、微分输出量并求和，得到期望角加速度，期望角加速度与转动惯量相乘得到所需扭矩，从而控制飞行器转动达到期望角度，进而达到期望位置。
4.根据权利要求3所述的一种可拼组飞行器的控制方法，其特征是，根据飞行器传感器采集的数据计算飞行器的姿态信息和位置信息的具体步骤如下：
a.在建立机体坐标系和导航坐标系的基础上确定核心融合计算算法：
所述核心融合计算算法包括状态空间模型建立和计算流程两部分：
Ⅰ.建立状态空间模型
构建离散系统状态空间方程
xk＝Φk|k-1xk-1+Buk-1+Γk|k-1wk-1
zk＝Hkxk+vk
式中xk为k时刻系统状态，xk-1表示k-1时刻系统状态，Φk|k-1为系统状态空间转移矩阵，B为控制输入的系数矩阵，uk-1为k-1时刻的系统输入量，Γk|k-1为系统噪声系数，wk-1为系统噪声，zk表示由k时刻估计出来的观测量，Hk为观测矩阵，vk为观测噪声，vk为k时刻的观测噪声；
Ⅱ.计算流程
包以下五个公式：
①状态预测：
Figure FDA0003288974470000021
式中
Figure FDA0003288974470000022
为k-1时刻对k时刻估计的系统状态，
Figure FDA0003288974470000023
表示k-1时刻的系统状态最优估计；
②协方差预测：
Figure FDA0003288974470000024
式中Pk-1|k-1为k-1时刻系统状态协方差的最优估计，Pk|k-1为k-1时刻对k时刻的状态协方差的估计，
Figure FDA0003288974470000025
为Φk|k-1的转置，Qk-1为系统状态的系统噪声；
③卡尔曼滤波增益
Figure FDA0003288974470000026
式中Kk为卡尔曼增益矩阵，Rk为观测噪声；
④状态估计校正：
Figure FDA0003288974470000031
式中
Figure FDA0003288974470000032
为k时刻系统状态的最优估计，zk为观测量，
Figure FDA0003288974470000033
为k-1时刻由系统状态对观测量的估计；
⑤协方差估计校正：
Pk|k＝Pk|k-1-KkHkPk|k-1
式中Pk|k为k时刻协方差的最优估计；
b.姿态融合解算：
①陀螺仪数据处理
读取陀螺仪测量的机体角速度ωm＝[ωmx ωmy ωmz]T，式中三个元素分别表示飞行器绕机体坐标系的x，y，z轴旋转的角速度测量值，用ωb＝[ωbx ωby ωbz]T,表示陀螺仪的偏移量，式中三个元素表示绕x，y，z轴的角速度的偏移量，设真实角速度为ω＝[ωx ωy ωz]T，三个元素表示绕机体坐标x，y，z轴的真实角速度，则陀螺仪测量模型为
ω＝ωm-ωb
即，
Figure FDA0003288974470000034
②读取加速度计测量的比力数据ab＝[ax ay az]T，式中三个元素分别表示机体坐标系下对应x，y，z方向的比力；
③读取磁罗盘测量的地球磁感应强度mb＝[mx my mz]T,三个元素分别表示机体坐标系下对应x，y，z磁感应强度，用mh＝[hx hy hz]T表示地磁坐标系下三个轴向的磁感应强度，则有：
Figure FDA0003288974470000035
其中
Figure FDA0003288974470000041
表示从n坐标系到b坐标系的旋转矩阵；
④建立姿态解算模型的状态空间方程：
x＝[q ωb]T＝[q0 q1 q2 q3 ωbx ωby ωbz]T
式中q＝[q0 q1 q2 q3]T为四元数，由此求出状态空间转移矩阵：
Figure FDA0003288974470000042
式中I7×7表示7行7列的单位矩阵；
系统输入参数矩阵为：
Figure FDA0003288974470000043
系统噪声系数矩阵为：
Γk|k-1＝diag{1,1,1,1,1,1,1}
系统噪声对应为四元数噪声，角速度偏移量的系统噪声，分别设定为10-7和10-11，
Qk-1＝diag{10-7,10-7,10-7,10-7,10-11,10-11,10-11}
观测量z＝[ax ay azmx my mz]T，观测噪声为：
Rk＝diag{0.032，0.032，0.032，0.04，0.04，0.04}
观测矩阵为：
Figure FDA0003288974470000051
其中bx，bz表示地磁向量在水平面的分量和垂直方向的分量：
Figure FDA0003288974470000052
将上述姿态解算模型的状态空间方程代入计算流程给出的五个公式，得到姿态四元数，再由下式计算出姿态角度：
Figure FDA0003288974470000053
其中
Figure FDA0003288974470000054
表示滚转角度，θ表示俯仰角度，ψ表示航向角度，
Figure FDA0003288974470000055
表示
Figure FDA0003288974470000056
的第i行第j列元素，其中i，j∈1,2,3，从而得到飞行器的姿态信息；姿态角度为当前欧拉角；
c.位置融合解算
①光流的速度信息转化
光流传感器读取到的速度数据记为[VX VY]T，则飞行器速度在北向和东向的投影VN，VE为：
Figure FDA0003288974470000057
②GPS测量的大地坐标转化
设导航坐标系原点的GPS坐标为[Hlat Hlon Halt]T，三个元素分别表示原点的维度、经度、海拔高度，飞行器的GPS坐标为[Plat Plon Palt]T，三个元素分别表示飞行器GPS测到的维度、经度、海拔高度，则导航坐标系下飞行器的空间位置为
Figure FDA0003288974470000061
其中
Figure FDA0003288974470000062
c＝arcos(sin(Hlat)sin(Plat)+cos(Hlat)cos(Plat)cos(Plon-Hlon))，R为地球半径；
③加速度坐标转化
利用下式将加速度计测量的比力数据ab＝[ax ay az]T转化为导航坐标系下的加速度：
Figure FDA0003288974470000063
Figure FDA0003288974470000064
式中A＝[AN AE AD]T为导航坐标系下的加速度，其中的三个元素分别表示北、东、地方向的加速度，AB＝[ABN ABE ABD]T为导航坐标系下的加速度偏移量，g为重力加速度；
④位置融合解算建模
首先建立北向位置状态空间模型：
系统状态选择位置PN，速度VN，加速度偏移ABN为状态量，系统输入为加速度AN，观测量为GPS测量值转换的北向位置GPS_PN，光流测量转换的速度FLOW_VN，则北向位置状态空间模型如下：
x＝[PN VN ABN]T
Figure FDA0003288974470000071
Figure FDA0003288974470000072
uk-1＝AN
Γk,k-1＝I3×3
Qk-1＝diag{2.5×10-6，5×10-4，0.05}
Figure FDA0003288974470000073
Zk＝[GPS_PN FLOW_VN]
将北向位置状态空间模型代入计算流程给出的五个公式，得到飞行器北向的实际位置、速度和加速度；
用同样的方法求得飞行器在东向和竖直向下方向的实际位置、速度和加速度，从而得到飞行器的所有位置信息。
CN201910017678.3A 2019-01-09 2019-01-09 一种可拼组飞行器的控制方法 Active CN109460052B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910017678.3A CN109460052B (zh) 2019-01-09 2019-01-09 一种可拼组飞行器的控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910017678.3A CN109460052B (zh) 2019-01-09 2019-01-09 一种可拼组飞行器的控制方法

Publications (2)

Publication Number Publication Date
CN109460052A CN109460052A (zh) 2019-03-12
CN109460052B true CN109460052B (zh) 2022-08-05

Family

ID=65616309

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910017678.3A Active CN109460052B (zh) 2019-01-09 2019-01-09 一种可拼组飞行器的控制方法

Country Status (1)

Country Link
CN (1) CN109460052B (zh)

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110044321B (zh) * 2019-03-22 2021-01-29 北京理工大学 利用地磁信息和角速率陀螺解算飞行器姿态的方法
CN110017809B (zh) * 2019-04-03 2021-08-27 北京理工大学 利用地磁信息和光流传感器解算飞行器姿态的方法
CN110174892B (zh) * 2019-04-08 2022-07-22 阿波罗智能技术(北京)有限公司 车辆朝向的处理方法、装置、设备及计算机可读存储介质
EP3862835B1 (en) * 2020-02-10 2023-10-25 Volocopter GmbH Method and system for monitoring a condition of a vtol-aircraft
CN111309049B (zh) * 2020-03-02 2023-07-04 中国人民解放军海军航空大学 一种微型飞行器的虚拟目标五米小幅高精度导引方法
CN112462794B (zh) * 2020-11-09 2024-03-26 航天科工火箭技术有限公司 一种演示验证火箭悬停制导方法及系统
CN114524071B (zh) * 2020-11-23 2022-12-20 中国科学院沈阳自动化研究所 一种rov悬停定位控制方法
CN112650263B (zh) * 2020-12-08 2022-03-15 电子科技大学 一种组合式无人机的控制方法
CN113155129B (zh) * 2021-04-02 2022-07-01 北京大学 一种基于扩展卡尔曼滤波的云台姿态估计方法
CN114415715B (zh) * 2021-12-17 2024-02-27 北京天玛智控科技股份有限公司 多无人机集成系统的控制方法及装置
CN113961020B (zh) * 2021-12-22 2022-04-08 普宙科技(深圳)有限公司 一种无人机三维空间运动控制方法及系统
CN117148854B (zh) * 2023-10-31 2024-02-09 深圳市苇渡智能科技有限公司 基于动力调节的电动水翼载具俯仰姿态控制方法及系统

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE1481548C3 (de) * 1966-06-29 1975-04-17 Bodenseewerk Geraetetechnik Gmbh, 7770 Ueberlingen Vortriebsregler
KR101283543B1 (ko) * 2012-11-19 2013-07-15 이용승 무인비행체의 자세 안정화 방법
CN104536453B (zh) * 2014-11-28 2017-08-04 深圳一电航空技术有限公司 飞行器的控制方法及装置
CN205068169U (zh) * 2015-09-25 2016-03-02 南京航空航天大学 基于双余度姿态传感器的六旋翼无人机
CN106681344B (zh) * 2016-12-26 2019-08-27 湖南纳雷科技有限公司 一种用于飞行器的高度控制方法及控制系统
CN106950981B (zh) * 2017-04-25 2020-02-11 深圳大学 一种无人机高度控制方法及系统
CN107329484B (zh) * 2017-05-11 2020-06-26 西安天问智能科技有限公司 油动变距多旋翼飞行器控制系统及控制方法
CN107505949A (zh) * 2017-09-30 2017-12-22 上海拓攻机器人有限公司 一种无人机飞行控制方法以及系统
CN107992080B (zh) * 2017-12-25 2020-12-15 成都纵横自动化技术股份有限公司 控制分配方法、装置及多旋翼飞行器

Also Published As

Publication number Publication date
CN109460052A (zh) 2019-03-12

Similar Documents

Publication Publication Date Title
CN109460052B (zh) 一种可拼组飞行器的控制方法
Shen et al. Observability analysis and adaptive information fusion for integrated navigation of unmanned ground vehicles
CN113029199B (zh) 一种激光陀螺惯导系统的系统级温度误差补偿方法
Ahn et al. Fast alignment using rotation vector and adaptive Kalman filter
CN104374388B (zh) 一种基于偏振光传感器的航姿测定方法
Wu et al. A novel approach for attitude estimation based on MEMS inertial sensors using nonlinear complementary filters
CN105806363B (zh) 基于srqkf的sins/dvl水下大失准角对准方法
CN106153073B (zh) 一种全姿态捷联惯导系统的非线性初始对准方法
CN101413800A (zh) 导航/稳瞄一体化系统的导航、稳瞄方法
CN110231029B (zh) 一种水下机器人多传感器融合数据处理方法
CN106052686A (zh) 基于dsptms320f28335的全自主捷联惯性导航系统
Fresk et al. A generalized reduced-complexity inertial navigation system for unmanned aerial vehicles
CN114543794A (zh) 一种视觉惯性里程计与间断性rtk融合的绝对定位方法
Lin et al. Robust observer-based visual servo control for quadrotors tracking unknown moving targets
Al-Jlailaty et al. Efficient attitude estimators: A tutorial and survey
Blachuta et al. Attitude and heading reference system based on 3D complementary filter
Fernando et al. Toward developing an indoor localization system for mavs using two or three rf range anchors: An observability based approach
Kopecki et al. A proposal of AHRS yaw angle correction with the use of roll angle
CN114543793B (zh) 车辆导航系统的多传感器融合定位方法
CN110260862B (zh) 一种基于捷联惯导系统的旋翼直升机载导航装置
Dadkhah et al. A model-aided ahrs for micro aerial vehicle application
Hidalgo et al. ESTEC Testbed Capabilities for the Performance Characterization of Planetary Rover Localization Sensors-First Results on IMU Investigations
CN114594675B (zh) 一种改进型pid的四旋翼飞行器控制系统及方法
Mukhina et al. Computer modeling of accuracy characteristics of strapdown inertial navigation system
Cuenca Geomagnetic Aided Dead-Reckoning Navigation

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
TA01 Transfer of patent application right
TA01 Transfer of patent application right

Effective date of registration: 20220614

Address after: 042100 No. 18, Si Village, Zaoling Township, Xiangning County, Linfen City, Shanxi Province

Applicant after: Weng Lei

Address before: 1719-41, floor 17, No. 66, North Fourth Ring West Road, Haidian District, Beijing 100080

Applicant before: BEIJING MINGXUESI ROBOT TECHNOLOGY Co.,Ltd.

GR01 Patent grant
GR01 Patent grant