【物理应用】车道检测仿真
程序员文章站
2024-01-27 12:04:16
...
clc
clear all
close all
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Create the waypoints with heading_n direction
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%set(gcf,'Visible','off') % turns current figure "off"
%set(0,'DefaultFigureVisible','off'); % all subsequent figures "off"
%realVideo();
heading_n=-1;
LostSignal_Button=0;
min_degree = 0.5;
arc_length=0.1; a=2.5; b=1.5;
lane_width=1.5;
lane_type=2;
if heading_n==1 %Clockwise
Left_curb = -call_create_ellipse(lane_type,arc_length,a,b,lane_width);
else %Anticlockwise
Left_curb = call_create_ellipse(lane_type,arc_length,a,b,lane_width);
end
%Find the angles between each waypoint
% yaw=zeros(size(Left_curb,1),1);
%
% for i=1:size(Left_curb,1)-1
% yaw(i,1)=atan2((Left_curb(i+1,2)-(Left_curb(i,2))),(Left_curb(i+1,1))-(Left_curb(i,1)));
% end
% i=size(Left_curb,1);
% yaw(i,1)=atan2((Left_curb(1,2)-(Left_curb(i,2))),(Left_curb(1,1))-(Left_curb(i,1)));
% Left_curb=[Left_curb, yaw*180/pi];
%Left_curb=[Left_curb, (1:size(Left_curb,1))'];
lane_type=3;
if heading_n==1 %Clockwise
Right_curb = -call_create_ellipse(lane_type,arc_length,a,b,lane_width);
else %Anticlockwise
Right_curb = call_create_ellipse(lane_type,arc_length,a,b,lane_width);
end
%Find the angles between each waypoint
% yaw=zeros(size(Right_curb,1),1);
% for i=1:size(Right_curb,1)-1
% yaw(i,1)=atan2((Right_curb(i+1,2)-(Right_curb(i,2))),(Right_curb(i+1,1))-(Right_curb(i,1)));
% end
% i=size(Right_curb,1);
% yaw(i,1)=atan2((Right_curb(1,2)-(Right_curb(i,2))),(Right_curb(1,1))-(Right_curb(i,1)));
% Right_curb=[Right_curb, yaw*180/pi];
%Right_curb=[Right_curb, (1:size(Right_curb,1))'];
lane_type=1;
if heading_n==1 %Clockwise
Driver_pose = -call_create_ellipse(lane_type,arc_length,a,b,lane_width);
else %Anticlockwise
Driver_pose = call_create_ellipse(lane_type,arc_length,a,b,lane_width);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Or ADD your own data points as follows:
x_inner=[-1.9191 -1.9419 -1.9584 -1.9764 -1.9905 -2.0088 -2.0188 -2.0352 -2.0483 -2.0666 -2.0702 -2.0885 -2.0982 -2.1099 -2.1195 -2.1324 -2.1397 -2.1515 -2.1518 -2.157 -2.1715 -2.179 -2.1819 -2.1867 -2.1895 -2.1918 -2.1954 -2.1945 -2.1967 -2.1952 -2.1945 -2.1936 -2.1907 -2.1872 -2.1857 -2.1794 -2.1804 -2.1271 -2.1244 -2.1039 -2.0904 -2.0708 -2.0637 -2.0455 -2.0232 -2.0098 -2.0035 -1.9802 -1.9661 -1.921 -1.9087 -1.8681 -1.8404 -1.8081 -1.7982 -1.7772 -1.7596 -1.7326 -1.7147 -1.6838 -1.6607 -1.6339 -1.5915 -1.5622 -1.5625 -1.517 -1.4942 -1.4667 -1.4529 -1.4279 -1.4116 -1.3835 -1.3602 -1.3341 -1.3197 -1.2942 -1.2713 -1.2461 -1.229 -1.202 -1.1915 -1.1688 -1.1537 -1.1369 -1.1205 -1.0986 -1.0806 -1.0594 -1.048 -1.0284 -1.0221 -1.0056 -0.995 -0.9815 -0.9748 -0.9618 -0.9537 -0.9422 -0.9363 -0.927 -0.9228 -0.9178 -0.8805 -0.8642 -0.8159 -0.7845 -0.7495 -0.7159 -0.6139 -0.5911 -0.5236 -0.4831 -0.416 -0.3959 -0.3767 -0.336 -0.3044 -0.2618 -0.2259 -0.1883 -0.1525 -0.1126 -0.0878 -0.0562 0.0581 0.0983 0.1554 0.1945 0.2483 0.2978 0.3555 0.3831 0.4305 0.4547 0.4864 0.5194 0.5735 0.614 0.6709 0.7101 0.7742 0.8072 0.8554 0.8878 0.9305 0.966 1.0126 1.0408 1.0821 1.1162 1.1601 1.1848 1.2092 1.2389 1.284 1.3112 1.3345 1.3643 1.3841 1.4126 1.4485 1.4877 1.5124 1.544 1.5704 1.6071 1.6235 1.6605 1.6691 1.7097 1.7176 1.7487 1.7678 1.7934 1.8213 1.8521 1.8683 1.8988 1.916 1.947 1.9662 1.9943 2.0262 2.0475 2.0698 2.0932 2.105 2.1264 2.1362 2.1551 2.1652 2.1793 2.1892 2.2023 2.2081 2.2194 2.223 2.229 2.232 2.2363 2.2375 2.2368 2.2362 2.2329 2.231 2.2229 2.2196 2.2083 2.2035 2.1897 2.179 2.162 2.1453 2.1271 2.1074 2.0936 2.0746 2.0661 2.0555 2.0508 2.0467 2.0484 2.0511 2.0561 2.0666 2.0797 2.0991 2.1127 2.1253 2.1357 2.1455 2.1579 2.1775 2.2002 2.2237 2.2463 2.2735 2.2918 2.31 2.3301 2.3459 2.3614 2.3749 2.3908 2.3975 2.4061 2.4115 2.4156 2.4184 2.42 2.4194 2.4159 2.4157 2.4022 2.3937 2.3806 2.37 2.3529 2.3464 2.3257 2.3075 2.2831 2.2674 2.242 2.2287 2.1986 2.185 2.1532 2.1372 2.104 2.0273 1.9813 1.9679 1.9306 1.9093 1.8785 1.8533 1.812 1.7887 1.7519 1.7326 1.7016 1.6759 1.6395 1.6184 1.5775 1.5598 1.4516 1.4469 1.3883 1.3776 1.3416 1.3214 1.2959 1.2831 1.2657 1.2519 1.2344 1.2241 1.2081 1.1987 1.1869 1.1818 1.1749 1.1699 1.1655 1.1629 1.1646 1.1642 1.1634 1.16 1.1544 1.1413 1.1326 1.0828 1.0531 0.9986 0.9634 0.9184 0.8922 0.8462 0.8086 0.7602 0.7107 0.6462 0.6035 0.4781 0.4562 0.419 0.3725 0.3655 0.3265 0.3232 0.3034 0.2976 0.282 0.279 0.2691 0.2631 0.2529 0.2434 0.2368 0.2267 0.216 0.2035 0.1934 0.1806 0.1699 0.151 0.1345 0.1066 0.0838 0.0014 -0.0285 -0.0909 -0.13 -0.1682 -0.2048 -0.2465 -0.279 -0.3252 -0.3662 -0.4254 -0.459 -0.5066 -0.5483 -0.6034 -0.6481 -0.7048 -0.7448 -0.8037 -0.8475 -0.9013 -0.9466 -0.9996 -1.0574 -1.1035 -1.1518 -1.1841 -1.2343 -1.2651 -1.3017 -1.3488 -1.4013 -1.4337 -1.4798 -1.5163 -1.5641 -1.6017 -1.6434 -1.6753 -1.7198 -1.7828 -1.8287 -1.8708 -1.9091 ];
y_inner=[1.4994 1.5148 1.5267 1.5397 1.5509 1.5652 1.5735 1.5883 1.6015 1.6205 1.626 1.6518 1.6643 1.6848 1.701 1.7252 1.743 1.7677 1.7644 1.7867 1.8207 1.8482 1.8637 1.8884 1.9 1.9283 1.9377 1.9632 1.9828 2.009 2.04 2.0683 2.0767 2.1007 2.1203 2.1528 2.1482 2.3032 2.3058 2.3403 2.3587 2.3848 2.3948 2.4134 2.4383 2.4542 2.4583 2.4782 2.4902 2.5225 2.5303 2.5576 2.5707 2.586 2.5906 2.5982 2.6048 2.6134 2.6181 2.6247 2.6283 2.6329 2.6365 2.6394 2.6391 2.6366 2.6335 2.6299 2.6282 2.6236 2.6206 2.6129 2.6059 2.5972 2.5912 2.5794 2.5679 2.5548 2.5442 2.5286 2.5211 2.5037 2.4923 2.4776 2.4639 2.4419 2.4221 2.3986 2.3821 2.3568 2.348 2.3198 2.3016 2.2751 2.2633 2.2341 2.2152 2.1795 2.1599 2.1216 2.1055 2.0755 1.9893 1.9643 1.9093 1.8804 1.8467 1.8193 1.7613 1.7491 1.7235 1.7115 1.6934 1.6876 1.6829 1.6681 1.6576 1.641 1.6272 1.612 1.5979 1.5815 1.5703 1.5552 1.4992 1.477 1.4457 1.4241 1.393 1.3643 1.3321 1.3196 1.3036 1.2968 1.2891 1.2822 1.2724 1.2679 1.2628 1.2628 1.2653 1.2701 1.2772 1.2852 1.2979 1.3094 1.3282 1.3407 1.3602 1.3774 1.4027 1.4186 1.4334 1.4516 1.4829 1.5021 1.5165 1.534 1.5452 1.5587 1.5766 1.5942 1.606 1.619 1.6283 1.6395 1.6432 1.6498 1.6506 1.65 1.6459 1.6397 1.6346 1.6272 1.6173 1.6071 1.5984 1.585 1.5743 1.5562 1.5435 1.5238 1.4961 1.4746 1.451 1.4258 1.4102 1.3809 1.3655 1.3325 1.3123 1.283 1.2609 1.2262 1.2077 1.1695 1.1525 1.1142 1.0875 1.0507 1.0364 1.0001 0.9728 0.9357 0.9221 0.8853 0.8653 0.8304 0.8108 0.7783 0.7532 0.7187 0.6835 0.6459 0.5992 0.565 0.5088 0.4745 0.4122 0.3604 0.3089 0.1617 0.1185 0.0507 -0.011 -0.0759 -0.1515 -0.1957 -0.2371 -0.2663 -0.2905 -0.3247 -0.3696 -0.4185 -0.4667 -0.5055 -0.5514 -0.5802 -0.6096 -0.6466 -0.6756 -0.7092 -0.7393 -0.7844 -0.8069 -0.8484 -0.8803 -0.9178 -0.948 -0.9926 -1.0072 -1.0506 -1.058 -1.0947 -1.1187 -1.1497 -1.1709 -1.2058 -1.2164 -1.248 -1.2752 -1.3067 -1.3234 -1.3508 -1.3617 -1.3881 -1.3982 -1.4208 -1.433 -1.4508 -1.4868 -1.5016 -1.5044 -1.5132 -1.5168 -1.5205 -1.5244 -1.5266 -1.5267 -1.5239 -1.5224 -1.5174 -1.5138 -1.5058 -1.5001 -1.4861 -1.4792 -1.4229 -1.4195 -1.3767 -1.3666 -1.3305 -1.3073 -1.274 -1.2576 -1.2295 -1.2062 -1.1755 -1.1537 -1.1179 -1.0929 -1.0568 -1.0371 -1.001 -0.9768 -0.9402 -0.9268 -0.8881 -0.8383 -0.7997 -0.7473 -0.7038 -0.6403 -0.6063 -0.5042 -0.4675 -0.4151 -0.383 -0.347 -0.3276 -0.2969 -0.2759 -0.2491 -0.2265 -0.201 -0.1883 -0.1316 -0.1206 -0.1005 -0.074 -0.0602 -0.0169 0.0015 0.0385 0.055 0.0977 0.1118 0.1518 0.1849 0.232 0.2712 0.3048 0.3538 0.3953 0.4518 0.4925 0.542 0.5825 0.6456 0.6921 0.7573 0.8018 0.9188 0.9514 1.0051 1.0343 1.0601 1.0831 1.1077 1.1255 1.1482 1.1675 1.19 1.2019 1.2174 1.2315 1.2475 1.2589 1.2697 1.2766 1.2846 1.2889 1.2929 1.2946 1.295 1.2934 1.2916 1.2909 1.2916 1.2954 1.2996 1.3063 1.3153 1.3264 1.3346 1.3483 1.3606 1.378 1.3931 1.41 1.4238 1.4456 1.4606 1.4746 1.4813 1.4953 ];
x_outer=[-2.0865 -2.1093 -2.1348 -2.1528 -2.1751 -2.1933 -2.2195 -2.2359 -2.2647 -2.2829 -2.3148 -2.3332 -2.3587 -2.3704 -2.3842 -2.3971 -2.4102 -2.422 -2.4441 -2.4493 -2.461 -2.4685 -2.4766 -2.4813 -2.4885 -2.4908 -2.4952 -2.4943 -2.4963 -2.4948 -2.4944 -2.4934 -2.4875 -2.484 -2.4802 -2.4739 -2.4641 -2.4108 -2.3823 -2.3618 -2.3302 -2.3107 -2.2782 -2.26 -2.2529 -2.2396 -2.1985 -2.1752 -2.1408 -2.0958 -2.0759 -2.0353 -1.9693 -1.937 -1.8997 -1.8788 -1.8503 -1.8233 -1.7778 -1.7469 -1.7106 -1.6838 -1.621 -1.5917 -1.546 -1.5005 -1.4553 -1.4278 -1.3983 -1.3733 -1.3326 -1.3044 -1.2655 -1.2393 -1.1937 -1.1682 -1.1329 -1.1077 -1.0792 -1.0521 -1.0084 -0.9858 -0.9564 -0.9396 -0.9075 -0.8856 -0.8575 -0.8363 -0.8105 -0.7909 -0.7633 -0.7468 -0.728 -0.7144 -0.7005 -0.6875 -0.6682 -0.6567 -0.6446 -0.6354 -0.6268 -0.6218 -0.6296 -0.6132 -0.6128 -0.5814 -0.56 -0.5264 -0.4728 -0.4499 -0.4382 -0.3977 -0.3336 -0.3135 -0.2747 -0.2339 -0.1953 -0.1526 -0.1133 -0.0757 -0.0381 0.0019 0.0412 0.0729 0.2032 0.2433 0.3004 0.3395 0.3985 0.448 0.4794 0.507 0.5116 0.5358 0.5481 0.5812 0.6068 0.6473 0.6714 0.7105 0.7312 0.7642 0.7829 0.8154 0.838 0.8735 0.891 0.9192 0.9464 0.9805 0.9971 1.0218 1.0521 1.0818 1.111 1.1382 1.1825 1.2124 1.2548 1.2833 1.3258 1.3649 1.3985 1.4302 1.4826 1.5193 1.5709 1.6079 1.6735 1.7141 1.7768 1.808 1.8507 1.8762 1.9157 1.9465 1.9894 2.0199 2.0674 2.0984 2.1383 2.1664 2.2394 2.2607 2.2894 2.3128 2.3474 2.3688 2.3967 2.4156 2.4358 2.4499 2.4699 2.483 2.4957 2.507 2.5193 2.5253 2.5299 2.5343 2.5374 2.5368 2.535 2.5317 2.5241 2.5161 2.5051 2.4939 2.4797 2.4659 2.4482 2.4312 2.4154 2.3971 2.3855 2.3716 2.3659 2.3575 2.3542 2.3495 2.3466 2.3484 2.3502 2.3553 2.3607 2.3738 2.3858 2.3994 2.408 2.4184 2.4273 2.4398 2.4496 2.4723 2.4833 2.5058 2.5264 2.5448 2.5736 2.5937 2.6181 2.6337 2.6581 2.6739 2.6913 2.6999 2.7097 2.7138 2.7182 2.7198 2.7184 2.7149 2.6973 2.6838 2.67 2.657 2.6392 2.6221 2.5974 2.5767 2.5449 2.5205 2.4874 2.462 2.427 2.3969 2.359 2.3272 2.2789 2.2457 2.119 2.073 2.0364 1.9991 1.9451 1.9143 1.8692 1.828 1.7654 1.7287 1.6851 1.6542 1.6117 1.5753 1.5208 1.4799 1.4214 1.3131 1.2697 1.2112 1.165 1.1291 1.0829 1.0574 1.0282 1.0108 0.991 0.9735 0.9502 0.9342 0.9135 0.9017 0.8872 0.8803 0.872 0.8676 0.8632 0.8649 0.8642 0.8635 0.8624 0.8568 0.8506 0.8419 0.8494 0.8198 0.7962 0.761 0.7401 0.7138 0.6997 0.6621 0.6357 0.5862 0.5602 0.5174 0.3436 0.3217 0.2703 0.2239 0.1426 0.1036 0.0586 0.0388 0.0158 0.0002 -0.0123 -0.0222 -0.0301 -0.0403 -0.051 -0.0576 -0.0638 -0.0745 -0.0876 -0.0977 -0.1095 -0.1202 -0.1317 -0.1482 -0.1604 -0.1832 -0.2195 -0.2494 -0.2704 -0.3095 -0.3279 -0.3645 -0.391 -0.4234 -0.453 -0.4941 -0.5258 -0.5594 -0.6028 -0.6444 -0.6777 -0.7224 -0.7557 -0.7956 -0.8335 -0.8773 -0.9126 -0.958 -0.991 -1.0487 -1.0991 -1.1474 -1.2068 -1.257 -1.3187 -1.3553 -1.4109 -1.4634 -1.5189 -1.565 -1.6189 -1.6668 -1.7144 -1.7561 -1.8073 -1.8517 -1.8701 -1.9161 -1.9736 -2.0119 ];
y_outer=[1.2505 1.2658 1.284 1.2971 1.3144 1.3287 1.3505 1.3653 1.3937 1.4127 1.4523 1.4781 1.5154 1.5359 1.5598 1.5839 1.6132 1.6379 1.6968 1.7191 1.7419 1.7695 1.8074 1.8322 1.8753 1.9036 1.9478 1.9733 2 2.0262 2.0499 2.0783 2.1203 2.1443 2.1775 2.2101 2.2458 2.4008 2.4591 2.4936 2.5389 2.5649 2.6046 2.6232 2.6312 2.6471 2.6862 2.7062 2.7341 2.7664 2.7795 2.8067 2.8416 2.8569 2.8729 2.8804 2.8908 2.8994 2.9114 2.918 2.9242 2.9287 2.9351 2.938 2.9386 2.9361 2.931 2.9274 2.9232 2.9186 2.91 2.9023 2.8906 2.8819 2.8635 2.8517 2.8341 2.8209 2.804 2.7885 2.7588 2.7413 2.7183 2.7036 2.6752 2.6531 2.6227 2.5991 2.5655 2.5401 2.4997 2.4715 2.4383 2.4119 2.3847 2.3555 2.3072 2.2715 2.2302 2.1919 2.1543 2.1244 2.1536 2.1286 2.1301 2.1011 2.0793 2.0519 2.0261 2.0139 2.0111 1.9991 1.9818 1.9761 1.965 1.9503 1.9371 1.9204 1.9053 1.8901 1.8753 1.8588 1.8411 1.8261 1.7618 1.7396 1.7083 1.6867 1.6527 1.624 1.6054 1.5929 1.5925 1.5857 1.5827 1.5757 1.5705 1.566 1.5628 1.5628 1.5622 1.5669 1.5683 1.5764 1.5833 1.5948 1.6025 1.615 1.6277 1.645 1.6545 1.6705 1.6889 1.7071 1.7279 1.7471 1.7751 1.7927 1.8159 1.8295 1.8504 1.8679 1.8835 1.8965 1.9152 1.9264 1.9386 1.9452 1.9506 1.95 1.94 1.9338 1.9229 1.9156 1.9021 1.8919 1.8729 1.8594 1.8333 1.8152 1.7892 1.7695 1.7071 1.6856 1.6554 1.6303 1.587 1.5577 1.5143 1.4813 1.4418 1.4125 1.3667 1.3321 1.293 1.2548 1.1995 1.1612 1.1227 1.0858 1.0311 0.9948 0.9463 0.9093 0.8583 0.8215 0.7733 0.7385 0.6939 0.6614 0.6209 0.5863 0.5527 0.5151 0.4866 0.4524 0.4373 0.4029 0.3851 0.3333 0.3124 0.1652 0.1408 0.0729 0.0482 -0.0168 -0.063 -0.1072 -0.1368 -0.1659 -0.1876 -0.2218 -0.2432 -0.2921 -0.3163 -0.3551 -0.3901 -0.4189 -0.4663 -0.5032 -0.5496 -0.5831 -0.6401 -0.6853 -0.746 -0.7875 -0.8477 -0.8853 -0.9372 -0.9818 -1.0314 -1.0748 -1.1613 -1.198 -1.2355 -1.2664 -1.3034 -1.3383 -1.3808 -1.4123 -1.4587 -1.4902 -1.5273 -1.5547 -1.5868 -1.6133 -1.6426 -1.6652 -1.6974 -1.7152 -1.7724 -1.7872 -1.7965 -1.8053 -1.8146 -1.8184 -1.8239 -1.8261 -1.8258 -1.823 -1.8186 -1.8137 -1.8069 -1.7989 -1.7838 -1.7697 -1.7453 -1.689 -1.6616 -1.6188 -1.5783 -1.5422 -1.4893 -1.456 -1.4158 -1.3877 -1.3543 -1.3236 -1.2761 -1.2403 -1.1861 -1.1499 -1.0938 -1.0578 -1.0126 -0.976 -0.9138 -0.8751 -0.8441 -0.8056 -0.7854 -0.7419 -0.7146 -0.6806 -0.6927 -0.656 -0.6365 -0.6044 -0.5882 -0.5688 -0.5587 -0.5377 -0.5221 -0.4995 -0.4884 -0.4757 -0.3997 -0.3887 -0.3611 -0.3345 -0.2609 -0.2176 -0.1398 -0.1029 -0.0478 -0.0051 0.0401 0.0802 0.1213 0.1684 0.2133 0.247 0.2791 0.3206 0.3796 0.4203 0.4654 0.5059 0.5453 0.5918 0.6205 0.665 0.7159 0.7484 0.7647 0.7939 0.8062 0.8292 0.8447 0.8625 0.8768 0.8961 0.9073 0.9192 0.9332 0.9473 0.9568 0.9682 0.974 0.9809 0.9861 0.9904 0.9931 0.9949 0.9952 0.9935 0.9917 0.991 0.9925 0.9963 1.0045 1.0111 1.0218 1.0329 1.047 1.0607 1.0787 1.0961 1.1151 1.132 1.1544 1.1762 1.1736 1.1876 1.1995 1.2135 ];
x_wp=[-2.0028 -2.0256 -2.0466 -2.0646 -2.0828 -2.101 -2.1192 -2.1356 -2.1565 -2.1748 -2.1925 -2.2109 -2.2285 -2.2402 -2.2519 -2.2647 -2.2749 -2.2868 -2.298 -2.3031 -2.3162 -2.3237 -2.3293 -2.334 -2.339 -2.3413 -2.3453 -2.3444 -2.3465 -2.345 -2.3445 -2.3435 -2.3391 -2.3356 -2.3329 -2.3266 -2.3222 -2.2689 -2.2533 -2.2328 -2.2103 -2.1907 -2.1709 -2.1527 -2.1381 -2.1247 -2.101 -2.0777 -2.0535 -2.0084 -1.9923 -1.9517 -1.9048 -1.8725 -1.849 -1.828 -1.8049 -1.778 -1.7462 -1.7153 -1.6856 -1.6588 -1.6062 -1.577 -1.5542 -1.5087 -1.4747 -1.4473 -1.4256 -1.4006 -1.3721 -1.344 -1.3129 -1.2867 -1.2567 -1.2312 -1.2021 -1.1769 -1.1541 -1.1271 -1.0999 -1.0773 -1.0551 -1.0382 -1.014 -0.9921 -0.969 -0.9478 -0.9293 -0.9096 -0.8927 -0.8762 -0.8615 -0.8479 -0.8376 -0.8247 -0.811 -0.7994 -0.7904 -0.7812 -0.7748 -0.7698 -0.755 -0.7387 -0.7144 -0.6829 -0.6548 -0.6211 -0.5434 -0.5205 -0.4809 -0.4404 -0.3748 -0.3547 -0.3257 -0.2849 -0.2499 -0.2072 -0.1696 -0.132 -0.0953 -0.0554 -0.0233 0.0083 0.1307 0.1708 0.2279 0.267 0.3234 0.3729 0.4175 0.4451 0.4711 0.4953 0.5172 0.5503 0.5901 0.6307 0.6711 0.7103 0.7527 0.7857 0.8192 0.8516 0.8842 0.9197 0.9518 0.98 1.0142 1.0483 1.0786 1.1033 1.1307 1.1603 1.1975 1.2247 1.2585 1.2884 1.3195 1.3479 1.3871 1.4263 1.4554 1.4871 1.5265 1.5632 1.5972 1.6342 1.6713 1.7119 1.7472 1.7783 1.8093 1.8348 1.8685 1.8993 1.9288 1.9593 1.9917 2.0227 2.0522 2.0803 2.1328 2.1541 2.1796 2.203 2.2262 2.2476 2.2665 2.2853 2.3005 2.3146 2.3295 2.3426 2.3519 2.3632 2.3711 2.3772 2.381 2.3853 2.3875 2.3868 2.3856 2.3823 2.3775 2.3695 2.3624 2.3511 2.3416 2.3278 2.3136 2.2966 2.2803 2.2621 2.2464 2.2326 2.2202 2.2118 2.2049 2.2002 2.1967 2.1984 2.2007 2.2057 2.2137 2.2267 2.2424 2.2561 2.2667 2.277 2.2864 2.2988 2.3135 2.3362 2.3535 2.3761 2.4 2.4183 2.4418 2.4619 2.482 2.4976 2.5165 2.5323 2.5444 2.553 2.5606 2.5647 2.5683 2.5699 2.5689 2.5654 2.5565 2.543 2.5319 2.5188 2.5046 2.4875 2.4719 2.4512 2.4262 2.4018 2.3774 2.352 2.3278 2.2978 2.272 2.2402 2.2081 2.1749 2.0732 2.0271 2.0022 1.9649 1.9272 1.8964 1.8613 1.82 1.7771 1.7403 1.7089 1.6779 1.6438 1.6074 1.5696 1.5287 1.4906 1.3823 1.3583 1.2997 1.2713 1.2353 1.2021 1.1767 1.1557 1.1382 1.1214 1.104 1.0872 1.0712 1.0561 1.0443 1.0345 1.0276 1.021 1.0166 1.013 1.0147 1.0142 1.0135 1.0112 1.0056 0.9959 0.9872 0.9661 0.9364 0.8974 0.8622 0.8293 0.803 0.7729 0.7353 0.6979 0.6484 0.6032 0.5604 0.4109 0.389 0.3447 0.2982 0.254 0.215 0.1909 0.1711 0.1567 0.1411 0.1333 0.1235 0.1165 0.1063 0.0962 0.0896 0.0814 0.0708 0.0579 0.0478 0.0356 0.0249 0.0096 -0.0069 -0.0269 -0.0497 -0.109 -0.1389 -0.1807 -0.2198 -0.248 -0.2846 -0.3187 -0.3512 -0.3891 -0.4301 -0.4756 -0.5092 -0.5547 -0.5964 -0.6406 -0.6853 -0.7303 -0.7702 -0.8186 -0.8624 -0.9069 -0.9523 -0.9953 -1.0531 -1.1013 -1.1496 -1.1955 -1.2457 -1.2919 -1.3285 -1.3798 -1.4324 -1.4763 -1.5224 -1.5676 -1.6154 -1.658 -1.6997 -1.7413 -1.7857 -1.8264 -1.8724 -1.9222 -1.9605 ];
y_wp=[1.375 1.3903 1.4053 1.4184 1.4327 1.4469 1.462 1.4768 1.4976 1.5166 1.5391 1.5649 1.5899 1.6103 1.6304 1.6546 1.6781 1.7028 1.7306 1.7529 1.7813 1.8089 1.8355 1.8603 1.8877 1.9159 1.9427 1.9682 1.9914 2.0176 2.0449 2.0733 2.0985 2.1225 2.1489 2.1814 2.197 2.352 2.3824 2.4169 2.4488 2.4748 2.4997 2.5183 2.5347 2.5507 2.5723 2.5922 2.6122 2.6445 2.6549 2.6821 2.7061 2.7215 2.7317 2.7393 2.7478 2.7564 2.7647 2.7714 2.7763 2.7808 2.7858 2.7887 2.7889 2.7863 2.7822 2.7787 2.7757 2.7711 2.7653 2.7576 2.7482 2.7395 2.7273 2.7155 2.701 2.6879 2.6741 2.6585 2.6399 2.6225 2.6053 2.5906 2.5695 2.5475 2.5224 2.4988 2.4738 2.4484 2.4239 2.3957 2.3699 2.3435 2.324 2.2948 2.2612 2.2255 2.1951 2.1567 2.1299 2.1 2.0715 2.0465 2.0197 1.9908 1.963 1.9356 1.8937 1.8815 1.8673 1.8553 1.8376 1.8319 1.8239 1.8092 1.7974 1.7807 1.7663 1.7511 1.7366 1.7201 1.7057 1.6907 1.6305 1.6083 1.577 1.5554 1.5228 1.4942 1.4688 1.4563 1.448 1.4412 1.4359 1.4289 1.4215 1.4169 1.4128 1.4128 1.4137 1.4185 1.4227 1.4308 1.4406 1.4521 1.4654 1.4779 1.4939 1.5112 1.5286 1.5446 1.5611 1.5794 1.6054 1.6246 1.6458 1.6634 1.6805 1.6941 1.7135 1.7311 1.7448 1.7578 1.7717 1.7829 1.7909 1.7975 1.8006 1.8 1.793 1.7867 1.7788 1.7714 1.7597 1.7495 1.7356 1.7222 1.7038 1.6857 1.6664 1.6467 1.6016 1.5801 1.5532 1.528 1.4986 1.4693 1.4399 1.4069 1.3771 1.3477 1.3138 1.2791 1.2504 1.2122 1.176 1.1377 1.1051 1.0683 1.0337 0.9974 0.9596 0.9225 0.8902 0.8534 0.8193 0.7844 0.7524 0.7198 0.687 0.6525 0.6181 0.5805 0.5429 0.5087 0.473 0.4387 0.3986 0.3468 0.3107 0.1635 0.1297 0.0618 0.0186 -0.0464 -0.1073 -0.1514 -0.187 -0.2161 -0.239 -0.2732 -0.3064 -0.3553 -0.3915 -0.4303 -0.4708 -0.4995 -0.5379 -0.5749 -0.6126 -0.6462 -0.6897 -0.7349 -0.7764 -0.818 -0.864 -0.9016 -0.9426 -0.9872 -1.0193 -1.0627 -1.1097 -1.1464 -1.1771 -1.208 -1.2372 -1.272 -1.2986 -1.3302 -1.3669 -1.3985 -1.4254 -1.4528 -1.4742 -1.5007 -1.5204 -1.543 -1.5652 -1.583 -1.6296 -1.6444 -1.6505 -1.6592 -1.6657 -1.6694 -1.6741 -1.6763 -1.6763 -1.6734 -1.6705 -1.6656 -1.6604 -1.6524 -1.642 -1.6279 -1.6123 -1.556 -1.5406 -1.4977 -1.4724 -1.4363 -1.3983 -1.365 -1.3367 -1.3086 -1.2803 -1.2496 -1.2149 -1.1791 -1.1395 -1.1033 -1.0655 -1.0294 -0.9947 -0.9581 -0.9203 -0.8816 -0.8412 -0.8026 -0.7663 -0.7228 -0.6774 -0.6435 -0.5985 -0.5618 -0.5258 -0.4937 -0.4676 -0.4482 -0.4278 -0.4068 -0.3856 -0.363 -0.3447 -0.332 -0.2657 -0.2547 -0.2308 -0.2043 -0.1606 -0.1172 -0.0692 -0.0322 0.0036 0.0463 0.076 0.116 0.1531 0.2002 0.2423 0.2759 0.3164 0.358 0.4157 0.4564 0.5037 0.5442 0.5954 0.642 0.6889 0.7334 0.8174 0.8499 0.8849 0.9141 0.9332 0.9562 0.9762 0.994 1.0125 1.0318 1.0486 1.0606 1.0753 1.0894 1.1022 1.1136 1.1219 1.1287 1.1353 1.1397 1.143 1.1447 1.1451 1.1434 1.1417 1.1409 1.142 1.1458 1.152 1.1587 1.1685 1.1797 1.1908 1.2045 1.2197 1.2371 1.2541 1.271 1.2891 1.3109 1.3171 1.3311 1.3404 1.3544 ];
Right_curb=[x_inner;y_inner]';
Left_curb=[x_outer;y_outer]';
Driver_pose=[x_wp;y_wp]';
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Find the angles between each waypoint
yaw=zeros(size(Driver_pose,1),1);
for i=1:size(Driver_pose,1)-1
yaw(i,1)=atan2((Driver_pose(i+1,2)-(Driver_pose(i,2))),(Driver_pose(i+1,1))-(Driver_pose(i,1)));
end
i=size(Driver_pose,1);
yaw(i,1)=atan2((Driver_pose(1,2)-(Driver_pose(i,2))),(Driver_pose(1,1))-(Driver_pose(i,1)));
Driver_pose=[Driver_pose, yaw];
Camera_width=3*pi/4;
rand_a=-1;
rand_b=2;
desired_theta=zeros(size(Driver_pose,1),1);
error_theta=zeros(size(Driver_pose,1),1);
rad_vis=1.2;
Frames = [1:size(Driver_pose,1) ]; %one more round add: 1:size(Driver_pose,1)
nFrames = length(Frames);
% Preallocate movie structure.
mov(1:nFrames) = struct('cdata', [], 'colormap', []);
xlabel('x [mts]')
ylabel('y [mts]')
title('Lane detection simulation in a known map')
grid on
for k = Frames
%for k=10;
%tic
%for k=2/arc_length
%L_curve_detected=[];
%R_curve_detected=[];
car_distance=mod(k+3.5*(rad_vis)/arc_length,nFrames)+1;
rand_k = car_distance+round(rand_a + (rand_b-rand_a)*rand(1,1));
rand_k = car_distance;
%if rand_k >= size(Driver_pose,1)
% rand_k = size(Driver_pose,1);
%end
Front_vehicle=Driver_pose(round(rand_k),:);
%axis tight
axis([-3 3 -3 3])
set(gca,'nextplot','replacechildren');
%hold off
%plot(0,0,'w')
%hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%% Plot
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
R_p=plot(Right_curb(:,1),Right_curb(:,2),'red.');
hold on
L_p=plot(Left_curb(:,1),Left_curb(:,2),'blue.');
hold on
W_p=plot(Driver_pose(:,1),Driver_pose(:,2),'black:');
hold on
%k=10;
K_p=plot(Driver_pose(k,1),Driver_pose(k,2),'green*');
hold on
F_p=plot(Front_vehicle(1,1),Front_vehicle(1,2),'magenta*');
hold on
C_p=plot(Driver_pose(k,1)+rad_vis*cos(Driver_pose(k,3) + Camera_width/2),Driver_pose(k,2)+rad_vis*sin(Driver_pose(k,3) + Camera_width/2),'blue.');
hold on
plot(Driver_pose(k,1)+rad_vis*cos(Driver_pose(k,3) - Camera_width/2),Driver_pose(k,2)+rad_vis*sin(Driver_pose(k,3) - Camera_width/2),'blue.')
hold on
%Plot f1,f2: Left and right border of the vision
X1=Driver_pose(k,1)+rad_vis*cos(-Camera_width/2+Driver_pose(k,3));
X0=Driver_pose(k,1);
Y1=Driver_pose(k,2)+rad_vis*sin(-Camera_width/2+Driver_pose(k,3));
Y0=Driver_pose(k,2);
if (X1 > X0)
Bx_min=X0;
Bx_max=X1;
By_min=Y0;
By_max=Y1;
else
Bx_min=X1;
Bx_max=X0;
By_min=Y1;
By_max=Y0;
end
PlotX=Bx_min:0.001:Bx_max;
PlotY=By_min:(By_max-By_min)/length(PlotX):By_max-(By_max-By_min)/length(PlotX);
Plotted1=[PlotX;PlotY]';
plot(PlotX,PlotY,'blue')
X1_L=Driver_pose(k,1)+rad_vis*cos(Camera_width/2+Driver_pose(k,3));
Y0=Driver_pose(k,2);
Y2=Driver_pose(k,2)+rad_vis*sin(Camera_width/2+Driver_pose(k,3));
if (X0 > X1_L)
Bx_min_L=X1_L;
Bx_max_L=X0;
By_min_L=Y2;
By_max_L=Y0;
else
Bx_min_L=X0;
Bx_max_L=X1_L;
By_min_L=Y0;
By_max_L=Y2;
end
PlotX=Bx_min_L:0.001:Bx_max_L;
PlotY=By_min_L:(By_max_L-By_min_L)/length(PlotX):By_max_L-(By_max_L-By_min_L)/length(PlotX);
Plotted2=[PlotX;PlotY]';
plot(PlotX,PlotY,'blue')
hold on
%Plot f3: Curve section of the cameras vision "the horizon"
% for i=[-Camera_width/2:0.01:Camera_width/2 Camera_width/2]
% Bx=Driver_pose(k,1)+rad_vis*cos(-i+(Driver_pose(k,3)));
% By=Driver_pose(k,2)+rad_vis*sin(-i+(Driver_pose(k,3)));
% plot(Bx,By,'blue')
% hold on
% end
i=[-Camera_width/2:0.01:Camera_width/2 Camera_width/2];
Bx=Driver_pose(k,1)+rad_vis*cos(-i+(Driver_pose(k,3)));
By=Driver_pose(k,2)+rad_vis*sin(-i+(Driver_pose(k,3)));
plot(Bx,By,'blue')
hold on
%Map of the points of the detected features (curbs or lane markers) in the x-y plane
%on the left side
L1_idx=0;
j_all=1:size(Left_curb,1);
L_distance_jk=sqrt((Left_curb(j_all,1)-Driver_pose(k,1)).^2+(Left_curb(j_all,2)-Driver_pose(k,2)).^2);
j_dist=find( (L_distance_jk(j_all,1) < rad_vis) );
%all possible angles
theta_temp=atan2(Left_curb(j_all,2)-Driver_pose(k,2),Left_curb(j_all,1)-Driver_pose(k,1))-(Driver_pose(k,3));
j_minus=find( theta_temp(j_all,1) < -pi );
theta_temp(j_minus,1) = theta_temp(j_minus,1)+ 2*pi;
j_plus=find( theta_temp(j_all,1) > pi );
theta_temp(j_plus,1) = theta_temp(j_plus,1)- 2*pi;
%feasible angles within distance
j_theta=find(abs (theta_temp(j_all,1)) < (Camera_width/2) );
common_idx=intersect(j_theta,j_dist);
Ld_p=plot(Left_curb(common_idx,1),Left_curb(common_idx,2),'cyan*');
hold on
L_curve_detected=[Left_curb(common_idx,1),Left_curb(common_idx,2),L_distance_jk(common_idx,1)];
%Map of the points of the detected features (curbs or lane markers) in the x-y plane
%on the left side
j_all=1:size(Right_curb,1);
R_distance_jk=sqrt((Right_curb(j_all,1)-Driver_pose(k,1)).^2+(Right_curb(j_all,2)-Driver_pose(k,2)).^2);
j_dist=find( (R_distance_jk(j_all,1) < rad_vis) );
%all possible angles
theta_temp=atan2(Right_curb(j_all,2)-Driver_pose(k,2),Right_curb(j_all,1)-Driver_pose(k,1))-(Driver_pose(k,3));
j_minus=find( theta_temp(j_all,1) < -pi );
theta_temp(j_minus,1) = theta_temp(j_minus,1)+ 2*pi;
j_plus=find( theta_temp(j_all,1) > pi );
theta_temp(j_plus,1) = theta_temp(j_plus,1)- 2*pi;
%feasible angles within distance
j_theta=find(abs (theta_temp(j_all,1)) < (Camera_width/2) );
common_idx=intersect(j_theta,j_dist);
Rd_p=plot(Right_curb(common_idx,1),Right_curb(common_idx,2),'green*');
hold on
R_curve_detected=[Right_curb(common_idx,1),Right_curb(common_idx,2),R_distance_jk(common_idx,1)];
%Sort rows depending on distance
L_curve_detected=sortrows(L_curve_detected,3);
R_curve_detected=sortrows(R_curve_detected,3);
%Check if one of the curves is empty
min_size_curve_det=5; %Or maybe the half of the other one
%Check the size of the shortest curve
if (size(R_curve_detected,1)<size(L_curve_detected,1))
n_shorter_curve = size(R_curve_detected,1);
if ((isempty(R_curve_detected)) || (size(R_curve_detected,1) < min_size_curve_det))
R_curve_detected = L_curve_detected*((lane_width)^2);
n_shorter_curve = size(R_curve_detected,1);
end
else
n_shorter_curve = size(L_curve_detected,1);
if ((isempty(L_curve_detected)) || (size(L_curve_detected,1) < min_size_curve_det))
L_curve_detected = R_curve_detected/((lane_width)^2);
n_shorter_curve = size(L_curve_detected,1);
end
end
%Sorting the left and right curves depending on distance
if (isempty(n_shorter_curve))
Trajectory_estimator = [0,0];
完整代码添加QQ1575304183
上一篇： 2012年5月阿里巴巴集团”去 IOE”运动的思考_MySQL
下一篇： Java连接MySQL
推荐阅读