小型四旋翼无人机姿态测量仿真研究
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
· －
（ 6） Δθ x 0
第 32 卷
第 02 期
ቤተ መጻሕፍቲ ባይዱ
计
算
机
仿
真
2015 年 02 月
文章编号： 1006 － 9348 （ 2015 ） 02 － 0067 － 07
小型四旋翼无人机姿态测量仿真研究
袁 楚仕彬， 亮
（ 新疆大学机械工程学院， 新疆 乌鲁木齐 830000 ） 摘要： 研究无人机姿态流通量优化问题， 由于低成本小型四旋翼无人机中所使用的姿态测量传感器存在精度低、 噪声大的缺 点， 并且易受温度等环境的影响， 不能准确的测量出无人机的姿态， 特别是在无人机加速飞行过程中， 姿态误差会很快累积 扩大， 不能长时间加速飞行 。为提高无人机姿态测量精度， 提出设计由陀螺仪、 加速度计和磁强计组成的姿态测量系统， 通 过计算加速度计信任度， 并利用卡尔曼滤波的方法实现对误差四元数的估计， 从而得到无人机的姿态角度。实验结果表明， 该卡尔曼滤波能够在低动态的飞行环境中准确测量出无人机姿态角度， 并能克服以往无人机不能长时间加速飞行的缺点， 证明卡尔曼滤波被成功应用于四旋翼无人机姿态测量优化提供了科学依据 。 关键词： 四旋翼； 误差四元数； 姿态估计； 卡尔曼滤波 中图分类号： V271. 4 文献标识码： B
q0 + q1 － q2 － q3 2 （ q1 q2 － q0 q3 ） 2 （ q1 q3 + q0 q2 ） 2 2 2 q2 2 （ q2 q3 － q0 q1 ） （ 2 ） Ｒ = 2 （ q1 q2 + q0 q3 ） 0 － q1 + q2 － q3 2 2 2 2 （ q1 q3 － q0 q2 ） 2 （ q2 q3 + q0 q1 ） q2 0 － q1 － q2 + q3
械陀螺仪与其它姿态传感器组合使用才能测量出长期 、 稳 定、 可靠的姿态角度。 目前应用于多传感器信息融合的方法很多， 各有特点，
— 67 —
还能提高测量精度， 合适的融合方法不仅能节省系统资源， 13］ 文献［ 采用加权平均法实现信号融合， 总体思想是保证总 方差最小的条件下根据各传感器的测量值， 通过自适应的方 式寻找与之对应的最优加权因子， 使融合后的测量值最优， 14］采用互补滤波的 适用于动态环境， 但其精度较低； 文献［ 方法进行飞行器姿态解算， 该方法计算量较小， 时时性高， 但 5］ 其精度低， 姿态漂移严重， 不能应用于动态环境下； 文献［ 采用判断无人机的运动状态来决定是否使用卡尔曼滤波， 优 点是可以实现无人机短时间低动态加速运动， 其缺点是在加 速飞行的过程中陀螺仪漂移过大， 不能长时间加速飞行。 为 了能够实现四旋翼无人机在低动态环境下稳定飞行， 本文采 用计算加速度计信任度的方法， 并利用卡尔曼滤波进行数据 融合， 实现对误差四元数的估计， 从而计算出四旋翼无人机 的姿态。实验结果表明该卡尔曼滤波方法能够在低动态的 环境中解算出准确的姿态角度， 抑制不良噪声。
图1 四旋翼无人机系统结构图
q2 = cos（
ψ θ β ψ θ β ） sin（ ） cos（ ） + sin（ ） cos（ ） sin（ ） 2 2 2 2 2 2 （ 4 c）
3
四旋翼无人机姿态解算的原理
四旋翼无人机姿态解算的实质是通过求解载体坐标系 q3 = sin（
ψ θ β ψ θ β ） cos（ ） cos（ ） － cos（ ） sin（ ） sin（ ） 2 2 2 2 2 2 （ 4 d）
3． 2
载体坐标系 载体坐标系 OX b Y b Z b ， 其原点为载体的转动中心， 也就
是说载体坐标系和参考坐标系的原点重合 。 并且规定绕 Z b 轴转动的角度为 ψ 角， 即航向角； 绕 Y b 轴转动的角度为 θ 即为横滚角。 角， 即俯仰角； 绕 X b 旋转 β 角， 3． 3 基于四元数运动方程的姿态解算方法 四元数姿态表达是一个四参数的表达式， 它基于的思路 是一个坐标系到另一个坐标系的变换可以通过绕一个定义 在参考坐标系的矢量的单次转动来实现 。 四元数用符号 q 表示， 它是一个具有 4 个元素的矢量， 这些元素是该矢量方 向和转动大小的函数。 q = [ q0 = cos q1 θ 2 q2 q3 ] θ 2 － r y sin θ 2 － r z sin θ 2
{
Δ θ0 cos ·I + 2
sin
Δ θ0 q（ 0 ） 2 ΔΘ Δ θ0
}
（ 5）
— 68 —
0 Δθ x ΔΘ = Ω b dt = t1 Δθ y Δθ z
－ Δθ x 0 － Δθ z Δθ y
－ Δθ y Δθ z 0 － Δθ x
∫
t2
－ Δθ z － Δθ y
( q2（ +q qq －+ qq q－ ）q )
2 3 2 3 2 0 0 1 2 1 2 2
由欧拉法和四元数法可知， 四元数与欧拉角的关系如下 q0 = cos（ ψ θ β ψ θ β ） cos（ ） cos（ ） + sin（ ） sin（ ） sin（ ） 2 2 2 2 2 2 （ 4 a） q1 = cos（ ψ θ β ψ θ β ） cos（ ） sin（ ） － sin（ ） sin（ ） cos（ ） 2 2 2 2 2 2 （ 4 b）
相对参考坐标系的姿态矩阵来求解姿态角度 。 3． 1 参考坐标系 参考坐标系 OX e Y e Z e ， 其坐标原点为运动载体的转动中 Xe 、 Y e 轴在当地水平面内， X e 轴指向北方， Y e 轴指向西 心， Xe 、 Ye 、 Z e 三轴满足右手 Z e 轴沿载体的垂线方向指向天， 方， 、 定则。 其中 q（ t） = 3． 4 四元数更新算法 本文四元数更新采用三阶比卡逼近算法 。
基金项目： 国家自然科学基金（ 61262059 ） ； 新疆优秀青年科技创新人 才培养项目（ 2013721016 ） ； 新疆大学博士启动基金 收稿日期： 2014 － 04 － 09 修回日期： 2014 － 05 － 23
1
引言
无人机姿态作为无人机控制的基础和核心， 直接决定了
无人机速度与位置的导航精度， 对无人机自主飞行影响非常 大， 因而备受重视。无人机的姿态测量系统的功能是通过读 取传感器的测量值， 经过相应的姿态解算方法， 从而测量出 无人机的航向角、 俯仰角和横滚角， 这三个角度是实现无人
的旋转， 则合成后的转动四元数 q 可以表达成： q = q2 q1 4． 1 卡尔曼滤波状态方程 假设无人机的姿态由陀螺仪估计姿态和姿态误差值所 组成， 并将无人机实时姿态定义为相对参考坐标系进行了两 次转动： 修正姿态， 相对参考坐标系转动一个误差四元数估 计值； 更新姿态， 转动一个姿态四元数估计值 。 利用四元数 转动合成原理可知， 这一转动过程可以描绘成： Qb = 珚 Q Qe 态误差四元数。表示四元数叉乘运算。 对式（ 13 ） 式两边微分得
与方向余弦法类比， 可以得到姿态矩阵与四元数的关 系， 并且得到用四元数表示的欧拉角， 表达式为 ψ = arctan
2
2
2
2
( q2（ +q qq －+ qq q－ ）q )
2 1 1 2 2 0 0 3 2 2 2 3
（ 3 a） （ 3 b） （ 3 c）
θ = － arcsin（ 2 （ q1 q3 － q0 q2 ） ） β = arctan
Study on Measurement and Simulation of the Low Cost and Compact four Ｒotor UAV
CHU Shi － bin， YUAN Liang
（ Mechanical Engineering Academy of Xinjiang University，Urumqi Xinjiang 830000 ，China） ABSTＲACT： UAV attitude is the basis and core of unmanned aerial vehicle． The attitude measuring sensor used in the low cost and compact four － rotor UAV has shortcoming of low precision， big noise and easily affected by the environment of temperature，so the UAV's attitude can not be measured accurately，especially in acceleration motion，the attitude error will achieve cumulative expansion， which can not acceleration flight for a long time． In order to improve the precision of the measurement system，an attitude measurement system is designed to compose gyroscope accelerometer and magnetometer，throough calculating the accelerometer trust and using the method of Kalman filter to estimate four － error quaternion，the attitude angle of UAV is obtained． The experimental results show that the Kalman filter method not only can accurately calculate the attitude angle in low dynamic flight environments，but also can overcome shortcoming of the UAV using the past Kalman filter can not accelerated flying for a long time． The Kalman filter method is successfully applied to the four － rotor UAV flight attitude measurement． KEYWOＲDS： The four rotor； Error quaternion； Attitude estimate； Kalman filter 机姿态控制的前提， 它不仅为无人机的控制系统提供当前的 姿态信息， 还为安装在无人机上的照相机等机载设备提供姿 态基准。 尽管单一使用微机械陀螺仪就可以进行姿态角度的测 量， 但是其精度难以得到保证， 并且其系统误差会随时间漂 移， 而高性能的陀螺仪价格高 、 体积大， 不适宜在四旋翼无人 机中使用。然而价格低、 体积小的微机械陀螺仪测量误差会 随时间漂移， 不适用于长时间姿态的确定 。 因此只有将微机
[
－ r x sin
]
（ 1）
2
系统组成
本文所采用的姿态测量系统主要由三轴加速度计 、 三轴
ry 、 r z 代表矢量 r 珒 在参考坐标系中的位置， θ 是动坐标 式中 r x 、 系绕矢量 r 珒 旋转的角度。 则从载体坐标系到参考坐标系的姿态矩阵为
陀螺仪和三轴磁强计组成， 如图 1 所示。 主控制器读取姿态 测量传感器的数据， 通过卡尔曼滤波对传感器数据进行融 合， 得到当前最优误差四元数， 并用该误差四元数修正得到 最优姿态， 与此同时通过 UAＲT 总线将数据上传至上位机， 以便观察， 与此同时主控制器还将当前输入信号转换成角 度， 并与当前解算出来的姿态角度进行对比， 经过 PID 运算 后， 输出 PWM 信号， 经电子调速器放大后控制无刷直流电 机， 从而实现对四旋翼无人机的姿态控制 。 系统结构图如图 1 所示。
（ 6） Δθ x 0
第 32 卷
第 02 期
ቤተ መጻሕፍቲ ባይዱ
计
算
机
仿
真
2015 年 02 月
文章编号： 1006 － 9348 （ 2015 ） 02 － 0067 － 07
小型四旋翼无人机姿态测量仿真研究
袁 楚仕彬， 亮
（ 新疆大学机械工程学院， 新疆 乌鲁木齐 830000 ） 摘要： 研究无人机姿态流通量优化问题， 由于低成本小型四旋翼无人机中所使用的姿态测量传感器存在精度低、 噪声大的缺 点， 并且易受温度等环境的影响， 不能准确的测量出无人机的姿态， 特别是在无人机加速飞行过程中， 姿态误差会很快累积 扩大， 不能长时间加速飞行 。为提高无人机姿态测量精度， 提出设计由陀螺仪、 加速度计和磁强计组成的姿态测量系统， 通 过计算加速度计信任度， 并利用卡尔曼滤波的方法实现对误差四元数的估计， 从而得到无人机的姿态角度。实验结果表明， 该卡尔曼滤波能够在低动态的飞行环境中准确测量出无人机姿态角度， 并能克服以往无人机不能长时间加速飞行的缺点， 证明卡尔曼滤波被成功应用于四旋翼无人机姿态测量优化提供了科学依据 。 关键词： 四旋翼； 误差四元数； 姿态估计； 卡尔曼滤波 中图分类号： V271. 4 文献标识码： B
q0 + q1 － q2 － q3 2 （ q1 q2 － q0 q3 ） 2 （ q1 q3 + q0 q2 ） 2 2 2 q2 2 （ q2 q3 － q0 q1 ） （ 2 ） Ｒ = 2 （ q1 q2 + q0 q3 ） 0 － q1 + q2 － q3 2 2 2 2 （ q1 q3 － q0 q2 ） 2 （ q2 q3 + q0 q1 ） q2 0 － q1 － q2 + q3
械陀螺仪与其它姿态传感器组合使用才能测量出长期 、 稳 定、 可靠的姿态角度。 目前应用于多传感器信息融合的方法很多， 各有特点，
— 67 —
还能提高测量精度， 合适的融合方法不仅能节省系统资源， 13］ 文献［ 采用加权平均法实现信号融合， 总体思想是保证总 方差最小的条件下根据各传感器的测量值， 通过自适应的方 式寻找与之对应的最优加权因子， 使融合后的测量值最优， 14］采用互补滤波的 适用于动态环境， 但其精度较低； 文献［ 方法进行飞行器姿态解算， 该方法计算量较小， 时时性高， 但 5］ 其精度低， 姿态漂移严重， 不能应用于动态环境下； 文献［ 采用判断无人机的运动状态来决定是否使用卡尔曼滤波， 优 点是可以实现无人机短时间低动态加速运动， 其缺点是在加 速飞行的过程中陀螺仪漂移过大， 不能长时间加速飞行。 为 了能够实现四旋翼无人机在低动态环境下稳定飞行， 本文采 用计算加速度计信任度的方法， 并利用卡尔曼滤波进行数据 融合， 实现对误差四元数的估计， 从而计算出四旋翼无人机 的姿态。实验结果表明该卡尔曼滤波方法能够在低动态的 环境中解算出准确的姿态角度， 抑制不良噪声。
图1 四旋翼无人机系统结构图
q2 = cos（
ψ θ β ψ θ β ） sin（ ） cos（ ） + sin（ ） cos（ ） sin（ ） 2 2 2 2 2 2 （ 4 c）
3
四旋翼无人机姿态解算的原理
四旋翼无人机姿态解算的实质是通过求解载体坐标系 q3 = sin（
ψ θ β ψ θ β ） cos（ ） cos（ ） － cos（ ） sin（ ） sin（ ） 2 2 2 2 2 2 （ 4 d）
3． 2
载体坐标系 载体坐标系 OX b Y b Z b ， 其原点为载体的转动中心， 也就
是说载体坐标系和参考坐标系的原点重合 。 并且规定绕 Z b 轴转动的角度为 ψ 角， 即航向角； 绕 Y b 轴转动的角度为 θ 即为横滚角。 角， 即俯仰角； 绕 X b 旋转 β 角， 3． 3 基于四元数运动方程的姿态解算方法 四元数姿态表达是一个四参数的表达式， 它基于的思路 是一个坐标系到另一个坐标系的变换可以通过绕一个定义 在参考坐标系的矢量的单次转动来实现 。 四元数用符号 q 表示， 它是一个具有 4 个元素的矢量， 这些元素是该矢量方 向和转动大小的函数。 q = [ q0 = cos q1 θ 2 q2 q3 ] θ 2 － r y sin θ 2 － r z sin θ 2
{
Δ θ0 cos ·I + 2
sin
Δ θ0 q（ 0 ） 2 ΔΘ Δ θ0
}
（ 5）
— 68 —
0 Δθ x ΔΘ = Ω b dt = t1 Δθ y Δθ z
－ Δθ x 0 － Δθ z Δθ y
－ Δθ y Δθ z 0 － Δθ x
∫
t2
－ Δθ z － Δθ y
( q2（ +q qq －+ qq q－ ）q )
2 3 2 3 2 0 0 1 2 1 2 2
由欧拉法和四元数法可知， 四元数与欧拉角的关系如下 q0 = cos（ ψ θ β ψ θ β ） cos（ ） cos（ ） + sin（ ） sin（ ） sin（ ） 2 2 2 2 2 2 （ 4 a） q1 = cos（ ψ θ β ψ θ β ） cos（ ） sin（ ） － sin（ ） sin（ ） cos（ ） 2 2 2 2 2 2 （ 4 b）
相对参考坐标系的姿态矩阵来求解姿态角度 。 3． 1 参考坐标系 参考坐标系 OX e Y e Z e ， 其坐标原点为运动载体的转动中 Xe 、 Y e 轴在当地水平面内， X e 轴指向北方， Y e 轴指向西 心， Xe 、 Ye 、 Z e 三轴满足右手 Z e 轴沿载体的垂线方向指向天， 方， 、 定则。 其中 q（ t） = 3． 4 四元数更新算法 本文四元数更新采用三阶比卡逼近算法 。
基金项目： 国家自然科学基金（ 61262059 ） ； 新疆优秀青年科技创新人 才培养项目（ 2013721016 ） ； 新疆大学博士启动基金 收稿日期： 2014 － 04 － 09 修回日期： 2014 － 05 － 23
1
引言
无人机姿态作为无人机控制的基础和核心， 直接决定了
无人机速度与位置的导航精度， 对无人机自主飞行影响非常 大， 因而备受重视。无人机的姿态测量系统的功能是通过读 取传感器的测量值， 经过相应的姿态解算方法， 从而测量出 无人机的航向角、 俯仰角和横滚角， 这三个角度是实现无人
的旋转， 则合成后的转动四元数 q 可以表达成： q = q2 q1 4． 1 卡尔曼滤波状态方程 假设无人机的姿态由陀螺仪估计姿态和姿态误差值所 组成， 并将无人机实时姿态定义为相对参考坐标系进行了两 次转动： 修正姿态， 相对参考坐标系转动一个误差四元数估 计值； 更新姿态， 转动一个姿态四元数估计值 。 利用四元数 转动合成原理可知， 这一转动过程可以描绘成： Qb = 珚 Q Qe 态误差四元数。表示四元数叉乘运算。 对式（ 13 ） 式两边微分得
与方向余弦法类比， 可以得到姿态矩阵与四元数的关 系， 并且得到用四元数表示的欧拉角， 表达式为 ψ = arctan
2
2
2
2
( q2（ +q qq －+ qq q－ ）q )
2 1 1 2 2 0 0 3 2 2 2 3
（ 3 a） （ 3 b） （ 3 c）
θ = － arcsin（ 2 （ q1 q3 － q0 q2 ） ） β = arctan
Study on Measurement and Simulation of the Low Cost and Compact four Ｒotor UAV
CHU Shi － bin， YUAN Liang
（ Mechanical Engineering Academy of Xinjiang University，Urumqi Xinjiang 830000 ，China） ABSTＲACT： UAV attitude is the basis and core of unmanned aerial vehicle． The attitude measuring sensor used in the low cost and compact four － rotor UAV has shortcoming of low precision， big noise and easily affected by the environment of temperature，so the UAV's attitude can not be measured accurately，especially in acceleration motion，the attitude error will achieve cumulative expansion， which can not acceleration flight for a long time． In order to improve the precision of the measurement system，an attitude measurement system is designed to compose gyroscope accelerometer and magnetometer，throough calculating the accelerometer trust and using the method of Kalman filter to estimate four － error quaternion，the attitude angle of UAV is obtained． The experimental results show that the Kalman filter method not only can accurately calculate the attitude angle in low dynamic flight environments，but also can overcome shortcoming of the UAV using the past Kalman filter can not accelerated flying for a long time． The Kalman filter method is successfully applied to the four － rotor UAV flight attitude measurement． KEYWOＲDS： The four rotor； Error quaternion； Attitude estimate； Kalman filter 机姿态控制的前提， 它不仅为无人机的控制系统提供当前的 姿态信息， 还为安装在无人机上的照相机等机载设备提供姿 态基准。 尽管单一使用微机械陀螺仪就可以进行姿态角度的测 量， 但是其精度难以得到保证， 并且其系统误差会随时间漂 移， 而高性能的陀螺仪价格高 、 体积大， 不适宜在四旋翼无人 机中使用。然而价格低、 体积小的微机械陀螺仪测量误差会 随时间漂移， 不适用于长时间姿态的确定 。 因此只有将微机
[
－ r x sin
]
（ 1）
2
系统组成
本文所采用的姿态测量系统主要由三轴加速度计 、 三轴
ry 、 r z 代表矢量 r 珒 在参考坐标系中的位置， θ 是动坐标 式中 r x 、 系绕矢量 r 珒 旋转的角度。 则从载体坐标系到参考坐标系的姿态矩阵为
陀螺仪和三轴磁强计组成， 如图 1 所示。 主控制器读取姿态 测量传感器的数据， 通过卡尔曼滤波对传感器数据进行融 合， 得到当前最优误差四元数， 并用该误差四元数修正得到 最优姿态， 与此同时通过 UAＲT 总线将数据上传至上位机， 以便观察， 与此同时主控制器还将当前输入信号转换成角 度， 并与当前解算出来的姿态角度进行对比， 经过 PID 运算 后， 输出 PWM 信号， 经电子调速器放大后控制无刷直流电 机， 从而实现对四旋翼无人机的姿态控制 。 系统结构图如图 1 所示。