机器人平面行走时只需要知道行进方向角度即可，也就是yaw. 但当行走在有坡度的地方时则需要识别出其它两个倾角：pitch 和 raw. 借助9轴传感器的姿态融合就可以识别。下面就是在Rviz中的识别效果测试(IE可能无法观看)：
1. 九轴姿态融合，欧拉角和四元数机器人三维空间的姿态的表达是用欧拉角, 就是ptich roll 和 yaw. 详见链接
ROS中使用的是四元数而不是欧拉角，好在提供了API进行转换tf::createQuaternionMsgFromRollPitchYaw(roll, pitch，yaw）
九轴传感器：重力加速计，陀螺仪和电子罗盘。9个数据的姿态融合能够得出欧拉角。
整体思路：
1）使用九轴传感姿态融获取ptich roll 和 yaw
2）使用ROS的API转换成四元数，交给tf，详细方法见我的文章
3 ) 3D建模在Rviz中显示，详细方法见我的文章1，文章2
2. pitch，roll的计算和滤波算法选择
重力加速计直接可以计算出pitch,roll. 根据三个重力场分量x,y,z 就可以算出来：
pitch = -atan2(x, sqrt((z * z) + (y * y));roll = atan2(y, sqrt((x * x) + (z * z));这时候在Rviz中观察会发现，即便实际世界中的机器人不动，计算机中的机器人却在抽筋儿似的乱动，但是用手翻滚机器人也能正确的反应变化，但是很不稳定。这是因为重力加速计自身的硬件误差引入的噪音数据造成的，只能通过陀螺仪进行滤波。
陀螺仪的动态性能比叫稳定，，但是也有噪音。两组数据的融合就必须采用滤波算法降噪，这也就是大名鼎鼎的卡尔曼滤波就是这么来的。但是该算法过于复杂，在实际使用中我们采用了补偿滤波算法，效果和卡尔曼几乎完全一样,但是算法相当简单。
如图所示，其中红色的点为单纯从 重力加速计 计算出的X轴向倾角pitch，而绿色的点为经过滤波后计算出的倾角。
补偿滤波算法公式：
pitch = K * (pitch + gyro_y * time_diff) + ( ( 1－K) * accelerometer.read_pitch());roll = K * (roll + gyro_x * time_diff) + ( (1－ K) * accelerometer.read_roll());
gyro_y , gyro_x是实时陀螺仪x,y方向分量
time_diff 是积分时间片，单位秒。该值越小越好
accelerometer.read_pitch() accelerometer.read_roll()是实时重力加速计算出的pitch roll
K 取值一般为0.9 -0.98 之间
3. yaw的数据融合
yaw需要电子罗盘的x,y,z分量融合pitch,roll得出，公式非常复杂，无法贴出来。
4.九轴芯片的选择
市场上多见的芯片是mpu9205,mpu9105. 我选用的是gy95, 包括加速度计（Accelerometer）ADXL345，陀螺仪（Gyroscope）ITG3205以及电子罗盘 （Compass）HMC5883L。它们都使用I2C总线通讯。
4. ROS 代码该代码需要配合RVIz使用，而且fixed_frame要设置为 "odom"
#include <ros/ros.h>#include <tf/transform_broadcaster.h>#include <nav_msgs/Odometry.h>#include "gy85/imu.h"int main(int argc, char** argv){ ros::init(argc, argv, "imu_publisher"); ros::NodeHandle n; ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50); tf::TransformBroadcaster odom_broadcaster; double x = 0.0; double y = 0.0; double th = 0.0; double vx = 0.1; double vy = -0.1; double vth = 0.1;IMU imu; imu.set_compass_offsets(750, -900, -200); imu.set_gyro_offsets(35, -5, -6); double pitch=0, roll=0, yaw=0; ros::Time current_time; current_time = ros::Time::now(); ros::Rate r(50.0); while(n.ok()){ros::spinOnce();//since all odometry is 9DOF we'll need a quaternionimu.read_pitch_roll_yaw(pitch, roll, yaw);geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, -yaw);//first, we'll publish the transform over tfgeometry_msgs::TransformStamped odom_trans;odom_trans.header.stamp = current_time;odom_trans.header.frame_id = "odom";odom_trans.child_frame_id = "base_link";odom_trans.transform.translation.x = x;odom_trans.transform.translation.y = y;odom_trans.transform.translation.z = 0.5;odom_trans.transform.rotation = odom_quat;//send the transformodom_broadcaster.sendTransform(odom_trans);//next, we'll publish the odometry message over ROSnav_msgs::Odometry odom;odom.header.stamp = current_time;odom.header.frame_id = "odom";//set the positionodom.pose.pose.position.x = x;odom.pose.pose.position.y = y;odom.pose.pose.position.z = odom_trans.transform.translation.z;odom.pose.pose.orientation = odom_quat;//set the velocityodom.child_frame_id = "base_link";odom.twist.twist.linear.x = vx;odom.twist.twist.linear.y = vy;odom.twist.twist.angular.z = vth;//publish the messageodom_pub.publish(odom);r.sleep(); }}
并且如此真实的活着——这，就是旅行的意义。
