在水下进行路径规划时,我们常常需要模拟声呐的探测.
本代码实现了一个简单的机器人的构建(用一个圆圈进行表示),发出若干互成角度的射线,(pi/4,pi/8),最大的探测的距离为10..
声呐的探测模型是需要进行构建的,在进行创建时,我们可以设置从一个点进行出发,发出若干条射线进行探测,为了方便说明这里只构建有限的几条射线,并且要说明最大的探测距离。
function h = createCarCircle(radius, sensor_lengde)
%这里定义的最大的感知距离为10.
[X,Y] = circle([0, 0], radius, 100);
car = patch(X,Y,'b');
set(car,'FaceLighting','phong','EdgeLighting','phong');
%car = patch(P_bil(:,1),P_bil(:,2), 'r');
% Sensor properties
n_punkter = 2; % Number of points of the sensors
%x-aksen for de 5 forskjellige sensor-linjene
x1 = linspace(radius, sensor_lengde + (radius), n_punkter); %middle sensor. 0 grader
x2 = linspace(radius, sensor_lengde*cos(pi/4) + radius, n_punkter); %the two sensors close to middle. pi/4 radianer
x3 = linspace(radius, sensor_lengde*cos(pi/8) + radius, n_punkter); %the two farthest sensors. pi/8 radianer
for i = 1:n_punkter
% Equations for straight lines (y-aksen)
y1(i) = tan(pi/4)*(x2(i) -radius);
y2(i) = tan(pi/8)*(-radius + x3(i));
y3(i) = 0; %中间感知器的纵坐标...
y4(i) = tan(pi/8)*(radius - x3(i));
y5(i) = tan(pi/4)*(radius - x2(i));
end
% Create sensors
L_h1 = patch(x2', y1', 'k');
L_h2 = patch(x3', y2', 'k');
L_h3 = patch(x1', y3', 'k');
L_h4 = patch(x3', y4', 'k');
L_h5 = patch(x2', y5', 'k');
h = [car ,L_h1, L_h2, L_h3, L_h4, L_h5];
end
运行的结果展示如下:
这里提供了一个函数,我们可以根据函数的这两个参数:radius(机器人本身的大小),sensor_lengde(最大的感知距离)这两个因素共同确定...