Smoothing a determined path using gradient descent and buffers? [MATLAB]

Question

I'm writing an algorithm to plan a path for an autonomous rover using MATLAB, and am having a lot of issue smoothing the path. Right now I generate a series of waypoints (134 points) so that my robot does not hit any obstacles and takes the shortest path from the start to end point. This generated (non-smoothed) path is as follows:

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

The attached image shows the mathematical relations I am using for the gradient descent, where beta = 0.5 and gamma = 0.1. When I apply these relations in my code to get the new path, my new path completely ignores the constraints and starts/ends at the wrong point. I also have no idea how to implement a buffer around obstacles (buffer = 0.25), so I would really appreciate any help!

The path smoothing code can be seen here:

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

The newPath my code generates is:

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

Well I'm quite certain we're in the same class. I think the profs equation might have a typo. I tried yours in a couple of different combinations and got really obscure results. Right off the bat I can tell you that the equation your using does not match the notes (but even if it did it still doesn't work). I'm going to office hours tm (10:30am) to clear this all up. I recommend you do the same. — Ben B, Oct 18 '16 at 20:49
Unfortunately I have to work during office hours, so I can never get to them. It's killing me :/ — , Oct 19 '16 at 23:13

score 0 · Answer 1 · answered Oct 19 '16 at 16:43

0

Initialize newPath = path, then re-iterate the if loop a few thousand times.

answered Oct 19 '16 at 16:43

Ben B

1

Yeah, I figured it out late last night! – Oct 19 '16 at 23:01

score 0 · Answer 2 · answered Jan 16 '18 at 17:08

0

I had the same issue, I was unable to figure out. Then I end up writing Java code to visualize it.

answered Jan 16 '18 at 17:08

M Faisal Hameed

673
1
7
25

Smoothing a determined path using gradient descent and buffers? [MATLAB]

2 Answers2