48.在ROS中實現(xiàn)local planner（1）- 實現(xiàn)一個可以用的模板實現(xiàn)了一個模板咕痛，接下來我們將實現(xiàn)一個簡單的純跟蹤控制茉贡，也就是沿著固定的路徑運動，全局規(guī)劃已經(jīng)規(guī)劃出路徑點腔丧，基于該路徑輸出相應(yīng)的控制速度
1. Pure Pursuit
Pure Pursuit路徑跟隨便是基于受約束移動機(jī)器人圓周運動的特性所開發(fā)出來的運動控制方式作烟。原理十分簡單，如圖所示拿撩，移動機(jī)器人有一個前視的搜索半徑，與機(jī)器人規(guī)劃的路徑有一個焦點，假設(shè)機(jī)器人從當(dāng)前位置到路徑焦點的運動為圓周運動错邦。其中的前視距離便是圖1中的L型宙。根據(jù)幾何關(guān)系便可以計算機(jī)器人的運動半徑。
受約束的機(jī)器人模型（不能橫向運動）可由兩個控制量組成早歇，即運動參考點的線速度v與角速度w讨勤。在極短的運行周期中，機(jī)器人都是以固定的線速度與角速度運動潭千。因此機(jī)器人的運動可以視為圓周運動（w=0時為直線運動）。
2. 運動半徑推算
如圖所示的機(jī)器人便是繞著一個旋轉(zhuǎn)中心進(jìn)行圓周運動屉来，于是移動機(jī)器人的運動控制可視為求解其在運動過程中的實時旋轉(zhuǎn)半徑。圖中茄靠，r為移動機(jī)器人的旋轉(zhuǎn)半徑。
我們以base坐標(biāo)系為例慨绳，及當(dāng)前機(jī)器人為坐標(biāo)原點真竖，x軸為前方，y軸為左方恢共，即ROS的坐標(biāo)系，(x,y)為目標(biāo)點讨韭， L為距離目標(biāo)點的距離（前視距離），如下計算狰闪，容易求得旋轉(zhuǎn)半徑r
由圖可得
即:
即:
即:
即:
即運動半徑=前視距離的平方/兩倍的y
我們知道r=v/w 即我們只需要給定v/w為固定的值即可
因v與L相關(guān) 我們?nèi)∫淮侮P(guān)系
令v=aL a為長數(shù)項
可得w=v/r=a*2y/L
3. 坐標(biāo)轉(zhuǎn)換
我們知道setPlan下發(fā)的坐標(biāo)一般使用的是map坐標(biāo)系蹬铺，我們計算的時候需要轉(zhuǎn)換為base坐標(biāo)系
我們可以使用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;
}
#else
geometry_msgs::PoseStamped base_pose_msg;
tf::Stamped<tf::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);
#endif
return base_pose_msg;
}
4. 前視距離
我們不斷根據(jù)當(dāng)前位置秋泄，更新前視距離，通過前面的接算恒序，給定速度
4.1 前視距離大小設(shè)置
前世距離可以根據(jù)V我們預(yù)設(shè)速度相關(guān)
如果前世距離較大，相當(dāng)于路徑采樣間隔較大歧胁，跟蹤路徑與規(guī)劃路徑的偏差會大。
如果前世距離較小喊巍，機(jī)器人容易抖動
4.2 前視距離更新策略
如果當(dāng)前距離路徑中前視距離的點后的n個點的距離小于前世距離，則更新前視距離
即如果當(dāng)前前視距離的點在路徑索引為n崭参，則判斷n+m索引距離當(dāng)前點位置是否小于預(yù)設(shè)前視距離值
5. 速度限制
一般機(jī)器人小車，線速度是>0的即奄喂，只能前進(jìn)，無法后退跨新。這就需要我們新增當(dāng)前前視點角度判斷， 如果角度超過90域帐，即在車的后方「┦鳎可以對速度修正強(qiáng)制旋轉(zhuǎn)
auto target_yaw_diff = atan2(goal.pose.position.y, goal.pose.position.x); // 當(dāng)前目標(biāo)點相對機(jī)器人的角度
.... // 計算半徑 速度
// 當(dāng)前目標(biāo)點相對機(jī)器人的角度 相差較大(即目標(biāo)點在機(jī)器人后面)贰盗， 需要直發(fā)角速度(即原地旋轉(zhuǎn))， 轉(zhuǎn)向目標(biāo)點
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. 完成判斷
我們在前視點到達(dá)規(guī)劃路徑的最后一個時舵盈，且當(dāng)前點與該最后一點距離差小于預(yù)設(shè)的容忍差,強(qiáng)制輸出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/PurepursuitPlanner
dwa_local_planner/DWAPlannerROS--->pure_pursuit_local_planner/PurepursuitPlanner
- 啟動模擬器
pibot_simulator
- 啟動rviz
pibot_view
