雷达墙位置模拟仿真

lidar_x_offset=[54 44 -54 -44 27 10 -27 -10 54 44 -54 -44 27 10 -27 -16 -10 -10 -10 -10 10 10 10 10
];
lidar_y_offset=[28 14 28 14 3 0 3 0 -28 -14 -28 -14 -3 0 -3 0 40 60 190 210 40 60 190 210];
STITCH_AVM_WIDTH=350;
STITCH_AVM_HEIGHT=550;
STITCH_CAR_WIDTH=100;
STITCH_CAR_HEIGHT=250;
angle = [ 35 45 145 135 75 85 105 95 325 315 215 225 285 275 255 265 180 180 180 180 0 0 0 0];
midx = STITCH_AVM_WIDTH / 2;
midy = STITCH_AVM_HEIGHT / 2;
lidar_distance=0;
lidar_x=zeros(4);
lidar_y=zeros(4);
lidar_z=zeros(4);

car_x=[STITCH_AVM_WIDTH / 2-STITCH_CAR_WIDTH/2 STITCH_AVM_WIDTH / 2+STITCH_CAR_WIDTH/2 STITCH_AVM_WIDTH / 2-STITCH_CAR_WIDTH/2 STITCH_AVM_WIDTH / 2+STITCH_CAR_WIDTH/2];
car_y=[(STITCH_AVM_HEIGHT-STITCH_CAR_HEIGHT)/2,(STITCH_AVM_HEIGHT-STITCH_CAR_HEIGHT)/2,(STITCH_AVM_HEIGHT+STITCH_CAR_HEIGHT)/2,(STITCH_AVM_HEIGHT+STITCH_CAR_HEIGHT)/2];
plot(car_x,car_y);
hold on
k=1;
for i=1:24
k=1;
if i<=8
lidpos_x= STITCH_AVM_WIDTH / 2 + lidar_x_offset(i);
lidpos_y= (STITCH_AVM_HEIGHT-STITCH_CAR_HEIGHT)/2-20+ lidar_y_offset(i);
elseif i>8 && i<=16
lidpos_x= STITCH_AVM_WIDTH / 2 + lidar_x_offset(i);
lidpos_y= (STITCH_AVM_HEIGHT+STITCH_CAR_HEIGHT)/2+20+ lidar_y_offset(i);
elseif i>16 && i<=20
lidpos_x= (STITCH_AVM_WIDTH-STITCH_CAR_WIDTH) / 2 + lidar_x_offset(i);
lidpos_y= (STITCH_AVM_HEIGHT-STITCH_CAR_HEIGHT)/2+ lidar_y_offset(i);
elseif i>20 && i<=24
lidpos_x= (STITCH_AVM_WIDTH+STITCH_CAR_WIDTH) / 2 + lidar_x_offset(i);
lidpos_y= (STITCH_AVM_HEIGHT-STITCH_CAR_HEIGHT)/2+ lidar_y_offset(i);
end

tmp_x=lidpos_x +lidar_distance / 20 * cos(angle(i)*(3.141592 / 180.0));
tmp_y=lidpos_y -lidar_distance / 20 * sin(angle(i)*(3.141592 / 180.0));
        
wall_width_base = 9;
wall_tickness = 300; 
wall_width = wall_width_base + 0.006 *lidar_distance;
tmp1_x=tmp_x - wall_width * sin(angle(i)*(3.141592 / 180.0));
tmp1_y=tmp_y - wall_width * cos(angle(i)*(3.141592 / 180.0));
tmp2_x=tmp_x + wall_width * sin(angle(i)*(3.141592 / 180.0));
tmp2_y=tmp_y + wall_width * cos(angle(i)*(3.141592 / 180.0));
lidar_x(k)=tmp1_x;
lidar_y(k)=tmp1_y;
k=k+1;
lidar_x(k)=tmp2_x;
lidar_y(k)=tmp2_y;
k=k+1;

tmp_out_x=lidpos_x + (lidar_distance + wall_tickness) / 20 * cos(angle(i)*(3.141592 / 180.0));
tmp_out_y=lidpos_y - (lidar_distance + wall_tickness) / 20 *  sin(angle(i)*(3.141592 / 180.0));
tmp3_x=tmp_out_x - wall_width * sin(angle(i)*(3.141592 / 180.0));
tmp3_y=tmp_out_y - wall_width * cos(angle(i)*(3.141592 / 180.0));
tmp4_x=tmp_out_x + wall_width * sin(angle(i)*(3.141592 / 180.0));
tmp4_y=tmp_out_y + wall_width * cos(angle(i)*(3.141592 / 180.0));
lidar_x(k)=tmp3_x;
lidar_y(k)=tmp3_y;
k=k+1;
lidar_x(k)=tmp4_x;
lidar_y(k)=tmp4_y;
k=k+1;
lidar_x(k)=tmp1_x;
lidar_y(k)=tmp1_y;
k=k+1;
plot(lidar_x,lidar_y);
hold on

end