基于车辆运动学生成轨迹,MATLAB代码


%%


function [xyphi]=plant()

clear all;
close all;
clc;

%%

l=2.9;             %轴距
ts=0.001;
t=0:ts:50;      %定义时间序列
v(1:length(t))=5;  %定义速度序列,匀速5m/s
delta_deg=5*sin(0.5*t)+1*sin(0.2*t);  % 前轮转角  单位为°
delta_rad=(delta_deg/180)*pi;         % 前轮转角  单位为rad
%%

delta=delta_rad;
%初始值
x(1)=0;
y(1)=0;
phi(1)=0;
x0=x(1);
y0=y(1);
phi0=phi(1);
for j=1:1:length(v)
   times(j+1)=j*ts; 
   x(j+1)=x0+v(j)*cos(phi0)*ts; 
   y(j+1)=y0+v(j)*sin(phi0)*ts;
   phi(j+1)=phi0+v(j)*tan(delta(j))/l*ts;
   phi0=phi(j+1);
   x0=x(j+1);
   y0=y(j+1);
end
xyphi=[x;y;phi];%后轴中心点位置和横摆角
figure(1);

plot(x,y);
axis equal;

end

  • 3
    点赞
  • 22
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
基于模型预测控制的车辆轨迹跟踪是一种控制算法,通过对车辆运动模型进行建模和预测,使车辆自动跟踪设定的轨迹。下面是一个基于模型预测控制的车辆轨迹跟踪的简单Matlab代码示例: ```matlab % 建立车辆运动模型 M = 500; % 车辆质量 (kg) C = 50; % 空气阻力系数 A = 4; % 车辆截面积 (m^2) Vx = 10; % 车辆速度 (m/s) % 设定轨迹相关参数 ref_x = [0, 5, 10, 15]; % x轴方向的轨迹点 ref_y = [0, 2, 0, 2]; % y轴方向的轨迹点 % 控制器参数 N = 20; % 预测时域 Q = 50; % 状态误差权重 R = 1; % 控制输入误差权重 Ts = 0.1; % 采样时间间隔 % 预测模型建立 A_pred = eye(N); % 状态矩阵 B_pred = zeros(N,1); % 输入矩阵 C_pred = zeros(N,1); % 输出矩阵 D_pred = zeros(N,1); % 无效矩阵 for i=1:N A_pred(i+1:end,i) = -1; B_pred(i,i) = Vx*Ts/M; C_pred(i,i) = -C*Vx*Ts/(M * A); end % 轨迹跟踪控制 x = zeros(N,1); % 状态变量初始化 y = zeros(N,1); % 输出变量初始化 u = zeros(N,1); % 输入变量初始化 x_pred = zeros(N,1); % 预测状态变量初始化 y_pred = zeros(N,1); % 预测输出变量初始化 u_pred = zeros(N,1); % 预测输入变量初始化 for k=1:length(ref_x) % 预测 x_pred(1:N-1) = x(2:N); y_pred(1:N-1) = y(2:N); x_pred(N) = x(N) + Ts * (Vx * cos(y(N)) - C * Vx^2 / (M * A)); y_pred(N) = y(N) + Ts * (Vx * sin(y(N))); % 控制器设计 H = 2 * (C_pred' * Q * C_pred + R); % 二次型矩阵 f = -2 * (x_pred' * Q * C_pred - ref_x(k) * C_pred' * Q); % 目标函数 Aeq = []; beq = []; lb = []; ub = []; u_pred = quadprog(H,f,Aeq,beq,[],[],lb,ub); % 二次规划求解 % 控制指令 u(k) = u_pred(1); % 系统响应 x(1:N-1) = x(2:N); y(1:N-1) = y(2:N); x(N) = x(N) + Ts * (Vx * cos(y(N)) - C * Vx^2 / (M * A)); y(N) = y(N) + Ts * (Vx * sin(y(N))); end % 轨迹结果显示 figure plot(ref_x, ref_y, 'r--', 'LineWidth', 1.5); % 设定轨迹 hold on plot(x, y, 'b-', 'LineWidth', 1.5); % 实际轨迹 legend('设定轨迹', '实际轨迹') xlabel('x (m)') ylabel('y (m)') ``` 这个简单的Matlab代码实现了一个基于模型预测控制的车辆轨迹跟踪算法。代码中定义了车辆运动模型和轨迹信息,并根据控制器参数建立预测模型。通过循环,对每一个设定的轨迹点进行轨迹跟踪控制,求解二次规划问题并更新控制指令,然后更新车辆的状态变量和输出变量。最后,将设定轨迹和实际轨迹进行显示和比较。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值