【路径规划-二维路径规划】基于人工势场求解机器人路径规划问题4附matlab代码 上传.zip

function evaluate_v2() nrows= 400; ncols= 600; obstacle= false( nrows, ncols); [ x, y]= meshgrid( 1: ncols, 1: nrows); obstacle( 300:eend , 100: 250)= true; obstacle( 150: 200, 400: 500)= true; t=(( x- 200).^ 2+( y- 50).^ 2)< 50^ 2; obstacle( t)= true; t=(( x- 400).^ 2+( y- 300).^ 2)< 100^ 2; obstacle( t)= true; d= bwdist( obstacle); d2=( d/ 100)+ 1; d0= 2; nu= 800; repulsive= nu*(( 1./ d2- 1/ d0).^ 2); repulsive( d2> d0)= 0; goal=[ 590, 50]; start=[ 50, 350]; xi= 1/ 700; attractive= xi*(( x- goal( 1)).^ 2+( y- goal( 2)).^ 2); f= attractive+ repulsive; route= GradientBasedPlanner( f, start, goal, 1000); trueRoute=[ 50, 350; 50.3848800659180, 349.077026367188; 50.7708396911621, 348.154510498047; 51.1093902587891, 347.213562011719; 51.4489440917969, 346.272979736328; 51.7895050048828, 345.332763671875; 52.0776367187500, 344.375183105469; 52.3666496276856, 343.417846679688; 52.6565704345703, 342.460784912109; 52.8887939453125, 341.488128662109; 53.1218109130859, 340.515655517578; 53.3548278808594, 339.543182373047; 53.5885658264160, 338.570892333984; 53.7592163085938, 337.585571289063; 53.9304389953613, 336.600341796875; 54.1022415161133, 335.615203857422; 54.2746276855469, 334.630187988281; 54.4476356506348, 333.645263671875; 54.6212005615234, 332.660430908203; 54.7257347106934, 331.665924072266; 54.8306388854981, 330.671447753906; 54.9359130859375, 329.677001953125; 55.0415611267090, 328.682586669922; 55.1475830078125, 327.688232421875; 55.2539749145508, 326.693908691406; 55.3607482910156, 325.699615478516; 55.4679069519043, 324.705383300781; 55.5754508972168, 323.711181640625; 55.6074409484863, 322.711700439453; 55.6395454406738, 321.712219238281; 55.6717681884766, 320.712738037109; 55.7040710449219, 319.713256835938; 55.7365341186523, 318.713775634766; 55.7690773010254, 317.714294433594; 55.8017425537109, 316.714813232422; 55.8345298767090, 315.715362548828; 55.8674774169922, 314.715911865234; 55.9005508422852, 313.716461181641; 55.9337501525879, 312.717010498047; 55.9670333862305, 311.717559814453; 56.0004463195801, 310.718109130859; 56.0340270996094, 309.718658447266; 56.0677337646484, 308.719238281250; 56.1015739440918, 307.719818115234; 56.1355018615723, 306.720397949219; 56.1695632934570, 305.720977783203; 56.2037582397461, 304.721557617188; 56.2381286621094, 303.722137451172; 56.2725906372070, 302.722717285156; 56.3072319030762, 301.723327636719; 56.3420104980469, 300.723937988281; 56.3769264221191, 299.724548339844; 56.4115257263184, 298.725158691406; 56.4464645385742, 297.725769042969; 56.4840850830078, 296.726470947266; 56.5266876220703, 295.727386474609; 56.5041160583496, 294.727630615234; 56.4936180114746, 293.727691650391; 56.5627212524414, 292.730072021484; 56.5804595947266, 291.730224609375; 56.6139984130859, 290.730773925781; 56.6643104553223, 289.732025146484; 56.7321434020996, 288.734313964844; 56.8181724548340, 287.738006591797; 56.9229545593262, 286.743499755859; 57.0469474792481, 285.751220703125; 57.1905441284180, 284.761596679688; 57.3540267944336, 283.775054931641; 57.5375938415527, 282.792053222656; 57.7003936767578, 281.805389404297; 57.8852539062500, 280.822631835938; 58.0921287536621, 279.844268798828; 58.3209190368652, 278.870788574219; 58.5715103149414, 277.902709960938; 58.8108940124512, 276.931793212891; 59.0730781555176, 275.966766357422; 59.3577766418457, 275.008148193359; 59.6646537780762, 274.056396484375; 59.9660873413086, 273.102905273438; 60.2901573181152, 272.156860351563; 60.6364250183106, 271.218719482422; 60.9808692932129, 270.279907226563; 61.3476219177246, 269.349578857422; 61.7361793518066, 268.428161621094; 62.1257286071777, 267.507141113281; 62.5152778625488, 266.586120605469; 62.9074707031250, 265.666229248047; 63.3215408325195, 264.755981445313; 63.7569122314453, 263.855743408203; 64.1967391967773, 262.957672119141; 64.6573944091797, 262.070098876953; 65.1237716674805, 261.185516357422; 65.6103820800781, 260.311889648438; 66.1037216186523, 259.442047119141; 66.6166152954102, 258.583587646484; 67.1175918579102, 257.718139648438; 67.6379852294922, 256.864227294922; 68.1666412353516, 256.015380859375; 68.7139053344727, 255.178421020508; 69.2698669433594, 254.347213745117; 69.8436050415039, 253.528167724609; 70.4087982177734, 252.703201293945; 70.9914932250977, 251.890518188477; 71.5835800170898, 251.084640502930; 72.1854248046875, 250.286026000977; 72.8035736083984, 249.499969482422; 73.4314270019531, 248.721633911133; 74.0592803955078, 247.943298339844; 74.6970596313477, 247.173080444336; 75.3449401855469, 246.411331176758; 76.0074920654297, 245.662307739258; 76.6655807495117, 244.909362792969; 77.3339538574219, 244.165542602539; 78.0162124633789, 243.434432983398; 78.7083892822266, 242.712707519531; 79.3973770141602, 241.987930297852; 80.0994491577148, 241.275817871094; 80.8113708496094, 240.573562622070; 81.5207977294922, 239.868789672852; 82.2403106689453, 239.174301147461; 82.9716949462891, 238.492340087891; 83.7126312255859, 237.820770263672; 84.4519119262695, 237.147369384766; 85.2022628784180, 236.486328125000; 85.9618606567383, 235.835937500000; 86.7202301025391, 235.184112548828; 87.4878616333008, 234.543212890625; 88.2554931640625, 233.902313232422; 89.0321807861328, 233.272415161133; 89.8177032470703, 232.653579711914; 90.6026916503906, 232.034072875977; 91.3964157104492, 231.425796508789; 92.1989364624023, 230.829162597656; 93.0011520385742, 230.232131958008; 93.8116073608398, 229.646331787109; 94.6220016479492, 229.060455322266; 95.4404602050781, 228.485900878906; 96.2666549682617, 227.922515869141; 97.0929260253906, 227.359237670898; 97.9267501831055, 226.807205200195; 98.7608184814453, 226.255538940430; 99.6022109985352, 225.715118408203; 100.443756103516, 225.174926757813; 101.292007446289, 224.645324707031; 102.139984130859, 224.115295410156; 102.993980407715, 223.595016479492; 103.847503662109, 223.073959350586; 104.706886291504, 222.562622070313; 105.565803527832, 222.050506591797; 106.430412292480, 221.548065185547; 107.295021057129, 221.045623779297; 108.165153503418, 220.552810668945; 109.034851074219, 220.059219360352; 109.909912109375, 219.575210571289; 110.784545898438, 219.090423583984; 111.664382934570, 55.6074409484863; 322.711700439453, 55.6395454406738; 321.712219238281, 55.6717681884766; 320.712738037109, 55.7040710449219; 319.713256835938, 55.7365341186523; 318.713775634766, 55.7690773010254; 317.714294433594, 55.8017425537109; 316.714813232422, 55.8345298767090; 315.715362548828, 55.8674774169922; 314.715911865234, 55.9005508422852; 313.716461181641, 55.9337501525879; 312.717010498047, 55.9670333862305; 311.717559814453, 56.0004463195801; 310.718109130859, 56.0340270996094; 309.718658447266, 56.0677337646484; 308.719238281250, 56.1015739440918; 307.719818115234, 56.1355018615723; 306.720397949219, 56.1695632934570; 305.720977783203, 56.2037582397461; 304.721557617188, 56.2381286621094; 303.722137451172, 56.2725906372070; 302.722717285156, 56.3072319030762; 301.723327636719, 56.3420104980469; 300.723937988281, 56.3769264221191; 299.724548339844, 56.4115257263184; 298.725158691406, 56.4464645385742; 297.725769042969, 56.4840850830078; 296.726470947266, 56.5266876220703; 295.727386474609, 56.5041160583496; 294.727630615234, 56.4936180114746; 293.727691650391, 56.5627212524414; 292.730072021484, 56.5804595947266; 291.730224609375, 56.6139984130859; 290.730773925781, 56.6643104553223; 289.732025146484, 56.7321434020996; 288.734313964844, 56.8181724548340; 287.738006591797, 56.9229545593262; 286.743499755859, 57.0469474792481; 285.751220703125, 57.1905441284180; 284.761596679688, 57.3540267944336; 283.775054931641, 57.5375938415527; 282.792053222656, 57.7003936767578; 281.805389404297, 57.8852539062500; 280.822631835938, 58.0921287536621; 279.844268798828, 58.3209190368652; 278.870788574219, 58.5715103149414; 277.902709960938, 58.8108940124512; 276.931793212891, 59.0730781555176; 275.966766357