聯系方式：[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
想要用鍵盤控制無人機首先要啟動無人機的電機
再啟動鍵盤控制節點
$ 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