2022-01-31<\/p>\n
\u8def\u5f84\u89c4\u5212\u4e0e\u4f18\u5316\u5b66\u4e60\u7cfb\u5217\uff08\u4e00\uff09\u2014\u8def\u5f84\u89c4\u5212\u7b97\u6cd5
\n\u524d\u8a00
\n\u51e0\u4e2a\u6708\u6765\u6d51\u6d51\u5669\u5669\uff0c\u4eba\u751f\u8fd9\u5f20\u5730\u56fe\u5b9e\u5728\u592a\u5927\u4e86\uff0c\u987f\u65f6\u89c9\u5f97\u4eba\u751f\u4e4b\u8def\u969c\u788d\u91cd\u91cd\u3001\u8ff7\u832b\u4e0d\u6e05\uff0c\u6545\u6b64\u53d7\u4eba\u542f\u53d1\uff0c\u4e00\u5b66\u8def\u5f84\u89c4\u5212\u4e4b\u6cd5\uff0c\u4ee5\u89e3\u5fc3\u5934\u4e4b\u56f0\uff0c\u4ee5\u4fbf\u627e\u5230\u5bfb\u627e\u6700\u4f18\u4e4b\u4eba\u751f\u8def\uff08\u7ed3\u5c3e\u6709\u4eba\u751f\u4e4b\u609f\uff09
\n\u672c\u6587\u4ec5\u7528\u4e8e\u7b14\u8bb0\u548c\u56de\u987e\uff0c\u5b66\u4e60\u6765\u6e90\u5982\u4e0b\u4ecb\u7ecd
\n\u5b66\u4e60\u76ee\u6807
\n\u5165\u95e8\u8def\u5f84\u89c4\u5212\u601d\u60f3\u6df1\u5165\u4e86\u89e3\u7ecf\u5178\u8def\u5f84\u89c4\u5212\u601d\u60f3\u6df1\u5165\u4f53\u4f1a\u5404\u79cd\u7ecf\u5178\u7b97\u6cd5\u7684\u9002\u7528\u573a\u666f\u6df1\u5165\u7406\u89e3\u7ecf\u5178\u7b97\u6cd5\u7684\u4ee3\u7801\u5b9e\u73b0\u6cdb\u8bfb\u8bba\u6587\uff0c\u63a2\u7d22\u7b97\u6cd5\u6539\u8fdb\u70b9
\n\u5b66\u4e60\u6765\u6e90
\n B\u7ad9\uff08IR\u827e\u82e5\u673a\u5668\u4eba\uff09\u673a\u5668\u4eba\u8def\u5f84\u89c4\u5212\u3001\u8f68\u8ff9\u4f18\u5316\u7cfb\u5217\u8bfe\u7a0b_\u54d4\u54e9\u54d4\u54e9_bilibili \u4e2d\u56fd\u77e5\u7f51\u3001SCI\u3001ieee\u8bba\u6587\u5e93
\n\u6b63\u6587
\n\u4e00\u3001\u8def\u5f84\u89c4\u5212\u601d\u60f3\u6982\u8ff0
\n\u200b \u8fd1\u5e74\u6765\uff0c\u8def\u5f84\u89c4\u5212\u7b97\u6cd5\u9ad8\u6548\u53d1\u5c55\uff0c\u5f97\u5230\u5e7f\u6cdb\u5e94\u7528\u3002\u4ee5\u65e0\u4eba\u673a\u8def\u5f84\u89c4\u5212\u4e3a\u4f8b\uff0c\u4e3b\u8981\u5305\u62ec
\n1.\u57fa\u4e8e\u98de\u884c\u533a\u57df
\nPRM\u6982\u7387\u8def\u5f84\u56fe\u6cd5\uff08\u907f\u514d\u78b0\u649e\uff09\u5206\u533a\u89c4\u5212+K-means+\u6a21\u62df\u9000\u706b\uff08\u590d\u6742\u7ea6\u675f\u4e0b\u591a\u4efb\u52a1\u591a\u673a\uff09\u4eba\u5de5\u52bf\u573a\uff08\u89e3\u51b3\u76ee\u7684\u70b9\u65e0\u6cd5\u51c6\u786e\u5230\u8fbe\uff09A*\u5b9a\u957f\u641c\u7d22\uff08\u8d77\u59cb\u70b9\u786e\u5207\u8def\u5f84\uff09voronoi\u52a0\u6743\u65b9\u5411\u56fe+\u8681\u7fa4\u7b97\u6cd5\uff08\u9ad8\u6548\u907f\u969c\uff09
\n2.\u57fa\u4e8e\u4efb\u52a1\u65f6\u95f4
\n\u65f6\u95f4\u6700\u4f18+\u670d\u52a1\u7ed3\u70b9\u6700\u5927\u5316\u901a\u4fe1\u4f18\u5316\u6846\u67b6
\n3.\u57fa\u4e8e\u80fd\u8017\u4f18\u5316
\nA*+\u7c92\u5b50\u7fa4\u4f18\u5316\uff08\u8def\u7ebf\u6700\u4f18\u3001\u964d\u4f4e\u80fd\u8017\uff09\u591a\u673a\u8054\u5408+\u6bd4\u7279\u5206\u914d\uff08\u6ee1\u8db3quality\u5e76\u4e14\u964d\u4f4e\u80fd\u8017\uff09
\n4.\u57fa\u4e8e\u4efb\u52a1\u5206\u914d
\n\u52a8\u6001\u8d1d\u53f6\u65af\u7f51\u7edc\u67b6\u6784\uff08\u52a8\u6001\u98de\u884c\u590d\u6742\u73af\u5883\u4e2d\u7684\u81ea\u4e3b\u98de\u884c\u548c\u4efb\u52a1\u6267\u884c\uff09\u5f02\u6784\u65e0\u4eba\u673a\u4e0e\u591a\u6837\u5316\u4efb\u52a1\u9002\u914d\uff08\u81ea\u7136\u707e\u5bb3\u548c\u73af\u5883\u591a\u53d8\u4e0b\u7684\u98de\u884c\uff09
\n\u53c2\u8003\uff1a\u65e0\u4eba\u673a\u8def\u5f84\u89c4\u5212\u7b97\u6cd5\u7814\u7a76_\u9b4f\u6d9b DIO:10.27675\/d.cnki.gcydx.2020.000204
\n\u4e8c\u3001\u8def\u5f84\u89c4\u5212\u7ecf\u5178\u7b97\u6cd5
\n\uff08\u4e00\uff09\u57fa\u4e8e\u641c\u7d22\u7684\u8def\u5f84\u89c4\u5212
\n1.Dijkstra
\n\u80cc\u666f\u53ca\u9002\u7528\u573a\u666f
\n\u4ece\u8d77\u59cb\u70b9\u5230\u7ec8\u70b9\u7684\u6700\u77ed\uff08\u6700\u4f18\uff09\u8def\u5f84\u95ee\u9898\u5e7f\u5ea6\u4f18\u5148\u641c\u7d22\u89e3\u51b3\u8d4b\u6743\u6709\u5411\u56fe\u6216\u8005\u65e0\u5411\u56fe\u7684\u5355\u6e90\u6700\u77ed\u8def\u5f84\u95ee\u9898\uff0c\u6700\u7ec8\u5f97\u5230\u4e00\u4e2a\u6700\u77ed\u8def\u5f84\u6811\u3002\u8be5\u7b97\u6cd5\u5e38\u7528\u4e8e\u8def\u5f84\u641c\u7d22\u6216\u8005\u4f5c\u4e3a\u5176\u4ed6\u56fe\u7b97\u6cd5\u7684\u4e00\u4e2a\u5b50\u6a21\u5757\u672c\u7b97\u6cd5\u4e5f\u53ef\u9002\u7528\u4e8e\u5bfb\u4f18\u95ee\u9898
\n\u539f\u7406\uff1a\u8d2a\u5fc3\u601d\u60f3
\n\u4ece\u8d77\u70b9\u5f00\u59cb\u9010\u6b65\u6269\u5c55\uff0c\u6bcf\u4e00\u6b65\u4e3a\u4e00\u4e2a\u8282\u70b9\u627e\u5230\u6700\u77ed\u8def\u5f84
\n\u6805\u683c\u5730\u56fe
\n \u5b9a\u4e49\uff1a\u5730\u56fe\u5212\u5206\u4e3a\u82e5\u5e72\u5206\u8fa8\u7387\u7684\u5c0f\u65b9\u683c\uff0c\u6bcf\u4e2a\u5c0f\u65b9\u683c\u6709\u4e0d\u540c\u7684\u6743\u503c\uff08\u989c\u8272\uff09\uff0c\u4ee3\u8868\u4e0d\u540c\u7684\u610f\u4e49\uff0c\u5982\u4e0b\uff1a \u6805\u683c\u5730\u56fe\u7684\u4f18\u52bf\uff1a
\n \u53ef\u4ee5\u5c06\u4efb\u610f\u5f62\u72b6\u8f6e\u5ed3\u7684\u5730\u56fe\uff0c\u7528\u8db3\u591f\u7cbe\u7ec6\u7684\u6805\u683c\u8fdb\u884c\u7ed8\u5236 \u6bcf\u4e00\u4e2a\u6805\u683c\uff0c\u53ef\u4ee5\u901a\u8fc7\u4e0d\u540c\u989c\u8272\u8868\u793a\u4e0d\u540c\u542b\u4e49 \u57fa\u4e8e\u6805\u683c\u5730\u56fe\u8fdb\u884c\u8def\u5f84\u89c4\u5212\u6709\u6a2a\u3001\u7eb5\u3001\u659c\u4e09\u4e2a\u89c4\u5212\u65b9\u5411\u3002\u5bf9\u5e94\u5ba4\u5185\u4f4e\u901f\u673a\u5668\u4eba\u53ef\u4ee5\u5b8c\u5168\u6309\u7167\u8def\u5f84\u884c\u8d70\uff1b\u5bf9\u4e8e\u4e2d\u9ad8\u901f\u673a\u5668\u4eba\uff0c\u53ef\u4ee5\u8003\u8651\u5c06\u8def\u5f84\u8fdb\u884c\u5e73\u6ed1\u5904\u7406\uff0c\u4ee5\u9002\u7528\u4e8e\u975e\u5b8c\u5168\u7ea6\u675f\u7cfb\u7edf\u3002 \u6709\u6743\u56fe\u8f6c\u5316\uff1a \u884c\u52a8\u65b9\u5411\u77e9\u9635\uff1a\u524d\u4e24\u4e2a\u53c2\u6570\u4ee3\u8868\u8fd0\u52a8\u65b9\u5411\uff0c\u540e\u4e00\u4e2a\u53c2\u6570\u4ee3\u8868\u8fd0\u52a8\u4ee3\u4ef7\uff08\u901a\u5e38\u4e3a\u8fd0\u52a8\u8ddd\u79bb\uff09 <\/p>\n
matlab\u7ed8\u5236\u6805\u683c\u5730\u56fe % MATLAB \u7ed8\u5236\u6805\u683c\u5730\u56fe\u7684\u6838\u5fc3\u51fd\u6570\u548c\u601d\u60f3
\n% colormap\uff1a\u4e3a\u6805\u683c\u5730\u56fe\u521b\u5efa\u81ea\u5b9a\u4e49\u989c\u8272\u3002\u5982\u9ec4\u8272\u6805\u683c\u4ee3\u8868\u7684\u8d77\u70b9\uff0c\u7ea2\u8272\u6805\u683c\u4ee3\u8868\u7684\u7ec8\u70b9,\u9ed1\u8272\u6805\u683c\u4ee3\u8868\u7684\u969c\u788d\u7269
\n% sub2ind\uff1a\u5c06\u884c\u5217\u7d22\u5f15\u8f6c\u4e3a\u7ebf\u6027\u7d22\u5f15 ind2sub\uff1a\u5c06\u7ebf\u6027\u7d22\u5f15\u8f6c\u4e3a\u884c\u5217\u7d22\u5f15
\n% image\uff1a\u5229\u7528colormap\u5efa\u7acb\u7684\u989c\u8272\u56fe\uff0c\u5c06\u6570\u7ec4\u4fe1\u606f\u663e\u793a\u4e3a\u56fe\u50cf
\nclc;
\nclear;
\nclose all;
\ncmap = [1 1 1; … % 1-\u767d\u8272-\u7a7a\u5730
\n 0 0 0; … % 2-\u9ed1\u8272-\u9759\u6001\u969c\u788d
\n 1 0 0; … % 3-\u7ea2\u8272-\u52a8\u6001\u969c\u788d
\n 1 1 0; … % 4-\u9ec4\u8272-\u8d77\u59cb\u70b9
\n 1 0 1; … % 5-\u54c1\u7ea2-\u76ee\u6807\u70b9
\n 0 1 0; … % 6-\u7eff\u8272-\u5230\u8fbe\u76ee\u6807\u70b9\u7684\u89c4\u5212\u8def\u5f84
\n 0 1 1]; % 7-\u9752\u8272-\u52a8\u6001\u89c4\u5212\u7684\u8def\u5f84<\/p>\n
% \u6784\u5efa\u989c\u8272map\u56fe
\ncolormap(cmap); <\/p>\n
%% \u6784\u5efa\u6805\u683c\u5730\u56fe\u573a\u666f
\n% \u6805\u683c\u5730\u56fe\u754c\u9762\u5927\u5c0f\uff1a\u884c\u6570\u548c\u5217\u6570
\nrows = 10;
\ncols = 20;
\n% \u5b9a\u4e49\u6805\u683c\u5730\u56fe\u7684\u5168\u57df\uff0c\u5e76\u521d\u59cb\u5316\u7a7a\u767d\u5730\u56fe
\nfield = ones(rows,cols);<\/p>\n
% \u969c\u788d\u7269\u533a\u57df
\nobsRate = 0.3;
\nobsNum = floor(rows * cols * obsRate); % \u53d6\u6574
\nobsIndex = randi([1,rows*cols],obsNum,1);
\nfield(obsIndex) = 2;<\/p>\n
% \u8d77\u59cb\u70b9\u548c\u76ee\u6807\u70b9
\nstartPos = 2;
\ngoalPos = rows * cols – 2;
\nfield(startPos) = 4;
\nfield(goalPos) = 5;<\/p>\n
%% \u753b\u6805\u683c\u56fe
\nimage(1.5,1.5,field);
\ngrid on;
\nset(gca , ‘gridline’ , ‘-‘ , ‘gridcolor’ , ‘k’ , ‘linewidth’ , 2 , ‘GridAlpha’ , 0.5);
\nset(gca , ‘xtick’ , 1 : cols + 1,’ytick’,1 : rows + 1);
\naxis image;
\n12345678910111213141516171819202122232425262728293031323334353637383940414243
\n\u5177\u4f53\u5b9e\u73b0
\n\u53d6\u81ea\uff1aIR\u827e\u82e5\u673a\u5668\u4eba <\/p>\n
2.A*
\n\u80cc\u666f\u53ca\u9002\u7528\u573a\u666f
\n\u4ece\u8d77\u59cb\u70b9\u5230\u7ec8\u70b9\u7684\u6700\u77ed\uff08\u6700\u4f18\uff09\u8def\u5f84\u95ee\u9898\u5e7f\u5ea6\u4f18\u5148\u641c\u7d22:BFS\u4ee5\u8d77\u70b9A\u4e3a\u5706\u5fc3\uff0c\u5148\u641c\u7d22A\u5468\u56f4\u7684\u6240\u6709\u70b9\uff0c\u5f62\u6210\u4e00\u4e2a\u7c7b\u4f3c\u5706\u7684\u641c\u7d22\u533a\u57df\uff0c\u518d\u6269\u5927\u641c\u7d22\u534a\u5f84\uff0c\u8fdb\u4e00\u6b65\u641c\u7d22\u5176\u5b83\u6ca1\u641c\u7d22\u5230\u7684\u533a\u57df\uff0c\u76f4\u5230\u7ec8\u70b9B\u8fdb\u5165\u641c\u7d22\u533a\u57df\u5185\u88ab\u627e\u5230\u6df1\u5ea6\u4f18\u5148\u641c\u7d22:DFS\u5219\u662f\u8ba9\u641c\u7d22\u7684\u533a\u57df\u79bbA\u5c3d\u91cf\u8fdc\uff0c\u79bbB\u5c3d\u91cf\u8fd1
\n\u539f\u7406\uff1a\u542f\u53d1\u5f0f\u51fd\u6570
\n A*\u7b97\u6cd5\u7684\u5bfc\u51fa \u9996\u5148\uff0cBFS\u4fdd\u8bc1\u7684\u662f\u4ece\u8d77\u70b9\u5230\u8fbe\u8def\u7ebf\u4e0a\u7684\u4efb\u610f\u70b9\u82b1\u8d39\u7684\u4ee3\u4ef7\u6700\u5c0f\uff08\u4f46\u662f\u4e0d\u8003\u8651\u8fd9\u4e2a\u8fc7\u7a0b\u662f\u5426\u8981\u641c\u7d22\u5f88\u591a\u683c\u5b50\uff09\u3002\u5176\u6b21\uff0cDFS\u4fdd\u8bc1\u7684\u662f\u901a\u8fc7\u4e0d\u65ad\u77eb\u6b63\u884c\u8d70\u65b9\u5411\u548c\u7ec8\u70b9\u7684\u65b9\u5411\u7684\u5173\u7cfb\uff0c\u4f7f\u53d1\u73b0\u7ec8\u70b9\u8981\u641c\u7d22\u7684\u683c\u5b50\u66f4\u5c11\uff08\u4f46\u662f\u4e0d\u8003\u8651\u8fd9\u4e2a\u8fc7\u7a0b\u662f\u5426\u7ed5\u8fdc\uff09\u3002 \u56e0\u6b64\uff0cA*\u7b97\u6cd5\u7684\u8bbe\u8ba1\u540c\u65f6\u878d\u5408\u4e86BFS\u548cDFS\u7684\u4f18\u52bf\uff0c\u65e2\u8003\u8651\u5230\u4e86\u4ece\u8d77\u70b9\u901a\u8fc7\u5f53\u524d\u8def\u7ebf\u7684\u4ee3\u4ef7\uff08\u4fdd\u8bc1\u4e86\u4e0d\u4f1a\u7ed5\u8def\uff09\uff0c\u53c8\u4e0d\u65ad\u7684\u8ba1\u7b97\u5f53\u524d\u8def\u7ebf\u65b9\u5411\u662f\u5426\u66f4\u8d8b\u8fd1\u7ec8\u70b9\u7684\u65b9\u5411\uff08\u4fdd\u8bc1\u4e86\u4e0d\u4f1a\u641c\u7d22\u5f88\u591a\u56fe\u5757\uff09\uff0c\u662f\u4e00\u79cd\u9759\u6001\u8def\u7f51\u4e2d\u6700\u6709\u6548\u7684\u76f4\u63a5\u641c\u7d22\u7b97\u6cd5\u3002 \u542f\u53d1\u5f0f\u51fd\u6570 <\/p>\n
\u8bbe\u5b9a\u5730\u56fe\u4e3a\u6805\u683c\u5730\u56fe\uff0c\u8fd0\u52a8\u65b9\u5411\u6709\u516b\u4e2a\uff0c\u6bcf\u4e2a\u65b9\u5411\u90fd\u6709\u5bf9\u5e94\u7684\u4ee3\u4ef7
\n F(n)\u4e3a\u603b\u4ee3\u4ef7\u51fd\u6570 g(n)\u4e3a\u8d77\u70b9\u79fb\u52a8\u5230\u6307\u5b9a\u7ed3\u70b9\uff08\u5f53\u524d\u7ed3\u70b9\uff09\u7684\u4ee3\u4ef7\u503c h(n)\u4e3a\u542f\u53d1\u5f0f\u51fd\u6570\uff0c\u7528\u6765\u7ea6\u675f\u8def\u5f84\u7684\u8d70\u5411\uff0c\u4ee3\u8868\u6307\u5b9a\u7ed3\u70b9\uff08\u5f53\u524d\u7ed3\u70b9\uff09\u5230\u7ec8\u70b9\u7684\u6a2a\u5411\u6216\u8005\u7eb5\u5411\u4ee3\u4ef7\u503c A*\u7b97\u6cd5\u7684\u4f18\u52bf\uff1a\u51cf\u5c11\u4e86\u91c7\u6837\u6805\u683c\u4e2a\u6570\uff0c\u589e\u52a0\u4e86\u8def\u5f84\u641c\u7d22\u901f\u5ea6\uff0c\u4f18\u5316\u4e86\u8def\u5f84\u8d70\u5411
\n\u5177\u4f53\u5b9e\u73b0 <\/p>\n
\uff08\u4e8c\uff09\u57fa\u4e8e\u91c7\u6837\u7684\u8def\u5f84\u89c4\u5212
\n1.RRT
\n\u5b9a\u4e49\u53ca\u9002\u7528\u573a\u666f
\n\u5feb\u901f\u62d3\u5c55\u968f\u673a\u6811\u6cd5
\n\u5355\u6e90\u8def\u5f84\u3001\u907f\u969c\u95ee\u9898
\n\u6838\u5fc3\u601d\u60f3
\n \u4e3b\u4f53\u601d\u60f3 \u5c06\u641c\u7d22\u7684\u8d77\u70b9\u4f4d\u7f6e\u4f5c\u4e3a\u6839\u8282\u70b9\uff0c\u7136\u540e\u901a\u8fc7\u968f\u673a\u91c7\u6837\u589e\u52a0\u53f6\u5b50\u8282\u70b9\u7684\u65b9\u5f0f\uff0c\u751f\u6210\u4e00\u4e2a\u968f\u673a\u6269\u5c55\u6811\uff0c\u5f53\u968f\u673a\u6811\u7684\u53f6\u5b50\u8282\u70b9\u8fdb\u5165\u76ee\u6807\u533a\u57df\uff08\u6545\u4e0d\u80fd\u51c6\u786e\u627e\u5230\u76ee\u6807\u70b9\uff09\uff0c\u5c31\u5f97\u5230\u4e86\u4ece\u8d77\u70b9\u4f4d\u7f6e\u5230\u76ee\u6807\u4f4d\u7f6e\u7684\u8def\u5f84 \u5173\u952e\u70b9\uff1a\u91c7\u6837 \u60c5\u51b5\u4e00\uff1a\u8def\u5f84\u6ca1\u6709\u7a7f\u8fc7\u969c\u788d \u5728\u5730\u56fe\u8303\u56f4\u5185\u968f\u673a\u9009\u53d6\u91c7\u6837\u70b9\uff0c\u7136\u540e\u9009\u53d6\u5f53\u524d\u8def\u5f84\u5217\u8868\u7ed3\u70b9\u7684\u6700\u8fd1\u70b9\uff0c\u4ee5\u4e00\u5b9a\u6b65\u957f\u8fde\u63a5\u4e24\u70b9 <\/p>\n
\u60c5\u51b5\u4e8c\uff1a\u8def\u5f84\u7a7f\u8fc7\u969c\u788d
\n\u9700\u8981\u820d\u5f03\u8be5\u6761\u8def\u5f84\u9009\u53d6
\n \u89c4\u8303\u8def\u5f84\u8d8b\u52bf \u8d4b\u4e88\u7ec8\u70b9\u4e00\u5b9a\u7684\u6982\u7387\u88ab\u9009\u4f5c\u91c7\u6837\u70b9\uff0c\u4ee5\u89c4\u8303\u8def\u5f84\u4f7f\u5176\u8d8b\u5411\u6700\u4f18\uff08\u5c3d\u7ba1\u6548\u679c\u4e0d\u4f73\uff09 \u52a8\u6001\u6b65\u957f\u4f18\u5316 \u8fde\u63a5\u4e24\u70b9\u4e4b\u95f4\u7684\u6b65\u957f\u53ef\u4ee5\u6839\u636e\u5f53\u524d\u6240\u5904\u4f4d\u7f6e\u800c\u52a8\u6001\u53d8\u5316\uff0c\u4f7f\u8def\u5f84\u66f4\u4f18
\n\u5177\u4f53\u5b9e\u73b0
\n\u4f2a\u4ee3\u7801\uff1a <\/p>\n
\u4e3b\u4f53\u6838\u5fc3\u4ee3\u7801+\u6ce8\u91ca\uff1a
\ndef rrt_planning(self, start, goal, animation=True):
\n start_time = time.time()
\n self.start = Node(start[0], start[1])
\n self.goal = Node(goal[0], goal[1])
\n self.node_list = [self.start]#\u628a\u8d77\u70b9\u653e\u5165\u6811
\n path = None
\n #rrt\u4e3b\u4f53\u601d\u60f3
\n for i in range(self.max_iter):
\n #\u91c7\u6837\u70b9
\n rnd = self.sample()
\n #\u5728self.node_list\u4e2d\u83b7\u53d6near\u70b9
\n n_ind = self.get_nearest_list_index(self.node_list, rnd)#\u8fd4\u56denear\u70b9\u7684index
\n nearestNode = self.node_list[n_ind]<\/p>\n
# steer\uff1a\u642d\u5efa\u6811\u679d\u65b9\u5411\uff0c\u4ee5theta\u4f53\u73b0
\n theta = math.atan2(rnd[1] – nearestNode.y, rnd[0] – nearestNode.x)
\n newNode = self.get_new_node(theta, n_ind, nearestNode)<\/p>\n
noCollision = self.check_segment_collision(newNode.x, newNode.y, nearestNode.x, nearestNode.y)
\n if noCollision:#noCollision=1\u8868\u793a\u6811\u679d\u6ca1\u6709\u7a7f\u8fc7\u969c\u788d,\u8bb0\u5f55path;\u5426\u5219\u820d\u5f03\u8fd9\u4e2anewNode\uff0c\u7ee7\u7eedsample
\n #\u628anewNode\u6dfb\u52a0\u5230node_list\u4e2d
\n self.node_list.append(newNode)
\n #\u56fe\u50cf\u5316\u4eff\u771f\uff1a\u663e\u793a\u5bfb\u627e\u7684\u8fc7\u7a0b
\n if animation:
\n self.draw_graph(newNode, path)
\n #\u5224\u65adnewNode\u662f\u5426\u9760\u8fd1goal\uff1a\u8fd4\u56de1\u5219\u662f
\n if self.is_near_goal(newNode):
\n #\u662f\u5426\u7a7f\u8fc7\u969c\u788d
\n if self.check_segment_collision(newNode.x, newNode.y,
\n self.goal.x, self.goal.y):
\n #\u4ece0\u5f00\u59cb\u7684\u4e0b\u6807\uff0c\u9700\u8981-1
\n lastIndex = len(self.node_list) – 1
\n #\u83b7\u53d6\u8def\u7ebf(\u627e\u5230\u7684\u5404\u4e2a\u70b9\u7684\u5750\u6807)
\n path = self.get_final_course(lastIndex)
\n pathLen = self.get_path_len(path)
\n print(“current path length: {}, It costs {} s”.format(pathLen, time.time()-start_time))
\n #\u56fe\u50cf\u5316\u4eff\u771f\uff1a\u663e\u793a\u6700\u7ec8\u8def\u7ebf
\n if animation:
\n self.draw_graph(newNode, path)
\n return path#\u8fd4\u56de\u6574\u6761\u8def\u7ebf
\n12345678910111213141516171819202122232425262728293031323334353637383940
\n\u6548\u679c\u5206\u6790
\n\u641c\u7d22\u901f\u5ea6\u5feb\u8def\u5f84\u975e\u6700\u4f18\u53ea\u80fd\u5230\u8fbe\u76ee\u6807\u533a\u57df\u9644\u8fd1 <\/p>\n
2.RRT*
\n\u5b9a\u4e49\u53ca\u9002\u7528\u573a\u666f
\n\u5728RRT\u601d\u60f3\u4e0a\u5bfb\u627e\u6700\u4f18\u7684\u8def\u5f84
\n\u5355\u6e90\u907f\u969c\u7684\u6700\u4f18\u8def\u5f84\u95ee\u9898
\n\u6838\u5fc3
\n parent_node \u5f53\u524d\u7ed3\u70b9\u52a0\u5165\u4e00\u6b65\u68c0\u7d22\uff0c\u5bfb\u627e\u5176\u5230\u9644\u8fd1\u7ed3\u70b9\u7684\u6700\u77ed\u8ddd\u79bb\uff0c\u4ece\u800c\u66f4\u65b0\u7236\u8282\u70b9\uff0c\u4ee5\u4f7f\u8def\u5f84\u8fbe\u5230\u6700\u4f18 <\/p>\n
def choose_parent(self, newNode, nearInds):
\n if len(nearInds) == 0:
\n return newNode<\/p>\n
dList = []
\n #\u904d\u5386\u6bcf\u4e00\u4e2a\u8303\u56f4\u5185\u7684\u7ed3\u70b9\uff0c\u5224\u65ad\u969c\u788d\u5e76\u8ba1\u7b97\u8ddd\u79bb
\n for i in nearInds:
\n dx = newNode.x – self.node_list[i].x
\n dy = newNode.y – self.node_list[i].y
\n d = math.hypot(dx, dy)#hypot(dx, dy) \u6c42\u5e73\u65b9\u548c\u7684\u5e73\u65b9
\n theta = math.atan2(dy, dx)
\n if self.check_collision(self.node_list[i], theta, d):
\n #\u6b63\u5e38\uff0c\u8bb0\u5f55\u8ddd\u79bb\u4e3a\u539f\u6765\u8def\u5f84\u957f\u5ea6\u52a0\u4e0a\u8fd9\u6761\u6811\u679d\u7684\u957f\u5ea6
\n dList.append(self.node_list[i].cost + d)
\n else:
\n #\u7a7f\u8fc7\u969c\u788d\uff0c\u8bb0\u5f55\u8ddd\u79bb\u4e3a\u65e0\u7a77\u5927
\n dList.append(float(‘inf’))<\/p>\n
minCost = min(dList)
\n minInd = nearInds[dList.index(minCost)]<\/p>\n
if minCost == float(‘inf’):
\n print(“min cost is inf”)
\n return newNode
\n #\u66f4\u65b0\u8def\u5f84\u548c\u8ddd\u79bb
\n newNode.cost = minCost
\n newNode.parent = minInd<\/p>\n
return newNode<\/p>\n
def find_near_nodes(self, newNode):
\n n_node = len(self.node_list)
\n #\u8303\u56f4\u5706\u7684\u534a\u5f84:\u968f\u7740node_list\u6811\u7ed3\u70b9\u7684\u5927\u5c0f\u800c\u52a8\u6001\u53d8\u5316\u7684
\n r = 50.0 * math.sqrt((math.log(n_node) \/ n_node))
\n #\u904d\u5386\u6811\u7ed3\u70b9\uff0c\u8ba1\u7b97\u8ddd\u79bb
\n d_list = [(node.x – newNode.x) ** 2 + (node.y – newNode.y) ** 2
\n for node in self.node_list]
\n #\u904d\u5386\u8ddd\u79bb\u5217\u8868\uff0c\u5bfb\u627e \u5728\u8303\u56f4\u5185\u7684\u70b9 \u5728d_list\u5217\u8868\u4e2d\u7684\u4e0b\u6807index
\n near_inds = [d_list.index(i) for i in d_list if i <= r ** 2]\n return near_inds#\u8fd4\u56de\u8303\u56f4\u5185\u70b9\u7684\u4e0b\u6807\n12345678910111213141516171819202122232425262728293031323334353637383940 \n rewire \u5bfb\u627e\u5230\u6700\u4f18\u7236\u8282\u70b9\u4e4b\u540e\u5728\u4e00\u5b9a\u5706\u8303\u56f4\u5185\u4e0d\u65ad\u68c0\u7d22\u6240\u6709\u4e34\u8fd1\u7ed3\u70b9\u7684\u7ec4\u5408\u65b9\u5f0f\uff0c\u91cd\u65b0\u7ec4\u5408\u8def\u5f84\uff0c\u4f7f\u5176\u8fbe\u5230\u6700\u4f18\u7684\u6548\u679c \n \n def rewire(self, newNode, nearInds):\n n_node = len(self.node_list)\n for i in nearInds:\n nearNode = self.node_list[i]\n d = math.sqrt((nearNode.x - newNode.x) ** 2\n + (nearNode.y - newNode.y) ** 2)\n s_cost = newNode.cost + d\n #\u5982\u679c\u9644\u8fd1\u7ed3\u70b9(\u53ef\u9009\u7236\u8282\u70b9)cost\u5927\u4e8e\u65b0\u9009\u7ed3\u70b9\u7684cost\uff0c\u5219\u66f4\u65b0\u4e34\u8fd1\u7ed3\u70b9\u7684\u7236\u8282\u70b9\u548c\u8ddd\u79bb\n if nearNode.cost > s_cost:
\n theta = math.atan2(newNode.y – nearNode.y,
\n newNode.x – nearNode.x)
\n if self.check_collision(nearNode, theta, d):
\n nearNode.parent = n_node – 1
\n nearNode.cost = s_cost
\n1234567891011121314
\n\u5177\u4f53\u5b9e\u73b0
\n #\u4e24\u4e2a\u4f18\u5316\u6280\u5de7\u4f7f\u8def\u5f84\u8d8b\u5411\u6700\u4f18
\n #\u5bfb\u627e\u4e00\u5b9a\u8303\u56f4\u5185\u7684\u4e34\u8fd1\u70b9(newNode\u7684\u53ef\u80fd\u7236\u8282\u70b9)
\n nearInds = self.find_near_nodes(newNode)
\n #\u91cd\u65b0\u9009\u62e9\u7236\u8282\u70b9:newNode\u7684\u5750\u6807\u4e0d\u53d8\uff0c\u4f46cost\u548cparent\u53d8\u4e86\uff0c\u5373\u8def\u5f84\u8d70\u5411\u6539\u53d8
\n newNode = self.choose_parent(newNode, nearInds)
\n #\u6811\u4e2d\u6dfb\u52a0\u65b0\u7ed3\u70b9\u4fe1\u606f\uff0c\u5e76\u4e14\u91cd\u65b0\u8fde\u63a5
\n self.node_list.append(newNode)
\n self.rewire(newNode, nearInds)
\n12345678
\n\u6548\u679c\u5b9e\u73b0
\n\u641c\u7d22\u901f\u5ea6\u5feb\u8def\u5f84\u751f\u6210\u8d8b\u4e8e\u6700\u4f18\u65e0\u6548\u7684\u641c\u7d22\u8fc7\u591a\u53ea\u80fd\u5230\u8fbe\u76ee\u6807\u533a\u57df <\/p>\n
3.informed RRT*
\n\u5b9a\u4e49\u53ca\u9002\u7528\u573a\u666f
\n\u5728RRT*\u601d\u60f3\u7684\u57fa\u7840\u4e0a\u6dfb\u52a0\u692d\u5706\u91c7\u6837\u7ea6\u675f,\u4f18\u5316\u8def\u5f84\u3001\u52a0\u5feb\u5bfb\u4f18\u901f\u5ea6
\n\u9002\u7528\u4e8e\u5355\u6e90\u5feb\u901f\u907f\u969c\u7684\u8def\u5f84\u89c4\u5212\u95ee\u9898
\n\u6838\u5fc3
\n \u692d\u5706\u91c7\u6837\u7ea6\u675f
\n \u4e8c\u7ef4\u7a7a\u95f4\u4e0b\uff0c\u4ee5\u8d77\u59cb\u70b9\u6240\u6709\u76f4\u7ebf\u4e3a\u692d\u5706\u957f\u8f74\u6784\u5efa\u692d\u5706\u8303\u56f4\uff1b\u4e09\u7ef4\u7a7a\u95f4\u4e0b\uff0c\u6784\u5efa\u692d\u7403\u4f53 \u786e\u5b9aXcenter\u70b9\uff0c\u4f5c\u4e3a\u5750\u6807\u7cfb\u8f6c\u5316\u7684\u504f\u7f6e\u4fee\u6b63 \u8ba1\u7b97\u8d77\u59cb\u70b9\u7684\u8d77\u59cb\u8ddd\u79bb\u4f5c\u4e3aCmin \u6bcf\u6b21\u91c7\u6837\u524d\uff0c\u8ba1\u7b97\u5f53\u524d\u8def\u5f84\u7684\u4ee3\u4ef7\u957f\u5ea6\u4f5c\u4e3aCbest\uff1b\u8ba1\u7b97\u65cb\u8f6c\u77e9\u9635Kabsch\u7b97\u6cd5\u6c42\u89e3\u65cb\u8f6c\u77e9\u9635 \u6bcf\u6b21\u91c7\u6837\u65f6\uff0c\u4ee5\u5355\u4f4d\u5706\u7684\u5f62\u5f0f\u91c7\u6837\u83b7\u53d6\u5750\u6807Xball\uff1b\u8ba1\u7b97r\u77e9\u9635\u3001C\u65cb\u8f6c\u77e9\u9635 \u6bcf\u6b21\u91c7\u6837\u540e\uff0c\u4ee5\u516c\u5f0f rCXball+Xcenter \u8f6c\u5316\u4e3a\u692d\u5706\u5750\u6807\u7cfb\u4e0b\u7684\u5750\u6807 \u91cd\u70b9\uff1a\u4e0d\u65ad\u7f29\u5c0f\u692d\u5706\u7684\u8303\u56f4 \u5177\u4f53\u5b9e\u73b0\uff1aCbest\u662f\u5728\u8fed\u4ee3\u4e2d\u4e0d\u65ad\u66f4\u65b0\u53d8\u5316\u7684\uff0c\u8bbe\u7f6er\u77e9\u9635\u7b2c\u4e00\u4e2a\u5143\u7d20\u4e3aCbest\u7684\u4e00\u534a\uff0c\u7b2c\u4e8c\u4e2a\u5143\u7d20\u662fCbest\u4e0e\u8d77\u59cb\u70b9\u957f\u5ea6\u7684\u5e73\u65b9\u548c\u7684\u5f00\u65b9\u7684\u4e00\u534a\uff0c\u56e0\u6b64\u692d\u5706\u8303\u56f4\u4f1a\u4e0d\u65ad\u66f4\u65b0\u53d8\u5316\uff08\u7f29\u5c0f\uff09 <\/p>\n
\u65cb\u8f6c\u77e9\u9635 \u8ba1\u7b97\u65cb\u8f6c\u77e9\u9635Kabsch\u7b97\u6cd5\u6c42\u89e3\u65cb\u8f6c\u77e9\u9635
\n\u5177\u4f53\u5b9e\u73b0
\n def informed_sample(self, cMax, cMin, xCenter, C):
\n if cMax < float('inf'):\n # L\u77e9\u9635\u7b2c\u4e00\u4e2a\u5143\u7d20\u4e3a\u957f\u8f74\u7684\u4e00\u534a\uff0c\u7b2c\u4e8c\u4e2a\u5143\u7d20\u662f\u5f53\u524d\u8def\u7ebf\u957f\u5ea6\u4e0e\u8d77\u59cb\u70b9\u957f\u5ea6\u7684\u5e73\u65b9\u548c\u7684\u5f00\u65b9\u7684\u4e00\u534a\n r = [cMax \/ 2.0,\n math.sqrt(cMax ** 2 - cMin ** 2) \/ 2.0,\n math.sqrt(cMax ** 2 - cMin ** 2) \/ 2.0]\n L = np.diag(r)#\u751f\u6210\u4ee5r\u4e3a\u5143\u7d20\u7684\u5bf9\u89d2\u77e9\u9635\n #\u751f\u6210\u91c7\u6837\u70b9(\u4ee5\u5355\u4f4d\u5706\u5185\u91c7\u6837)\n xBall = self.sample_unit_ball()\n #\u538b\u6241\u4e3a\u5728\u692d\u5706\u8303\u56f4\u5185\n rnd = np.dot(np.dot(C, L), xBall) + xCenter\n rnd = [rnd[(0, 0)], rnd[(1, 0)]]\n else:\n #\u672a\u751f\u6210\u692d\u5706\n rnd = self.sample()\n\n return rnd#\u8fd4\u56de\u91c7\u6837\u70b9\u7684\u5750\u6807\n1234567891011121314151617 \n\u6548\u679c\u5b9e\u73b0 \n\u6e10\u8fdb\u5bfb\u4f18\u901f\u5ea6\u52a0\u5feb\u8def\u5f84\u8d8b\u5411\u6700\u4f18\u7684\u6548\u679c\u6781\u4f73\u51cf\u5c11\u4e86\u65e0\u6548\u7684\u641c\u7d22 \n \n4.PRM \n\u5b9a\u4e49 \n\u4ee5\u968f\u673a\u91c7\u6837\u7684\u5f62\u5f0f\u91c7\u70b9\uff0c\u5f53\u4ee3\u4ef7<\u9608\u503c\u65f6\uff0c\u751f\u6210\u70b9\u4e0e\u70b9\u4e4b\u95f4\u7684\u76f4\u7ebf\u8def\u7ebf\uff0c\u6700\u540e\u5728\u642d\u5efa\u7684\u8def\u7ebf\u56fe\u4e2d\u5bfb\u627e\u6700\u4f18\u8def\u5f84 \n\u6982\u7387\u8def\u7ebf\u56fe\u6784\u5efa \n \u968f\u673a\u91c7\u6837 \u91c7\u6837\u70b9\u548c\u8ddd\u79bb<\u9608\u503c\uff0c\u5219\u751f\u6210\u8def\u7ebf\u8fde\u63a5 \n\u56fe\u4e0a\u5bfb\u627e\u6700\u4f18\u8def\u5f84 \n\u4ee5\u4ee3\u4ef7\u6700\u4f4e\u4e3a\u76ee\u6807\u5bfb\u627e\u6700\u4f18\u8def\u7ebf\u5373\u53ef \n\uff08\u4e09\uff09\u57fa\u4e8e\u542f\u53d1\u5f0f\u667a\u80fd\u7b97\u6cd5\u7684\u8def\u5f84\u89c4\u5212 \n1.\u9057\u4f20\u7b97\u6cd5 \n2.\u8681\u7fa4\u7b97\u6cd5 \n\u7531\u4e8e\u5b9e\u9645\u5e94\u7528\u4e2d\u8f83\u5c11\uff0c\u4e14\u4ee5\u5f80\u5df2\u7ecf\u63a5\u89e6\u548c\u5b9e\u73b0\u8fc7\uff0c\u6545\u5728\u6b64\u4e0d\u505a\u7b14\u8bb0 \n\u4e09\u3001\u7ecf\u5178\u7b97\u6cd5\u7684\u4ee3\u7801\u5b9e\u73b0 \n \u4ee3\u7801\u7684\u5b9e\u73b0\u53c2\u8003\u4e86IR\u827e\u82e5\u673a\u5668\u4eba\u7684\u7b97\u6cd5\u601d\u8def\uff0c\u6df1\u5165\u7406\u89e3\u4e86\u7b97\u6cd5\u7684\u6838\u5fc3\u90e8\u5206 \u7531\u4e8e\u7b97\u6cd5\u5e94\u7528\u4e8e\u4e0d\u540c\u7684\u95ee\u9898\u6709\u4e0d\u540c\u7684\u903b\u8f91\uff0c\u6545\u540e\u7eed\u9700\u8981\u5728\u4e0d\u540c\u95ee\u9898\u573a\u666f\u4e0b\u6216\u8005ros\u4e0b\u8dd1\u4e00\u4e0b\u4eff\u771f \uff08\u672a\u5b8c\u5f85\u7eed\u2026\uff09 \n\u56db\u3001\u7ecf\u5178\u7b97\u6cd5\u7684\u9002\u7528\u573a\u666f\u603b\u7ed3 \n \n\u4e94\u3001\u8bba\u6587\u6cdb\u8bfb\u2014\u7b97\u6cd5\u6539\u8fdb \n\uff08\u53e6\u5199\u4e00\u7bc7\u6587\u7ae0\uff0c\u672a\u5b8c\u5f85\u7eed\u2026\uff09 \n\u540e\u8bb0 \n\u5165\u95e8\u8def\u5f84\u4f18\u5316\u7b97\u6cd5\u221a \n \n \n\u4eba\u751f\u4e4b\u609f \n\u8def\u5f84\u867d\u6709\u89c4\u5212\u4e4b\u6cd5\uff0c\u4f46\u4eba\u751f\u8def\u6bd5\u7adf\u591a\u53d8\uff0c\u8fd8\u9700\u4e0d\u65ad\u63d0\u5347\u5e94\u53d8\u80fd\u529b\u624d\u80fd\u8fbe\u5230\u66f4\u597d\u7684\u4e0b\u4e00\u4e2a\u8d77\u70b9\uff08\u4eba\u751f\u6ca1\u6709\u7ec8\u70b9\uff09 \n\u4eba\u751f\u8def\u5982\u6b64\uff0c\u65e0\u4eba\u673a\u8def\u5f84\u4e5f\u5982\u6b64\uff0c\u98de\u884c\u73af\u5883\u591a\u53d8\uff0c\u6545\u6b64\u9700\u8981\u5b66\u4e60\u548c\u63a2\u7d22\u5e94\u5bf9\u591a\u53d8\u73af\u5883\u7684\u65b9\u6cd5\uff0c\u4f7f\u5176\u589e\u5f3a\u751f\u547d\u7684\u786c\u5ea6\uff01\uff01\uff01\uff01\n<\/p>\n","protected":false},"excerpt":{"rendered":"
2022-01-31 \u8def\u5f84\u89c4\u5212\u4e0e\u4f18\u5316\u5b66\u4e60\u7cfb\u5217\uff08 […]<\/p>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[3,1],"tags":[],"class_list":["post-321","post","type-post","status-publish","format-standard","hentry","category-technology-frontier","category-home"],"views":1,"_links":{"self":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/321"}],"collection":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts"}],"about":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/comments?post=321"}],"version-history":[{"count":1,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/321\/revisions"}],"predecessor-version":[{"id":327,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/321\/revisions\/327"}],"wp:attachment":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/media?parent=321"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/categories?post=321"},{"taxonomy":"post_tag","embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/tags?post=321"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}