% -----------------------------------------------------
% 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)
评论0