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

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

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

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

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

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

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

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

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

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

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

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

轨迹跟踪控制器设计

车辆运动学建模

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

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)];

仿真结果如下,其中红色为参考轨迹,蓝色为实际轨迹

实际轨迹与期望轨迹跟踪误差较小,跟踪效果良好,但因只考虑运动学,故仅适用于低速场景

基于运动学模型的轨迹跟踪控制文章来源地址https://www.toymoban.com/news/detail-445019.html

到了这里,关于基于运动学模型的轨迹跟踪控制的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • 车辆行驶控制运动学模型的matlab建模与仿真,仿真输出车辆动态行驶过程

    目录 1.课题概述 2.系统仿真结果 3.核心程序与模型 4.系统原理简介 4.1 基本假设 4.2 运动学方程 5.完整工程文件 车辆行驶控制运动学模型的matlab建模与仿真,仿真输出车辆动态行驶过程. 版本:MATLAB2022a        车辆运动学模型从几何学的角度研究车辆的运动规律。包括车辆的空

    2024年01月20日
    浏览(72)
  • 基于STM32F103控制舵机 仿真 简单二轴机器人逆运动学(20220615完成 正在处理三轴)

    本项目基于 正点原子精英(stm32f103zet6) 控制小舵机 模拟 二轴机器人逆运动学控制。 目录 概述: 1. 我的问题总结 2.stm32控制部分 与机械部分 2.1 对于二轴机器人设计 2.2 stm32 输出 pwm 2.3 舵机控制 3.正运动学 3.1 D-H建模下对姿态的描述 4.逆运动学 4.1 几何解法 4.2 代数解法 4.3 多重

    2023年04月20日
    浏览(64)
  • 车辆运动学和动力学模型概述

    对车辆建立数字化模型,分为车辆运动学和动力学模型。 车辆运动学模型(Kinematic Model )把车辆完全视为刚体,主要考虑车辆的位姿(位置坐标、航向角)、速度、前轮转角等的关系,不考虑任何力的影响。 1.前提假设: 不考虑Z轴方向运动,默认车在二维平面上的运动 假设

    2024年02月13日
    浏览(51)
  • 连续体机器人的正逆向运动学模型-(4)雅可比矩阵解决逆向运动

    Winter:连续体机器人的正逆向运动学模型-(1)概述 Winter:连续体机器人的正逆向运动学模型-(2)DH参数法与雅可比矩阵 Winter:连续体机器人的正逆向运动学模型-(3)利用DH参数法解决正向运动学 连续体机器人的正逆向运动学模型-(4)雅可比矩阵解决逆向运动 - 知乎 《

    2024年02月04日
    浏览(47)
  • 基于 SIMULINK 平面五杆机构运动学仿真

    ] 以 MATLAB 程序设计语言为平台 , 以平面可调五杆机构为主要研究对象 , 给定机构的尺寸参数 , 列出所 要分析机构的闭环矢量方程, 使用 MATLAB 软件中 SIMULINK 仿真工具 , 在 SIMULINK 模型窗口下建立数学模 型、 进行运动学仿真 , 优化机构参数 . 该方法编程工作量小 、 轨迹图形显

    2023年04月08日
    浏览(35)
  • 机器人运动学标定:基于DH建模方法

    作者:桂凯 链接:https://www.zhihu.com/question/401957723/answer/1298513878 来源:知乎 著作权归作者所有。商业转载请联系作者获得授权,非商业转载请注明出处。 当然,运动学标定这种很基础的问题,理论已非常成熟了,基于激光或拉线编码器的标定系统也已经商业化了。我们在接

    2024年02月12日
    浏览(52)
  • Matlab/Simulink六自由度机器人运动学与控制系统仿真(一)【附源文件】

    本文章为系列文章,以IRB1600机器人为例,建立机器人正运动、逆运动学、控制系统模型,并在simulink中进行仿真,与理论计算结果进行对比验证 (一) irb1600机器人坐标系建立、正运动学计算与simulink验证 (二) 机器人逆运动学计算 (三) 机器人运动学控制系统仿真 本文以

    2024年01月16日
    浏览(70)
  • 机器人控制算法十之运动学与动力学:DH建模与雅可比矩阵详解

    简介: 正运动学建模 逆运动学求解 动力学-雅可比矩阵 … 1.1 标准DH建模方法 关于标准DH建模的详细说明,可参考:一文带你完全掌握机器人DH参数建模(详细步骤+实例+代码) 例:Puma560: 6自由度关节机器人,6个关节都是旋转副;前3个关节用于确定手腕参考点的位置,后3个关

    2024年02月14日
    浏览(44)
  • 基于C#的机器人仿真平台和机器人运动学算法实现

    一、平台搭建 1.利用wpf自带的库进行机器人各关节导入 相关代码段: 导入效果如图: 效果视频: 2.通过正运动学显示机器人当前位置信息 拖动机器人并且实时改变其位置信息: xaml代码部分: 算法部分:  3.功能实现(在X/Y/Z轴上设置一个移动距离,然后机器人自动移动该

    2024年02月16日
    浏览(55)
  • MATLAB仿真Gough-Stewart并联机器人斯图尔特6自由度并联机器人逆运动学仿真 动力学控制pid控制

    MATLAB仿真Gough-Stewart并联机器人斯图尔特6自由度并联机器人逆运动学仿真 动力学控制pid控制 1.搭建了六自由度Stewart并联机器人simulink simscape仿真模型 2.建立了逆向运动学仿真 输入位置和姿态求解各个杆长 3.运用pid控制器进行动力学跟踪控制 使用MATLAB进行了Gough-Stewart并联机器

    2024年01月16日
    浏览(58)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包