文章目录
import math
import time
import numpy as np
import matplotlib.pyplot as plt
def plot_arrow(x, y, yaw, length=5, width=1):
dx = length * math.cos(yaw)
dy = length * math.sin(yaw)
plt.arrow(x, y, dx, dy, head_length=width, head_width=width)
plt.plot([x, x + dx], [y, y + dy])
def plot_robot(x, y, yaw, robot_length, robot_width):
corner_x = x - robot_length / 2
corner_y = y - robot_width / 2
rectangle = plt.Rectangle((corner_x, corner_y), robot_length, robot_width, angle=math.degrees(yaw), fill=False)
plt.gca().add_patch(rectangle)
def main():
# x_trajectory = [0.0, 1.0, 2.0, 3.0, 4.0] # x坐标轨迹
x_trajectory = [
1,
1.1,
1.2,
1.3,
1.4,
1.5,
1.6,
1.7,
1.8,
1.9,
2,
2.1,
2.2,
2.3,
2.4,
2.5,
2.6,
2.7,
2.8,
2.9,
3,
3.1,
3.2,
3.3,
3.4,
3.5,
3.6,
3.7,
3.8,
3.9,
4,
4.1,
4.2,
4.3,
4.4,
4.5,
4.6,
4.7,
4.8,
4.9,
5,
5.1,
5.2,
5.3,
5.4,
5.5,
5.6,
5.7,
5.8,
5.9,
6,
6.1,
6.2,
6.3,
6.4,
6.5,
6.6,
6.7,
6.8,
6.9,
7,
7.1,
7.2,
7.3,
7.4,
7.5,
7.6,
7.7,
7.8,
7.9,
8,
8.1,
8.2,
8.3,
8.4,
8.5,
8.6,
8.7,
8.8,
8.9,
9,
9.1,
9.2,
9.3,
9.4,
9.5,
9.6,
9.7,
9.8,
9.9,
10,
10.1,
10.2,
10.3,
10.4,
10.5,
10.6,
10.7,
10.8,
10.9,
11,
11.1,
11.2,
11.3,
11.4,
11.5,
11.6,
11.7,
11.8,
11.9,
12,
12.1,
12.2,
12.3,
12.4,
12.5,
12.6,
12.7,
12.8,
12.9,
13,
13.1,
13.2,
13.3,
13.4,
13.5,
13.6,
13.7,
13.8,
13.9,
14,
14.1,
14.2,
14.4,
14.5,
14.6,
14.7,
14.8,
14.9,
15,
15.1,
15.2,
15.3,
15.4,
15.5,
15.6,
15.7,
15.8,
15.9,
16,
16.1,
16.2,
16.3,
16.4,
16.5,
16.6,
16.7,
16.9,
17,
17.1,
17.2,
17.3,
17.4,
17.5,
17.6,
17.7,
17.8,
17.9,
18,
18.1,
18.2,
18.3,
18.4,
18.5,
18.6,
18.7,
18.8,
18.9,
19,
19.1,
19.2,
19.3,
19.4,
19.5,
19.6,
19.7,
19.8,
19.9,
20,
20.1,
20.2,
20.3,
20.4,
20.5,
20.6,
20.7,
20.8,
20.9,
21,
21,
21.09999833,
21.19998667,
21.299955,
21.39989334,
21.49979169,
21.59964006,
21.69942847,
21.79914694,
21.89878549,
21.99833417,
22.09778301,
22.19712207,
22.29634143,
22.39543115,
22.49438132,
22.59318207,
22.69182349,
22.79029573,
22.88858895,
22.98669331,
23.084599,
23.18229623,
23.27977524,
23.37702626,
23.47403959,
23.57080552,
23.66731437,
23.76355649,
23.85952225,
23.95520207,
24.05058636,
24.14566561,
24.24043028,
24.33487092,
24.42897807,
24.52274233,
24.61615432,
24.70920469,
24.80188415,
24.89418342,
24.98609328,
25.07760453,
25.16870802,
25.25939465,
25.34965534,
25.43948107,
25.52886285,
25.61779176,
25.70625888,
25.79425539,
25.88177247,
25.96880138,
26.05533341,
26.14135992,
26.22687229,
26.31186198,
26.39632049,
26.48023937,
26.56361023,
26.64642473,
26.7286746,
26.81035161,
26.89144758,
26.97195441,
27.05186406,
27.13116852,
27.20985987,
27.28793024,
27.36537182,
27.44217687,
27.51833771,
27.59384672,
27.66869635,
27.74287912,
27.8163876,
27.88921445,
27.96135239,
28.03279419,
28.10353272,
28.17356091,
28.24287174,
28.3114583,
28.37931371,
28.4464312,
28.51280405,
28.57842563,
28.64328937,
28.70738879,
28.77071748,
28.8332691,
28.8950374,
28.9560162,
29.01619941,
29.075581,
29.13415505,
29.19191568,
29.24885713,
29.3049737,
29.36025979,
29.41470985,
29.46831845,
29.52108022,
29.57298989,
29.62404227,
29.67423226,
29.72355482,
29.77200504,
29.81957807,
29.86626914,
29.9120736,
29.95698686,
30.00100442,
30.04412189,
30.08633496,
30.1276394,
30.16803109,
30.20750598,
30.24606012,
30.28368967,
30.32039086,
30.35616002,
30.39099356,
30.42488802,
30.45783999,
30.48984619,
30.52090342,
30.55100856,
30.5801586,
30.60835064,
30.63558185,
30.66184952,
30.687151,
30.71148378,
30.73484542,
30.75723358,
30.77864602,
30.79908061,
30.8185353,
30.83700815,
30.8544973,
30.87100101,
30.88651763,
30.9010456,
30.91458348,
30.92712991,
30.93868363,
30.9492435,
30.95880845,
30.96737752,
30.97494987,
30.98152472,
30.98710144,
30.99167945,
30.99525831,
30.99783764,
30.9994172,
30.99999683,
30.9995,
30.99800007,
30.99550034,
30.99200107,
30.9875026,
30.9820054,
30.97551,
30.96801706,
30.95952733,
30.95004165,
30.93956098,
30.92808636,
30.91561894,
30.90215996,
30.88771078,
30.87227283,
30.85584767,
30.83843693,
30.82004235,
30.80066578,
30.78030915,
30.75897449,
30.73666395,
30.71337975,
30.68912422,
30.66389978,
30.63770896,
30.61055438,
30.58243876,
30.55336489,
30.5233357,
30.49235418,
30.46042344,
30.42754666,
30.39372713,
30.35896824,
30.32327346,
30.28664636,
30.2490906,
30.21060994,
30.17120823,
30.1308894,
30.0896575,
30.04751663,
30.00447102,
29.96052498,
29.91568288,
29.86994923,
29.82332859,
29.77582562,
29.72744508,
29.6781918,
29.62807071,
29.57708681,
29.52524522,
29.47255111,
29.41900975,
29.3646265,
29.30940679,
29.25335615,
29.19648018,
29.13878457,
29.08027508,
29.02095758,
28.96083799,
28.89992231,
28.83821666,
28.77572719,
28.71246015,
28.64842187,
28.58361876,
28.51805729,
28.45174402,
28.38468559,
28.31688869,
28.24836011,
28.1791067,
28.10913538,
28.03845316,
27.96706709,
27.89498433,
27.82221207,
27.7487576,
27.67462826,
27.59983146,
27.52437468,
27.44826547,
27.37151144,
27.29412027,
27.21609968,
27.13745749,
27.05820157,
26.97833982,
26.89788025,
26.81683089,
26.73519986,
26.65299531,
26.57022547,
26.48689861,
26.40302306,
26.31860721,
26.23365951,
26.14818845,
26.06220257,
25.97571048,
25.88872082,
25.80124229,
25.71328364,
25.62485367,
25.53596121,
25.44661517,
25.35682446,
25.26659808,
25.17594504,
25.08487441,
24.99339529,
24.90151684,
24.80924824,
24.71659872,
24.62357754,
24.53019401,
24.43645746,
24.34237727,
24.24796284,
24.15322362,
24.05816908,
23.96280873,
23.8671521,
23.77120875,
23.67498829,
23.57850033,
23.48175452,
23.38476053,
23.28752808,
23.19006687,
23.09238666,
22.99449721,
22.89640831,
22.79812978,
22.69967143,
22.60104312,
22.5022547,
22.40331606,
22.30423709,
22.20502769,
22.1056978,
22.00625733,
21.90671624,
21.80708448,
21.70737202,
21.60758881,
21.50774485,
21.40785011,
21.30791459,
21.20794828,
21.10796117,
21.00796327,
21,
20.9,
20.8,
20.7,
20.6,
20.5,
20.4,
20.3,
20.2,
20.1,
20,
19.9,
19.8,
19.7,
19.6,
19.5,
19.4,
19.3,
19.2,
19.1,
19,
18.9,
18.8,
18.7,
18.6,
18.5,
18.4,
18.3,
18.2,
18.1,
18,
17.9,
17.8,
17.7,
17.6,
17.5,
17.4,
17.3,
17.2,
17.1,
17,
16.9,
16.8,
16.7,
16.6,
16.5,
16.4,
16.3,
16.2,
16.1,
16,
15.9,
15.8,
15.7,
15.6,
15.5,
15.4,
15.3,
15.2,
15.1,
15,
14.9,
14.8,
14.7,
14.6,
14.5,
14.4,
14.3,
14.2,
14.1,
14,
13.9,
13.8,
13.7,
13.6,
13.5,
13.4,
13.3,
13.2,
13.1,
13,
12.9,
12.8,
12.7,
12.6,
12.5,
12.4,
12.3,
12.2,
12.1,
12,
11.9,
11.8,
11.7,
11.6,
11.5,
11.4,
11.3,
11.2,
11.1,
11,
10.9,
10.8,
10.7,
10.6,
10.5,
10.4,
10.3,
10.2,
10.1,
10,
9.9,
9.8,
9.7,
9.6,
9.5,
9.4,
9.3,
9.2,
9.1,
9,
8.9,
8.8,
8.7,
8.6,
8.5,
8.4,
8.3,
8.2,
8.1,
8,
7.9,
7.8,
7.7,
7.6,
7.5,
7.4,
7.3,
7.2,
7.1,
7,
6.9,
6.8,
6.7,
6.6,
6.5,
6.4,
6.3,
6.2,
6.1,
6,
5.9,
5.8,
5.7,
5.6,
5.5,
5.4,
5.3,
5.2,
5.1,
5,
4.9,
4.8,
4.7,
4.6,
4.5,
4.4,
4.3,
4.2,
4.1,
4,
3.9,
3.8,
3.7,
3.6,
3.5,
3.4,
3.3,
3.2,
3.1,
3,
2.9,
2.8,
2.7,
2.6,
2.5,
2.4,
2.3,
2.2,
2.1,
2,
1.9,
1.8,
1.7,
1.6,
1.5,
1.4,
1.3,
1.2,
1.1,
1,
1,
0.900001667,
0.800013333,
0.700044998,
0.600106658,
0.500208307,
0.400359935,
0.300571527,
0.20085306,
0.101214508,
0.001665834,
- 0.097783008,
- 0.197122073,
- 0.296341426,
- 0.395431146,
- 0.494381325
- 0.593182066,
- 0.691823491,
- 0.790295734,
- 0.88858895,
- 0.986693308,
- 1.084598998,
- 1.182296231,
- 1.279775235,
- 1.377026264,
- 1.474039593,
- 1.570805519,
- 1.667314367,
- 1.763556486,
- 1.859522251,
- 1.955202067,
- 2.050586364,
- 2.145665606,
- 2.240430284,
- 2.334870921,
- 2.428978075,
- 2.522742333,
- 2.61615432,
- 2.709204694,
- 2.801884151,
- 2.894183423,
- 2.98609328,
- 3.077604531,
- 3.168708024,
- 3.259394651,
- 3.349655341,
- 3.43948107,
- 3.528862854,
- 3.617791755,
- 3.706258882,
- 3.794255386,
- 3.881772469,
- 3.968801378,
- 4.055333412,
- 4.141359917,
- 4.226872289,
- 4.311861979,
- 4.396320487,
- 4.480239368,
- 4.563610229,
- 4.646424734,
- 4.728674601,
- 4.810351605,
- 4.891447579,
- 4.971954414,
- 5.051864057,
- 5.13116852,
- 5.20985987,
- 5.28793024,
- 5.365371822,
- 5.442176872,
- 5.51833771,
- 5.59384672,
- 5.66869635,
- 5.742879116,
- 5.8163876,
- 5.889214451,
- 5.961352386,
- 6.032794192,
- 6.103532724,
- 6.173560909,
- 6.242871744,
- 6.311458297,
- 6.379313711,
- 6.4464312,
- 6.512804051,
- 6.578425629,
- 6.64328937,
- 6.707388789,
- 6.770717475,
- 6.833269096,
- 6.895037397,
- 6.9560162,
- 7.016199409,
- 7.075581004,
- 7.134155048,
- 7.191915683,
- 7.248857133,
- 7.304973705,
- 7.360259786,
- 7.414709848,
- 7.468318446,
- 7.521080219,
- 7.572989892,
- 7.624042272,
- 7.674232256,
- 7.723554823,
- 7.772005043,
- 7.819578069,
- 7.866269144,
- 7.912073601,
- 7.956986857,
- 8.001004422,
- 8.044121894,
- 8.086334961,
- 8.127639403,
- 8.168031088,
- 8.207505977,
- 8.246060124,
- 8.283689672,
- 8.32039086,
- 8.356160016,
- 8.390993563,
- 8.424888019,
- 8.457839994,
- 8.489846194,
- 8.520903416,
- 8.551008556,
- 8.580158603,
- 8.608350642,
- 8.635581854,
- 8.661849516,
- 8.687151001,
- 8.711483779,
- 8.734845417,
- 8.757233578,
- 8.778646024,
- 8.799080614,
- 8.818535304,
- 8.837008148,
- 8.8544973,
- 8.87100101,
- 8.886517629,
- 8.901045603,
- 8.914583482,
- 8.92712991,
- 8.938683634,
- 8.949243498,
- 8.958808445,
- 8.96737752,
- 8.974949866,
- 8.981524725,
- 8.98710144,
- 8.991679453,
- 8.995258306,
- 8.997837642,
- 8.999417202,
- 8.999996829,
- 8.999576465,
- 8.998156151,
- 8.99573603,
- 8.992316344,
- 8.987897435,
- 8.982479744,
- 8.976063813,
- 8.968650285,
- 8.960239899,
- 8.950833498,
- 8.940432022,
- 8.929036511,
- 8.916648105,
- 8.903268042,
- 8.88889766,
- 8.873538397,
- 8.857191788,
- 8.839859469,
- 8.821543171,
- 8.802244728,
- 8.781966068,
- 8.76070922,
- 8.738476309,
- 8.715269558,
- 8.691091289,
- 8.665943918,
- 8.639829962,
- 8.61275203,
- 8.584712831,
- 8.555715169,
- 8.525761943,
- 8.494856149,
- 8.463000877,
- 8.430199313,
- 8.396454737,
- 8.361770523,
- 8.32615014,
- 8.28959715,
- 8.252115208,
- 8.213708062,
- 8.174379553,
- 8.134133613,
- 8.092974268,
- 8.050905633,
- 8.007931915,
- 7.964057412,
- 7.91928651,
- 7.873623686,
- 7.827073508,
- 7.77964063,
- 7.731329795,
- 7.682145834,
- 7.632093666,
- 7.581178296,
- 7.529404816,
- 7.476778401,
- 7.423304316,
- 7.368987908,
- 7.313834608,
- 7.257849931,
- 7.201039476,
- 7.143408924,
- 7.084964038,
- 7.025710662,
- 6.965654722,
- 6.904802223,
- 6.843159251,
- 6.780731969,
- 6.71752662,
- 6.653549525,
- 6.588807082,
- 6.523305764,
- 6.457052122,
- 6.390052781,
- 6.32231444,
- 6.253843875,
- 6.184647931,
- 6.114733528,
- 6.044107658,
- 5.972777383
- 5.900749836,
- 5.828032219,
- 5.754631806,
- 5.680555934,
- 5.605812013,
- 5.530407516,
- 5.454349983,
- 5.377647021,
- 5.3003063,
- 5.222335553,
- 5.143742578,
- 5.064535234,
- 4.984721441,
- 4.904309181,
- 4.823306495,
- 4.741721484,
- 4.659562304,
- 4.576837174,
- 4.493554364,
- 4.409722204,
- 4.325349076,
- 4.240443417,
- 4.155013718,
- 4.069068522,
- 3.982616424,
- 3.895666068,
- 3.80822615,
- 3.720305413,
- 3.631912649,
- 3.543056698,
- 3.453746445,
- 3.363990822,
- 3.273798802,
- 3.183179407,
- 3.092141697,
- 3.000694776,
- 2.908847789,
- 2.816609921,
- 2.723990394,
- 2.630998472,
- 2.537643453,
- 2.443934673,
- 2.349881502,
- 2.255493345,
- 2.160779642,
- 2.065749864,
- 1.970413513,
- 1.874780123,
- 1.778859258,
- 1.682660509,
- 1.586193497,
- 1.489467867,
- 1.392493292,
- 1.29527947,
- 1.197836122,
- 1.100172993,
- 1.002299847,
- 0.904226474,
- 0.805962679,
- 0.70751829,
- 0.60890315,
- 0.510127121,
- 0.411200081,
- 0.312131922,
- 0.21293255,
- 0.113611887,
- 0.014179863,
0.085353578,
0.184978482,
0.284684889,
0.384462826,
0.484302316,
0.584193376,
0.684126016,
0.784090243,
0.884076061,
0.984073471,
1,
1.1,
1.2,
1.3,
1.4,
1.5,
1.6,
1.7,
1.8,
1.9,
2,
2.1,
2.2,
2.3,
2.4,
2.5,
2.6,
2.7,
2.8,
2.9,
3,
3.1,
3.2,
3.3,
3.4,
3.5,
3.6,
3.7,
3.8,
3.9,
4,
4.1,
4.2,
4.3,
4.4,
4.5,
4.6,
4.7,
4.8,
4.9,
5,
5.1,
5.2,
5.3,
5.4,
5.5,
5.6,
5.7,
5.8,
5.9,
6,
6.1,
6.2,
6.3,
6.4,
6.5,
6.6,
6.7,
6.8,
6.9,
7,
7.1,
7.2,
7.3,
7.4,
7.5,
7.6,
7.7,
7.8,
7.9,
8,
8.1,
8.2,
8.3,
8.4,
8.5,
8.6,
8.7,
8.8,
8.9,
9,
9.1,
9.2,
9.3,
9.4,
9.5,
9.6,
9.7,
9.8,
9.9,
10,
10.1,
10.2,
10.3,
10.4,
10.5,
10.6,
10.7,
10.8,
10.9,
11,
11.1,
11.2,
11.3,
11.4,
11.5,
11.6,
11.7,
11.8,
11.9,
12,
12.1,
12.2,
12.3,
12.4,
12.5,
12.6,
12.7,
12.8,
12.9,
13,
13.1,
13.2,
13.3,
13.4,
13.5,
13.6,
13.7,
13.8,
13.9,
14,
14.1,
14.2,
14.3,
14.4,
14.5,
14.6,
14.7,
14.8,
14.9,
15,
15.1,
15.2,
15.3,
15.4,
15.5,
15.6,
15.7,
15.8,
15.9,
16,
16.1,
16.2,
16.3,
16.4,
16.5,
16.6,
16.7,
16.8,
16.9,
17,
17.1,
17.2,
17.3,
17.4,
17.5,
17.6,
17.7,
17.8,
17.9,
18,
18.1,
18.2,
18.3,
18.4,
18.5,
18.6,
18.7,
18.8,
18.9,
19,
19.1,
19.2,
19.3,
19.4,
19.5,
19.6,
19.7,
19.8,
19.9,
20,
20.1,
20.2,
20.3,
20.4,
20.5,
20.6,
20.7,
20.8,
20.9,
20.9,
20.9,
21
]
# y_trajectory = [0.0, 1.0, 2.0, 3.0, 4.0] # y坐标轨迹
y_trajectory = [
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1,
1.000499996,
1.001999933,
1.004499663,
1.007998933,
1.012497396,
1.017994601,
1.024489997,
1.031982937,
1.04047267,
1.049958347,
1.06043902,
1.071913641,
1.084381063,
1.097840038,
1.112289221,
1.127727166,
1.144152331,
1.161563072,
1.179957649,
1.199334222,
1.219690853,
1.241025507,
1.26333605,
1.286620251,
1.310875783,
1.336100219,
1.362291036,
1.389445617,
1.417561245,
1.446635109,
1.476664301,
1.507645819,
1.539576565,
1.572453345,
1.606272872,
1.641031763,
1.676726544,
1.713353644,
1.750909401,
1.78939006,
1.828791772,
1.869110597,
1.910342503,
1.952483368,
1.995528976,
2.039475025,
2.084317118,
2.130050772,
2.176671414,
2.224174381,
2.272554924,
2.321808203,
2.371929295,
2.422913186,
2.474754779,
2.52744889,
2.580990248,
2.635373501,
2.690593209,
2.746643851,
2.803519822,
2.861215433,
2.919724917,
2.979042421,
3.039162015,
3.100077685,
3.161783341,
3.224272812,
3.28753985,
3.351578127,
3.41638124,
3.481942709,
3.548255977,
3.615314413,
3.683111311,
3.751639893,
3.820893304,
3.89086462,
3.961546843,
4.032932907,
4.10501567,
4.177787927,
4.251242399,
4.325371742,
4.400168541,
4.475625318,
4.551734528,
4.628488558,
4.705879734,
4.783900317,
4.862542505,
4.941798434,
5.021660177,
5.10211975,
5.183169105,
5.264800139,
5.347004688,
5.429774532,
5.513101394,
5.596976941,
5.681392786,
5.766340487,
5.85181155,
5.937797428,
6.024289521,
6.111279181,
6.19875771,
6.286716358,
6.375146331,
6.464038786,
6.553384833,
6.643175537,
6.733401921,
6.82405496,
6.915125591,
7.006604706,
7.098483157,
7.190751756,
7.283401277,
7.376422455,
7.469805988,
7.563542537,
7.657622729,
7.752037156,
7.846776376,
7.941830916,
8.037191271,
8.132847904,
8.228791249,
8.325011714,
8.421499675,
8.518245483,
8.615239466,
8.712471922,
8.809933129,
8.907613341,
9.00550279,
9.103591687,
9.201870223,
9.300328571,
9.398956884,
9.497745301,
9.596683942,
9.695762913,
9.794972306,
9.894302202,
9.993742666,
10.09328376,
10.19291552,
10.29262798,
10.39241119,
10.49225515,
10.59214989,
10.69208541,
10.79205172,
10.89203883,
10.99203673,
11.09999833,
11.19998667,
11.299955,
11.39989334,
11.49979169,
11.59964006,
11.69942847,
11.79914694,
11.89878549,
11.99833417,
12.09778301,
12.19712207,
12.29634143,
12.39543115,
12.49438132,
12.59318207,
12.69182349,
12.79029573,
12.88858895,
12.98669331,
13.084599,
13.18229623,
13.27977524,
13.37702626,
13.47403959,
13.57080552,
13.66731437,
13.76355649,
13.85952225,
13.95520207,
14.05058636,
14.14566561,
14.24043028,
14.33487092,
14.42897807,
14.52274233,
14.61615432,
14.70920469,
14.80188415,
14.89418342,
14.98609328,
15.07760453,
15.16870802,
15.25939465,
15.34965534,
15.43948107,
15.52886285,
15.61779176,
15.70625888,
15.79425539,
15.88177247,
15.96880138,
16.05533341,
16.14135992,
16.22687229,
16.31186198,
16.39632049,
16.48023937,
16.56361023,
16.64642473,
16.7286746,
16.81035161,
16.89144758,
16.97195441,
17.05186406,
17.13116852,
17.20985987,
17.28793024,
17.36537182,
17.44217687,
17.51833771,
17.59384672,
17.66869635,
17.74287912,
17.8163876,
17.88921445,
17.96135239,
18.03279419,
18.10353272,
18.17356091,
18.24287174,
18.3114583,
18.37931371,
18.4464312,
18.51280405,
18.57842563,
18.64328937,
18.70738879,
18.77071748,
18.8332691,
18.8950374,
18.9560162,
19.01619941,
19.075581,
19.13415505,
19.19191568,
19.24885713,
19.3049737,
19.36025979,
19.41470985,
19.46831845,
19.52108022,
19.57298989,
19.62404227,
19.67423226,
19.72355482,
19.77200504,
19.81957807,
19.86626914,
19.9120736,
19.95698686,
20.00100442,
20.04412189,
20.08633496,
20.1276394,
20.16803109,
20.20750598,
20.24606012,
20.28368967,
20.32039086,
20.35616002,
20.39099356,
20.42488802,
20.45783999,
20.48984619,
20.52090342,
20.55100856,
20.5801586,
20.60835064,
20.63558185,
20.66184952,
20.687151,
20.71148378,
20.73484542,
20.75723358,
20.77864602,
20.79908061,
20.8185353,
20.83700815,
20.8544973,
20.87100101,
20.88651763,
20.9010456,
20.91458348,
20.92712991,
20.93868363,
20.9492435,
20.95880845,
20.96737752,
20.97494987,
20.98152472,
20.98710144,
20.99167945,
20.99525831,
20.99783764,
20.9994172,
20.99999683,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21,
21.0005,
21.00199993,
21.00449966,
21.00799893,
21.0124974,
21.0179946,
21.02449,
21.03198294,
21.04047267,
21.04995835,
21.06043902,
21.07191364,
21.08438106,
21.09784004,
21.11228922,
21.12772717,
21.14415233,
21.16156307,
21.17995765,
21.19933422,
21.21969085,
21.24102551,
21.26333605,
21.28662025,
21.31087578,
21.33610022,
21.36229104,
21.38944562,
21.41756124,
21.44663511,
21.4766643,
21.50764582,
21.53957656,
21.57245334,
21.60627287,
21.64103176,
21.67672654,
21.71335364,
21.7509094,
21.78939006,
21.82879177,
21.8691106,
21.9103425,
21.95248337,
21.99552898,
22.03947502,
22.08431712,
22.13005077,
22.17667141,
22.22417438,
22.27255492,
22.3218082,
22.37192929,
22.42291319,
22.47475478,
22.52744889,
22.58099025,
22.6353735,
22.69059321,
22.74664385,
22.80351982,
22.86121543,
22.91972492,
22.97904242,
23.03916201,
23.10007769,
23.16178334,
23.22427281,
23.28753985,
23.35157813,
23.41638124,
23.48194271,
23.54825598,
23.61531441,
23.68311131,
23.75163989,
23.8208933,
23.89086462,
23.96154684,
24.03293291,
24.10501567,
24.17778793,
24.2512424,
24.32537174,
24.40016854,
24.47562532,
24.55173453,
24.62848856,
24.70587973,
24.78390032,
24.86254251,
24.94179843,
25.02166018,
25.10211975,
25.18316911,
25.26480014,
25.34700469,
25.42977453,
25.51310139,
25.59697694,
25.68139279,
25.76634049,
25.85181155,
25.93779743,
26.02428952,
26.11127918,
26.19875771,
26.28671636,
26.37514633,
26.46403879,
26.55338483,
26.64317554,
26.73340192,
26.82405496,
26.91512559,
27.00660471,
27.09848316,
27.19075176,
27.28340128,
27.37642246,
27.46980599,
27.56354254,
27.65762273,
27.75203716,
27.84677638,
27.94183092,
28.03719127,
28.1328479,
28.22879125,
28.32501171,
28.42149967,
28.51824548,
28.61523947,
28.71247192,
28.80993313,
28.90761334,
29.00550279,
29.10359169,
29.20187022,
29.30032857,
29.39895688,
29.4977453,
29.59668394,
29.69576291,
29.79497231,
29.8943022,
29.99374267,
30.09328376,
30.19291552,
30.29262798,
30.49225515,
30.59214989,
30.69208541,
30.79205172,
30.89203883,
30.99203673,
31.09203543,
31.19202493,
31.29199522,
31.39193632,
31.49183822,
31.59169094,
31.69148449,
31.79120889,
31.89085417,
31.99041037,
32.08986752,
32.18921569,
32.28844494,
32.38754535,
32.486507,
32.58532001,
32.68397448,
32.78246056,
32.88076839,
32.97888815,
33.07681002,
33.17452421,
33.27202095,
33.36929049,
33.4663231,
33.56310908,
33.65963876,
33.75590247,
33.85189059,
33.94759353,
34.04300171,
34.13810559,
34.23289567,
34.32736246,
34.42149651,
34.51528842,
34.6087288,
34.70180831,
34.79451765,
34.88684753,
34.97878874,
35.07033207,
35.16146837,
35.25218852,
35.34248346,
35.43234416,
35.52176162,
35.61072691,
35.69923114,
35.78726544,
35.87482102,
35.96188913,
36.04846105,
36.13452812,
36.22008175,
36.30511338,
36.38961449,
36.47357665,
36.55699146,
36.63985058,
36.72214571,
36.80386863,
36.88501117,
36.96556522,
37.04552271,
37.12487566,
37.20361612,
37.28173623,
37.35922817,
37.43608419,
37.51229661,
37.5878578,
37.66276021,
37.73699636,
37.81055881,
37.8834402,
37.95563326,
38.02713077,
38.09792556,
38.16801057,
38.23737879,
38.30602327,
38.37393716,
38.44111365,
38.50754605,
38.57322769,
38.63815202,
38.70231254,
38.76570284,
38.82831657,
38.89014747,
38.95118937,
39.01143616,
39.0708818,
39.12952037,
39.18734599,
39.24435289,
39.30053535,
39.35588777,
39.41040461,
39.46408041,
39.51690981,
39.56888753,
39.62000837,
39.67026721,
39.71965904,
39.7681789,
39.81582196,
39.86258344,
39.90845867,
39.95344306,
39.99753211,
40.04072142,
40.08300666,
40.12438361,
40.16484813,
40.20439618,
40.24302379,
40.2807271,
40.31750235,
40.35334586,
40.38825404,
40.42222341,
40.45525056,
40.48733219,
40.5184651,
40.54864616,
40.57787238,
40.60614081,
40.63344863,
40.65979312,
40.68517164,
40.70958165,
40.73302071,
40.75548648,
40.7769767,
40.79748924,
40.81702203,
40.83557313,
40.85314068,
40.86972293,
40.88531821,
40.89992497,
40.91354174,
40.92616717,
40.93779999,
40.94843903,
40.95808325,
40.96673166,
40.9743834,
40.98103772,
40.98669394,
40.9913515,
40.99500994,
40.99766888,
40.99932806,
40.99998732,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41,
41]
robot_length = 4 # 小车的长度
robot_width = 2 # 小车的宽度
fig, ax = plt.subplots() # 创建一个图形和坐标轴对象
for i in range(len(x_trajectory) - 1):
x = x_trajectory[i]
y = y_trajectory[i]
yaw = math.atan2(y_trajectory[i+1] - y_trajectory[i], x_trajectory[i+1] - x_trajectory[i]) # 计算小车的朝向角度
plt.cla()
plot_robot(x+0.2, y-0.2, yaw, robot_length, robot_width) # 绘制小车
plt.plot(x_trajectory, y_trajectory, "-r") # 绘制轨迹
plt.plot(x, y, "bo") # 绘制小车位置点
plot_arrow(x, y, yaw) # 绘制箭头
plt.axis("equal")
plt.grid(True)
plt.pause(0.001) # 暂停一段时间，使动画更流畅
plt.show()
main()
3593
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
