这里写目录标题
- 实验内容
- 实验准备
- msg数据类型
- 给uwb和odom增加噪声
- robot_pose_ekf
- 发布路径
- 实验结果
实验内容
本实验将在gazebo仿真环境中使用ekf进行传感器数据融合。本文使用turtlebot3进行实验,turtlebot本身会发布odom和imu。imu的误差可以在urdf文件中进行调整,但是gazebo提供的odom却是完美的,因此我们需要手动给其添加误差,这个误差是累积的。另外我们还将提供一个全局的定位观测(通常为gps或uwb),这个我们将直接获取机器人的真实位姿并加上误差。
实验代码在github中,里面robot_pose_ekf是和官网上有点不同的(增加了gps,并且取消了发布odom的tf,因为会冲突)。sensor_data_processing则是实验内容。
实验准备
msg数据类型
首先我们对传感器的消息类型要有所了解,需要用到的基本上都在下面三个包中:
geometry_msgs最基本的包,包含了速度、加速度、角速度、角加速度、位姿的各种表示方法等多种基本的消息类型,被其他自定义数据类型包裹
sensor_msgs提供了imu,可以看到里面包含了四元数表示的旋转、角速度、三轴加速度以及各自的误差矩阵。其中会被用到的其实只有旋转,这也说明这个程序中imu无法单独使用。
nav_msg提供了odom,odom有位姿pose和速度twist两个数据,被用到的只有pose。
gps/uwb只能提供位置信息,没有旋转信息,我们还是使用odom类型,但是把误差矩阵后三项和旋转有关的都置一个很大的数字。
给uwb和odom增加噪声
gazebo中给的odom是完美的,所以我们需要给它增加噪声,这个噪声是累加的,因此我们需要记录前一次odom数据做差,加上噪声融入到新的odom_noise中去。另外,噪声我设置xy轴一样,theta单独误差,可以通过launch配置。并且只有在机器人移动时才增加误差,避免漂移,平移时增加xy方向误差,旋转时增加theta误差。
#include"ros/ros.h"
#include"nav_msgs/Odometry.h"
#include"geometry_msgs/PoseStamped.h"
#include"sensor_msgs/Imu.h"
#include"tf2/LinearMath/Quaternion.h"
#include"tf2/LinearMath/Matrix3x3.h"
// boost
#include"boost/thread/thread.hpp"
class NoiseAdder
{
public:
NoiseAdder(ros::NodeHandle &nh, ros::NodeHandle &private_nh)
:nh_(nh),
private_nh_(private_nh)
{
// 初始化参数
if(!private_nh_.getParam("uwb_noise",uwb_std))
uwb_std=0.1;
if(!private_nh_.getParam("odom_noise_xy",odom_noise_xy))
odom_noise_xy=0.05;
if(!private_nh_.getParam("odom_noise_theta",odom_noise_theta))
odom_noise_theta=0.01;
// 订阅odom
odom_sub = nh_.subscribe("odom", 10, &NoiseAdder::odomCallback, this);
// 订阅imu(为了时间同步)
imu_sub = nh_.subscribe("imu", 10, &NoiseAdder::imuCallback, this);
// 发布odom_noise
odom_noise_pub = nh_.advertise<nav_msgs::Odometry>("odom_noise", 10);
// 发布uwb
uwb_noise_pub = nh_.advertise<nav_msgs::Odometry>("uwb", 10);
// 发布imu
imu_pub = nh_.advertise<sensor_msgs::Imu>("imu_fake", 10);
}
// imu回调函数
void imuCallback(const sensor_msgs::Imu::ConstPtr &imuu)
{
// 上锁
boost::mutex::scoped_lock lock(mutex);
// copy imu
imu = *imuu;
publish();
}
// odom回调函数
void odomCallback(const nav_msgs::Odometry::ConstPtr &odom)
{
// 上锁
boost::mutex::scoped_lock lock(mutex);
// 判断是否为第一次收到
if(first_odom)
{
last_odom=*odom;
odom_noise=*odom;
first_odom=false;
return ;
}
// 发布uwb_noise
uwb_noise.header = odom->header;
uwb_noise.child_frame_id = odom->child_frame_id;
uwb_noise.pose.pose.position.x = odom->pose.pose.position.x + gaussianNoise(0, uwb_std);
uwb_noise.pose.pose.position.y = odom->pose.pose.position.y + gaussianNoise(0, uwb_std);
uwb_noise.pose.pose.position.z = odom->pose.pose.position.z;
uwb_noise.pose.pose.orientation.w = 1.0;
// 修改协方差
uwb_noise.pose.covariance[0] = uwb_std;
uwb_noise.pose.covariance[7] = uwb_std;
uwb_noise.pose.covariance[14] = 0.000001;
uwb_noise.pose.covariance[21] = 9999999;
uwb_noise.pose.covariance[28] = 9999999;
uwb_noise.pose.covariance[35] = 9999999;
uwb_noise_pub.publish(uwb_noise);
// 判断与上一次有没有运动,eps表示误差
if (fabs(odom->pose.pose.position.x - last_odom.pose.pose.position.x) < eps &&
fabs(odom->pose.pose.position.y - last_odom.pose.pose.position.y) < eps &&
fabs(odom->pose.pose.orientation.w - last_odom.pose.pose.orientation.w) < eps)
{
ROS_INFO("Stop");
last_odom = *odom;
odom_noise_pub.publish(odom_noise);
return;
}
ROS_INFO("Move");
odom_noise.header = odom->header;
odom_noise.child_frame_id = odom->child_frame_id;
// 如发生平移,位置累加噪声
if(fabs(odom->pose.pose.position.x - last_odom.pose.pose.position.x) > eps ||
fabs(odom->pose.pose.position.y - last_odom.pose.pose.position.y) > eps )
{
odom_noise.pose.pose.position.x = odom_noise.pose.pose.position.x + (odom->pose.pose.position.x-last_odom.pose.pose.position.x) + gaussianNoise(0, odom_noise_xy);
odom_noise.pose.pose.position.y = odom_noise.pose.pose.position.y + (odom->pose.pose.position.y-last_odom.pose.pose.position.y) + gaussianNoise(0, odom_noise_xy);
odom_noise.pose.pose.position.z = odom->pose.pose.position.z;
}
// 添加姿态噪声
tf2::Quaternion q;
q.setX(odom->pose.pose.orientation.x);
q.setY(odom->pose.pose.orientation.y);
q.setZ(odom->pose.pose.orientation.z);
q.setW(odom->pose.pose.orientation.w);
double roll, pitch, yaw;
tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
tf2::Quaternion q_last;
q_last.setX(last_odom.pose.pose.orientation.x);
q_last.setY(last_odom.pose.pose.orientation.y);
q_last.setZ(last_odom.pose.pose.orientation.z);
q_last.setW(last_odom.pose.pose.orientation.w);
double roll_last, pitch_last, yaw_last;
tf2::Matrix3x3(q_last).getRPY(roll_last, pitch_last, yaw_last);
// 计算和上一帧累计误差
double delta_yaw = yaw - yaw_last;
// 如发生旋转,则增加旋转误差
if(fabs(delta_yaw)> eps)
{
tf2::Quaternion q_noise;
q_noise.setX(odom_noise.pose.pose.orientation.x);
q_noise.setY(odom_noise.pose.pose.orientation.y);
q_noise.setZ(odom_noise.pose.pose.orientation.z);
q_noise.setW(odom_noise.pose.pose.orientation.w);
double roll_noise, pitch_noise, yaw_noise;
tf2::Matrix3x3(q).getRPY(roll_noise, pitch_noise, yaw_noise);
//odom_noise旋转delta_yaw
yaw_noise += delta_yaw + gaussianNoise(0, odom_noise_theta);
// RPY转四元数
q_noise.setRPY(roll_noise, pitch_noise, yaw_noise);
odom_noise.pose.pose.orientation.x = q_noise.x();
odom_noise.pose.pose.orientation.y = q_noise.y();
odom_noise.pose.pose.orientation.z = q_noise.z();
odom_noise.pose.pose.orientation.w = q_noise.w();
// 修改协方差
odom_noise.pose.covariance[0] = odom_noise_xy;
odom_noise.pose.covariance[7] = odom_noise_xy;
odom_noise.pose.covariance[14] = 0.000001;
odom_noise.pose.covariance[21] = 9999999;
odom_noise.pose.covariance[28] = 9999999;
odom_noise.pose.covariance[35] = odom_noise_theta;
}
// 更新odom
last_odom = *odom;
}
// 生成高斯噪声
double uniform_rand(double lowerBndr, double upperBndr)
{
return lowerBndr + ((double) std::rand() / (RAND_MAX + 1.0)) * (upperBndr - lowerBndr);
}
double gaussianNoise(double mean, double sigma)
{
double x, y, r2;
do {
x = -1.0 + 2.0 * uniform_rand(0.0, 1.0);
y = -1.0 + 2.0 * uniform_rand(0.0, 1.0);
r2 = x * x + y * y;
} while (r2 > 1.0 || r2 == 0.0);
return mean + sigma * y * std::sqrt(-2.0 * log(r2) / r2);
}
// 发布所有数据,并同步时间戳
void publish()
{
if(first_odom)
{
return;
}
// 发布imu
imu.header.stamp = ros::Time::now();
imu_pub.publish(imu);
// 发布odom_noise
odom_noise.header.stamp = ros::Time::now();
odom_noise_pub.publish(odom_noise);
// 发布uwb_noise
uwb_noise.header.stamp = ros::Time::now();
uwb_noise_pub.publish(uwb_noise);
}
private:
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
ros::Subscriber odom_sub;
ros::Subscriber imu_sub;
ros::Publisher odom_noise_pub;
ros::Publisher uwb_noise_pub;
ros::Publisher imu_pub;
nav_msgs::Odometry odom_noise;
nav_msgs::Odometry uwb_noise;
sensor_msgs::Imu imu;
// 上一次odom
nav_msgs::Odometry last_odom;
// eps
const double eps = 0.005;
// 噪声大小
double uwb_std;
double odom_noise_xy;
double odom_noise_theta;
// 互斥锁
boost::mutex mutex;
// 判断是否为第一次收到odom
bool first_odom = true;
};
int main(int argc,char**argv)
{
ros::init(argc, argv, "add_noise");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
NoiseAdder noise_adder(nh, private_nh);
ros::spin();
return 0;
}
robot_pose_ekf
首先我们需要安装robot_pose_ekf。
我们直接看launch文件,这个包只支持三种传感器(odom、imu和vo)其中imu类型对应imu,而odom和vo都对应odom。因为gps/uwb只提供位置,这个页面说可以用vo姿态协方差填99999,但是我失败了。我们这里是通过修改代码可以直接增加gps,可以看我的github链接,里面有改过的robot_pose_ekf包。
<launch>
<!-- 发布传感器 -->
<node pkg="sensor_data_processing" type="add_noise" name="noise_publisher" >
<param name="uwb_noise" value="0.1" />
<param name="odom_noise_xy" value="0.05" />
<param name="odom_noise_theta" value="0.1" />
</node>
<!-- odom_combined和odom_noise到odom静态变换 -->
<node pkg="tf" type="static_transform_publisher" name="odom_combined_to_odom" args="0 0 0 0 0 0 odom odom_combined 100" />
<node pkg="tf" type="static_transform_publisher" name="odom_noise_to_odom" args="0 0 0 0 0 0 odom odom_noise 100" />
<!-- Robot pose ekf 拓展卡尔曼滤波-->
<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen">
<param name="output_frame" value="odom_combined"/>
<param name="base_footprint_frame" value="base_footprint"/>
<param name="freq" value="30.0"/>
<param name="sensor_timeout" value="2.0"/>
<param name="odom_used" value="false"/>
<param name="imu_used" value="true"/>
<param name="vo_used" value="false"/>
<param name="gps_used" value="true"/>
<remap from="imu_data" to="imu_fake" />
<remap from="odom" to="odom_noise" />
<remap from="gps" to="uwb" />
</node>
</launch>
发布路径
// 本文件会发布odom和EKF修正后的odom的路径
#include"ros/ros.h"
#include"nav_msgs/Odometry.h"
#include"geometry_msgs/PoseStamped.h"
#include"geometry_msgs/PoseWithCovarianceStamped.h"
// path
#include"nav_msgs/Path.h"
// odom路径
nav_msgs::Path odom_path;
// odom_combined路径
nav_msgs::Path odom_combined_path;
// publisher和subscriber
ros::Publisher odom_path_pub;
ros::Publisher odom_combined_path_pub;
ros::Subscriber odom_sub;
ros::Subscriber odom_combined_sub;
// odom回调函数
void odomCallback(const nav_msgs::Odometry::ConstPtr &odom)
{
// 将odom插入odom_path
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header = odom->header;
pose_stamped.pose = odom->pose.pose;
odom_path.poses.push_back(pose_stamped);
// 发布odom_path
odom_path_pub.publish(odom_path);
// sleep
ros::Duration(0.3).sleep();
ROS_INFO("odom_path size: %d",odom_path.poses.size());
}
// odom_combined回调函数
void odomCombinedCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr &odom_combined)
{
// 将odom_combined插入odom_combined_path
geometry_msgs::PoseStamped pose_stamped;
pose_stamped.header = odom_combined->header;
pose_stamped.pose = odom_combined->pose.pose;
odom_combined_path.poses.push_back(pose_stamped);
// 发布odom_combined_path
odom_combined_path_pub.publish(odom_combined_path);
// sleep
ros::Duration(0.3).sleep();
ROS_INFO("odom_combined_path size: %d",odom_combined_path.poses.size());
}
int main(int argc,char **argv)
{
ros::init(argc,argv,"publish_path");
ros::NodeHandle nh;
// 初始化odom_path
odom_path.header.frame_id = "odom";
odom_path.poses.clear();
// 初始化odom_combined_path
odom_combined_path.header.frame_id = "odom";
odom_combined_path.poses.clear();
odom_path_pub = nh.advertise<nav_msgs::Path>("odom_path",10);
odom_combined_path_pub = nh.advertise<nav_msgs::Path>("odom_combined_path",10);
odom_sub = nh.subscribe("odom_noise",10,odomCallback);
odom_combined_sub = nh.subscribe("/robot_pose_ekf/odom_combined",10,odomCombinedCallback);
ros::spin();
return 0;
}
将融合前融合后的odom分别用nav_msgs/Path来记录,并在rviz中显示出来。
实验结果
执行roslaunch sensor_data_processing create_environment.launch
用于创建环境
执行roslaunch sensor_data_processing sensors_ekf.launch
用于发布数据
可以看到,noise_publisher将gazebo发布的odom增加噪声后发布了odom_noise,并且创造了uwb这个数据,imu_fake则是为了控制时间戳一致。三者被ekf融合输出了odom_combined
执行rosrun sensor_data_processing publish_path
使用rosrun teleop_twist_keyboard teleop_twist_keyboard.py
操纵机器人。可以看到红色为融合后的数据,绿色为带噪声的里程计。