聯系方式：[email protected]

一、實驗目的

使用慣性測量單元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

姿態融合效果

第二種算法

~/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

姿態融合效果

四、結果分析

從視頻上看，在旋轉時，對于無人機的偏航角第一種方法比第二種方法響應更快，估計更準。分析原因：第一種算法使用加速度/磁力計矯正時，可同時對俯仰角、滾動角和偏航角進行矯正，且狀態矩陣共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

---