2022-04-02<\/p>\n
\u524d\u8a00\uff1a\u7531\u4e8e\u540e\u7eed\u53ef\u80fd\u8981\u505a\u4e00\u4e9b\u65e0\u4eba\u9a7e\u9a76\u76f8\u5173\u7684\u9879\u76ee\u548c\u5b9e\u9a8c\uff0c\u6240\u4ee5\u8fd9\u6bb5\u65f6\u95f4\u5b66\u4e60\u4e00\u4e9b\u8def\u5f84\u89c4\u5212\u7b97\u6cd5\u5e76\u81ea\u5df1\u7f16\u5199\u4e86matlab\u7a0b\u5e8f\u8fdb\u884c\u4eff\u771f\u3002\u5f00\u542f\u8fd9\u4e2a\u7cfb\u5217\u662f\u5bf9\u81ea\u5df1\u5b66\u4e60\u5185\u5bb9\u7684\u4e00\u4e2a\u603b\u7ed3\uff0c\u4e5f\u5e0c\u671b\u80fd\u591f\u548c\u4f18\u79c0\u7684\u524d\u8f88\u4eec\u591a\u5b66\u4e60\u7ecf\u9a8c\u3002
\n\u4e00\u3001\u65e0\u4eba\u9a7e\u9a76\u8def\u5f84\u89c4\u5212
\n\u4f17\u6240\u5468\u77e5\uff0c\u65e0\u4eba\u9a7e\u9a76\u5927\u81f4\u53ef\u4ee5\u5206\u4e3a\u4e09\u4e2a\u65b9\u9762\u7684\u5de5\u4f5c\uff1a\u611f\u77e5\uff0c\u51b3\u7b56\u53ca\u63a7\u5236\u3002
\n\u8def\u5f84\u89c4\u5212\u662f\u611f\u77e5\u548c\u63a7\u5236\u4e4b\u95f4\u7684\u51b3\u7b56\u9636\u6bb5\uff0c\u4e3b\u8981\u76ee\u7684\u662f\u8003\u8651\u5230\u8f66\u8f86\u52a8\u529b\u5b66\u3001\u673a\u52a8\u80fd\u529b\u4ee5\u53ca\u76f8\u5e94\u89c4\u5219\u548c\u9053\u8def\u8fb9\u754c\u6761\u4ef6\u4e0b\uff0c\u4e3a\u8f66\u8f86\u63d0\u4f9b\u901a\u5f80\u76ee\u7684\u5730\u7684\u5b89\u5168\u548c\u65e0\u78b0\u649e\u7684\u8def\u5f84\u3002
\n\u8def\u5f84\u89c4\u5212\u95ee\u9898\u53ef\u4ee5\u5206\u4e3a\u4e24\u4e2a\u65b9\u9762\uff1a
\n\uff08\u4e00\uff09\u5168\u5c40\u8def\u5f84\u89c4\u5212\uff1a\u5168\u5c40\u8def\u5f84\u89c4\u5212\u7b97\u6cd5\u5c5e\u4e8e\u9759\u6001\u89c4\u5212\u7b97\u6cd5\uff0c\u6839\u636e\u5df2\u6709\u7684\u5730\u56fe\u4fe1\u606f\uff08SLAM\uff09\u4e3a\u57fa\u7840\u8fdb\u884c\u8def\u5f84\u89c4\u5212\uff0c\u5bfb\u627e\u4e00\u6761\u4ece\u8d77\u70b9\u5230\u76ee\u6807\u70b9\u7684\u6700\u4f18\u8def\u5f84\u3002\u901a\u5e38\u5168\u5c40\u8def\u5f84\u89c4\u5212\u7684\u5b9e\u73b0\u5305\u62ecDijikstra\u7b97\u6cd5\uff0cA*\u7b97\u6cd5\uff0cRRT\u7b97\u6cd5\u7b49\u7ecf\u5178\u7b97\u6cd5\uff0c\u4e5f\u5305\u62ec\u8681\u7fa4\u7b97\u6cd5\u3001\u9057\u4f20\u7b97\u6cd5\u7b49\u667a\u80fd\u7b97\u6cd5\uff1b
\n\uff08\u4e8c\uff09\u5c40\u90e8\u8def\u5f84\u89c4\u5212\uff1a\u5c40\u90e8\u8def\u5f84\u89c4\u5212\u5c5e\u4e8e\u52a8\u6001\u89c4\u5212\u7b97\u6cd5\uff0c\u662f\u65e0\u4eba\u9a7e\u9a76\u6c7d\u8f66\u6839\u636e\u81ea\u8eab\u4f20\u611f\u5668\u611f\u77e5\u5468\u56f4\u73af\u5883\uff0c\u89c4\u5212\u5904\u4e00\u6761\u8f66\u8f86\u5b89\u5168\u884c\u9a76\u6240\u9700\u7684\u8def\u7ebf\uff0c\u5e38\u5e94\u7528\u4e8e\u8d85\u8f66\uff0c\u907f\u969c\u7b49\u60c5\u666f\u3002\u901a\u5e38\u5c40\u90e8\u8def\u5f84\u89c4\u5212\u7684\u5b9e\u73b0\u5305\u62ec\u52a8\u6001\u7a97\u53e3\u7b97\u6cd5\uff08DWA\uff09\uff0c\u4eba\u5de5\u52bf\u573a\u7b97\u6cd5\uff0c\u8d1d\u585e\u5c14\u66f2\u7ebf\u7b97\u6cd5\u7b49\uff0c\u4e5f\u6709\u5b66\u8005\u63d0\u51fa\u795e\u7ecf\u7f51\u7edc\u7b49\u667a\u80fd\u7b97\u6cd5\u3002
\n\u672c\u7cfb\u5217\u5c31\u4ece\u65e0\u4eba\u9a7e\u9a76\u8def\u5f84\u89c4\u5212\u7684\u8fd9\u4e24\u65b9\u9762\u8fdb\u884c\u5c55\u5f00\uff0c\u5bf9\u4e00\u4e9b\u7ecf\u5178\u7684\u7b97\u6cd5\u539f\u7406\u8fdb\u884c\u4ecb\u7ecd\uff0c\u5e76\u6839\u636e\u4e2a\u4eba\u7684\u4e00\u4e9b\u7406\u89e3\u548c\u60f3\u6cd5\u63d0\u51fa\u4e86\u4e00\u4e9b\u6539\u8fdb\u7684\u610f\u89c1\uff0c\u901a\u8fc7Matlab2019\u5bf9\u7b97\u6cd5\u8fdb\u884c\u4e86\u4eff\u771f\u548c\u9a8c\u8bc1\u3002\u8fc7\u7a0b\u4e2d\u5982\u679c\u6709\u9519\u8bef\u7684\u5730\u65b9\uff0c\u6b22\u8fce\u5728\u8bc4\u8bba\u533a\u7559\u8a00\u8ba8\u8bba\uff0c\u5982\u6709\u4fb5\u6743\u8bf7\u53ca\u65f6\u8054\u7cfb\u3002
\n\u90a3\u4e48\u5e9f\u8bdd\u4e0d\u591a\u8bf4\uff0c\u76f4\u63a5\u8fdb\u5165\u7b2c\u4e00\u90e8\u5206\u7684\u4ecb\u7ecd\uff0c\u5168\u5c40\u8def\u5f84\u89c4\u5212\u7b97\u6cd5-RRT\u7b97\u6cd5\u3002
\n\u4e8c\u3001\u5168\u5c40\u8def\u5f84\u89c4\u5212 – RRT\u7b97\u6cd5\u539f\u7406
\nRRT\u7b97\u6cd5\uff0c\u5373\u5feb\u901f\u968f\u673a\u6811\u7b97\u6cd5\uff08Rapid Random Tree\uff09\uff0c\u662fLaValle\u57281998\u5e74\u9996\u6b21\u63d0\u51fa\u7684\u4e00\u79cd\u9ad8\u6548\u7684\u8def\u5f84\u89c4\u5212\u7b97\u6cd5\u3002RRT\u7b97\u6cd5\u4ee5\u521d\u59cb\u7684\u4e00\u4e2a\u6839\u8282\u70b9\uff0c\u901a\u8fc7\u968f\u673a\u91c7\u6837\u7684\u65b9\u6cd5\u5728\u7a7a\u95f4\u641c\u7d22\uff0c\u7136\u540e\u6dfb\u52a0\u4e00\u4e2a\u53c8\u4e00\u4e2a\u7684\u53f6\u8282\u70b9\u6765\u4e0d\u65ad\u6269\u5c55\u968f\u673a\u6811\u3002\u5f53\u76ee\u6807\u70b9\u8fdb\u5165\u968f\u673a\u6811\u91cc\u9762\u540e\uff0c\u968f\u673a\u6811\u6269\u5c55\u7acb\u5373\u505c\u6b62\uff0c\u6b64\u65f6\u80fd\u627e\u5230\u4e00\u6761\u4ece\u8d77\u59cb\u70b9\u5230\u76ee\u6807\u70b9\u7684\u8def\u5f84\u3002\u7b97\u6cd5\u7684\u8ba1\u7b97\u8fc7\u7a0b\u5982\u4e0b\uff1a
\nstep1\uff1a\u521d\u59cb\u5316\u968f\u673a\u6811\u3002\u5c06\u73af\u5883\u4e2d\u8d77\u70b9\u4f5c\u4e3a\u968f\u673a\u6811\u641c\u7d22\u7684\u8d77\u70b9\uff0c\u6b64\u65f6\u6811\u4e2d\u53ea\u5305\u542b\u4e00\u4e2a\u8282\u70b9\u5373\u6839\u8282\u70b9\uff1b\u00a0
\nstpe2\uff1a\u5728\u73af\u5883\u4e2d\u968f\u673a\u91c7\u6837\u3002\u5728\u73af\u5883\u4e2d\u968f\u673a\u4ea7\u751f\u4e00\u4e2a\u70b9\uff0c\u82e5\u8be5\u70b9\u4e0d\u5728\u969c\u788d\u7269\u8303\u56f4\u5185\u5219\u8ba1\u7b97\u968f\u673a\u6811\u4e2d\u6240\u6709\u8282\u70b9\u5230\u7684\u6b27\u5f0f\u8ddd\u79bb\uff0c\u5e76\u627e\u5230\u8ddd\u79bb\u6700\u8fd1\u7684\u8282\u70b9\uff0c\u82e5\u5728\u969c\u788d\u7269\u8303\u56f4\u5185\u5219\u91cd\u65b0\u751f\u6210\u5e76\u91cd\u590d\u8be5\u8fc7\u7a0b\u76f4\u81f3\u627e\u5230;\u00a0\u00a0
\nstpe3\uff1a\u751f\u6210\u65b0\u8282\u70b9\u3002\u5728\u548c\u8fde\u7ebf\u65b9\u5411\uff0c\u7531\u6307\u5411\u56fa\u5b9a\u751f\u957f\u8ddd\u79bb\u751f\u6210\u4e00\u4e2a\u65b0\u7684\u8282\u70b9\uff0c\u5e76\u5224\u65ad\u8be5\u8282\u70b9\u662f\u5426\u5728\u969c\u788d\u7269\u8303\u56f4\u5185\uff0c\u82e5\u4e0d\u5728\u969c\u788d\u7269\u8303\u56f4\u5185\u5219\u5c06\u6dfb\u52a0\u5230\u968f\u673a\u6811\u00a0\u4e2d\uff0c\u5426\u5219\u7684\u8bdd\u8fd4\u56destep2\u91cd\u65b0\u5bf9\u73af\u5883\u8fdb\u884c\u968f\u673a\u91c7\u6837\uff1b
\nstep4\uff1a\u00a0\u505c\u6b62\u641c\u7d22\u3002\u5f53\u548c\u76ee\u6807\u70b9\u4e4b\u95f4\u7684\u8ddd\u79bb\u5c0f\u4e8e\u8bbe\u5b9a\u7684\u9608\u503c\u65f6\uff0c\u5219\u4ee3\u8868\u968f\u673a\u6811\u5df2\u7ecf\u5230\u8fbe\u4e86\u76ee\u6807\u70b9\uff0c\u5c06\u4f5c\u4e3a\u6700\u540e\u4e00\u4e2a\u8def\u5f84\u8282\u70b9\u52a0\u5165\u5230\u968f\u673a\u6811\u4e2d\uff0c\u7b97\u6cd5\u7ed3\u675f\u5e76\u5f97\u5230\u6240\u89c4\u5212\u7684\u8def\u5f84\u00a0\u3002
\nRRT\u7b97\u6cd5\u7531\u4e8e\u5176\u968f\u673a\u91c7\u6837\u53ca\u6982\u7387\u5b8c\u5907\u6027\u7684\u7279\u70b9\uff0c\u4f7f\u5f97\u5176\u5177\u6709\u5982\u4e0b\u4f18\u52bf\uff1a
\n\uff081\uff09\u4e0d\u9700\u8981\u5bf9\u73af\u5883\u5177\u4f53\u5efa\u6a21\uff0c\u6709\u5f88\u5f3a\u7a7a\u95f4\u641c\u7d22\u80fd\u529b\uff1b
\n\uff082\uff09\u8def\u5f84\u89c4\u5212\u901f\u5ea6\u5feb\uff1b
\n\uff083\uff09\u53ef\u4ee5\u5f88\u597d\u89e3\u51b3\u590d\u6742\u73af\u5883\u4e0b\u7684\u8def\u5f84\u89c4\u5212\u95ee\u9898\u3002
\n\u4f46\u540c\u6837\u662f\u56e0\u4e3a\u968f\u673a\u6027\uff0cRRT\u7b97\u6cd5\u4e5f\u5b58\u5728\u5f88\u591a\u4e0d\u8db3\u7684\u65b9\u9762\uff1a
\n\uff081\uff09\u968f\u673a\u6027\u5f3a\uff0c\u641c\u7d22\u6ca1\u6709\u76ee\u6807\u6027\uff0c\u5197\u4f59\u70b9\u591a\uff0c\u4e14\u6bcf\u6b21\u89c4\u5212\u4ea7\u751f\u7684\u8def\u5f84\u90fd\u4e0d\u4e00\u6837\uff0c\u5747\u4e0d\u4e00\u662f\u6700\u4f18\u8def\u5f84\uff1b
\n\uff082\uff09\u53ef\u80fd\u51fa\u73b0\u8ba1\u7b97\u590d\u6742\u3001\u6240\u9700\u7684\u65f6\u95f4\u8fc7\u957f\u3001\u6613\u4e8e\u9677\u5165\u6b7b\u533a\u7684\u95ee\u9898\uff1b
\n\uff083\uff09\u7531\u4e8e\u6811\u7684\u6269\u5c55\u662f\u8282\u70b9\u4e4b\u95f4\u76f8\u8fde\uff0c\u4f7f\u5f97\u6700\u7ec8\u751f\u6210\u7684\u8def\u5f84\u4e0d\u5e73\u6ed1\uff1b
\n\uff084\uff09\u4e0d\u9002\u5408\u52a8\u6001\u73af\u5883\uff0c\u5f53\u73af\u5883\u4e2d\u51fa\u73b0\u52a8\u6001\u969c\u788d\u7269\u65f6\uff0cRRT\u7b97\u6cd5\u65e0\u6cd5\u8fdb\u884c\u6709\u6548\u7684\u68c0\u6d4b\uff1b
\n\uff085\uff09\u5bf9\u4e8e\u72ed\u957f\u5730\u5f62\uff0c\u53ef\u80fd\u65e0\u6cd5\u89c4\u5212\u51fa\u8def\u5f84\u3002
\n\u4e09\u3001RRT\u7b97\u6cd5Matlab\u5b9e\u73b0
\n\u4f7f\u7528matlab2019\u6765\u7f16\u5199RRT\u7b97\u6cd5\uff0c\u4e0b\u9762\u5c06\u8d34\u51fa\u90e8\u5206\u4ee3\u7801\u8fdb\u884c\u89e3\u91ca\u3002
\n1\u3001\u751f\u6210\u969c\u788d\u7269
\n\u5728matlab\u4e2d\u6a21\u62df\u6805\u683c\u5730\u56fe\u73af\u5883\uff0c\u81ea\u5b9a\u4e49\u969c\u788d\u7269\u4f4d\u7f6e\u3002
\n%% \u751f\u6210\u969c\u788d\u7269ob1 = [0,-10,10,5]; % \u4e09\u4e2a\u77e9\u5f62\u969c\u788d\u7269ob2 = [-5,5,5,10];ob3 = [-5,-2,5,4]; ob_limit_1 = [-15,-15,0,31]; % \u8fb9\u754c\u969c\u788d\u7269ob_limit_2 = [-15,-15,30,0];ob_limit_3 = [15,-15,0,31];ob_limit_4 = [-15,16,30,0]; ob = [ob1;ob2;ob3;ob_limit_1;ob_limit_2;ob_limit_3;ob_limit_4]; % \u653e\u5230\u4e00\u4e2a\u6570\u7ec4\u4e2d\u7edf\u4e00\u7ba1\u7406 x_left_limit = -16; % \u5730\u56fe\u7684\u8fb9\u754cx_right_limit = 15;y_left_limit = -16;y_right_limit = 16;
\n\u6211\u5728\u8fd9\u968f\u4fbf\u9009\u62e9\u751f\u6210\u4e09\u4e2a\u77e9\u5f62\u7684\u969c\u788d\u7269\uff0c\u5e76\u7edf\u4e00\u653e\u5728ob\u6570\u7ec4\u4e2d\u7ba1\u7406\uff0c\u540c\u65f6\u5b9a\u4e49\u5730\u56fe\u7684\u8fb9\u754c\u3002 <\/p>\n
2\u3001\u521d\u59cb\u5316\u53c2\u6570\u8bbe\u7f6e
\n\u521d\u59cb\u5316\u969c\u788d\u7269\u81a8\u80c0\u8303\u56f4\u3001\u5730\u56fe\u5206\u8fa8\u7387\uff0c\u673a\u5668\u4eba\u534a\u5f84\u3001\u8d77\u59cb\u70b9\u3001\u76ee\u6807\u70b9\u3001\u751f\u957f\u8ddd\u79bb\u548c\u76ee\u6807\u70b9\u641c\u7d22\u9608\u503c\u3002
\n%% \u521d\u59cb\u5316\u53c2\u6570\u8bbe\u7f6eextend_area = 0.2; % \u81a8\u80c0\u8303\u56f4resolution = 1; % \u5206\u8fa8\u7387robot_radius = 0.2; % \u673a\u5668\u4eba\u534a\u5f84 goal = [-10, -10]; % \u76ee\u6807\u70b9x_start = [13, 10]; % \u8d77\u70b9 grow_distance = 1; % \u751f\u957f\u8ddd\u79bbgoal_radius = 1.5; % \u5728\u76ee\u6807\u70b9\u4e3a\u5706\u5fc3\uff0c1.5m\u5185\u5c31\u505c\u6b62\u641c\u7d22 <\/p>\n
3\u3001\u521d\u59cb\u5316\u968f\u673a\u6811
\n\u521d\u59cb\u5316\u968f\u673a\u6811\uff0c\u5b9a\u4e49\u6811\u7ed3\u6784\u4f53tree\u4ee5\u4fdd\u5b58\u65b0\u8282\u70b9\u53ca\u5176\u7236\u8282\u70b9\uff0c\u4fbf\u4e8e\u540e\u7eed\u4ece\u76ee\u6807\u70b9\u56de\u63a8\u89c4\u5212\u7684\u8def\u5f84\u3002
\n%% \u521d\u59cb\u5316\u968f\u673a\u6811tree.child = []; % \u5b9a\u4e49\u6811\u7ed3\u6784\u4f53\uff0c\u4fdd\u5b58\u65b0\u8282\u70b9\u53ca\u5176\u7236\u8282\u70b9tree.parent = [];tree.child = x_start; % \u8d77\u70b9\u4f5c\u4e3a\u7b2c\u4e00\u4e2a\u8282\u70b9 flag = 1; % \u6807\u5fd7\u4f4d new_node_x = x_start(1,1); % \u5c06\u8d77\u70b9\u4f5c\u4e3a\u7b2c\u4e00\u4e2a\u751f\u6210\u70b9new_node_y = x_start(1,2);new_node = [new_node_x, new_node_y];
\n4\u3001\u4e3b\u51fd\u6570\u90e8\u5206
\n\u4e3b\u51fd\u6570\u4e2d\u9996\u5148\u751f\u6210\u968f\u673a\u70b9\uff0c\u5e76\u5224\u65ad\u662f\u5426\u5728\u5730\u56fe\u8303\u56f4\u5185\uff0c\u82e5\u8d85\u51fa\u8303\u56f4\u5219\u5c06\u6807\u5fd7\u4f4d\u7f6e\u4e3a0\u3002
\nrd_x = 30 * rand() – 15; % \u751f\u6210\u968f\u673a\u70b9rd_y = 30 * rand() – 15; if (rd_x >= x_right_limit || rd_x <= x_left_limit ||... % \u5224\u65ad\u968f\u673a\u70b9\u662f\u5426\u5728\u5730\u56fe\u8fb9\u754c\u8303\u56f4\u5185 rd_y >= y_right_limit || rd_y <= y_left_limit) flag = 0;end \n\u8c03\u7528\u51fd\u6570cal_distance\u8ba1\u7b97tree\u4e2d\u8ddd\u79bb\u968f\u673a\u70b9\u6700\u8fd1\u7684\u8282\u70b9\u7684\u7d22\u5f15\uff0c\u5e76\u8ba1\u7b97\u8be5\u8282\u70b9\u4e0e\u968f\u673a\u70b9\u8fde\u7ebf\u548cx\u6b63\u5411\u7684\u5939\u89d2\u3002 \n[angle, min_idx] = cal_distance(rd_x, rd_y, tree); % \u8fd4\u56detree\u4e2d\u6700\u77ed\u8ddd\u79bb\u8282\u70b9\u7d22\u5f15\u53ca\u5bf9\u5e94\u7684\u548cx\u6b63\u5411\u5939\u89d2 \ncal_distance\u51fd\u6570\u5b9a\u4e49\u5982\u4e0b\uff1a \nfunction [angle, min_idx] = cal_distance(rd_x, rd_y, tree) distance = []; i = 1; while i<=size(tree.child,1) dx = rd_x - tree.child(i,1); dy = rd_y - tree.child(i,2); d = sqrt(dx^2 + dy^2); distance(i) = d; i = i+1; end [~, min_idx] = min(distance); angle = atan2(rd_y - tree.child(min_idx,2),rd_x - tree.child(min_idx,1));end \n\u968f\u540e\u751f\u6210\u65b0\u8282\u70b9\u3002 \nnew_node_x = tree.child(min_idx,1)+grow_distance*cos(angle);% \u751f\u6210\u65b0\u7684\u8282\u70b9new_node_y = tree.child(min_idx,2)+grow_distance*sin(angle);new_node = [new_node_x, new_node_y]; \n\u63a5\u4e0b\u6765\u9700\u8981\u5bf9\u8be5\u8282\u70b9\u8fdb\u884c\u5224\u65ad\uff1a \n\u2460 \u65b0\u8282\u70b9\u662f\u5426\u5728\u969c\u788d\u7269\u8303\u56f4\u5185\uff1b \n\u2461 \u00a0\u65b0\u8282\u70b9\u548c\u7236\u8282\u70b9\u7684\u8fde\u7ebf\u7ebf\u6bb5\u662f\u5426\u548c\u969c\u788d\u7269\u6709\u91cd\u5408\u90e8\u5206\u3002 \n\u82e5\u4efb\u610f\u4e00\u70b9\u4e0d\u6ee1\u8db3\uff0c\u5219\u5c06\u6807\u5fd7\u4f4d\u7f6e\u4e3a0\u3002\u5b9e\u9645\u4e0a\u53ef\u4ee5\u5c06\u4e24\u4e2a\u5224\u65ad\u7ed3\u5408\uff0c\u5373\u5224\u65ad\u65b0\u8282\u70b9\u548c\u7236\u8282\u70b9\u7684\u8fde\u7ebf\u7ebf\u6bb5\u4e0a\u7684\u70b9\u662f\u5426\u5728\u969c\u788d\u7269\u8303\u56f4\u5185\u3002 \nfor k=1:1:size(ob,1) for i=min(tree.child(min_idx,1),new_node_x):0.01:max(tree.child(min_idx,1),new_node_x) % \u5224\u65ad\u751f\u957f\u4e4b\u540e\u8def\u5f84\u4e0e\u969c\u788d\u7269\u6709\u65e0\u4ea4\u53c9\u90e8\u5206 j = (tree.child(min_idx,2) - new_node_y)\/(tree.child(min_idx,1) - new_node_x) *(i - new_node_x) + new_node_y; if(i >=ob(k,1)-resolution && i <= ob(k,1)+ob(k,3) && j >= ob(k,2)-resolution && j <= ob(k,2)+ob(k,4)) flag = 0; break end endend \n\u5728\u8fd9\u6211\u91c7\u7528\u7684\u65b9\u6cd5\u662f\u5199\u51fa\u65b0\u8282\u70b9\u548c\u7236\u8282\u70b9\u8fde\u7ebf\u7684\u76f4\u7ebf\u65b9\u7a0b\uff0c\u7136\u540e\u5c06x\u53d8\u5316\u8303\u56f4\u9650\u5236\u5728min(tree.child(min_idx,1),new_node_x)\u5230max(tree.child(min_idx,1),new_node_x)\u5185\uff0c0.01\u5373\u5750\u6807\u53d8\u6362\u7684\u6b65\u957f\uff0c\u6b65\u957f\u8d8a\u5c0f\u5224\u65ad\u7684\u8d8a\u7cbe\u786e\uff0c\u4f46\u540c\u65f6\u4f1a\u589e\u52a0\u8ba1\u7b97\u91cf\uff1b\u6b65\u957f\u8d8a\u5927\u8ba1\u7b97\u901f\u5ea6\u5feb\u4f46\u662f\u5f88\u53ef\u80fd\u51fa\u73b0\u8bef\u5224\uff0c\u5982\u4e0b\u56fe\u6240\u5f0f\u3002 \n \n\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u5de6\u56fe\uff1a\u5408\u9002\u7684\u6b65\u957f\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0\u00a0\u00a0 \u00a0 \u53f3\u56fe\uff1a\u6b65\u957f\u8fc7\u5927 \n\u5224\u65ad\u6807\u5fd7\u4f4d\u82e5\u4e3a1\uff0c\u5219\u53ef\u4ee5\u5c06\u8be5\u65b0\u8282\u70b9\u52a0\u5165\u5230tree\u4e2d\uff0c\u6ce8\u610f\u4fdd\u5b58\u65b0\u8282\u70b9\u548c\u5b83\u7684\u7236\u8282\u70b9\uff0c\u540c\u65f6\u663e\u793a\u5728figure\u4e2d\uff0c\u4e4b\u540e\u91cd\u7f6e\u6807\u5fd7\u4f4d\u3002 \nif (flag == true) % \u82e5\u6807\u5fd7\u4f4d\u4e3a1\uff0c\u5219\u53ef\u4ee5\u5c06\u8be5\u65b0\u8282\u70b9\u52a0\u5165tree\u4e2d tree.child(end+1,:) = new_node; tree.parent(end+1,:) = [tree.child(min_idx,1), tree.child(min_idx,2)]; plot(rd_x, rd_y, '.r');hold on plot(new_node_x, new_node_y,'.g');hold on plot([tree.child(min_idx,1),new_node_x], [tree.child(min_idx,2),new_node_y],'-b');end flag = 1; % \u6807\u5fd7\u4f4d\u5f52\u4f4d \n\u6700\u540e\u5c31\u662f\u628a\u969c\u788d\u7269\u3001\u8d77\u70b9\u7ec8\u70b9\u7b49\u663e\u793a\u5728figure\u4e2d\uff0c\u5e76\u5224\u65ad\u65b0\u8282\u70b9\u5230\u76ee\u6807\u70b9\u8ddd\u79bb\u3002\u82e5\u5c0f\u4e8e\u9608\u503c\u5219\u505c\u6b62\u641c\u7d22\uff0c\u5e76\u5c06\u76ee\u6807\u70b9\u52a0\u5165\u5230node\u4e2d\uff0c\u5426\u5219\u91cd\u590d\u8be5\u8fc7\u7a0b\u76f4\u81f3\u627e\u5230\u76ee\u6807\u70b9\u3002 \n%% \u663e\u793afor i=1:1:size(ob,1) % \u7ed8\u5236\u969c\u788d\u7269 fill([ob(i,1)-resolution, ob(i,1)+ob(i,3),ob(i,1)+ob(i,3),ob(i,1)-resolution],... [ob(i,2)-resolution,ob(i,2)-resolution,ob(i,2)+ob(i,4),ob(i,2)+ob(i,4)],'k');endhold on plot(x_start(1,1)-0.5*resolution, x_start(1,2)-0.5*resolution,'b^','MarkerFaceColor','b','MarkerSize',4*resolution); % \u8d77\u70b9plot(goal(1,1)-0.5*resolution, goal(1,2)-0.5*resolution,'m^','MarkerFaceColor','m','MarkerSize',4*resolution); % \u7ec8\u70b9set(gca,'XLim',[x_left_limit x_right_limit]); % X\u8f74\u7684\u6570\u636e\u663e\u793a\u8303\u56f4set(gca,'XTick',[x_left_limit:resolution:x_right_limit]); % \u8bbe\u7f6e\u8981\u663e\u793a\u5750\u6807\u523b\u5ea6set(gca,'YLim',[y_left_limit y_right_limit]); % Y\u8f74\u7684\u6570\u636e\u663e\u793a\u8303\u56f4set(gca,'YTick',[y_left_limit:resolution:y_right_limit]); % \u8bbe\u7f6e\u8981\u663e\u793a\u5750\u6807\u523b\u5ea6grid ontitle('D-RRT');xlabel('\u6a2a\u5750\u6807 x'); ylabel('\u7eb5\u5750\u6807 y');pause(0.05);if (sqrt((new_node_x - goal(1,1))^2 + (new_node_y- goal(1,2))^2) <= goal_radius) % \u82e5\u65b0\u8282\u70b9\u5230\u76ee\u6807\u70b9\u8ddd\u79bb\u5c0f\u4e8e\u9608\u503c\uff0c\u5219\u505c\u6b62\u641c\u7d22\uff0c\u5e76\u5c06\u76ee\u6807\u70b9\u52a0\u5165\u5230node\u4e2d tree.child(end+1,:) = goal; % \u628a\u7ec8\u70b9\u52a0\u5165\u5230\u6811\u4e2d tree.parent(end+1,:) = new_node; disp('find goal!'); breakend \n5\u3001\u7ed8\u5236\u6700\u4f18\u8def\u5f84 \n\u4ece\u76ee\u6807\u70b9\u5f00\u59cb\uff0c\u4f9d\u6b21\u6839\u636e\u8282\u70b9\u53ca\u7236\u8282\u70b9\u56de\u63a8\u89c4\u5212\u7684\u8def\u5f84\u76f4\u81f3\u8d77\u70b9\uff0c\u8981\u6ce8\u610ftree\u7ed3\u6784\u4f53\u4e2dparent\u7684\u957f\u5ea6\u6bd4child\u8981\u5c0f1\u3002\u6700\u540e\u5c06\u89c4\u5212\u7684\u8def\u5f84\u663e\u793a\u5728figure\u4e2d\u3002 \n%% \u7ed8\u5236\u6700\u4f18\u8def\u5f84temp = tree.parent(end,:);trajectory = [tree.child(end,1)-0.5*resolution, tree.child(end,2)-0.5*resolution];for i=size(tree.child,1):-1:2 if(size(tree.child(i,:),2) ~= 0 & tree.child(i,:) == temp) temp = tree.parent(i-1,:); trajectory(end+1,:) = tree.child(i,:); if(temp == x_start) trajectory(end+1,:) = [temp(1,1) - 0.5*resolution, temp(1,2) - 0.5*resolution]; end endendplot(trajectory(:,1), trajectory(:,2), '-r','LineWidth',2);pause(2); \n\u7a0b\u5e8f\u8fd0\u884c\u6700\u7ec8\u6548\u679c\u5982\u4e0b\uff1a \n \n\u00a0\u7ea2\u70b9\u90fd\u662f\u751f\u6210\u70b9\u968f\u673a\u70b9\uff0c\u7eff\u70b9\u662ftree\u4e2d\u8282\u70b9\uff0c\u7ea2\u8272\u8def\u5f84\u5373\u4e3aRRT\u7b97\u6cd5\u89c4\u5212\u7684\u8def\u5f84\u3002 \n6\u3001\u8def\u5f84\u5e73\u6ed1\uff08B\u6837\u6761\u66f2\u7ebf\uff09 \n\u7531\u4e8e\u89c4\u5212\u7684\u8def\u5f84\u90fd\u662f\u7ebf\u6bb5\u8fde\u63a5\uff0c\u5728\u8282\u70b9\u5904\u8def\u5f84\u4e0d\u5e73\u6ed1\uff0c\u8fd9\u4e5f\u662fRRT\u7b97\u6cd5\u7684\u5f0a\u7aef\u4e4b\u4e00\u3002\u4e00\u822c\u6765\u8bf4\u8f68\u8ff9\u5e73\u6ed1\u7684\u65b9\u6cd5\u6709\u5f88\u591a\u79cd\uff0c\u7c7b\u4f3c\u4e8e\u8d1d\u585e\u5c14\u66f2\u7ebf\uff0cB\u6837\u6761\u66f2\u7ebf\u7b49\u3002\u6211\u5728\u8fd9\u91c7\u7528B\u6837\u6761\u66f2\u7ebf\u5bf9\u89c4\u5212\u7684\u8def\u5f84\u8fdb\u884c\u5e73\u6ed1\u5904\u7406\uff0c\u5177\u4f53\u7684\u65b9\u6cd5\u548c\u539f\u7406\u6211\u540e\u7eed\u6709\u65f6\u95f4\u518d\u8fdb\u884c\u8bf4\u660e\uff0c\u8fd9\u91cc\u5148\u7ed9\u51fa\u7ed3\u679c\uff1a \n \n\u00a0\u9ed1\u8272\u66f2\u7ebf\u5373\u4f4d\u5e73\u6ed1\u5904\u7406\u540e\u7684\u8def\u5f84\u3002 \n\u56db\u3001\u591a\u7ec4\u7ed3\u679c\u5bf9\u6bd4 \n\u2460 \u76f8\u90bb\u4e24\u6b21\u4eff\u771f\u7ed3\u679c\u5bf9\u6bd4\uff1a \n \n\u53ef\u4ee5\u770b\u51fa\u7531\u4e8e\u968f\u673a\u91c7\u6837\u7684\u539f\u56e0\uff0c\u4efb\u610f\u4e24\u6b21\u89c4\u5212\u7684\u8def\u5f84\u90fd\u662f\u4e0d\u4e00\u6837\u7684\u3002\u00a0 \n\u2461 \u590d\u6742\u73af\u5883\u4e0b\u7684\u8def\u5f84\u89c4\u5212\u3002\u9009\u53d6\u4e00\u4e2a\u76f8\u5bf9\u590d\u6742\u7684\u73af\u5883\uff0c\u4eff\u771f\u7ed3\u679c\u5982\u4e0b\uff1a \n \n\u53ef\u4ee5\u770b\u51faRRT\u7b97\u6cd5\u53ef\u4ee5\u5f88\u597d\u89e3\u51b3\u590d\u6742\u73af\u5883\u4e0b\u7684\u8def\u5f84\u89c4\u5212\u95ee\u9898\u3002 \n\u2462 \u72ed\u7a84\u901a\u9053\u4e0b\u7684\u8def\u5f84\u89c4\u5212\u3002\u9009\u53d6\u4e00\u4e2a\u72ed\u7a84\u901a\u9053\u73af\u5883\uff0c\u4eff\u771f\u7ed3\u679c\u5982\u4e0b\uff1a \n \n\u00a0\u7531\u4e8e\u73af\u5883\u91c7\u6837\u7684\u968f\u673a\u6027\uff0c\u5728\u72ed\u957f\u901a\u9053\u5185\u751f\u6210\u968f\u673a\u70b9\u7684\u6982\u7387\u76f8\u5bf9\u8f83\u4f4e\uff0c\u5bfc\u81f4\u53ef\u80fd\u65e0\u6cd5\u89c4\u5212\u51fa\u8def\u5f84\u3002 \n\u4e94\u3001\u7ed3\u8bed \n\u7531\u6700\u7ec8\u4eff\u771f\u7ed3\u679c\u53ef\u4ee5\u770b\u51fa\uff0cRRT\u7b97\u6cd5\u901a\u8fc7\u5bf9\u7a7a\u95f4\u7684\u968f\u673a\u91c7\u6837\u53ef\u4ee5\u89c4\u5212\u51fa\u4e00\u6761\u4ece\u8d77\u70b9\u5230\u7ec8\u70b9\u7684\u8def\u5f84\uff0c\u89c4\u5212\u901f\u5ea6\u5f88\u5feb\uff0c\u540c\u65f6\u4e0d\u4f9d\u8d56\u4e8e\u73af\u5883\u3002\u4f46\u89c4\u5212\u8fc7\u7a0b\u968f\u673a\u6027\u5f88\u5f3a\uff0c\u6ca1\u6709\u76ee\u7684\u6027\uff0c\u4f1a\u4ea7\u751f\u5f88\u591a\u5197\u4f59\u70b9\uff0c\u4e14\u6bcf\u6b21\u89c4\u5212\u7684\u8def\u5f84\u90fd\u4e0d\u4e00\u6837\uff0c\u5bf9\u4e8e\u72ed\u7a84\u901a\u9053\u53ef\u80fd\u65e0\u6cd5\u89c4\u5212\u51fa\u8def\u5f84\u3002 \n\u4e0b\u7bc7\u6587\u7ae0\u6211\u5c06\u5bf9RRT\u7b97\u6cd5\u7684\u4f18\u5316\u63d0\u51fa\u4e00\u4e9b\u81ea\u5df1\u7684\u60f3\u6cd5\uff0c\u5e76\u5728\u73b0\u6709\u7684\u7a0b\u5e8f\u4e0a\u8fdb\u884c\u4fee\u6539\uff0c\u6700\u7ec8\u5bf9\u6bd4\u6539\u8fdb\u524d\u540e\u7684RRT\u7b97\u6cd5\u6548\u679c\u3002 \n \n\u6587\u4e2d\u5982\u6709\u9519\u8bef\u6216\u4fb5\u6743\u7684\u5730\u65b9\u8fd8\u6b22\u8fce\u5404\u4f4d\u6307\u51fa\uff0c\u6211\u4f1a\u53ca\u65f6\u56de\u590d\u5e76\u8fdb\u884c\u4fee\u6539\u3002 \n \nPS\uff1a\u9700\u8981matlab\u6e90\u7801\u7684\u670b\u53cb\u53ef\u4ee5\u5728\u8bc4\u8bba\u533a\u7559\u4e0b\u90ae\u7bb1\u3002\n<\/p>\n","protected":false},"excerpt":{"rendered":"
2022-04-02 \u524d\u8a00\uff1a\u7531\u4e8e\u540e\u7eed\u53ef\u80fd\u8981\u505a\u4e00 […]<\/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-322","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\/322"}],"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=322"}],"version-history":[{"count":1,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/322\/revisions"}],"predecessor-version":[{"id":325,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/322\/revisions\/325"}],"wp:attachment":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/media?parent=322"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/categories?post=322"},{"taxonomy":"post_tag","embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/tags?post=322"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}