【移动机器人技术】路径规划技术框架

路径规划技术框架
路径规划单元包含全局路径规划与局部路径规划；在智能轮椅项目中，全局路径规划加入了基于结构化道路的路径规划功能，在此介绍了此部分的实现框架，重点描述了move_base中对此的处理流程；在局部路径规划中，增加和修改了运动策略机制，包含遇到障碍物的处理方式，修改了DWA的评分准则；增加了基于lattice局部路径规划的实现方法和原理。

1 全局路径规划

全局路径规划包含两种模式：
自由规划和基于规则道路的规划；

1.1 自由全局路径规划

基于Dijkstra路径搜索算法的路径规划，将障碍物地图作为对象，完成路径搜索。路径搜索分为两部分，正向蔓延和反向梯度搜索。

• 正向蔓延
  将当前位置点作为起点，采用逐层扩散的方式向外蔓延，并记录每个栅格的cost值，cost包含障碍物代价值和蔓延走过的路径；此过程一直到寻找到目标点；
• 反向梯度搜索
  当正向蔓延过程完成后，生成了覆盖起始点和目标点的cost片图。反向梯度法将目标点作为起点，利用梯度下降方式，在cost片图上搜索目标点到起点的路线。将搜索到的路线做反向处理，就得到从起点到目标点的综合最优路线。

1.2 基于规则道路的规划

基于规则道路规划，是指根据实际的运行状况需要，在地面上设定好单向/双向路线，实际工作时将以这些线路作为全局路径。
规则道路由基本道路片段和节点组成，道路片段是直线和圆弧的基础线条，也可以是根据实际运行轨迹采集到的点；节点将基础道路片段连接起来，构成了拓扑路径结构。

• 拓扑道路结构创建
  在初始化阶段，创建拓扑道路地图结构，通过单向路径将所有的节点关联起来，并赋值不同的基础道路片段以不同的权重。
• 道路搜索
  当道路存在时，可以搜索出基于起点到目标点的基础道路编号序列，将所有的这些基于道路片段拼接成完整的道路路径后发布，可作为全局参考路径。
• move_base处理规则道路
  对move_base进行了改进，保留原有的接收单一目标点的路径规划功能基础上，增加了可遵循参考轨道运动的功能。
  -订阅参考轨道话题-
  在move_base中新增加回调函数goalCB_path，将订阅到的路径点加入到本地变量planner_goal_vector_中；
  -新参考路径点管理-
  将订阅到的轨迹点做直线插补处理，将增加路径点之间的密集度。

makePlan_line(temp_goal_vector[i-1], temp_goal_vector[i], section_path);

再将插补后的稠密片段路径拼接成整条路径allPathPose_plan；
-参考轨道序列点管理-
计算机器人距离路径的最短距离；
基于上一周期的路径投影点，沿着机器人前进方向寻找局部最近点，或者沿着相反方向找到局部最近点，用局部最近近似全局最近。

double d_robot = findPathRobotNearestIndex_and_distance(allPathPose_plan);

机器人偏离路径远，则规划衔接路线；
当机器人距离路径的最短距离查出一段距离时，启动自由全局路径规划功能，规划出当前位置到前瞻点的全局路径。自由规划的目标点是前瞻点，而非最近距离点，原因是前瞻点考虑了自由规划与规则道路片段衔接时的朝向角度变化不会太大。

寻找前瞻点
forword_next_index = findPathForwardBestIndex(allPathPose_plan);

自由规划
plan_all_ok = makePlan(allPathPose_plan[forword_next_index], section_path);

自由规划与规则道路片段的衔接
planner_plan_->insert(planner_plan_->end(),section_path.begin(), section_path.end());

将最终路径赋值给planner_plan_，此变量作为全局变量，供发布给Rviz显示，以及供局部路径规划参考。

2 局部路径规划

局部路径规划有两种方法，基于DWA思路的base_local_palnner模块，参考autoware的lattice曲线规划模块。

2.1 base_local_palnner

局部路径规划单元所参考的全局路径仅为局部地图截取的部分路径。DWA是一种基于动作空间采样算法，基于当前的速度状态，根据运动学约束（最大速度、最大加速度），确定采样窗口，然后对窗口里面的每一个样本曲线进行评分，去除非法曲线（碰撞到障碍物，或者不再地图范围内，由于DWA基于局部代价地图进行评分，所以代价地图尺寸过小会影响DWA规划出的最大速度），剩下就是可以具有正评分值的曲线，最终挑选出评分值最高的曲线，最为本次局部路径规划的输出。

createTrajectories

实现速度采样窗口的确定，包含机器人四周压障碍物检测、速度限制、采样分辨率设定、防止震荡、倒退处理机制等。

• 四周压障碍物检测
  根据机器人的尺寸，分别定义位于机器人四周的四个封闭轮廓footprint_front_、footprint_left_、footprint_right_、footprint_back_，用来检测机器人对应边是否压到了障碍物。
• 速度限制
  速度限制因素包含：
  最大速度的限制：

max_vel_x = max_vel_x_;
min_vel_x = min_vel_x_;

预留刹车距离的速度限制：

// 0 < k_smooth <= 1.0， 减速停止的平滑参数；
vel_stop = sqrt(2.0*final_goal_dist*acc_x*k_smooth);
max_vel_x = min(max_vel_x, vel_stop);

最大加速度限制：

max_vel_x = min(max_vel_x, vx + acc_x * sim_time_);
min_vel_x = max(min_vel_x, vx - acc_x * sim_time_);

基于单边压障碍物的限制：
压到障碍物的边，禁止朝该方向运动，仅可以朝着向反方向运动；某一边距离障碍物很近，则限制该方向的运行速度。

// 压到障碍物的边，禁止该方向运动；
if (foot_front_cost_ < 0)
{
    foot_front_hit_ = true;
    max_vel_x = 0.0;
    printf("HF hit front \n");
}
// 没有压到障碍物，则根据障碍距离做一定的限制速度处理；
else
{
    if (foot_front_cost_ > obstacle_limit_max)
    {
        max_vel_x = v_limit_min;
    }
    else if (foot_front_cost_ > obstacle_limit_min)
    {
        double k = 1.0 - (foot_front_cost_ - obstacle_limit_min)
                            /(obstacle_limit_max - obstacle_limit_min);
        max_vel_x = k*(max_vel_x - v_limit_min) + v_limit_min;
    }
}

基于运动方向的速度限制：
根据全局路径的前n（20）个点确定参考的运动方向，当机器人速度与参考运动方向角度偏差较大时，限制运动速度：大偏差角度（调头）时，禁止前进；目标路径在左，禁止右转；目标路径在右，禁止左转；

• 原地停止基准分数
  当采样产生的轨迹分值，小于原地停止的分数，则放弃该轨迹。

• 防止震荡处理
  防止机器人在探索过程中出现反馈左右震荡，或者反复前后震荡。需要做一个震荡锁定和解锁机制，当机器人进入震荡锁定状态后，只有在该方向一定的距离后，在可以解锁，防止频繁的原地震荡。

• 前进状态的速度窗口
  遍历所有vx，基于某采样速度vx_sample，先进行直线采样，然后遍历基于该线速度vx_sample的所有角速度；

• 原地旋转速度采样
  评估原地旋转后，再基础朝向上预测一段距离，评估轨迹的代价值。

• 后退机制
  当前进、原定转向都不存在合理的轨迹时，则进入后退锁定状态；一旦进入后退锁定机制后，必须运动一定距离或者机器人前方空旷时，才可以退出后退锁定状态。

generateTrajectory

基于某动作采样点产生预测轨迹，并计算其cost值。

• 确定迭代步数
  轨迹预测是基于采样动作和当前状态，迭代n步运动后形成的一条轨迹。如何合理的设定预测过程需要迭代的步数非常重要。
  首先，近似根据当前运动状态和轨迹所需的距离、角度分辨率，预测时间长度固定不变，确定采样步数。

double vmag = hypot(vx_samp, vy_samp);
num_steps = int(max((vmag * sim_time_) / sim_granularity_, fabs(vtheta_samp * sim_time_) / angular_sim_granularity_) + 0.5);

其次，在采样动作为原地旋转时，如果时间预测长度为固定至，则基于当前速度运动预测时间，轨迹末端点的机器人朝向将可能超越目标朝向，其轨迹抹点的cost值就会变差，甚至可能碰撞障碍物。所以，原地旋转时将根据目标角度与当前角度的偏差，及采样角速度大小，确定预测时间长度，目的是基于当前采样角速度运行预测时间后，机器人不会超出目标角度。

if (fabs(vx_samp) < 0.01)
{
    double small_error = angles::shortest_angular_distance(goal_th, theta);
    double left_error = small_error < 0 ? fabs(small_error) : small_error - 2.0*3.1415926;
    double right_error = 2.0*3.1415926 - left_error;
    double t_modify = vtheta_samp > 0
                    ? left_error/max(0.1, max_vel_theta_real_)
                    : right_error/fabs(min(-0.1, min_vel_theta_real_));
    num_steps_modify = ceil(t_modify/sim_time_*(double)num_steps);
    num_steps_modify = min(max(num_steps_modify, num_steps), 5*num_steps);  // 超参，default:2
    num_steps = num_steps_modify;
}

最后计算采样时间间隔

double dt = sim_time_ / num_steps;

• 障碍碰撞检测机制
  默认机制是机器人在预测轨迹上与障碍物发生碰撞，则该轨迹失效；该方法存在明显缺点，机器人的轮廓、障碍物尺寸相比实际障碍物都会有一定程度的膨胀、扩大，当障碍物贴近机器人时很容易判断成机器人某边沿已经压到了障碍物，这中情况下无论什么阳的预测轨迹，起点总是会碰撞障碍物，这样就不存在可行轨迹，机器人就无法运动。
  为了解决这个问题，采用了改进机制。若果原先轮椅某侧边缘已经压到了障碍物，则不对该边缘进行判断；仅判断原本没有压到障碍物的边，在产生新轨迹的运动过程中压到障碍物，则此动作采样生成的轨迹失效。
  此方法缺点：如果左侧原先压到了障碍物，则继续向左运动不会发生运动失效的判断，此时碰撞检测会失效；
  已经改进的方法：在DWA生成速度窗口的函数中，根据机器人周边压障碍物的判断结果，限制对应方向上的速度输出。

• 轨迹评分
  轨迹评分包含以下项目：
  轨迹距离障碍物的最近距离：即本轨迹所有点及对应的机器人轮廓点中，障碍物occ_cost最大值;
  横向偏差：轨迹点指定位置处（末端）点坐标距离全局路径的横向偏差(最短距离)；
  目标距离：轨迹点指定位置处（末端）点坐标距离局部地图截取的全局路径末端点的距离；
  朝向偏差：将轨迹末端点投影至全局路径，根据全局路径上投影点的前后点坐标关系，计算出投影点处的参考角度，计算轨迹末端朝向与参考角度的偏差；

2.2 参考autoware的lattice曲线规划模块

在autoware中的局部路径规划主要负责将全局路径规划出的路径拟合成车辆可以运动的lattice曲线，满足平滑连续、最大曲率、最大转向速度等约束。
lattice处理的基本流程包括：将车辆当前位置投影至全局路径，将投影点作为起点，沿着运动方向前进一段距离（当前速度乘以预测时间长度）选取末端点；如果在选取末端点的过程中遇到了障碍物，则需要在障碍物前面的一段安全距离内停止，末端点位停止点，末端速度为0；将末端点根据道路上的信息，转换成车辆的末端状态；根据车辆当前状态和末端目标状态，生成一种保行的lattice曲线，曲线横坐标表示纵向路程，纵坐标表示曲率信息，当前线速度根据起点速度、末端速度决定，根据速度和曲率可以计算出角速度；这样可以根据lattice曲线生成线速度和角速度信息，可构成标准的twist格式。

2.2.1 关于lattice规划的调用及结果发布

TrajectoryPlannerROS::call_lattice_traj

调用lattice规划器产生局部参考路径的过程分为如下：
首先，根据机器人当前状态global_pose、robot_vel赋值给lattice中的当前状态变量current_pose_、current_twist_；
其次，判断路径上是否有障碍物，获取障碍物处或者终点的index，作为lattice规划的目标点索引；
最终，将全局路径和目标点索引作为参数传递给lattice_planner_，调用函数产生lattice曲线；
此函数调用后，将产生可以产生理想轨迹的lattice曲线参数。在lattice_cmd_vel_Gen_thread线程中，负责调用实现基于曲线参数产生速度指令。

TrajectoryPlannerROS::lattice_cmd_vel_Gen_thread

这是一个定时发布cmd_vel话题的线程，在trajectory_planner工作过程中，该线程固定周期发布话题，当trajectory_planner不被调用后，该线程将停止发布话题。机器人运动控制部分在收不到速度话题时，将停止运动。
当lattice规划成功时，选择发送lattice规划结果；当lattice规划失败时，选择发送DWA结果。之所以采用这种二选一的机制，是因为lattice是一种高阶曲线拟合的机制去跟随全局路径，当全局路径形状不太规则的情况下，lattice会失效，此时为了有备选的速度规划机制，就调用了备选的DWA作为局部路径规划，当DWA失效时其内部机制是输出零速。

2.2.2 关于lattice规划的基本原理

在genSplineFromPath函数中调用了lattice曲线生成过程。

• 寻找机器人在全局路径上的最近投影点
  寻找到投影点后，就可以确定机器人距离全局路径的最短距离，投影点至路径剩余部分的路径信息。如下情况不满足lattice规划条件：
  若距离最近点的距离较大，即横向偏差较大；
  如果全局路径剩余点数过少，剩余路段不规则的概率就很大；
  路径剩余距离过小，或则路径增量中的相对坐标系下dx相对于dy较小，即纵向移动距离相比横向移动距离不够大；
  若路径末端的朝向角度变化大，说明整个路径的曲率变化过大。

• 全局参考路径平滑
  从最近投影点至前瞻预测点之间的全局路径段，做平滑处理，降低全局路径采样引入的噪声，增加lattice曲线的平滑性；
  平滑中曲线的表述形式为x(s)、y(s)，及横纵坐标独立，都是关于s的因变量，这样做的好处是避免超函数的出现，比如圆，降低函数表述难度。
  当全局路径曲线剩余点数较多(不小于5)时，采用三次多项式拟合全局路径，并在原先路径对应的纵向距离s(i)处进行路点采样。

• 路点格式转换
  将路径点上附加参考速度信息，转换位traj轨迹格式。

• 障碍物前停车处理
  当参考轨迹上有障碍物时，需要在障碍物前安全停车，并给减速度段路径赋值合理的速度参考值。

• 寻找前瞻点index
  从投影点开始，根据轨迹点上的参考速度运动一段时间后达到的轨迹点，或者停止点，选两者较近的点作为前瞻点，获取其索引号。

• 计算目标轨迹点的车辆目标状态
  前瞻点即局部路径规划的目前参考点，将其投影至车载坐标系下，根据该轨迹点的坐标、朝向、曲率、速度信息，转换为车辆的目标状态。

• 计算车辆当前状态
  根据反馈信息，计算在车载坐标系下的当前车辆状态值。

• 计算lattice曲线参数
  根据车辆的当前状态和目标状态，计算lattice曲线参数，使得参数的起点和终点可以拟合当前、目标状态。