├── DW_Main.m ├── Done.ps ├── Dynamic Window Approach.avi ├── FrenetTrajectory.m ├── Goal!!.ps ├── OptimalFrenetPlanner.m ├── QuarticPoly.m ├── QuinticPoly.m ├── README.md ├── RS.m ├── RSPath.m ├── Spline.m ├── Spline2D.m ├── WerlingOptimalTrajectoryGenerationforDynamicStreetScenariosinaFrenetFrame.pdf ├── cal_s.m ├── calcSplineCourse.m ├── complete goal.ps ├── generate_target_course.m ├── main_path_planning.m ├── param.m └── test.avi /DW_Main.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/caokaifa/Matlab-planning/08db498c02139997cd59d13df9c163710b8494f0/DW_Main.m -------------------------------------------------------------------------------- /Done.ps: -------------------------------------------------------------------------------- 1 | %!PS-Adobe-3.0 2 | %%Creator: (MATLAB, The Mathworks, Inc. Version 9.7.0.1190202 \(R2019b\). Operating System: Windows 10) 3 | %%Title: ./Done.ps 4 | %%CreationDate: 2020-08-16T17:48:08 5 | %%Pages: (atend) 6 | %%BoundingBox: 127 283 468 559 7 | %%LanguageLevel: 3 8 | %%EndComments 9 | %%BeginDefaults 10 | %%EndDefaults 11 | %%BeginProlog 12 | %%EndProlog 13 | %%BeginSetup 14 | %%BeginResource: procset (Apache XML Graphics Std ProcSet) 1.2 0 15 | %%Version: 1.2 0 16 | %%Copyright: (Copyright 2001-2003,2010 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) 17 | /bd{bind def}bind def 18 | /ld{load def}bd 19 | /GR/grestore ld 20 | /GS/gsave ld 21 | /RM/rmoveto ld 22 | /C/curveto ld 23 | /t/show ld 24 | /L/lineto ld 25 | /ML/setmiterlimit ld 26 | /CT/concat ld 27 | /f/fill ld 28 | /N/newpath ld 29 | /S/stroke ld 30 | /CC/setcmykcolor ld 31 | /A/ashow ld 32 | /cp/closepath ld 33 | /RC/setrgbcolor ld 34 | /LJ/setlinejoin ld 35 | /GC/setgray ld 36 | /LW/setlinewidth ld 37 | /M/moveto ld 38 | /re {4 2 roll M 39 | 1 index 0 rlineto 40 | 0 exch rlineto 41 | neg 0 rlineto 42 | cp } bd 43 | /_ctm matrix def 44 | /_tm matrix def 45 | /BT { _ctm currentmatrix pop matrix _tm copy pop 0 0 moveto } bd 46 | /ET { _ctm setmatrix } bd 47 | /iTm { _ctm setmatrix _tm concat } bd 48 | /Tm { _tm astore pop iTm 0 0 moveto } bd 49 | /ux 0.0 def 50 | /uy 0.0 def 51 | /F { 52 | /Tp exch def 53 | /Tf exch def 54 | Tf findfont Tp scalefont setfont 55 | /cf Tf def /cs Tp def 56 | } bd 57 | /ULS {currentpoint /uy exch def /ux exch def} bd 58 | /ULE { 59 | /Tcx currentpoint pop def 60 | gsave 61 | newpath 62 | cf findfont cs scalefont dup 63 | /FontMatrix get 0 get /Ts exch def /FontInfo get dup 64 | /UnderlinePosition get Ts mul /To exch def 65 | /UnderlineThickness get Ts mul /Tt exch def 66 | ux uy To add moveto Tcx uy To add lineto 67 | Tt setlinewidth stroke 68 | grestore 69 | } bd 70 | /OLE { 71 | /Tcx currentpoint pop def 72 | gsave 73 | newpath 74 | cf findfont cs scalefont dup 75 | /FontMatrix get 0 get /Ts exch def /FontInfo get dup 76 | /UnderlinePosition get Ts mul /To exch def 77 | /UnderlineThickness get Ts mul /Tt exch def 78 | ux uy To add cs add moveto Tcx uy To add cs add lineto 79 | Tt setlinewidth stroke 80 | grestore 81 | } bd 82 | /SOE { 83 | /Tcx currentpoint pop def 84 | gsave 85 | newpath 86 | cf findfont cs scalefont dup 87 | /FontMatrix get 0 get /Ts exch def /FontInfo get dup 88 | /UnderlinePosition get Ts mul /To exch def 89 | /UnderlineThickness get Ts mul /Tt exch def 90 | ux uy To add cs 10 mul 26 idiv add moveto Tcx uy To add cs 10 mul 26 idiv add lineto 91 | Tt setlinewidth stroke 92 | grestore 93 | } bd 94 | /QT { 95 | /Y22 exch store 96 | /X22 exch store 97 | /Y21 exch store 98 | /X21 exch store 99 | currentpoint 100 | /Y21 load 2 mul add 3 div exch 101 | /X21 load 2 mul add 3 div exch 102 | /X21 load 2 mul /X22 load add 3 div 103 | /Y21 load 2 mul /Y22 load add 3 div 104 | /X22 load /Y22 load curveto 105 | } bd 106 | /SSPD { 107 | dup length /d exch dict def 108 | { 109 | /v exch def 110 | /k exch def 111 | currentpagedevice k known { 112 | /cpdv currentpagedevice k get def 113 | v cpdv ne { 114 | /upd false def 115 | /nullv v type /nulltype eq def 116 | /nullcpdv cpdv type /nulltype eq def 117 | nullv nullcpdv or 118 | { 119 | /upd true def 120 | } { 121 | /sametype v type cpdv type eq def 122 | sametype { 123 | v type /arraytype eq { 124 | /vlen v length def 125 | /cpdvlen cpdv length def 126 | vlen cpdvlen eq { 127 | 0 1 vlen 1 sub { 128 | /i exch def 129 | /obj v i get def 130 | /cpdobj cpdv i get def 131 | obj cpdobj ne { 132 | /upd true def 133 | exit 134 | } if 135 | } for 136 | } { 137 | /upd true def 138 | } ifelse 139 | } { 140 | v type /dicttype eq { 141 | v { 142 | /dv exch def 143 | /dk exch def 144 | /cpddv cpdv dk get def 145 | dv cpddv ne { 146 | /upd true def 147 | exit 148 | } if 149 | } forall 150 | } { 151 | /upd true def 152 | } ifelse 153 | } ifelse 154 | } if 155 | } ifelse 156 | upd true eq { 157 | d k v put 158 | } if 159 | } if 160 | } if 161 | } forall 162 | d length 0 gt { 163 | d setpagedevice 164 | } if 165 | } bd 166 | /RE { % /NewFontName [NewEncodingArray] /FontName RE - 167 | findfont dup length dict begin 168 | { 169 | 1 index /FID ne 170 | {def} {pop pop} ifelse 171 | } forall 172 | /Encoding exch def 173 | /FontName 1 index def 174 | currentdict definefont pop 175 | end 176 | } bind def 177 | %%EndResource 178 | %%BeginResource: procset (Apache XML Graphics EPS ProcSet) 1.0 0 179 | %%Version: 1.0 0 180 | %%Copyright: (Copyright 2002-2003 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) 181 | /BeginEPSF { %def 182 | /b4_Inc_state save def % Save state for cleanup 183 | /dict_count countdictstack def % Count objects on dict stack 184 | /op_count count 1 sub def % Count objects on operand stack 185 | userdict begin % Push userdict on dict stack 186 | /showpage { } def % Redefine showpage, { } = null proc 187 | 0 setgray 0 setlinecap % Prepare graphics state 188 | 1 setlinewidth 0 setlinejoin 189 | 10 setmiterlimit [ ] 0 setdash newpath 190 | /languagelevel where % If level not equal to 1 then 191 | {pop languagelevel % set strokeadjust and 192 | 1 ne % overprint to their defaults. 193 | {false setstrokeadjust false setoverprint 194 | } if 195 | } if 196 | } bd 197 | /EndEPSF { %def 198 | count op_count sub {pop} repeat % Clean up stacks 199 | countdictstack dict_count sub {end} repeat 200 | b4_Inc_state restore 201 | } bd 202 | %%EndResource 203 | %FOPBeginFontDict 204 | %%IncludeResource: font Courier-Oblique 205 | %%IncludeResource: font Courier-BoldOblique 206 | %%IncludeResource: font Courier-Bold 207 | %%IncludeResource: font ZapfDingbats 208 | %%IncludeResource: font Symbol 209 | %%IncludeResource: font Helvetica 210 | %%IncludeResource: font Helvetica-Oblique 211 | %%IncludeResource: font Helvetica-Bold 212 | %%IncludeResource: font Helvetica-BoldOblique 213 | %%IncludeResource: font Times-Roman 214 | %%IncludeResource: font Times-Italic 215 | %%IncludeResource: font Times-Bold 216 | %%IncludeResource: font Times-BoldItalic 217 | %%IncludeResource: font Courier 218 | %FOPEndFontDict 219 | %%BeginResource: encoding WinAnsiEncoding 220 | /WinAnsiEncoding [ 221 | /.notdef /.notdef /.notdef /.notdef /.notdef 222 | /.notdef /.notdef /.notdef /.notdef /.notdef 223 | /.notdef /.notdef /.notdef /.notdef /.notdef 224 | /.notdef /.notdef /.notdef /.notdef /.notdef 225 | /.notdef /.notdef /.notdef /.notdef /.notdef 226 | /.notdef /.notdef /.notdef /.notdef /.notdef 227 | /.notdef /.notdef /space /exclam /quotedbl 228 | /numbersign /dollar /percent /ampersand /quotesingle 229 | /parenleft /parenright /asterisk /plus /comma 230 | /hyphen /period /slash /zero /one 231 | /two /three /four /five /six 232 | /seven /eight /nine /colon /semicolon 233 | /less /equal /greater /question /at 234 | /A /B /C /D /E 235 | /F /G /H /I /J 236 | /K /L /M /N /O 237 | /P /Q /R /S /T 238 | /U /V /W /X /Y 239 | /Z /bracketleft /backslash /bracketright /asciicircum 240 | /underscore /quoteleft /a /b /c 241 | /d /e /f /g /h 242 | /i /j /k /l /m 243 | /n /o /p /q /r 244 | /s /t /u /v /w 245 | /x /y /z /braceleft /bar 246 | /braceright /asciitilde /bullet /Euro /bullet 247 | /quotesinglbase /florin /quotedblbase /ellipsis /dagger 248 | /daggerdbl /circumflex /perthousand /Scaron /guilsinglleft 249 | /OE /bullet /Zcaron /bullet /bullet 250 | /quoteleft /quoteright /quotedblleft /quotedblright /bullet 251 | /endash /emdash /asciitilde /trademark /scaron 252 | /guilsinglright /oe /bullet /zcaron /Ydieresis 253 | /space /exclamdown /cent /sterling /currency 254 | /yen /brokenbar /section /dieresis /copyright 255 | /ordfeminine /guillemotleft /logicalnot /sfthyphen /registered 256 | /macron /degree /plusminus /twosuperior /threesuperior 257 | /acute /mu /paragraph /middot /cedilla 258 | /onesuperior /ordmasculine /guillemotright /onequarter /onehalf 259 | /threequarters /questiondown /Agrave /Aacute /Acircumflex 260 | /Atilde /Adieresis /Aring /AE /Ccedilla 261 | /Egrave /Eacute /Ecircumflex /Edieresis /Igrave 262 | /Iacute /Icircumflex /Idieresis /Eth /Ntilde 263 | /Ograve /Oacute /Ocircumflex /Otilde /Odieresis 264 | /multiply /Oslash /Ugrave /Uacute /Ucircumflex 265 | /Udieresis /Yacute /Thorn /germandbls /agrave 266 | /aacute /acircumflex /atilde /adieresis /aring 267 | /ae /ccedilla /egrave /eacute /ecircumflex 268 | /edieresis /igrave /iacute /icircumflex /idieresis 269 | /eth /ntilde /ograve /oacute /ocircumflex 270 | /otilde /odieresis /divide /oslash /ugrave 271 | /uacute /ucircumflex /udieresis /yacute /thorn 272 | /ydieresis 273 | ] def 274 | %%EndResource 275 | %FOPBeginFontReencode 276 | /Courier-Oblique findfont 277 | dup length dict begin 278 | {1 index /FID ne {def} {pop pop} ifelse} forall 279 | /Encoding WinAnsiEncoding def 280 | currentdict 281 | end 282 | /Courier-Oblique exch definefont pop 283 | /Courier-BoldOblique findfont 284 | dup length dict begin 285 | {1 index /FID ne {def} {pop pop} ifelse} forall 286 | /Encoding WinAnsiEncoding def 287 | currentdict 288 | end 289 | /Courier-BoldOblique exch definefont pop 290 | /Courier-Bold findfont 291 | dup length dict begin 292 | {1 index /FID ne {def} {pop pop} ifelse} forall 293 | /Encoding WinAnsiEncoding def 294 | currentdict 295 | end 296 | /Courier-Bold exch definefont pop 297 | /Helvetica findfont 298 | dup length dict begin 299 | {1 index /FID ne {def} {pop pop} ifelse} forall 300 | /Encoding WinAnsiEncoding def 301 | currentdict 302 | end 303 | /Helvetica exch definefont pop 304 | /Helvetica-Oblique findfont 305 | dup length dict begin 306 | {1 index /FID ne {def} {pop pop} ifelse} forall 307 | /Encoding WinAnsiEncoding def 308 | currentdict 309 | end 310 | /Helvetica-Oblique exch definefont pop 311 | /Helvetica-Bold findfont 312 | dup length dict begin 313 | {1 index /FID ne {def} {pop pop} ifelse} forall 314 | /Encoding WinAnsiEncoding def 315 | currentdict 316 | end 317 | /Helvetica-Bold exch definefont pop 318 | /Helvetica-BoldOblique findfont 319 | dup length dict begin 320 | {1 index /FID ne {def} {pop pop} ifelse} forall 321 | /Encoding WinAnsiEncoding def 322 | currentdict 323 | end 324 | /Helvetica-BoldOblique exch definefont pop 325 | /Times-Roman findfont 326 | dup length dict begin 327 | {1 index /FID ne {def} {pop pop} ifelse} forall 328 | /Encoding WinAnsiEncoding def 329 | currentdict 330 | end 331 | /Times-Roman exch definefont pop 332 | /Times-Italic findfont 333 | dup length dict begin 334 | {1 index /FID ne {def} {pop pop} ifelse} forall 335 | /Encoding WinAnsiEncoding def 336 | currentdict 337 | end 338 | /Times-Italic exch definefont pop 339 | /Times-Bold findfont 340 | dup length dict begin 341 | {1 index /FID ne {def} {pop pop} ifelse} forall 342 | /Encoding WinAnsiEncoding def 343 | currentdict 344 | end 345 | /Times-Bold exch definefont pop 346 | /Times-BoldItalic findfont 347 | dup length dict begin 348 | {1 index /FID ne {def} {pop pop} ifelse} forall 349 | /Encoding WinAnsiEncoding def 350 | currentdict 351 | end 352 | /Times-BoldItalic exch definefont pop 353 | /Courier findfont 354 | dup length dict begin 355 | {1 index /FID ne {def} {pop pop} ifelse} forall 356 | /Encoding WinAnsiEncoding def 357 | currentdict 358 | end 359 | /Courier exch definefont pop 360 | %FOPEndFontReencode 361 | %%EndSetup 362 | %%Page: 1 1 363 | %%PageBoundingBox: 0 0 595 842 364 | %%BeginPageSetup 365 | << 366 | /PageSize [595 842] 367 | /ImagingBBox null 368 | >> setpagedevice 369 | [1 0 0 -1 0 842] CT 370 | %%EndPageSetup 371 | GS 372 | [0.5 0 0 0.5 87.63778 263.55511] CT 373 | 1 GC 374 | N 375 | 0 0 840 630 re 376 | f 377 | GR 378 | GS 379 | [0.5 0 0 0.5 87.63778 263.55511] CT 380 | 1 GC 381 | N 382 | 0 0 840 630 re 383 | f 384 | GR 385 | GS 386 | [0.5 0 0 0.5 87.63778 263.55511] CT 387 | 1 GC 388 | N 389 | 109 560.725 M 390 | 760 560.725 L 391 | 760 47.275 L 392 | 109 47.275 L 393 | cp 394 | f 395 | GR 396 | GS 397 | [0.5 0 0 0.5 87.63778 263.55511] CT 398 | 2 setlinecap 399 | 1 LJ 400 | N 401 | 109 560.725 M 402 | 760 560.725 L 403 | S 404 | GR 405 | GS 406 | [0.5 0 0 0.5 87.63778 263.55511] CT 407 | 2 setlinecap 408 | 1 LJ 409 | N 410 | 109 47.275 M 411 | 760 47.275 L 412 | S 413 | GR 414 | GS 415 | [0.5 0 0 0.5 87.63778 263.55511] CT 416 | 2 setlinecap 417 | 1 LJ 418 | N 419 | 145.684 560.725 M 420 | 145.684 554.215 L 421 | S 422 | GR 423 | GS 424 | [0.5 0 0 0.5 87.63778 263.55511] CT 425 | 2 setlinecap 426 | 1 LJ 427 | N 428 | 209.866 560.725 M 429 | 209.866 554.215 L 430 | S 431 | GR 432 | GS 433 | [0.5 0 0 0.5 87.63778 263.55511] CT 434 | 2 setlinecap 435 | 1 LJ 436 | N 437 | 274.047 560.725 M 438 | 274.047 554.215 L 439 | S 440 | GR 441 | GS 442 | [0.5 0 0 0.5 87.63778 263.55511] CT 443 | 2 setlinecap 444 | 1 LJ 445 | N 446 | 338.228 560.725 M 447 | 338.228 554.215 L 448 | S 449 | GR 450 | GS 451 | [0.5 0 0 0.5 87.63778 263.55511] CT 452 | 2 setlinecap 453 | 1 LJ 454 | N 455 | 402.409 560.725 M 456 | 402.409 554.215 L 457 | S 458 | GR 459 | GS 460 | [0.5 0 0 0.5 87.63778 263.55511] CT 461 | 2 setlinecap 462 | 1 LJ 463 | N 464 | 466.591 560.725 M 465 | 466.591 554.215 L 466 | S 467 | GR 468 | GS 469 | [0.5 0 0 0.5 87.63778 263.55511] CT 470 | 2 setlinecap 471 | 1 LJ 472 | N 473 | 530.772 560.725 M 474 | 530.772 554.215 L 475 | S 476 | GR 477 | GS 478 | [0.5 0 0 0.5 87.63778 263.55511] CT 479 | 2 setlinecap 480 | 1 LJ 481 | N 482 | 594.953 560.725 M 483 | 594.953 554.215 L 484 | S 485 | GR 486 | GS 487 | [0.5 0 0 0.5 87.63778 263.55511] CT 488 | 2 setlinecap 489 | 1 LJ 490 | N 491 | 659.134 560.725 M 492 | 659.134 554.215 L 493 | S 494 | GR 495 | GS 496 | [0.5 0 0 0.5 87.63778 263.55511] CT 497 | 2 setlinecap 498 | 1 LJ 499 | N 500 | 723.316 560.725 M 501 | 723.316 554.215 L 502 | S 503 | GR 504 | GS 505 | [0.5 0 0 0.5 87.63778 263.55511] CT 506 | 2 setlinecap 507 | 1 LJ 508 | N 509 | 145.684 47.275 M 510 | 145.684 53.785 L 511 | S 512 | GR 513 | GS 514 | [0.5 0 0 0.5 87.63778 263.55511] CT 515 | 2 setlinecap 516 | 1 LJ 517 | N 518 | 209.866 47.275 M 519 | 209.866 53.785 L 520 | S 521 | GR 522 | GS 523 | [0.5 0 0 0.5 87.63778 263.55511] CT 524 | 2 setlinecap 525 | 1 LJ 526 | N 527 | 274.047 47.275 M 528 | 274.047 53.785 L 529 | S 530 | GR 531 | GS 532 | [0.5 0 0 0.5 87.63778 263.55511] CT 533 | 2 setlinecap 534 | 1 LJ 535 | N 536 | 338.228 47.275 M 537 | 338.228 53.785 L 538 | S 539 | GR 540 | GS 541 | [0.5 0 0 0.5 87.63778 263.55511] CT 542 | 2 setlinecap 543 | 1 LJ 544 | N 545 | 402.409 47.275 M 546 | 402.409 53.785 L 547 | S 548 | GR 549 | GS 550 | [0.5 0 0 0.5 87.63778 263.55511] CT 551 | 2 setlinecap 552 | 1 LJ 553 | N 554 | 466.591 47.275 M 555 | 466.591 53.785 L 556 | S 557 | GR 558 | GS 559 | [0.5 0 0 0.5 87.63778 263.55511] CT 560 | 2 setlinecap 561 | 1 LJ 562 | N 563 | 530.772 47.275 M 564 | 530.772 53.785 L 565 | S 566 | GR 567 | GS 568 | [0.5 0 0 0.5 87.63778 263.55511] CT 569 | 2 setlinecap 570 | 1 LJ 571 | N 572 | 594.953 47.275 M 573 | 594.953 53.785 L 574 | S 575 | GR 576 | GS 577 | [0.5 0 0 0.5 87.63778 263.55511] CT 578 | 2 setlinecap 579 | 1 LJ 580 | N 581 | 659.134 47.275 M 582 | 659.134 53.785 L 583 | S 584 | GR 585 | GS 586 | [0.5 0 0 0.5 87.63778 263.55511] CT 587 | 2 setlinecap 588 | 1 LJ 589 | N 590 | 723.316 47.275 M 591 | 723.316 53.785 L 592 | S 593 | GR 594 | GS 595 | [0.5 0 0 0.5 160.47997 547.9176] CT 596 | /Helvetica 20 F 597 | GS 598 | [1 0 0 1 0 0] CT 599 | -9 21 moveto 600 | 1 -1 scale 601 | (-2) t 602 | GR 603 | GR 604 | GS 605 | [0.5 0 0 0.5 192.57059 547.9176] CT 606 | /Helvetica 20 F 607 | GS 608 | [1 0 0 1 0 0] CT 609 | -6 21 moveto 610 | 1 -1 scale 611 | (0) t 612 | GR 613 | GR 614 | GS 615 | [0.5 0 0 0.5 224.66122 547.9176] CT 616 | /Helvetica 20 F 617 | GS 618 | [1 0 0 1 0 0] CT 619 | -6 21 moveto 620 | 1 -1 scale 621 | (2) t 622 | GR 623 | GR 624 | GS 625 | [0.5 0 0 0.5 256.75184 547.9176] CT 626 | /Helvetica 20 F 627 | GS 628 | [1 0 0 1 0 0] CT 629 | -6 21 moveto 630 | 1 -1 scale 631 | (4) t 632 | GR 633 | GR 634 | GS 635 | [0.5 0 0 0.5 288.84246 547.9176] CT 636 | /Helvetica 20 F 637 | GS 638 | [1 0 0 1 0 0] CT 639 | -6 21 moveto 640 | 1 -1 scale 641 | (6) t 642 | GR 643 | GR 644 | GS 645 | [0.5 0 0 0.5 320.9331 547.9176] CT 646 | /Helvetica 20 F 647 | GS 648 | [1 0 0 1 0 0] CT 649 | -6 21 moveto 650 | 1 -1 scale 651 | (8) t 652 | GR 653 | GR 654 | GS 655 | [0.5 0 0 0.5 353.0237 547.9176] CT 656 | /Helvetica 20 F 657 | GS 658 | [1 0 0 1 0 0] CT 659 | -11.5 21 moveto 660 | 1 -1 scale 661 | (10) t 662 | GR 663 | GR 664 | GS 665 | [0.5 0 0 0.5 385.11434 547.9176] CT 666 | /Helvetica 20 F 667 | GS 668 | [1 0 0 1 0 0] CT 669 | -11.5 21 moveto 670 | 1 -1 scale 671 | (12) t 672 | GR 673 | GR 674 | GS 675 | [0.5 0 0 0.5 417.20498 547.9176] CT 676 | /Helvetica 20 F 677 | GS 678 | [1 0 0 1 0 0] CT 679 | -11.5 21 moveto 680 | 1 -1 scale 681 | (14) t 682 | GR 683 | GR 684 | GS 685 | [0.5 0 0 0.5 449.29559 547.9176] CT 686 | /Helvetica 20 F 687 | GS 688 | [1 0 0 1 0 0] CT 689 | -11.5 21 moveto 690 | 1 -1 scale 691 | (16) t 692 | GR 693 | GR 694 | GS 695 | [0.5 0 0 0.5 87.63778 263.55511] CT 696 | 2 setlinecap 697 | 1 LJ 698 | N 699 | 109 560.725 M 700 | 109 47.275 L 701 | S 702 | GR 703 | GS 704 | [0.5 0 0 0.5 87.63778 263.55511] CT 705 | 2 setlinecap 706 | 1 LJ 707 | N 708 | 760 560.725 M 709 | 760 47.275 L 710 | S 711 | GR 712 | GS 713 | [0.5 0 0 0.5 87.63778 263.55511] CT 714 | 2 setlinecap 715 | 1 LJ 716 | N 717 | 109 528.634 M 718 | 115.51 528.634 L 719 | S 720 | GR 721 | GS 722 | [0.5 0 0 0.5 87.63778 263.55511] CT 723 | 2 setlinecap 724 | 1 LJ 725 | N 726 | 109 368.181 M 727 | 115.51 368.181 L 728 | S 729 | GR 730 | GS 731 | [0.5 0 0 0.5 87.63778 263.55511] CT 732 | 2 setlinecap 733 | 1 LJ 734 | N 735 | 109 207.728 M 736 | 115.51 207.728 L 737 | S 738 | GR 739 | GS 740 | [0.5 0 0 0.5 87.63778 263.55511] CT 741 | 2 setlinecap 742 | 1 LJ 743 | N 744 | 109 47.275 M 745 | 115.51 47.275 L 746 | S 747 | GR 748 | GS 749 | [0.5 0 0 0.5 87.63778 263.55511] CT 750 | 2 setlinecap 751 | 1 LJ 752 | N 753 | 760 528.634 M 754 | 753.49 528.634 L 755 | S 756 | GR 757 | GS 758 | [0.5 0 0 0.5 87.63778 263.55511] CT 759 | 2 setlinecap 760 | 1 LJ 761 | N 762 | 760 368.181 M 763 | 753.49 368.181 L 764 | S 765 | GR 766 | GS 767 | [0.5 0 0 0.5 87.63778 263.55511] CT 768 | 2 setlinecap 769 | 1 LJ 770 | N 771 | 760 207.728 M 772 | 753.49 207.728 L 773 | S 774 | GR 775 | GS 776 | [0.5 0 0 0.5 87.63778 263.55511] CT 777 | 2 setlinecap 778 | 1 LJ 779 | N 780 | 760 47.275 M 781 | 753.49 47.275 L 782 | S 783 | GR 784 | GS 785 | [0.5 0 0 0.5 138.13778 527.87231] CT 786 | /Helvetica 20 F 787 | GS 788 | [1 0 0 1 0 0] CT 789 | -12 8 moveto 790 | 1 -1 scale 791 | (0) t 792 | GR 793 | GR 794 | GS 795 | [0.5 0 0 0.5 138.13778 447.64574] CT 796 | /Helvetica 20 F 797 | GS 798 | [1 0 0 1 0 0] CT 799 | -12 8 moveto 800 | 1 -1 scale 801 | (5) t 802 | GR 803 | GR 804 | GS 805 | [0.5 0 0 0.5 138.13778 367.41917] CT 806 | /Helvetica 20 F 807 | GS 808 | [1 0 0 1 0 0] CT 809 | -23 8 moveto 810 | 1 -1 scale 811 | (10) t 812 | GR 813 | GR 814 | GS 815 | [0.5 0 0 0.5 138.13778 287.19262] CT 816 | /Helvetica 20 F 817 | GS 818 | [1 0 0 1 0 0] CT 819 | -23 8 moveto 820 | 1 -1 scale 821 | (15) t 822 | GR 823 | GR 824 | GS 825 | [0.5 0 0 0.5 87.63778 263.55511] CT 826 | 1 LJ 827 | N 828 | 504.032 182.982 M 829 | 507.098 183.93 L 830 | 510.112 185.031 L 831 | 513.067 186.283 L 832 | 515.955 187.682 L 833 | 518.769 189.225 L 834 | 521.501 190.908 L 835 | 524.145 192.727 L 836 | 526.694 194.677 L 837 | 529.142 196.752 L 838 | 531.481 198.948 L 839 | 533.707 201.26 L 840 | 535.814 203.681 L 841 | 537.796 206.205 L 842 | 539.648 208.825 L 843 | 541.365 211.536 L 844 | 542.944 214.33 L 845 | 544.38 217.2 L 846 | 545.669 220.138 L 847 | 546.809 223.138 L 848 | 547.796 226.192 L 849 | 548.627 229.291 L 850 | 549.302 232.429 L 851 | 549.817 235.596 L 852 | 550.172 238.786 L 853 | 550.366 241.989 L 854 | 550.398 245.198 L 855 | 550.268 248.404 L 856 | 549.976 251.6 L 857 | 549.524 254.777 L 858 | 548.912 257.927 L 859 | S 860 | GR 861 | GS 862 | [0.5 0 0 0.5 341.18676 355.5202] CT 863 | 10.0 ML 864 | N 865 | -4.243 -4.243 M 866 | 4.243 4.243 L 867 | -4.243 4.243 M 868 | 4.243 -4.243 L 869 | S 870 | GR 871 | GS 872 | [0.5 0 0 0.5 353.0237 367.41917] CT 873 | 10.0 ML 874 | N 875 | -4.243 -4.243 M 876 | 4.243 4.243 L 877 | -4.243 4.243 M 878 | 4.243 -4.243 L 879 | S 880 | GR 881 | GS 882 | [0.5 0 0 0.5 176.52528 543.9176] CT 883 | N 884 | 0 -5.5 M 885 | 3.038 -5.5 5.5 -3.038 5.5 0 C 886 | 5.5 0 L 887 | 5.5 3.038 3.038 5.5 0 5.5 C 888 | -3.038 5.5 -5.5 3.038 -5.5 0 C 889 | -5.5 -3.038 -3.038 -5.5 0 -5.5 C 890 | cp 891 | 0 -6.5 M 892 | -3.59 -6.5 -6.5 -3.59 -6.5 0 C 893 | -6.5 3.59 -3.59 6.5 0 6.5 C 894 | 3.59 6.5 6.5 3.59 6.5 0 C 895 | 6.5 0 L 896 | 6.5 -3.59 3.59 -6.5 0 -6.5 C 897 | cp 898 | f 899 | GR 900 | GS 901 | [0.5 0 0 0.5 192.57059 495.78168] CT 902 | N 903 | /f-1521356186{0 -5.5 M 904 | 3.038 -5.5 5.5 -3.038 5.5 0 C 905 | 5.5 0 L 906 | 5.5 3.038 3.038 5.5 0 5.5 C 907 | -3.038 5.5 -5.5 3.038 -5.5 0 C 908 | -5.5 -3.038 -3.038 -5.5 0 -5.5 C 909 | cp 910 | 0 -6.5 M 911 | -3.59 -6.5 -6.5 -3.59 -6.5 0 C 912 | -6.5 3.59 -3.59 6.5 0 6.5 C 913 | 3.59 6.5 6.5 3.59 6.5 0 C 914 | 6.5 0 L 915 | 6.5 -3.59 3.59 -6.5 0 -6.5 C 916 | cp}def 917 | f-1521356186 918 | f 919 | GR 920 | GS 921 | [0.5 0 0 0.5 256.75184 495.78168] CT 922 | N 923 | f-1521356186 924 | f 925 | GR 926 | GS 927 | [0.5 0 0 0.5 272.79716 463.69106] CT 928 | N 929 | f-1521356186 930 | f 931 | GR 932 | GS 933 | [0.5 0 0 0.5 272.79716 447.64574] CT 934 | N 935 | f-1521356186 936 | f 937 | GR 938 | GS 939 | [0.5 0 0 0.5 272.79716 431.60043] CT 940 | N 941 | f-1521356186 942 | f 943 | GR 944 | GS 945 | [0.5 0 0 0.5 272.79716 383.46449] CT 946 | N 947 | f-1521356186 948 | f 949 | GR 950 | GS 951 | [0.5 0 0 0.5 320.9331 383.46449] CT 952 | N 953 | f-1521356186 954 | f 955 | GR 956 | GS 957 | [0.5 0 0 0.5 304.88778 383.46449] CT 958 | N 959 | f-1521356186 960 | f 961 | GR 962 | GS 963 | [0.5 0 0 0.5 320.9331 367.41917] CT 964 | N 965 | f-1521356186 966 | f 967 | GR 968 | GS 969 | [0.5 0 0 0.5 385.11434 319.28324] CT 970 | N 971 | f-1521356186 972 | f 973 | GR 974 | GS 975 | [0.5 0 0 0.5 385.11434 335.32855] CT 976 | N 977 | f-1521356186 978 | f 979 | GR 980 | GS 981 | [0.5 0 0 0.5 433.25027 287.19262] CT 982 | N 983 | f-1521356186 984 | f 985 | GR 986 | GS 987 | [0.5 0 0 0.5 401.15966 319.28324] CT 988 | N 989 | f-1521356186 990 | f 991 | GR 992 | showpage 993 | %%Trailer 994 | %%Pages: 1 995 | %%EOF 996 | -------------------------------------------------------------------------------- /Dynamic Window Approach.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/caokaifa/Matlab-planning/08db498c02139997cd59d13df9c163710b8494f0/Dynamic Window Approach.avi -------------------------------------------------------------------------------- /FrenetTrajectory.m: -------------------------------------------------------------------------------- 1 | classdef FrenetTrajectory 2 | %CFRENETPATH Summary of this class goes here 3 | % Detailed explanation goes here 4 | 5 | properties 6 | % time stamps 7 | t 8 | % Longitudinal coordinate 9 | s % Longitudinal position 10 | ds % Longitudinal velocity 11 | dds % Longitudinal acceleration 12 | ddds % Longitudinal jerk 13 | 14 | % Lateral coordinate 15 | d % Lateral position 16 | dd % Lateral velocity 17 | ddd % Lateral acceleration 18 | dddd % Lateral jerk 19 | 20 | % Costs 21 | Jd % Lateral cost 22 | Js % Longitudinal cost 23 | J % Total cost 24 | 25 | % World coordinates 26 | x % x position 27 | y % y position 28 | theta % Orientation 29 | kappa % Curvature 30 | dL % Running length / arc length 31 | v % Tangential velocity 32 | a % Tangential acceleration 33 | end 34 | 35 | methods 36 | end 37 | 38 | end -------------------------------------------------------------------------------- /Goal!!.ps: -------------------------------------------------------------------------------- 1 | %!PS-Adobe-3.0 2 | %%Creator: (MATLAB, The Mathworks, Inc. Version 9.7.0.1190202 \(R2019b\). Operating System: Windows 10) 3 | %%Title: ./Goal!!.ps 4 | %%CreationDate: 2020-08-16T17:48:09 5 | %%Pages: (atend) 6 | %%BoundingBox: 127 283 468 559 7 | %%LanguageLevel: 3 8 | %%EndComments 9 | %%BeginDefaults 10 | %%EndDefaults 11 | %%BeginProlog 12 | %%EndProlog 13 | %%BeginSetup 14 | %%BeginResource: procset (Apache XML Graphics Std ProcSet) 1.2 0 15 | %%Version: 1.2 0 16 | %%Copyright: (Copyright 2001-2003,2010 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) 17 | /bd{bind def}bind def 18 | /ld{load def}bd 19 | /GR/grestore ld 20 | /GS/gsave ld 21 | /RM/rmoveto ld 22 | /C/curveto ld 23 | /t/show ld 24 | /L/lineto ld 25 | /ML/setmiterlimit ld 26 | /CT/concat ld 27 | /f/fill ld 28 | /N/newpath ld 29 | /S/stroke ld 30 | /CC/setcmykcolor ld 31 | /A/ashow ld 32 | /cp/closepath ld 33 | /RC/setrgbcolor ld 34 | /LJ/setlinejoin ld 35 | /GC/setgray ld 36 | /LW/setlinewidth ld 37 | /M/moveto ld 38 | /re {4 2 roll M 39 | 1 index 0 rlineto 40 | 0 exch rlineto 41 | neg 0 rlineto 42 | cp } bd 43 | /_ctm matrix def 44 | /_tm matrix def 45 | /BT { _ctm currentmatrix pop matrix _tm copy pop 0 0 moveto } bd 46 | /ET { _ctm setmatrix } bd 47 | /iTm { _ctm setmatrix _tm concat } bd 48 | /Tm { _tm astore pop iTm 0 0 moveto } bd 49 | /ux 0.0 def 50 | /uy 0.0 def 51 | /F { 52 | /Tp exch def 53 | /Tf exch def 54 | Tf findfont Tp scalefont setfont 55 | /cf Tf def /cs Tp def 56 | } bd 57 | /ULS {currentpoint /uy exch def /ux exch def} bd 58 | /ULE { 59 | /Tcx currentpoint pop def 60 | gsave 61 | newpath 62 | cf findfont cs scalefont dup 63 | /FontMatrix get 0 get /Ts exch def /FontInfo get dup 64 | /UnderlinePosition get Ts mul /To exch def 65 | /UnderlineThickness get Ts mul /Tt exch def 66 | ux uy To add moveto Tcx uy To add lineto 67 | Tt setlinewidth stroke 68 | grestore 69 | } bd 70 | /OLE { 71 | /Tcx currentpoint pop def 72 | gsave 73 | newpath 74 | cf findfont cs scalefont dup 75 | /FontMatrix get 0 get /Ts exch def /FontInfo get dup 76 | /UnderlinePosition get Ts mul /To exch def 77 | /UnderlineThickness get Ts mul /Tt exch def 78 | ux uy To add cs add moveto Tcx uy To add cs add lineto 79 | Tt setlinewidth stroke 80 | grestore 81 | } bd 82 | /SOE { 83 | /Tcx currentpoint pop def 84 | gsave 85 | newpath 86 | cf findfont cs scalefont dup 87 | /FontMatrix get 0 get /Ts exch def /FontInfo get dup 88 | /UnderlinePosition get Ts mul /To exch def 89 | /UnderlineThickness get Ts mul /Tt exch def 90 | ux uy To add cs 10 mul 26 idiv add moveto Tcx uy To add cs 10 mul 26 idiv add lineto 91 | Tt setlinewidth stroke 92 | grestore 93 | } bd 94 | /QT { 95 | /Y22 exch store 96 | /X22 exch store 97 | /Y21 exch store 98 | /X21 exch store 99 | currentpoint 100 | /Y21 load 2 mul add 3 div exch 101 | /X21 load 2 mul add 3 div exch 102 | /X21 load 2 mul /X22 load add 3 div 103 | /Y21 load 2 mul /Y22 load add 3 div 104 | /X22 load /Y22 load curveto 105 | } bd 106 | /SSPD { 107 | dup length /d exch dict def 108 | { 109 | /v exch def 110 | /k exch def 111 | currentpagedevice k known { 112 | /cpdv currentpagedevice k get def 113 | v cpdv ne { 114 | /upd false def 115 | /nullv v type /nulltype eq def 116 | /nullcpdv cpdv type /nulltype eq def 117 | nullv nullcpdv or 118 | { 119 | /upd true def 120 | } { 121 | /sametype v type cpdv type eq def 122 | sametype { 123 | v type /arraytype eq { 124 | /vlen v length def 125 | /cpdvlen cpdv length def 126 | vlen cpdvlen eq { 127 | 0 1 vlen 1 sub { 128 | /i exch def 129 | /obj v i get def 130 | /cpdobj cpdv i get def 131 | obj cpdobj ne { 132 | /upd true def 133 | exit 134 | } if 135 | } for 136 | } { 137 | /upd true def 138 | } ifelse 139 | } { 140 | v type /dicttype eq { 141 | v { 142 | /dv exch def 143 | /dk exch def 144 | /cpddv cpdv dk get def 145 | dv cpddv ne { 146 | /upd true def 147 | exit 148 | } if 149 | } forall 150 | } { 151 | /upd true def 152 | } ifelse 153 | } ifelse 154 | } if 155 | } ifelse 156 | upd true eq { 157 | d k v put 158 | } if 159 | } if 160 | } if 161 | } forall 162 | d length 0 gt { 163 | d setpagedevice 164 | } if 165 | } bd 166 | /RE { % /NewFontName [NewEncodingArray] /FontName RE - 167 | findfont dup length dict begin 168 | { 169 | 1 index /FID ne 170 | {def} {pop pop} ifelse 171 | } forall 172 | /Encoding exch def 173 | /FontName 1 index def 174 | currentdict definefont pop 175 | end 176 | } bind def 177 | %%EndResource 178 | %%BeginResource: procset (Apache XML Graphics EPS ProcSet) 1.0 0 179 | %%Version: 1.0 0 180 | %%Copyright: (Copyright 2002-2003 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) 181 | /BeginEPSF { %def 182 | /b4_Inc_state save def % Save state for cleanup 183 | /dict_count countdictstack def % Count objects on dict stack 184 | /op_count count 1 sub def % Count objects on operand stack 185 | userdict begin % Push userdict on dict stack 186 | /showpage { } def % Redefine showpage, { } = null proc 187 | 0 setgray 0 setlinecap % Prepare graphics state 188 | 1 setlinewidth 0 setlinejoin 189 | 10 setmiterlimit [ ] 0 setdash newpath 190 | /languagelevel where % If level not equal to 1 then 191 | {pop languagelevel % set strokeadjust and 192 | 1 ne % overprint to their defaults. 193 | {false setstrokeadjust false setoverprint 194 | } if 195 | } if 196 | } bd 197 | /EndEPSF { %def 198 | count op_count sub {pop} repeat % Clean up stacks 199 | countdictstack dict_count sub {end} repeat 200 | b4_Inc_state restore 201 | } bd 202 | %%EndResource 203 | %FOPBeginFontDict 204 | %%IncludeResource: font Courier-Oblique 205 | %%IncludeResource: font Courier-BoldOblique 206 | %%IncludeResource: font Courier-Bold 207 | %%IncludeResource: font ZapfDingbats 208 | %%IncludeResource: font Symbol 209 | %%IncludeResource: font Helvetica 210 | %%IncludeResource: font Helvetica-Oblique 211 | %%IncludeResource: font Helvetica-Bold 212 | %%IncludeResource: font Helvetica-BoldOblique 213 | %%IncludeResource: font Times-Roman 214 | %%IncludeResource: font Times-Italic 215 | %%IncludeResource: font Times-Bold 216 | %%IncludeResource: font Times-BoldItalic 217 | %%IncludeResource: font Courier 218 | %FOPEndFontDict 219 | %%BeginResource: encoding WinAnsiEncoding 220 | /WinAnsiEncoding [ 221 | /.notdef /.notdef /.notdef /.notdef /.notdef 222 | /.notdef /.notdef /.notdef /.notdef /.notdef 223 | /.notdef /.notdef /.notdef /.notdef /.notdef 224 | /.notdef /.notdef /.notdef /.notdef /.notdef 225 | /.notdef /.notdef /.notdef /.notdef /.notdef 226 | /.notdef /.notdef /.notdef /.notdef /.notdef 227 | /.notdef /.notdef /space /exclam /quotedbl 228 | /numbersign /dollar /percent /ampersand /quotesingle 229 | /parenleft /parenright /asterisk /plus /comma 230 | /hyphen /period /slash /zero /one 231 | /two /three /four /five /six 232 | /seven /eight /nine /colon /semicolon 233 | /less /equal /greater /question /at 234 | /A /B /C /D /E 235 | /F /G /H /I /J 236 | /K /L /M /N /O 237 | /P /Q /R /S /T 238 | /U /V /W /X /Y 239 | /Z /bracketleft /backslash /bracketright /asciicircum 240 | /underscore /quoteleft /a /b /c 241 | /d /e /f /g /h 242 | /i /j /k /l /m 243 | /n /o /p /q /r 244 | /s /t /u /v /w 245 | /x /y /z /braceleft /bar 246 | /braceright /asciitilde /bullet /Euro /bullet 247 | /quotesinglbase /florin /quotedblbase /ellipsis /dagger 248 | /daggerdbl /circumflex /perthousand /Scaron /guilsinglleft 249 | /OE /bullet /Zcaron /bullet /bullet 250 | /quoteleft /quoteright /quotedblleft /quotedblright /bullet 251 | /endash /emdash /asciitilde /trademark /scaron 252 | /guilsinglright /oe /bullet /zcaron /Ydieresis 253 | /space /exclamdown /cent /sterling /currency 254 | /yen /brokenbar /section /dieresis /copyright 255 | /ordfeminine /guillemotleft /logicalnot /sfthyphen /registered 256 | /macron /degree /plusminus /twosuperior /threesuperior 257 | /acute /mu /paragraph /middot /cedilla 258 | /onesuperior /ordmasculine /guillemotright /onequarter /onehalf 259 | /threequarters /questiondown /Agrave /Aacute /Acircumflex 260 | /Atilde /Adieresis /Aring /AE /Ccedilla 261 | /Egrave /Eacute /Ecircumflex /Edieresis /Igrave 262 | /Iacute /Icircumflex /Idieresis /Eth /Ntilde 263 | /Ograve /Oacute /Ocircumflex /Otilde /Odieresis 264 | /multiply /Oslash /Ugrave /Uacute /Ucircumflex 265 | /Udieresis /Yacute /Thorn /germandbls /agrave 266 | /aacute /acircumflex /atilde /adieresis /aring 267 | /ae /ccedilla /egrave /eacute /ecircumflex 268 | /edieresis /igrave /iacute /icircumflex /idieresis 269 | /eth /ntilde /ograve /oacute /ocircumflex 270 | /otilde /odieresis /divide /oslash /ugrave 271 | /uacute /ucircumflex /udieresis /yacute /thorn 272 | /ydieresis 273 | ] def 274 | %%EndResource 275 | %FOPBeginFontReencode 276 | /Courier-Oblique findfont 277 | dup length dict begin 278 | {1 index /FID ne {def} {pop pop} ifelse} forall 279 | /Encoding WinAnsiEncoding def 280 | currentdict 281 | end 282 | /Courier-Oblique exch definefont pop 283 | /Courier-BoldOblique findfont 284 | dup length dict begin 285 | {1 index /FID ne {def} {pop pop} ifelse} forall 286 | /Encoding WinAnsiEncoding def 287 | currentdict 288 | end 289 | /Courier-BoldOblique exch definefont pop 290 | /Courier-Bold findfont 291 | dup length dict begin 292 | {1 index /FID ne {def} {pop pop} ifelse} forall 293 | /Encoding WinAnsiEncoding def 294 | currentdict 295 | end 296 | /Courier-Bold exch definefont pop 297 | /Helvetica findfont 298 | dup length dict begin 299 | {1 index /FID ne {def} {pop pop} ifelse} forall 300 | /Encoding WinAnsiEncoding def 301 | currentdict 302 | end 303 | /Helvetica exch definefont pop 304 | /Helvetica-Oblique findfont 305 | dup length dict begin 306 | {1 index /FID ne {def} {pop pop} ifelse} forall 307 | /Encoding WinAnsiEncoding def 308 | currentdict 309 | end 310 | /Helvetica-Oblique exch definefont pop 311 | /Helvetica-Bold findfont 312 | dup length dict begin 313 | {1 index /FID ne {def} {pop pop} ifelse} forall 314 | /Encoding WinAnsiEncoding def 315 | currentdict 316 | end 317 | /Helvetica-Bold exch definefont pop 318 | /Helvetica-BoldOblique findfont 319 | dup length dict begin 320 | {1 index /FID ne {def} {pop pop} ifelse} forall 321 | /Encoding WinAnsiEncoding def 322 | currentdict 323 | end 324 | /Helvetica-BoldOblique exch definefont pop 325 | /Times-Roman findfont 326 | dup length dict begin 327 | {1 index /FID ne {def} {pop pop} ifelse} forall 328 | /Encoding WinAnsiEncoding def 329 | currentdict 330 | end 331 | /Times-Roman exch definefont pop 332 | /Times-Italic findfont 333 | dup length dict begin 334 | {1 index /FID ne {def} {pop pop} ifelse} forall 335 | /Encoding WinAnsiEncoding def 336 | currentdict 337 | end 338 | /Times-Italic exch definefont pop 339 | /Times-Bold findfont 340 | dup length dict begin 341 | {1 index /FID ne {def} {pop pop} ifelse} forall 342 | /Encoding WinAnsiEncoding def 343 | currentdict 344 | end 345 | /Times-Bold exch definefont pop 346 | /Times-BoldItalic findfont 347 | dup length dict begin 348 | {1 index /FID ne {def} {pop pop} ifelse} forall 349 | /Encoding WinAnsiEncoding def 350 | currentdict 351 | end 352 | /Times-BoldItalic exch definefont pop 353 | /Courier findfont 354 | dup length dict begin 355 | {1 index /FID ne {def} {pop pop} ifelse} forall 356 | /Encoding WinAnsiEncoding def 357 | currentdict 358 | end 359 | /Courier exch definefont pop 360 | %FOPEndFontReencode 361 | %%EndSetup 362 | %%Page: 1 1 363 | %%PageBoundingBox: 0 0 595 842 364 | %%BeginPageSetup 365 | << 366 | /PageSize [595 842] 367 | /ImagingBBox null 368 | >> setpagedevice 369 | [1 0 0 -1 0 842] CT 370 | %%EndPageSetup 371 | GS 372 | [0.5 0 0 0.5 87.63778 263.55511] CT 373 | 1 GC 374 | N 375 | 0 0 840 630 re 376 | f 377 | GR 378 | GS 379 | [0.5 0 0 0.5 87.63778 263.55511] CT 380 | 1 GC 381 | N 382 | 0 0 840 630 re 383 | f 384 | GR 385 | GS 386 | [0.5 0 0 0.5 87.63778 263.55511] CT 387 | 1 GC 388 | N 389 | 109 560.725 M 390 | 760 560.725 L 391 | 760 47.275 L 392 | 109 47.275 L 393 | cp 394 | f 395 | GR 396 | GS 397 | [0.5 0 0 0.5 87.63778 263.55511] CT 398 | 2 setlinecap 399 | 1 LJ 400 | N 401 | 109 560.725 M 402 | 760 560.725 L 403 | S 404 | GR 405 | GS 406 | [0.5 0 0 0.5 87.63778 263.55511] CT 407 | 2 setlinecap 408 | 1 LJ 409 | N 410 | 109 47.275 M 411 | 760 47.275 L 412 | S 413 | GR 414 | GS 415 | [0.5 0 0 0.5 87.63778 263.55511] CT 416 | 2 setlinecap 417 | 1 LJ 418 | N 419 | 145.684 560.725 M 420 | 145.684 554.215 L 421 | S 422 | GR 423 | GS 424 | [0.5 0 0 0.5 87.63778 263.55511] CT 425 | 2 setlinecap 426 | 1 LJ 427 | N 428 | 209.866 560.725 M 429 | 209.866 554.215 L 430 | S 431 | GR 432 | GS 433 | [0.5 0 0 0.5 87.63778 263.55511] CT 434 | 2 setlinecap 435 | 1 LJ 436 | N 437 | 274.047 560.725 M 438 | 274.047 554.215 L 439 | S 440 | GR 441 | GS 442 | [0.5 0 0 0.5 87.63778 263.55511] CT 443 | 2 setlinecap 444 | 1 LJ 445 | N 446 | 338.228 560.725 M 447 | 338.228 554.215 L 448 | S 449 | GR 450 | GS 451 | [0.5 0 0 0.5 87.63778 263.55511] CT 452 | 2 setlinecap 453 | 1 LJ 454 | N 455 | 402.409 560.725 M 456 | 402.409 554.215 L 457 | S 458 | GR 459 | GS 460 | [0.5 0 0 0.5 87.63778 263.55511] CT 461 | 2 setlinecap 462 | 1 LJ 463 | N 464 | 466.591 560.725 M 465 | 466.591 554.215 L 466 | S 467 | GR 468 | GS 469 | [0.5 0 0 0.5 87.63778 263.55511] CT 470 | 2 setlinecap 471 | 1 LJ 472 | N 473 | 530.772 560.725 M 474 | 530.772 554.215 L 475 | S 476 | GR 477 | GS 478 | [0.5 0 0 0.5 87.63778 263.55511] CT 479 | 2 setlinecap 480 | 1 LJ 481 | N 482 | 594.953 560.725 M 483 | 594.953 554.215 L 484 | S 485 | GR 486 | GS 487 | [0.5 0 0 0.5 87.63778 263.55511] CT 488 | 2 setlinecap 489 | 1 LJ 490 | N 491 | 659.134 560.725 M 492 | 659.134 554.215 L 493 | S 494 | GR 495 | GS 496 | [0.5 0 0 0.5 87.63778 263.55511] CT 497 | 2 setlinecap 498 | 1 LJ 499 | N 500 | 723.316 560.725 M 501 | 723.316 554.215 L 502 | S 503 | GR 504 | GS 505 | [0.5 0 0 0.5 87.63778 263.55511] CT 506 | 2 setlinecap 507 | 1 LJ 508 | N 509 | 145.684 47.275 M 510 | 145.684 53.785 L 511 | S 512 | GR 513 | GS 514 | [0.5 0 0 0.5 87.63778 263.55511] CT 515 | 2 setlinecap 516 | 1 LJ 517 | N 518 | 209.866 47.275 M 519 | 209.866 53.785 L 520 | S 521 | GR 522 | GS 523 | [0.5 0 0 0.5 87.63778 263.55511] CT 524 | 2 setlinecap 525 | 1 LJ 526 | N 527 | 274.047 47.275 M 528 | 274.047 53.785 L 529 | S 530 | GR 531 | GS 532 | [0.5 0 0 0.5 87.63778 263.55511] CT 533 | 2 setlinecap 534 | 1 LJ 535 | N 536 | 338.228 47.275 M 537 | 338.228 53.785 L 538 | S 539 | GR 540 | GS 541 | [0.5 0 0 0.5 87.63778 263.55511] CT 542 | 2 setlinecap 543 | 1 LJ 544 | N 545 | 402.409 47.275 M 546 | 402.409 53.785 L 547 | S 548 | GR 549 | GS 550 | [0.5 0 0 0.5 87.63778 263.55511] CT 551 | 2 setlinecap 552 | 1 LJ 553 | N 554 | 466.591 47.275 M 555 | 466.591 53.785 L 556 | S 557 | GR 558 | GS 559 | [0.5 0 0 0.5 87.63778 263.55511] CT 560 | 2 setlinecap 561 | 1 LJ 562 | N 563 | 530.772 47.275 M 564 | 530.772 53.785 L 565 | S 566 | GR 567 | GS 568 | [0.5 0 0 0.5 87.63778 263.55511] CT 569 | 2 setlinecap 570 | 1 LJ 571 | N 572 | 594.953 47.275 M 573 | 594.953 53.785 L 574 | S 575 | GR 576 | GS 577 | [0.5 0 0 0.5 87.63778 263.55511] CT 578 | 2 setlinecap 579 | 1 LJ 580 | N 581 | 659.134 47.275 M 582 | 659.134 53.785 L 583 | S 584 | GR 585 | GS 586 | [0.5 0 0 0.5 87.63778 263.55511] CT 587 | 2 setlinecap 588 | 1 LJ 589 | N 590 | 723.316 47.275 M 591 | 723.316 53.785 L 592 | S 593 | GR 594 | GS 595 | [0.5 0 0 0.5 160.47997 547.9176] CT 596 | /Helvetica 20 F 597 | GS 598 | [1 0 0 1 0 0] CT 599 | -9 21 moveto 600 | 1 -1 scale 601 | (-2) t 602 | GR 603 | GR 604 | GS 605 | [0.5 0 0 0.5 192.57059 547.9176] CT 606 | /Helvetica 20 F 607 | GS 608 | [1 0 0 1 0 0] CT 609 | -6 21 moveto 610 | 1 -1 scale 611 | (0) t 612 | GR 613 | GR 614 | GS 615 | [0.5 0 0 0.5 224.66122 547.9176] CT 616 | /Helvetica 20 F 617 | GS 618 | [1 0 0 1 0 0] CT 619 | -6 21 moveto 620 | 1 -1 scale 621 | (2) t 622 | GR 623 | GR 624 | GS 625 | [0.5 0 0 0.5 256.75184 547.9176] CT 626 | /Helvetica 20 F 627 | GS 628 | [1 0 0 1 0 0] CT 629 | -6 21 moveto 630 | 1 -1 scale 631 | (4) t 632 | GR 633 | GR 634 | GS 635 | [0.5 0 0 0.5 288.84246 547.9176] CT 636 | /Helvetica 20 F 637 | GS 638 | [1 0 0 1 0 0] CT 639 | -6 21 moveto 640 | 1 -1 scale 641 | (6) t 642 | GR 643 | GR 644 | GS 645 | [0.5 0 0 0.5 320.9331 547.9176] CT 646 | /Helvetica 20 F 647 | GS 648 | [1 0 0 1 0 0] CT 649 | -6 21 moveto 650 | 1 -1 scale 651 | (8) t 652 | GR 653 | GR 654 | GS 655 | [0.5 0 0 0.5 353.0237 547.9176] CT 656 | /Helvetica 20 F 657 | GS 658 | [1 0 0 1 0 0] CT 659 | -11.5 21 moveto 660 | 1 -1 scale 661 | (10) t 662 | GR 663 | GR 664 | GS 665 | [0.5 0 0 0.5 385.11434 547.9176] CT 666 | /Helvetica 20 F 667 | GS 668 | [1 0 0 1 0 0] CT 669 | -11.5 21 moveto 670 | 1 -1 scale 671 | (12) t 672 | GR 673 | GR 674 | GS 675 | [0.5 0 0 0.5 417.20498 547.9176] CT 676 | /Helvetica 20 F 677 | GS 678 | [1 0 0 1 0 0] CT 679 | -11.5 21 moveto 680 | 1 -1 scale 681 | (14) t 682 | GR 683 | GR 684 | GS 685 | [0.5 0 0 0.5 449.29559 547.9176] CT 686 | /Helvetica 20 F 687 | GS 688 | [1 0 0 1 0 0] CT 689 | -11.5 21 moveto 690 | 1 -1 scale 691 | (16) t 692 | GR 693 | GR 694 | GS 695 | [0.5 0 0 0.5 87.63778 263.55511] CT 696 | 2 setlinecap 697 | 1 LJ 698 | N 699 | 109 560.725 M 700 | 109 47.275 L 701 | S 702 | GR 703 | GS 704 | [0.5 0 0 0.5 87.63778 263.55511] CT 705 | 2 setlinecap 706 | 1 LJ 707 | N 708 | 760 560.725 M 709 | 760 47.275 L 710 | S 711 | GR 712 | GS 713 | [0.5 0 0 0.5 87.63778 263.55511] CT 714 | 2 setlinecap 715 | 1 LJ 716 | N 717 | 109 528.634 M 718 | 115.51 528.634 L 719 | S 720 | GR 721 | GS 722 | [0.5 0 0 0.5 87.63778 263.55511] CT 723 | 2 setlinecap 724 | 1 LJ 725 | N 726 | 109 368.181 M 727 | 115.51 368.181 L 728 | S 729 | GR 730 | GS 731 | [0.5 0 0 0.5 87.63778 263.55511] CT 732 | 2 setlinecap 733 | 1 LJ 734 | N 735 | 109 207.728 M 736 | 115.51 207.728 L 737 | S 738 | GR 739 | GS 740 | [0.5 0 0 0.5 87.63778 263.55511] CT 741 | 2 setlinecap 742 | 1 LJ 743 | N 744 | 109 47.275 M 745 | 115.51 47.275 L 746 | S 747 | GR 748 | GS 749 | [0.5 0 0 0.5 87.63778 263.55511] CT 750 | 2 setlinecap 751 | 1 LJ 752 | N 753 | 760 528.634 M 754 | 753.49 528.634 L 755 | S 756 | GR 757 | GS 758 | [0.5 0 0 0.5 87.63778 263.55511] CT 759 | 2 setlinecap 760 | 1 LJ 761 | N 762 | 760 368.181 M 763 | 753.49 368.181 L 764 | S 765 | GR 766 | GS 767 | [0.5 0 0 0.5 87.63778 263.55511] CT 768 | 2 setlinecap 769 | 1 LJ 770 | N 771 | 760 207.728 M 772 | 753.49 207.728 L 773 | S 774 | GR 775 | GS 776 | [0.5 0 0 0.5 87.63778 263.55511] CT 777 | 2 setlinecap 778 | 1 LJ 779 | N 780 | 760 47.275 M 781 | 753.49 47.275 L 782 | S 783 | GR 784 | GS 785 | [0.5 0 0 0.5 138.13778 527.87231] CT 786 | /Helvetica 20 F 787 | GS 788 | [1 0 0 1 0 0] CT 789 | -12 8 moveto 790 | 1 -1 scale 791 | (0) t 792 | GR 793 | GR 794 | GS 795 | [0.5 0 0 0.5 138.13778 447.64574] CT 796 | /Helvetica 20 F 797 | GS 798 | [1 0 0 1 0 0] CT 799 | -12 8 moveto 800 | 1 -1 scale 801 | (5) t 802 | GR 803 | GR 804 | GS 805 | [0.5 0 0 0.5 138.13778 367.41917] CT 806 | /Helvetica 20 F 807 | GS 808 | [1 0 0 1 0 0] CT 809 | -23 8 moveto 810 | 1 -1 scale 811 | (10) t 812 | GR 813 | GR 814 | GS 815 | [0.5 0 0 0.5 138.13778 287.19262] CT 816 | /Helvetica 20 F 817 | GS 818 | [1 0 0 1 0 0] CT 819 | -23 8 moveto 820 | 1 -1 scale 821 | (15) t 822 | GR 823 | GR 824 | GS 825 | [0.5 0 0 0.5 87.63778 263.55511] CT 826 | 1 LJ 827 | N 828 | 507.098 183.93 M 829 | 510.104 185.052 L 830 | 513.042 186.345 L 831 | 515.899 187.805 L 832 | 518.669 189.426 L 833 | 521.341 191.203 L 834 | 523.907 193.131 L 835 | 526.357 195.202 L 836 | 528.685 197.411 L 837 | 530.882 199.75 L 838 | 532.942 202.212 L 839 | 534.856 204.787 L 840 | 536.62 207.468 L 841 | 538.227 210.245 L 842 | 539.672 213.111 L 843 | 540.95 216.054 L 844 | 542.057 219.066 L 845 | 542.989 222.137 L 846 | 543.744 225.256 L 847 | 544.318 228.413 L 848 | 544.71 231.598 L 849 | 544.919 234.801 L 850 | 544.944 238.01 L 851 | 544.784 241.215 L 852 | 544.441 244.405 L 853 | 543.915 247.571 L 854 | 543.209 250.701 L 855 | 542.323 253.786 L 856 | 541.263 256.815 L 857 | 540.03 259.778 L 858 | 538.629 262.665 L 859 | S 860 | GR 861 | GS 862 | [0.5 0 0 0.5 342.68998 356.08134] CT 863 | 10.0 ML 864 | N 865 | -4.243 -4.243 M 866 | 4.243 4.243 L 867 | -4.243 4.243 M 868 | 4.243 -4.243 L 869 | S 870 | GR 871 | GS 872 | [0.5 0 0 0.5 353.0237 367.41917] CT 873 | 10.0 ML 874 | N 875 | -4.243 -4.243 M 876 | 4.243 4.243 L 877 | -4.243 4.243 M 878 | 4.243 -4.243 L 879 | S 880 | GR 881 | GS 882 | [0.5 0 0 0.5 176.52528 543.9176] CT 883 | N 884 | 0 -5.5 M 885 | 3.038 -5.5 5.5 -3.038 5.5 0 C 886 | 5.5 0 L 887 | 5.5 3.038 3.038 5.5 0 5.5 C 888 | -3.038 5.5 -5.5 3.038 -5.5 0 C 889 | -5.5 -3.038 -3.038 -5.5 0 -5.5 C 890 | cp 891 | 0 -6.5 M 892 | -3.59 -6.5 -6.5 -3.59 -6.5 0 C 893 | -6.5 3.59 -3.59 6.5 0 6.5 C 894 | 3.59 6.5 6.5 3.59 6.5 0 C 895 | 6.5 0 L 896 | 6.5 -3.59 3.59 -6.5 0 -6.5 C 897 | cp 898 | f 899 | GR 900 | GS 901 | [0.5 0 0 0.5 192.57059 495.78168] CT 902 | N 903 | /f-1521356186{0 -5.5 M 904 | 3.038 -5.5 5.5 -3.038 5.5 0 C 905 | 5.5 0 L 906 | 5.5 3.038 3.038 5.5 0 5.5 C 907 | -3.038 5.5 -5.5 3.038 -5.5 0 C 908 | -5.5 -3.038 -3.038 -5.5 0 -5.5 C 909 | cp 910 | 0 -6.5 M 911 | -3.59 -6.5 -6.5 -3.59 -6.5 0 C 912 | -6.5 3.59 -3.59 6.5 0 6.5 C 913 | 3.59 6.5 6.5 3.59 6.5 0 C 914 | 6.5 0 L 915 | 6.5 -3.59 3.59 -6.5 0 -6.5 C 916 | cp}def 917 | f-1521356186 918 | f 919 | GR 920 | GS 921 | [0.5 0 0 0.5 256.75184 495.78168] CT 922 | N 923 | f-1521356186 924 | f 925 | GR 926 | GS 927 | [0.5 0 0 0.5 272.79716 463.69106] CT 928 | N 929 | f-1521356186 930 | f 931 | GR 932 | GS 933 | [0.5 0 0 0.5 272.79716 447.64574] CT 934 | N 935 | f-1521356186 936 | f 937 | GR 938 | GS 939 | [0.5 0 0 0.5 272.79716 431.60043] CT 940 | N 941 | f-1521356186 942 | f 943 | GR 944 | GS 945 | [0.5 0 0 0.5 272.79716 383.46449] CT 946 | N 947 | f-1521356186 948 | f 949 | GR 950 | GS 951 | [0.5 0 0 0.5 320.9331 383.46449] CT 952 | N 953 | f-1521356186 954 | f 955 | GR 956 | GS 957 | [0.5 0 0 0.5 304.88778 383.46449] CT 958 | N 959 | f-1521356186 960 | f 961 | GR 962 | GS 963 | [0.5 0 0 0.5 320.9331 367.41917] CT 964 | N 965 | f-1521356186 966 | f 967 | GR 968 | GS 969 | [0.5 0 0 0.5 385.11434 319.28324] CT 970 | N 971 | f-1521356186 972 | f 973 | GR 974 | GS 975 | [0.5 0 0 0.5 385.11434 335.32855] CT 976 | N 977 | f-1521356186 978 | f 979 | GR 980 | GS 981 | [0.5 0 0 0.5 433.25027 287.19262] CT 982 | N 983 | f-1521356186 984 | f 985 | GR 986 | GS 987 | [0.5 0 0 0.5 401.15966 319.28324] CT 988 | N 989 | f-1521356186 990 | f 991 | GR 992 | showpage 993 | %%Trailer 994 | %%Pages: 1 995 | %%EOF 996 | -------------------------------------------------------------------------------- /OptimalFrenetPlanner.m: -------------------------------------------------------------------------------- 1 | classdef OptimalFrenetPlanner 2 | %define the frenet plannar algorithm 3 | properties 4 | % 1.set up parameters 5 | MAX_SPEED=50.0/3.6; % maximum speed [m/s] 6 | MAX_ACCEL=2.0; % maximum accleration [m/ss] 7 | MAX_CURVATURE=1.0; % maximum curvature [1/m] 8 | MAX_ROAD_WIDTH=7.0; % maximum road width [m] 9 | MIN_ROAD_WIDTH= -7.0 % minimum road width [m] 10 | D_ROAD_W=1.0; % road width sampling length[m] 11 | DT=0.2; % time tick [s] 12 | MAXT=5.0; % max prediction time [s] 13 | MINT=4.0; % min prediction time[s] 14 | TARGET_SPEED=30.0/3.6; % target speed [s] 15 | D_T_S=5/3.6; % target speed sampling length [m/s] 16 | N_S_SAMPLE=1; % sampling number of target speed 17 | ROBOT_RADIUS=2.0; % robot radius [m] 18 | 19 | % cost weight 20 | KJ=0.1; % Jerk 21 | KT=0.1; % time 22 | KD=1.0; % Distance from reference path 23 | KV=1.0; % Target speed 24 | KLAT=1.0; % Lateral 25 | KLON=1.0; % Longitudinal 26 | 27 | numberObjects 28 | end 29 | 30 | methods ( Access = public) 31 | % \brief Calculate trajectories in the frenet space 32 | % \details Use the given start dynamics and generate trajectory variations 33 | % by sampling 34 | % - The lateral position -max_road_width:delta_road_width:max_road_width 35 | % s0: Longitudinal start position 36 | % ds0: Initial longitudinal velocity 37 | % d0: Lateral start position (offset from reference path) 38 | % dd0: Initial lateral velocity 39 | % ddd0: Initial lateral acceleration 40 | function frenetTrajectories = CalcFrenetTrajectories(obj, s0, ds0,d0,dd0,ddd0) 41 | % generate path for each offset goal 42 | % Lateral sampling space 43 | sizeLatSampleSpace = length(obj.MIN_ROAD_WIDTH:obj.D_ROAD_W:obj.MAX_ROAD_WIDTH); 44 | sizeTimeSpace=length(obj.MINT:obj.DT:obj.MAXT); 45 | sizeLonSampleSpace=length((obj.TARGET_SPEED - obj.D_T_S * obj.N_S_SAMPLE):obj.D_T_S: ... 46 | (obj.TARGET_SPEED + obj.D_T_S * obj.N_S_SAMPLE)); 47 | numberTrajectories=sizeLatSampleSpace*sizeTimeSpace*sizeLonSampleSpace; 48 | frenetTrajectories=cell(1, numberTrajectories); 49 | iTraj=1; 50 | for di=obj.MIN_ROAD_WIDTH: obj.D_ROAD_W:obj.MAX_ROAD_WIDTH 51 | %Lateral motion palnning 52 | for Ti=obj.MINT:obj.DT:obj.MAXT 53 | 54 | % Generate quintic polynomial for lateral plan using dynamics 55 | % d0; start position offset 56 | % dd0: start lateral velocity 57 | % ddd0: start lateral acceleration 58 | % di: Varoated lateral target lateral position 59 | % ddT: lateral target velocity 60 | % dddT: Lateral target acceleration 61 | ddT=0; 62 | dddT=0; 63 | latPoly5 = QuinticPoly(d0, dd0, ddd0, di, ddT, dddT, Ti); 64 | 65 | %create a frenet trajetory consisting of 66 | % s (longitudinal) and d (lateral) dynamics 67 | % and initialize the lateral (d) part 68 | ft=FrenetTrajectory(); 69 | ft.t=0.0:obj.DT:Ti; 70 | ft.d=latPoly5.X(ft.t); 71 | ft.dd=latPoly5.dX(ft.t); 72 | ft.ddd=latPoly5.ddX(ft.t); 73 | ft.dddd=latPoly5.dddX(ft.t); 74 | % hold on 75 | % plot(ft.t,ft.d,'*r') 76 | 77 | % Longitudinal motion planning (velocity keeping) 78 | for tv = (obj.TARGET_SPEED - obj.D_T_S * obj.N_S_SAMPLE): obj.D_T_S: (obj.TARGET_SPEED... 79 | + obj.D_T_S * obj.N_S_SAMPLE) 80 | targetft=ft; 81 | lonPoly4=QuarticPoly(s0, ds0, 0.0, tv, 0.0, Ti); 82 | 83 | targetft.s=lonPoly4.X(ft.t); 84 | targetft.ds=lonPoly4.dX(ft.t); 85 | targetft.dds=lonPoly4.ddX(ft.t); 86 | targetft.ddds=lonPoly4.dddX(ft.t); 87 | % hold on 88 | % plot(ft.t,targetft.s,'*g') 89 | % Square of lateral jerk 90 | Jd = sum(targetft.dddd.^2); 91 | % Square of longitudinal jerk 92 | Js = sum(targetft.ddds.^2); 93 | 94 | % Square of diff from target speed 95 | dv = (obj.TARGET_SPEED - targetft.ds(end)).^2; 96 | 97 | targetft.Jd = obj.KJ * Jd + obj.KT * Ti + obj.KD * targetft.d(end)^2 ; 98 | targetft.Js = obj.KJ * Js + obj.KT * Ti + obj.KV * dv; 99 | targetft.J=obj.KLAT * targetft.Jd + obj.KLON * targetft.Js; 100 | 101 | frenetTrajectories{iTraj} = targetft; 102 | iTraj = iTraj + 1; 103 | end 104 | end 105 | end 106 | end 107 | 108 | function frenetTrajectories = CalcGlobalTrajectories(obj, frenetTrajectories, referencePath) 109 | for iTarj = 1: length(frenetTrajectories) 110 | ft = frenetTrajectories{iTarj}; 111 | % calc global positions 112 | for i = 1:(length(ft.s)) 113 | [ix, iy] = referencePath.calc_position(ft.s(i)); 114 | if isnan(ix) 115 | break 116 | end 117 | iyaw = referencePath.calc_yaw(ft.s(i)); 118 | di = ft.d(i); 119 | fx = ix + di * cos(iyaw + pi /2.0); 120 | fy = iy + di * sin(iyaw + pi / 2.0); 121 | ft.x(end+1)=fx; 122 | ft.y(end+1)=fy; 123 | 124 | end 125 | % plot(ft.x, ft.y, 'color', [1, 1, 1]*0.5) 126 | % drawnow; 127 | 128 | % calc theta and dL (running length) 129 | for i = 1: (length(ft.x) - 1) 130 | dx = ft.x(i+1) - ft.x(i); 131 | dy = ft.y(i+1) - ft.y(i); 132 | ft.theta(end+1) = atan2(dy, dx); 133 | ft.dL(end+1) = sqrt(dx^2 + dy^2); 134 | end 135 | 136 | ft.theta(end+1) = ft.theta(end); 137 | ft.dL(end+1) = ft.dL(end); 138 | 139 | % calc curvature 140 | for i = 1: (length(ft.theta) - 1) 141 | ft.kappa(end+1) = (ft.theta(i+1) - ft.theta(i)) / ft.dL(i) ; 142 | end 143 | ft.kappa(end+1) = ft.kappa(end); 144 | 145 | frenetTrajectories{iTarj} = ft; 146 | 147 | end 148 | end 149 | 150 | 151 | function collision = CheckCollision(obj, ft, objects) 152 | 153 | for i = 1:obj.numberObjects 154 | ox = objects(i, 1); 155 | oy = objects(i, 2); 156 | d = zeros(length(ft.x), 1); 157 | for idxPoint = 1:length(ft.x) 158 | ix = ft.x(idxPoint); 159 | iy = ft.y(idxPoint); 160 | d(idxPoint) = ((ix - ox)^2 + (iy - oy)^2); 161 | end 162 | collision = any(d <= 1^2); 163 | if collision 164 | % plot(ft.x, ft.y, 'rx') 165 | % plot(ox, oy, 'yo'); 166 | % drawnow; 167 | return; 168 | end 169 | end 170 | collision = 0; 171 | end 172 | 173 | function okTrajectories = CheckTrajectories(obj, frenetTrajectories, objects) 174 | okTrajectories = {}; 175 | for i = 1 : (length( frenetTrajectories)) 176 | ft = frenetTrajectories{i}; 177 | if any(ft.ds > obj.MAX_SPEED) % Max speed check 178 | continue 179 | elseif any(abs(ft.dds) > obj.MAX_ACCEL) % Max accleration check 180 | continue 181 | elseif any(abs(ft.kappa) > obj.MAX_CURVATURE) % Max curvature check 182 | continue 183 | elseif (obj.CheckCollision(ft, objects)==1) 184 | continue 185 | end 186 | okTrajectories{end+1} = ft; 187 | % plot(ft.x, ft.y, 'g'); 188 | % drawnow; 189 | end 190 | end 191 | 192 | function bestpath = FrenetOptimalPlanning(obj, referencePath, s0, ds0, d0, dd0, ddd0, objects) 193 | % Initialization 194 | obj.numberObjects = size(objects, 1); 195 | 196 | frenetTrajectories = obj.CalcFrenetTrajectories(s0, ds0, d0, dd0, ddd0); 197 | frenetTrajectories = obj.CalcGlobalTrajectories(frenetTrajectories, referencePath); 198 | frenetTrajectories = obj.CheckTrajectories(frenetTrajectories, objects); 199 | 200 | % Find minimum cost trajectory 201 | mincost = inf; 202 | bestpath = NaN; 203 | for iTraj = 1: (length(frenetTrajectories)) 204 | ft = frenetTrajectories{iTraj}; 205 | if (mincost >= ft.J) 206 | mincost = ft.J; 207 | bestpath = ft; 208 | end 209 | end 210 | 211 | end 212 | 213 | function referencePath = CalcReferencePath(x, y, ds) 214 | referencePath = CalcSplineCourse(x, y, ds); 215 | end 216 | 217 | end 218 | end 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | -------------------------------------------------------------------------------- /QuarticPoly.m: -------------------------------------------------------------------------------- 1 | classdef QuarticPoly 2 | % QuarticPoly Summary 3 | 4 | properties 5 | %coefficients (c0, c1, c2, c3, c4) 6 | c 7 | end 8 | 9 | methods (Access = public) 10 | 11 | function obj = QuarticPoly(xi0, dxi0, ddxi0, dxiT, ddxiT, T) 12 | c012= [xi0; dxi0 ; ddxi0 / 2.0]; 13 | 14 | M1 = [0, 1, 2*T; 15 | 0, 0, 2 ]; 16 | 17 | M2= [3*T^2, 4*T^3; 18 | 6*T, 12*T^2]; 19 | 20 | c34 = M2\([dxiT; ddxiT] - M1 * c012); 21 | obj.c = [c012; c34]; 22 | end 23 | 24 | function x = X(obj, t) 25 | x = obj.c(1) + obj.c(2).*t + obj.c(3).*t.^2 + obj.c(4).*t.^3 + obj.c(5).*t.^4; 26 | end 27 | 28 | function x = dX(obj, t) 29 | x = obj.c(2) + 2*obj.c(3).*t + 3*obj.c(4).*t.^2 + 4*obj.c(5).*t.^3; 30 | end 31 | 32 | function x = ddX(obj, t) 33 | x = 2*obj.c(3) + 6*obj.c(4).*t + 12*obj.c(5).*t.^2; 34 | end 35 | 36 | function x = dddX(obj, t) 37 | x = 2 + 6*obj.c(4) + 24*obj.c(5).*t; 38 | end 39 | 40 | 41 | end 42 | end -------------------------------------------------------------------------------- /QuinticPoly.m: -------------------------------------------------------------------------------- 1 | classdef QuinticPoly 2 | % solve the quinticPoly 5th order equation 3 | 4 | properties 5 | % coefficients (c0, c1, c2, c3, c4, c5) 6 | c 7 | end 8 | 9 | methods (Access = public) 10 | 11 | function obj = QuinticPoly (xi0, dxi0, ddxi0, xiT, dxiT, ddxiT, T) 12 | 13 | c012= [xi0; dxi0; ddxi0 / 2.0]; 14 | 15 | M1=[1, T, T*T; 16 | 0, 1, 2*T; 17 | 0, 0, 2 ]; 18 | 19 | M2=[ T^3, T^4, T^5; 20 | 3*T^2, 4*T^3, 5*T^4; 21 | 6*T, 12*T^2, 20*T^3 ]; 22 | 23 | c345 = M2\([xiT; dxiT; ddxiT] - M1 * c012); 24 | obj.c = [c012; c345]; 25 | end 26 | 27 | function x=X(obj, t) 28 | x=obj.c(1) + obj.c(2).*t + obj.c(3).*t.^2 + obj.c(4).*t.^3 + obj.c(5).*t.^4 + obj.c(6).*t.^5; 29 | end 30 | 31 | function x=dX(obj, t) 32 | x=obj.c(2) + 2*obj.c(3).*t + 3*obj.c(4).*t.^2 + 4*obj.c(5).*t.^3 + 5* obj.c(6).*t.^4; 33 | end 34 | 35 | function x=ddX(obj, t) 36 | x= 2*obj.c(3) + 6*obj.c(4).*t + 12*obj.c(5).*t.^2 + 20* obj.c(6).*t.^3; 37 | end 38 | 39 | 40 | function x=dddX(obj, t) 41 | x= 2 + 6*obj.c(4) + 24*obj.c(5).*t + 60* obj.c(6).*t.^2; 42 | end 43 | 44 | 45 | end 46 | end 47 | 48 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Optimal Trajectory generation for Lane Changing in Frenet Frame 2 | 3 | Learn from paper WerlingOptimalTrajectoryGenerationforDynamicStreetScenariosinaFrenetFrame 4 | 5 | - Planning the lane changing trajectory by combine the lateral and longitudinal cost function in frenet frame 6 | - Simulated with the control algorithm to avoid collision from moving and static obstacles 7 | 8 | 1. Frenet坐标下动态街道场景的最优轨迹生成,这个是使用matlab完成 Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame论文里面的方法，运行这个方法只要 9 | 运行主函数main_path_planning.m即可，具体内容可见博客：https://blog.csdn.net/caokaifa/article/details/108015374 10 | 11 | 2.机器人局部避障的动态窗口法（DynamicWindowApproach）是对论文Mobile robot motion planning sample with Dynamic Window Approach的实现，运行DW_Main.m即可。 12 | 13 | 14 | 3.RS曲线的生成 15 | -------------------------------------------------------------------------------- /RS.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/caokaifa/Matlab-planning/08db498c02139997cd59d13df9c163710b8494f0/RS.m -------------------------------------------------------------------------------- /RSPath.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/caokaifa/Matlab-planning/08db498c02139997cd59d13df9c163710b8494f0/RSPath.m -------------------------------------------------------------------------------- /Spline.m: -------------------------------------------------------------------------------- 1 | classdef Spline 2 | 3 | properties(Access=public) 4 | a 5 | b 6 | c 7 | d 8 | w 9 | 10 | x 11 | y 12 | 13 | nx 14 | end 15 | methods(Access=public) 16 | 17 | function obj =Spline(x,y) 18 | obj.b=[]; 19 | obj.c=[]; 20 | obj.d=[]; 21 | obj.w=[]; 22 | 23 | obj.x=x; 24 | obj.y=y; 25 | 26 | obj.nx=length(x) %dimension of x 27 | h=diff(x); 28 | 29 | %% cal coefficient c 30 | obj.a=y; 31 | A=obj.calc_A(h); 32 | B=obj.calc_B(h); 33 | obj.c=A\B; 34 | obj.c=(obj.c)'; 35 | 36 | %% cal coefficient b and d 37 | for i = 1:(obj.nx-1) 38 | obj.d(end+1)=(obj.c(i+1)-obj.c(i)) / (3.0*h(i)); 39 | tb= (obj.a(i+1) - obj.a(i) ) / h(i)- h(i)* (obj.c(i+1)+2.0*obj.c(i))/ 3.0; 40 | obj.b(end+1)=tb; 41 | 42 | end 43 | end 44 | 45 | function result=calc(obj,t) 46 | %% calculate position 47 | %if t is outside of the input x, return None 48 | if (tobj.x(end)) 52 | result=NaN; 53 | return; 54 | end 55 | i = obj.research_index(t); 56 | dx=t-obj.x(i); 57 | result=obj.a(i) + obj.b(i)*dx + obj.c(i)*dx.^2.0 + obj.d(i)*dx.^3.0; 58 | end 59 | 60 | function result = calcd(obj, t) 61 | %% calculate frist derivative 62 | %if t is outside of the input x, return None 63 | if (tobj.x(end)) 67 | result=NaN; 68 | return; 69 | end 70 | i = obj.research_index(t); 71 | dx=t-obj.x(i); 72 | result=obj.b(i) +2.0* obj.c(i)*dx+3.0* obj.d(i)*dx.^2.0; 73 | end 74 | 75 | function result =calcdd(obj,t) 76 | %% calculate second derivative 77 | %if t is outside of the input x, return None 78 | if (tobj.x(end)) 82 | result=NaN; 83 | return; 84 | end 85 | i = obj.research_index(t); 86 | dx=t-obj.x(i); 87 | result=2.0* obj.c(i)+6.0* obj.d(i)*dx; 88 | end 89 | 90 | function idx = research_index(obj, x) 91 | %research date segment index 92 | idx=obj.bisect(obj.x, x, 1, length(obj.x))-1; 93 | end 94 | 95 | function lo = bisect(obj, list, x, lo, hi) 96 | %Return the index where to insert item x in list a, assuming a is sorted. 97 | % The return value i is such that all e in a[:i] have e <= x, and all e in 98 | % a[i:] have e > x. So if x already appears in the list, a.insert(x) will 99 | % insert just after the rightmost x already there. 100 | % Optional args lo (default 0) and hi (default len(a)) bound the 101 | % slice of a to be searched. 102 | if lo < 1 103 | error('lo must be positive integer'); 104 | end 105 | if isnan(hi) 106 | hi =length(list); 107 | end 108 | while lo < hi 109 | mid = floor((lo+hi)/2); 110 | if x < list(mid) 111 | hi=mid; 112 | else 113 | lo=mid+1; 114 | end 115 | end 116 | end 117 | 118 | function A = calc_A(obj, h) 119 | %% calculate matrix A for spline coefficient c 120 | A= zeros(obj.nx, obj.nx); 121 | A(1, 1)= 1.0; 122 | for i = 1:(obj.nx - 1) 123 | if i ~= (obj.nx - 1) 124 | A(i+1,i+1) = 2.0*(h(i) + h(i + 1)); 125 | end 126 | A(i + 1,i) = h(i); 127 | A(i, i + 1)= h(i); 128 | end 129 | A(1, 2)=0.0; 130 | A(obj.nx, obj.nx-1) = 0.0; 131 | A(obj.nx, obj.nx) = 1.0; 132 | end 133 | 134 | function B = calc_B(obj, h) 135 | %% calc matrix B for spline coefficient c 136 | B= zeros(obj.nx,1); 137 | for i = 1:(obj.nx-2) 138 | B(i+1)=3.0*(obj.a(i+2)-obj.a(i+1))/ ... 139 | h(i+1)-3.0*(obj.a(i+1)-obj.a(i))/h(i) ; 140 | end 141 | end 142 | 143 | end 144 | end -------------------------------------------------------------------------------- /Spline2D.m: -------------------------------------------------------------------------------- 1 | classdef Spline2D 2 | 3 | properties(Access=public) 4 | s 5 | sx 6 | sy 7 | ds 8 | end 9 | 10 | methods(Access=public) 11 | 12 | function obj=Spline2D(x, y) 13 | %initial the variable 14 | obj.s=obj.calc_s(x, y); 15 | obj.sx=Spline(obj.s, x); 16 | obj.sy=Spline(obj.s, y); 17 | end 18 | 19 | function s=calc_s(obj, x, y) 20 | %% calculate running length s 21 | dx=diff(x); 22 | dy=diff(y); 23 | obj.ds=zeros(length(dx),1); 24 | for i=1:length(dx) 25 | idx=dx(i); 26 | idy=dy(i); 27 | obj.ds(i)=sqrt(idx^2+idy^2); 28 | end 29 | s=[0, cumsum(obj.ds)']; 30 | end 31 | 32 | function [x, y]=calc_position(obj, s) 33 | %% calculate position 34 | x=obj.sx.calc(s); 35 | y=obj.sy.calc(s); 36 | end 37 | 38 | function k=calc_curvature(obj, s) 39 | %% calculate the curvature 40 | dx=obj.sx.calcd(s); 41 | ddx=obj.sx.calcdd(s); 42 | dy=obj.sy.calcd(s); 43 | ddy=obj.sy.calcdd(s); 44 | k=(ddy * dx - ddx * dy ) / (dx^2 + dy^2); 45 | end 46 | 47 | function yaw=calc_yaw(obj,s) 48 | %% calculate yaw angle 49 | dx=obj.sx.calcd(s); 50 | dy=obj.sy.calcd(s); 51 | yaw=atan2(dy,dx); 52 | end 53 | 54 | end 55 | end 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /WerlingOptimalTrajectoryGenerationforDynamicStreetScenariosinaFrenetFrame.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/caokaifa/Matlab-planning/08db498c02139997cd59d13df9c163710b8494f0/WerlingOptimalTrajectoryGenerationforDynamicStreetScenariosinaFrenetFrame.pdf -------------------------------------------------------------------------------- /cal_s.m: -------------------------------------------------------------------------------- 1 | function s=cal_s(x,y) 2 | 3 | dx=diff(x); 4 | dy=diff(y); 5 | ds=sqrt(dx*dx'+dy*dy'); 6 | s=[0, cusum(s)]; 7 | 8 | 9 | end -------------------------------------------------------------------------------- /calcSplineCourse.m: -------------------------------------------------------------------------------- 1 | function [rx,ry ,ryaw,rk,s,objSpline]=calcSplineCourse(x, y, ds) 2 | objSpline=Spline2D(x,y); 3 | s=0:ds:objSpline.s(end); 4 | 5 | rx=[]; 6 | ry=[]; 7 | ryaw=[]; 8 | rk=[]; 9 | for i_s = s 10 | [ix,iy]=objSpline.calc_position(i_s); 11 | rx(end+1)=ix; 12 | ry(end+1)=iy; 13 | ryaw(end+1)=objSpline.calc_yaw(i_s); 14 | rk(end+1)=objSpline.calc_curvature(i_s); 15 | end 16 | 17 | 18 | end -------------------------------------------------------------------------------- /complete goal.ps: -------------------------------------------------------------------------------- 1 | %!PS-Adobe-3.0 2 | %%Creator: (MATLAB, The Mathworks, Inc. Version 9.7.0.1190202 \(R2019b\). Operating System: Windows 10) 3 | %%Title: (./complete goal.ps) 4 | %%CreationDate: 2020-08-15T21:13:30 5 | %%Pages: (atend) 6 | %%BoundingBox: 123 283 470 556 7 | %%LanguageLevel: 3 8 | %%EndComments 9 | %%BeginDefaults 10 | %%EndDefaults 11 | %%BeginProlog 12 | %%EndProlog 13 | %%BeginSetup 14 | %%BeginResource: procset (Apache XML Graphics Std ProcSet) 1.2 0 15 | %%Version: 1.2 0 16 | %%Copyright: (Copyright 2001-2003,2010 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) 17 | /bd{bind def}bind def 18 | /ld{load def}bd 19 | /GR/grestore ld 20 | /GS/gsave ld 21 | /RM/rmoveto ld 22 | /C/curveto ld 23 | /t/show ld 24 | /L/lineto ld 25 | /ML/setmiterlimit ld 26 | /CT/concat ld 27 | /f/fill ld 28 | /N/newpath ld 29 | /S/stroke ld 30 | /CC/setcmykcolor ld 31 | /A/ashow ld 32 | /cp/closepath ld 33 | /RC/setrgbcolor ld 34 | /LJ/setlinejoin ld 35 | /GC/setgray ld 36 | /LW/setlinewidth ld 37 | /M/moveto ld 38 | /re {4 2 roll M 39 | 1 index 0 rlineto 40 | 0 exch rlineto 41 | neg 0 rlineto 42 | cp } bd 43 | /_ctm matrix def 44 | /_tm matrix def 45 | /BT { _ctm currentmatrix pop matrix _tm copy pop 0 0 moveto } bd 46 | /ET { _ctm setmatrix } bd 47 | /iTm { _ctm setmatrix _tm concat } bd 48 | /Tm { _tm astore pop iTm 0 0 moveto } bd 49 | /ux 0.0 def 50 | /uy 0.0 def 51 | /F { 52 | /Tp exch def 53 | /Tf exch def 54 | Tf findfont Tp scalefont setfont 55 | /cf Tf def /cs Tp def 56 | } bd 57 | /ULS {currentpoint /uy exch def /ux exch def} bd 58 | /ULE { 59 | /Tcx currentpoint pop def 60 | gsave 61 | newpath 62 | cf findfont cs scalefont dup 63 | /FontMatrix get 0 get /Ts exch def /FontInfo get dup 64 | /UnderlinePosition get Ts mul /To exch def 65 | /UnderlineThickness get Ts mul /Tt exch def 66 | ux uy To add moveto Tcx uy To add lineto 67 | Tt setlinewidth stroke 68 | grestore 69 | } bd 70 | /OLE { 71 | /Tcx currentpoint pop def 72 | gsave 73 | newpath 74 | cf findfont cs scalefont dup 75 | /FontMatrix get 0 get /Ts exch def /FontInfo get dup 76 | /UnderlinePosition get Ts mul /To exch def 77 | /UnderlineThickness get Ts mul /Tt exch def 78 | ux uy To add cs add moveto Tcx uy To add cs add lineto 79 | Tt setlinewidth stroke 80 | grestore 81 | } bd 82 | /SOE { 83 | /Tcx currentpoint pop def 84 | gsave 85 | newpath 86 | cf findfont cs scalefont dup 87 | /FontMatrix get 0 get /Ts exch def /FontInfo get dup 88 | /UnderlinePosition get Ts mul /To exch def 89 | /UnderlineThickness get Ts mul /Tt exch def 90 | ux uy To add cs 10 mul 26 idiv add moveto Tcx uy To add cs 10 mul 26 idiv add lineto 91 | Tt setlinewidth stroke 92 | grestore 93 | } bd 94 | /QT { 95 | /Y22 exch store 96 | /X22 exch store 97 | /Y21 exch store 98 | /X21 exch store 99 | currentpoint 100 | /Y21 load 2 mul add 3 div exch 101 | /X21 load 2 mul add 3 div exch 102 | /X21 load 2 mul /X22 load add 3 div 103 | /Y21 load 2 mul /Y22 load add 3 div 104 | /X22 load /Y22 load curveto 105 | } bd 106 | /SSPD { 107 | dup length /d exch dict def 108 | { 109 | /v exch def 110 | /k exch def 111 | currentpagedevice k known { 112 | /cpdv currentpagedevice k get def 113 | v cpdv ne { 114 | /upd false def 115 | /nullv v type /nulltype eq def 116 | /nullcpdv cpdv type /nulltype eq def 117 | nullv nullcpdv or 118 | { 119 | /upd true def 120 | } { 121 | /sametype v type cpdv type eq def 122 | sametype { 123 | v type /arraytype eq { 124 | /vlen v length def 125 | /cpdvlen cpdv length def 126 | vlen cpdvlen eq { 127 | 0 1 vlen 1 sub { 128 | /i exch def 129 | /obj v i get def 130 | /cpdobj cpdv i get def 131 | obj cpdobj ne { 132 | /upd true def 133 | exit 134 | } if 135 | } for 136 | } { 137 | /upd true def 138 | } ifelse 139 | } { 140 | v type /dicttype eq { 141 | v { 142 | /dv exch def 143 | /dk exch def 144 | /cpddv cpdv dk get def 145 | dv cpddv ne { 146 | /upd true def 147 | exit 148 | } if 149 | } forall 150 | } { 151 | /upd true def 152 | } ifelse 153 | } ifelse 154 | } if 155 | } ifelse 156 | upd true eq { 157 | d k v put 158 | } if 159 | } if 160 | } if 161 | } forall 162 | d length 0 gt { 163 | d setpagedevice 164 | } if 165 | } bd 166 | /RE { % /NewFontName [NewEncodingArray] /FontName RE - 167 | findfont dup length dict begin 168 | { 169 | 1 index /FID ne 170 | {def} {pop pop} ifelse 171 | } forall 172 | /Encoding exch def 173 | /FontName 1 index def 174 | currentdict definefont pop 175 | end 176 | } bind def 177 | %%EndResource 178 | %%BeginResource: procset (Apache XML Graphics EPS ProcSet) 1.0 0 179 | %%Version: 1.0 0 180 | %%Copyright: (Copyright 2002-2003 The Apache Software Foundation. License terms: http://www.apache.org/licenses/LICENSE-2.0) 181 | /BeginEPSF { %def 182 | /b4_Inc_state save def % Save state for cleanup 183 | /dict_count countdictstack def % Count objects on dict stack 184 | /op_count count 1 sub def % Count objects on operand stack 185 | userdict begin % Push userdict on dict stack 186 | /showpage { } def % Redefine showpage, { } = null proc 187 | 0 setgray 0 setlinecap % Prepare graphics state 188 | 1 setlinewidth 0 setlinejoin 189 | 10 setmiterlimit [ ] 0 setdash newpath 190 | /languagelevel where % If level not equal to 1 then 191 | {pop languagelevel % set strokeadjust and 192 | 1 ne % overprint to their defaults. 193 | {false setstrokeadjust false setoverprint 194 | } if 195 | } if 196 | } bd 197 | /EndEPSF { %def 198 | count op_count sub {pop} repeat % Clean up stacks 199 | countdictstack dict_count sub {end} repeat 200 | b4_Inc_state restore 201 | } bd 202 | %%EndResource 203 | %FOPBeginFontDict 204 | %%IncludeResource: font Courier-Oblique 205 | %%IncludeResource: font Courier-BoldOblique 206 | %%IncludeResource: font Courier-Bold 207 | %%IncludeResource: font ZapfDingbats 208 | %%IncludeResource: font Symbol 209 | %%IncludeResource: font Helvetica 210 | %%IncludeResource: font Helvetica-Oblique 211 | %%IncludeResource: font Helvetica-Bold 212 | %%IncludeResource: font Helvetica-BoldOblique 213 | %%IncludeResource: font Times-Roman 214 | %%IncludeResource: font Times-Italic 215 | %%IncludeResource: font Times-Bold 216 | %%IncludeResource: font Times-BoldItalic 217 | %%IncludeResource: font Courier 218 | %FOPEndFontDict 219 | %%BeginResource: encoding WinAnsiEncoding 220 | /WinAnsiEncoding [ 221 | /.notdef /.notdef /.notdef /.notdef /.notdef 222 | /.notdef /.notdef /.notdef /.notdef /.notdef 223 | /.notdef /.notdef /.notdef /.notdef /.notdef 224 | /.notdef /.notdef /.notdef /.notdef /.notdef 225 | /.notdef /.notdef /.notdef /.notdef /.notdef 226 | /.notdef /.notdef /.notdef /.notdef /.notdef 227 | /.notdef /.notdef /space /exclam /quotedbl 228 | /numbersign /dollar /percent /ampersand /quotesingle 229 | /parenleft /parenright /asterisk /plus /comma 230 | /hyphen /period /slash /zero /one 231 | /two /three /four /five /six 232 | /seven /eight /nine /colon /semicolon 233 | /less /equal /greater /question /at 234 | /A /B /C /D /E 235 | /F /G /H /I /J 236 | /K /L /M /N /O 237 | /P /Q /R /S /T 238 | /U /V /W /X /Y 239 | /Z /bracketleft /backslash /bracketright /asciicircum 240 | /underscore /quoteleft /a /b /c 241 | /d /e /f /g /h 242 | /i /j /k /l /m 243 | /n /o /p /q /r 244 | /s /t /u /v /w 245 | /x /y /z /braceleft /bar 246 | /braceright /asciitilde /bullet /Euro /bullet 247 | /quotesinglbase /florin /quotedblbase /ellipsis /dagger 248 | /daggerdbl /circumflex /perthousand /Scaron /guilsinglleft 249 | /OE /bullet /Zcaron /bullet /bullet 250 | /quoteleft /quoteright /quotedblleft /quotedblright /bullet 251 | /endash /emdash /asciitilde /trademark /scaron 252 | /guilsinglright /oe /bullet /zcaron /Ydieresis 253 | /space /exclamdown /cent /sterling /currency 254 | /yen /brokenbar /section /dieresis /copyright 255 | /ordfeminine /guillemotleft /logicalnot /sfthyphen /registered 256 | /macron /degree /plusminus /twosuperior /threesuperior 257 | /acute /mu /paragraph /middot /cedilla 258 | /onesuperior /ordmasculine /guillemotright /onequarter /onehalf 259 | /threequarters /questiondown /Agrave /Aacute /Acircumflex 260 | /Atilde /Adieresis /Aring /AE /Ccedilla 261 | /Egrave /Eacute /Ecircumflex /Edieresis /Igrave 262 | /Iacute /Icircumflex /Idieresis /Eth /Ntilde 263 | /Ograve /Oacute /Ocircumflex /Otilde /Odieresis 264 | /multiply /Oslash /Ugrave /Uacute /Ucircumflex 265 | /Udieresis /Yacute /Thorn /germandbls /agrave 266 | /aacute /acircumflex /atilde /adieresis /aring 267 | /ae /ccedilla /egrave /eacute /ecircumflex 268 | /edieresis /igrave /iacute /icircumflex /idieresis 269 | /eth /ntilde /ograve /oacute /ocircumflex 270 | /otilde /odieresis /divide /oslash /ugrave 271 | /uacute /ucircumflex /udieresis /yacute /thorn 272 | /ydieresis 273 | ] def 274 | %%EndResource 275 | %FOPBeginFontReencode 276 | /Courier-Oblique findfont 277 | dup length dict begin 278 | {1 index /FID ne {def} {pop pop} ifelse} forall 279 | /Encoding WinAnsiEncoding def 280 | currentdict 281 | end 282 | /Courier-Oblique exch definefont pop 283 | /Courier-BoldOblique findfont 284 | dup length dict begin 285 | {1 index /FID ne {def} {pop pop} ifelse} forall 286 | /Encoding WinAnsiEncoding def 287 | currentdict 288 | end 289 | /Courier-BoldOblique exch definefont pop 290 | /Courier-Bold findfont 291 | dup length dict begin 292 | {1 index /FID ne {def} {pop pop} ifelse} forall 293 | /Encoding WinAnsiEncoding def 294 | currentdict 295 | end 296 | /Courier-Bold exch definefont pop 297 | /Helvetica findfont 298 | dup length dict begin 299 | {1 index /FID ne {def} {pop pop} ifelse} forall 300 | /Encoding WinAnsiEncoding def 301 | currentdict 302 | end 303 | /Helvetica exch definefont pop 304 | /Helvetica-Oblique findfont 305 | dup length dict begin 306 | {1 index /FID ne {def} {pop pop} ifelse} forall 307 | /Encoding WinAnsiEncoding def 308 | currentdict 309 | end 310 | /Helvetica-Oblique exch definefont pop 311 | /Helvetica-Bold findfont 312 | dup length dict begin 313 | {1 index /FID ne {def} {pop pop} ifelse} forall 314 | /Encoding WinAnsiEncoding def 315 | currentdict 316 | end 317 | /Helvetica-Bold exch definefont pop 318 | /Helvetica-BoldOblique findfont 319 | dup length dict begin 320 | {1 index /FID ne {def} {pop pop} ifelse} forall 321 | /Encoding WinAnsiEncoding def 322 | currentdict 323 | end 324 | /Helvetica-BoldOblique exch definefont pop 325 | /Times-Roman findfont 326 | dup length dict begin 327 | {1 index /FID ne {def} {pop pop} ifelse} forall 328 | /Encoding WinAnsiEncoding def 329 | currentdict 330 | end 331 | /Times-Roman exch definefont pop 332 | /Times-Italic findfont 333 | dup length dict begin 334 | {1 index /FID ne {def} {pop pop} ifelse} forall 335 | /Encoding WinAnsiEncoding def 336 | currentdict 337 | end 338 | /Times-Italic exch definefont pop 339 | /Times-Bold findfont 340 | dup length dict begin 341 | {1 index /FID ne {def} {pop pop} ifelse} forall 342 | /Encoding WinAnsiEncoding def 343 | currentdict 344 | end 345 | /Times-Bold exch definefont pop 346 | /Times-BoldItalic findfont 347 | dup length dict begin 348 | {1 index /FID ne {def} {pop pop} ifelse} forall 349 | /Encoding WinAnsiEncoding def 350 | currentdict 351 | end 352 | /Times-BoldItalic exch definefont pop 353 | /Courier findfont 354 | dup length dict begin 355 | {1 index /FID ne {def} {pop pop} ifelse} forall 356 | /Encoding WinAnsiEncoding def 357 | currentdict 358 | end 359 | /Courier exch definefont pop 360 | %FOPEndFontReencode 361 | %%EndSetup 362 | %%Page: 1 1 363 | %%PageBoundingBox: 0 0 595 842 364 | %%BeginPageSetup 365 | << 366 | /PageSize [595 842] 367 | /ImagingBBox null 368 | >> setpagedevice 369 | [1 0 0 -1 0 842] CT 370 | %%EndPageSetup 371 | GS 372 | [0.5 0 0 0.5 87.63778 263.55511] CT 373 | 1 GC 374 | N 375 | 0 0 840 630 re 376 | f 377 | GR 378 | GS 379 | [0.5 0 0 0.5 87.63778 263.55511] CT 380 | 1 GC 381 | N 382 | 0 0 840 630 re 383 | f 384 | GR 385 | GS 386 | [0.5 0 0 0.5 87.63778 263.55511] CT 387 | 1 GC 388 | N 389 | 109 560.725 M 390 | 760 560.725 L 391 | 760 47.275 L 392 | 109 47.275 L 393 | cp 394 | f 395 | GR 396 | GS 397 | [0.5 0 0 0.5 87.63778 263.55511] CT 398 | 0.851 GC 399 | 1 LJ 400 | N 401 | 109 560.725 M 402 | 109 47.275 L 403 | S 404 | GR 405 | GS 406 | [0.5 0 0 0.5 87.63778 263.55511] CT 407 | 0.851 GC 408 | 1 LJ 409 | N 410 | 217.654 560.725 M 411 | 217.654 47.275 L 412 | S 413 | GR 414 | GS 415 | [0.5 0 0 0.5 87.63778 263.55511] CT 416 | 0.851 GC 417 | 1 LJ 418 | N 419 | 326.309 560.725 M 420 | 326.309 47.275 L 421 | S 422 | GR 423 | GS 424 | [0.5 0 0 0.5 87.63778 263.55511] CT 425 | 0.851 GC 426 | 1 LJ 427 | N 428 | 434.963 560.725 M 429 | 434.963 47.275 L 430 | S 431 | GR 432 | GS 433 | [0.5 0 0 0.5 87.63778 263.55511] CT 434 | 0.851 GC 435 | 1 LJ 436 | N 437 | 543.617 560.725 M 438 | 543.617 47.275 L 439 | S 440 | GR 441 | GS 442 | [0.5 0 0 0.5 87.63778 263.55511] CT 443 | 0.851 GC 444 | 1 LJ 445 | N 446 | 652.272 560.725 M 447 | 652.272 47.275 L 448 | S 449 | GR 450 | GS 451 | [0.5 0 0 0.5 87.63778 263.55511] CT 452 | 0.851 GC 453 | 1 LJ 454 | N 455 | 760 510.43 M 456 | 109 510.43 L 457 | S 458 | GR 459 | GS 460 | [0.5 0 0 0.5 87.63778 263.55511] CT 461 | 0.851 GC 462 | 1 LJ 463 | N 464 | 760 456.103 M 465 | 109 456.103 L 466 | S 467 | GR 468 | GS 469 | [0.5 0 0 0.5 87.63778 263.55511] CT 470 | 0.851 GC 471 | 1 LJ 472 | N 473 | 760 401.776 M 474 | 109 401.776 L 475 | S 476 | GR 477 | GS 478 | [0.5 0 0 0.5 87.63778 263.55511] CT 479 | 0.851 GC 480 | 1 LJ 481 | N 482 | 760 347.449 M 483 | 109 347.449 L 484 | S 485 | GR 486 | GS 487 | [0.5 0 0 0.5 87.63778 263.55511] CT 488 | 0.851 GC 489 | 1 LJ 490 | N 491 | 760 293.122 M 492 | 109 293.122 L 493 | S 494 | GR 495 | GS 496 | [0.5 0 0 0.5 87.63778 263.55511] CT 497 | 0.851 GC 498 | 1 LJ 499 | N 500 | 760 238.795 M 501 | 109 238.795 L 502 | S 503 | GR 504 | GS 505 | [0.5 0 0 0.5 87.63778 263.55511] CT 506 | 0.851 GC 507 | 1 LJ 508 | N 509 | 760 184.467 M 510 | 109 184.467 L 511 | S 512 | GR 513 | GS 514 | [0.5 0 0 0.5 87.63778 263.55511] CT 515 | 0.851 GC 516 | 1 LJ 517 | N 518 | 760 130.14 M 519 | 109 130.14 L 520 | S 521 | GR 522 | GS 523 | [0.5 0 0 0.5 87.63778 263.55511] CT 524 | 0.851 GC 525 | 1 LJ 526 | N 527 | 760 75.813 M 528 | 109 75.813 L 529 | S 530 | GR 531 | GS 532 | [0.5 0 0 0.5 87.63778 263.55511] CT 533 | 2 setlinecap 534 | 1 LJ 535 | N 536 | 109 560.725 M 537 | 760 560.725 L 538 | S 539 | GR 540 | GS 541 | [0.5 0 0 0.5 87.63778 263.55511] CT 542 | 2 setlinecap 543 | 1 LJ 544 | N 545 | 109 47.275 M 546 | 760 47.275 L 547 | S 548 | GR 549 | GS 550 | [0.5 0 0 0.5 87.63778 263.55511] CT 551 | 2 setlinecap 552 | 1 LJ 553 | N 554 | 109 560.725 M 555 | 109 554.215 L 556 | S 557 | GR 558 | GS 559 | [0.5 0 0 0.5 87.63778 263.55511] CT 560 | 2 setlinecap 561 | 1 LJ 562 | N 563 | 217.654 560.725 M 564 | 217.654 554.215 L 565 | S 566 | GR 567 | GS 568 | [0.5 0 0 0.5 87.63778 263.55511] CT 569 | 2 setlinecap 570 | 1 LJ 571 | N 572 | 326.309 560.725 M 573 | 326.309 554.215 L 574 | S 575 | GR 576 | GS 577 | [0.5 0 0 0.5 87.63778 263.55511] CT 578 | 2 setlinecap 579 | 1 LJ 580 | N 581 | 434.963 560.725 M 582 | 434.963 554.215 L 583 | S 584 | GR 585 | GS 586 | [0.5 0 0 0.5 87.63778 263.55511] CT 587 | 2 setlinecap 588 | 1 LJ 589 | N 590 | 543.617 560.725 M 591 | 543.617 554.215 L 592 | S 593 | GR 594 | GS 595 | [0.5 0 0 0.5 87.63778 263.55511] CT 596 | 2 setlinecap 597 | 1 LJ 598 | N 599 | 652.272 560.725 M 600 | 652.272 554.215 L 601 | S 602 | GR 603 | GS 604 | [0.5 0 0 0.5 87.63778 263.55511] CT 605 | 2 setlinecap 606 | 1 LJ 607 | N 608 | 109 47.275 M 609 | 109 53.785 L 610 | S 611 | GR 612 | GS 613 | [0.5 0 0 0.5 87.63778 263.55511] CT 614 | 2 setlinecap 615 | 1 LJ 616 | N 617 | 217.654 47.275 M 618 | 217.654 53.785 L 619 | S 620 | GR 621 | GS 622 | [0.5 0 0 0.5 87.63778 263.55511] CT 623 | 2 setlinecap 624 | 1 LJ 625 | N 626 | 326.309 47.275 M 627 | 326.309 53.785 L 628 | S 629 | GR 630 | GS 631 | [0.5 0 0 0.5 87.63778 263.55511] CT 632 | 2 setlinecap 633 | 1 LJ 634 | N 635 | 434.963 47.275 M 636 | 434.963 53.785 L 637 | S 638 | GR 639 | GS 640 | [0.5 0 0 0.5 87.63778 263.55511] CT 641 | 2 setlinecap 642 | 1 LJ 643 | N 644 | 543.617 47.275 M 645 | 543.617 53.785 L 646 | S 647 | GR 648 | GS 649 | [0.5 0 0 0.5 87.63778 263.55511] CT 650 | 2 setlinecap 651 | 1 LJ 652 | N 653 | 652.272 47.275 M 654 | 652.272 53.785 L 655 | S 656 | GR 657 | GS 658 | [0.5 0 0 0.5 142.13778 547.9176] CT 659 | /Helvetica 20 F 660 | GS 661 | [1 0 0 1 0 0] CT 662 | -6 21 moveto 663 | 1 -1 scale 664 | (0) t 665 | GR 666 | GR 667 | GS 668 | [0.5 0 0 0.5 196.46496 547.9176] CT 669 | /Helvetica 20 F 670 | GS 671 | [1 0 0 1 0 0] CT 672 | -11.5 21 moveto 673 | 1 -1 scale 674 | (10) t 675 | GR 676 | GR 677 | GS 678 | [0.5 0 0 0.5 250.79214 547.9176] CT 679 | /Helvetica 20 F 680 | GS 681 | [1 0 0 1 0 0] CT 682 | -11.5 21 moveto 683 | 1 -1 scale 684 | (20) t 685 | GR 686 | GR 687 | GS 688 | [0.5 0 0 0.5 305.11932 547.9176] CT 689 | /Helvetica 20 F 690 | GS 691 | [1 0 0 1 0 0] CT 692 | -11.5 21 moveto 693 | 1 -1 scale 694 | (30) t 695 | GR 696 | GR 697 | GS 698 | [0.5 0 0 0.5 359.4465 547.9176] CT 699 | /Helvetica 20 F 700 | GS 701 | [1 0 0 1 0 0] CT 702 | -11.5 21 moveto 703 | 1 -1 scale 704 | (40) t 705 | GR 706 | GR 707 | GS 708 | [0.5 0 0 0.5 413.77367 547.9176] CT 709 | /Helvetica 20 F 710 | GS 711 | [1 0 0 1 0 0] CT 712 | -11.5 21 moveto 713 | 1 -1 scale 714 | (50) t 715 | GR 716 | GR 717 | GS 718 | [0.5 0 0 0.5 87.63778 263.55511] CT 719 | 2 setlinecap 720 | 1 LJ 721 | N 722 | 109 560.725 M 723 | 109 47.275 L 724 | S 725 | GR 726 | GS 727 | [0.5 0 0 0.5 87.63778 263.55511] CT 728 | 2 setlinecap 729 | 1 LJ 730 | N 731 | 760 560.725 M 732 | 760 47.275 L 733 | S 734 | GR 735 | GS 736 | [0.5 0 0 0.5 87.63778 263.55511] CT 737 | 2 setlinecap 738 | 1 LJ 739 | N 740 | 109 510.43 M 741 | 115.51 510.43 L 742 | S 743 | GR 744 | GS 745 | [0.5 0 0 0.5 87.63778 263.55511] CT 746 | 2 setlinecap 747 | 1 LJ 748 | N 749 | 109 456.103 M 750 | 115.51 456.103 L 751 | S 752 | GR 753 | GS 754 | [0.5 0 0 0.5 87.63778 263.55511] CT 755 | 2 setlinecap 756 | 1 LJ 757 | N 758 | 109 401.776 M 759 | 115.51 401.776 L 760 | S 761 | GR 762 | GS 763 | [0.5 0 0 0.5 87.63778 263.55511] CT 764 | 2 setlinecap 765 | 1 LJ 766 | N 767 | 109 347.449 M 768 | 115.51 347.449 L 769 | S 770 | GR 771 | GS 772 | [0.5 0 0 0.5 87.63778 263.55511] CT 773 | 2 setlinecap 774 | 1 LJ 775 | N 776 | 109 293.122 M 777 | 115.51 293.122 L 778 | S 779 | GR 780 | GS 781 | [0.5 0 0 0.5 87.63778 263.55511] CT 782 | 2 setlinecap 783 | 1 LJ 784 | N 785 | 109 238.795 M 786 | 115.51 238.795 L 787 | S 788 | GR 789 | GS 790 | [0.5 0 0 0.5 87.63778 263.55511] CT 791 | 2 setlinecap 792 | 1 LJ 793 | N 794 | 109 184.467 M 795 | 115.51 184.467 L 796 | S 797 | GR 798 | GS 799 | [0.5 0 0 0.5 87.63778 263.55511] CT 800 | 2 setlinecap 801 | 1 LJ 802 | N 803 | 109 130.14 M 804 | 115.51 130.14 L 805 | S 806 | GR 807 | GS 808 | [0.5 0 0 0.5 87.63778 263.55511] CT 809 | 2 setlinecap 810 | 1 LJ 811 | N 812 | 109 75.813 M 813 | 115.51 75.813 L 814 | S 815 | GR 816 | GS 817 | [0.5 0 0 0.5 87.63778 263.55511] CT 818 | 2 setlinecap 819 | 1 LJ 820 | N 821 | 760 510.43 M 822 | 753.49 510.43 L 823 | S 824 | GR 825 | GS 826 | [0.5 0 0 0.5 87.63778 263.55511] CT 827 | 2 setlinecap 828 | 1 LJ 829 | N 830 | 760 456.103 M 831 | 753.49 456.103 L 832 | S 833 | GR 834 | GS 835 | [0.5 0 0 0.5 87.63778 263.55511] CT 836 | 2 setlinecap 837 | 1 LJ 838 | N 839 | 760 401.776 M 840 | 753.49 401.776 L 841 | S 842 | GR 843 | GS 844 | [0.5 0 0 0.5 87.63778 263.55511] CT 845 | 2 setlinecap 846 | 1 LJ 847 | N 848 | 760 347.449 M 849 | 753.49 347.449 L 850 | S 851 | GR 852 | GS 853 | [0.5 0 0 0.5 87.63778 263.55511] CT 854 | 2 setlinecap 855 | 1 LJ 856 | N 857 | 760 293.122 M 858 | 753.49 293.122 L 859 | S 860 | GR 861 | GS 862 | [0.5 0 0 0.5 87.63778 263.55511] CT 863 | 2 setlinecap 864 | 1 LJ 865 | N 866 | 760 238.795 M 867 | 753.49 238.795 L 868 | S 869 | GR 870 | GS 871 | [0.5 0 0 0.5 87.63778 263.55511] CT 872 | 2 setlinecap 873 | 1 LJ 874 | N 875 | 760 184.467 M 876 | 753.49 184.467 L 877 | S 878 | GR 879 | GS 880 | [0.5 0 0 0.5 87.63778 263.55511] CT 881 | 2 setlinecap 882 | 1 LJ 883 | N 884 | 760 130.14 M 885 | 753.49 130.14 L 886 | S 887 | GR 888 | GS 889 | [0.5 0 0 0.5 87.63778 263.55511] CT 890 | 2 setlinecap 891 | 1 LJ 892 | N 893 | 760 75.813 M 894 | 753.49 75.813 L 895 | S 896 | GR 897 | GS 898 | [0.5 0 0 0.5 138.13778 518.77034] CT 899 | /Helvetica 20 F 900 | GS 901 | [1 0 0 1 0 0] CT 902 | -29 8 moveto 903 | 1 -1 scale 904 | (-15) t 905 | GR 906 | GR 907 | GS 908 | [0.5 0 0 0.5 138.13778 491.60675] CT 909 | /Helvetica 20 F 910 | GS 911 | [1 0 0 1 0 0] CT 912 | -29 8 moveto 913 | 1 -1 scale 914 | (-10) t 915 | GR 916 | GR 917 | GS 918 | [0.5 0 0 0.5 138.13778 464.44316] CT 919 | /Helvetica 20 F 920 | GS 921 | [1 0 0 1 0 0] CT 922 | -18 8 moveto 923 | 1 -1 scale 924 | (-5) t 925 | GR 926 | GR 927 | GS 928 | [0.5 0 0 0.5 138.13778 437.27957] CT 929 | /Helvetica 20 F 930 | GS 931 | [1 0 0 1 0 0] CT 932 | -12 8 moveto 933 | 1 -1 scale 934 | (0) t 935 | GR 936 | GR 937 | GS 938 | [0.5 0 0 0.5 138.13778 410.11598] CT 939 | /Helvetica 20 F 940 | GS 941 | [1 0 0 1 0 0] CT 942 | -12 8 moveto 943 | 1 -1 scale 944 | (5) t 945 | GR 946 | GR 947 | GS 948 | [0.5 0 0 0.5 138.13778 382.95239] CT 949 | /Helvetica 20 F 950 | GS 951 | [1 0 0 1 0 0] CT 952 | -23 8 moveto 953 | 1 -1 scale 954 | (10) t 955 | GR 956 | GR 957 | GS 958 | [0.5 0 0 0.5 138.13778 355.7888] CT 959 | /Helvetica 20 F 960 | GS 961 | [1 0 0 1 0 0] CT 962 | -23 8 moveto 963 | 1 -1 scale 964 | (15) t 965 | GR 966 | GR 967 | GS 968 | [0.5 0 0 0.5 138.13778 328.62521] CT 969 | /Helvetica 20 F 970 | GS 971 | [1 0 0 1 0 0] CT 972 | -23 8 moveto 973 | 1 -1 scale 974 | (20) t 975 | GR 976 | GR 977 | GS 978 | [0.5 0 0 0.5 138.13778 301.46162] CT 979 | /Helvetica 20 F 980 | GS 981 | [1 0 0 1 0 0] CT 982 | -23 8 moveto 983 | 1 -1 scale 984 | (25) t 985 | GR 986 | GR 987 | GS 988 | [0.5 0 0 0.5 87.63778 263.55511] CT 989 | 1 LJ 990 | 2 LW 991 | N 992 | 109 347.449 M 993 | 110.011 348.067 L 994 | 111.022 348.685 L 995 | 112.033 349.302 L 996 | 113.044 349.919 L 997 | 114.055 350.536 L 998 | 115.067 351.152 L 999 | 116.078 351.768 L 1000 | 117.089 352.383 L 1001 | 118.1 352.996 L 1002 | 119.111 353.609 L 1003 | 120.122 354.221 L 1004 | 121.133 354.832 L 1005 | 122.144 355.441 L 1006 | 123.155 356.049 L 1007 | 124.166 356.655 L 1008 | 125.177 357.259 L 1009 | 126.188 357.862 L 1010 | 127.199 358.463 L 1011 | 128.21 359.062 L 1012 | 129.22 359.659 L 1013 | 130.231 360.253 L 1014 | 131.242 360.846 L 1015 | 132.253 361.436 L 1016 | 133.264 362.023 L 1017 | 134.275 362.607 L 1018 | 135.285 363.189 L 1019 | 136.296 363.768 L 1020 | 137.307 364.344 L 1021 | 138.317 364.917 L 1022 | 139.328 365.487 L 1023 | 140.338 366.053 L 1024 | 141.349 366.616 L 1025 | 142.359 367.175 L 1026 | 143.37 367.731 L 1027 | 144.38 368.283 L 1028 | 145.391 368.831 L 1029 | 146.401 369.375 L 1030 | 147.411 369.915 L 1031 | 148.421 370.45 L 1032 | 149.432 370.982 L 1033 | 150.442 371.509 L 1034 | 151.452 372.031 L 1035 | 152.462 372.549 L 1036 | 153.472 373.062 L 1037 | 154.482 373.57 L 1038 | 155.492 374.073 L 1039 | 156.501 374.571 L 1040 | 157.511 375.064 L 1041 | 158.521 375.551 L 1042 | 159.531 376.033 L 1043 | 160.54 376.51 L 1044 | 161.55 376.981 L 1045 | 162.559 377.446 L 1046 | 163.569 377.905 L 1047 | 164.578 378.358 L 1048 | 165.587 378.805 L 1049 | 166.597 379.246 L 1050 | 167.606 379.681 L 1051 | 168.615 380.109 L 1052 | 169.624 380.531 L 1053 | 170.633 380.946 L 1054 | 171.642 381.354 L 1055 | 172.65 381.755 L 1056 | 173.659 382.15 L 1057 | 174.668 382.537 L 1058 | 175.676 382.917 L 1059 | 176.685 383.29 L 1060 | 177.693 383.655 L 1061 | 178.702 384.013 L 1062 | 179.71 384.363 L 1063 | 180.718 384.705 L 1064 | 181.726 385.04 L 1065 | 182.734 385.366 L 1066 | 183.742 385.685 L 1067 | 184.75 385.995 L 1068 | 185.758 386.297 L 1069 | 186.765 386.59 L 1070 | 187.773 386.875 L 1071 | 188.781 387.151 L 1072 | 189.788 387.419 L 1073 | 190.795 387.677 L 1074 | 191.802 387.927 L 1075 | 192.81 388.168 L 1076 | 193.817 388.399 L 1077 | 194.823 388.621 L 1078 | 195.83 388.834 L 1079 | 196.837 389.037 L 1080 | 197.844 389.23 L 1081 | 198.85 389.414 L 1082 | 199.856 389.588 L 1083 | 200.863 389.752 L 1084 | 201.869 389.905 L 1085 | 202.875 390.049 L 1086 | 203.881 390.182 L 1087 | 204.887 390.305 L 1088 | 205.893 390.417 L 1089 | 206.898 390.519 L 1090 | 207.904 390.61 L 1091 | 208.909 390.69 L 1092 | 209.914 390.759 L 1093 | 210.92 390.817 L 1094 | 211.925 390.863 L 1095 | 212.93 390.899 L 1096 | 213.934 390.923 L 1097 | 214.939 390.935 L 1098 | 215.944 390.936 L 1099 | 216.948 390.925 L 1100 | 217.952 390.903 L 1101 | 218.957 390.868 L 1102 | 219.961 390.822 L 1103 | 220.964 390.763 L 1104 | 221.968 390.694 L 1105 | 222.972 390.613 L 1106 | 223.975 390.52 L 1107 | 224.979 390.416 L 1108 | 225.982 390.301 L 1109 | 226.984 390.176 L 1110 | 227.987 390.039 L 1111 | 228.99 389.891 L 1112 | 229.992 389.733 L 1113 | 230.994 389.564 L 1114 | 231.996 389.385 L 1115 | 232.998 389.195 L 1116 | 233.999 388.995 L 1117 | 235 388.785 L 1118 | 236.001 388.565 L 1119 | 237.002 388.336 L 1120 | 238.002 388.096 L 1121 | 239.002 387.847 L 1122 | 240.002 387.588 L 1123 | 241.002 387.319 L 1124 | 242.001 387.042 L 1125 | 243 386.755 L 1126 | 243.999 386.459 L 1127 | 244.997 386.154 L 1128 | 245.995 385.84 L 1129 | 246.993 385.518 L 1130 | 247.99 385.186 L 1131 | 248.987 384.846 L 1132 | 249.984 384.498 L 1133 | 250.98 384.142 L 1134 | 251.976 383.777 L 1135 | 252.972 383.404 L 1136 | 253.967 383.023 L 1137 | 254.962 382.634 L 1138 | 255.956 382.238 L 1139 | 256.95 381.833 L 1140 | 257.944 381.422 L 1141 | 258.937 381.003 L 1142 | 259.93 380.576 L 1143 | 260.922 380.142 L 1144 | 261.914 379.702 L 1145 | 262.906 379.254 L 1146 | 263.897 378.799 L 1147 | 264.888 378.338 L 1148 | 265.878 377.87 L 1149 | 266.867 377.395 L 1150 | 267.857 376.914 L 1151 | 268.845 376.427 L 1152 | 269.834 375.933 L 1153 | 270.821 375.434 L 1154 | 271.809 374.928 L 1155 | 272.795 374.417 L 1156 | 273.782 373.9 L 1157 | 274.767 373.377 L 1158 | 275.752 372.849 L 1159 | 276.737 372.315 L 1160 | 277.721 371.776 L 1161 | 278.705 371.232 L 1162 | 279.688 370.682 L 1163 | 280.67 370.128 L 1164 | 281.652 369.569 L 1165 | 282.633 369.005 L 1166 | 283.614 368.437 L 1167 | 284.594 367.864 L 1168 | 285.573 367.286 L 1169 | 286.552 366.704 L 1170 | 287.53 366.119 L 1171 | 288.508 365.529 L 1172 | 289.485 364.935 L 1173 | 290.461 364.337 L 1174 | 291.437 363.735 L 1175 | 292.412 363.13 L 1176 | 293.387 362.522 L 1177 | 294.36 361.91 L 1178 | 295.333 361.294 L 1179 | 296.306 360.676 L 1180 | 297.277 360.054 L 1181 | 298.248 359.43 L 1182 | 299.219 358.802 L 1183 | 300.188 358.172 L 1184 | 301.157 357.54 L 1185 | 302.125 356.904 L 1186 | 303.093 356.267 L 1187 | 304.059 355.627 L 1188 | 305.025 354.985 L 1189 | 305.991 354.341 L 1190 | 306.955 353.695 L 1191 | 307.919 353.047 L 1192 | 308.882 352.397 L 1193 | 309.844 351.746 L 1194 | 310.805 351.094 L 1195 | 311.765 350.44 L 1196 | 312.725 349.784 L 1197 | 313.684 349.128 L 1198 | 314.642 348.47 L 1199 | 315.6 347.812 L 1200 | 316.556 347.153 L 1201 | 317.512 346.493 L 1202 | 318.466 345.832 L 1203 | 319.42 345.171 L 1204 | 320.373 344.509 L 1205 | 321.326 343.848 L 1206 | 322.277 343.186 L 1207 | 323.228 342.524 L 1208 | 324.177 341.862 L 1209 | 325.126 341.201 L 1210 | 326.074 340.539 L 1211 | 327.021 339.878 L 1212 | 327.966 339.218 L 1213 | 328.912 338.558 L 1214 | 329.856 337.899 L 1215 | 330.799 337.241 L 1216 | 331.741 336.584 L 1217 | 332.682 335.928 L 1218 | 333.623 335.273 L 1219 | 334.562 334.619 L 1220 | 335.501 333.966 L 1221 | 336.439 333.314 L 1222 | 337.376 332.664 L 1223 | 338.312 332.015 L 1224 | 339.247 331.367 L 1225 | 340.182 330.721 L 1226 | 341.116 330.075 L 1227 | 342.049 329.431 L 1228 | 342.982 328.789 L 1229 | 343.913 328.148 L 1230 | 344.845 327.508 L 1231 | 345.775 326.87 L 1232 | 346.705 326.233 L 1233 | 347.635 325.598 L 1234 | 348.564 324.964 L 1235 | 349.493 324.332 L 1236 | 350.421 323.701 L 1237 | 351.348 323.072 L 1238 | 352.276 322.445 L 1239 | 353.202 321.819 L 1240 | 354.129 321.195 L 1241 | 355.055 320.573 L 1242 | 355.981 319.953 L 1243 | 356.906 319.334 L 1244 | 357.831 318.717 L 1245 | 358.756 318.102 L 1246 | 359.681 317.488 L 1247 | 360.606 316.877 L 1248 | 361.53 316.267 L 1249 | 362.454 315.66 L 1250 | 363.379 315.054 L 1251 | 364.303 314.45 L 1252 | 365.227 313.849 L 1253 | 366.151 313.249 L 1254 | 367.075 312.652 L 1255 | 367.999 312.056 L 1256 | 368.922 311.463 L 1257 | 369.847 310.871 L 1258 | 370.771 310.282 L 1259 | 371.695 309.695 L 1260 | 372.619 309.111 L 1261 | 373.544 308.528 L 1262 | 374.468 307.948 L 1263 | 375.393 307.37 L 1264 | 376.319 306.794 L 1265 | 377.244 306.221 L 1266 | 378.17 305.65 L 1267 | 379.096 305.082 L 1268 | 380.022 304.516 L 1269 | 380.949 303.952 L 1270 | 381.876 303.391 L 1271 | 382.803 302.833 L 1272 | 383.731 302.276 L 1273 | 384.66 301.723 L 1274 | 385.589 301.172 L 1275 | 386.518 300.624 L 1276 | 387.448 300.078 L 1277 | 388.378 299.535 L 1278 | 389.31 298.995 L 1279 | 390.241 298.457 L 1280 | 391.174 297.922 L 1281 | 392.107 297.39 L 1282 | 393.04 296.861 L 1283 | 393.975 296.334 L 1284 | 394.91 295.811 L 1285 | 395.846 295.29 L 1286 | 396.783 294.772 L 1287 | 397.72 294.257 L 1288 | 398.658 293.745 L 1289 | 399.598 293.236 L 1290 | 400.538 292.73 L 1291 | 401.479 292.227 L 1292 | 402.421 291.727 L 1293 | 403.364 291.23 L 1294 | 404.308 290.737 L 1295 | 405.253 290.246 L 1296 | 406.199 289.759 L 1297 | 407.146 289.274 L 1298 | 408.094 288.793 L 1299 | 409.043 288.315 L 1300 | 409.994 287.841 L 1301 | 410.945 287.37 L 1302 | 411.898 286.902 L 1303 | 412.852 286.437 L 1304 | 413.807 285.976 L 1305 | 414.764 285.518 L 1306 | 415.721 285.064 L 1307 | 416.681 284.613 L 1308 | 417.641 284.166 L 1309 | 418.603 283.722 L 1310 | 419.566 283.281 L 1311 | 420.531 282.845 L 1312 | 421.497 282.411 L 1313 | 422.464 281.982 L 1314 | 423.433 281.556 L 1315 | 424.404 281.134 L 1316 | 425.376 280.715 L 1317 | 426.35 280.3 L 1318 | 427.325 279.889 L 1319 | 428.302 279.482 L 1320 | 429.281 279.078 L 1321 | 430.261 278.678 L 1322 | 431.243 278.282 L 1323 | 432.226 277.89 L 1324 | 433.212 277.502 L 1325 | 434.199 277.118 L 1326 | 435.188 276.738 L 1327 | 436.179 276.361 L 1328 | 437.171 275.989 L 1329 | 438.166 275.62 L 1330 | 439.162 275.256 L 1331 | 440.16 274.895 L 1332 | 441.159 274.538 L 1333 | 442.16 274.184 L 1334 | 443.164 273.834 L 1335 | 444.168 273.488 L 1336 | 445.175 273.146 L 1337 | 446.183 272.806 L 1338 | 447.192 272.471 L 1339 | 448.204 272.138 L 1340 | 449.217 271.81 L 1341 | 450.231 271.484 L 1342 | 451.247 271.162 L 1343 | 452.265 270.843 L 1344 | 453.284 270.527 L 1345 | 454.305 270.215 L 1346 | 455.327 269.905 L 1347 | 456.351 269.599 L 1348 | 457.376 269.295 L 1349 | 458.403 268.995 L 1350 | 459.431 268.697 L 1351 | 460.46 268.403 L 1352 | 461.491 268.111 L 1353 | 462.524 267.822 L 1354 | 463.557 267.536 L 1355 | 464.593 267.252 L 1356 | 465.629 266.972 L 1357 | 466.667 266.693 L 1358 | 467.706 266.418 L 1359 | 468.747 266.145 L 1360 | 469.788 265.874 L 1361 | 470.832 265.606 L 1362 | 471.876 265.34 L 1363 | 472.921 265.077 L 1364 | 473.968 264.815 L 1365 | 475.016 264.557 L 1366 | 476.066 264.3 L 1367 | 477.116 264.046 L 1368 | 478.168 263.793 L 1369 | 479.22 263.543 L 1370 | 480.274 263.295 L 1371 | 481.329 263.049 L 1372 | 482.385 262.804 L 1373 | 483.442 262.562 L 1374 | 484.501 262.321 L 1375 | 485.56 262.083 L 1376 | 486.62 261.846 L 1377 | 487.682 261.611 L 1378 | 488.744 261.377 L 1379 | 489.808 261.145 L 1380 | 490.872 260.915 L 1381 | 491.937 260.686 L 1382 | 493.004 260.459 L 1383 | 494.071 260.233 L 1384 | 495.139 260.009 L 1385 | 496.208 259.786 L 1386 | 497.278 259.564 L 1387 | 498.349 259.344 L 1388 | 499.421 259.124 L 1389 | 500.493 258.906 L 1390 | 501.566 258.689 L 1391 | 502.641 258.474 L 1392 | 503.715 258.259 L 1393 | 504.791 258.045 L 1394 | 505.868 257.833 L 1395 | 506.945 257.621 L 1396 | 508.023 257.41 L 1397 | 509.101 257.2 L 1398 | 510.181 256.99 L 1399 | 511.261 256.782 L 1400 | 512.341 256.574 L 1401 | 513.423 256.367 L 1402 | 514.505 256.16 L 1403 | 515.587 255.954 L 1404 | 516.671 255.748 L 1405 | 517.754 255.543 L 1406 | 518.839 255.338 L 1407 | 519.924 255.134 L 1408 | 521.009 254.93 L 1409 | 522.095 254.727 L 1410 | 523.182 254.523 L 1411 | 524.269 254.32 L 1412 | 525.356 254.117 L 1413 | 526.444 253.914 L 1414 | 527.532 253.711 L 1415 | 528.621 253.508 L 1416 | 529.711 253.305 L 1417 | 530.8 253.102 L 1418 | 531.89 252.899 L 1419 | 532.981 252.696 L 1420 | 534.071 252.493 L 1421 | 535.162 252.289 L 1422 | 536.254 252.085 L 1423 | 537.345 251.881 L 1424 | 538.437 251.676 L 1425 | 539.53 251.471 L 1426 | 540.622 251.265 L 1427 | 541.715 251.059 L 1428 | 542.808 250.852 L 1429 | 543.901 250.645 L 1430 | 544.994 250.437 L 1431 | 546.088 250.229 L 1432 | 547.182 250.019 L 1433 | 548.276 249.809 L 1434 | 549.37 249.598 L 1435 | 550.464 249.387 L 1436 | 551.558 249.174 L 1437 | 552.652 248.961 L 1438 | 553.747 248.747 L 1439 | 554.841 248.533 L 1440 | 555.936 248.318 L 1441 | 557.03 248.103 L 1442 | 558.125 247.888 L 1443 | 559.219 247.673 L 1444 | 560.314 247.458 L 1445 | 561.409 247.243 L 1446 | 562.503 247.028 L 1447 | 563.598 246.813 L 1448 | 564.692 246.599 L 1449 | 565.787 246.385 L 1450 | 566.881 246.172 L 1451 | 567.975 245.959 L 1452 | 569.07 245.747 L 1453 | 570.164 245.536 L 1454 | 571.258 245.325 L 1455 | 572.351 245.116 L 1456 | 573.445 244.908 L 1457 | 574.539 244.701 L 1458 | 575.632 244.495 L 1459 | 576.725 244.29 L 1460 | 577.818 244.087 L 1461 | 578.911 243.886 L 1462 | 580.003 243.686 L 1463 | 581.096 243.488 L 1464 | 582.188 243.291 L 1465 | 583.279 243.097 L 1466 | 584.371 242.904 L 1467 | 585.462 242.714 L 1468 | 586.553 242.526 L 1469 | 587.643 242.34 L 1470 | 588.734 242.156 L 1471 | 589.824 241.975 L 1472 | 590.913 241.797 L 1473 | 592.002 241.621 L 1474 | 593.091 241.447 L 1475 | 594.18 241.277 L 1476 | 595.268 241.109 L 1477 | 596.355 240.945 L 1478 | 597.443 240.783 L 1479 | 598.529 240.625 L 1480 | 599.616 240.47 L 1481 | 600.701 240.318 L 1482 | 601.787 240.17 L 1483 | 602.872 240.025 L 1484 | 603.956 239.884 L 1485 | 605.04 239.747 L 1486 | 606.123 239.613 L 1487 | 607.206 239.483 L 1488 | 608.288 239.358 L 1489 | 609.37 239.236 L 1490 | 610.451 239.119 L 1491 | 611.532 239.006 L 1492 | 612.612 238.897 L 1493 | 613.691 238.792 L 1494 | 614.769 238.693 L 1495 | 615.847 238.597 L 1496 | 616.925 238.507 L 1497 | 618.002 238.421 L 1498 | 619.078 238.341 L 1499 | 620.153 238.265 L 1500 | 621.227 238.194 L 1501 | 622.301 238.129 L 1502 | 623.375 238.068 L 1503 | 624.447 238.014 L 1504 | 625.519 237.964 L 1505 | 626.59 237.92 L 1506 | 627.66 237.882 L 1507 | 628.729 237.85 L 1508 | 629.797 237.823 L 1509 | 630.865 237.802 L 1510 | 631.932 237.787 L 1511 | 632.998 237.779 L 1512 | 634.063 237.776 L 1513 | 635.128 237.78 L 1514 | 636.191 237.79 L 1515 | 637.254 237.806 L 1516 | 638.315 237.829 L 1517 | 639.376 237.859 L 1518 | 640.436 237.896 L 1519 | 641.494 237.939 L 1520 | 642.552 237.989 L 1521 | 643.609 238.046 L 1522 | 644.665 238.11 L 1523 | 645.72 238.182 L 1524 | 646.774 238.261 L 1525 | 647.827 238.347 L 1526 | 648.879 238.44 L 1527 | 649.929 238.541 L 1528 | 650.979 238.65 L 1529 | 652.028 238.766 L 1530 | 653.075 238.891 L 1531 | 654.122 239.023 L 1532 | 655.167 239.162 L 1533 | 656.212 239.31 L 1534 | 657.255 239.465 L 1535 | 658.297 239.627 L 1536 | 659.339 239.797 L 1537 | 660.379 239.974 L 1538 | 661.418 240.158 L 1539 | 662.456 240.35 L 1540 | 663.493 240.549 L 1541 | 664.53 240.755 L 1542 | 665.565 240.967 L 1543 | 666.599 241.187 L 1544 | 667.633 241.414 L 1545 | 668.665 241.647 L 1546 | 669.697 241.887 L 1547 | 670.727 242.134 L 1548 | 671.757 242.387 L 1549 | 672.786 242.646 L 1550 | 673.814 242.912 L 1551 | 674.841 243.185 L 1552 | 675.867 243.463 L 1553 | 676.892 243.748 L 1554 | 677.917 244.039 L 1555 | 678.94 244.336 L 1556 | 679.963 244.639 L 1557 | 680.985 244.948 L 1558 | 682.006 245.262 L 1559 | 683.027 245.583 L 1560 | 684.046 245.909 L 1561 | 685.065 246.24 L 1562 | 686.083 246.578 L 1563 | 687.1 246.92 L 1564 | 688.117 247.268 L 1565 | 689.133 247.622 L 1566 | 690.148 247.98 L 1567 | 691.162 248.344 L 1568 | 692.176 248.713 L 1569 | 693.189 249.086 L 1570 | 694.201 249.465 L 1571 | 695.212 249.849 L 1572 | 696.223 250.237 L 1573 | 697.234 250.63 L 1574 | 698.243 251.028 L 1575 | 699.252 251.431 L 1576 | 700.261 251.837 L 1577 | 701.268 252.249 L 1578 | 702.276 252.664 L 1579 | 703.282 253.084 L 1580 | 704.288 253.509 L 1581 | 705.293 253.937 L 1582 | 706.298 254.369 L 1583 | 707.303 254.806 L 1584 | 708.306 255.246 L 1585 | 709.31 255.69 L 1586 | 710.312 256.138 L 1587 | 711.314 256.59 L 1588 | 712.316 257.045 L 1589 | 713.317 257.504 L 1590 | 714.318 257.966 L 1591 | 715.318 258.432 L 1592 | 716.318 258.901 L 1593 | 717.317 259.374 L 1594 | 718.316 259.849 L 1595 | 719.314 260.328 L 1596 | 720.312 260.81 L 1597 | 721.31 261.294 L 1598 | 722.307 261.782 L 1599 | 723.304 262.272 L 1600 | 724.3 262.766 L 1601 | 725.296 263.262 L 1602 | 726.292 263.76 L 1603 | 727.287 264.261 L 1604 | 728.282 264.765 L 1605 | 729.277 265.271 L 1606 | 730.271 265.779 L 1607 | 731.265 266.289 L 1608 | 732.259 266.802 L 1609 | 733.252 267.317 L 1610 | 734.245 267.834 L 1611 | 735.238 268.353 L 1612 | 736.23 268.873 L 1613 | 737.223 269.396 L 1614 | 738.215 269.92 L 1615 | 739.207 270.446 L 1616 | 740.198 270.973 L 1617 | 741.19 271.503 L 1618 | 742.181 272.033 L 1619 | 743.172 272.565 L 1620 | 744.163 273.098 L 1621 | 745.154 273.632 L 1622 | 746.144 274.168 L 1623 | 747.134 274.705 L 1624 | 748.125 275.242 L 1625 | 749.115 275.781 L 1626 | 750.105 276.32 L 1627 | 751.095 276.861 L 1628 | 752.084 277.402 L 1629 | 753.074 277.943 L 1630 | 754.064 278.485 L 1631 | 755.053 279.028 L 1632 | 756.043 279.571 L 1633 | 757.032 280.115 L 1634 | 758.021 280.659 L 1635 | 759.011 281.203 L 1636 | 760 281.747 L 1637 | S 1638 | GR 1639 | GS 1640 | [0.5 0 0 0.5 250.79214 382.95239] CT 1641 | 10.0 ML 1642 | 2 LW 1643 | N 1644 | -4.243 -4.243 M 1645 | 4.243 4.243 L 1646 | -4.243 4.243 M 1647 | 4.243 -4.243 L 1648 | S 1649 | GR 1650 | GS 1651 | [0.5 0 0 0.5 305.11932 404.68326] CT 1652 | 10.0 ML 1653 | 2 LW 1654 | N 1655 | -4.243 -4.243 M 1656 | 4.243 4.243 L 1657 | -4.243 4.243 M 1658 | 4.243 -4.243 L 1659 | S 1660 | GR 1661 | GS 1662 | [0.5 0 0 0.5 305.11932 410.11598] CT 1663 | 10.0 ML 1664 | 2 LW 1665 | N 1666 | -4.243 -4.243 M 1667 | 4.243 4.243 L 1668 | -4.243 4.243 M 1669 | 4.243 -4.243 L 1670 | S 1671 | GR 1672 | GS 1673 | [0.5 0 0 0.5 332.28291 399.25055] CT 1674 | 10.0 ML 1675 | 2 LW 1676 | N 1677 | -4.243 -4.243 M 1678 | 4.243 4.243 L 1679 | -4.243 4.243 M 1680 | 4.243 -4.243 L 1681 | S 1682 | GR 1683 | GS 1684 | [0.5 0 0 0.5 413.77367 372.08696] CT 1685 | 10.0 ML 1686 | 2 LW 1687 | N 1688 | -4.243 -4.243 M 1689 | 4.243 4.243 L 1690 | -4.243 4.243 M 1691 | 4.243 -4.243 L 1692 | S 1693 | GR 1694 | GS 1695 | [0.5 0 0 0.5 87.63778 263.55511] CT 1696 | 1 LJ 1697 | 2 LW 1698 | N 1699 | 745.978 274.087 M 1700 | 756.156 279.634 L 1701 | S 1702 | GR 1703 | GS 1704 | [0.5 0 0 0.5 460.62685 400.59846] CT 1705 | N 1706 | 0 -5 M 1707 | 2.761 -5 5 -2.761 5 0 C 1708 | 5 0 L 1709 | 5 2.761 2.761 5 0 5 C 1710 | -2.761 5 -5 2.761 -5 0 C 1711 | -5 -2.761 -2.761 -5 0 -5 C 1712 | cp 1713 | 0 -7 M 1714 | -3.866 -7 -7 -3.866 -7 0 C 1715 | -7 3.866 -3.866 7 0 7 C 1716 | 3.866 7 7 3.866 7 0 C 1717 | 7 0 L 1718 | 7 -3.866 3.866 -7 0 -7 C 1719 | cp 1720 | f 1721 | GR 1722 | GS 1723 | [0.5 0 0 0.5 465.71569 403.37207] CT 1724 | N 1725 | /f-206068286{0 -5 M 1726 | 2.761 -5 5 -2.761 5 0 C 1727 | 5 0 L 1728 | 5 2.761 2.761 5 0 5 C 1729 | -2.761 5 -5 2.761 -5 0 C 1730 | -5 -2.761 -2.761 -5 0 -5 C 1731 | cp 1732 | 0 -7 M 1733 | -3.866 -7 -7 -3.866 -7 0 C 1734 | -7 3.866 -3.866 7 0 7 C 1735 | 3.866 7 7 3.866 7 0 C 1736 | 7 0 L 1737 | 7 -3.866 3.866 -7 0 -7 C 1738 | cp}def 1739 | f-206068286 1740 | f 1741 | GR 1742 | showpage 1743 | %%Trailer 1744 | %%Pages: 1 1745 | %%EOF 1746 | -------------------------------------------------------------------------------- /generate_target_course.m: -------------------------------------------------------------------------------- 1 | function [tx, ty, tyaw, tc, csp]=generate_target_course(x,y) 2 | 3 | [sp,obj2D]=Spline2D(x,y); 4 | 5 | 6 | 7 | 8 | 9 | end 10 | -------------------------------------------------------------------------------- /main_path_planning.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/caokaifa/Matlab-planning/08db498c02139997cd59d13df9c163710b8494f0/main_path_planning.m -------------------------------------------------------------------------------- /param.m: -------------------------------------------------------------------------------- 1 | classdef param 2 | 3 | properties(Access=public) 4 | max_speed = 1.0; %[m/s] 5 | min_speed = -0.5; % [m/s] 6 | max_yaw_rate = 40.0 * pi / 180.0; % [rad/s] 7 | max_accel = 0.2; % [m/ss] 8 | max_delta_yaw_rate = 40.0 * pi / 180.0; % [rad/ss] 9 | v_resolution = 0.01; % [m/s] 10 | yaw_rate_resolution = 0.1 * pi / 180.0; % [rad/s] 11 | dt = 0.1; % [s] Time tick for motion prediction 12 | predict_time = 3.0; % [s] 13 | to_goal_cost_gain = 0.15; 14 | speed_cost_gain = 1.0; 15 | obstacle_cost_gain = 1.0; 16 | robot_type = 0; 17 | % if robot_type == RobotType.circle result=0 18 | % Also used to check if goal is reached in both types 19 | robot_radius = 1.0; %[m] for collision check 20 | % if robot_type == RobotType.rectangle result=1 21 | robot_width = 0.5; % [m] for collision check 22 | robot_length = 1.2; % [m] for collision check 23 | 24 | end 25 | end -------------------------------------------------------------------------------- /test.avi: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/caokaifa/Matlab-planning/08db498c02139997cd59d13df9c163710b8494f0/test.avi --------------------------------------------------------------------------------