stabilization_to_beta_line_positive.m

轮式移动机器人镇定到倾角为任意的直线上

% -----------------------------------------------------
% simulate_hua_general_orientation.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 = 20;                            % 齿轮减速比
m_max_rads = rpm2rads(m_max_rpm);       % 电机最大的角速度（单位：弧度/秒）函数rpm2rads用于单位转换
w_max_rads = m_max_rads / gratio;       % 机器人轮子最大的角速度（单位：弧度/秒）
st_max_deg = 20;                        % 机器人前轮最大驱动角（单位：度）
v_max = w_max_rads * R_w               % 机器人最大的线速度，即前进的速度（单位：米/秒）                 控制输入的限制值
st_max_rad = deg2rad(st_max_deg);       % 机器人最大驱动角度（单位：弧度）函数deg2rad把角度值转换为弧度值  控制输入的限制值

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

% 机器人运动范围参数,机器人运动过程中不允许超过这个范围-----------------------------去除圆形范围
% 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;%0;                              % 移动机器人初始x坐标（单位：米）
y0 = -0.5;%1.5;%1.5;%1;                            % 移动机器人初始y坐标（单位：米）
th_deg0 = -30;%-90;%-45;%225;%0;%135                       % 移动机器人初始方位角度，轴线相对于坐标系x轴角度值（单位：度）
th_rad0 = deg2rad(th_deg0);             % 机器人初始方位角度（单位：弧度）
beta_deg= 40;                            % 镇定方向倾角
beta_deg1=mod(beta_deg,360);
beta_rad=deg2rad(beta_deg1);

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

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

%-------------------------------也不再用判断角度确定初始线速度方向了
% 得到机器人初始状态矢量z0和机器人初始控制矢量u0
%if x0 == 0                              % 判断分母是否为0
% if x0*cos(beta_rad)+y0*sin(beta_rad) == 0                              % 判断分母是否为0
%     x0_denominator=eps;
% else
%     x0_denominator=x0*cos(beta_rad)+y0*sin(beta_rad);
% end
% alpa=atan((-x0*sin(beta_rad)+y0*cos(beta_rad))/x0_denominator);
% if cos(alpa-(th_rad0-beta_rad))>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-beta_rad)+k1*v0*(sin(th_rad0-beta_rad)/(th_rad0-beta_rad))*( -x0*sin(beta_rad)+y0*cos(beta_rad) ) ));%   (sign(y0-(tan(beta_rad))*x0)*abs(x0-y0)/sqrt(tan(beta_rad)*tan(beta_rad)+1))));
%st_rad0 = atan(-L/v0*(k2*(th_rad0-beta_rad)+k1*v0*(sin(th_rad0-beta_rad)/(th_rad0-beta_rad))*(sign(y0-x0)*abs(x0-y0)/sqrt(2))));
%st_rad0 = atan(-L/v0*(k2*(th_rad0-pi/4)+k1*v0*(sin(th_rad0-pi/4)/(th_rad0-pi/4))*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];                     % 机器人初始控制输入向量（线速度、驱动角度）

%开始时刻作图,画机器人活动范围和机器人的初始位置
fig1 = figure(1);                       % 建立第一个图
%plot(x_boundary,y_boundary,'m','linewidth',2);            % 在第一个图上画机器人运动范围
%axis([-4.5 4.5 -3.6 3.6]);              % 设定图形的范围,分别是左、右、下、上
%hold on;
plot([-4.5 4.5],[-4.5*tan(beta_rad) 4.5*tan(beta_rad)],':');
axis([-4.5 4.5 -3.6 3.6]);              % 设定图形的范围,分别是左、右、下、上
hold on;
plot([-4.5 4.5],[0 0],':');
plot([0 0],[-3.6 3.6],':');
plot([4.4 4.4],[0 0],'>');
plot([0 0],[3.5 3.5],'^');
xlabel('x (m)');                   % 给x轴标注
ylabel('y (m)');             % 给y轴标注

% 获取机器人构型准备画图
% 机器人基体四个顶点坐标，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(1)-z1(2))/sqrt(tan(beta_rad)*tan(beta_rad)+1)>delta) | abs(z1(3)-beta_rad)>delta
        %if abs(z1(2))>delta | abs(z1(3))>delta % 尚未镇定到x轴上
            %if (R-sqrt(z1(1)*z1(1)+z1(2)*z1(2))) >= kethi
            %if sqrt(z1(1)*z1(1)+z1(2)*z1(2))<R % 机器人未出范围
            %(sign(y0-x0)*abs(x0-y0)/sqrt(2))
            %(sign(y0-(tan(beta_rad))*x0)*abs(x0-y0)/sqrt(tan(beta_rad)*tan(beta_rad)+1))))
                v(i)=v0;
                %st_rad0 = atan(-L/v0*(k2*th_rad0+k1*v0*(sin(th_rad0)/th_rad0)*(x0*sin(beta-rad)+y0*cos(beta_rad));
                if z1(3)-beta_rad ==0
                    z1(3)=beta_rad+eps;
                end
                st_rad(i)=atan( -L/v(i)* (k2*(z1(3)-beta_rad)+k1*v(i)*sin(z1(3)-beta_rad)/(z1(3)-beta_rad)* ( -z1(1)*sin(beta_rad)+z1(2)*cos(beta_rad) ) ));%    (sign(z1(2)-tan(beta_rad)*z1(1))*abs(z1(1)-z1(2))/sqrt(tan(beta_rad)*tan(beta_rad)+1))));%     (sign(z1(2)-z1(1))*abs(z1(1)-z1(2))/sqrt(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)-beta_rad)+k1*v(i)*sin(z1(3)-beta_rad)/(z1(3)-beta_rad)* ( -z1(1)*sin(beta_rad)+z1(2)*cos(beta_rad) ) ));
%                     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)-beta_rad)+k1*v(i)*sin(z1(3)-beta_rad)/(z1(3)-beta_rad)* ( -z1(1)*sin(beta_rad)+z1(2)*cos(beta_rad) ) ));
%                     %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*sign(z1(1))*sign(cos(beta_rad))*sqrt(z1(1)*z1(1)+z1(2)*z1(2));
%             %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(1)-z1(2))/sqrt(tan(beta_rad)*tan(beta_rad)+1)>delta | abs(z1(3)-beta_rad)>delta
        %if abs(z1(1)-z1(2))/sqrt(2)>delta | abs(z1(3)-pi/4)>delta
        %if abs(z1(2))>delta | abs(z1(3))>delta % 尚未镇定到x轴上
%            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)-beta_rad)+k1*v(i)*sin(z1(3)-beta_rad)/(z1(3)-beta_rad)* ( -z1(1)*sin(beta_rad)+z1(2)*cos(beta_rad) ) ));
                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)-beta_rad)+k1*v(i)*sin(z1(3)-beta_rad)/(z1(3)-beta_rad)* ( -z1(1)*sin(beta_rad)+z1(2)*cos(beta_rad) ) ));
%                     %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)-beta_rad)+k1*v(i)*sin(z1(3)-beta_rad)/(z1(3)-beta_rad)* ( -z1(1)*sin(beta_rad)+z1(2)*cos(beta_rad) ) ));
%                     %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*sign(z1(1))*sign(cos(beta_rad))*sqrt(z1(1)*z1(1)+z1(2)*z1(2));
%             %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

%plot(x,y);




% 画速度和驱动角度矢量
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');
text(0.15,1.2,'x');
text(0.25,-0.8,'y');
text(0.35,0.8,'\theta');
xlabel('时间 (s)');                   % 给x轴标注
ylabel('x (m), y (m), \theta (rad)');             % 给y轴标注
grid
%-----------------------------------------