ROS 学习系列 -- 使用Rviz 可视化调试9轴机器人姿态融合
2015-03-19 20:06
666 查看
机器人平面行走时只需要知道行进方向角度即可,也就是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
陀螺仪的动态性能比叫稳定,但是也有噪音。两组数据的融合就必须采用滤波算法降噪,这也就是大名鼎鼎的卡尔曼滤波就是这么来的。但是该算法过于复杂,在实际使用中我们采用了补偿滤波算法,效果和卡尔曼几乎完全一样,但是算法相当简单。
如图所示,其中红色的点为单纯从 重力加速计 计算出的X轴向倾角pitch,而绿色的点为经过滤波后计算出的倾角。
补偿滤波算法公式:
gyro_y , gyro_x是实时陀螺仪x,y方向分量
time_diff 是积分时间片,单位秒。该值越小越好
accelerometer.read_pitch() accelerometer.read_roll()是实时重力加速计算出的pitch roll
K 取值一般为0.9 -0.98 之间
4. ROS 代码
该代码需要配合RVIz使用,而且fixed_frame要设置为 "odom"
版权声明:本文为博主原创文章,未经博主允许不得转载。
直接看优酷视频吧: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(); } }
版权声明:本文为博主原创文章,未经博主允许不得转载。
相关文章推荐
- ROS 学习系列 -- 使用urdf创建机器人模型在Rviz中3D观察 之一 link使用
- ROS 学习系列 -- 使用urdf创建机器人模型在Rviz中3D观察 之二 joint 使用
- ROS 学习系列 -- 使用urdf创建机器人模型在Rviz中3D观察 之一 link使用
- ROS 学习系列 -- RViz 实时观测机器人建立导航2D封闭空间地图过程 (SLAM)
- ROS 学习系列 -- RViz中移动机器人来学习 URDF,TF,base_link, map,odom和odom 主题的关系
- ROS 学习系列 -- RViz 实时观测机器人建立导航2D封闭空间地图过程 (SLAM)
- ROS 学习系列 -- RViz中移动机器人来学习 URDF,TF,base_link, map,odom和odom 主题的关系
- ROS 学习系列 -- RViz 实时观测机器人建立导航2D封闭空间地图过程 (SLAM)
- ROS 学习系列 -- RViz 实时观测机器人建立导航2D封闭空间地图过程 (SLAM)
- ROS 学习系列 -- 使用Rviz观察智能车的运动轨迹 无陀螺仪计算角度转动
- ROS 学习系列 -- RViz中移动机器人来学习 URDF,TF,base_link, map,odom和odom 主题的关系
- ROS学习系列---RViz中移动机器人来学习 URDF,TF,base_link, map,odom和odom 主题的关系
- ROS 学习系列 -- RViz中移动机器人来学习 URDF,TF,base_link, map,odom和odom 主题的关系
- ROS 学习系列 -- RViz 实时观测机器人建立导航2D封闭空间地图过程 (SLAM)
- ROS 学习系列 -- RViz 实时观测机器人建立导航2D封闭空间地图过程 (SLAM)
- ROS 学习系列 -- RViz 实时观测机器人建立导航2D封闭空间地图过程 (SLAM)
- ROS学习(1)使用URDF创建机器人3D仿真模型
- ROS机器人程序设计(原书第2版)学习镜像分享及使用说明
- ROS机器人程序设计(原书第2版)学习镜像分享及使用说明
- Ubuntu 16.04 + ROS Kinetic 机器人操作系统学习镜像分享与使用安装说明