Matlab类阿克曼车机器人运动学演示

news2024/11/27 13:39:56

在这里插入图片描述

v1是后驱动轮轮速， v2是转向角变化速度，实际上我们只需要关注XQ， YQ和Phi的变化率。通过这三项和时间步长，我们就可以计算出变化量，再结合初始值就能推断出每个时刻的值。

% 清理当前运行环境
% 清除所有变量
clear all;
% 关闭所有窗口
close all;

% 车辆参数
% 轴距
Vehicle.WheelBase = 3.7;
% 车辆宽度
Vehicle.Width = 2.6;
% 车尾到车头前端的距离
Vehicle.LF = 4.5; 
% 车尾到车尾后端的距离
Vehicle.LB = 1.0;
% 最大转向角， rad
Vehicle.MaxSteer = 0.6;
% 最小转弯半径
Vehicle.MinCircle = Vehicle.WheelBase/tan(Vehicle.MaxSteer);
% 后轮轮速
Vehicle.Velocity = 0.6;
% 转向角
Vehicle.TurnningAngle = 0.3;
% 方位角
Vehicle.Yaw = 0.5;
% 全局X坐标
Vehicle.X = 0.0;
% 全局Y坐标
Vehicle.Y = 0.0;

% 后轴中心点
BackAxisCenter = [0.0, 0.0]

scatter(BackAxisCenter(1), BackAxisCenter(2), 'r');
% 使x y坐标的比例一致， 避免图像变形
 %axis equal
xlim([-30, 30])
ylim([-30, 30])
hold on;
dt = 1;
for i=0:dt:1000
    % 清屏
    cla
    % 更新矩阵
    PosTrans = UpdateTransMatrix(Vehicle);
    % 计算当前状态：[dX, dY, dYaw]
    state = PosTrans * Vehicle.Velocity;
    % 更新后轴中心点位置
    BackAxisCenter = BackAxisCenter + [state(1), state(2)] * dt;
    % 更新航向角
    Vehicle.Yaw = Vehicle.Yaw + state(3) * dt;
    % 将航向角限制在-pi到pi
    Vehicle.Yaw = ConstrainToPi(Vehicle.Yaw);
    % 更新机器人全局坐标
    Vehicle.X = BackAxisCenter(1);
    Vehicle.Y = BackAxisCenter(2);
    % 可视化车辆轮廓
    Visulization(Vehicle);
    % 暂停一段时间
    pause(0.01)
end

% 将angle限制在-pi到pi
function result = ConstrainToPi(angle)
    result = mod(angle + pi, 2*pi) - pi;
end

% 计算状态转移矩阵
function result = UpdateTransMatrix(vehicle)
    result = [cos(vehicle.Yaw), sin(vehicle.Yaw), (1.0/vehicle.WheelBase) * tan(vehicle.TurnningAngle)]';
end


% 可视化
function Visulization(vehicle)
    px = vehicle.X;
    py = vehicle.Y;
    % 根据后轴中心的位姿计算车辆边框的位姿
    [vehx,vehy] = getVehTran(px,py,vehicle);
    % 车辆边框
    h1 = plot(vehx,vehy,'k'); 
    % 车辆后轴中心
    h2 = plot(px, py,'rx','MarkerSize',10); 
    xlabel('x');
    ylabel('y');
end

% 根据后轴中心的位姿计算车辆边框的位姿
function [x,y] = getVehTran(x,y,vehicle)
    W = vehicle.Width;
    LF = vehicle.LF;
    LB = vehicle.LB;
    
    % 车辆的边框由四个角点确定
    Cornerfl = [LF, W/2]; % 左前方角点
    Cornerfr = [LF, -W/2]; % 右前方角点
    Cornerrl = [-LB, W/2]; % 左后方角点
    Cornerrr = [-LB, -W/2]; % 右后方角点
    Pos = [x,y]; % 后轴中心坐标
    dcm = angle2dcm(-vehicle.Yaw, 0, 0); % 计算四个角点的旋转矩阵,由于是刚体的一部分，旋转矩阵相同，将角度转换为方向余弦矩阵，旋转顺序是ZYX
    
    tvec = dcm*[Cornerfl';0]; % 旋转变换，Cornerfl旋转后形成的列向量，位置向量3*1，最后一个是z坐标
    tvec = tvec';
    Cornerfl = tvec(1:2)+Pos; % 平移变换
    
    tvec = dcm*[Cornerfr';0];
    tvec = tvec';
    Cornerfr = tvec(1:2)+Pos;
    
    tvec = dcm*[Cornerrl';0];
    tvec = tvec';
    Cornerrl = tvec(1:2)+Pos;
    
    tvec = dcm*[Cornerrr';0];
    tvec = tvec';
    Cornerrr = tvec(1:2)+Pos;
    
    % 返回车辆边框四个角点的x,y坐标
    x = [Cornerfl(1),Cornerfr(1),Cornerrr(1),Cornerrl(1),Cornerfl(1)];
    y = [Cornerfl(2),Cornerfr(2),Cornerrr(2),Cornerrl(2),Cornerfl(2)];
end