在第三章中講述了executeCycle的總體作用，可以看到這個(gè)函數(shù)的作用主要是將全局路徑規(guī)劃的路徑給到局部路徑規(guī)劃，并判斷機(jī)器人是否到位，如果沒(méi)有到位就調(diào)用computeVelocityCommands函數(shù)計(jì)算機(jī)器人的速度。這里也就是move_base的局部路徑規(guī)劃的所在之處。這章簡(jiǎn)單張開(kāi)看一下move_base的局部路徑規(guī)劃的流程：
move_base中的局部路徑規(guī)劃函數(shù)的默認(rèn)入口函數(shù)應(yīng)該是TrajectoryPlannerROS::computeVelocityCommands，這個(gè)函數(shù)主要包含了以下幾個(gè)部分：
1、獲取機(jī)器人的全局坐標(biāo)
路徑規(guī)劃首先需要知道機(jī)器人在哪兒，所以算法的第一步先得到機(jī)器人的位姿
std::vector<geometry_msgs::PoseStamped> local_plan;
geometry_msgs::PoseStamped global_pose;
if (!costmap_ros_->getRobotPose(global_pose)) {
return false;
}
2、獲取全局路徑規(guī)劃
在獲取了機(jī)器人的坐標(biāo)后，下一步就是獲取之前規(guī)劃出來(lái)的全局路徑規(guī)劃了：
if (!transformGlobalPlan(*tf_, global_plan_, global_pose, *costmap_, global_frame_, transformed_plan)) {
ROS_WARN("Could not transform the global plan to the frame of the controller");
return false;
}
這里的transformGlobalPlan是定義在goal_function中的函數(shù)，該函數(shù)的作用是根據(jù)本地代價(jià)地圖，從全局路徑global_plan截取一段，并將路徑點(diǎn)從/map坐標(biāo)系轉(zhuǎn)換到/odom坐標(biāo)系，生成結(jié)果放在transformed_plan。所以這里transformed_plan大小是小于全局路徑點(diǎn)的。它的大小由：
double dist_threshold = std::max(costmap.getSizeInCellsX() * costmap.getResolution() / 2.0,
costmap.getSizeInCellsY() * costmap.getResolution() / 2.0);
決定，也就是代價(jià)地圖的大小。
3、路徑清除
在機(jī)器人運(yùn)動(dòng)過(guò)程中，全局路徑中的前面一部分點(diǎn)位是被經(jīng)歷過(guò)的，局部路徑中的一部分點(diǎn)也是已經(jīng)走過(guò)的。為了減小后面的計(jì)算量。所以將這些點(diǎn)從路徑中刪除：
void prunePlan(const geometry_msgs::PoseStamped& global_pose, std::vector<geometry_msgs::PoseStamped>& plan, std::vector<geometry_msgs::PoseStamped>& global_plan){
ROS_ASSERT(global_plan.size() >= plan.size());
std::vector<geometry_msgs::PoseStamped>::iterator it = plan.begin();
std::vector<geometry_msgs::PoseStamped>::iterator global_it = global_plan.begin();
while(it != plan.end()){
const geometry_msgs::PoseStamped& w = *it;
// Fixed error bound of 2 meters for now. Can reduce to a portion of the map size or based on the resolution
double x_diff = global_pose.pose.position.x - w.pose.position.x;
double y_diff = global_pose.pose.position.y - w.pose.position.y;
double distance_sq = x_diff * x_diff + y_diff * y_diff;
if(distance_sq < 1){
ROS_DEBUG("Nearest waypoint to <%f, %f> is <%f, %f>\n", global_pose.pose.position.x, global_pose.pose.position.y, w.pose.position.x, w.pose.position.y);
break;
}
it = plan.erase(it);
global_it = global_plan.erase(global_it);
}
}
這里的清除方式很簡(jiǎn)單，因?yàn)榇鎯?chǔ)全局路徑的容器是按順序存放的，所以先經(jīng)歷過(guò)的點(diǎn)肯定是在global_plan的最前端，則我們判斷這個(gè)點(diǎn)離機(jī)器人的距離。因?yàn)殡S著機(jī)器人的運(yùn)動(dòng)這個(gè)點(diǎn)會(huì)逐漸遠(yuǎn)離機(jī)器人。當(dāng)其遠(yuǎn)離機(jī)器人的距離超過(guò)1m以上，則我們將這個(gè)點(diǎn)從全局路徑global_plan以及代價(jià)地圖中刪除。
4、如果機(jī)器人到達(dá)目標(biāo)點(diǎn)附近但是角度不滿足要求
在計(jì)算速度時(shí)，首先判斷當(dāng)前機(jī)器人是否到達(dá)目標(biāo)點(diǎn)，如果到達(dá)目標(biāo)點(diǎn)的話，則需要再判斷角度是否也滿足閾值要求：
if (xy_tolerance_latch_ || (getGoalPositionDistance(global_pose, goal_x, goal_y) <= xy_goal_tolerance_)) {
if (latch_xy_goal_tolerance_) {
xy_tolerance_latch_ = true;
}
//檢查是否到達(dá)目標(biāo)“朝向、姿態(tài)”
//獲取當(dāng)前朝向和目標(biāo)姿態(tài)的差值
double angle = getGoalOrientationAngleDifference(global_pose, goal_th);
//check to see if the goal orientation has been reached
//到達(dá)目標(biāo)位置，如果差值小于容忍度
if (fabs(angle) <= yaw_goal_tolerance_)
如果機(jī)器人的坐標(biāo)已經(jīng)再目標(biāo)點(diǎn)附近，同時(shí)判斷出來(lái)角度的差值也小于閾值，則我們認(rèn)為機(jī)器人已經(jīng)再目標(biāo)點(diǎn)離，此時(shí)將速度全都設(shè)置為0。如果此時(shí)角度差超過(guò)閾值，則認(rèn)為機(jī)器人當(dāng)前還沒(méi)有完全到位，則調(diào)用函數(shù)更新機(jī)器人運(yùn)動(dòng)速度：
//如果到達(dá)位置，但朝向和姿態(tài)沒(méi)達(dá)到目標(biāo)要求
//將全局路徑拷貝進(jìn)來(lái)，并認(rèn)為全局路徑的最后一個(gè)點(diǎn)就是終點(diǎn)
tc_->updatePlan(transformed_plan);
//給定當(dāng)前機(jī)器人的位置和朝向，計(jì)算機(jī)器人應(yīng)該跟隨的“best”軌跡，存儲(chǔ)在drive_cmds中
Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
//發(fā)布代價(jià)地圖點(diǎn)云
map_viz_.publishCostCloud(costmap_);
//copy over the odometry information
//得到里程計(jì)信息
nav_msgs::Odometry base_odom;
odom_helper_.getOdom(base_odom);
首先這里的updatePlan(transformed_plan)將前面獲取的全局路徑規(guī)劃中代價(jià)地圖部分的位姿傳遞給trajectory_planner。然后再調(diào)用findBestPath(global_pose, robot_vel, drive_cmds)函數(shù)計(jì)算出實(shí)際的機(jī)器人的速度。數(shù)據(jù)存儲(chǔ)在drive_cmds中返回。展開(kāi)看一下findBestPath這個(gè)函數(shù)：
4.1、獲取機(jī)器人的footprint
//用當(dāng)前位姿獲取機(jī)器人footprint
std::vector<base_local_planner::Position2DInt> footprint_list =
footprint_helper_.getFootprintCells(
pos,
footprint_spec_,
costmap_,
true);
在給定位置獲取足跡的單元格。這里應(yīng)該會(huì)根據(jù)機(jī)器人模型填充不同的單元格。然后將初始footprint內(nèi)的所有cell標(biāo)記 “within_robot = true”，表示在機(jī)器人足跡內(nèi)：
for (unsigned int i = 0; i < footprint_list.size(); ++i) {
path_map_(footprint_list[i].x, footprint_list[i].y).within_robot = true;
}
4.2、更新地圖
//確保根據(jù)全局規(guī)劃global plan更新路徑，并計(jì)算代價(jià)
//更新哪些地圖上的cell是在全局規(guī)劃路徑上的，target_dist設(shè)置為0
//并且通過(guò)它們和其他點(diǎn)的相對(duì)位置計(jì)算出來(lái)地圖上所有點(diǎn)的target_dist
path_map_.setTargetCells(costmap_, global_plan_);
goal_map_.setLocalGoal(costmap_, global_plan_);
對(duì)于setLocalGoal函數(shù)，是用來(lái)計(jì)算MapGrid地圖中所有點(diǎn)到局部地圖路徑末端點(diǎn)的最短距離。在該函數(shù)中，首先對(duì)映射到局部地圖中的路徑進(jìn)行柵格級(jí)的插補(bǔ)(MapGrid::adjustPlanResolution)，從而使得整個(gè)路徑是柵格連續(xù)的。然后，查找路徑在局部地圖中的最后一個(gè)點(diǎn)，并將MapGrid地圖中的該點(diǎn)標(biāo)記為已訪問(wèn)（標(biāo)記為已訪問(wèn)的點(diǎn)可以進(jìn)行最短路徑計(jì)算），然后調(diào)用computeTargetDistance函數(shù)采用類似dijkstra算法的逐步探索的方式，計(jì)算出MapGrid地圖中所有點(diǎn)（柵格級(jí)）相對(duì)于與標(biāo)記點(diǎn)的最短距離。
對(duì)于setTargetCells函數(shù)，是用來(lái)計(jì)算軌跡MapGrid地圖中所有點(diǎn)到局部地圖路徑的最短距離。與setLocalGoal函數(shù)的區(qū)別主要在于，首先將局部地圖路徑中所有的點(diǎn)標(biāo)記為已訪問(wèn)，然后調(diào)用computeTargetDistance函數(shù)就會(huì)計(jì)算MapGrid地圖中所有點(diǎn)到整個(gè)局部地圖路徑的最短距離，具體算法需要進(jìn)一步了解。
因此，在經(jīng)過(guò)預(yù)計(jì)算后，對(duì)于path_costs_和alignment_costs這兩個(gè)考慮與路徑距離的打分項(xiàng)，其內(nèi)部的MapGrid地圖中存儲(chǔ)的是所有點(diǎn)到路徑的最短距離；對(duì)于goal_costs_和goal_front_costs_這兩個(gè)考慮與局部目標(biāo)點(diǎn)距離的打分項(xiàng)，其內(nèi)部的MapGrid地圖中存儲(chǔ)的是所有點(diǎn)到局部目標(biāo)點(diǎn)的最短距離。
4.3、找到代價(jià)最低的軌跡
當(dāng)我們有了地圖，有了路徑，下一步自然是要找到一條代價(jià)最低的路徑：
//輸入分別是目前機(jī)器人位置，速度以及加速度限制
Trajectory best = createTrajectories(pos[0], pos[1], pos[2],
vel[0], vel[1], vel[2],
acc_lim_x_, acc_lim_y_, acc_lim_theta_);//生成諸多可能軌跡，選取其中打分最高的。這里也就是最關(guān)鍵的一步。
4.3.1、計(jì)算可行的線速度和角速度范圍
這里先對(duì)最大線速度進(jìn)行一個(gè)限制，即保證速度既不超過(guò)預(yù)設(shè)的最大速度限制，也不超過(guò)“起點(diǎn)與目標(biāo)直線距離/總仿真時(shí)間”。
//compute feasible velocity limits in robot space
//聲明最大/小線速度，最大/小角速度
double max_vel_x = max_vel_x_, max_vel_theta;
double min_vel_x, min_vel_theta;
//如果最終的目標(biāo)是有效的（全局規(guī)劃不為空）
//檢查最終點(diǎn)是否是有效的，判斷變量在updatePlan中被賦值
if( final_goal_position_valid_ ){
//計(jì)算當(dāng)前位置和目標(biāo)位置之間的距離：final_goal_dist
double final_goal_dist = hypot( final_goal_x_ - x, final_goal_y_ - y );
//最大速度：在預(yù)設(shè)的最大速度和
max_vel_x = min( max_vel_x, final_goal_dist / sim_time_ );
}
4.3.2、計(jì)算線速度與角速度的上下限
使用的限制是由當(dāng)前速度在一段時(shí)間內(nèi)，由最大加減速度達(dá)到的速度范圍，這里進(jìn)行了一個(gè)判斷，即是否使用dwa法：
① 使用dwa法，則用的是軌跡前向模擬的周期sim_period_（專用于dwa法計(jì)算速度的一個(gè)時(shí)間間隔）；
② 不使用dwa法，則用的是整段仿真時(shí)間sim_time_
//should we use the dynamic window approach?
//是否使用dwa算法，sim_peroid_是1/controller_frequency_，暫時(shí)不清楚sim_period_和sim_time_的區(qū)別
if (dwa_) {
//使用dwa窗口法，sim_period_是dwa計(jì)算最大、最小速度用的時(shí)間
//計(jì)算速度、角速度范圍，引入加速度限制條件（用sim_period_）
max_vel_x = max(min(max_vel_x, vx + acc_x * sim_period_), min_vel_x_);
min_vel_x = max(min_vel_x_, vx - acc_x * sim_period_);
max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_period_);
min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_period_);
} else {
//忽略其中的邏輯，按照不同的規(guī)則生成路徑，調(diào)用的子函數(shù)是generateTrajectory
//不使用dwa窗口法
//計(jì)算速度、角速度范圍，引入加速度限制條件（用sim_time_）
max_vel_x = max(min(max_vel_x, vx + acc_x * sim_time_), min_vel_x_);
min_vel_x = max(min_vel_x_, vx - acc_x * sim_time_);
max_vel_theta = min(max_vel_th_, vtheta + acc_theta * sim_time_);
min_vel_theta = max(min_vel_th_, vtheta - acc_theta * sim_time_);
}
在生成范圍時(shí)注意用預(yù)先給定的速度和角速度范圍參數(shù)進(jìn)行保護(hù)。
4.3.3、計(jì)算采樣間隔以及初始采樣速度
根據(jù)預(yù)設(shè)的線速度與角速度的采樣數(shù)，和上面計(jì)算得到的范圍，分別計(jì)算出采樣間隔，并把范圍內(nèi)最小的線速度和角速度作為初始采樣速度。不考慮全向機(jī)器人的情況，即不能y向移動(dòng)，故暫不考慮vy上采樣。
//we want to sample the velocity space regularly
//計(jì)算速度采樣間隔
double dvx = (max_vel_x - min_vel_x) / (vx_samples_ - 1);
double dvtheta = (max_vel_theta - min_vel_theta) / (vtheta_samples_ - 1);//計(jì)算角速度采樣間隔
double vx_samp = min_vel_x;//x方向速度采樣點(diǎn)
double vtheta_samp = min_vel_theta;//角速度采樣點(diǎn)
double vy_samp = 0.0;
//keep track of the best trajectory seen so far
Trajectory* best_traj = &traj_one;
best_traj->cost_ = -1.0;//先初始化一個(gè)最優(yōu)路徑的代價(jià)=-1
//下面生成的軌跡放到這里，和best_traj作比較，如果生成的軌跡代價(jià)更小，就選擇它
Trajectory* comp_traj = &traj_two;
//先初始化一個(gè)當(dāng)前“對(duì)比”路徑的代價(jià)=-1，等會(huì)再用generateTrajectory函數(shù)生成路徑和新的代價(jià)存放在里面
comp_traj->cost_ = -1.0;
//用于交換的指針
Trajectory* swap = NULL;
為了迭代比較不同采樣速度生成的不同路徑的代價(jià)，這里聲明best_traj和comp_traj并都將其代價(jià)初始化為-1
4.3.4、軌跡生成
遍歷所有線速度和角速度，調(diào)用類內(nèi)generateTrajectory函數(shù)用它們生成軌跡。二重迭代時(shí)，外層遍歷線速度（正值），內(nèi)層遍歷角速度。在遍歷時(shí)，單獨(dú)拎出角速度=0，即直線前進(jìn)的情況，避免由于采樣間隔的設(shè)置而躍過(guò)了這種特殊情況。
邊迭代生成邊比較，獲得代價(jià)最小的路徑，存放在best_traj。
//【循環(huán)所有速度和角速度、打分】
//外側(cè)循環(huán)所有x速度
for(int i = 0; i < vx_samples_; ++i) {
//x速度循環(huán)內(nèi)部遍歷角速度
//①角速度=0
vtheta_samp = 0;
//first sample the straight trajectory
//傳入當(dāng)前位姿、速度、加速度限制，采樣起始x向速度、y向速度、角速度，代價(jià)賦給comp_traj
generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it
//對(duì)比生成路徑和當(dāng)前最優(yōu)路徑的分?jǐn)?shù)，如果生成的路徑分?jǐn)?shù)更小，就把當(dāng)前路徑和最優(yōu)路徑交換
//這里會(huì)將第一次生成路徑的代價(jià)賦給best_traj
if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
swap = best_traj;
best_traj = comp_traj;
comp_traj = swap;
}
//接下來(lái)產(chǎn)生機(jī)器人在原地旋轉(zhuǎn)的軌跡
vtheta_samp = min_vel_theta;
//next sample all theta trajectories
//接下來(lái)迭代循環(huán)生成所有角速度的路徑、打分
for(int j = 0; j < vtheta_samples_ - 1; ++j){
generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it
//同樣地，如果新路徑代價(jià)更小，和best_traj作交換
if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
swap = best_traj;
best_traj = comp_traj;
comp_traj = swap;
}
//遍歷角速度
vtheta_samp += dvtheta;
}
//遍歷x速度
vx_samp += dvx;
}
這里需要注意到一個(gè)重要的函數(shù)就是generateTrajectory函數(shù)，局部路徑規(guī)劃的路徑生成器就是來(lái)自于這里。這個(gè)函數(shù)出要邏輯如下：
4.3.4.1、計(jì)算總體速度
//compute the magnitude of the velocities
double vmag = hypot(vx_samp, vy_samp);
C ++hypot()函數(shù)是cmath標(biāo)頭的庫(kù)函數(shù),用于查找給定數(shù)字的斜邊,接受兩個(gè)數(shù)字并返回斜邊的計(jì)算結(jié)果,即sqrt(x * x + y * y)。
4.3.4.2、計(jì)算步長(zhǎng)
int num_steps;
if(!heading_scoring_) {
//sim_granularity_：仿真點(diǎn)之間的距離間隔
//步數(shù) = max(速度?！量偡抡鏁r(shí)間/距離間隔，角速度/角速度間隔)，四舍五入
num_steps = int(max((vmag * sim_time_) / sim_granularity_, fabs(vtheta_samp) / angular_sim_granularity_) + 0.5);
} else {
//步數(shù) = 總仿真時(shí)間/距離間隔，四舍五入
num_steps = int(sim_time_ / sim_granularity_ + 0.5);
}
局部路徑規(guī)劃的終點(diǎn)在cost_map的邊界處，一般還是距離比較遠(yuǎn)的。所以需要在路徑上按照一定的角度以及距離閾值采樣。
4.3.4.3、計(jì)算每個(gè)步長(zhǎng)的時(shí)間
//至少一步
if(num_steps == 0) {
num_steps = 1;
}
//每一步的時(shí)間
double dt = sim_time_ / num_steps;
double time = 0.0;
4.3.4.4、聲明一條軌跡
resetPoints函數(shù)創(chuàng)造一個(gè)新的軌跡，軌跡速度設(shè)置為sample，cost設(shè)置為-1
//create a potential trajectory
//聲明軌跡
traj.resetPoints();
traj.xv_ = vx_samp;//線速度
traj.yv_ = vy_samp;
traj.thetav_ = vtheta_samp;//角速度
traj.cost_ = -1.0;//默認(rèn)代價(jià)-1.0
4.3.4.5、模擬每一步運(yùn)動(dòng)
在得到具體步數(shù)以及步長(zhǎng)后，接下來(lái)就是計(jì)算每一步機(jī)器人的落點(diǎn)了，這里是通過(guò)一個(gè)for循環(huán)獲取所有的落點(diǎn)，主要是執(zhí)行了以下幾個(gè)步驟：
- 使用footprintCost計(jì)算出footprint
footprint為負(fù)，即在這一步遇到了障礙物，規(guī)劃失敗
double footprint_cost = footprintCost(x_i, y_i, theta_i);
//if the footprint hits an obstacle this trajectory is invalid
//若返回一個(gè)負(fù)值，說(shuō)明機(jī)器人在路徑上遇障，直接return
if(footprint_cost < 0){
traj.cost_ = -1.0;
return;
footprint_cost的計(jì)算方式是將當(dāng)前機(jī)器人的footprint投影到costmap下，然后判斷這個(gè)區(qū)域內(nèi)的點(diǎn)的cost。對(duì)于costmap，其中每個(gè)柵格的cost計(jì)算方式如下：
在給定一個(gè)單元格與障礙物之間的距離的情況下，計(jì)算距離的成本值。首先進(jìn)行距離的判斷，如果距離為0，則返回代價(jià)為致命障礙物的代價(jià)值LETHAL_OBSTACLE;再繼續(xù)往下判斷，距離x分辨率是否小于內(nèi)切半徑，是的話，則返回代價(jià)為機(jī)器人內(nèi)切圓膨脹障礙物的代價(jià)值INSCRIBED_INFLATED_OBSTACLE;否的話，則代價(jià)按照歐幾里德距離遞減，歐幾里德距離為距離x分辨率，定義factor因子為指數(shù)函數(shù)，最后計(jì)算出cost值。可以看出，距離障礙物越遠(yuǎn)，代價(jià)值就越?。?/p>
static const unsigned char NO_INFORMATION = 255; 未知空間
static const unsigned char LETHAL_OBSTACLE = 254; 說(shuō)明該單元格中存在一個(gè)實(shí)際的障礙。若機(jī)器人的中心在該單元格中，機(jī)器人必然會(huì)跟障礙物相撞
static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; 說(shuō)明該單元格離障礙物的距離小于機(jī)器人內(nèi)切圓半徑。若機(jī)器人的中心位于等于或高于"Inscribed" cost的單元格，機(jī)器人必然會(huì)跟障礙物相撞
static const unsigned char FREE_SPACE = 0; 沒(méi)有障礙的空間
/** @brief Given a distance, compute a cost.給定一個(gè)距離，計(jì)算一個(gè)cost代價(jià)
* @param distance The distance from an obstacle in cells 單元格與障礙物之間的距離
* @return A cost value for the distance 距離的成本值
*
* */
virtual inline unsigned char computeCost(double distance) const
{
unsigned char cost = 0;
if (distance == 0)
cost = LETHAL_OBSTACLE;
else if (distance * resolution_ <= inscribed_radius_)
cost = INSCRIBED_INFLATED_OBSTACLE;
else
{
// make sure cost falls off by Euclidean distance 確保成本以歐幾里德距離遞減
double euclidean_distance = distance * resolution_;
double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
cost = (unsigned char)((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
}
return cost;
}
而根據(jù)機(jī)器人模型又可以分為兩種情況：
圓形機(jī)器人：對(duì)于圓形機(jī)器人，需要判斷機(jī)器人的圓心是否處于FREE_SPACE就可以了。之所以不用考慮半徑問(wèn)題是因?yàn)樵赾ostmap初始化問(wèn)題時(shí)已經(jīng)考慮了膨脹半徑的問(wèn)題了。一般情況下costmap的膨脹半徑會(huì)略大于機(jī)器人內(nèi)切圓半徑，這樣子如果機(jī)器人處于膨脹層就說(shuō)明機(jī)器人的邊會(huì)碰到障礙物。而這些膨脹層就是上面的INSCRIBED_INFLATED_OBSTACLE。所以對(duì)于圓形的機(jī)器人，不管它的中心點(diǎn)處于NO_INFORMATION、LETHAL_OBSTACLE還是INSCRIBED_INFLATED_OBSTACLE都是不可取的，所以圓形機(jī)器人的代價(jià)值應(yīng)該在0-252。
多邊形機(jī)器人：而多邊形機(jī)器人則需要計(jì)算每一條邊上的點(diǎn)，判斷這條邊有沒(méi)有與障礙物重合，這里的邊進(jìn)入膨脹層其實(shí)是可以通行的，因?yàn)榕蛎泴釉诒举|(zhì)上從空間角度來(lái)看是可通行的空間，如果機(jī)器人的邊界都在膨脹層而沒(méi)有入侵到障礙物層，則機(jī)器人本身也不會(huì)與障礙物發(fā)生沖突。所以多邊形機(jī)器人的代價(jià)值范圍應(yīng)該在0-253。
- 計(jì)算occcost
//更新occ_cost：max(max(occ_cost，路徑足跡代價(jià))，當(dāng)前位置的代價(jià))
//也就是把所有路徑點(diǎn)的最大障礙物代價(jià)設(shè)置為路徑的occ_cost
occ_cost = std::max(std::max(occ_cost, footprint_cost), double(costmap_.getCost(cell_x, cell_y)));
costmap_.getCost的作用為給定一個(gè)柵格（mx, my）,返回這個(gè)單元的代價(jià)值。每個(gè)柵格具體的代價(jià)值在前面局部地圖初始化的時(shí)候應(yīng)該都計(jì)算好了的(4.2)，這里只需要查詢就可以了。所以這里occ_cost的值相當(dāng)于獲取了0、footprint_cost以及getCost(cell_x, cell_y)三者中的最大者。這個(gè)值越大說(shuō)明了機(jī)器人走這條路前往目的地的代價(jià)就越大。對(duì)于圓形機(jī)器人來(lái)說(shuō)footprint_cost應(yīng)該等于getCost(cell_x, cell_y)>0,而矩形機(jī)器人的話則既可能footprint_cost>getCost(cell_x, cell_y)也可能footprint_cost<getCost(cell_x, cell_y)。前者是機(jī)器人footprint空間內(nèi)無(wú)障礙物，所以邊界處離障礙物更近。后者可能是該次仿真后存在一個(gè)障礙物點(diǎn)在機(jī)器人footprint空間內(nèi)部。
- 計(jì)算goaldist和pathdist
這里似乎是計(jì)算了兩個(gè)距離值，一個(gè)是到目標(biāo)點(diǎn)的距離，一個(gè)是到全局路徑的距離？這兩個(gè)值越大代表當(dāng)前仿真得到的路徑距離目標(biāo)點(diǎn)的代價(jià)越大。
- 判斷是否需要根據(jù)朝向打分
heading_scoring_timestep_是給朝向打分時(shí)在時(shí)間上要看多遠(yuǎn)，也就是在路徑上走過(guò)一個(gè)特定時(shí)刻后，才為朝向打分一次。
- 軌跡打分
前面4.3.4.2計(jì)算了一個(gè)迭代次數(shù)作為for循環(huán)的次數(shù)，在達(dá)到該次數(shù)之前會(huì)一直循環(huán)前面四個(gè)步驟并將每次循環(huán)計(jì)算得到的軌跡點(diǎn)添加到traj.point內(nèi)作為備選路徑點(diǎn)，直到達(dá)到循環(huán)次數(shù)，此時(shí)機(jī)器人基本上也就位于了這條軌跡的端點(diǎn)。此時(shí)需要計(jì)算并返回這條軌跡的最終代價(jià)值，這個(gè)值最后也會(huì)作為這條軌跡是否被選上的依據(jù)：
double cost = -1.0;
if (!heading_scoring_) {
//代價(jià)=路徑距離+目標(biāo)距離+障礙物代價(jià)（乘以各自的比例）
cost = path_distance_bias_ * path_dist + goal_dist * goal_distance_bias_ + occdist_scale_ * occ_cost;
} else {
//代價(jià)=路徑距離+目標(biāo)距離+障礙物代價(jià)+航向角（如果有航偏則會(huì)增大代價(jià)）
cost = occdist_scale_ * occ_cost + path_distance_bias_ * path_dist + 0.3 * heading_diff + goal_dist * goal_distance_bias_;
}
//設(shè)置稱當(dāng)前路徑的代價(jià)
traj.cost_ = cost;
最終的打分原則比較簡(jiǎn)單，就是三個(gè)代價(jià)值再乘以各自的比例，如果考慮角度分的話再加一個(gè)角度分，最后cost存儲(chǔ)到這條軌跡的traj.cost中。至此，一條路徑的規(guī)劃以及這條路徑的分值就確定下來(lái)了。
4.3.5、軌跡選擇
在上一步驟中我們逐步產(chǎn)生了很多條軌跡，但是這些軌跡中實(shí)際使用的只有一條，怎么區(qū)分出我們需要的這一條軌跡，就是根據(jù)traj.cost參數(shù)：
if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
swap = best_traj;
best_traj = comp_traj;
comp_traj = swap;
}
首先這條軌跡的分?jǐn)?shù)得大于零，這是由于cost中我們根據(jù)它的值是否大于零判斷這條路徑是否會(huì)跟障礙物有沖突，對(duì)于小于零的路徑是走不了的，所以一條合適的路徑必須大于零。其次它的代價(jià)值必須足夠小。代價(jià)值越小，說(shuō)明這條路徑離全局路徑越接近，同時(shí)離障礙物越遠(yuǎn)，那么從規(guī)劃的角度類說(shuō)這是一條很合適的路徑。因?yàn)殡x全局路徑越接近說(shuō)明實(shí)際要走的路徑越短，離障礙物越遠(yuǎn)說(shuō)明這條路徑很安全不容易出問(wèn)題。
4.3.6、原地旋轉(zhuǎn)軌跡評(píng)分
在軌跡生成中，還需要考慮一種情況就是只有角速度沒(méi)有線速度的情況，考慮一下這種情況什么時(shí)候會(huì)發(fā)生：
1、當(dāng)TrajectoryPlannerROS中，位置已經(jīng)到達(dá)目標(biāo)（誤差范圍內(nèi)），姿態(tài)已達(dá)，則直接發(fā)送0速；姿態(tài)未達(dá)，則調(diào)用降速函數(shù)和原地旋轉(zhuǎn)函數(shù)，并調(diào)用checkTrajectory函數(shù)檢查合法性，直到旋轉(zhuǎn)至目標(biāo)姿態(tài)。而checkTrajectory函數(shù)調(diào)用的是scoreTrajectory和generateTrajectory，不會(huì)調(diào)用createTrajectory函數(shù)，所以，快要到達(dá)目標(biāo)附近時(shí)的原地旋轉(zhuǎn)，不會(huì)進(jìn)入到這個(gè)函數(shù)的這部分來(lái)處理。
2、由于局部規(guī)劃器的路徑打分機(jī)制（后述）是：“與目標(biāo)點(diǎn)的距離”和“與全局路徑的偏離”這兩項(xiàng)打分都只考慮路徑終點(diǎn)的cell，而不是考慮路徑上所有cell的綜合效果，機(jī)器人運(yùn)動(dòng)到一個(gè)cell上，哪怕有任何一條能向目標(biāo)再前進(jìn)的無(wú)障礙路徑，它的最終得分一定是要比原地旋轉(zhuǎn)的路徑得分來(lái)得高的。
所以，這里的原地自轉(zhuǎn)，是行進(jìn)過(guò)程中的、未達(dá)目標(biāo)附近時(shí)的原地自轉(zhuǎn)，并且，是機(jī)器人行進(jìn)過(guò)程中遇到障礙、前方無(wú)路可走只好原地自轉(zhuǎn)，或是連原地自轉(zhuǎn)都不能滿足，要由逃逸狀態(tài)后退一段距離，再原地自轉(zhuǎn)調(diào)整方向，準(zhǔn)備接下來(lái)的行動(dòng)。一種可能情況是機(jī)器人行進(jìn)前方遇到了突然出現(xiàn)而不在地圖上的障礙。
在處理這種情況時(shí)，由于機(jī)器人原地旋轉(zhuǎn)時(shí)的角速度限制范圍要比運(yùn)動(dòng)時(shí)的角速度限制范圍更嚴(yán)格，底盤無(wú)法處理過(guò)小的原地轉(zhuǎn)速，故要注意處理這層限制。同樣調(diào)用generateTrajectory函數(shù)，生成路徑。
//循環(huán)所有角速度
double heading_dist = DBL_MAX;
for(int i = 0; i < vtheta_samples_; ++i) {
//enforce a minimum rotational velocity because the base can't handle small in-place rotations
//強(qiáng)制最小原地旋轉(zhuǎn)速度
double vtheta_samp_limited = vtheta_samp > 0 ? max(vtheta_samp, min_in_place_vel_th_)
: min(vtheta_samp, -1.0 * min_in_place_vel_th_);
//產(chǎn)生遍歷角速度的路徑
generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp_limited,
acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it...
//note if we can legally rotate in place we prefer to do that rather than move with y velocity
//如果新路徑代價(jià)更小
if(comp_traj->cost_ >= 0
&& (comp_traj->cost_ <= best_traj->cost_ || best_traj->cost_ < 0 || best_traj->yv_ != 0.0)
&& (vtheta_samp > dvtheta || vtheta_samp < -1 * dvtheta)){
//獲取新路徑的終點(diǎn)（原地）
double x_r, y_r, th_r;
comp_traj->getEndpoint(x_r, y_r, th_r);
//坐標(biāo)計(jì)算
x_r += heading_lookahead_ * cos(th_r);
y_r += heading_lookahead_ * sin(th_r);
unsigned int cell_x, cell_y;
//make sure that we'll be looking at a legal cell
//轉(zhuǎn)換到地圖坐標(biāo)系，判斷與目標(biāo)點(diǎn)的距離
if (costmap_.worldToMap(x_r, y_r, cell_x, cell_y)) {
double ahead_gdist = goal_map_(cell_x, cell_y).target_dist;
//取距離最小的，放進(jìn)best_traj
if (ahead_gdist < heading_dist) {
//if we haven't already tried rotating left since we've moved forward
if (vtheta_samp < 0 && !stuck_left) {
swap = best_traj;
best_traj = comp_traj;
comp_traj = swap;
heading_dist = ahead_gdist;
}
//if we haven't already tried rotating right since we've moved forward
else if(vtheta_samp > 0 && !stuck_right) {
swap = best_traj;
best_traj = comp_traj;
comp_traj = swap;
heading_dist = ahead_gdist;
}
}
}
}
vtheta_samp += dvtheta;
}
4.3.7、軌跡判斷
在上述幾種情況下的軌跡都考慮完后需要對(duì)整個(gè)軌跡的結(jié)果進(jìn)行判斷：如果說(shuō)軌跡的cost最終大于0，說(shuō)明找到了一條軌跡，則我們會(huì)返回這條軌跡：
//檢查最優(yōu)軌跡的分?jǐn)?shù)是否大于0
if (best_traj->cost_ >= 0) {
// avoid oscillations of in place rotation and in place strafing
//抑制震蕩影響：當(dāng)機(jī)器人在某方向移動(dòng)時(shí)，對(duì)下一個(gè)周期的與其相反方向標(biāo)記為無(wú)效
//直到機(jī)器人從標(biāo)記震蕩的位置處離開(kāi)一定距離，返回最佳軌跡。
//若軌跡的采樣速度≤0
if ( ! (best_traj->xv_ > 0)) {
//若軌跡的角速度<0
if (best_traj->thetav_ < 0) {
if (rotating_right) {
stuck_right = true;
}
rotating_right = true;
} else if (best_traj->thetav_ > 0) {
if (rotating_left){
stuck_left = true;
}
rotating_left = true;
} else if(best_traj->yv_ > 0) {
if (strafe_right) {
stuck_right_strafe = true;
}
strafe_right = true;
} else if(best_traj->yv_ < 0){
if (strafe_left) {
stuck_left_strafe = true;
}
strafe_left = true;
}
//set the position we must move a certain distance away from
prev_x_ = x;
prev_y_ = y;
}
double dist = hypot(x - prev_x_, y - prev_y_);
if (dist > oscillation_reset_dist_) {
rotating_left = false;
rotating_right = false;
strafe_left = false;
strafe_right = false;
stuck_left = false;
stuck_right = false;
stuck_left_strafe = false;
stuck_right_strafe = false;
}
dist = hypot(x - escape_x_, y - escape_y_);
if(dist > escape_reset_dist_ ||
fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_){
escaping_ = false;
}
return *best_traj;
}
若找不到有效軌跡，進(jìn)入逃逸狀態(tài)，后退、原地自轉(zhuǎn)，若找不到下一步的有效路徑則再后退、自轉(zhuǎn)，直到后退的距離或轉(zhuǎn)過(guò)的角度達(dá)到一定標(biāo)準(zhǔn)，才會(huì)退出逃逸狀態(tài)，重新規(guī)劃前向軌跡。其中再加上震蕩抑制。
vtheta_samp = 0.0;
vx_samp = backup_vel_;//后退速度
vy_samp = 0.0;
generateTrajectory(x, y, theta, vx, vy, vtheta, vx_samp, vy_samp, vtheta_samp,
acc_x, acc_y, acc_theta, impossible_cost, *comp_traj);
//if the new trajectory is better... let's take it
/*
if(comp_traj->cost_ >= 0 && (comp_traj->cost_ < best_traj->cost_ || best_traj->cost_ < 0)){
swap = best_traj;
best_traj = comp_traj;
comp_traj = swap;
}
*/
//we'll allow moving backwards slowly even when the static map shows it as blocked
swap = best_traj;
best_traj = comp_traj;
comp_traj = swap;
double dist = hypot(x - prev_x_, y - prev_y_);
if (dist > oscillation_reset_dist_) {
rotating_left = false;
rotating_right = false;
strafe_left = false;
strafe_right = false;
stuck_left = false;
stuck_right = false;
stuck_left_strafe = false;
stuck_right_strafe = false;
}
//only enter escape mode when the planner has given a valid goal point
if (!escaping_ && best_traj->cost_ > -2.0) {
escape_x_ = x;
escape_y_ = y;
escape_theta_ = theta;
escaping_ = true;
}
dist = hypot(x - escape_x_, y - escape_y_);
if (dist > escape_reset_dist_ ||
fabs(angles::shortest_angular_distance(escape_theta_, theta)) > escape_reset_theta_) {
escaping_ = false;
}
//if the trajectory failed because the footprint hits something, we're still going to back up
if(best_traj->cost_ == -1.0)
best_traj->cost_ = 1.0;
return *best_traj;
若后退速度生成的軌跡的終點(diǎn)有效（> -2.0），進(jìn)入逃逸狀態(tài)，循環(huán)后退、自轉(zhuǎn)，并且記錄下的逃逸位置和姿態(tài)，只有當(dāng)離開(kāi)逃逸位置一定距離或轉(zhuǎn)過(guò)一定角度，才能退出逃逸狀態(tài)，再次規(guī)劃前向速度。逃逸判斷和震蕩判斷一樣，也已在上面多次執(zhí)行，只要發(fā)布best_traj前就執(zhí)行一次判斷。
至此，4.3節(jié)最優(yōu)的路徑就已經(jīng)得到了，這個(gè)路徑會(huì)被返回到TrajectoryPlanner::findBestPath函數(shù)，再由這里返回到TrajectoryPlannerROS::computeVelocityCommands函數(shù)中。
4.4、旋轉(zhuǎn)使機(jī)器人到達(dá)目標(biāo)點(diǎn)
前面4.3雖然講了一大通局部路徑規(guī)劃，但是emmmm其實(shí)這里沒(méi)有用到，對(duì)于這種位置到達(dá)但是角度沒(méi)有到的情況，其實(shí)是通過(guò)：
rotateToGoal(global_pose, robot_vel, goal_th, cmd_vel)
來(lái)旋轉(zhuǎn)到最終目標(biāo)點(diǎn)的。這個(gè)函數(shù)的原理是：在達(dá)到目標(biāo)點(diǎn)誤差范圍內(nèi)，且速度降至極小后，最后一步的工作是原地旋轉(zhuǎn)至目標(biāo)姿態(tài)。它采用一種類似“反饋控制”的思想，通過(guò)計(jì)算當(dāng)前姿態(tài)與目標(biāo)姿態(tài)的差值，通過(guò)這個(gè)差值來(lái)控制下一步的角速度。
5、如果機(jī)器人還沒(méi)有到達(dá)目標(biāo)點(diǎn)附近
5.1 軌跡生成
如果機(jī)器人還沒(méi)有到達(dá)目標(biāo)點(diǎn)，那么這里就很簡(jiǎn)單了，肯定是局部規(guī)劃軌跡，也就是前面的4.3節(jié)的全部?jī)?nèi)容：
//若未到達(dá)目標(biāo)點(diǎn)誤差范圍內(nèi)，調(diào)用TrajectoryPlanner類的updatePlan函數(shù)，將global系下的全局規(guī)劃傳入，再調(diào)用findBestPath函數(shù)，進(jìn)行局部規(guī)劃，
//速度結(jié)果填充在drive_cmds中，并得到局部路線plan。
//如果沒(méi)到目標(biāo)“位置”，更新全局規(guī)劃
tc_->updatePlan(transformed_plan);
//compute what trajectory to drive along
//用當(dāng)前機(jī)器人位姿和速度，計(jì)算速度控制命令
//【核心：TrajectoryPlanner的findBestPath函數(shù)】
Trajectory path = tc_->findBestPath(global_pose, robot_vel, drive_cmds);
5.2 最優(yōu)軌跡判斷以及速度獲取
然后判斷代價(jià)值，也就是生成出來(lái)的最優(yōu)的代價(jià)值是否大于零，大于零代表是有效的軌跡，這時(shí)候可以給定速度以及角速度：
cmd_vel.linear.x = drive_cmds.pose.position.x;
cmd_vel.linear.y = drive_cmds.pose.position.y;
cmd_vel.angular.z = tf2::getYaw(drive_cmds.pose.orientation);
這里的drive_cmds來(lái)自于findBestPath中的返回值：
Trajectory TrajectoryPlanner::findBestPath(const geometry_msgs::PoseStamped& global_pose,
geometry_msgs::PoseStamped& global_vel, geometry_msgs::PoseStamped& drive_velocities)
而它的計(jì)算結(jié)果其實(shí)是來(lái)自于最優(yōu)軌跡：
//找到代價(jià)最低的軌跡，輸入分別是目前機(jī)器人位置，速度以及加速度限制
Trajectory best = createTrajectories(pos[0], pos[1], pos[2],
vel[0], vel[1], vel[2],
acc_lim_x_, acc_lim_y_, acc_lim_theta_);//生成諸多可能軌跡，選取其中打分最高的。這里也就是最關(guān)鍵的一步。
ROS_DEBUG("Trajectories created");
drive_velocities.pose.position.x = best.xv_;
drive_velocities.pose.position.y = best.yv_;
drive_velocities.pose.position.z = 0;
tf2::Quaternion q;
q.setRPY(0, 0, best.thetav_);
tf2::convert(q, drive_velocities.pose.orientation);
在createTrajectories中我們對(duì)每一條軌跡采樣時(shí)都有一個(gè)當(dāng)前軌跡使用的速度以及角速度值，它來(lái)自于generateTrajectory的返回：
traj.resetPoints();
traj.xv_ = vx_samp;//線速度
traj.yv_ = vy_samp;
traj.thetav_ = vtheta_samp;//角速度
所以至此我們就得到了一條最優(yōu)軌跡所需要的速度以及角速度。
5.3 發(fā)布局部路徑
在得到速度后，不僅要發(fā)布速度還要發(fā)布局部路徑點(diǎn)位，前面4.3做最優(yōu)路徑規(guī)劃時(shí)得到了最優(yōu)路徑上的連續(xù)點(diǎn)會(huì)在這里被發(fā)布出來(lái)，可以作為可視化的一部分便于調(diào)試，判斷機(jī)器人的運(yùn)動(dòng)是否與規(guī)劃的結(jié)果類似。
總結(jié)：move_base的局部路徑規(guī)劃主要執(zhí)行了以下內(nèi)容：首先獲取機(jī)器人的全局坐標(biāo)，然后生成局部代價(jià)地圖。根據(jù)全局規(guī)劃的結(jié)果判斷當(dāng)前機(jī)器人是否到達(dá)目標(biāo)點(diǎn)，這里分兩種情況考慮：如果位置到達(dá)了但是角度沒(méi)有到達(dá)，則會(huì)調(diào)用rotateToGoal函數(shù)生成一個(gè)旋轉(zhuǎn)的角速度使機(jī)器人的朝向最終到達(dá)目標(biāo)點(diǎn)；如果機(jī)器人連位置都沒(méi)有到達(dá)，則會(huì)調(diào)用findBestPath函數(shù)生成出一條前往目標(biāo)點(diǎn)的最小代價(jià)路徑。最小代價(jià)路徑的生成方式主要是通過(guò)對(duì)機(jī)器人的速度以及角速度分別采樣，再給定一定的采樣周期，判斷這一段時(shí)間內(nèi)的機(jī)器人最終運(yùn)動(dòng)的目標(biāo)點(diǎn)的代價(jià)值是否是所有采樣樣本中最小的，代價(jià)值的計(jì)算公式為：路徑距離+目標(biāo)距離+障礙物代價(jià)（乘以各自的比例）。即這條路徑離目標(biāo)點(diǎn)越近，離全局路徑越近，離障礙物越遠(yuǎn)則它的得分越低，則它是最優(yōu)的一條路徑。然后再將這條采樣路徑的速度角度給到最終的發(fā)布器，就完成了computeVelocityCommands全部工作。
參考
1、Base Local Planner局部規(guī)劃-TrajectoryPlanner源碼解讀-2文章來(lái)源：http://www.zghlxwxcb.cn/news/detail-636079.html
https://blog.csdn.net/neo11111/article/details/104713086/
2、MapGridCostFunction文章來(lái)源地址http://www.zghlxwxcb.cn/news/detail-636079.html
https://www.cswamp.com/post/113
到了這里，關(guān)于move_base代碼解析（四）局部路徑規(guī)劃：TrajectoryPlannerROS::computeVelocityCommands的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
