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

问题描述

在ros仿真平台（ros_tutorials）上做《履带式移动机器人轨迹跟踪控制技术研究》的仿真实验

跟踪控制公式：
① Lyapunov控制律下的跟踪函数：
v p =  v B cos ⁡ e θ + K 2 e x {{text{v}}_p}{text{ = }}{{text{v}}_B}cos {e_theta } + {K_2}{e_x} vp​ = vB​coseθ​+K2​ex​
ω p =  ω B + e y v B K 1 + K 3 sin ⁡ e θ K 1 {omega _p}{text{ = }}{omega _B} + frac{{{e_y}{{text{v}}_B}}}{{{K_1}}} + frac{{{K_3}sin {e_theta }}}{{{K_1}}} ωp​ = ωB​+K1​ey​vB​​+K1​K3​sineθ​​
② 原跟踪函数：
v p =  1 2 e x 2 + e y 2 {{text{v}}_p}{text{ = }}frac{1}{2}sqrt {{e_x}^2 + {e_y}^2} vp​ = 21​ex​2+ey​2 ​
ω p =  4 × arctan ⁡ ( e y e x ) {omega _p}{text{ = }}4 times arctan (frac{{{e_y}}}{{{e_x}}}) ωp​ = 4×arctan(ex​ey​​)

关键词：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_lyapunov.launch
 

代码

/*** 底盘控制基于利亚普洛夫控制律跟踪控制*/
 #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;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;// 请求产生turtle3ros::service::waitForService("/spawn");ros::ServiceClient add_turtle = node.serviceClient("/spawn");turtlesim::Spawn srv;add_turtle.call(srv);sub1 = node.subscribe("/turtle1/pose",1000,doPose1);sub2 = node.subscribe("/turtle3/pose",1000,doPose2);// 创建发布turtle2速度控制指令的发布者turtle_vel = node.advertise("/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);ROS_INFO("乌龟1位姿信息:x=%.2f,y=%.2f,theta=%.2f,lv=%.2f,av=%.2f",x1,yl,yaw1,v1,omega1);ROS_INFO("乌龟2位姿信息:x=%.2f,y=%.2f,theta=%.2f,lv=%.2f,av=%.2f",x2,y2,yaw2,v2,omega2);ROS_INFO("乌龟位姿差:ex=%.2f,ey=%.2f,etheta=%.2f",transform.getOrigin().x()-ex_t,transform.getOrigin().y()-ey_t,eyaw-eyaw_t);// 根据turtle1与turtle3坐标系之间的位置关系，发布turtle2的速度控制指令geometry_msgs::Twist vel_msg;K1=0.909;K2=1.250;K3=0.818;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.
链接: http://qikan.cqvip.com/Qikan/Article/Detail?id=676015892.