无人机姿态融合——EKF

联系方式：860122112@qq.com

一、实验目的

使用惯性测量单元IMU和磁场传感器(磁力计)的信息，通过EKF对四旋翼无人机进行姿态融合。

二、实验环境

ROS机器人操作系统

三、实验步骤

1. 安装hector quadrotor
hector quadrotor 是德国老牌理工学校Technische Universität Darmstadt大学开发的ros包，整合了ros和gazebo，可以进行uav相关的很多仿真实验，例如飞行动力学，机载传感器例如imu、gps、camera，复杂环境仿真，姿态融合，SLAM等等。

首先下载和编译hector quadrotor的ros包（注意自己安装的ros版本，这里用的是kinetic版）

~$ mkdir catkin_ws/src/hector_quadrotor_tutorial
~$ cd catkin_ws/src/hector_quadrotor_tutorial
$ wstool init src https://raw.github.com/tu-darmstadt-ros-pkg/hector_quadrotor/kinetic-devel/tutorials.rosinstall
$ cd ../..
~/catkin_ws$ catkin_make
~/catkin_ws$ source devel/setup.bash

然后可以启动launch文件（共有两个，一个是outdoor，一个是indoor）测试无人机了。

~/catkin_ws$ roslaunch hector_quadrotor_demo outdoor_flight_gazebo.launch

或者

~/catkin_ws$ roslaunch hector_quadrotor_demo indoor_slam_gazebo.launch

想要用键盘控制无人机首先要启动无人机的电机

$ rosservice call /enable_motors true

再启动键盘控制节点

$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

测试效果图
outdoor

indoor

参考教程

2. EKF姿态融合

这里使用了两种EKF方法进行姿态融合
第一种的参考文献在此
第二种的参考文献在此
其中第二种基于四元数实现。

算法实现及使用：
首先下载和编译算法代码ros包

~$ cd catkin_ws/src/
~/catkin_ws/src$ git clone https://github.com/DajunZhou/uav_att_EKF.git
~/catkin_ws/src$ cd ..
~/catkin_ws$ catkin_make
~/catkin_ws$ source devel/setup.bash

启动测试环境（可以自己写一个empty world的测试环境）：

~/catkin_ws$ roslaunch hector_quadrotor_demo outdoor_flight_gazebo.launch
~/catkin_ws$ roslaunch uav_att_EKF uav_att_est.launch

（上面使用了两个终端）

启动算法节点（因为是使用python实现，包含了自己实现的类，所以用python启动）
第一种算法

~/catkin_ws$ cd src/uav_att_EKF/scripts/
$ python uav_att_node.py

启动键盘控制

$ rosservice call /enable_motors true
$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

姿态融合效果
src="http://player.youku.com/embed/XMjgxNDc1MzMzNg==" width="800" height="500">

第二种算法

~/catkin_ws$ cd src/uav_att_EKF/scripts/
$ python uav_att_2stageEKF_node.py

启动键盘控制

$ rosservice call /enable_motors true
$ rosrun teleop_twist_keyboard teleop_twist_keyboard.py

姿态融合效果
src="http://player.youku.com/embed/XMjgxNTYzMDU5Mg==" width="800" height="500">

四、结果分析

从视频上看，在旋转时，对于无人机的偏航角第一种方法比第二种方法响应更快，估计更准。分析原因：第一种算法使用加速度/磁力计矫正时，可同时对俯仰角、滚动角和偏航角进行矫正，且状态矩阵共12维，包含信息更多；第二种方法状态使用的是四元数，加速度计不矫正偏航角，磁力计只矫正偏航角，由于实验环境中IMU的频率是100Hz，磁力计是10Hz，所以无人机偏航角的估计会比较慢。但从计算量和计算时间的角度来说，第二种方法应该会优于第一种方法。

参考文献
Comprehensive Simulation of Quadrotor UAVs Using ROS and Gazebo
A Double-Stage Kalman Filter for Orientation Tracking With an Integrated Processor in 9-D IMU