基于运动学模型的轨迹跟踪控制-Toy模板网

这篇具有很好参考价值的文章主要介绍了基于运动学模型的轨迹跟踪控制。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

基于运动学模型的轨迹跟踪控制

章四基于运动学模型的轨迹跟踪控制

MPC（4）基于运动学模型的轨迹跟踪控制器设计

无人驾驶车辆模型预测控制（龚建伟）第四章基于运动学模型的轨迹跟踪控制（仿真部分）

无人车辆在惯性坐标系中，车辆必须从一个给定的初始状态出发，这个初始点可以在期望轨迹上，也可以不在期望轨迹上

期望轨迹和参考控制输入可以由轨迹规划模块提供，也可以预先设定，本章中为预先设定

基于运动学模型的轨迹跟踪控制

假设系统可提供两类有效信息：

可行驶区域的几何描述、路面特征及路面摩擦系数
车辆位置及内部状态，包括横纵向速度、加速度、轮速等参数

即轨迹跟踪控制是在周围环境及车辆内部状态完全已知的情况下进行的，不涉及环境感知和车辆状态的估计

轨迹跟踪控制器设计

车辆运动学建模

基于运动学模型的轨迹跟踪控制

k+1时刻的状态可由k时刻状态及k时刻输入计算得到

将上式线性化可以得到新的状态空间表达式，推导可以得到系统的预测输出表达式

目标函数设计

在传统目标函数式中加入权重系数和松弛因子，并用控制增量代替控制量

不仅能对控制增量进行直接限制，同时可以防止执行过程中出现无可行解的情况

约束条件设计

主要考虑控制过程中控制量极限约束和控制增量约束

约束条件中涉及的具体数值需要通过纵横向跟踪能力测试获得

参考路径的自适应分段拟合

路网文件提供的参考路径通常是一系列不包含曲率信息的离散路点序列

需要对这些离散路点进行拟合，得到期望轨迹，主要有贝塞尔及B样条两种拟合方法

CarSim与Simulink联合仿真

Simulink模型

基于运动学模型的轨迹跟踪控制

CarSim配置

基于运动学模型的轨迹跟踪控制

S-Function

% 主函数
function [sys,x0,str,ts] = YDModelTrajectoryTracking(t,x,u,flag)
    switch flag,
        case 0 
            [sys,x0,str,ts] = mdlInitializeSizes; 
        case 2 
            sys = mdlUpdates(t,x,u); 
        case 3 
            sys = mdlOutputs(t,x,u); 
        case {1,4,9} 
            sys = [];            
        otherwise 
            error(['unhandled flag = ',num2str(flag)]); 
    end
% 初始化子函数
function [sys,x0,str,ts] = mdlInitializeSizes
    sizes = simsizes;
    sizes.NumContStates  = 0;
    sizes.NumDiscStates  = 3;  
    sizes.NumOutputs     = 2;
    sizes.NumInputs      = 3;
    sizes.DirFeedthrough = 1;
    sizes.NumSampleTimes = 1;
    sys = simsizes(sizes);
    x0  = [0;0;0];
    str = [];
    ts  = [0.05 0];
    global U; U=[0;0];
% 状态更新子函数
function sys = mdlUpdates(t,x,u)
    sys = x;
% 输出子函数
function sys = mdlOutputs(t,x,u)
    % 半径为25m的圆形轨迹, 圆心为(0，35), 速度为5m/s
    % 轴距L=2.6,根据阿克曼转向车辆运动学模型求取参考值
    r(1) = 25 * sin( 0.2 * t );           % 纵向参考坐标
    r(2) = 35 - 25 * cos( 0.2 * t );      % 横向参考坐标
    r(3) = 0.2 * t;                       % 参考航向角
    vd1  = 5;                             % 纵向参考车速
    vd2  = 0.104;                         % 参考前轮偏角
    
    % 参数
    global a b u_piao;     
    global U;              
    global kesi;
    Nx  = 3;                % 状态量的个数
    Nu  = 2;                % 控制量的个数
    Np  = 80;               % 预测步长
    Nc  = 30;               % 控制步长
    T   = 0.05;             % 采样时间
    L   = 2.60;             % 车辆的轴距
    u_piao = zeros(Nu, 1);
    
    % 输入向量单位转化
    yaw_angle =u(3)*pi/180; % CarSim输出的角度转换为弧度
    
    %% 模型预测控制器设计
    % 1.误差模型
    a = [1    0   -vd1*sin(r(3))*T;
         0    1    vd1*cos(r(3))*T;
         0    0                  1;];
    b = [cos(r(3))*T                           0;
         sin(r(3))*T                           0;
         tan(vd2)*T/L       vd1*T/((cos(vd2))^2);];
    % 为了转化为标准二次规划问题，进行矩阵转化
    % 状态和控制合并为一个新的向量
    kesi = zeros(Nx+Nu,1);    
    kesi(1) = u(1) - r(1);               
    kesi(2) = u(2) - r(2);               
    heading_offset = yaw_angle - r(3);   
    if (heading_offset < -pi)
        heading_offset = heading_offset + 2*pi;
    end
    if (heading_offset > pi)
        heading_offset = heading_offset - 2*pi;
    end
    kesi(3) = heading_offset;
    kesi(4) = U(1); 
    kesi(5) = U(2); 
    A_cell=cell(2,2);
    B_cell=cell(2,1);
    A_cell{1,1}=a;
    A_cell{1,2}=b;
    A_cell{2,1}=zeros(Nu,Nx);
    A_cell{2,2}=eye(Nu);
    B_cell{1,1}=b;
    B_cell{2,1}=eye(Nu);
    A=cell2mat(A_cell);
    B=cell2mat(B_cell);
    C=[ 1 0 0 0 0;
        0 1 0 0 0;
        0 0 1 0 0;];
    % 2.预测
    PHI_cell=cell(Np,1);
    THETA_cell=cell(Np,Nc);
    for j=1:1:Np
        PHI_cell{j,1}=C*A^j;
        for k=1:1:Nc
            if k<=j
                THETA_cell{j,k}=C*A^(j-k)*B;
            else 
                THETA_cell{j,k}=zeros(Nx,Nu);
            end
        end
    end
    PHI=cell2mat(PHI_cell);
    THETA=cell2mat(THETA_cell);
    % 3.目标函数
    Row = 10;              %松弛因子
    Q = eye(Nx * Np, Nx * Np);    
    R = 5*eye(Nu * Nc);
    % 二次目标项
    H_cell=cell(2,2);
    H_cell{1,1}=THETA'*Q*THETA+R;
    H_cell{1,2}=zeros(Nu*Nc,1);
    H_cell{2,1}=zeros(1,Nu*Nc);
    H_cell{2,2}=Row;
    H=cell2mat(H_cell);
    % 线性目标项
    error=PHI*kesi;
    f_cell=cell(1,2);
    f_cell{1,1} = (error'*Q*THETA);
    f_cell{1,2} = 0;
    f=cell2mat(f_cell);
    % 4.约束
    A_t=zeros(Nc,Nc);
    for p=1:1:Nc
        for q=1:1:Nc
            if q<=p 
                A_t(p,q)=1;
            else 
                A_t(p,q)=0;
            end
        end 
    end 
    A_I=kron(A_t,eye(Nu));
    Ut=kron(ones(Nc,1), U);
    % 控制量约束
    umin=[-0.2;  -0.436];
    umax=[ 0.2;   0.436]; 
    Umin=kron(ones(Nc,1),umin);
    Umax=kron(ones(Nc,1),umax);
    A_cons_cell={A_I zeros(Nu*Nc, 1); -A_I zeros(Nu*Nc, 1)};
    b_cons_cell={Umax-Ut;-Umin+Ut};
    A_cons=cell2mat(A_cons_cell);
    b_cons=cell2mat(b_cons_cell);
    % 控制增量及松弛因子约束
    delta_umin = [-0.05;  -0.0082]; 
    delta_umax = [ 0.05;   0.0082];
    delta_Umin = kron(ones(Nc,1),delta_umin);
    delta_Umax = kron(ones(Nc,1),delta_umax);
    lb = [delta_Umin;  0];
    ub = [delta_Umax; 10];
    % 5.优化求解
    options = optimset('Algorithm','interior-point-convex'); 
    warning off all    
    [X, fval,exitflag]=quadprog(H, f, A_cons, b_cons,[], [],lb,ub,[],options);
    % 6.输出   
    u_piao(1)=X(1);
    u_piao(2)=X(2);
    U(1)=kesi(4)+u_piao(1);
    U(2)=kesi(5)+u_piao(2);
    u_real(1) = U(1) + vd1;
    u_real(2) = U(2) + vd2;
    sys= [u_real(1); u_real(2)];