机器人平面行走时只需要知道行进方向角度即可，也就是yaw. 但当行走在有坡度的地方时则需要识别出其它两个倾角：pitch 和 raw. 借助9轴传感器的姿态融合就可以识别。下面就是在Rviz中的识别效果测试(IE可能无法观看)：

直接看优酷视频吧：http://v.youku.com/v_show/id_XOTE0OTEwNzM2.html

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 quaternion imu.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();}
}

版权声明：本文为博主原创文章，未经博主允许不得转载。