勾配降下とバッファを使用して決定されたパスを平滑化しますか？ [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 =パス、再度反復千かのループ数回。

Answer 2

私は同じ問題を抱えていました。私は理解できませんでした。それから私はJava codeと書いてそれを視覚化します。