ROS导航使用贝塞尔曲线对全局路径进行平滑处理

前言

ROS原生的全局路径规划GlobalPlanner包含A*和Dijkstra，两者原理基本相同，能够规划出从起点到终点的路径，但是由于栅格地图存在锯齿形，得到的全局路径也会出现“折弯”，不够平滑的现象，虽然不影响导航的使用，但对于路径跟踪来讲，会存在运动不够平滑的情况，因此本文将会使用贝塞尔曲线对globalplanner规划的路径进行处理后给到局部路径，供导航使用。

一、贝塞尔曲线的使用

网上关于对贝塞尔曲线的介绍有很多，这里仅仅做一个简单的介绍。
贝塞尔曲线原理
贝塞尔曲线常用于绘制曲线，具有以下特征：

1. 使用n个控制点{p1,p2,…,pn}来控制曲线的形状；
2. 曲线通过起点平p1和终点pn，接近但不通过中间点

一阶贝塞尔曲线：给定点P0，P1，此时贝塞尔曲线只是两点之间的一条直线，描述为：
$$B(t)=P0+(P1-P0)t=(1-t)P0+tP1,t\in [0,1]$$
二阶贝塞尔曲线：给定点P0，P1，P2，贝塞尔曲线描述为：
$$B(t)=(1-t{)}^{2}P0+2t(1-t)P1+t^{2}P2,t\in [0,1]$$

二、全局路经修改

ROS运动规划学习五介绍过全局路径的生成过程，是由planer_core文件中makePlan函数实现，因此在可以在makePlan函数中进行路径平滑。
步骤如下：

1. 声明定义二阶和三阶贝塞尔曲线函数。

//二阶贝塞尔曲线
float GlobalPlanner::bezier2func(float uu,float* controlPoint){  
   float part0 = controlPoint[0] * (1-uu) * (1-uu);  
   float part1 = 2 * controlPoint[1] * uu * (1-uu);  
   float part2 = controlPoint[2] * uu * uu ;     
   return part0 + part1 + part2 ;   
}
//三阶贝塞尔曲线
float GlobalPlanner::bezier3func(float uu,float* controlPoint){  
   float part0 = controlPoint[0] * (1-uu) * (1-uu) * (1-uu);  
   float part1 = 3 * controlPoint[1] * uu * (1-uu) * (1 - uu);  
   float part2 = 3 * controlPoint[2] * uu * uu * (1 - uu);  
   float part3 = controlPoint[3] * uu * uu * uu;     
   return part0 + part1 + part2 + part3;   
}  

2. 全局路径处理，获取要平滑的路径点。由于全局路径上的点太过密集，为减少计算量，这里每隔distance个点取一个点作为放入要平滑的路径，同时将路径终点也放入。

void GlobalPlanner::deal_path(const nav_msgs::Path& input,nav_msgs::Path& output,int distance){
    output.header.stamp = ros::Time::now();
    output.header.frame_id = "map";
    output.poses.clear();
    geometry_msgs::PoseStamped pathVehicle;
    int length = input.poses.size();
    for (int i = 0; i < (length / distance); i++) {
        pathVehicle.header.frame_id = "map";
        pathVehicle.header.stamp = ros::Time::now();
        pathVehicle.pose.position.x = input.poses[i*distance].pose.position.x;
        pathVehicle.pose.position.y = input.poses[i*distance].pose.position.y;
        pathVehicle.pose.position.z = 0;
        pathVehicle.pose.orientation.x = input.poses[i*distance].pose.orientation.x;
        pathVehicle.pose.orientation.y = input.poses[i*distance].pose.orientation.y;
        pathVehicle.pose.orientation.z = input.poses[i*distance].pose.orientation.z;
        pathVehicle.pose.orientation.w = input.poses[i*distance].pose.orientation.w;
        output.poses.push_back(pathVehicle);
    }
    pathVehicle.header.frame_id = "map";
    pathVehicle.header.stamp = ros::Time::now();
    pathVehicle.pose.position.x = input.poses[length-1].pose.position.x;
    pathVehicle.pose.position.y = input.poses[length-1].pose.position.y;
    pathVehicle.pose.position.z = 0;
    pathVehicle.pose.orientation.x = input.poses[length-1].pose.orientation.x;
    pathVehicle.pose.orientation.y = input.poses[length-1].pose.orientation.y;
    pathVehicle.pose.orientation.z = input.poses[length-1].pose.orientation.z;
    pathVehicle.pose.orientation.w = input.poses[length-1].pose.orientation.w;
    output.poses.push_back(pathVehicle);
}

3. 使用贝塞尔曲线函数对2中得到的路径进行平滑，并创建新的全局路径。

void GlobalPlanner::createCurve(const nav_msgs::Path& originPoint,nav_msgs::Path& output){

}

三、结果对比

图中蓝色曲线为原始路径，红色曲线为平滑后的路径，能够看出，相比于原始路径存在折弯的部分，贝塞尔曲线处理后的路径更加平滑。