基于模型预测(MPC)的四轮转向车辆轨迹规划(Matlab代码实现)

    目录

💥1 概述

📚2 运行结果

🎉3 参考文献

👨‍💻4 Matlab代码

💥1 概述

在轨迹跟踪应用领域,通常 MPC 建模可根据机器人的控制方式选择基于运动学运动状态方程建模或者基于动力学运动状态方程建模。前者是根据车辆转向的几何学角度关系和速度位置关系来建立描述车辆的运动的预测模型,一般只适用于低速运动场景;后者是对被控对象进行综合受力分析,从受力平衡的角度建模,一般应用在高速运动场景,如汽车无人驾驶。

📚2 运行结果

主函数部分代码:

addpath(genpath('YALMIP-master'));
clear; close all;clc;
​
% constraints that is representing a car or a thiner road 
% final state with a higher weight to track the fianl state reference
​
%% Parameters definition
%
 model parametersparams.a_acc                = 1;                 % acceleration limit
params.a_dec                = 2;                   % decelerationlimit
params.delta_max            = 0.4;                 % maximum steering angle 
params.beta_max             = pi;                  % crabbing slip angle limit
params.beta_dot_max         = (pi/180)*(100);      % crabbing slip angle rate limit
params.l_f                  = 2;                   % distance between center of gravity and front axle
params.l_r                  = 2;                   % distance between center of gravity and rear axle
params.vehicle_width        = 2;                   % vehicle width
params.Ts                   = 0.1;                 % sampling time (both of MPC and simulated vehicle)
params.nstates              = 4;                   % number of states
params.ninputs              = 3;                   % number of inputs
​
% environment parameters
params.road                 = 'DLC';
params.activate_obstacles   = 0; 
params.obstacle_centers     = [10 -2; 20  0; 30 -2; 40 6]; % x and y coordinate of the 4 obstacles
params.obstacle_size        = [2 6];                    % size of the obstacles
switch params.road
    case 'real'
        [X,Y,X_park,Y_park]                   = data_generate(params.road);
    case 'DLC'
        [X,Y,X2,Y2]                   = data_generate(params.road);
    otherwise
        [X,Y]                   = data_generate(params.road);
end
params.X                    = X;
params.Y                    = Y;
params.lane_semiwidth       = 4;                        % semi-width of the lane
params.track_end_x          = X(end);                   % Tracking end X coordinate of the planner
params.track_end_y          = Y(end);                   % Tracking end Y coordinate of the planner
params.xlim                 = max(X);                   % x limit in the plot
params.ylim                 = max(Y);                   % y limit in the plot
​
​
​
% simulation parameters in frenet coord
params.s0                   = 0;                           % initial s coordinate
params.y0                   = params.lane_semiwidth/2;     % initial y coordinate
params.theta0               = pi/2;                        % initial relative heading angle
params.v0                   = 5;                          % initial speed
params.theta_c0             = pi/2;                        % initial heading angle of the curve
params.N_max                = 1000;                        % maximum number of simulation steps
params.vn                   = params.v0/2;                 % average speed during braking
params.vs                   = 1;                           % Stopping speed during the last stop 
​
%%% in this case, theta = theta_m (angle of the velocity vector)- theta_c (angle of the tangent vector of the curve)
​
% simulation parameters in cartesian coord %%%%% change according to
%
 different roadparams.x_c0                 = X(1)-params.y0; %initial x coordinate
params.y_c0                 = 0;                            % initial y coordinate
​
​
% plotting parameters
params.window_size          = 10;                       % plot window size for the
%during simulation, centered at the
%current position (x,y) of the vehicle
​
params.plot_full            = 1;                        % 0 for plotting only the window size,
​
​
% 1 for plotting from 0 to track_end
%% Simulation environment
%
 initializationsref         = zeros(params.N_max,1);
z            = zeros(params.N_max,params.nstates);             % z(k,j) denotes state j at step k-1(s and y)
z_cart       = zeros(params.N_max,3);                          % x,y coordinate in cartesian system and the heading angle
theta_c      = zeros(params.N_max,1);                          % initial value of theta_c
u            = zeros(params.N_max,params.ninputs);             % z(k,j) denotes input j at step k-1
z(1,:)       = [params.s0,params.y0,params.theta0,params.v0];  % definition of the intial state
z_cart(1,:)  = [params.x_c0,params.y_c0,params.theta0];
%theta_c(1)   = pi/2;    %%%%%% This value is changable according to different roads
comp_times   = zeros(params.N_max);                            % total computation time each sample
solve_times  = zeros(params.N_max);                            % optimization solver time each sample
i = 0;
N = 10;                                                 %predicted horizon length
m = params.N_max;
% formulating the reference of the system
switch params.road
    case 'real'
        path1 = [X;Y]';
        path2 = [X_park;Y_park]'
;        [S1, ~, ~, ~, ~] = getPathProperties(path1);
        num        = S1(end)/(params.v0*params.Ts);
        n_step1    = round(num);
        [S2, ~, ~, ~, ~] = getPathProperties(path2);
        num        = S2(end)/(params.vn*params.Ts);
        n_step2    = round(num);
        n_steps    = n_step1+n_step2;
        vq1         = linspace(Y(1),Y(end),n_step1);    % this need to be the 'variable' of the path function
        centerX1    = interp1(Y,X,vq1,'pchip');      % initialize the centerline y coordinate
        centerY1    = vq1;
        path_new_1   = [centerX1;centerY1]';
        vq2         = linspace(X_park(1),X_park(end),n_step2);
        centerY2    = interp1(X_park,Y_park,vq2,'
pchip
');      % initialize the centerline y coordinate
        centerX2    = vq2;
        path_new_2   = [centerX2;centerY2]'
;        path_new     = [path_new_1;path_new_2];
        [S, dX, dY, theta_c, C_c] = getPathProperties(path_new);
        theta_c_ref    = zeros(n_steps+N,1);
        C_c_ref        = zeros(n_steps+N,1);
        v_ref          = zeros(n_steps+N,1);
        for ii = 1:length(theta_c_ref)
            if ii<n_steps
                theta_c_ref(ii) = theta_c(ii);
                C_c_ref(ii)     = C_c(ii);
            else
                theta_c_ref(ii) = theta_c(end);
                C_c_ref(ii)     = C_c(end);
            end
        end
​

🎉3 参考文献

​[1]张鑫,吴伊凡,崔永琪.基于改进MPC算法的四旋翼无人机轨迹跟踪控制[J].电子技术与软件工程,2023,No.245(03):148-153.

部分理论引用网络文献,若有侵权联系博主删除。

  • 0
    点赞
  • 15
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
以下是基于 MPC 算法实现车辆轨迹跟踪的 MATLAB 代码示例: ```matlab %% 建立车辆动力学模型和环境模型 % 定义车辆动力学参数 m = 1000; % 质量 Cf = 16000; % 前轮侧向刚度 Cr = 18000; % 后轮侧向刚度 Lf = 1.2; % 前轴到重心距离 Lr = 1.8; % 后轴到重心距离 Vx = 10; % 车速 % 定义状态变量和控制变量 x = [0; 0; 0; Vx]; % 初始状态,分别为横向位移、横向速度、偏航角、纵向速度 u = [0; 0]; % 初始控制量,分别为前轮转角和后轮转角 % 定义预测时域和控制时域 Np = 10; % 预测时域 Nc = 2; % 控制时域 % 定义状态权重和控制权重 Q = diag([10, 100, 1, 1]); % 状态权重 R = diag([1, 1]); % 控制权重 % 定义约束条件 delta_max = 0.5; % 前轮转角限制 alpha_max = deg2rad(10); % 前轮侧偏角限制 % 定义 MPC 控制器 mpc_controller = mpc(tf({Cf, Cr}, {m*Lr, m*Lf+ m*Lr, m*Vx, 0}), Vx, Np, Nc, Q, R, [], [], [delta_max; alpha_max], [], []); %% MPC 控制循环 for i = 1:100 % 从传感器获取当前状态 y = [0; 0; 0; Vx]; % 假设当前状态与初始状态相同 % 更新 MPC 控制器并生成控制命令 mpc_controller = mpc_controller.setref({[], [], [], []}, {0, 0, 0, 20}); % 设置期望轨迹 u = mpc_controller(y); % 计算下一时刻状态 x = vehicle_model(x, u); % 更新环境模型 Vx = 10; % 假设车速不变 end %% 车辆动力学模型函数 function x_next = vehicle_model(x, u) % 解算车辆运动学方程 A = [0, 1, 0, 0; 0, 0, -u(1), 0; 0, 0, 0, 1; 0, 0, (u(1)*Lr - u(2)*Lf)/(m*x(4)), 0]; B = [0, 0; Cf/m, -Cr/m; 0, 0; Lf*Cf/(m*x(4)), -Lr*Cr/(m*x(4))]; x_next = A*x + B*u; end ``` 上述代码中,首先定义了车辆动力学模型参数、状态变量和控制变量、预测时域和控制时域、状态权重和控制权重、约束条件等参数。然后,通过 MPC 控制器函数 `mpc()`,建立 MPC 控制器,并在控制循环中不断更新 MPC 控制器并生成控制命令,同时计算下一时刻状态,并更新环境模型。最后,定义了车辆动力学模型函数 `vehicle_model()`,用于解算车辆运动学方程。 需要注意的是,这只是一个简单的示例,实际应用中需要根据具体情况进行参数调整和优化。

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

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

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值