simulation_hua_square_obstacle_2.m

轮式移动机器人的避障

% -----------------------------------------------------
% simulate_hua_square_obstacle2(障碍物在左边).m 主要进行仿真的函数，障碍物位于左边
% 对机器人进行仿真的主要的脚本文件
% 
% -----------------------------------------------------


clear all;                              % 清除所有变量 全局变量，函数和MEX链接.
close all;                              % 关闭所有打开的图形窗口.

% 机器人几何参数
global L;                               % 定义了三个全局变量 L 表示机器人前后轮之间的距离  定义全局变量，其它函数以及工作空间该全局变量都可见
global d;                               % d用来表示机器人两个后轮之间的距离，在其它函数和工作空间中都可见
global R_w;                             % R_w表示机器人的轮子的半径，其它函数和工作空间中都可见
global Thick_wheel                      % 轮子的厚度
global Clearance;                       % 机器人后轮和机体的间隙
L = 0.9;%0.6;%0.25;                          % 机器人长度（单位：米）
d = 0.75;%0.5;%0.18;                          % 机器人宽度（单位：米）
R_w = 0.2;%0.05;                        % 轮半径（单位：米）
Thick_wheel=0.1;                        % 轮子的厚度(单位:米)
Clearance=0.05;                         % 机器人后轮和机体的间隙

% 机器人的速度约束参数
m_max_rpm = 10000;                      % 电机最大转速（单位：转/分）
gratio = 60;                            % 齿轮减速比
m_max_rads = rpm2rads(m_max_rpm);       % 电机最大的角速度（单位：弧度/秒）函数rpm2rads用于单位转换
w_max_rads = m_max_rads / gratio;       % 机器人轮子最大的角速度（单位：弧度/秒）
st_max_deg = 45;                        % 机器人前轮最大驱动角（单位：度）
v_max = w_max_rads * R_w;               % 机器人最大的线速度，即前进的速度（单位：米/秒）                 控制输入的限制值
st_max_rad = deg2rad(st_max_deg);       % 机器人最大驱动角度（单位：弧度）函数deg2rad把角度值转换为弧度值  控制输入的限制值

% 仿真时间参数
t_max = 7;                              % 仿真时间（单位：秒）
n = 700;                                % 循环次数
dt = t_max/n;                           % 每次循环的时间增量（单位：秒）
t = [0:dt:t_max];                       % 时间矢量(包括n+1个元素)

% 机器人运动范围参数,机器人运动过程中不允许超过这个范围
X=1.5;       % 右界
Y_high=0.75;     % 上界
Y_low=-0.75;     % 下界

% R=3;                                  % 机器人活动范围半径（单位：米）
% %R1=3.1;
% gama=0:0.01:2*pi;
% x_boundary=R*cos(gama);                 % 机器人活动范围的横坐标矢量（单位：米）
% y_boundary=R*sin(gama);                 % 机器人活动范围的纵坐标矢量（单位：米）

% 机器人的初始状￥态矢量(这个矢量可以由用户自己设置)
x0 = 2.5;%0;%0;                              % 移动机器人初始x坐标（单位：米）
y0 = 2.5;%1.5;%1;                            % 移动机器人初始y坐标（单位：米）
th_deg0 = -135;%0;%135                       % 移动机器人初始方位角度，轴线相对于坐标系x轴角度值（单位：度）
th_rad0 = deg2rad(th_deg0);             % 机器人初始方位角度（单位：弧度）

% 机器人控制器参数
k1=1;%2;
k2=25;%115;%15;
k3=2;

% 误差参数,当机器人首先镇定到x轴上时.判断机器人y坐标是否小于这个值,并且防卫角度也小于这个值
delta=0.01;
kethi=0.05;  % 机器人运动范围边界余量

% 得到机器人初始状态矢量z0和机器人初始控制矢量u0
if x0 == 0                              % 判断分母是否为0
    x0_denominator=eps;
else
    x0_denominator=x0;
end
alpa=atan(y0/x0_denominator);
if cos(alpa-th_rad0)>0                  % 确定初始线速度方向
    v0=-v_max;
else
    v0=v_max;
end
if th_rad0 == 0
    th_rad0 = eps;
end
st_rad0 = atan(-L/v0*(k2*th_rad0+k1*v0*(sin(th_rad0)/th_rad0)*y0));               % 机器人初始驱动角（单位：弧度）
if abs(st_rad0) > st_max_rad
    st_rad0 = sign(st_rad0)*st_max_rad;
end


z0 = [x0, y0, th_rad0];                 % 机器人初始状态向量（横坐标、纵坐标、方位角度）
u0 = [v0, st_rad0];                     % 机器人初始控制输入向量（线速度、驱动角度）


% 图形边框坐标,用左右上下的顺序给出
X_l=-1;  
X_r=9.5;
Y_t=4.6;
Y_l=-4.6;
%开始时刻作图,画机器人活动范围和机器人的初始位置
fig1 = figure(1);                       % 建立第一个图
plot([X_l X_r],[0 0],':');              % 画横坐标轴
% plot(x_boundary,y_boundary,'m','linewidth',2);            % 在第一个图上画机器人运动范围
axis([X_l X_r Y_l Y_t]);              % 设定图形的范围,分别是左、右、下、上
hold on;
%plot([-4.5 4.5],[0 0],':');
plot([0 0],[Y_l Y_t],':');              % 画纵坐标轴
plot([X_r-0.15 X_r-0.15],[0 0],'>');    % 画横坐标箭头
plot([0 0],[Y_t-0.15 Y_t-0.15],'^');    % 画纵坐标箭头

plot([X X],[Y_t Y_high],'-.');          % 画障碍物边界曲线
plot([X X_l],[Y_high Y_high],'-.');
plot([X_l X],[Y_low Y_low],'-.');
plot([X X],[Y_low Y_l],'-.');

%plot([X_right 4.5],[Y_high Y_high],'-.');
xfill_region_top=[X_l X X X_l];                     % 填充障碍物区域
yfill_region_top=[Y_t Y_t Y_high Y_high];
xfill_region_bottom=[X_l X X X_l];
yfill_region_bottom=[Y_low Y_low Y_l Y_l];
fill(xfill_region_top,yfill_region_top,'k');
fill(xfill_region_bottom,yfill_region_bottom,'k');


% 获取机器人构型准备画图
% 机器人基体四个顶点坐标，xb和yb为两个矢量，分别记录四个顶点的横坐标和纵坐标
% 机器人前轮前后的横坐标和纵坐标
% 机器人后左轮前后的横坐标和纵坐标
% 机器人后右轮前后的横坐标和纵坐标
[xb, yb, xfw, yfw, xrlw, yrlw, xrrw, yrrw] = model_config(z0, u0);  %获取机器人的构型，就是在初始时得到机器人的构型
                                                                    %根据机器人的
                                                                    %状态矢量z0和控制矢量计算机器人基体和轮子的构型
%画机器人并定义曲线id号
plotzb = plot(xb, yb,'linewidth',2);              % 画机器人基体，因为xb和yb已经是一个五维的矢量了，所以可以直接使用函数plot  机器人基体默认为兰色边框，并返回句柄，保存在变量plotzb中
plotzfw = plot(xfw, yfw, 'k','linewidth',3);      % 画前轮   实际上是画一条线段，这条线段由其两个端点定义
plotzrlw = plot(xrlw, yrlw, 'k','linewidth',3);   % 画后左轮 画后左轮，也是画一条线段，这条线段由其端点定义
plotzrrw = plot(xrrw, yrrw, 'k','linewidth',3);   % 画后右轮 画后右轮，画一条线段，由其端点定义

% Draw fast and erase fast
set(gca, 'drawmode','fast');        %gca表示返回当前轴的句柄  xor
set(plotzb, 'erasemode', 'xor');
set(plotzfw, 'erasemode', 'xor');
set(plotzrlw, 'erasemode', 'xor');
set(plotzrrw, 'erasemode', 'xor');

%------------------------------------------------------------------------------------------------------
z1 = z0;                            % Set initial state to z1 for simulation

% 开始仿真
for i = 1:n+1
    if i == 1 % 特别处理i=1时的情况
        if abs(z1(2))>delta | abs(z1(3))>delta % 尚未镇定到x轴上
            %kethi=0.05;  % 机器人运动范围边界余量
            if (z1(1)-X>kethi) | ((X- z1(1)>kethi)&(z1(1)-X_l>kethi)&(Y_high-z1(2)>kethi)&(z1(2)-Y_low>kethi))
            %if (z1(1) > X) | ((z1(1) < X) & (z1(1) > X_l) & (z1(2) < Y_high) & (z1(2) > Y_low))
            %if (z1(2)>Y_high) | (abs(z1(2))<=Y_high & abs(z1(1))<X_right)
            %if (R-sqrt(z1(1)*z1(1)+z1(2)*z1(2))) >= kethi
            %if sqrt(z1(1)*z1(1)+z1(2)*z1(2))<R % 机器人未出范围
                v(i)=v0;
                st_rad(i)=atan(-L/v(i)*(k2*z1(3)+k1*v(i)*(sin(z1(3))/z1(3))*z1(2)));
                if abs(st_rad(i))>st_max_rad   % 机器人驱动角度超过限定
                    st_rad(i)=sign(st_rad(i))*st_max_rad;
                end
            else % 机器人出范围
                if v0>0 % 机器人以前进方向超出范围
                    v(i)=-v0;
                    st_rad(i)=atan(-L/v(i)*(k2*z1(3)+k1*v(i)*(sin(z1(3))/z1(3))*z1(2)));
                    if abs(st_rad(i))>st_max_rad   % 机器人驱动角度超过限定
                        st_rad(i)=sign(st_rad(i))*st_max_rad;
                    end
                else % 机器人以后退方向超出范围
                    v(i)=-v0;
                    st_rad(i)=atan(-L/v(i)*(k2*z1(3)+k1*v(i)*(sin(z1(3))/z1(3))*z1(2)));
                    if abs(st_rad(i))>st_max_rad   % 机器人驱动角度超过限定
                        st_rad(i)=sign(st_rad(i))*st_max_rad;
                    end
                end
            end
        else % 机器人已经镇定到x轴上
            v(i)=-k3*z1(1);
            st_rad(i)=0;
            if abs(v(i))>v_max % 机器人线速度超出范围
                v(i)=sign(v(i))*v_max;
            end
        end
    else % i不等于1时的情况
        if abs(z1(2))>delta | abs(z1(3))>delta % 尚未镇定到x轴上
            if (z1(1)-X>kethi) | ((X- z1(1)>kethi)&(z1(1)-X_l>kethi)&(Y_high-z1(2)>kethi)&(z1(2)-Y_low>kethi))
            %if (z1(1) > X) | ((z1(1) < X) & (z1(1) > X_l) & (z1(2) < Y_high) & (z1(2) > Y_low))
            %if (z1(2)>Y_high) | (abs(z1(2))<=Y_high & abs(z1(1))<X_right)
            %if (R-sqrt(z1(1)*z1(1)+z1(2)*z1(2))) >= kethi
            %if sqrt(z1(1)*z1(1)+z1(2)*z1(2))<R % 机器人未出范围
                v(i)=v(i-1);
                st_rad(i)=atan(-L/v(i)*(k2*z1(3)+k1*v(i)*(sin(z1(3))/z1(3))*z1(2)));
                if abs(st_rad(i))>st_max_rad   % 机器人驱动角度超过限定
                    st_rad(i)=sign(st_rad(i))*st_max_rad;
                end
            else % 机器人出范围
                if v(i-1)>0 % 机器人以前进方向超出范围
                    v(i)=-v(i-1);
                    st_rad(i)=atan(-L/v(i)*(k2*z1(3)+k1*v(i)*(sin(z1(3))/z1(3))*z1(2)));
                    if abs(st_rad(i))>st_max_rad   % 机器人驱动角度超过限定
                        st_rad(i)=sign(st_rad(i))*st_max_rad;
                    end
                else % 机器人以后退方向超出范围
                    v(i)=-v(i-1);
                    st_rad(i)=atan(-L/v(i)*(k2*z1(3)+k1*v(i)*(sin(z1(3))/z1(3))*z1(2)));
                    if abs(st_rad(i))>st_max_rad   % 机器人驱动角度超过限定
                        st_rad(i)=sign(st_rad(i))*st_max_rad;
                    end
                end
            end
        else % 机器人已经镇定到x轴上
            v(i)=-k3*z1(1);
            st_rad(i)=0;
            if abs(v(i))>v_max % 机器人线速度超出范围
                v(i)=sign(v(i))*v_max;
            end
        end
    end
    
    
    u = [v(i), st_rad(i)];            % 设置控制输入，线速度和驱动角度
    zdot = robot_model(z1, u);        % 用机器人模型获得z的导数，机器人状态矢量、控制矢量
    z2 = z1 + zdot * dt;              % 更新机器人状态
    z1 = z2;                          % 用z2代替z1
    
    %用线括起来的是自己加的部分，这里是保存机器人的轨迹
    %--------------------------------------
    x(i)=z1(1);
    y(i)=z1(2);
    theta(i)=z1(3);
    %--------------------------------------
    
    % 获得机器人的构型以便于画机器人
    [xb, yb, xfw, yfw, xrlw, yrlw, xrrw, yrrw] = model_config(z1, u);

    % 画机器人
    set(plotzb,'xdata',xb);
    set(plotzb,'ydata',yb);
    set(plotzfw,'xdata',xfw);
    set(plotzfw,'ydata',yfw);
    set(plotzrlw,'xdata',xrlw);
    set(plotzrlw,'ydata',yrlw);
    set(plotzrrw,'xdata',xrrw);
    set(plotzrrw,'ydata',yrrw);
    
    if i>1
        plot([x(i-1) x(i)],[y(i-1) y(i)],'k');
    end

    pause(0.05);                       % Pause by 0.2s for slower simulation
end


% 画速度和驱动角度矢量
fig2 = figure(2);                     % 画图形2  第二个图形的句柄保存在变量fig2中
subplot(2,1,1);                       % 图形2的上半部分
plot(t, v);                           % 画速度-时间曲线
xlabel('Time [s]');                   % 给x轴标注
ylabel('Velocity [m/s]');             % 给y轴标注
grid;

subplot(2,1,2);                       % 图形2的下半部分
std = rad2deg(st_rad);                % 弧度值转换为角度值，并保存在矢量std中
plot(t,std);                          % 画驱动角度-时间曲线
xlabel('Time [s]');                   % 给x轴标注
ylabel('Steering angle [deg]');       % 给y轴标注
grid;


%用线括起来的是自己加的部分
%-----------------------------------------
fig3=figure(3);
plot(t,x,'r');
hold on;
plot(t,y,'b');
plot(t,theta,'k');
grid
%-----------------------------------------