stabilization_to_x_axis.m

轮式移动机器人的镇定

% -----------------------------------------------------
% stabilization_to_x_axis.m 机器人以不同的初始位姿镇定到x轴上
%
% -----------------------------------------------------


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

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

st_max_deg = 20;                        % 机器人前轮最大驱动角（单位：度）
v_max = 3.4907;                            % 机器人最大的线速度，即前进的速度（单位：米/秒）                 控制输入的限制值
st_max_rad = deg2rad(st_max_deg);       % 机器人最大驱动角度（单位：弧度）函数deg2rad把角度值转换为弧度值  控制输入的限制值

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


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

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


fig1 = figure(1);                       % 建立第一个图
axis([-10 10 -5 10]);                % 设定图形的范围,分别是左、右、下、上
grid;
hold on;


for i = 1:8 % 间隔45度进行循环
    if x0==0                            % 判断分母是否为0
        x0_denominator=eps;
    else
        x0_denominator=x0;
    end
    alpa=atan(y0/x0_denominator);
    th_rad0(i) = deg2rad(th_deg0(i));
    if cos(alpa-th_rad0(i))>0           % 确定初始线速度方向
        v0=-v_max;
    else
        v0=v_max;
    end

    if th_rad0(i) == 0 % 判断机器人初始方位是否为0
        th_rad0(i) = eps;
    end
    st_rad0 = atan(-L/v0*(k2*th_rad0(i)+k1*v0*(sin(th_rad0(i))/th_rad0(i))*y0));               % 机器人初始驱动角（单位：弧度）
    if abs(st_rad0) > st_max_rad
        st_rad0 = sign(st_rad0)*st_max_rad;
    end

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

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

    % 开始仿真
    for j = 1:n+1
        if j == 1 % 特别处理i=1时的情况
            v(j)=v0;
            if z1(3)== 0
                z1(3)=eps;
            end
            st_rad(j)=atan(-L/v(j)*(k2*z1(3)+k1*v(j)*(sin(z1(3))/z1(3))*z1(2)));
            if abs(st_rad(j))>st_max_rad   % 机器人驱动角度超过限定
                st_rad(j)=sign(st_rad(j))*st_max_rad;
            end
        else % i不等于1时的情况
            v(j)=v(j-1);
            st_rad(j)=atan(-L/v(j)*(k2*z1(3)+k1*v(j)*(sin(z1(3))/z1(3))*z1(2)));
            if abs(st_rad(j))>st_max_rad   % 机器人驱动角度超过限定
                st_rad(j)=sign(st_rad(j))*st_max_rad;
            end
        end
        u = [v(j), st_rad(j)];            % 设置控制输入，线速度和驱动角度
        zdot = robot_model(z1, u);        % 用机器人模型获得z的导数，机器人状态矢量、控制矢量
        z2 = z1 + zdot * dt;              % 更新机器人状态
        z1 = z2;                          % 用z2代替z1
      
        %--------------------------------------
        x(j)=z1(1);
        y(j)=z1(2);
        theta(j)=z1(3);
        %--------------------------------------
    end
    
    %-------------------------------------------------------------------------------------------
    figure(1);
    if i==1
        plot(x,y,'-c');
    elseif i==2
        plot(x,y,':r');
    elseif i==3
        plot(x,y,'-.g');
    elseif i==4
        plot(x,y,'--b');
    elseif i==5
        plot(x,y,'-k');
    elseif i==6
        plot(x,y,'-y');
    elseif i==7
        plot(x,y,':m');
    else 
        plot(x,y,'-.r');
    end
                
    %plot(x,y,);
    figure(i+1);
    plot(t,v);
    pause(1.5);
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
% %-----------------------------------------