{"id":16846,"date":"2024-10-14T14:37:17","date_gmt":"2024-10-14T06:37:17","guid":{"rendered":"http:\/\/192.168.10.115\/?p=16846"},"modified":"2024-10-14T14:37:34","modified_gmt":"2024-10-14T06:37:34","slug":"2024-10-14-%e6%97%a0%e4%ba%ba%e9%a9%be%e9%a9%b6%e8%b7%af%e5%be%84%e8%a7%84%e5%88%92%ef%bc%88%e4%b8%89%ef%bc%89%e5%b1%80%e9%83%a8%e8%b7%af%e5%be%84%e8%a7%84%e5%88%92-frenet%e5%9d%90%e6%a0%87%e7%b3%bb","status":"publish","type":"post","link":"http:\/\/222.128.65.89:18086\/index.php\/2024\/10\/14\/16846\/","title":{"rendered":"2024-10-14 \u65e0\u4eba\u9a7e\u9a76\u8def\u5f84\u89c4\u5212\uff08\u4e09\uff09\u5c40\u90e8\u8def\u5f84\u89c4\u5212-Frenet\u5750\u6807\u7cfb\u4e0b\u7684\u52a8\u6001\u8f68\u8ff9\u89c4\u5212"},"content":{"rendered":"\n

\u4e00\u3001\u8f68\u8ff9\u89c4\u5212\u65b9\u6cd5\u7b80\u4ecb<\/p>\n\n\n\n

\u5728\u7b2c\u4e00\u7bc7\u6587\u7ae0\u4e2d\u6211\u5bf9\u65e0\u4eba\u9a7e\u9a76\u8def\u5f84\u89c4\u5212\u6280\u672f\u505a\u4e86\u4e2a\u7b80\u5355\u7684\u4ecb\u7ecd\uff0c\u5373\u53ef\u4ee5\u5206\u4e3a\u5168\u5c40\u8def\u5f84\u89c4\u5212\u548c\u5c40\u90e8\u8def\u5f84\u89c4\u5212\u4e24\u90e8\u5206\u3002\u4eca\u5929\u4ecb\u7ecd\u7684Frenet\u5750\u6807\u7cfb\u4e0b\u52a8\u6001\u8f68\u8ff9\u89c4\u5212\u5c31\u5c5e\u4e8e\u5c40\u90e8\u8def\u5f84\u89c4\u5212\u7684\u5185\u5bb9\u3002\u5728\u8fd9\u6211\u5148\u7b80\u8981\u4ecb\u7ecd\u4e00\u4e0b\u8f68\u8ff9\u89c4\u5212\u7684\u65b9\u6cd5\u3002\u53c2\u8003\u8bba\u6587\uff1a\u57fa\u4e8e\u6fc0\u5149\u96f7\u8fbe\u7684\u73af\u5883\u8bc6\u522b\u53ca\u5c40\u90e8\u8def\u5f84\u89c4\u5212\u6280\u672f\u7814\u7a76\u3002<\/p>\n\n\n\n

\u65e0\u4eba\u8f66\u7684\u5c40\u90e8\u8def\u5f84\u89c4\u5212\u4e0d\u4ec5\u9700\u8981\u8003\u8651\u5230\u9053\u8def\u4ea4\u901a\u4e2d\u7684\u9759\u6001\u969c\u788d\u7269\u7684\u907f\u969c\u95ee\u9898\uff0c\u8fd8\u9700\u8981\u5bf9\u73af\u5883\u4e2d\u884c\u4eba\u3001\u8f66\u8f86\u7b49\u52a8\u6001\u969c\u788d\u7269\u672a\u6765\u8f68\u8ff9\u505a\u51fa\u7684\u9884\u5224\uff0c\u5bf9\u672a\u6765\u65f6\u95f4\u6bb5\u5185\u7684\u78b0\u649e\u53ef\u80fd\u6027\u8fdb\u884c\u5206\u6790\uff0c\u4fdd\u8bc1\u8f66\u8f86\u5b89\u5168\u3001\u8212\u9002\u7684\u884c\u9a76\uff0c\u56e0\u800c\u5728\u6c42\u89e3\u8fc7\u7a0b\u4e2d\u9700\u8981\u8003\u8651\u65f6\u95f4\u7ef4\u5ea6 t\u3002\u5f53\u8def\u5f84\u70b9\u589e\u52a0\u4e86\u65f6\u95f4\u8fd9\u4e00\u7ef4\u5ea6\uff0c\u5b9e\u8d28\u4e0a\u8f6c\u53d8\u4e3a\u4e86\u8f68\u8ff9\u70b9 ( x,y,t)\uff0c\u56e0\u800c\u5728\u81ea\u52a8\u9a7e\u9a76\u6280\u672f\u9886\u57df\u5c40\u90e8\u8def\u5f84\u89c4\u5212\u4e5f\u53ef\u4ee5\u79f0\u4e3a\u8f68\u8ff9\u89c4\u5212\u3002\u8f66\u8f86\u5728\u4e09\u7ef4\u7a7a\u95f4\u5185\u7684\u8f68\u8ff9\u89c4\u5212\uff0c\u4e00\u822c\u6709\u4e09\u79cd\u5e38\u7528\u7684\u7b56\u7565\uff1a<\/p>\n\n\n\n

\u4e00\u5c06\u4e09\u7ef4\u7a7a\u95f4\u79bb\u6563\u5316\uff0c\u901a\u8fc7\u542f\u53d1\u5f0f\u641c\u7d22\u7b97\u6cd5\u8fdb\u884c\u6c42\u89e3\u3002\n\u4e8c\u662f\u5c06\u8f68\u8ff9\u5206\u89e3\u4e3a\u8def\u5f84\u89c4\u5212\u548c\u901f\u5ea6\u89c4\u5212\u4e24\u4e2a\u8fc7\u7a0b\u3002\u8be5\u65b9\u6cd5\u7684\u601d\u60f3\u5728\u4e8e\u901a\u8fc7\u8def\u5f84\u89c4\u5212\u5b8c\u6210\u9759\u6001\u73af\u5883\u4e0b\u8def\u5f84\u6c42\u89e3\uff0c\u7136\u540e\u4f7f\u7528\u901f\u5ea6\u89c4\u5212\u65b9\u6cd5\u89e3\u51b3\u8fd0\u52a8\u969c\u788d\u7269\u7684\u907f\u649e\u95ee\u9898\u3002\n\u4e09\u662f\u5c06\u8f68\u8ff9\u5206\u89e3\u4e3a\u4e24\u4e2a\u65b9\u5411\u5173\u4e8e\u65f6\u95f4\u7684\u4f18\u5316\u6c42\u89e3\u95ee\u9898\u3002\u6700\u65e9\u7531\u4e8e Werling \u63d0\u51fa\u5c06\u4e09\u7ef4\u7684\u8f68\u8ff9\u89c4\u5212\u5206\u89e3\u4e3a\u6a2a\u5411\u7684\u8fd0\u52a8\u89c4\u5212\u548c\u7eb5\u5411\u7684\u8fd0\u52a8\u89c4\u5212\u95ee\u9898\uff0c\u8fd9\u91cc\u7684\u6a2a\u5411\u662f\u6307\u8f66\u8f86\u76f8\u5bf9\u4e8e\u9053\u8def\u4e2d\u5fc3\u7684\u6cd5\u7ebf\u65b9\u5411\uff0c\u7eb5\u5411\u6307\u4e2d\u5fc3\u7ebf\u7684\u5207\u7ebf\u65b9\u5411\u3002<\/p>\n\n\n\n

\u90a3\u4e48Frenet\u5750\u6807\u7cfb\u4e0b\u52a8\u6001\u8f68\u8ff9\u89c4\u5212\u5b9e\u9645\u4e0a\u5c31\u5c5e\u4e8e\u7b2c\u4e09\u79cd\uff0c\u5c06\u89c4\u5212\u95ee\u9898\u5206\u89e3\u4e3a\u7eb5\u5411\u548c\u6a2a\u5411\u6765\u5206\u522b\u6c42\u89e3\u3002<\/p>\n\n\n\n

\u4e8c\u3001Frenet\u5750\u6807\u7cfb\u8f6c\u6362<\/p>\n\n\n\n

1\u3001Frenet\u5750\u6807\u7cfb\u6982\u5ff5<\/p>\n\n\n\n

\u5728\u8f68\u8ff9\u89c4\u5212\u8fc7\u7a0b\u4e2d\uff0c\u4f7f\u7528\u7b1b\u5361\u5c14\u76f4\u89d2\u5750\u6807\u7cfb\u5e76\u4e0d\u80fd\u5f88\u597d\u7684\u63cf\u8ff0\u8f66\u8f86\u5f53\u524d\u4f4d\u7f6e\u4e0e\u5f53\u524d\u6240\u5728\u8f66\u9053\u7684\u5173\u7cfb\uff0c\u56e0\u6b64\u9700\u8981\u5f15\u5165Frenet\u5750\u6807\u7cfb\u7684\u6982\u5ff5\u3002\u5728Frenet\u5750\u6807\u7cfb\u4e2d\uff0c\u4f7f\u7528\u9053\u8def\u4e2d\u5fc3\u7ebf\u4f5c\u4e3a\u53c2\u8003\u7ebf\uff0c\u5c06\u8f66\u8f86\u7684\u8f68\u8ff9\u70b9\u6295\u5f71\u5230\u53c2\u8003\u7ebf\u4e0a\u5f97\u5230\u53c2\u8003\u70b9\uff0c\u4ee4\u6cbf\u53c2\u8003\u7ebf\u65b9\u5411\u4e3a\u7eb5\u8f74s\uff0c\u5782\u76f4\u4e8e\u53c2\u8003\u7ebf\u65b9\u5411\u4e3a\u6a2a\u8f74d\u3002Frenet\u5750\u6807\u7cfb\u53ef\u4ee5\u5f88\u5bb9\u6613\u786e\u5b9a\u8f66\u8f86\u504f\u79bb\u8f66\u9053\u4e2d\u5fc3\u7ebf\u7684\u8ddd\u79bb\u53ca\u6cbf\u7740\u8f66\u9053\u7ebf\u884c\u9a76\u7684\u8ddd\u79bb\uff0c\u53ef\u4ee5\u5ffd\u7565\u9053\u8def\u66f2\u7387\u7684\u5f71\u54cd\uff0c\u76f8\u6bd4\u4e8e\u7b1b\u5361\u5c14\u5750\u6807\u7cfb\u4e0b\u7684\u63cf\u8ff0\u66f4\u4e3a\u7b80\u6d01\u548c\u76f4\u89c2\u3002 Frenet\u5750\u6807\u7cfb\u548c\u7b1b\u5361\u5c14\u5750\u6807\u7cfb\u5173\u7cfb\u5982\u4e0b\uff1a<\/p>\n\n\n\n

\"\"<\/figure>\n\n\n\n

2\u3001Frenet-\u7b1b\u5361\u5c14\u5750\u6807\u7cfb\u8f6c\u6362
<\/p>\n\n\n\n

\"\"<\/figure>\n\n\n\n
\"\"<\/figure>\n\n\n\n

\u4e09\u3001\u52a8\u6001\u8f68\u8ff9\u89c4\u5212\u8fc7\u7a0b<\/p>\n\n\n\n

1\u3001\u57fa\u4e8ejerk\u5b9e\u65f6\u8f68\u8ff9\u89c4\u5212<\/p>\n\n\n\n

\"\"<\/figure>\n\n\n\n

2\u3001\u6a2a\u5411\u8f68\u8ff9\u89c4\u5212\u6c42\u89e3<\/p>\n\n\n\n

\"\"<\/figure>\n\n\n\n
\"\"<\/figure>\n\n\n\n

3\u3001\u7eb5\u5411\u8f68\u8ff9\u89c4\u5212\u6c42\u89e3<\/p>\n\n\n\n

\"\"<\/figure>\n\n\n\n
\"\"<\/figure>\n\n\n\n

4\u3001\u6700\u4f18\u8f68\u8ff9\u9009\u62e9<\/p>\n\n\n\n

\"\"<\/figure>\n\n\n\n

5\u3001\u78b0\u649e\u68c0\u6d4b<\/p>\n\n\n\n

Atsushi Sakai\u5728Python\u4ee3\u7801\u4e2d\u8bbe\u7f6e\u7684\u78b0\u649e\u68c0\u6d4b\u65b9\u6cd5\u662f\uff1a<\/p>\n\n\n\n

\u5047\u8bbe\u8f66\u8f86\u5728\u5907\u9009\u8f68\u8ff9\u4e00\u4e2a\u91c7\u6837\u65f6\u523b\u5185\u6cbf\u53c2\u8003\u7ebf\u65b9\u5411\u884c\u9a76\u7eb5\u5411\u8ddd\u79bbs\uff0c\u82e5\u7ed3\u675f\u540e\u8f66\u8f86\u4e0e\u969c\u788d\u7269\u7684\u8ddd\u79bb\u5c0f\u4e8e\u8bbe\u5b9a\u7684\u9608\u503c\uff0c\u5219\u8be5\u8f68\u8ff9\u4e0d\u6ee1\u8db3\u78b0\u649e\u68c0\u6d4b\u9700\u6c42\uff0c\u8fdb\u884c\u5254\u9664\u3002\u5bf9\u6240\u6709\u5907\u9009\u8f68\u8ff9\u8fdb\u884c\u8be5\u9879\u64cd\u4f5c\u3002<\/p>\n\n\n\n

\u6211\u5728matlab\u79fb\u690d\u4ee3\u7801\u7684\u65f6\u5019\uff0c\u6ca1\u6709\u5bf9\u8fd9\u90e8\u5206\u8fdb\u884c\u4fee\u6539\u3002<\/p>\n\n\n\n

\u56db\u3001Python-Matlab\u4ee3\u7801\u79fb\u690d<\/p>\n\n\n\n

\u6211\u5728\u7f51\u4e0a\u627e\u5230\u7684\u7b97\u6cd5\u7684Python\u4ee3\u7801\u662f\u65e5\u672c\u65e0\u4eba\u9a7e\u9a76\u5de5\u7a0b\u5e08Atsushi Sakai\u7f16\u5199\u7684\uff0c\u5e76\u5df2\u7ecf\u5728github\u4e0a\u5f00\u6e90\u3002\u6211\u5728\u6587\u672b\u4e5f\u7ed9\u51fa\u4e86\u6e90\u7801\u7684\u94fe\u63a5\uff0c\u6709\u9700\u8981\u7684\u8bfb\u8005\u53ef\u4ee5\u81ea\u884c\u83b7\u53d6\u3002<\/p>\n\n\n\n

\u4e3a\u4e86\u52a0\u6df1\u5bf9\u7b97\u6cd5\u7684\u7406\u89e3\uff0c\u540c\u65f6\u4fdd\u6301\u4e4b\u524d\u6240\u6709\u7b97\u6cd5\u9a8c\u8bc1\u5e73\u53f0\u7684\u4e00\u81f4\u6027\uff0c\u6211\u5c06Python\u6e90\u7801\u79fb\u690d\u5230\u4e86matlab2019\u3002\u5728\u8fd9\u6211\u4e0d\u5bf9Python\u6e90\u7801\u8fdb\u884c\u89e3\u91ca\uff0c\u4ec5\u5bf9\u6211\u79fb\u690d\u540e\u7684matlab\u4ee3\u7801\u8fdb\u884c\u4e00\u4e2a\u4ecb\u7ecd\u3002<\/p>\n\n\n\n

1\u3001\u521d\u59cb\u53c2\u6570\u5b9a\u4e49\uff08\u5b8f\u5b9a\u4e49\uff09<\/p>\n\n\n\n

\u5728\u7a0b\u5e8f\u6700\u5f00\u59cb\u5148\u7ed9\u51fa\u9700\u8981\u7528\u5230\u7684\u53c2\u6570\u7684\u5b9a\u4e49\u53ca\u6570\u503c\uff0c\u4f8b\u5982\u6700\u5927\u91c7\u6837\u6a2a\u5411\u8ddd\u79bb\u3001\u6a2a\u5411\u8ddd\u79bb\u91c7\u6837\u95f4\u9694\u3001\u6700\u5927\u91c7\u6837\u65f6\u95f4\u3001\u91c7\u6837\u65f6\u95f4\u95f4\u9694\u3001\u6700\u5927\u52a0\u901f\u5ea6\/\u901f\u5ea6\u7b49\u7b49\uff0c\u5177\u4f53\u5982\u4e0b\uff1a<\/p>\n\n\n\n

SIM_LOOP = 500;\n \nMAX_SPEED = 50.0 \/ 3.6;  % \u6700\u5927\u901f\u5ea6 [m\/s]\nMAX_ACCEL = 2.0;  % \u6700\u5927\u52a0\u901f\u5ea6 [m\/ss]\nMAX_CURVATURE = 1.0;  % \u6700\u5927\u66f2\u7387 [1\/m]\nMAX_ROAD_WIDTH = 7.0;  % \u6700\u5927\u91c7\u6837\u6a2a\u5411\u8ddd\u79bb [m]\nD_ROAD_W = 1.0;  % \u6a2a\u5411\u8ddd\u79bb\u91c7\u6837\u95f4\u9694 [m]\nDT = 0.2;  % \u91c7\u6837\u65f6\u95f4\u95f4\u9694 [s]\nMAX_T = 5.0;  % \u6700\u5927\u91c7\u6837\u65f6\u95f4 [s]\nMIN_T = 4.0;  % \u6700\u5c0f\u91c7\u6837\u65f6\u95f4 [s]\nTARGET_SPEED = 30.0 \/ 3.6;  % \u671f\u671b\u901f\u5ea6 [m\/s]\nD_T_S = 5.0 \/ 3.6;  % \u7eb5\u5411\u901f\u5ea6\u91c7\u6837\u95f4\u9694 [m\/s]\nN_S_SAMPLE = 1;\nROBOT_RADIUS = 2;  % \u78b0\u649e\u68c0\u6d4b\u9608\u503c [m]\nrobot_width = 1;   % \u673a\u5668\u4eba\u5bbd [m]\nrobot_length = 2;  % \u673a\u5668\u4eba\u957f [m]\n \n% \u8bc4\u4ef7\u51fd\u6570\u6743\u91cd\nK_J = 0.1;\nK_T = 0.1;\nK_D = 1.0;\nK_LAT = 1.0;\nK_LON = 1.0;\n \n%% \u53c2\u6570\u8303\u56f4\u786e\u5b9a\nc_speed = 10.0 \/ 3.6;   % \u5f53\u524d\u901f\u5ea6\nc_d = 2.0;              % \u5f53\u524d\u6a2a\u5411\u4f4d\u79fb\nc_d_d = 0.0;            % \u5f53\u524d\u6a2a\u5411\u901f\u5ea6\nc_d_dd = 0.0;           % \u5f53\u524d\u6a2a\u5411\u52a0\u901f\u5ea6\ns0 = 0.0;               % \u5f53\u524d\u6cbf\u8f66\u9053\u7ebf\u4f4d\u79fb\narea = 20.0;<\/code><\/pre>\n\n\n\n
2\u3001\u969c\u788d\u7269\u70b9\u53ca\u53c2\u8003\u8def\u5f84\u751f\u6210<\/p>\n\n\n\n
\u539f\u6587\u4e2d\u662f\u7ed9\u5b9a\u4e86\u51e0\u4e2a\u53c2\u8003\u8def\u5f84\u70b9\uff0c\u91c7\u7528\u4e09\u6b21\u6837\u6761\u7684\u65b9\u6cd5\u751f\u6210\u53c2\u8003\u8def\u5f84\u3002<\/p>\n\n\n\n
%% \u53c2\u8003\u8def\u5f84\u70b9\nnodes = [0, 0;\n        10, -6;\n        20.5 5;\n        35, 6.5;\n        70.5, 0;\n        100, 5];\n \n%% \u8bbe\u7f6e\u969c\u788d\u7269\u5750\u6807\u70b9\nob = [20, 10\n      30, 9;\n      30, 6;\n      35, 9;\n      50, 3;\n      75, 0];\n%% \u751f\u6210\u671f\u671b\u8def\u5f84\ncsp = Cubic_spline(nodes);<\/code><\/pre>\n\n\n\n
Cubic_spline\u662f\u6211\u81ea\u5b9a\u4e49\u7684\u4e09\u6b21\u6837\u6761\u751f\u6210\u51fd\u6570\u3002<\/p>\n\n\n\n
3\u3001\u5b9a\u4e49FrenetPath\u7ed3\u6784\u4f53<\/p>\n\n\n\n
\u5b9a\u4e49FrenetPath\u7ed3\u6784\u4f53\uff0c\u5b58\u50a8\u6bcf\u4e00\u4e2a\u5907\u9009\u8def\u5f84\u7684\u6a2a\u5411\u3001\u7eb5\u5411\u89c4\u5212\u7ed3\u679c\uff0c\u53ca\u7b1b\u5361\u5c14\u5750\u6807\u7cfb\u4e0b\u7684\u72b6\u6001\u91cf\uff1a<\/p>\n\n\n\n
%%  Frenet\u8f68\u8ff9\u7ed3\u6784\u4f53\nFrenetPath.t = [];\nFrenetPath.d = [];\nFrenetPath.d_d = [];\nFrenetPath.d_dd = [];\nFrenetPath.d_ddd = [];\nFrenetPath.s = [];\nFrenetPath.s_d = [];\nFrenetPath.s_dd = [];\nFrenetPath.s_ddd = [];\nFrenetPath.cd = 0.0;\nFrenetPath.cv = 0.0;\nFrenetPath.cf = 0.0;\n \nFrenetPath.x = [];\nFrenetPath.y = [];\nFrenetPath.yaw = [];\nFrenetPath.ds = [];\nFrenetPath.c = [];<\/code><\/pre>\n\n\n\n
4\u3001Frenet\u52a8\u6001\u8f68\u8ff9\u89c4\u5212\u7b97\u6cd5<\/p>\n\n\n\n
\u8fd9\u4e00\u90e8\u5206\u662f\u8be5\u7b97\u6cd5\u7684\u6838\u5fc3\u51fd\u6570\uff0c\u5373\u5b8c\u6210\u4e86\u7eb5\u5411\u3001\u6a2a\u5411\u8f68\u8ff9\u7684\u89c4\u5212\u8fc7\u7a0b\uff0c\u5907\u9009\u8f68\u8ff9\u7684\u8bc4\u4ef7\u8fc7\u7a0b\u53ca\u78b0\u649e\u68c0\u6d4b\u8fc7\u7a0b\u3002\u6700\u540e\u5c06\u5f53\u524d\u65f6\u523b\u6700\u4f18\u8f68\u8ff9\u8f93\u51fa\u7ed9path\uff0cpath\u5c5e\u4e8eFrenetPath\u7ed3\u6784\u4f53\u7c7b\u578b\u3002<\/p>\n\n\n\n
path =  calc_frenet_paths_fixed_velocity(csp, c_speed, c_d, c_d_d, c_d_dd, ...\n                               s0,MAX_ROAD_WIDTH,D_ROAD_W, MIN_T, MAX_T, DT, ...\n                               FrenetPath, TARGET_SPEED,D_T_S, N_S_SAMPLE, ...\n                               K_D, K_J, K_LAT, K_LON, K_T, ob, MAX_ACCEL, ...\n                               MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS);<\/code><\/pre>\n\n\n\n
calc_frenet_path_fixed_velocity\u51fd\u6570\u5177\u4f53\u5185\u5bb9\u5982\u4e0b\uff1a<\/p>\n\n\n\n
function fplist = calc_frenet_paths_fixed_velocity(csp, c_speed, c_d, c_d_d, c_d_dd, ...\n                               s0,MAX_ROAD_WIDTH,D_ROAD_W, MIN_T, MAX_T, DT, ...\n                               FrenetPath, TARGET_SPEED,D_T_S, N_S_SAMPLE, ...\n                               K_D, K_J, K_LAT, K_LON, K_T, ob, MAX_ACCEL, ...\n                               MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS);\n    min_cost = [inf,inf];\n    cost = [];\n    flag = 1;\n    for di=-MAX_ROAD_WIDTH:D_ROAD_W:MAX_ROAD_WIDTH\n        for Ti = MIN_T:DT:MAX_T\n            fp = FrenetPath;\n            lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0, 0, Ti);\n            \n            for t=0:DT:Ti-DT\n                fp.t(1,end+1) = t;\n            end\n            \n            for t=fp.t(1,1):DT:fp.t(1,end)\n                fp.d(1,end+1)     = lat_qp.a0   + lat_qp.a1*t    + lat_qp.a2*t^2    + lat_qp.a3*t^3   + lat_qp.a4*t^4  + lat_qp.a5*t^5;\n                fp.d_d(1,end+1)   = lat_qp.a1   + 2*lat_qp.a2*t  + 3*lat_qp.a3*t^2  + 4*lat_qp.a4*t^3 + 5*lat_qp.a5*t^4;\n                fp.d_dd(1,end+1)  = 2*lat_qp.a2 + 6*lat_qp.a3*t  + 12*lat_qp.a4*t^2 + 20*lat_qp.a5*t^3;\n                fp.d_ddd(1,end+1) = 6*lat_qp.a3 + 24*lat_qp.a4*t + 60*lat_qp.a5*t^2;\n            end\n            \n            for tv = TARGET_SPEED - D_T_S * N_S_SAMPLE : D_T_S: TARGET_SPEED + D_T_S * N_S_SAMPLE\n                tfp = fp;\n                lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti);\n                \n                for t=fp.t(1,1):DT:fp.t(1,end)\n                    tfp.s(1,end+1)     = lon_qp.b0   + lon_qp.b1*t   + lon_qp.b2*t^2   + lon_qp.b3*t^3  + lon_qp.b4*t^4;\n                    tfp.s_d(1,end+1)   = lon_qp.b1   + 2*lon_qp.b2*t + 3*lon_qp.b3*t^2 + 4*lon_qp.b4*t^3;\n                    tfp.s_dd(1,end+1)  = 2*lon_qp.b2 + 6*lon_qp.b3*t + 12*lon_qp.b4*t^2 ;\n                    tfp.s_ddd(1,end+1) = 6*lon_qp.b3 + 24*lon_qp.b4*t;\n                end\n                Jp = sum(tfp.d_ddd .^2);\n                Js = sum(tfp.s_ddd .^2);\n                \n                ds = (TARGET_SPEED - tfp.s_d(1,end))^ 2;\n                tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d(1,end)^2;\n                tfp.cv = K_J * Js + K_T * Ti + K_D * ds;\n                tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv;\n                \n                tfp = calc_global_paths(tfp, csp);\n                flag = check_paths(tfp, ob, MAX_ACCEL, MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS);  \n \n                if (flag == 1)\n                    cost(end+1,:) = [tfp.cf, di];\n                    if min_cost(end,1) >= tfp.cf\n                        min_cost(end+1,:) = [tfp.cf, di];\n                        fplist = tfp;\n                    end\n                else\n                    flag = 1;\n                end\n                \n            end\n        end\n    end\nend<\/code><\/pre>\n\n\n\n
\u5176\u4e2dQuinticPolynomial\u548cQuarticPolynomial\u51fd\u6570\u5c31\u662f\u8f68\u8ff9\u89c4\u5212\u7684\u5177\u4f53\u6c42\u89e3\u516c\u5f0f\uff1a<\/p>\n\n\n\n
function lat_qp = QuinticPolynomial(xs, vxs, axs, xe, vxe, axe, time)\n    lat_qp.a0 = xs;\n    lat_qp.a1 = vxs;\n    lat_qp.a2 = axs\/2;\n    A = [time^3,   time^4,     time^5;\n         3*time^2, 4*time^3,   5*time^4;\n         6*time,   12*time^2,  20*time^3];\n    B = [xe - lat_qp.a0 - lat_qp.a1*time - lat_qp.a2*time^2;\n         vxe - lat_qp.a1 - 2*lat_qp.a2*time;\n         axe - 2*lat_qp.a2];\n    temp = A^(-1)*B;\n    lat_qp.a3 = temp(1,1);\n    lat_qp.a4 = temp(2,1);\n    lat_qp.a5 = temp(3,1);   \nend<\/code><\/pre>\n\n\n\n
function lon_qp = QuarticPolynomial(xs, vxs, axs, vxe, axe, time)\n    lon_qp.b0 = xs;\n    lon_qp.b1 = vxs;\n    lon_qp.b2 = axs \/ 2.0;\n    A = [3*time^2, 4*time^3\n         6*time,   12*time^2];\n    B = [vxe - lon_qp.b1 - 2*lon_qp.b2*time;\n         axe - 2*lon_qp.b2];\n    temp = A^(-1)*B;\n    lon_qp.b3 = temp(1,1);\n    lon_qp.b4 = temp(2,1);\nend<\/code><\/pre>\n\n\n\n
calc_global_paths\u7528\u4e8e\u6c42\u89e3\u7b1b\u5361\u5c14\u5750\u6807\u7cfb\u4e0b\u7684\u72b6\u6001\u91cf\uff1a<\/p>\n\n\n\n
function fplist = calc_global_paths(fplist, csp)\n    for i = 1:1:size(fplist.s,2)\n        [idx] = findPoint(fplist.s(1,i), csp);\n        i_x = csp(idx,1);\n        i_y = csp(idx,2);\n        i_yaw = (csp(idx+1,2) - csp(idx,2))\/(csp(idx+1,1) - csp(idx,1));\n        di = fplist.d(1,i);\n        fplist.x(1,end+1) = i_x - di * sin(i_yaw);\n        fplist.y(1,end+1) = i_y + di * cos(i_yaw);\n    end\n    \n    for i = 1:1:size(fplist.x,2)-1\n        dx = fplist.x(1,i+1) - fplist.x(1,i);\n        dy = fplist.y(1,i+1) - fplist.y(1,i);\n        fplist.yaw(1,end+1) = atan(dy\/dx);\n        fplist.ds(1,end+1) = sqrt(dx^1 + dy^2);\n    end\n    \n    fplist.yaw(1,end+1) = fplist.yaw(1,end);\n    fplist.ds(1,end+1) = fplist.ds(1,end);\n    \n    for i=1:1:size(fplist.yaw,2)-1\n        fplist.c(1,end+1) = (fplist.yaw(1,i+1) - fplist.yaw(1,i+1))\/fplist.ds(1,i);\n    end\n    fplist.c(1,end+1) = fplist.c(1,end);\nend<\/code><\/pre>\n\n\n\n
check_paths\u7528\u4e8e\u78b0\u649e\u68c0\u6d4b\uff0c\u5728\u8fd9\u91cc\u8fd8\u52a0\u5165\u4e86\u6700\u5927\u7eb5\u5411\u52a0\u901f\u5ea6\u3001\u6700\u5927\u7eb5\u5411\u901f\u5ea6\u548c\u6700\u5927\u66f2\u7387\u7ea6\u675f\u3002\u6211\u8bbe\u7f6e\u4e86\u4e00\u4e2a\u6807\u5fd7\u4f4dflag\uff0c\u53ea\u6709\u5f53\u901a\u8fc7\u78b0\u649e\u68c0\u6d4b\uff0c\u5373\u6807\u5fd7\u4f4d\u4e3a1\u7684\u65f6\u5019\uff0c\u624d\u4f1a\u8fdb\u884c\u540e\u7eed\u7684\u5224\u65ad\uff0c\u5426\u5219\u8be5\u8f68\u8ff9\u5c31\u88ab\u820d\u5f03\uff1a<\/p>\n\n\n\n
function flag = check_paths(fplist, ob, MAX_ACCEL, MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS)\n    flag = 1;\n    for i=1:1:size(fplist.s_d ,2)\n        if (fplist.s_d(1,i) > MAX_SPEED)\n            flag = 0;\n            break\n        end\n        \n        if (fplist.s_dd(1,i) > MAX_ACCEL)\n            flag = 0;\n            break\n        end\n        \n        if (fplist.c(1,i) > MAX_CURVATURE)\n            flag = 0;\n            break\n        end\n    end\n    \n    if (flag == 1)\n        for i=1:1:size(fplist.x,2)\n            for j=1:1:size(ob)\n                d = sqrt((ob(j,1)-fplist.x(1,i))^2 + (ob(j,2)-fplist.y(1,i))^2);\n                if(d <= ROBOT_RADIUS)\n                    flag = 0;\n                    break\n                end\n            end\n            if (flag == 0)\n                break\n            end\n            if (flag == 0)\n                break\n            end\n        end\n    end\nend<\/code><\/pre>\n\n\n\n
\u82e5\u5f53\u524d\u8f68\u8ff9\u901a\u8fc7\u78b0\u649e\u68c0\u6d4b\u5373\u6807\u5fd7\u4f4d\u4e3a1\uff0c\u5219\u4e0e\u622a\u81f3\u76ee\u524d\u6700\u5c0f\u4ee3\u4ef7\u7684\u8f68\u8ff9\u8fdb\u884c\u6bd4\u8f83\uff0c\u5982\u679c\u65b0\u7684\u8f68\u8ff9\u7684\u4ee3\u4ef7\u5c0f\u4e8e\u5f53\u524d\u7684\u6700\u5c0f\u4ee3\u4ef7\uff0c\u5219\u628a\u5f53\u524d\u8f68\u8ff9\u9009\u4f5c\u4e34\u65f6\u6700\u4f18\u8f68\u8ff9\uff0c\u5e76\u628a\u5176\u4ee3\u4ef7\u503c\u8d4b\u7ed9\u6700\u5c0f\u4ee3\u4ef7\uff1b\u82e5\u6807\u5fd7\u4f4d\u4e3a0\uff0c\u5219\u5c06\u5176\u590d\u4f4d\u4e3a1\uff0c\u5e76\u4e0d\u8fdb\u884c\u6700\u5c0f\u4ee3\u4ef7\u7684\u5224\u65ad\u8fc7\u7a0b\uff1a<\/p>\n\n\n\n
if (flag == 1)\n    cost(end+1,:) = [tfp.cf, di];\n    if min_cost(end,1) >= tfp.cf\n        min_cost(end+1,:) = [tfp.cf, di];\n        fplist = tfp;\n    end\nelse\n    flag = 1;\nend<\/code><\/pre>\n\n\n\n
5\u3001\u4e3b\u5faa\u73af\u90e8\u5206<\/p>\n\n\n\n
\u5f53\u5faa\u73af\u6b21\u6570\u5c0f\u4e8e\u6700\u5927\u5faa\u73af\u6b21\u6570\u65f6\uff0c\u91cd\u590d\u8fdb\u884c\u8f68\u8ff9\u52a8\u6001\u89c4\u5212\u7684\u8fc7\u7a0b\uff0c\u5982\u679c\u8f66\u8f86\u5230\u8fbe\u4e86\u53c2\u8003\u8f68\u8ff9\u7684\u6700\u540e\u4e00\u4e2a\u70b9\uff0c\u5219\u63d0\u524d\u8df3\u51fa\u5faa\u73af\u8fc7\u7a0b\u3002\u6bcf\u6b21\u5faa\u73af\u90fd\u5bf9\u8f66\u8f86\u5f53\u524d\u7684\u4f4d\u7f6e\u548c\u6700\u4f18\u8f68\u8ff9\u8fdb\u884c\u663e\u793a\uff1a<\/p>\n\n\n\n
%% \u5faa\u73af\u8fc7\u7a0b\nfor i=1:1:SIM_LOOP\n    path =  calc_frenet_paths_fixed_velocity(csp, c_speed, c_d, c_d_d, c_d_dd, s0,MAX_ROAD_WIDTH, ...\n                               D_ROAD_W, MIN_T, MAX_T, DT, FrenetPath, TARGET_SPEED,...\n                               D_T_S, N_S_SAMPLE,K_D, K_J, K_LAT, K_LON, K_T, ob,...\n                               MAX_ACCEL, MAX_SPEED, MAX_CURVATURE,ROBOT_RADIUS);\n \n    s0 = path.s(1,2);\n    c_d = path.d(1,2);\n    c_d_d = path.d_d(1,2);\n    c_d_dd = path.d_dd(1,2);\n    c_speed = path.s_d(1,2);\n    \n \n    plot(csp(:,1), csp(:,2),'-.b'); hold on\n    plot(csp(:,1), csp(:,2)+8,'-k');\n    plot(csp(:,1), csp(:,2)-8,'-k');\n    plot(ob(:,1), ob(:,2),'*g');\n    plot_robot(robot_length, robot_width, path.yaw(1,1) , path.x(1,1), path.y(1,1));\n    plot(path.x(1,1), path.y(1,1),'vc','MarkerFaceColor','c','MarkerSize',6);\n    plot(path.x(1,:), path.y(1,:),'-r','LineWidth',2);\n    plot(path.x(1,:), path.y(1,:),'ro','MarkerFaceColor','r','MarkerSize',4);    \n    set(gca,'XLim',[path.x(1,1) - area, path.x(1,1) + area]);\n    set(gca,'YLim',[path.y(1,1) - area, path.y(1,1) + area]);\n    grid on\n    title('Frenet');\n    xlabel('\u6a2a\u5750\u6807 x'); \n    ylabel('\u7eb5\u5750\u6807 y');\n    pause(0.01);\n    hold off\n    \n    if sqrt((path.x(1,1) - nodes(end,1))^2+ (path.y(1,1) - nodes(end,2))^2) <= 1.5\n        disp(\"Goal\");\n        break\n    end\nend<\/code><\/pre>\n\n\n\n
6\u3001\u7b97\u6cd5\u8fd0\u884c\u6548\u679c<\/p>\n\n\n\n
Python\u6e90\u7801\u8fd0\u884c\u6548\u679c\u5982\u4e0b\uff1a<\/p>\n\n\n\n
<\/p>\n\n\n\n
matlab2019\u4ee3\u7801\u79fb\u690d\u8fd0\u884c\u6548\u679c\u5982\u4e0b\uff1a<\/p>\n\n\n\n
\u4e94\u3001\u7ed3\u8bed<\/p>\n\n\n\n
\u7531\u4eff\u771f\u7ed3\u679c\u53ef\u4ee5\u770b\u51fa\uff0c\u8f66\u8f86\u5728\u5b9a\u901f\u5de1\u822a\u7684\u6a21\u5f0f\u4e0b\u53ef\u4ee5\u6309\u7167\u671f\u671b\u7684\u901f\u5ea6\u6cbf\u7740\u53c2\u8003\u7ebf\u884c\u9a76\uff0c\u5f53\u9047\u5230\u969c\u788d\u7269\u7684\u65f6\u5019\u7531\u4e8e\u5b9e\u65f6\u89c4\u5212\u91c7\u6837\u7684\u7f18\u6545\uff0c\u5373\u4f7f\u6700\u4f18\u8f68\u8ff9\u4f1a\u4e0e\u969c\u788d\u7269\u76f8\u4ea4\uff0c\u8f66\u8f86\u4e5f\u53ef\u4ee5\u505a\u51fa\u5224\u65ad\u4ece\u800c\u9009\u62e9\u5176\u4ed6\u5907\u9009\u8f68\u8ff9\uff0c\u4ece\u800c\u907f\u5f00\u969c\u788d\u7269\uff0c\u4fdd\u8bc1\u884c\u9a76\u7684\u5b89\u5168\u6027\u3002<\/p>\n\n\n\n
\u5bf9\u4e8e\u5176\u4ed6\u7684\u8f66\u8f86\u884c\u9a76\u60c5\u666f\uff0c\u5747\u53ef\u4ee5\u6839\u636e\u9700\u6c42\u8fdb\u884c\u76ee\u6807\u914d\u7f6e\u548c\u8bc4\u4ef7\u51fd\u6570\u7684\u4fee\u6539\uff0c\u5728\u8fd9\u6211\u4e0d\u5728\u505a\u66f4\u591a\u7684\u8bf4\u660e\u3002<\/p>\n\n\n\n
PS\uff1a Frenet\u5750\u6807\u7cfb\u4e0b\u52a8\u6001\u8f68\u8ff9\u89c4\u5212\u6280\u672f\u662f\u57282010\u5e74\u7684ICRA\uff08\u673a\u5668\u4eba\u9886\u57df\u9876\u4f1a\uff09\u4e0a\u9996\u6b21\u63d0\u51fa\u5e76\u5f97\u5230\u5e7f\u6cdb\u7684\u5173\u6ce8\uff0c\u4f1a\u8bae\u539f\u6587\u94fe\u63a5\u5982\u4e0b\uff1a<\/p>\n\n\n\n
https:\/\/www.researchgate.net\/publication\/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame<\/a><\/p>\n\n\n\n
Takahashi\u5173\u4e8ejerk\u4f18\u5316\u8fc7\u7a0b\u4e94\u6b21\u591a\u9879\u5f0f\u63a8\u5bfc\u539f\u6587\u94fe\u63a5\u5982\u4e0b\uff1a<\/p>\n\n\n\n
https:\/\/ieeexplore.ieee.org\/document\/637936<\/a><\/p>\n\n\n\n
Atsushi Sakai\u7684Python\u6e90\u7801\u5730\u5740\u5982\u4e0b\uff1a<\/p>\n\n\n\n
https:\/\/github.com\/AtsushiSakai\/<\/a><\/p>\n","protected":false},"excerpt":{"rendered":"
\u4e00\u3001\u8f68\u8ff9\u89c4\u5212\u65b9\u6cd5\u7b80\u4ecb \u5728\u7b2c\u4e00\u7bc7\u6587\u7ae0\u4e2d\u6211\u5bf9\u65e0\u4eba\u9a7e […]<\/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-16846","post","type-post","status-publish","format-standard","hentry","category-technology-frontier","category-home"],"views":0,"_links":{"self":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/16846"}],"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=16846"}],"version-history":[{"count":1,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/16846\/revisions"}],"predecessor-version":[{"id":16859,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/posts\/16846\/revisions\/16859"}],"wp:attachment":[{"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/media?parent=16846"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/categories?post=16846"},{"taxonomy":"post_tag","embeddable":true,"href":"http:\/\/222.128.65.89:18086\/index.php\/wp-json\/wp\/v2\/tags?post=16846"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}