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到piVehicle.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); % 计算四个角点的旋转矩阵,由于是刚体的一部分，旋转矩阵相同，将角度转换为方向余弦矩阵，旋转顺序是ZYXtvec = 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