青海省建设网站企业,怎么让WORDPRESS首页显示菜单,wordpress企业免费主题,做公众号排版的网站48.在ROS中实现local planner#xff08;1#xff09;- 实现一个可以用的模板实现了一个模板#xff0c;接下来我们将实现一个简单的纯跟踪控制#xff0c;也就是沿着固定的路径运动#xff0c;全局规划已经规划出路径点#xff0c;基于该路径输出相应的控制速度
1. Pur…48.在ROS中实现local planner1- 实现一个可以用的模板实现了一个模板接下来我们将实现一个简单的纯跟踪控制也就是沿着固定的路径运动全局规划已经规划出路径点基于该路径输出相应的控制速度
1. Pure Pursuit
Pure Pursuit路径跟随便是基于受约束移动机器人圆周运动的特性所开发出来的运动控制方式。原理十分简单如图所示移动机器人有一个前视的搜索半径与机器人规划的路径有一个焦点假设机器人从当前位置到路径焦点的运动为圆周运动。其中的前视距离便是图1中的L。根据几何关系便可以计算机器人的运动半径。
受约束的机器人模型不能横向运动可由两个控制量组成即运动参考点的线速度v与角速度w。在极短的运行周期中机器人都是以固定的线速度与角速度运动。因此机器人的运动可以视为圆周运动w0时为直线运动。
2. 运动半径推算
如图所示的机器人便是绕着一个旋转中心进行圆周运动于是移动机器人的运动控制可视为求解其在运动过程中的实时旋转半径。图中r为移动机器人的旋转半径。
我们以base坐标系为例及当前机器人为坐标原点x轴为前方y轴为左方即ROS的坐标系(x,y)为目标点 L为距离目标点的距离前视距离如下计算容易求得旋转半径r
由图可得dr−yd {r - y} dr−y d2x2r2d^2x^2 r^2 d2x2r2 即:r2−2ryy2x2r2r^2-2ryy^2x^2 r^2 r2−2ryy2x2r2 即:x2y22ryx^2y^2 2ry x2y22ry 即:L22ryL^2 2ry L22ry 即:rL2/2yr L^2/2y rL2/2y
即运动半径前视距离的平方/两倍的y
我们知道rv/w 即我们只需要给定v/w为固定的值即可
因v与L相关 我们取一次关系 令vaL a为长数项 可得wv/ra*2y/L
3. 坐标转换
我们知道setPlan下发的坐标一般使用的是map坐标系我们计算的时候需要转换为base坐标系
我们可以使用init接口提供的tf::TransformListener即可完成, 1.14.0版本后接口更新使用新的接口 geometry_msgs::PoseStamped PurepursuitPlanner::goalToBaseFrame(const geometry_msgs::PoseStamped goal_pose_msg) {#if ROS_VERSION_GE(ROS_VERSION_MAJOR, ROS_VERSION_MINOR, ROS_VERSION_PATCH, 1, 14, 0)geometry_msgs::PoseStamped goal_pose, base_pose_msg;goal_pose goal_pose_msg;goal_pose.header.stamp ros::Time(0.0);try {base_pose_msg tf_-transform(goal_pose, base_link);} catch (tf2::TransformException ex) {ROS_WARN(transform err);return base_pose_msg;}#elsegeometry_msgs::PoseStamped base_pose_msg;tf::Stampedtf::Pose goal_pose, base_pose;poseStampedMsgToTF(goal_pose_msg, goal_pose);goal_pose.stamp_ ros::Time();try {tf_-transformPose(costmap_ros_-getBaseFrameID(), goal_pose, base_pose);} catch (tf::TransformException ex) {ROS_WARN(transform err);return base_pose_msg;}tf::poseStampedTFToMsg(base_pose, base_pose_msg);#endifreturn base_pose_msg;}
4. 前视距离
我们不断根据当前位置更新前视距离通过前面的接算给定速度
4.1 前视距离大小设置
前世距离可以根据V我们预设速度相关 如果前世距离较大相当于路径采样间隔较大跟踪路径与规划路径的偏差会大。 如果前世距离较小机器人容易抖动
4.2 前视距离更新策略
如果当前距离路径中前视距离的点后的n个点的距离小于前世距离则更新前视距离 即如果当前前视距离的点在路径索引为n则判断nm索引距离当前点位置是否小于预设前视距离值
5. 速度限制
一般机器人小车线速度是0的即只能前进无法后退。这就需要我们新增当前前视点角度判断 如果角度超过90即在车的后方。可以对速度修正强制旋转 auto target_yaw_diff atan2(goal.pose.position.y, goal.pose.position.x); // 当前目标点相对机器人的角度.... // 计算半径 速度// 当前目标点相对机器人的角度 相差较大(即目标点在机器人后面) 需要直发角速度(即原地旋转) 转向目标点if (target_yaw_diff PI*0.5) {cmd_vel.linear.x 0;cmd_vel.angular.z 0.8;} else if (target_yaw_diff -PI*0.5) {cmd_vel.linear.x 0;cmd_vel.angular.z -0.8;}6. 完成判断
我们在前视点到达规划路径的最后一个时且当前点与该最后一点距离差小于预设的容忍差,强制输出0速度 if (got l GOAL_TOERANCE_XY) {goal_reached_ true;cmd_vel.angular.z 0;cmd_vel.linear.x 0;}7. 测试
修改move_base配置文件move_base_params.yaml
# base_local_planner: dwa_local_planner/DWAPlannerROS
base_local_planner: pure_pursuit_local_planner/PurepursuitPlannerdwa_local_planner/DWAPlannerROS—pure_pursuit_local_planner/PurepursuitPlanner
启动模拟器
pibot_simulator启动rviz
pibot_view
