MATLAB制作GIF动图,将图片保存,并制作

 1. 画图matlab

clc;
clear;
close all;

% URE
X_car_0 = [-1.0148,-1.0151,-1.0154,-1.0156,-1.0159,-1.0163,-1.0166,-1.0136,-1.0104,-1,-0.98,-0.96,-0.94,-0.92,-0.9,-0.88,-0.86,-0.84,-0.83074,-0.82636,-0.823,-0.81976,-0.81652,-0.8107,-0.80439,-0.79809,-0.78807,-0.77719,-0.76533,-0.75001,-0.73469,-0.72039,-0.70636,-0.69305,-0.68068,-0.66804,-0.65,-0.63,-0.61,-0.59,-0.57,-0.55,-0.53,-0.51,-0.49,-0.47,-0.45,-0.43,-0.41,-0.39,-0.37,-0.35,-0.33,-0.31,-0.3,-0.27,-0.24,-0.21,-0.18,-0.15,-0.12,-0.09,-0.06,-0.03,0,0.03,0.06,0.09,0.12,0.15,0.18,0.21,0.24,0.27,0.3,0.33,0.36,0.39,0.42,0.45,0.48,0.51,0.54,0.57,0.6,0.63,0.66,0.69,0.72,0.75,0.78,0.81,0.84,0.87,0.9,0.93,0.96,0.99,1.02,1.05,1.08,1.11,1.14,1.17,1.2,1.23,1.26,1.29,1.32,1.35,1.38,1.41,1.44,1.47,1.5,1.53,1.56,1.59,1.62,1.65,1.68,1.71,1.74,1.77,1.8,1.83,1.86,1.89,1.92,1.95,1.98,2.01,2.04,2.07,2.1,2.13,2.16,2.19,2.22,2.25,2.28,2.31,2.34,2.37,2.4,2.43,2.46,2.49,2.52,2.55,2.58,2.61,2.64,2.67,2.7,2.73,2.76,2.79,2.82,2.85,2.88,2.91,2.94,2.97,3,3,3.02,3.04,3.06,3.08,3.1,3.12,3.14,3.16,3.18,3.2,3.22,3.24,3.26,3.28,3.3,3.32,3.34,3.36,3.38,3.4,3.42,3.44,3.46,3.48,3.4938,3.5103,3.5267,3.5348,3.5393,3.5438,3.5483,3.5528,3.5573,3.5611,3.5645,3.5679,3.5712,3.5731,3.575,3.5767,3.5776,3.5785,3.5794,3.5803,3.5809,3.5815,3.5821,3.5827,3.5831,3.5835,3.5839,3.5843,3.5843,3.5839,3.5835,3.5831,3.5827,3.5821,3.5815,3.5809,3.5803,3.5794,3.5785,3.5776,3.5767,3.575,3.5731,3.5712,3.5679,3.5645,3.5611,3.5573,3.5528,3.5483,3.5438,3.5393,3.5348,3.5267,3.5103,3.4938,3.48,3.46,3.44,3.42,3.4,3.38,3.36,3.34,3.32,3.3,3.28,3.26,3.24,3.22,3.2,3.18,3.16,3.14,3.12,3.1,3.08,3.06,3.04,3.02,3,3,2.97,2.94,2.91,2.88,2.85,2.82,2.79,2.76,2.73,2.7,2.67,2.64,2.61,2.58,2.55,2.52,2.49,2.46,2.43,2.4,2.37,2.34,2.31,2.28,2.25,2.22,2.19,2.16,2.13,2.1,2.07,2.04,2.01,1.98,1.95,1.92,1.89,1.86,1.83,1.8,1.77,1.74,1.71,1.68,1.65,1.62,1.59,1.56,1.53,1.5,1.47,1.44,1.41,1.38,1.35,1.32,1.29,1.26,1.23,1.2,1.17,1.14,1.11,1.08,1.05,1.02,0.99,0.96,0.93,0.9,0.87,0.84,0.81,0.78,0.75,0.72,0.69,0.66,0.63,0.6,0.57,0.54,0.51,0.48,0.45,0.42,0.39,0.36,0.33,0.3,0.27,0.24,0.21,0.18,0.15,0.12,0.09,0.06,0.03,0,-0.03,-0.06,-0.09,-0.12,-0.15,-0.18,-0.21,-0.24,-0.27,-0.3,-0.31,-0.33,-0.35,-0.37,-0.39,-0.41,-0.43,-0.45,-0.47,-0.49,-0.51,-0.53,-0.55,-0.57,-0.59,-0.61,-0.63,-0.65,-0.66804,-0.68068,-0.69305,-0.70636,-0.72039,-0.73469,-0.75001,-0.76533,-0.77719,-0.78807,-0.79809,-0.80439,-0.8107,-0.81652,-0.81976,-0.823,-0.82636,-0.83074,-0.84,-0.86,-0.88,-0.9,-0.92,-0.94,-0.96,-0.98,-1,-1.0104,-1.0136,-1.0166,-1.0163,-1.0159,-1.0156,-1.0154,-1.0151,-1.0148];
Y_car_0 = [-0.01,-0.04,-0.07,-0.1,-0.13,-0.16,-0.19,-0.22,-0.25,-0.27948,-0.30309,-0.31945,-0.32854,-0.33763,-0.34567,-0.34697,-0.34826,-0.34956,-0.355,-0.38,-0.405,-0.43,-0.455,-0.48,-0.505,-0.53,-0.555,-0.58,-0.605,-0.63,-0.655,-0.68,-0.705,-0.73,-0.755,-0.78,-0.79938,-0.82087,-0.84235,-0.8522,-0.85975,-0.8673,-0.87556,-0.8845,-0.89343,-0.90237,-0.91349,-0.92579,-0.93809,-0.94088,-0.94343,-0.94598,-0.94852,-0.95001,-0.95014,-0.95051,-0.95088,-0.95126,-0.95174,-0.9523,-0.95286,-0.95342,-0.95397,-0.95453,-0.95509,-0.95565,-0.95614,-0.95662,-0.9571,-0.95757,-0.95805,-0.95853,-0.959,-0.95948,-0.95763,-0.95569,-0.95375,-0.9518,-0.94986,-0.94791,-0.9461,-0.9461,-0.9461,-0.9461,-0.9461,-0.9461,-0.9461,-0.94574,-0.94292,-0.9401,-0.93728,-0.93446,-0.93164,-0.92881,-0.92599,-0.92445,-0.9235,-0.92255,-0.9216,-0.92065,-0.9197,-0.91875,-0.9178,-0.91847,-0.91933,-0.9202,-0.92106,-0.92193,-0.92279,-0.92328,-0.92343,-0.92358,-0.92373,-0.92388,-0.92406,-0.92433,-0.92459,-0.92486,-0.92513,-0.9254,-0.92589,-0.92694,-0.928,-0.92906,-0.93012,-0.93117,-0.93339,-0.9364,-0.93941,-0.94241,-0.94542,-0.94843,-0.9495,-0.9495,-0.9495,-0.9495,-0.9495,-0.94957,-0.94998,-0.95039,-0.95081,-0.95122,-0.95163,-0.95208,-0.95263,-0.95318,-0.95372,-0.95427,-0.95406,-0.9538,-0.95354,-0.95328,-0.9531,-0.9531,-0.9531,-0.9531,-0.95303,-0.95228,-0.95153,-0.95078,-0.94996,-0.94856,-0.94716,-0.9461,-0.94515,-0.94515,-0.94452,-0.94325,-0.94067,-0.93809,-0.9364,-0.93471,-0.93213,-0.92926,-0.91597,-0.89905,-0.89015,-0.88124,-0.87233,-0.86342,-0.85444,-0.84541,-0.83637,-0.81929,-0.801,-0.7831,-0.76609,-0.74908,-0.72638,-0.69598,-0.675,-0.65,-0.625,-0.6,-0.575,-0.55,-0.525,-0.5,-0.475,-0.45,-0.425,-0.4,-0.375,-0.35,-0.325,-0.3,-0.275,-0.25,-0.225,-0.2,-0.175,-0.15,-0.125,-0.1,-0.075,-0.05,-0.025,0,-0,0.025,0.05,0.075,0.1,0.125,0.15,0.175,0.2,0.225,0.25,0.275,0.3,0.325,0.35,0.375,0.4,0.425,0.45,0.475,0.5,0.525,0.55,0.575,0.6,0.625,0.65,0.675,0.69598,0.72638,0.74908,0.76609,0.7831,0.801,0.81929,0.83637,0.84541,0.85444,0.86342,0.87233,0.88124,0.89015,0.89905,0.91597,0.92926,0.93213,0.93471,0.9364,0.93809,0.94067,0.94325,0.94452,0.94515,0.94515,0.9461,0.94716,0.94856,0.94996,0.95078,0.95153,0.95228,0.95303,0.9531,0.9531,0.9531,0.9531,0.95328,0.95354,0.9538,0.95406,0.95427,0.95372,0.95318,0.95263,0.95208,0.95163,0.95122,0.95081,0.95039,0.94998,0.94957,0.9495,0.9495,0.9495,0.9495,0.9495,0.94843,0.94542,0.94241,0.93941,0.9364,0.93339,0.93117,0.93012,0.92906,0.928,0.92694,0.92589,0.9254,0.92513,0.92486,0.92459,0.92433,0.92406,0.92388,0.92373,0.92358,0.92343,0.92328,0.92279,0.92193,0.92106,0.9202,0.91933,0.91847,0.9178,0.91875,0.9197,0.92065,0.9216,0.92255,0.9235,0.92445,0.92599,0.92881,0.93164,0.93446,0.93728,0.9401,0.94292,0.94574,0.9461,0.9461,0.9461,0.9461,0.9461,0.9461,0.9461,0.94791,0.94986,0.9518,0.95375,0.95569,0.95763,0.95948,0.959,0.95853,0.95805,0.95757,0.9571,0.95662,0.95614,0.95565,0.95509,0.95453,0.95397,0.95342,0.95286,0.9523,0.95174,0.95126,0.95088,0.95051,0.95014,0.95001,0.94852,0.94598,0.94343,0.94088,0.93809,0.92579,0.91349,0.90237,0.89343,0.8845,0.87556,0.8673,0.85975,0.8522,0.84235,0.82087,0.79938,0.78,0.755,0.73,0.705,0.68,0.655,0.63,0.605,0.58,0.555,0.53,0.505,0.48,0.455,0.43,0.405,0.38,0.355,0.34956,0.34826,0.34697,0.34567,0.33763,0.32854,0.31945,0.30309,0.27948,0.25,0.22,0.19,0.16,0.13,0.1,0.07,0.04,0.01];
X_car_1 = [ -1.2147,-1.2149,-1.2152,-1.2154,-1.2157,-1.2135,-1.2113,-1.2086,-1.2056,-1.2028,-1.1999,-1.1971,-1.1942,-1.1914,-1.1885,-1.1726,-1.1626,-1.1496,-1.1367,-1.1237,-1.1107,-1.0977,-1.0848,-1.0651,-1.0512,-1.0345,-1.0177,-1.0079,-1.0011,-0.9929,-0.98342,-0.97265,-0.96061,-0.94732,-0.93713,-0.92119,-0.90573,-0.89075,-0.8765,-0.86396,-0.84973,-0.83385,-0.81639,-0.79742,-0.77703,-0.75528,-0.73227,-0.71726,-0.70225,-0.68724,-0.67223,-0.64402,-0.6161,-0.58848,-0.56119,-0.57835,-0.5955,-0.57227,-0.54763,-0.52172,-0.49471,-0.46677,-0.43805,-0.40875,-0.37903,-0.34909,-0.32055,-0.29055,-0.26055,-0.23055,-0.20055,-0.17055,-0.14055,-0.11055,-0.080551,-0.050551,-0.020551,0.0094492,0.039449,0.069449,0.099449,0.12945,0.15945,0.18945,0.21945,0.24945,0.27945,0.30944,0.33944,0.36944,0.39944,0.42944,0.45944,0.48943,0.51943,0.54943,0.56558,0.58173,0.61171,0.6417,0.67169,0.70168,0.73167,0.76166,0.79165,0.82165,0.85164,0.88163,0.91163,0.94162,0.97162,1.0016,1.0316,1.0616,1.0916,1.1216,1.1516,1.1816,1.2116,1.2416,1.2716,1.3016,1.3316,1.3616,1.3916,1.4216,1.4516,1.4816,1.5116,1.5416,1.5716,1.6016,1.6316,1.6616,1.6699,1.6998,1.7298,1.7597,1.7897,1.8197,1.8496,1.8796,1.9096,1.9396,1.9696,1.9995,2.0295,2.0595,2.0895,2.1195,2.1495,2.1795,2.2095,2.2395,2.2695,2.2995,2.3295,2.3463,2.3763,2.4063,2.4363,2.4663,2.4963,2.5263,2.5563,2.5863,2.6163,2.6463,2.6763,2.7063,2.7363,2.7663,2.7963,2.8263,2.8437,2.8737,2.9037,2.9337,2.9637,2.9937,3.0236,3.0535,3.0834,3.1132,3.143,3.126,3.156,3.1857,3.2149,3.243,3.2952,3.3185,3.2709,3.3063,3.3337,3.3611,3.3885,3.4158,3.4319,3.4479,3.464,3.4801,3.502,3.5243,3.547,3.5699,3.5819,3.5939,3.6144,3.6337,3.6518,3.6687,3.6842,3.6983,3.711,3.7223,3.7322,3.7405,3.7473,3.7526,3.7564,3.7585,3.7592,3.7651,3.7687,3.7717,3.7741,3.7764,3.7776,3.7787,3.7797,3.7806,3.7814,3.7821,3.7826,3.7836,3.7841,3.7843,3.7843,3.7841,3.7836,3.7826,3.7821,3.7814,3.7806,3.7797,3.7787,3.7776,3.7764,3.7741,3.7717,3.7687,3.7651,3.7592,3.7585,3.7564,3.7526,3.7473,3.7405,3.7322,3.7223,3.711,3.6983,3.6842,3.6687,3.6518,3.6337,3.6144,3.5939,3.5819,3.5699,3.547,3.5243,3.502,3.4801,3.464,3.4479,3.4319,3.4158,3.3885,3.3611,3.3337,3.3063,3.2709,3.3185,3.2952,3.243,3.2149,3.1857,3.156,3.126,3.143,3.1132,3.0834,3.0535,3.0236,2.9937,2.9637,2.9337,2.9037,2.8737,2.8437,2.8263,2.7963,2.7663,2.7363,2.7063,2.6763,2.6463,2.6163,2.5863,2.5563,2.5263,2.4963,2.4663,2.4363,2.4063,2.3763,2.3463,2.3295,2.2995,2.2695,2.2395,2.2095,2.1795,2.1495,2.1195,2.0895,2.0595,2.0295,1.9995,1.9696,1.9396,1.9096,1.8796,1.8496,1.8197,1.7897,1.7597,1.7298,1.6998,1.6699,1.6616,1.6316,1.6016,1.5716,1.5416,1.5116,1.4816,1.4516,1.4216,1.3916,1.3616,1.3316,1.3016,1.2716,1.2416,1.2116,1.1816,1.1516,1.1216,1.0916,1.0616,1.0316,1.0016,0.97162,0.94162,0.91163,0.88163,0.85164,0.82165,0.79165,0.76166,0.73167,0.70168,0.67169,0.6417,0.61171,0.58173,0.56558,0.54943,0.51943,0.48943,0.45944,0.42944,0.39944,0.36944,0.33944,0.30944,0.27945,0.24945,0.21945,0.18945,0.15945,0.12945,0.099449,0.069449,0.039449,0.0094492,-0.020551,-0.050551,-0.080551,-0.11055,-0.14055,-0.17055,-0.20055,-0.23055,-0.26055,-0.29055,-0.32055,-0.34909,-0.37903,-0.40875,-0.43805,-0.46677,-0.49471,-0.52172,-0.54763,-0.57227,-0.5955,-0.57835,-0.56119,-0.58848,-0.6161,-0.64402,-0.67223,-0.68724,-0.70225,-0.71726,-0.73227,-0.75528,-0.77703,-0.79742,-0.81639,-0.83385,-0.84973,-0.86396,-0.8765,-0.89075,-0.90573,-0.92119,-0.93713,-0.94732,-0.96061,-0.97265,-0.98342,-0.9929,-1.0011,-1.0079,-1.0177,-1.0345,-1.0512,-1.0651,-1.0848,-1.0977,-1.1107,-1.1237,-1.1367,-1.1496,-1.1626,-1.1726,-1.1885,-1.1914,-1.1942,-1.1971,-1.1999,-1.2028,-1.2056,-1.2086,-1.2113,-1.2135,-1.2157,-1.2154,-1.2152,-1.2149,-1.2147];
Y_car_1 = [ 0.0017561,-0.024834,-0.051424,-0.078014,-0.1046,-0.12577,-0.14694,-0.17927,-0.19927,-0.22544,-0.2516,-0.27777,-0.30394,-0.3301,-0.35627,-0.37569,-0.38687,-0.40218,-0.41748,-0.43278,-0.44809,-0.46339,-0.47869,-0.48594,-0.49393,-0.50156,-0.50918,-0.52063,-0.54984,-0.57871,-0.60717,-0.63516,-0.66264,-0.68953,-0.70735,-0.73276,-0.75847,-0.78446,-0.81299,-0.84024,-0.86664,-0.89209,-0.91648,-0.93972,-0.96171,-0.98237,-1.0016,-1.0099,-1.0182,-1.0265,-1.0349,-1.0451,-1.056,-1.0678,-1.0802,-1.0678,-1.0553,-1.0743,-1.0914,-1.1065,-1.1195,-1.1304,-1.1391,-1.1455,-1.1495,-1.1513,-1.1499,-1.1501,-1.1503,-1.1504,-1.1505,-1.1506,-1.1507,-1.1508,-1.1508,-1.1508,-1.1508,-1.1508,-1.1507,-1.1507,-1.1506,-1.1505,-1.1503,-1.1502,-1.15,-1.1498,-1.1495,-1.1493,-1.149,-1.1487,-1.1484,-1.1481,-1.1477,-1.1473,-1.1469,-1.1465,-1.1463,-1.146,-1.1451,-1.1442,-1.1434,-1.1426,-1.1418,-1.1411,-1.1404,-1.1398,-1.1391,-1.1385,-1.138,-1.1375,-1.137,-1.1365,-1.1361,-1.1357,-1.1354,-1.1351,-1.1348,-1.1346,-1.1343,-1.1342,-1.134,-1.1339,-1.1339,-1.1338,-1.1338,-1.1339,-1.1339,-1.134,-1.1342,-1.1343,-1.1345,-1.1348,-1.1351,-1.1354,-1.1352,-1.1371,-1.1389,-1.1406,-1.1422,-1.1437,-1.1451,-1.1463,-1.1475,-1.1485,-1.1495,-1.1503,-1.151,-1.1516,-1.1521,-1.1525,-1.1528,-1.153,-1.153,-1.153,-1.1529,-1.1526,-1.1522,-1.1519,-1.1522,-1.1526,-1.1528,-1.153,-1.1531,-1.1532,-1.1532,-1.1531,-1.153,-1.1528,-1.1526,-1.1523,-1.152,-1.1516,-1.1511,-1.1506,-1.1501,-1.1504,-1.1503,-1.1497,-1.1488,-1.1473,-1.1455,-1.1432,-1.1404,-1.1372,-1.1336,-1.1349,-1.1343,-1.1304,-1.1233,-1.113,-1.0835,-1.0647,-1.0993,-1.0707,-1.0585,-1.0463,-1.034,-1.0216,-1.0113,-1.001,-0.99074,-0.98045,-0.96,-0.93993,-0.92022,-0.90089,-0.8903,-0.8797,-0.8578,-0.83485,-0.81093,-0.78609,-0.76041,-0.73396,-0.7068,-0.67901,-0.65067,-0.62186,-0.59265,-0.56312,-0.53336,-0.50344,-0.47345,-0.43949,-0.4097,-0.37985,-0.34996,-0.31195,-0.28197,-0.25199,-0.22201,-0.19202,-0.16203,-0.13204,-0.10204,-0.072039,-0.042039,0.012426,-0.012426,0.042039,0.072039,0.10204,0.13204,0.16203,0.19202,0.22201,0.25199,0.28197,0.31195,0.34996,0.37985,0.4097,0.43949,0.47345,0.50344,0.53336,0.56312,0.59265,0.62186,0.65067,0.67901,0.7068,0.73396,0.76041,0.78609,0.81093,0.83485,0.8578,0.8797,0.8903,0.90089,0.92022,0.93993,0.96,0.98045,0.99074,1.001,1.0113,1.0216,1.034,1.0463,1.0585,1.0707,1.0993,1.0647,1.0835,1.113,1.1233,1.1304,1.1343,1.1349,1.1336,1.1372,1.1404,1.1432,1.1455,1.1473,1.1488,1.1497,1.1503,1.1504,1.1501,1.1506,1.1511,1.1516,1.152,1.1523,1.1526,1.1528,1.153,1.1531,1.1532,1.1532,1.1531,1.153,1.1528,1.1526,1.1522,1.1519,1.1522,1.1526,1.1529,1.153,1.153,1.153,1.1528,1.1525,1.1521,1.1516,1.151,1.1503,1.1495,1.1485,1.1475,1.1463,1.1451,1.1437,1.1422,1.1406,1.1389,1.1371,1.1352,1.1354,1.1351,1.1348,1.1345,1.1343,1.1342,1.134,1.1339,1.1339,1.1338,1.1338,1.1339,1.1339,1.134,1.1342,1.1343,1.1346,1.1348,1.1351,1.1354,1.1357,1.1361,1.1365,1.137,1.1375,1.138,1.1385,1.1391,1.1398,1.1404,1.1411,1.1418,1.1426,1.1434,1.1442,1.1451,1.146,1.1463,1.1465,1.1469,1.1473,1.1477,1.1481,1.1484,1.1487,1.149,1.1493,1.1495,1.1498,1.15,1.1502,1.1503,1.1505,1.1506,1.1507,1.1507,1.1508,1.1508,1.1508,1.1508,1.1508,1.1507,1.1506,1.1505,1.1504,1.1503,1.1501,1.1499,1.1513,1.1495,1.1455,1.1391,1.1304,1.1195,1.1065,1.0914,1.0743,1.0553,1.0678,1.0802,1.0678,1.056,1.0451,1.0349,1.0265,1.0182,1.0099,1.0016,0.98237,0.96171,0.93972,0.91648,0.89209,0.86664,0.84024,0.81299,0.78446,0.75847,0.73276,0.70735,0.68953,0.66264,0.63516,0.60717,0.57871,0.54984,0.52063,0.50918,0.50156,0.49393,0.48594,0.47869,0.46339,0.44809,0.43278,0.41748,0.40218,0.38687,0.37569,0.35627,0.3301,0.30394,0.27777,0.2516,0.22544,0.19927,0.17927,0.14694,0.12577,0.1046,0.078014,0.051424,0.024834,-0.0017561];


% UXE
% X_car_0 = [3.8443,3.8438,3.843,3.8419,3.8407,3.8392,3.8374,3.8364,3.8349,3.8328,3.83,3.8266,3.8225,3.8179,3.8153,3.8074,3.7987,3.7893,3.7791,3.7681,3.7564,3.7446,3.7317,3.7174,3.7015,3.6842,3.6655,3.6456,3.6244,3.6061,3.5869,3.5668,3.5458,3.524,3.5014,3.4865,3.4602,3.4326,3.4041,3.3749,3.3452,3.3152,3.3131,3.2831,3.2532,3.2232,3.1932,3.1633,3.1333,3.1293,3.0993,3.0693,3.0393,3.0094,2.9794,2.9494,2.9194,2.8894,2.8594,2.8474,2.8174,2.7874,2.7574,2.7274,2.6974,2.6674,2.6374,2.6074,2.5774,2.5474,2.5174,2.4874,2.4574,2.4275,2.3975,2.3675,2.3489,2.319,2.289,2.2591,2.2291,2.1991,2.1691,2.1392,2.1092,2.0792,2.0492,2.0192,1.9892,1.9592,1.9292,1.8992,1.8692,1.8392,1.8092,1.7792,1.7492,1.7192,1.6892,1.6831,1.6531,1.6231,1.5931,1.5631,1.5331,1.5031,1.4731,1.4431,1.4131,1.3831,1.3531,1.3231,1.2931,1.2631,1.2331,1.2031,1.1731,1.1431,1.1131,1.0831,1.0531,1.0231,0.99309,0.96309,0.93309,0.90309,0.8731,0.8431,0.8131,0.78311,0.75311,0.72312,0.69312,0.66313,0.63314,0.60314,0.5756,0.54574,0.51586,0.48597,0.45607,0.42616,0.39623,0.3663,0.33636,0.3064,0.27644,0.24647,0.2165,0.18652,0.15653,0.12654,0.096546,0.066549,0.036551,0.0065512,-0.023449,-0.053448,-0.083447,-0.11344,-0.14344,-0.17343,-0.20341,-0.2334,-0.26337,-0.28629,-0.31626,-0.3462,-0.37609,-0.40593,-0.4357,-0.4654,-0.46845,-0.49796,-0.52739,-0.55671,-0.58593,-0.61503,-0.63509,-0.6626,-0.68908,-0.71436,-0.73831,-0.76075,-0.77728,-0.7989,-0.81897,-0.83738,-0.85405,-0.86888,-0.87258,-0.88439,-0.89542,-0.90566,-0.91511,-0.92364,-0.93165,-0.93923,-0.94638,-0.9531,-0.95939,-0.96026,-0.96513,-0.96982,-0.97432,-0.97863,-0.98275,-0.98558,-0.9883,-0.99066,-0.99268,-0.99435,-0.99566,-0.99662,-0.99674,-0.99836,-0.99839,-0.99698,-0.99563,-0.99434,-0.99312,-0.99282,-0.9911,-0.98877,-0.98583,-0.98228,-0.97811,-0.9737,-0.96964,-0.96489,-0.95946,-0.95335,-0.94656,-0.94236,-0.93511,-0.92723,-0.91871,-0.90956,-0.90011,-0.89024,-0.87849,-0.86489,-0.84953,-0.83496,-0.81568,-0.79543,-0.77425,-0.75219,-0.72927,-0.70806,-0.68255,-0.65578,-0.62796,-0.59928,-0.56996,-0.55362,-0.52433,-0.4949,-0.46533,-0.43566,-0.40588,-0.38318,-0.35326,-0.32333,-0.29338,-0.26343,-0.23346,-0.20348,-0.1735,-0.14351,-0.11352,-0.083518,-0.053518,-0.023518,0.00105,0.030948,0.060855,0.09077,0.12069,0.15062,0.18056,0.2105,0.24045,0.27041,0.30037,0.33034,0.36031,0.39029,0.42027,0.45025,0.48024,0.51023,0.54022,0.57021,0.60021,0.63021,0.66021,0.69021,0.72021,0.75021,0.78021,0.81021,0.8402,0.8702,0.90019,0.93018,0.96017,0.99016,1.0201,1.0501,1.0801,1.1101,1.1179,1.1479,1.1779,1.2079,1.2379,1.2679,1.2979,1.3279,1.3579,1.3879,1.4179,1.4479,1.4779,1.5079,1.5379,1.5679,1.5979,1.6279,1.6579,1.6879,1.7179,1.7479,1.7779,1.8079,1.8379,1.8679,1.8979,1.9279,1.9579,1.9738,2.0037,2.0337,2.0637,2.0937,2.1237,2.1536,2.1836,2.2136,2.2436,2.2736,2.3035,2.3335,2.3635,2.3935,2.4235,2.4535,2.4835,2.5135,2.5435,2.5734,2.5937,2.6237,2.6537,2.6837,2.7137,2.7437,2.7737,2.8037,2.8337,2.8637,2.8937,2.9237,2.9537,2.9837,3.0136,3.0436,3.0736,3.0763,3.1063,3.1363,3.1663,3.1962,3.2133,3.2433,3.2732,3.3031,3.333,3.3627,3.3924,3.4044,3.433,3.4606,3.4867,3.5111,3.5335,3.5537,3.5554,3.5748,3.5942,3.6137,3.6333,3.6529,3.6726,3.6923,3.6926,3.7085,3.7235,3.7376,3.7508,3.763,3.7742,3.7845,3.7848,3.7952,3.8043,3.8121,3.8186,3.8237,3.8274,3.8291,3.8315,3.8337,3.8358,3.8377,3.8394,3.8409,3.8416,3.843,3.8439,3.8443];
% Y_car_0 = [-1.6e-05,-0.030011,-0.06,-0.089982,-0.11996,-0.14992,-0.17987,-0.19559,-0.22555,-0.25548,-0.28535,-0.31515,-0.34488,-0.37451,-0.38959,-0.41854,-0.44726,-0.47574,-0.50395,-0.53188,-0.5595,-0.58557,-0.61268,-0.639,-0.66446,-0.68897,-0.71246,-0.73483,-0.756,-0.77982,-0.80288,-0.82515,-0.84658,-0.86715,-0.88681,-0.89885,-0.91312,-0.92497,-0.93432,-0.94108,-0.94521,-0.94667,-0.94667,-0.94853,-0.95023,-0.95177,-0.95314,-0.95436,-0.95542,-0.95555,-0.95691,-0.95813,-0.95921,-0.96015,-0.96094,-0.96159,-0.9621,-0.96246,-0.96268,-0.96273,-0.96296,-0.96311,-0.96318,-0.96315,-0.96305,-0.96285,-0.96258,-0.96221,-0.96177,-0.96123,-0.96062,-0.95991,-0.95912,-0.95825,-0.95729,-0.95625,-0.95556,-0.95384,-0.95222,-0.95072,-0.94933,-0.94805,-0.94689,-0.94583,-0.94488,-0.94405,-0.94333,-0.94271,-0.94221,-0.94182,-0.94155,-0.94138,-0.94133,-0.94138,-0.94155,-0.94183,-0.94222,-0.94272,-0.94333,-0.94347,-0.94353,-0.94358,-0.9436,-0.94361,-0.94359,-0.94356,-0.94351,-0.94343,-0.94334,-0.94323,-0.9431,-0.94294,-0.94277,-0.94258,-0.94237,-0.94214,-0.94189,-0.94162,-0.94133,-0.94102,-0.9407,-0.94035,-0.93998,-0.93959,-0.93919,-0.93876,-0.93831,-0.93785,-0.93736,-0.93686,-0.93633,-0.93579,-0.93523,-0.93464,-0.93404,-0.93342,-0.93283,-0.93571,-0.93844,-0.94102,-0.94345,-0.94573,-0.94785,-0.94983,-0.95165,-0.95332,-0.95484,-0.95621,-0.95743,-0.95849,-0.95941,-0.96017,-0.96078,-0.96124,-0.96155,-0.96171,-0.96171,-0.96157,-0.96127,-0.96082,-0.96022,-0.95946,-0.95856,-0.9575,-0.9563,-0.95527,-0.95395,-0.95203,-0.94952,-0.94642,-0.94273,-0.93845,-0.93798,-0.9326,-0.92674,-0.92041,-0.91361,-0.90633,-0.90103,-0.88907,-0.87499,-0.85886,-0.8408,-0.82091,-0.80405,-0.78326,-0.76097,-0.7373,-0.71236,-0.68629,-0.67902,-0.65144,-0.62354,-0.59535,-0.56688,-0.53856,-0.50965,-0.48062,-0.45149,-0.42225,-0.39292,-0.38871,-0.35911,-0.32948,-0.29982,-0.27013,-0.24041,-0.21921,-0.18933,-0.15943,-0.1295,-0.099542,-0.069571,-0.039586,-0.03493,-0.0049818,-0.00022,0.029747,0.059716,0.089689,0.11966,0.12722,0.15717,0.18708,0.21693,0.24672,0.27643,0.30396,0.33368,0.3633,0.39281,0.42218,0.4514,0.4681,0.49721,0.52616,0.55492,0.58349,0.61093,0.63925,0.66685,0.69359,0.71935,0.74062,0.7636,0.78573,0.80698,0.8273,0.84666,0.86315,0.87892,0.89244,0.90364,0.91242,0.91871,0.9211,0.9276,0.93339,0.93848,0.94286,0.94653,0.94884,0.95106,0.95307,0.95487,0.95647,0.95787,0.95906,0.96005,0.96083,0.96141,0.96178,0.96194,0.9619,0.96172,0.95925,0.95688,0.95463,0.95247,0.95043,0.94849,0.94665,0.94492,0.9433,0.94179,0.94038,0.93907,0.93787,0.93678,0.9358,0.93492,0.93415,0.93348,0.93292,0.93247,0.93212,0.93188,0.93174,0.93171,0.93179,0.93197,0.93226,0.93266,0.93316,0.93377,0.93449,0.93531,0.93624,0.93727,0.93841,0.93966,0.94101,0.94138,0.94166,0.94191,0.94215,0.94237,0.94257,0.94275,0.94292,0.94307,0.94319,0.9433,0.94339,0.94347,0.94352,0.94356,0.94357,0.94357,0.94355,0.94352,0.94346,0.94339,0.94329,0.94318,0.94305,0.9429,0.94274,0.94255,0.94235,0.94212,0.942,0.94325,0.94446,0.94565,0.94682,0.94795,0.94905,0.95013,0.95118,0.9522,0.95319,0.95416,0.95509,0.956,0.95688,0.95773,0.95855,0.95934,0.96011,0.96085,0.96156,0.96202,0.96244,0.96277,0.96301,0.96316,0.96322,0.96319,0.96307,0.96286,0.96256,0.96217,0.96169,0.96112,0.96046,0.95971,0.95888,0.95795,0.95786,0.95653,0.95526,0.95404,0.95288,0.95224,0.95144,0.94989,0.94759,0.94456,0.94079,0.93628,0.93424,0.92536,0.91349,0.89876,0.88133,0.86141,0.83922,0.837,0.81408,0.79121,0.76841,0.74566,0.72297,0.70033,0.67776,0.67748,0.65204,0.62607,0.59959,0.57263,0.54524,0.51743,0.48925,0.48857,0.46045,0.43188,0.40291,0.37362,0.34405,0.31428,0.29409,0.26419,0.23427,0.20434,0.1744,0.14445,0.11449,0.0992,0.069234,0.039248,0.0092508];
% X_car_1 = [4.0443,4.0438,4.043,4.042,4.0407,4.0393,4.0376,4.0362,4.0348,4.0328,4.0302,4.0271,4.0234,4.0191,4.0144,4.0089,4.0011,3.9926,3.9835,3.9737,3.9633,3.9522,3.9405,3.9282,3.9277,3.9151,3.9013,3.8865,3.8706,3.8536,3.8357,3.8167,3.7968,3.776,3.7856,3.7675,3.7486,3.729,3.7086,3.6876,3.6659,3.6435,3.6206,3.605,3.5894,3.5633,3.5364,3.5087,3.4805,3.4518,3.4226,3.393,3.3633,3.3333,3.326,3.296,3.2661,3.2361,3.2062,3.1762,3.1462,3.1388,3.1089,3.0789,3.0489,3.0189,2.9889,2.9589,2.9289,2.8989,2.8689,2.8492,2.8192,2.7892,2.7592,2.7292,2.6992,2.6692,2.6392,2.6092,2.5792,2.5493,2.5193,2.4893,2.4593,2.4293,2.3993,2.3693,2.3532,2.3371,2.3071,2.2772,2.2472,2.2172,2.1872,2.1573,2.1273,2.0973,2.0673,2.0373,2.0073,1.9773,1.9473,1.9173,1.8873,1.8573,1.8273,1.7973,1.7673,1.7373,1.7073,1.6836,1.6536,1.6236,1.5936,1.5636,1.5336,1.5036,1.4736,1.4436,1.4136,1.3836,1.3536,1.3236,1.2936,1.2636,1.2336,1.2036,1.1736,1.1436,1.1136,1.0836,1.0536,1.0236,0.99358,0.96358,0.93358,0.90359,0.87359,0.84359,0.8136,0.7836,0.7536,0.72361,0.69361,0.66362,0.63363,0.60363,0.57364,0.59531,0.56545,0.53557,0.50569,0.47579,0.44588,0.41595,0.38602,0.35608,0.32613,0.29617,0.2662,0.23623,0.20625,0.17627,0.14628,0.11629,0.08629,0.056293,0.026294,-0.003706,-0.033706,-0.063705,-0.093703,-0.1237,-0.15369,-0.18368,-0.21367,-0.24365,-0.27362,-0.29313,-0.3231,-0.35305,-0.38296,-0.41282,-0.44263,-0.47237,-0.48756,-0.50274,-0.53226,-0.5617,-0.59105,-0.6203,-0.64946,-0.67851,-0.69302,-0.70754,-0.73521,-0.76225,-0.78857,-0.81413,-0.83883,-0.86262,-0.88544,-0.90723,-0.91063,-0.93249,-0.95333,-0.9731,-0.99174,-1.0092,-1.0255,-1.0405,-1.0479,-1.0553,-1.0672,-1.0784,-1.089,-1.0988,-1.1081,-1.116,-1.124,-1.1316,-1.1389,-1.1458,-1.1522,-1.1575,-1.1624,-1.1671,-1.1716,-1.1759,-1.1801,-1.1824,-1.1846,-1.1874,-1.1898,-1.1919,-1.1936,-1.195,-1.1961,-1.1958,-1.1979,-1.198,-1.1982,-1.1967,-1.1954,-1.1941,-1.1929,-1.1926,-1.1909,-1.1887,-1.1859,-1.1826,-1.1788,-1.1744,-1.1722,-1.1681,-1.1635,-1.1583,-1.1525,-1.1462,-1.1392,-1.137,-1.1298,-1.122,-1.1137,-1.1048,-1.0954,-1.0911,-1.0815,-1.0706,-1.0585,-1.045,-1.0304,-1.0145,-0.99744,-0.99088,-0.97171,-0.95177,-0.93111,-0.90973,-0.88766,-0.86493,-0.84156,-0.82037,-0.7951,-0.76899,-0.74211,-0.71455,-0.68638,-0.6577,-0.62858,-0.59911,-0.59928,-0.57,-0.5406,-0.51107,-0.48145,-0.45173,-0.42194,-0.39863,-0.36871,-0.33878,-0.30884,-0.27888,-0.24892,-0.21894,-0.18896,-0.15897,-0.12898,-0.098983,-0.068984,-0.038985,-0.0089849,0.017873,0.047771,0.077678,0.10759,0.13752,0.16745,0.19739,0.22733,0.25728,0.28724,0.3172,0.34717,0.37714,0.40712,0.4371,0.46709,0.49708,0.52707,0.55706,0.58706,0.61705,0.64705,0.67705,0.70705,0.73705,0.76705,0.79705,0.82705,0.85704,0.88704,0.91703,0.94702,0.97701,1.007,1.037,1.0669,1.0969,1.116,1.146,1.176,1.206,1.236,1.266,1.296,1.326,1.356,1.386,1.416,1.446,1.476,1.506,1.536,1.566,1.596,1.626,1.656,1.686,1.716,1.746,1.776,1.806,1.836,1.866,1.896,1.926,1.956,1.9654,1.9953,2.0253,2.0553,2.0853,2.1152,2.1452,2.1752,2.2052,2.2352,2.2652,2.2951,2.3251,2.3551,2.3851,2.4151,2.4451,2.4751,2.5051,2.535,2.565,2.5906,2.6206,2.6506,2.6806,2.7106,2.7406,2.7706,2.8006,2.8306,2.8606,2.8906,2.9206,2.9506,2.9806,3.0106,3.0405,3.0705,3.0854,3.1154,3.1453,3.1753,3.2053,3.2162,3.2462,3.2761,3.3061,3.3359,3.3657,3.3955,3.4251,3.4533,3.4822,3.5104,3.538,3.5648,3.5908,3.6157,3.6395,3.6622,3.6836,3.7036,3.7084,3.7278,3.7472,3.7667,3.7863,3.8059,3.8256,3.8429,3.8603,3.8763,3.8915,3.9061,3.9199,3.9329,3.9451,3.9566,3.9673,3.9706,3.9812,3.9908,3.9994,4.0069,4.0135,4.0189,4.0234,4.0268,4.0283,4.0308,4.033,4.0351,4.037,4.0388,4.0403,4.0413,4.0427,4.0437,4.0442];
% Y_car_1 = [-0.0029061,-0.032901,-0.062891,-0.092874,-0.12285,-0.15281,-0.18277,-0.2034,-0.23336,-0.2633,-0.29319,-0.32302,-0.35279,-0.38249,-0.41211,-0.4395,-0.46846,-0.49723,-0.52581,-0.55417,-0.5823,-0.61018,-0.6378,-0.66514,-0.666,-0.69321,-0.71988,-0.74594,-0.77138,-0.79613,-0.82016,-0.84342,-0.86588,-0.88749,-0.87434,-0.89824,-0.92155,-0.94424,-0.9663,-0.98769,-1.0084,-1.0284,-1.0477,-1.059,-1.0703,-1.0851,-1.0983,-1.11,-1.1201,-1.1286,-1.1355,-1.1408,-1.1444,-1.1463,-1.1463,-1.1481,-1.1498,-1.1514,-1.1528,-1.154,-1.1551,-1.1553,-1.1567,-1.1579,-1.159,-1.16,-1.1608,-1.1614,-1.162,-1.1624,-1.1626,-1.1627,-1.163,-1.1631,-1.1632,-1.1632,-1.1631,-1.1629,-1.1626,-1.1623,-1.1618,-1.1613,-1.1607,-1.16,-1.1592,-1.1584,-1.1575,-1.1564,-1.1558,-1.1552,-1.1535,-1.1519,-1.1504,-1.149,-1.1477,-1.1466,-1.1455,-1.1446,-1.1438,-1.1431,-1.1425,-1.1421,-1.1417,-1.1415,-1.1413,-1.1413,-1.1414,-1.1417,-1.142,-1.1424,-1.143,-1.1435,-1.1435,-1.1436,-1.1436,-1.1436,-1.1436,-1.1436,-1.1435,-1.1434,-1.1433,-1.1432,-1.1431,-1.1429,-1.1428,-1.1426,-1.1424,-1.1422,-1.1419,-1.1416,-1.1413,-1.141,-1.1407,-1.1404,-1.14,-1.1396,-1.1392,-1.1388,-1.1383,-1.1379,-1.1374,-1.1369,-1.1364,-1.1358,-1.1353,-1.1347,-1.1341,-1.1335,-1.1328,-1.1319,-1.1347,-1.1375,-1.1401,-1.1425,-1.1448,-1.147,-1.149,-1.1508,-1.1525,-1.1541,-1.1555,-1.1568,-1.1579,-1.1589,-1.1598,-1.1604,-1.161,-1.1614,-1.1616,-1.1617,-1.1617,-1.1615,-1.1612,-1.1607,-1.16,-1.1593,-1.1583,-1.1573,-1.156,-1.1552,-1.1539,-1.1521,-1.1497,-1.1469,-1.1435,-1.1396,-1.1373,-1.135,-1.1297,-1.1239,-1.1177,-1.111,-1.1039,-1.0965,-1.0919,-1.0874,-1.0759,-1.0629,-1.0485,-1.0328,-1.0158,-0.99749,-0.97802,-0.9574,-0.95311,-0.93257,-0.91099,-0.88843,-0.86493,-0.84055,-0.81536,-0.7894,-0.77485,-0.7603,-0.73275,-0.70493,-0.67685,-0.64852,-0.61998,-0.59337,-0.56446,-0.53545,-0.50634,-0.47714,-0.44784,-0.42181,-0.39221,-0.36258,-0.33293,-0.30324,-0.27353,-0.25601,-0.2385,-0.20862,-0.17872,-0.14879,-0.11884,-0.088875,-0.058896,-0.053784,-0.023857,-0.0072265,0.0094037,0.03937,0.06934,0.099313,0.12929,0.13662,0.16658,0.19649,0.22637,0.25618,0.28594,0.31562,0.32873,0.35846,0.3881,0.41765,0.44708,0.4764,0.50558,0.51427,0.54339,0.57237,0.60119,0.62985,0.65833,0.67032,0.69875,0.7267,0.75412,0.78094,0.8071,0.83256,0.85724,0.86588,0.88895,0.91137,0.93311,0.95415,0.97448,0.99405,1.0129,1.0286,1.0448,1.0596,1.0729,1.0847,1.095,1.1038,1.111,1.1166,1.1158,1.1224,1.1283,1.1336,1.1384,1.1425,1.146,1.1482,1.1505,1.1525,1.1543,1.1559,1.1574,1.1586,1.1597,1.1605,1.1612,1.1616,1.1619,1.162,1.1618,1.161,1.1585,1.1562,1.1539,1.1518,1.1498,1.1478,1.146,1.1443,1.1427,1.1412,1.1398,1.1385,1.1374,1.1363,1.1354,1.1345,1.1338,1.1332,1.1327,1.1323,1.132,1.1318,1.1317,1.1317,1.1319,1.1321,1.1325,1.133,1.1335,1.1342,1.135,1.1359,1.1369,1.138,1.1393,1.1406,1.1414,1.1416,1.1419,1.1421,1.1424,1.1426,1.1427,1.1429,1.1431,1.1432,1.1433,1.1434,1.1435,1.1435,1.1436,1.1436,1.1436,1.1436,1.1435,1.1435,1.1434,1.1433,1.1432,1.1431,1.1429,1.1428,1.1426,1.1424,1.1421,1.1418,1.1431,1.1443,1.1455,1.1466,1.1478,1.1489,1.15,1.151,1.152,1.153,1.154,1.1549,1.1558,1.1567,1.1576,1.1584,1.1592,1.16,1.1607,1.1614,1.162,1.1624,1.1627,1.163,1.1631,1.1632,1.1632,1.1631,1.1629,1.1626,1.1622,1.1618,1.1612,1.1606,1.1599,1.159,1.1581,1.1577,1.1563,1.1551,1.1538,1.1527,1.1522,1.1515,1.1501,1.148,1.1454,1.1421,1.1381,1.1336,1.1282,1.1199,1.1099,1.0981,1.0847,1.0696,1.0529,1.0347,1.015,0.994,0.97168,0.96581,0.94289,0.92003,0.89722,0.87448,0.85179,0.82916,0.8078,0.78644,0.76105,0.73523,0.70898,0.68233,0.65531,0.62793,0.60021,0.57218,0.5626,0.53454,0.50612,0.47738,0.44835,0.41907,0.38958,0.35991,0.3301,0.31096,0.28106,0.25114,0.22121,0.19128,0.16132,0.13136,0.11028,0.080315,0.050331,0.020336];
% 

car_0 = [X_car_0;Y_car_0];
car_1 = [X_car_1;Y_car_1];

X = [];
Y = [];
for i =0:249
    for j = 0:249
        x = 12.45 - 0.1 * i;
        y = 12.45 - 0.1 * j;
        X = [X,x];
        Y = [Y,y];
    end
end

index_L = 334;
Flag_plot_veh = false;
 for index_plan = 0
     fid = fopen('C:\Users\16501\Desktop\rizhi_fenxi_0723\plan.txt', 'rb');
        %fid = fopen('D:\Work\测试\04UXE\1206\1206\1057\plan.txt', 'rb');
    
    request = 1;
    log_list = 0;
    
    path = zeros(300,5);
    while ~feof(fid)
        
        log_list = log_list + 1;
        tline = fgetl(fid);
        
        if log_list == (request-1)*index_L+12
            num_str = regexp(tline,'\-?\d*\.?\d*','match');
            num = str2double(num_str);
            park_ = num;
        elseif log_list == (request-1)*index_L+14
            num_str = regexp(tline,'\-?\d*\.?\d*','match');
            num = str2double(num_str);
            START = num;
        elseif log_list == (request-1)*index_L+15
            num_str = regexp(tline,'\-?\d*\.?\d*','match');
            num = str2double(num_str);
            END = num;
        elseif log_list == (request-1)*index_L+19
            num_str = regexp(tline,'\-?\d*\.?\d*','match');
            num = str2double(num_str);
            map = num(1:62500);
        elseif log_list == (request-1)*index_L+26
            num_str = regexp(tline,'\-?\d*\.?\d*','match');
            num = str2double(num_str);
            AstarOrGeo = num(end);
        elseif log_list == (request-1)*index_L+27
            num_str = regexp(tline,'\-?\d*\.?\d*','match');
            num = str2double(num_str);
            CloseSizeMax = num(3);
        elseif log_list == (request-1)*index_L+28
            num_str = regexp(tline,'\-?\d*\.?\d*','match');
            num = str2double(num_str);
            gplan_time = num(1);
            aplan_time = num(2);
        elseif (log_list >= (request-1)*index_L+34) && (log_list <= (request-1)*index_L+333)
            num_str = regexp(tline,'\-?\d*\.?\d*','match');
            num = str2double(num_str);
            path(log_list-((request-1)*index_L+34)+1,:) = num;
        end
       %% 
        if (mod(log_list,330) == 0)%||(mod(log_list,327) == 25)
            status = reshape(map,250,250);
            Map = status';
            X_car = reshape(X,250,250);
            X_car = X_car';
            Y_car = reshape(Y,250,250);
            Y_car = Y_car';
            
            park = [park_(1:2);park_(3:4);park_(5:6);park_(7:8);park_(1:2)];
            
            figure(request)
            if AstarOrGeo == 0
                title(['几何','      ','plantime=',num2str(gplan_time)],'fontsize',10);hold on;
            elseif AstarOrGeo == 1
                title(['Astar','      ','plantime=',num2str(aplan_time),'      ','CloseSizeMax=',num2str(CloseSizeMax)],'fontsize',10);hold on;
            end
            for index_x = 1:250
                for index_y = 1:250
                    if Map(index_x,index_y) == 0 
                        plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.r');
                        rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor','r');hold on;
                    elseif Map(index_x,index_y) == 8
                        plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.r');
                        rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor','r');hold on;
%                     elseif Map(index_x,index_y) == 2
%                         plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.','Color',[0.5,0.5,0.5]);hold on;
                    elseif Map(index_x,index_y) == 7
                        plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.b');
                        rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor','y');hold on;
                    elseif Map(index_x,index_y) == 6
                        plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.r');
                        rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor','c');hold on;
                    elseif Map(index_x,index_y) == 3
                        plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.b');
                        rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor','b');hold on;
                    elseif Map(index_x,index_y) == 4
                        plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.k');
                        rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor','k');hold on;
                    elseif Map(index_x,index_y) == 5
                        plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.c');
                        rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor','c');hold on;
                    elseif Map(index_x,index_y) == 9
                        plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.','Color',[0.6,0.2,0.5]);hold on;
                        rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor',[0.6,0.2,0.5]);hold on;
                    end
                end
            end
            
            for pathplan_index = 1:300
                if path(pathplan_index,5) == 0
                    break;
                end
            end
            plot(path(1:pathplan_index-1,1)*0.01,path(1:pathplan_index-1,2)*0.01,'k');
            if pathplan_index <= 1
                pathplan_index = 2;
            end
            path_end = path(pathplan_index-1,:);
            path_end = [path_end(1)*0.01,path_end(2)*0.01,path_end(3)*pi/180];
            
%             [START(1),START(2)] = CoordinateTrans(Nav_pos(1)/100, Nav_pos(2)/100, Fusion_pos(1)/100, Fusion_pos(2)/100,mod2pi(Fusion_pos(3)*3.141592653589793/180));
%             START(3) = mod2pi(Nav_pos(3)*3.141592653589793/180 - Fusion_pos(3)*3.141592653589793/180);
            CAR_0s = [cos(START(3)),-sin(START(3));sin(START(3)),cos(START(3))]*car_0+[START(1);START(2)];
            CAR_1s = [cos(START(3)),-sin(START(3));sin(START(3)),cos(START(3))]*car_1+[START(1);START(2)];
            
            CAR_0 = [cos(END(3)),-sin(END(3));sin(END(3)),cos(END(3))]*car_0+[END(1);END(2)];
            CAR_1 = [cos(END(3)),-sin(END(3));sin(END(3)),cos(END(3))]*car_1+[END(1);END(2)];
            
            plot(START(1),START(2),'ob');
            plot(END(1),END(2),'or');
            plot(CAR_0s(1,:),CAR_0s(2,:),'.b');hold on;
            plot(CAR_1s(1,:),CAR_1s(2,:),'.b');hold on;

            plot(CAR_0(1,:),CAR_0(2,:),'.y');hold on;
            plot(CAR_1(1,:),CAR_1(2,:),'.y');hold on;

            plot(park(:,1),park(:,2),'g');hold on;
            %%
            for pathplan_index = 1:300
                if path(pathplan_index,5) == 0
                    break;
                end
            end
            plot(path(1:pathplan_index-1,1)*0.01,path(1:pathplan_index-1,2)*0.01,'k');
            if pathplan_index <= 1
                pathplan_index = 2;
            end
            path_end = path(pathplan_index-1,:);
            path_end = [path_end(1)*0.01,path_end(2)*0.01,path_end(3)*pi/180];
%%
            if Flag_plot_veh
                path(:,1) = path(:,1)*0.01;
                path(:,2) = path(:,2)*0.01;
                path(:,3) = path(:,3)*pi/180;
                 for i = 1 : length(path(:,1))
                     if ~(path(i,1) == 0 && path(i,2) == 0 && path(i,3) == 0 && path(i,4) == 0 && path(i,5) == 0)
                        CAR_0 = [cos(path(i,3)),-sin(path(i,3));sin(path(i,3)),cos(path(i,3))]*car_0+[path(i,1);path(i,2)];
                        CAR_1 = [cos(path(i,3)),-sin(path(i,3));sin(path(i,3)),cos(path(i,3))]*car_1+[path(i,1);path(i,2)];
                        plot(CAR_0(1,:),CAR_0(2,:),'.k');hold on;
                        plot(CAR_1(1,:),CAR_1(2,:),'.b');hold on;
                     end
                 end
            end
            %%
            path = zeros(300,5);
%             plot(park_12(:,1),park_12(:,2),'k');hold on;
            axis equal
            xlim([-12.5,12.5]);ylim([-12.5,12.5]);
            drawnow;
            request = request +1;
        end
    end
    %%
    if (mod(log_list,336) ~= 0)
        status = reshape(map,250,250);
        Map = status';
        X_car = reshape(X,250,250);
        X_car = X_car';
        Y_car = reshape(Y,250,250);
        Y_car = Y_car';

        park = [park_(1:2);park_(3:4);park_(5:6);park_(7:8);park_(1:2)];

        figure(request)
%         if AstarOrGeo == 0
%             title(['几何','      ','plantime=',num2str(gplan_time)],'fontsize',10);hold on;
%         elseif AstarOrGeo == 1
%             title(['Astar','      ','plantime=',num2str(aplan_time),'      ','CloseSizeMax=',num2str(CloseSizeMax)],'fontsize',10);hold on;
%         end
        for index_x = 1:250
            for index_y = 1:250
                if Map(index_x,index_y) == 0 
                    plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.r');
                    rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor','r');hold on;
                elseif Map(index_x,index_y) == 8
                    plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.r');
                    rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor','r');hold on;
%                 elseif Map(index_x,index_y) == 2
%                     plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.','Color',[0.5,0.5,0.5]);hold on;
                elseif Map(index_x,index_y) == 7
                    plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.b');
                    rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor','y');hold on;
                elseif Map(index_x,index_y) == 6
                    plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.r');
                        rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor','c');hold on;
                elseif Map(index_x,index_y) == 3
                    plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.b');
                    rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor','b');hold on;
                elseif Map(index_x,index_y) == 4
                    plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.k');
                    rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor','k');hold on;
                elseif Map(index_x,index_y) == 5
                    plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.c');
                    rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor','c');hold on;
                elseif Map(index_x,index_y) == 9
                    plot(X_car(index_x,index_y),Y_car(index_x,index_y),'.','Color',[0.6,0.2,0.5]);hold on;
                    rectangle('Position',[X_car(index_x,index_y)-0.05,Y_car(index_x,index_y)-0.05,0.1,0.1],'EdgeColor',[0.6,0.2,0.5]);hold on;
                end
            end
        end
        CAR_0s = [cos(START(3)),-sin(START(3));sin(START(3)),cos(START(3))]*car_0+[START(1);START(2)];
        CAR_1s = [cos(START(3)),-sin(START(3));sin(START(3)),cos(START(3))]*car_1+[START(1);START(2)];

        CAR_0 = [cos(END(3)),-sin(END(3));sin(END(3)),cos(END(3))]*car_0+[END(1);END(2)];
        CAR_1 = [cos(END(3)),-sin(END(3));sin(END(3)),cos(END(3))]*car_1+[END(1);END(2)];
        plot(START(1),START(2),'ob');
        plot(END(1),END(2),'or');
        plot(CAR_0s(1,:),CAR_0s(2,:),'.b');hold on;
        plot(CAR_1s(1,:),CAR_1s(2,:),'.b');hold on;
        plot(CAR_0(1,:),CAR_0(2,:),'.y');hold on;
        plot(CAR_1(1,:),CAR_1(2,:),'.y');hold on;
        plot(park(:,1),park(:,2),'g');hold on;
        
        for pathplan_index = 1:300
            if path(pathplan_index,5) == 0
                break;
            end
        end
        if pathplan_index <= 1
            pathplan_index = 2;
        end
        plot(path(1:pathplan_index-1,1)*0.01,path(1:pathplan_index-1,2)*0.01,'k');
        %             plot(park_12(:,1),park_12(:,2),'k');hold on;
        axis equal
        xlim([-12.5,12.5]);ylim([-12.5,12.5]);
    end
    fclose(fid);
%     pause
 end
 
 function [x,y] = CoordinateTrans(x_o,y_o,x_nav,y_nav,theta)
x = (x_o - x_nav) * cos(theta) + (y_o - y_nav) * sin(theta);
y = (y_o - y_nav) * cos(theta) - (x_o - x_nav) * sin(theta);
 end
function v = mod2pi(x)
%UNTITLED2 此处显示有关此函数的摘要
%   此处显示详细说明
v = rem(x,2*pi);
if v < -pi
    v = v+2*pi;
elseif v > pi
    v = v-2*pi;
end
end


2. 制作动画matlab

numFigs = numel(get(0,'Children')); % 获取当前打开的图形窗口数量 
num = numFigs;

location= pwd;
for i=num:-1:1
     print(gcf,'-djpeg',[location,num2str(i),'.png']);
     close(gcf)
end

pics = dir(location);
[picsnum,~] = size(pics); %
for i=1:num
    str = strcat(location,num2str(i), '.png');
    A=imread(str);
    [I,map]=rgb2ind(A,256);
    if(i==1)
        imwrite(I,map,'movefig.gif','DelayTime',0.5,'LoopCount',Inf)
    else
        imwrite(I,map,'movefig.gif','WriteMode','append','DelayTime',0.5)
    end
end

3. 操作说明

先执行1操作,替换plan.txt路径即可后,点击run,

再执行2操作,生成movefig.gif动画

完成

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值