从程序中学习UKF-SLAM（一）

最新推荐文章于 2024-05-20 00:00:29 发布

happy_earth

最新推荐文章于 2024-05-20 00:00:29 发布

阅读量4.1k

点赞数 5

分类专栏： SLAM 文章标签： ukf

本文链接：https://blog.csdn.net/qq_36355662/article/details/84189701

版权

SLAM 专栏收录该内容

22 篇文章 23 订阅

订阅专栏

然间从GitHub上发现一个小项目，做的是一个基于UKF的状态估计，使用C++版本的mathplot做显示，编译之后却发现显示一直有问题，于是萌生了一个想法：能不能把它移植到ROS中？然后就有了接下来的学习过程，以此记录。如有问题，欢迎提出。

这是需要复写的代码：在GitHub上（project的核心代码是来自源码的，如有侵权，请联系我更改，谢谢。）
源码最终实现的效果如下：
在这里插入图片描述

同样也可参考UKF最初代码：GitHub_Self-Driving Car
同样也有相关博客对该代码进行了解析，仅供参考：UKF算法解析

复写完成的代码请看最后一篇博客。
在ROS中实现的效果如下：百度网盘视频或者YouTube视频
在这里插入图片描述

一、前提说明

首先解释一下，路标数据，里程计数据，激光数据各自的组成部分，以及project涉及的运动模型。

1.landmark路标

在程序中（在ukf_console.h的51行），用一个vector表示：

std::vector<Eigen::Vector3f> Landmarks;

各表示点的id号，点的x坐标以及y坐标。此处说明路标具有id标识的，也就决定了后续过程中数据关联的方式。
在param/world_data.txt中，写明了各个landmark的详细信息：

在程序中直接用C++的文件读取方法，将landmark信息读入vector数组中：

bool ukf_console::getLandmarks(const std::string& path)
{
	std::ifstream in_file(path, std::ifstream::in);
	if (!in_file.is_open()) 
	{
		std::cerr << "Cannot open input file: " << (path) << std::endl;
		exit(EXIT_FAILURE);
	}

	std::string line;
	while(std::getline(in_file, line)) 
	{
		std::istringstream ss(line);
		Eigen::Vector3f mp;
		ss>>mp(0);
		ss>>mp(1);
		ss>>mp(2);
		Landmarks.push_back(mp);
		if (Debug)
			std::cout << (Landmarks.back())(0) << ": " << (Landmarks.back())(1)  << ": " << (Landmarks.back())(2)  << std::endl;
	}

	if (in_file.is_open()) 
		in_file.close();
	
}

2.观测信息

这里的观测信息包括里程计信息和激光信息，在tool.h中定义了一组观测信息的格式：

struct Record {
		Eigen::Vector3f odom;					//里程计数据： 起点夹角, 行走距离, 终点夹角
		std::vector<Eigen::Vector3f> radar;		//激光数据：路标id号, 射线距离, 射线角度
	};

在实际project中，会有多组观测信息，所以这里用一个vector提前保存了所有数据：

std::vector<Record> Measurements;

首先解释一下里程计数据的格式，其中的起点夹角和终点夹角是指 $d\theta _{t+1}^{'}$ 和 $d\theta _{t+1}$ ，行走距离应该是直线距离 $R$ 。（可能这里的角度关系还是有点理解不对，，，）
在这里插入图片描述

激光数据主要是关于landmark的观测信息，这里出了观测距离ranges和观测角度 $\varphi$ ，还有路标的特殊id代号。激光观测原理如下

在这里插入图片描述
实际的观测数据由param/sensor_data.txt文件读入，某一项数据格式如下：

ODOMETRY 0.100692392654 0.100072845247 0.000171392857486
SENSOR 1 1.89645381418 0.374031885671
SENSOR 2 3.85367751107 1.51951017943

同样，用C++的文件读取方法将信息读入vector中：

bool ukf_console::getMeasurements(const std::string& path)
{
	std::ifstream in_file(path, std::ifstream::in);
	if (!in_file.is_open()) {
		std::cerr << "Cannot open input file: " << path << std::endl;
		exit(EXIT_FAILURE);
	}

	std::string line;
	Record record;
	int index = 0;
	while(getline(in_file, line)) 
	{
		std::string sensor_type;
		std::istringstream ss(line);
		ss >> sensor_type;
		//measurement type r1 t r2
		if (sensor_type.compare("ODOMETRY") == 0) 
		{//end the first record;
			if (record.radar.size() != 0) 
			{
				Measurements.push_back(record);
				record.radar.clear();
				if (Debug && index < 50)
					std::cout << index << "-----------" << std::endl;
				index++;
			}
			auto& odo = record.odom;
			ss >> odo(0);
			ss >> odo(1);
			ss >> odo(2);
			if (Debug && index < 50)
				std::cout << (record.odom)(0) << ": " << (record.odom)(1) << ": " << (record.odom)(2) << std::endl;
		} else if (sensor_type.compare("SENSOR") == 0) 
		{
			auto& radars = record.radar;
			Eigen::Vector3f radarR;
			ss >> radarR(0);
			ss >> radarR(1);
			ss >> radarR(2);
			radars.push_back(radarR);
			if (Debug && index < 50)
				std::cout << (radars.back())(0) << ": " << (radars.back())(1) << ": " << (radars.back())(2) << std::endl;
		}
	}
	if (record.radar.size() != 0) 
		Measurements.push_back(record);
	
	if (in_file.is_open()) 
		in_file.close();			
}

3.运动模型

项目中的小车可以认为是两轮差动运动模型，如上图所示，即很容易就能得出下面的运行模型：

$\left\{ \begin{array}{c} x_{t+1}=x_t+R\cdot \cos \left( \theta _t+\varDelta \theta \right)\\ y_{t+1}=y_t+R\cdot \sin \left( \theta _t+\varDelta \theta \right)\\ {\theta _{t+1}=\theta _t+\varDelta \theta +\varDelta \acute{\theta}}\\ \end{array} \right.$

在程序（unscented_kf.cpp的90-93）中，这样实现：

double angle = Xsig(2,i);
Xsig(0, i) = Xsig(0, i) + t * cos(angle + r1);
Xsig(1, i) = Xsig(1, i) + t * sin(angle + r1);
Xsig(2, i) = Xsig(2, i) + r1 + r2;

二、相关Markers的显示

喜欢使用ROS的一个很重要的原因就是它的可视化界面了，RVIZ里的各种marker可以显示很多丰富的信息，令程序调试十分的方便。

1.landmark以及pose的显示

这里采用圆柱体进行显示，直接看landmark的代码，estimatePose的不看了：

void ukf_console::LandmarksPublish()
{
	visualization_msgs::MarkerArray ma;
	visualization_msgs::Marker marker;
	// 设置该标记的命名空间和ID，ID应该是独一无二的
	// 具有相同命名空间和ID的标记将会覆盖前一个
	marker.header.frame_id = "map";
	marker.ns = "2Dlandmarks";
	// 设置标记行为：ADD（添 加），DELETE（删 除）
	marker.action = visualization_msgs::Marker::ADD;
	// 设置标记类型，初始值为球体。在立方体、球体、箭头和 圆柱体之间循环
	marker.type = visualization_msgs::Marker::CUBE;// 设置初始形状为圆柱体
	marker.id = 0;
	
	marker.pose.position.z = 0;
	marker.pose.orientation.x = 0.0;
	marker.pose.orientation.y = 0.0;
	marker.pose.orientation.z = 0.0;
	marker.pose.orientation.w = 1.0;
	// 设置标记的比例，所有方向上尺度1表示1米
	marker.scale.x = marker_scale;
	marker.scale.y = marker_scale;
	marker.scale.z = marker_scale;
	//设置标记颜色，确保alpha（不透明度）值不为0, 紫色
	marker.color.r = 160.0/255.0;
	marker.color.g = 32.0/255.0;
	marker.color.b = 240.0/255.0;
	marker.color.a = 1.0;
	marker.lifetime = ros::Duration(0);
	
	int markers_size = Landmarks.size();
	for(int i=0 ; i < markers_size ; i++)
	{
		// 设置帧 ID和时间戳
		marker.header.stamp = ros::Time::now();
		marker.id++;
		//设置标记位姿。 
		marker.pose.position.x = (Landmarks[i])(1);
		marker.pose.position.y = (Landmarks[i])(2);
		ma.markers.push_back(marker);
	}
	Landmarks_pub.publish(ma);
}

2.观测信息显示

这里采用的是线段类型LINE_STRIP进行显示，看代码：

void ukf_console::AssociationPublish(const std::vector< Eigen::Vector3f >& radars)
{
	// visualization of the data association
	visualization_msgs::MarkerArray ma;
	visualization_msgs::Marker line_strip;

	line_strip.header.frame_id = "map";
	line_strip.header.stamp = ros::Time::now();

	line_strip.id = 0;
	line_strip.ns = "data_association";
	line_strip.type = visualization_msgs::Marker::LINE_STRIP;
	line_strip.action = visualization_msgs::Marker::ADD;

	line_strip.lifetime = ros::Duration(0.1);
	line_strip.pose.position.x = 0;
	line_strip.pose.position.y = 0;
	line_strip.pose.position.z = 0;
	line_strip.pose.orientation.x = 0.0;
	line_strip.pose.orientation.y = 0.0;
	line_strip.pose.orientation.z = 0.0;
	line_strip.pose.orientation.w = 1.0;
	line_strip.scale.x = marker_scale*0.3;  // line uses only x component
	line_strip.scale.y = marker_scale*0.3;
	line_strip.scale.z = marker_scale*0.3;
	line_strip.color.a = 1.0;
	line_strip.color.r = 255/255;
	line_strip.color.g = 0;
	line_strip.color.b = 0;

	// robot pose		
	geometry_msgs::Point pointRobotPose;
	pointRobotPose.x = (ukf_tool->Mu_x)(0);
	pointRobotPose.y = (ukf_tool->Mu_x)(1);
	pointRobotPose.z = 0;
	
	//draw observation lines
	int visible_num = radars.size();
	for(int i = 0; i < visible_num; i++) 
	{
		int index = 0;
		for ( ; index < ukf_tool->LandmarkINmap.size() ; index++) 
		{
			if(ukf_tool->LandmarkINmap[index] == (int)(radars[i])(0)) 
				break;
		}
	
		line_strip.points.clear();
		line_strip.points.push_back(pointRobotPose);
		
		geometry_msgs::Point pointLm;
		pointLm.x = ukf_tool->Mu_x(2 * index + 3);
		pointLm.y = ukf_tool->Mu_x(2 * index + 4);
		pointLm.z = 0.0;
		line_strip.points.push_back(pointLm);
		ma.markers.push_back(line_strip);
		line_strip.id++;
	}   

	Assoc_pub.publish(ma);
}

3.误差的显示

这里对位姿估计的误差，landmark估计的误差都进行了显示，通常都是采用不确定椭圆进行显示，这里也不例外。首先需要对相应的协方差矩阵求解特征向量和特征值，在tool.cpp文件用下面这个函数实现了这样的功能：

bool tool::computeEllipseOrientationScale2D(Eigen::Matrix2d& eigenvectors, Eigen::Vector2d& eigenvalues, const Eigen::Matrix2d& covariance)
{
	eigenvectors.setZero(2, 2);
	eigenvalues.setIdentity(2, 1);
	
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eigensolver(covariance);
	if (eigensolver.info() == Eigen::Success)
	{
		eigenvalues = eigensolver.eigenvalues();
		eigenvectors = eigensolver.eigenvectors();
	}else{
		eigenvalues = Eigen::Vector2d::Zero();  // Setting the scale to zero will hide it on the screen
		eigenvectors = Eigen::Matrix2d::Identity();
		return false;
	}
	
	// Be sure we have a right-handed orientation system
	makeRightHanded(eigenvectors, eigenvalues);
	return true;
}

另外，为了确保是右手坐标系（因为ROS系统中的tf坐标系是右手坐标系），这里需要进行一次检查：

void tool::makeRightHanded(Eigen::Matrix2d& eigenvectors, Eigen::Vector2d& eigenvalues)
{
	Eigen::Vector3d c0;
	c0.setZero();
	c0.head(2) = eigenvectors.col(0);
	c0.normalize();
	Eigen::Vector3d c1;
	c1.setZero();
	c1.head(2) = eigenvectors.col(1);
	c1.normalize();
	Eigen::Vector3d cc = c0.cross(c1);
	if (cc(2) < 0)
	{
		eigenvectors << c1.head(2), c0.head(2);
		double e = eigenvalues(0);
		eigenvalues(0) = eigenvalues(1);
		eigenvalues(1) = e;
	}else
		eigenvectors << c0.head(2), c1.head(2);
}

然后，就可以用协方差矩阵进行不确定性椭圆的绘制了，看代码：

void ukf_console::UncertaintyEllipseShow()
{
	visualization_msgs::MarkerArray ma;
	visualization_msgs::Marker marker;
	marker.header.frame_id = "map";
	marker.id = 0;
	marker.ns = "ekf_predict";
	marker.type = visualization_msgs::Marker::SPHERE;
	marker.action = visualization_msgs::Marker::ADD;
	marker.lifetime = ros::Duration(0.5);
	
	marker.pose.position.z = 0.1;
	marker.color.r = 135/255.0;
	marker.color.g = 206/255.0;
	marker.color.b = 235/255.0;
	marker.color.a = 0.8;//设置标记颜色，确保alpha（不透明度）值为0
	marker.scale.x = marker_scale;
	marker.scale.y = marker_scale;
	marker.scale.z = marker_scale;
	
	// covariance ellipses
	tf::Quaternion orientation;
	tf::Matrix3x3 tf3d;
	Eigen::Matrix2d eigenvectors;
	Eigen::Vector2d eigenvalues;
	Eigen::Matrix2d covariance;// get the covariance matrix 2x2 for each ellipsoid including robot pose
	
	// Count the number of landmarks + robot pose
	unsigned int objs_counter = ((ukf_tool->Mu_x.rows())-3 )/2 + 1;
	unsigned int pose_id=0;
	double quantiles=1 ;
	double ellipse_scale_=5;
	for (size_t i = 0; i < objs_counter; i++)
	{
		marker.id++;
		if( i != 0)
			pose_id = i * 2 + 1;
		
		covariance=ukf_tool->Sigma_y.block(pose_id, pose_id, 2, 2);
		if(tool::computeEllipseOrientationScale2D(eigenvectors, eigenvalues, covariance) == false)
			continue;
		
		// Rotation matrix around  z axis
		tf3d.setValue(eigenvectors(0, 0), eigenvectors(0, 1), 0, eigenvectors(1, 0), eigenvectors(1, 1), 0, 0, 0, 1);

		// get orientation from rotation matrix
		tf3d.getRotation(orientation);
		
		marker.pose.position.x = (ukf_tool->Mu_x)(pose_id);
		marker.pose.position.y = (ukf_tool->Mu_x)(pose_id+1);
		marker.pose.position.z = 0;
		marker.pose.orientation.x = orientation.x();
		marker.pose.orientation.y = orientation.y();
		marker.pose.orientation.z = orientation.z();
		marker.pose.orientation.w = orientation.w();
		marker.scale.x = ellipse_scale_ * quantiles * sqrt(eigenvalues(0));
		marker.scale.y = ellipse_scale_ * quantiles * sqrt(eigenvalues(1));
		marker.scale.z = 0.00001;  // Z can't be 0, limitation of ROS
		ma.markers.push_back(marker);
	}
	
	Uncertainty_pub.publish(ma);
}

三、主循环

在结束各项准备工作之后，就可以进入主循环了，主循环其实也就是两个工作，预测和更新，非常简单：

void ukf_console::run()
{
	//read the map data for all landmarks
	getLandmarks(Worlddata_filepath);

	//read the measurements with odometry and radar data
	getMeasurements(Sensordata_filepath);
	
	LandmarksPublish();
	
	ukf_tool = new unscented_kf(Landmarks.size());
	
	ros::Rate rat(10);
	for (const auto& record : Measurements) 
	{ 
		if (!ros::ok())	
			break;
		
		ukf_tool->ProcessMeasurement(record);
		EstimatePosePublish((ukf_tool->Mu_x).head(3));
		AssociationPublish(record.radar);
		UncertaintyEllipseShow();
		
		rat.sleep();
	}

}

大致思路就是，将观测数据逐条送入ukf系统，进行ukf估计，观测结束，也就预示着程序结束了。

下一篇将会重点说说ukf的估计过程，这里就告一段落了。

happy_earth

关注

5
点赞
踩
47

收藏

觉得还不错? 一键收藏
4
评论
从程序中学习UKF-SLAM（一）

偶然间从GitHub上发现一个小项目，做的是一个基于UKF的状态估计，使用C++版本的mathplot做显示，编译之后却发现显示一直有问题，于是萌生了一个想法：能不能把它移植到ROS中？然后就有了接下来的学习过程，以此记录。如有问题，欢迎提出。这是需要复写的代码：在GitHub上它最终实现的效果如下：复写完成的代码请看最后一篇博客。在ROS中实现的效果如下：一、项目解决的问题首先解释...
复制链接

扫一扫

专栏目录