MATLAB基于几何图形法的障碍物地图构建算法

function [ Bexist,crosspiont,num,other_crosspoint ] = searchBarrier( xnext,ynext,beta1,beta2,r,confine ) %输入参数： % xnext,ynext----机器人下一时刻位置 % beta1,beta2----机器人视野范围大小 % r----机器人视野长度，半径 % confine----地图边界值 % 输出参数： % Bexist----当前位置是否检测到障碍物,0--没有，1--有 % crosspiont----搜索区域与障碍物区域的边界交点坐标 %%测试时使用 % quaBarrier----返回改点在第几象限 % num----该象限第几个障碍物,若numbarrier == 32代表到达地图边界,若numbarrier ==0代表不在障碍物区域 %% %机器人探测区域 %将圆形区域与所有该坐标系的曲线求解，若解在Barrier区域且同时在搜索区域则，表示探测到障碍物 Bexist = 0; crosspiont = []; count = 1; other_crosspoint = []; count_other = 1; quadrant = 0; quaBarrier = 0; num = 0; %判断点在哪一个象限 if xnext >= 0 && ynext >= 0 quadrant = 1; elseif xnext < 0 && ynext > 0 quadrant = 2; elseif xnext < 0 && ynext < 0 quadrant = 3; else quadrant = 4; end syms x y eqn = (x-xnext)^2+(y-ynext)^2 == r^2; %% %第一象限所有边界曲线 if quadrant == 1 eqn1_1(1) = x-confine == 0; %地图边界1 eqn1_1(2) = y-confine == 0; %地图边界2 eqn1_1(3) = x-2-y == 0; eqn1_1(4) = 0.5-y == 0; eqn1_1(5) = -x+2-y ==0; eqn1_1(6) = y-x-1 == 0; eqn1_1(7) = y-2.5 == 0; eqn1_1(8) = y+x-5 == 0; eqn1_1(9) = y == 0; eqn1_1(10) = x == 0; end %第二象限所有边界曲线 if quadrant == 2 eqn2_1(1) = x+confine == 0; %地图边界1 eqn2_1(2) = y-confine == 0; %地图边界2 eqn2_1(3) = x+2.5-y == 0; eqn2_1(4) = -x-1.5-y == 0; eqn2_1(5) = y-x/3-13/6 ==0; % eqn2_1(6) = x+4.5-y == 0; eqn2_1(6) = y-2*x-8.5 == 0; eqn2_1(7) = y+x+0.5 == 0; eqn2_1(8) = y == 0; eqn2_1(9) = x == 0; end %第三象限所有边界曲线 if quadrant == 3 eqn3_1(1) = x+confine == 0; %地图边界1 eqn3_1(2) = y+confine == 0; %地图边界2 eqn3_1(3) = x+1 == 0; eqn3_1(4) = x+3.5 == 0; eqn3_1(5) = y+1 ==0; eqn3_1(6) = y+1.5 == 0; eqn3_1(7) = x+1.5 == 0; eqn3_1(8) = y+2 == 0; eqn3_1(9) = x+2.5 == 0; eqn3_1(10) = y+2.5 == 0; eqn3_1(11) = y == 0; eqn3_1(12) = x == 0; end %第四象限所有边界曲线 if quadrant == 4 eqn4_1(1) = x-confine == 0; %地图边界1 eqn4_1(2) = y+confine == 0; %地图边界2 eqn4_1(3) = 1.5*x-5-y == 0; eqn4_1(4) = -x-y == 0; eqn4_1(5) = y+0.5 ==0; eqn4_1(6) = y == 0; eqn4_1(7) = x == 0; end %% %按象限匹配 switch quadrant case 1 %========================================================================== n = length(eqn1_1); for i = 1:1:n [sx,sy] = solve(eqn,eqn1_1(i),x,y,'Real',true);%只求实数解 if ~isempty(sx)%有交点 %判断交点是否在Barrier中 n1 = length(sx);%确认有几个交点 for j = 1:1:n1 [numbarrier,quadrantBarrier] = Barrier(sx(j),sy(j),confine); % if sx(j) < 0 || sy(j) < 0 || sx(j) > confine || sy(j) > confine%说明点不在第一象限地图区域内,那么结束当前循环 % continue; % end if sx(j) < -confine || sy(j) < -confine || sx(j) > confine || sy(j) > confine%说明点不在地图区域内,那么结束当前循环 continue; end if numbarrier ~=0%在障碍物空间中 %还得判断是否在视野范围中 beta = asin((sy(j)-ynext)/sqrt((sx(j)-xnext)^2+(sy(j)-ynext)^2));%sin(beta) = y-ynext/(sqrt(x-xnext)^2+(y-ynext)^2) 得到的是弧度值 %因为asin只能得出 -90 <= beta <= 90之间的角度，所以还需要细化判断 if sx(j)-xnext <0 && sy(j)-ynext > 0%说明在第二象限 beta = beta+pi/2; end if sx(j)-xnext < 0 && sy(j)-ynext < 0%说明在第三象限 beta = beta-pi/2; end %% %当x=0或y=0时，会出现奇异性要单独讨论 if sy(j)-ynext ==0 if sx(j)-xnext > 0 beta = 0; elseif sx(j)-xnext < 0 beta = pi; end end if (beta >= beta1 && beta <= beta2) || ((beta-2*pi) >= beta1 && (beta-2*pi) <= beta2) || ( (2*pi+beta) >= beta1 && (2*pi+beta) <= beta2 )%因为一个角度有两种表达方式，在-270 <= beta <= 270范围中 Bexist = 1; if sx(j) < 0 || sy(j) < 0 || sx(j) > confine || sy(j) > confine%说明点不在第一象限地图区域内,那么结束当前循环 other_crosspoint(:,count_other) = [sx(j);sy(j)]; count_other = count_other+1; continue; end crosspiont(:,count) = [sx(j);sy(j)]; num(count) = numbarrier; count = count+1; quaBarrier = quadrant; end end end end end %=========================================================================== %=========================================================================== case 2 n = length(eqn2_1); for i = 1:1:n [sx,sy] = solve(eqn,eqn2_1(i),x,y,'Real',true);%只求实数解 if ~isempty(sx)%有交点 %判断交点是否在Barrier中 n1 = length(sx);%确认有几个交点 for j = 1:1:n1 [numbarrier,quadrantBarrier] = Barrier(sx(j),sy(j),confine); if sx(j) > 0 || sy(j) < 0 || sx(j) < -confine || sy(j) > confine%说明点不在第一象限,那么结束当前循环 continue; end if numbarrier ~=0%在障碍物空间中 %还得判断是否在视野范围中 beta = asin((sy(j)-ynext)/sqrt((sx(j)-xnext)^2+(sy(j)-ynext)^2));%sin(beta) = y-ynext/(sqrt(x-xnext)^2+(y-ynext)^2) 得到的是弧度值 %因为asin只能得出 -90 <= beta <= 90之间的角度，所以还需要细化判断 if sx(j)-xnext <0 && sy(j)-ynext > 0%说明在第二象限 beta = beta+pi/2; end if sx(j)-xnext < 0 && sy(j)-ynext < 0%说明在第三象限 beta = beta-pi/2; end %% %当x=0或y=0时，会出现奇异性要单独讨论 if sy(j)-ynext ==0 if sx(j)-xnext > 0 beta = 0; elseif sx(j)-xnext < 0 beta = pi; end end if beta >= beta1 && beta <= beta2 || ((beta-2*pi) >= beta1 && (beta-2*pi) <= beta2) || ( (2*pi+beta) >= beta1 && (2*pi+beta) <= beta2 )%因为一个角度有两种表达方式，在-270 <= beta <= 270范围中 Bexist = 1; crosspiont(:,count) = [sx(j);sy(j)]; num(count) = numbarrier; count = count+1; quaBarrier = quadrant; end end end end end case 3 n = length(eqn3_1); for i = 1:1:n [sx,sy] = solve(eqn,eqn3_1(i),x,y,'Real',true);%只求实数解 if ~isempty(sx)%有交点 %判断交点是否在Barrier中 n1 = length(sx);%