一种使 Dijkstra A* 规划出来的路径更平滑的方法

    我们知道一般用Dijkstra A* 等路径规划算法规划出来的路径都不是很平滑的   机器人沿着这些不平滑路径驾驶，效果差强人意，为了时机器人能自然的沿着给定路径行驶，必须对路径点做平滑处理

如上图  S表示路线规划的起点， E表示路线规划的重点 ，蓝色的线为路线规划 Dijkstra A*规划出来的路路线，但在实际行驶中，车辆不可能遇到大拐弯原地停下后转个90度的大弯，不仅可操作性不强，对于乘客也是非常危险的行为。在实际的行驶中，我们更希望车辆的路线如平滑处理后橙色线路，非常平缓平稳连贯的从S点行走到终点E。

 

假设路线规划的结果为点序列:[x1,x2,…xn] 
平滑后路线规划的点序列:[y1,y2,…yn] 

路线平滑的过程本质上是如下2种距离最小

<1 > 求解y(i)使得||x(i) - y(i)|| 的平方最小 (平滑后的点yi与原始点xi的偏离程度）

<2> 同时y(i)满足 || y(i) - y(i+1)||   (平滑后 点之间的距离)取值最小

 

即 最小化目标 alpha*||x(i) - y(i)||平方 + beta*|| y(i) - y(i+1)||  

求解最优解的方法采用梯度下降法(gradient descent)，即通过多次迭代，调整y(i) 使得目标函数取得最小值。

初始化 y(i) = x(i)

迭代 y(i) := y(i) + alpha * [ x(i) - y(i) ] + beta * [y(i-1) - 2*y(i) + y(i + 1)]

matlab 代码：

clc;
clear;
% (x,y)一系列离散点 构成路径原始
x = [1 2 3 4 5 5 5 5 6 7 8 9 10 10 10 10 11 12 13 14 15 16 16 17];
y = [7 7 7 7 7 6 5 4 4 4 4 4 4 3 2 2  1 1 1 2 2 3 3 4];
alpha = 0.5; %
beta = 0.5; % 平滑程度

xi = x;% 初始化
yi = y;
% 迭代 8 次
 for k=1:8
    for i = 2:1:(length(x)-1) % 不优化起始点S 和 终点 E
       xi(i) = xi(i) + alpha*(x(i) - xi(i)) + beta*(xi(i-1) - 2*xi(i) + xi(i+1));
       yi(i) = yi(i) + alpha*(y(i) - yi(i)) + beta*(yi(i-1) - 2*yi(i) + yi(i+1));
    end
end
plot(x,y,xi,yi)
grid on

调节alpha beta参数 达到不同程度的平滑