ROS运动控制——跟踪控制乌龟仿真（2）

问题描述

在ros仿真平台（ros_tutorials）上做《考虑运动受限的履带式移动机器人轨迹跟踪控制》的仿真实验

跟踪控制公式：
①角速度有限时间控制律和线速度滑模变结构控制律跟踪函数：
不限速度

受限速度

② 原跟踪函数：
$${\text{v}}_p\text{ = }\frac{1}{2}\sqrt{{e_x}^2+{e_y}^2}$$
$${\omega }_p\text{ = }4\times \arctan (\frac{e_y}{e_x})$$

关键词：ros、滑模控制、运动受限、跟踪控制、多话题发布与订阅

方法

这里以ros自带的乌龟案例为基础，进行改写。首先，生成第一只乌龟让其做圆周运动或直线运动，作为跟踪的目标；再生成第二只乌龟，用案例中的跟踪控制方法，作为对比实验；最后，生成第三只乌龟，用上文中设计的跟踪控制，进行实验。

代码下载运行

代码下载
在放置文件的路径下打开终端，运行

git clone https://gitee.com/Luweizhiyuan2020/demo02_ws.git

编译文件

cd demo02_ws
catkin_make

运行文件

roscore

source demo02_ws/devel/setup.bash
roslaunch learning_tf  start_tf_demo_restricted_movement.launch

注：速度不受限与受限是两个实验只需改动start_tf_demo_restricted_movement.launch中的注释部分，不受限用；受限用；默认受限

代码

1速度不受限

/**
 * 底盘控制基于利亚普洛夫控制律
 */

#include 
#include 
#include 
#include 
#include "turtlesim/Pose.h"
#include 


static double x1,yl,yaw1,pitch1,roll1,     x2,y2,yaw2,pitch2,roll2,
         	                dx,dy,dyaw       ,dpitch,droll, yaw, pitch, roll,
						    ex,ey,eyaw,         ex_t,ey_t,eyaw_t,
          	                v1,omega1,v2,omega2,
         	                K1,K2,K3,
							t,ts,A,P,Q,K,dyaw0=0,d,flag0;


static ros::Subscriber sub1;
static ros::Subscriber sub2;
static ros::Publisher turtle_vel;

void doPose1(const turtlesim::Pose::ConstPtr    & p){
	x1=p->x;
	yl=p->y;
	yaw1=p->theta;
	v1=p->linear_velocity;
	omega1=p->angular_velocity;
}

void doPose2(const turtlesim::Pose::ConstPtr& p){
	x2=p->x;
	y2=p->y;
	yaw2=p->theta;
	v2=p->linear_velocity;
	omega2=p->angular_velocity;
}

// 方法一：订阅在发布中运行

int main(int argc, char** argv)
{
	setlocale(LC_ALL,"");

	// 初始化ROS节点
	ros::init(argc, argv, "my_tf_listener");

    // 创建节点句柄
	ros::NodeHandle node;

	// 请求产生turtle3
	ros::service::waitForService("/spawn");
	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
	turtlesim::Spawn srv;
	srv.request.x=9.0;
	srv.request.y=9.0;
	srv.request.theta=3.14;
	add_turtle.call(srv);

	sub1 = node.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose1);
	sub2 = node.subscribe<turtlesim::Pose>("/turtle3/pose",1000,doPose2);
	// 创建发布turtle2速度控制指令的发布者
	turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle3/cmd_vel", 10);

	// 创建tf的监听器
	tf::TransformListener listener;

	ros::Rate rate(10);
	while (node.ok())
	{

		// 获取turtle1与turtle2坐标系之间的tf数据
		tf::StampedTransform transform;
		try
		{
			listener.waitForTransform("/turtle3", "/turtle1", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/turtle3", "/turtle1", ros::Time(0), transform);
		}

		catch (tf::TransformException &ex) 
		{
			ROS_ERROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}

		dx=x1-x2;
		dy=yl-y2;
		dyaw=yaw1-yaw2;
        ex_t=cos(yaw2)*dx+sin(yaw2)*dy;
        ey_t=-sin(yaw2)*dx+cos(yaw2)*dy;
        eyaw_t=dyaw;

        ex=transform.getOrigin().x();
        ey=transform.getOrigin().y();
		transform.getBasis().getEulerYPR(eyaw, pitch, roll);

		geometry_msgs::Twist vel_msg;

        K1=0.909;
        K2=1.250;
        K3=0.818;

		A=1;
		P=9;
		Q=11;
		K=0.1;
		d=0.02;


		if(yaw1!=0&&yaw2!=0&&dyaw0==0){
			dyaw0=eyaw;
			ts=Q/A/(Q-P)*pow(abs(dyaw0),(Q-P)/Q);
			// ROS_INFO("yaw1=%.2f",yaw1);
			// ROS_INFO("yaw2=%.2f",yaw2);
			t++;
			// ROS_INFO("t=%.2f",t);
		}

		if(t>0){
			t++;
			// ROS_INFO("t=%.2f",t);
		}

		if(eyaw>0){
			flag0=1;
		}
		else{
			flag0=-1;
		}



		if(t==2){
			vel_msg.linear.x =0;
			vel_msg.angular.z =0;
			// ROS_INFO("t1=%.2f",t);
		}else if(t>2&&t<ts*10){
			vel_msg.angular.z = omega1+flag0*A*pow(eyaw,(9/11));//+A*pow(eyaw,(9/11))
			vel_msg.linear.x =v1+omega1*ex+omega1*ey+K*(ex-ey)/(abs(ex-ey)+d);
			// ROS_INFO("t2=%.2f",t);
		}else if(t>ts*10){
			vel_msg.angular.z = omega1+flag0*A*pow(eyaw,(9/11));//+A*pow(eyaw,(9/11))
			vel_msg.linear.x =v1+omega1*ex+omega1*ey+K*(ex-ey)/(abs(ex-ey)+d);
			// ROS_INFO("t3=%.2f",t);
		}

		 ROS_INFO("test=%.2f",pow(-1,1/11));
		//lyapunov
		// vel_msg.linear.x = v1*cos(eyaw)+K2*ex;
		// vel_msg.angular.z = omega1+ey*v1/K1+K3*sin(eyaw)/K1;
		//源函数
		// vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
		// 		                        transform.getOrigin().x());
		// vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
		// 		                      pow(transform.getOrigin().y(), 2));


		turtle_vel.publish(vel_msg);
		rate.sleep();
		ros::spinOnce();
	}
	return 0;
};

2速度受限

/*
 * 底盘控制基于利亚普洛夫控制律
 */

#include 
#include 
#include 
#include 
#include "turtlesim/Pose.h"
#include 


static double x1,yl,yaw1,pitch1,roll1,     x2,y2,yaw2,pitch2,roll2,
         	                dx,dy,dyaw       ,dpitch,droll, yaw, pitch, roll,
						    ex,ey,eyaw,         ex_t,ey_t,eyaw_t,
          	                v1,omega1,v2,omega2,
         	                K1,K2,K3,
							t,ts,A,P,Q,K,dyaw0=0,d,flag0,
							v3_pre,omega3_pre,v3_max,omega3_max,
							a3,beita3,a3_pre,beita3_pre,a3_max,beita3_max,dt,
							v3,omega3;


static ros::Subscriber sub1;
static ros::Subscriber sub2;
static ros::Publisher turtle_vel;

void doPose1(const turtlesim::Pose::ConstPtr    & p){
	x1=p->x;
	yl=p->y;
	yaw1=p->theta;
	v1=p->linear_velocity;
	omega1=p->angular_velocity;
}

void doPose2(const turtlesim::Pose::ConstPtr& p){
	x2=p->x;
	y2=p->y;
	yaw2=p->theta;
	v2=p->linear_velocity;
	omega2=p->angular_velocity;
}

int sgn(double para)
{
	if (para> 0) return 1;
	if (para <0) return -1;
	return 0;
}

// 方法一：订阅在发布中运行

int main(int argc, char** argv)
{
	setlocale(LC_ALL,"");

	// 初始化ROS节点
	ros::init(argc, argv, "my_tf_listener");

    // 创建节点句柄
	ros::NodeHandle node;

	// 请求产生turtle3
	ros::service::waitForService("/spawn");
	ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
	turtlesim::Spawn srv;
	srv.request.x=9.0;
	srv.request.y=9.0;
	srv.request.theta=3.14;
	add_turtle.call(srv);

	sub1 = node.subscribe<turtlesim::Pose>("/turtle1/pose",1000,doPose1);
	sub2 = node.subscribe<turtlesim::Pose>("/turtle3/pose",1000,doPose2);
	// 创建发布turtle2速度控制指令的发布者
	turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle3/cmd_vel", 10);

	// 创建tf的监听器
	tf::TransformListener listener;

	ros::Rate rate(10);
	while (node.ok())
	{

		// 获取turtle1与turtle2坐标系之间的tf数据
		tf::StampedTransform transform;
		try
		{
			listener.waitForTransform("/turtle3", "/turtle1", ros::Time(0), ros::Duration(3.0));
			listener.lookupTransform("/turtle3", "/turtle1", ros::Time(0), transform);
		}

		catch (tf::TransformException &ex) 
		{
			ROS_ERROR("%s",ex.what());
			ros::Duration(1.0).sleep();
			continue;
		}

		dx=x1-x2;
		dy=yl-y2;
		dyaw=yaw1-yaw2;
        ex_t=cos(yaw2)*dx+sin(yaw2)*dy;
        ey_t=-sin(yaw2)*dx+cos(yaw2)*dy;
        eyaw_t=dyaw;

        ex=transform.getOrigin().x();
        ey=transform.getOrigin().y();
		transform.getBasis().getEulerYPR(eyaw, pitch, roll);

		geometry_msgs::Twist vel_msg;

        // K1=0.909;
        // K2=1.250;
        // K3=0.818;

		A=1;
		P=9;
		Q=11;
		K=0.1;
		d=0.02;
		dt=0.1;

		if(yaw1!=0&&yaw2!=0&&dyaw0==0){
			dyaw0=eyaw;
			ts=Q/A/(Q-P)*pow(abs(dyaw0),(Q-P)/Q);
			// ROS_INFO("yaw1=%.2f",yaw1);
			// ROS_INFO("yaw2=%.2f",yaw2);
			t++;
			// ROS_INFO("t=%.2f",t);
		}

		if(t>0){
			t++;
			omega3= omega1+sgn(eyaw)*A*pow(eyaw,(9/11));//+A*pow(eyaw,(9/11))
			v3 =v1+omega1*ex+omega1*ey+K*(ex-ey)/(abs(ex-ey)+d);
		}

		v3_max=2.0;
		omega3_max=1.0;
		a3_max=20;
		beita3_max=15;

		a3=(v3-v3_pre)/dt;
		beita3=(omega3-omega3_pre)/dt;

		//测最大加速度
		// if(abs(a3)>a3_max)a3_max=abs(a3);
		// 	ROS_INFO("a3_max=%.2f",a3_max);
		// if(abs(beita3)>beita3_max)beita3_max=abs(beita3);
		// 	ROS_INFO("beita3_max=%.2f",beita3_max);

		if(beita3>beita3_max){
			vel_msg.angular.z =omega3_pre+sgn(omega3-omega3_pre)*beita3_max*dt;
			// ROS_INFO("t1=%.2f",t);
		}else if(omega3>omega3_max){
			vel_msg.angular.z = omega3_max*sgn(omega3);
			// ROS_INFO("t2=%.2f",t);
		}else{
			vel_msg.angular.z = omega3;
			// ROS_INFO("t3=%.2f",t);
		}

		if(a3>a3_max){
			vel_msg.linear.x =v3_pre+sgn(v3-v3_pre)*a3_max*dt;
			// ROS_INFO("t1=%.2f",t);
		}else if(v3>v3_max){
			vel_msg.linear.x =v3_max*sgn(v3);	
			// ROS_INFO("t2=%.2f",t);
		}else{
			vel_msg.linear.x =v3;
			// ROS_INFO("t3=%.2f",t);
		}

		v3_pre=v3;
		omega3_pre=omega3;
		a3_pre=a3;
		beita3_pre=beita3;

		//角度控制律，速度滑膜
		// if(t==2){
		// 	vel_msg.linear.x =0;
		// 	vel_msg.angular.z =0;
		// 	// ROS_INFO("t1=%.2f",t);
		// }else if(t>2&&t
		// 	vel_msg.angular.z = omega1+flag0*A*pow(eyaw,(9/11));//+A*pow(eyaw,(9/11))
		// 	vel_msg.linear.x =v1+omega1*ex+omega1*ey+K*(ex-ey)/(abs(ex-ey)+d);
		// 	// ROS_INFO("t2=%.2f",t);
		// }else if(t>ts*10){
		// 	vel_msg.angular.z = omega1+flag0*A*pow(eyaw,(9/11));//+A*pow(eyaw,(9/11))
		// 	vel_msg.linear.x =v1+omega1*ex+omega1*ey+K*(ex-ey)/(abs(ex-ey)+d);
		// 	// ROS_INFO("t3=%.2f",t);
		// }

		//lyapunov控制律
		// vel_msg.linear.x = v1*cos(eyaw)+K2*ex;
		// vel_msg.angular.z = omega1+ey*v1/K1+K3*sin(eyaw)/K1;
		//源函数
		// vel_msg.angular.z = 4.0 * atan2(transform.getOrigin().y(),
		// 		                        transform.getOrigin().x());
		// vel_msg.linear.x = 0.5 * sqrt(pow(transform.getOrigin().x(), 2) +
		// 		                      pow(transform.getOrigin().y(), 2));


		turtle_vel.publish(vel_msg);
		rate.sleep();
		ros::spinOnce();
	}
	return 0;
};

注：①这里只提供了利亚普洛夫控制律跟踪控制的代码，其他代码请参考古月居的坐标变换：https://www.bilibili.com/video/BV1zt411G7Vn?p=18
②重点多话题订阅和发布

结果展示

不受限

受限

参考

链接: https://www.bilibili.com/video/BV1zt411G7Vn?p=18.
链接: https://www.zhangqiaokeyan.com/academic-journal-cn_computer-measurement-control_thesis/0201242108175.html.