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


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

matlab 复制代码
% 清理当前运行环境
% 清除所有变量
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
相关推荐
睡觉狂魔er3 小时前
机器人仿真(2)Ubuntu24.04下RTX5090配置IsaacSim与IsaacLab
机器人
青春不败 177-3266-052012 小时前
MATLAB近红外光谱分析技术及实践技术应用
随机森林·机器学习·支持向量机·matlab·卷积神经网络·遗传算法·近红外光谱
想要成为计算机高手17 小时前
11. isaacsim4.2教程-Transform 树与Odometry
人工智能·机器人·自动驾驶·ros·rviz·isaac sim·仿真环境
AI赋能19 小时前
自动驾驶训练-tub详解
人工智能·深度学习·自动驾驶
RPA+AI十二工作室21 小时前
影刀RPA_抖音评价获取_源码解读
运维·机器人·自动化·源码·rpa·影刀
音视频牛哥1 天前
如何打造毫秒级响应的RTSP播放器:架构拆解与实战优化指南
人工智能·机器人·音视频开发
Monkey PilotX1 天前
VLA:自动驾驶的“新大脑”?
人工智能·机器学习·计算机视觉·自动驾驶
简简单单做算法1 天前
基于LSTM深度学习网络的视频类型分类算法matlab仿真
深度学习·matlab·分类·lstm·视频类型分类
计算机sci论文精选1 天前
CVPR 前沿洞察 | 人机交互论文出圈,引领交互模式变革
计算机网络·机器学习·机器人·人机交互·cvpr·计算机系统·并行与分布计算
WSSWWWSSW2 天前
认识自我的机器人:麻省理工学院基于视觉的系统让机器了解自身机体
人工智能·机器人