欢迎您访问程序员文章站本站旨在为大家提供分享程序员计算机编程知识!
您现在的位置是: 首页

【物理应用】车道检测仿真

程序员文章站 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

相关标签: matlab 物理应用