matlab模拟2d激光雷达生成目标量测点

lidar = SetLidarParameters_2d; % 激光雷达 rectangleLenght = 6; % 长方形长 rectangleWidth = 4; % 长方形宽 a1 = 80;b1 = 80;r1 = 50; % 第一个圆参数 a2 = 180;b2 = 80;r2 = 50; % 第二个圆参数 theta = pi:-pi/25:-pi; % 每个圆产生51个点 x1 = a1 + r1 * cos(theta); y1 = b1 + r1 * sin(theta); x2 = a2 + r2 * cos(theta); y2 = b2 + r2 * sin(theta); % 通过截取获得倒8字型轨迹 x = [x1(1:25) fliplr(x2) x1(27:end)]; % x轨迹 y = [y1(1:25) fliplr(y2) y1(27:end)]; % y轨迹 % 画轨迹点 % hold on; % plot(x,y,'.'); % axis equal % 画所有竖着的轨迹长方形 % for i=1:100 % rectangle('Position',[x(i)-rectangleWidth/2 , y(i)-rectangleLenght/2,rectangleWidth,rectangleLenght]) % end % hold on; rectangEquation = cell(100,1); % 100个时刻 的四个线段 pointSet = cell(100,1); % 100个时刻 的交点集 allKK = zeros(100,1); % 所有切线的k for i=1:100 % i=14时出现问题(求出来k为负的)：已解决 pointSet{i,1} = cell(100,1); % 雷达100个射线 与长方形四条线段的最近一个交点 % 求四条线段 if i<=25 || i>=76 % 在第一个圆上 % 切线公式(x(1,i) - a1) * (x - a1) + (y(1,i) - b1) * (y - b1) = r1^2; a = a1; b = b1; r = r1; else % 在第二个圆上 % (x(1,i) - a2) * (x - a2) + (y(1,i) - b2) * (y - b2) = r2^2; a = a2; b = b2; r = r2; end A = x(1,i) - a; B = y(1,i) - b; C = r^2; type = 0; dingDian = zeros(4,2); %四个顶点 allKK(i) = -A/B; if allKK(i) == -inf allKK(i) = inf; end infNum = 0; if B ~= 0 type = 0; kk = -A/B; % 切线的k bb = (C+A*a+B*b)/B; % 切线的b topOrbottom_bb = (a * y(1,i) - x(1,i) * b) / (a - x(1,i)); % 切点与圆心连线 的b （两点确认一条直线） % 下面的b 通过两平行线之间距离求出 n = bb + rectangleWidth/2 * sqrt(kk^2 + 1); % 左线段的b nn = bb - rectangleWidth/2 * sqrt(kk^2 + 1); % 右线段的b if kk > 0 m = topOrbottom_bb + rectangleLenght/2 * sqrt((-1/kk)^2 + 1); % 上线段的b mm = topOrbottom_bb - rectangleLenght/2 * sqrt((-1/kk)^2 + 1); % 下线段的b elseif kk < 0 % 如果k小于0的话，目标方向朝着右下角，求上下线段的b调换 m = topOrbottom_bb - rectangleLenght/2 * sqrt((-1/kk)^2 + 1); % 上线段的b mm = topOrbottom_bb + rectangleLenght/2 * sqrt((-1/kk)^2 + 1); % 下线段的b end rectangEquation{i,1}.left.k = kk; % 左线段 rectangEquation{i,1}.left.b = n; rectangEquation{i,1}.right.k = kk; % 右线段 rectangEquation{i,1}.right.b = nn; rectangEquation{i,1}.top.k = -1/kk; % 上线段 rectangEquation{i,1}.top.b = m; rectangEquation{i,1}.bottom.k = -1/kk; % 下线段 rectangEquation{i,1}.bottom.b = mm; dingDian(1,1) = (kk*m - kk*n) / (kk^2 + 1); %左上顶点 dingDian(1,2) = kk*dingDian(1,1) + n; dingDian(2,1) = (kk*mm - kk*n) / (kk^2 + 1); % 左下顶点 dingDian(2,2) = kk*dingDian(2,1) + n; dingDian(3,1) = (kk*m - kk*nn) / (kk^2 + 1); % 右上顶点 dingDian(3,2) = kk*dingDian(3,1) + nn; dingDian(4,1) = (kk*mm - kk*nn) / (kk^2 + 1); % 右下顶点 dingDian(4,2) = kk*dingDian(4,1) + nn; else infNum = infNum + 1; type = 1; rectangEquation{i,1}.left.x1 = x(1,i) - rectangleWidth/2; % 左线段 rectangEquation{i,1}.right.x1 = x(1,i) + rectangleWidth/2; % 右线段 rectangEquation{i,1}.top.y1 = y(1,i) + rectangleLenght/2; % 上线段 rectangEquation{i,1}.bottom.y1 = y(1,i) - rectangleLenght/2; % 下线段 dingDian(1,1) = rectangEquation{i,1}.left.x1; %左上顶点x dingDian(1,2) = rectangEquation{i,1}.top.y1; %左上顶点y dingDian(2,1) = rectangEquation{i,1}.left.x1; % 左下顶点 dingDian(2,2) = rectangEquation{i,1}.bottom.y1; dingDian(3,1) = rectangEquation{i,1}.right.x1; % 右上顶点 dingDian(3,2) = rectangEquation{i,1}.top.y1; dingDian(4,1) = rectangEquation{i,1}.right.x1; % 右下顶点 dingDian(4,2) = rectangEquation{i,1}.bottom.y1; end % 画出 目标长方形 % line([dingDian(1,1),dingDian(2,1),dingDian(4,1),dingDian(3,1),dingDian(1,1)],... % [dingDian(1,2),dingDian(2,2),dingDian(4,2),dingDian(3,2),dingDian(1,2)]); openChane = 0; for j=1:100 if lidar.k(j,1) ~= 0 % 如果等于0 说明这条射线就是x轴，不会有交点 if type == 0 % 第一种情况(k!=inf) 两直线求交点y=k1*x+b、y=k2*x(射线) x1 = rectangEquation{i,1}.left.b / (lidar.k(j,1) - rectangEquation{i,1}.left.k); % 左线段交点 y1 = lidar.k(j,1) * x1; x2 = rectangEquation{i,1}.right.b / (lidar.k(j,1) - rectangEquation{i,1}.right.k); % 右线段交点 y2 = lidar.k(j,1) * x2; x3 = rectangEquation{i,1}.top.b / (lidar.k(j,1) - rectangEquation{i,1}.top.k); % 上线段交点 y3 = lidar.k(j,1) * x3; x4 = rectangEquation{i,1}.bottom.b / (lidar.k(j,1) - rectangEquation{i,1}.bottom.k); % 下线段交点 y4 = lidar.k(j,1) * x4; else % 第二种情况 (k==inf) 长方形是竖着的 x1 = rectangEquation{i,1}.left.x1; y1 = lidar.k(j,1) * rectangEquation{i,1}.left.x1; % 雷达 左线段交点 x2 = rectangEquation{i,1}.right.x1; y2 = lidar.k(j,1) * rectangEquation{i,1}.right.x1; % 雷达 右线段交点 x3 = rectangEquation{i,1}.top.y1 / lidar.k(j,1); % 雷达 上线段交点 y3 = rectangEquation{i,1}.top.y1; x4 = rectangEquation{i,1}.bottom.y1 / lidar.k(j,1); % 雷达 下线段交点 y4 = rectangEquation{i,1}.bottom.y1; end % 下面这段没用到(但运行正确)，之前思路(切换顶点)有问题(infNum也不可能为2)。 if infNum == 2 && allKK(i) == inf || infNum == 3 && allKK(i) ~= inf temp = dingDian(1,1); dingDian(1,1) = dingDian(4,1); dingDian(4,1) = temp; temp = dingDian(1,2); dingDian(1,2) = dingDian(4,2); dingDian(4,2) = temp; temp = dingDian(2,1); dingDian(2,1) = dingDian(3,1); dingDian(3,1) = temp; temp = dingDian(2,2); dingDian(2,2) = dingDian(3,2); dingDian(3,2) = temp; end % 距离(判断是否在长方形上) if allKK(i) > 0 % 如果与左线段的交点 交点x小于左下顶点的x 或者 交点x大于左上顶点的x 或者..... if y1 < dingDian(2,2) || y1 > dingDian(1,2) jl1 = 10000; else jl1 = sqrt(x1^2 + y1^2); end if y2 < dingDian(4,2) || y2 > dingDian(3,2) jl2 = 10000; else jl2 = sqrt(x2^2 + y2^2); end if x3 < dingDian(1,1) || x3 > dingDian(3,1) jl3 = 10000; else jl3 = sqrt(x3^2 + y3^2); end if x4 < dingDian(2,1) || x4 > dingDian(4,1) jl4 = 10000; else jl4 = sqrt(x