使用梯度下降和缓冲平滑确定的路径？ [MATLAB]

Question

我正在编写一个算法来使用MATLAB规划自动流动站的路径，并且在路径上有很多问题。现在我生成了一系列航点（134点），这样我的机器人就不会碰到任何障碍物并从起点到终点采取最短路径。生成（非平滑）路径如下：

1.1000    1.0000
1.2000    1.0000
1.3000    1.0000
1.4000    1.0000
1.5000    1.0000
1.6000    1.0000
1.7000    1.0000
1.8000    1.0000
1.9000    1.0000
2.0000    1.0000
2.1000    1.0000
2.2000    1.0000
2.3000    1.0000
2.4000    1.0000
2.5000    1.0000
2.6000    1.0000
2.7000    1.0000
2.8000    1.0000
2.9000    1.0000
3.0000    1.0000
3.1000    1.0000
3.2000    1.0000
3.3000    1.0000
3.4000    1.0000
3.5000    1.0000
3.6000    1.0000
3.7000    1.0000
3.8000    1.0000
3.9000    1.0000
4.0000    1.1000
4.1000    1.1000
4.2000    1.1000
4.3000    1.1000
4.4000    1.1000
4.5000    1.1000
4.6000    1.1000
4.7000    1.1000
4.8000    1.2000
4.9000    1.3000
5.0000    1.4000
5.1000    1.5000
5.2000    1.6000
5.3000    1.7000
5.4000    1.8000
5.5000    1.9000
5.6000    2.0000
5.6000    2.1000
5.6000    2.2000
5.6000    2.3000
5.6000    2.4000
5.6000    2.5000
5.6000    2.6000
5.6000    2.7000
5.6000    2.8000
5.6000    2.9000
5.6000    3.0000
5.6000    3.1000
5.6000    3.2000
5.6000    3.3000
5.6000    3.4000
5.6000    3.5000
5.6000    3.6000
5.6000    3.7000
5.6000    3.8000
5.6000    3.9000
5.6000    4.0000
5.6000    4.1000
5.6000    4.2000
5.6000    4.3000
5.6000    4.4000
5.6000    4.5000
5.6000    4.6000
5.6000    4.7000
5.6000    4.8000
5.6000    4.9000
5.6000    5.0000
5.6000    5.1000
5.6000    5.2000
5.6000    5.3000
5.6000    5.4000
5.6000    5.5000
5.6000    5.6000
5.6000    5.7000
5.6000    5.8000
5.6000    5.9000
5.6000    6.0000
5.6000    6.1000
5.6000    6.2000
5.6000    6.3000
5.6000    6.4000
5.6000    6.5000
5.6000    6.6000
5.6000    6.7000
5.6000    6.8000
5.6000    6.9000
5.6000    7.0000
5.6000    7.1000
5.6000    7.2000
5.6000    7.3000
5.6000    7.4000
5.6000    7.5000
5.6000    7.6000
5.6000    7.7000
5.6000    7.8000
5.6000    7.9000
5.6000    8.0000
5.6000    8.1000
5.6000    8.2000
5.7000    8.3000
5.8000    8.4000
5.9000    8.5000
6.0000    8.6000
6.1000    8.7000
6.2000    8.8000
6.3000    8.9000
6.4000    9.0000
6.5000    9.1000
6.6000    9.2000
6.7000    9.3000
6.8000    9.4000
6.9000    9.5000
7.0000    9.6000
7.1000    9.6000
7.2000    9.6000
7.3000    9.6000
7.4000    9.6000
7.5000    9.6000
7.6000    9.6000
7.7000    9.6000
7.8000    9.6000
7.9000    9.6000
8.0000    9.6000
8.1000    9.6000
8.2000    9.6000
8.3000    9.6000
8.4000    9.6000
8.5000    9.5000
8.6000    9.4000
8.7000    9.3000
8.8000    9.2000
8.9000    9.1000
9.0000    9.0000
9.1000    9.0000

附图显示了我用于梯度下降的数学关系，其中β= 0.5且γ= 0.1。当我在我的代码中应用这些关系来获取新路径时，我的新路径完全忽略了约束并在错误的点开始/结束。我也不知道如何在障碍物周围实现缓冲区（缓冲区= 0.25），所以我真的很感激任何帮助！

路径平滑代码可以在这里看到：

newPath = zeros(length(path),2);
newPath(1,:) = path(1,:);
newPath(end,:)=path(end,:);

for i = 2:length(path)-1
newPath(i,:) = newPath(i,:)+0.5*(path(i,:)-newPath(i,:))+0.1*(newPath(i-1,:)-2*newPath(i,:)+newPath(i+1,:));
end

我的代码生成的newPath是：

1.1000    1.0000
0.7100    0.6000
0.7210    0.5600
0.7721    0.5560
0.8272    0.5556
0.8827    0.5556
0.9383    0.5556
0.9938    0.5556
1.0494    0.5556
1.1049    0.5556
1.1605    0.5556
1.2160    0.5556
1.2716    0.5556
1.3272    0.5556
1.3827    0.5556
1.4383    0.5556
1.4938    0.5556
1.5494    0.5556
1.6049    0.5556
1.6605    0.5556
1.7160    0.5556
1.7716    0.5556
1.8272    0.5556
1.8827    0.5556
1.9383    0.5556
1.9938    0.5556
2.0494    0.5556
2.1049    0.5556
2.1605    0.5556
2.2160    0.6056
2.2716    0.6106
2.3272    0.6111
2.3827    0.6111
2.4383    0.6111
2.4938    0.6111
2.5494    0.6111
2.6049    0.6111
2.6605    0.6611
2.7160    0.7161
2.7716    0.7716
2.8272    0.8272
2.8827    0.8827
2.9383    0.9383
2.9938    0.9938
3.0494    1.0494
3.1049    1.1049
3.1105    1.1605
3.1110    1.2160
3.1111    1.2716
3.1111    1.3272
3.1111    1.3827
3.1111    1.4383
3.1111    1.4938
3.1111    1.5494
3.1111    1.6049
3.1111    1.6605
3.1111    1.7160
3.1111    1.7716
3.1111    1.8272
3.1111    1.8827
3.1111    1.9383
3.1111    1.9938
3.1111    2.0494
3.1111    2.1049
3.1111    2.1605
3.1111    2.2160
3.1111    2.2716
3.1111    2.3272
3.1111    2.3827
3.1111    2.4383
3.1111    2.4938
3.1111    2.5494
3.1111    2.6049
3.1111    2.6605
3.1111    2.7160
3.1111    2.7716
3.1111    2.8272
3.1111    2.8827
3.1111    2.9383
3.1111    2.9938
3.1111    3.0494
3.1111    3.1049
3.1111    3.1605
3.1111    3.2160
3.1111    3.2716
3.1111    3.3272
3.1111    3.3827
3.1111    3.4383
3.1111    3.4938
3.1111    3.5494
3.1111    3.6049
3.1111    3.6605
3.1111    3.7160
3.1111    3.7716
3.1111    3.8272
3.1111    3.8827
3.1111    3.9383
3.1111    3.9938
3.1111    4.0494
3.1111    4.1049
3.1111    4.1605
3.1111    4.2160
3.1111    4.2716
3.1111    4.3272
3.1111    4.3827
3.1111    4.4383
3.1111    4.4938
3.1111    4.5494
3.1611    4.6049
3.2161    4.6605
3.2716    4.7160
3.3272    4.7716
3.3827    4.8272
3.4383    4.8827
3.4938    4.9383
3.5494    4.9938
3.6049    5.0494
3.6605    5.1049
3.7160    5.1605
3.7716    5.2160
3.8272    5.2716
3.8827    5.3272
3.9383    5.3327
3.9938    5.3333
4.0494    5.3333
4.1049    5.3333
4.1605    5.3333
4.2160    5.3333
4.2716    5.3333
4.3272    5.3333
4.3827    5.3333
4.4383    5.3333
4.4938    5.3333
4.5494    5.3333
4.6049    5.3333
4.6605    5.3333
4.7160    5.2833
4.7716    5.2283
4.8272    5.1728
4.8827    5.1173
4.9383    5.0617
5.9038    5.9062
9.1000    9.0000

Answer 1

初始化newPath = path，然后重复if循环几千次。

Answer 2

我有同样的问题，我无法弄明白。然后我最终写Java code来形象化它。