ROS 学习系列 -- 使用Rviz 可视化调试9轴机器人姿态融合

机器人平面行走时只需要知道行进方向角度即可，也就是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 tf
    geometry_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 transform
    odom_broadcaster.sendTransform(odom_trans);

    //next, we'll publish the odometry message over ROS
    nav_msgs::Odometry odom;
    odom.header.stamp = current_time;
    odom.header.frame_id = "odom";

    //set the position
    odom.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 velocity
    odom.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 message
    odom_pub.publish(odom);
    r.sleep();
  }
}

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