面向阿克曼移动机器人（自行车模型）的LQR（最优二次型调节器）路径跟踪方法

news2024/10/7 19:22:34

        线性二次调节器（Linear Quadratic Regulator，LQR）是针对线性系统的最优控制方法。LQR 方法标准的求解体系是在考虑到损耗尽可能小的情况下, 以尽量小的代价平衡其他状态分量。一般情况下，线性系统在LQR 控制方法中用状态空间方程描述，性能能指标函数由二次型函数描述。

LQR 方法存在以下优点：

  1. 最小能量消耗和最高路径跟踪精度。
  2. 求解时能够考虑多状态情况。
  3. 鲁棒性较强。

缺点：

  1. 控制效果和模型精确程度有很大相关性。
  2. 实时计算状态反馈矩阵和控制增益。

一、系统模型

1.1 车辆模型

        一般来说阿克曼移动机器人可以简化为自行车模型，是一个非线性时变系统，工程上一般通过在工作平衡点附近差分线性化转化为线性系统来来分析和控制，具体就不推导了，我直接给出模型。

1.2 线性系统状态反馈控制示意图

状态反馈是线性能控线性系统镇定的一个有效方法，主要是通过极点配置方法寻找一组非正的闭环极点使得闭环系统大范围渐进稳定。

 A，B，C分别代表系统矩阵、输入矩阵和输出矩阵，K是待设计的状态反馈增益。

二、控制器设计

2.1 代价函数泛函设计

最优控制里，代价函数一般设计为控制性能和控制代价的范数加权和，形式如下

其中，期望和实际的误差系统定义为

2.2 最优状态反馈控制律推导

当想要状态与期望状态之间的误差越差越小，同时控制消耗更少的能量。求解极小值点时，新定义拉格朗日函数如下 

在拉格朗日函数基础上对各个优化变量的一阶导为零 ，得

 当\Delta_{x(t)}=0时候，

推导LQR控制律时候，设\lambda(t)=P_tx(t) ,求偏导后可得

由 得

 由于状态量初始不为零，只能是

又由于当上述方程成立时候，P收敛到了期望的范围 ，\dot{P}为零，因此得到立卡提方程形式的矩阵微分方程

综上，通过迭代或者近似方法求解上述立卡提方程后，最优的控制律为

 2.3 连续立卡提方程求解流程

For~~ iteration=1: iteration_{max}\\ ~~~~~~~\bar{P}=Q+A^{\top}PB(R+B^TPB)^{-1}B^{\top}PA\\ ~~~~~~~criteria=\overline\lambda(\vert\bar{P}-P\vert)\\ ~~~~~~~IF~~criteria<\epsilon\\ ~~~~~~~~~~Break\\ ~~~~~~P=\bar{P}

三、具体实现代码

3.1 main函数

close all
clear;
clc; 
cx = [];
cy= [];
y0 = @(t_step) 10*sin(2 * t_step + 1);
x0_dot= @(t_step) 5 * 2 * cos(2 * t_step + 1);
x0 = @(t_step) -40*cos(t_step + 0.5);
for theta=0:pi/200:2*pi
    cx(end + 1) = x0(theta);
    cy(end + 1) = y0(theta);
end
refer_path_primary= [cx', cy'];
x = refer_path_primary(:, 1)';
y = refer_path_primary(:, 2)';
points = [x; y]';
ds = 0.1 ;%等距插值处理的间隔
distance = [0, cumsum(hypot(diff(x, 1), diff(y, 1)))]';
distance_specific = 0:ds:distance(end);
hypot(diff(x, 1), diff(y, 1));
diff(x, 1);
diff(y, 1);
s = 0:ds:distance(end);
refer_path= interp1(distance, points, distance_specific, 'spline');
figure(1)
% 绘制拟合曲线
plot(refer_path(:, 1), refer_path(:, 2),'b-',x, y,'r.');
hold on
refer_path_x = refer_path(:,1);  % x
refer_path_y = refer_path(:,2); % y
for i=1:length(refer_path)
    if i==1
         dx = refer_path(i + 1, 1) - refer_path(i, 1);
         dy = refer_path(i + 1, 2) - refer_path(i, 2);
         ddx = refer_path(3, 1) + refer_path(1, 1) - 2 * refer_path(2, 1);
         ddy = refer_path(3, 2) + refer_path(1, 2) - 2 * refer_path(2, 2);
    elseif  i==length(refer_path)
         dx = refer_path(i, 1) - refer_path(i - 1, 1);
         dy = refer_path(i, 2) - refer_path(i - 1, 2);
         ddx = refer_path(i, 1) + refer_path(i - 2, 1) - 2 * refer_path(i - 1, 1);
         ddy = refer_path(i, 2) + refer_path(i - 2, 2) - 2 * refer_path(i - 1, 2);
    else
          dx = refer_path(i + 1, 1) - refer_path(i, 1);
          dy = refer_path(i + 1, 2) - refer_path(i, 2);
          ddx = refer_path(i + 1, 1) + refer_path(i - 1, 1) - 2 * refer_path(i, 1);
          ddy = refer_path(i + 1, 2) + refer_path(i - 1, 2) - 2 * refer_path(i, 2);
    end
    refer_path(i,3)=atan2(dy, dx);%yaw
    refer_path(i,4)=(ddy * dx - ddx * dy) / ((dx ^ 2 + dy ^ 2) ^ (3 / 2));
end
figure(2)
plot(refer_path(:, 3),'b-');
figure(3)
plot(refer_path(:, 4),'b-')
% 
%%目标及初始状态
L=2;%车辆轴距
v=2;%初始速度
dt=0.05;%时间间隔
goal=refer_path(end,1:2);
x_0=refer_path_x(1);
y_0=refer_path_y(1);
psi_0 = refer_path(1, 3);
% %运动学模型
ugv = KinematicModel(x_0, y_0, psi_0, v, dt, L);
Q = eye(3) * 3.0;
R = eye(2) * 2.0;
robot_state = zeros(4, 1);
step_points = length(refer_path(:, 1));
for i=1:1:step_points
    robot_state(1)=ugv.x;
    robot_state(2)=ugv.y;
    robot_state(3)=ugv.psi;
    robot_state(4)=ugv.v;
    [e, k, ref_yaw, min_idx] = calc_track_error(robot_state(1), robot_state(2), refer_path);
    ref_delta = atan2(L * k, 1);
    [A, B] = state_space( robot_state(4), ref_delta, ref_yaw, dt, L);
    delta = lqr(robot_state, refer_path, min_idx, A, B, Q, R);
    delta = delta + ref_delta;
    [ugv.x, ugv.y, ugv.psi, ugv.v] = update(0, delta, dt, L, robot_state(1), robot_state(2),robot_state(3), robot_state(4));
    ugv.record_x(end + 1 ) = ugv.x;
    ugv.record_y(end + 1 ) = ugv.y;
    ugv.record_psi(end + 1 ) = ugv.psi;
     ugv.record_phy(end + 1 ) = ref_delta;
end
figure(4)
% 绘制拟合曲线
%  scr_size = get(0,'screensize');
% set(gcf,'outerposition', [1 1 scr_size(4) scr_size(4)]);
plot(ugv.record_x , ugv.record_y, Color='m',LineStyle='--',LineWidth=2);
axis([-40,40,-40,40])
grid on
hold on
% 绘制车辆曲线
axis equal
for ii = 1:1:length(ugv.record_x)
       
        h =  PlotCarbox(ugv.record_x(ii), ugv.record_y(ii), ugv.record_psi(ii), 'Color', 'r',LineWidth=2);
        h1 = plot(ugv.record_x(1:ii), ugv.record_y(1:ii),'Color', 'b');
        th1 = text(ugv.record_x(ii), ugv.record_y(ii)+10, ['#car', num2str(1)], 'Color', 'm');
        set(th1, {'HorizontalAlignment'},{'center'});
        h2 = PlotCarWheels(ugv.record_x(ii), ugv.record_y(ii), ugv.record_psi(ii),ugv.record_phy(ii),'k',LineWidth=2);
        h3 = plot(ugv.record_x(1:ii) , ugv.record_y(1:ii), Color='b',LineStyle='-',LineWidth=4);
        drawnow
        delete(h); delete(h1);delete(th1);delete(h3);
        for jj = 1:1:size(h2)
            delete(h2{jj});
        end
end
% 
function [P] =  cal_Ricatti(A, B, Q, R)
 Qf = Q;
 P = Qf;
  iter_max = 100;
  Eps = 1e-4;
 for step = 1:1:iter_max
     P_bar = Q + A' * P * A - A' * P * B * pinv(R + B' * P *B) * B' * P * A;
     criteria = max(abs(P_bar - P));
     if  criteria < Eps
         break;
     end
     P = P_bar;
 end

end
%%LQR控制器
function[u_star]=lqr(robot_state, refer_path, s0, A, B, Q, R)
    x = robot_state(1:3) - refer_path(s0,1:3)';
    P =  cal_Ricatti(A, B, Q, R);
    K= -pinv(R + B' * P * B) * B' * P * A;
    u = K * x;%状态反馈
    u_star = u(2);
end

function [e, k, yaw, min_idx]=calc_track_error(x, y, refer_path)
    p_num = length(refer_path);
    d_x = zeros(p_num, 1);
    d_y = zeros(p_num, 1);
    d = zeros(p_num, 1);
    for i=1:1:p_num 
        d_x(i) = refer_path(i, 1) - x; 
        d_y(i) = refer_path(i, 2) - y;
    end
    for i=1:1:p_num 
        d(i) = sqrt(d_x(i) ^2 + d_y(i) ^ 2) ;
    end
    [~, min_idx] = min(d); 
    yaw = refer_path(min_idx, 3);
    k = refer_path(min_idx, 4);
    angle= normalize_angle(yaw - atan2(d_y(min_idx), d_x(min_idx)));
    e = d(min_idx);
    if angle < 0
        e = e*(-1);
    end
end
%%将角度取值范围限定为[-pi,pi]
function [angle]=normalize_angle(angle)
    while angle > pi
        angle = angle - 2*pi;
    end
    while angle < pi
        angle = angle + 2*pi;
    end
end
function [x_next, y_next, psi_next, v_next] = update(a, delta_f, dt, L, x, y, psi, v)
     x_next = x + v * cos(psi) * dt;
     y_next = y + v * sin(psi) * dt;
     psi_next = psi + v / L * tan(delta_f) * dt;
     v_next = v + a * dt;
end
function [A, B]=state_space(v, ref_delta, ref_yaw, dt, L)
    A=[ 1.0, 0.0,  -v * dt * sin(ref_yaw);
           0.0, 1.0,   v *  dt * cos(ref_yaw);
           0.0, 0.0,   1.0  ];
    B =[ dt * cos(ref_yaw), 0;
           dt * sin(ref_yaw), 0;
           dt * tan(ref_delta) / L, v * dt / (L * cos(ref_delta) * cos(ref_delta))];
end
function h = PlotCarbox(x, y, theta, varargin)
Params = GetVehicleParams();

carbox = [-Params.Lr -Params.Lb/2; Params.Lw+Params.Lf -Params.Lb/2; Params.Lw+Params.Lf Params.Lb/2; -Params.Lr Params.Lb/2];
carbox = [carbox; carbox(1, :)];

transformed_carbox = [carbox ones(5, 1)] * GetTransformMatrix(x, y, theta)';
h = plot(transformed_carbox(:, 1), transformed_carbox(:, 2), varargin{:});

end
function hs = PlotCarWheels(x, y, theta, phy, varargin)
Params = GetVehicleParams();

wheel_box = [-Params.wheel_radius -Params.wheel_width / 2;
    +Params.wheel_radius -Params.wheel_width / 2;
    +Params.wheel_radius +Params.wheel_width / 2;
    -Params.wheel_radius +Params.wheel_width / 2];

front_x = x + Params.Lw * cos(theta);
front_y = y + Params.Lw * sin(theta);
track_width_2 = (Params.Lb - Params.wheel_width / 2) / 2;

boxes = {
    TransformBox(wheel_box, x - track_width_2 * sin(theta), y + track_width_2 * cos(theta), theta);
    TransformBox(wheel_box, x + track_width_2 * sin(theta), y - track_width_2 * cos(theta), theta);
    TransformBox(wheel_box, front_x - track_width_2 * sin(theta), front_y + track_width_2 * cos(theta), theta+phy);
    TransformBox(wheel_box, front_x + track_width_2 * sin(theta), front_y - track_width_2 * cos(theta), theta+phy);
};

hs = cell(4, 1);
for ii = 1:4
    hs{ii} = fill(boxes{ii}(:, 1), boxes{ii}(:, 2), varargin{:});
end

end

function transformed = TransformBox(box, x, y, theta)
transformed = [box; box(1, :)];
transformed = [transformed ones(5, 1)] * GetTransformMatrix(x, y, theta)';
transformed = transformed(:, 1:2);
end
function mat = GetTransformMatrix(x, y, theta)
mat = [ ...
    cos(theta) -sin(theta), x; ...
    sin(theta) cos(theta), y; ...
    0 0 1];
end

3.2 运动学结构体： 

classdef  KinematicModel<handle
    properties
        x;
        y;
        psi;
        v;
        dt;
        L;
        record_x;
        record_y;
        record_psi;
        record_phy;
    end
    methods
        function self=KinematicModel(x, y, psi, v, dt, L)
            self.x=x;
            self.y=y;
            self.psi=psi;
            self.v = v;
            self.L = L;
            % 实现是离散的模型
            self.dt = dt;
            self.record_x = [];
            self.record_y= [];
            self.record_psi= [];
            self.record_phy= [];
        end
end
end

四、仿真参数和效果

 4.1 参数配置

%期望轨迹
y0 = @(t_step) 10*sin(2 * t_step + 1);
x0_dot= @(t_step) 5 * 2 * cos(2 * t_step + 1);
L=2;%车辆轴距
v=2;%初始速度
dt=0.05;%时间间隔
Q = eye(3) * 3.0;
R = eye(2) * 2.0;
robot_state = zeros(4, 1);
VehicleParams.Lw = 2.8 * 2; % wheel base
VehicleParams.Lf = 0.96 * 2; % front hang
VehicleParams.Lr = 0.929 * 2; % rear hang
VehicleParams.Lb = 1.942 * 2; % width
VehicleParams.Ll = VehicleParams.Lw + VehicleParams.Lf + VehicleParams.Lr; % length
VehicleParams.f2x = 1/4 * (3*VehicleParams.Lw + 3*VehicleParams.Lf - VehicleParams.Lr);
VehicleParams.r2x = 1/4 * (VehicleParams.Lw + VehicleParams.Lf - 3*VehicleParams.Lr);
VehicleParams.radius = 1/2 * sqrt((VehicleParams.Lw + VehicleParams.Lf + VehicleParams.Lr) ^ 2 / 4 + VehicleParams.Lb ^ 2);
VehicleParams.a_max = 0.5;
VehicleParams.v_max = 2.5;
VehicleParams.phi_max = 0.7;
VehicleParams.omega_max = 0.5;
% for wheel visualization
VehicleParams.wheel_radius = 0.32*2;
VehicleParams.wheel_width = 0.22*2;
iter_max = 100;
Eps = 1e-4;

  4.1 仿真效果

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.coloradmin.cn/o/1883231.html

如若内容造成侵权/违法违规/事实不符，请联系多彩编程网进行投诉反馈，一经查实，立即删除！

相关文章

工程化：Commitlint / 规范化Git提交消息格式

工程化：Commitlint / 规范化Git提交消息格式

一、理解Commitlint Commitlint是一个用于规范化Git提交消息格式的工具。它基于Node.js&#xff0c;通过一系列的规则来检查Git提交信息的格式&#xff0c;确保它们遵循预定义的标准。 1.1、Commitlint的核心功能 代码规则检查&#xff1a;Commitlint基于代码规则进行检查&a…
阅读更多...
16_C语言编程基础

16_C语言编程基础

目录 C语言入门 程序段和代码段 C语言入门实例 英文分号(;) 注释 标识符 保留字 C中的空格 C数据类型 基本类型 sizeof获取存储字节 void类型 变量 C数组 C枚举 C中的左值(lvalue)和右值(rvalue) C常量 变量存储类型 C常用关键字(保留字) 宏定义#define co…
阅读更多...
视频孪生助力智慧工厂：可视化安防管理与报警告警

视频孪生助力智慧工厂：可视化安防管理与报警告警

在当今快速迭代的工业4.0时代&#xff0c;智慧工厂已成为提升生产效率、优化资源配置的关键所在。面对日益复杂的生产环境和多元化的业务需求&#xff0c;如何构建一个高效、智能且具备强大适应能力的智慧工厂也成为了众多厂商关注的焦点。为了满足工业制造的转型需求&#xff…
阅读更多...
element el-table表格切换分页保留分页数据+限制多选数量

element el-table表格切换分页保留分页数据+限制多选数量

el-table表格并没有相关的方法来禁用表头里面的多选按钮 那么我们可以另辟蹊径&#xff0c;来实现相同的多选切换分页&#xff08;保留分页数据&#xff09; 限制多选数量的效果 <el-table:data"tableData"style"width: 100%">// 不使用el-talbe自带…
阅读更多...
DX-11A信号继电器 0.5A 柜内板前接线 约瑟JOSEF

DX-11A信号继电器 0.5A 柜内板前接线 约瑟JOSEF

DX-11,11A,11B,11C型信号继电器 DX-11信号继电器 DX-11B信号继电器 DX-11A信号继电器 DX-11C信号继电器 1 用途 该继电器用于直流操作的保护线路中&#xff0c;作为信号指示器。 2 结构和原理 该继电器具有电磁铁和带公共点的三付动合触点及一个信号牌&#xff0c;为电…
阅读更多...
什么是原始权益人？

什么是原始权益人？

摘要&#xff1a;每天学习一点金融小知识 原始权益人&#xff0c;在资产证券化&#xff08;ABS&#xff09;和公募REITs等金融产品中&#xff0c;指的是证券化基础资产的原始所有者&#xff0c;即金融产品的真正融资方。他们是按照相关规定及约定向资产支持专项计划转移其合法拥…
阅读更多...
Victor CMS v1.0 SQL 注入漏洞（CVE-2022-28060）

Victor CMS v1.0 SQL 注入漏洞（CVE-2022-28060）

前言 CVE-2022-28060 是 Victor CMS v1.0 中的一个SQL注入漏洞。该漏洞存在于 /includes/login.php 文件中的 user_name 参数。攻击者可以通过发送特制的 SQL 语句&#xff0c;利用这个漏洞执行未授权的数据库操作&#xff0c;从而访问或修改数据库中的敏感信息。 漏洞详细信…
阅读更多...
武汉星起航：跨境电商流量红利爆发，2023年出海企业迎突破增长

武汉星起航：跨境电商流量红利爆发，2023年出海企业迎突破增长

在数字时代的浪潮中&#xff0c;中国跨境电商以惊人的爆发力崭露头角&#xff0c;成为全球贸易的璀璨新星。2023年数据显示&#xff0c;跨境电商出口额高达1.83万亿元&#xff0c;同比增长19.6%&#xff0c;这一显著增速不仅刷新纪录&#xff0c;更为众多出海企业带来了前所未有…
阅读更多...
MySQL-行级锁（行锁、间隙锁、临键锁）

MySQL-行级锁（行锁、间隙锁、临键锁）

文章目录 1、介绍2、查看意向锁及行锁的加锁情况3、行锁的演示3.1、普通的select语句&#xff0c;执行时&#xff0c;不会加锁3.2、select * from stu where id 1 lock in share mode;3.3、共享锁与共享锁之间兼容。3.4、共享锁与排他锁之间互斥。3.5、排它锁与排他锁之间互斥3…
阅读更多...
TopK问题与如何在有限内存找出前几最大(小)项(纯c语言版)

TopK问题与如何在有限内存找出前几最大(小)项(纯c语言版)

目录 0.前言 1.知识准备 2.实现 1.首先是必要的HeapSort 2.造数据 其他注意事项 3.TopK的实现 0.前言 在我们的日常生活中总有排名系统&#xff0c;找出前第k个分数最高的人&#xff0c;而现在让我们用堆来在有限内存中进行实现 1.知识准备 想要实现topk问题首先我们要…
阅读更多...
【stm32】大一上学期笔记复制

【stm32】大一上学期笔记复制

砌墙单片机 外设是什么&#xff1f; ipage 8 nx轴 128 X0-127 y0-63 PWM脉冲宽度调制 PWM脉冲宽度调制 2023年10月13日 基本特性&#xff1a;脉冲宽度调制PWM是一种对模拟信号进行数字编码的方法。广泛引用于电机控制&#xff0c;灯光的亮度调节&#xff0c;功率控制等领域…
阅读更多...
科普文：一文搞懂jvm原理(二)类加载器

科普文：一文搞懂jvm原理(二)类加载器

概叙 科普文&#xff1a;一文搞懂jvm(一)jvm概叙-CSDN博客 前面我们介绍了jvm&#xff0c;jvm主要包括两个子系统和两个组件&#xff1a; Class loader(类装载器) 子系统&#xff0c;Execution engine(执行引擎) 子系统&#xff1b;Runtime data area (运行时数据区域)组件&am…
阅读更多...
类和对象【上】【C++】

类和对象【上】【C++】

P. S.&#xff1a;以下代码均在VS2019环境下测试&#xff0c;不代表所有编译器均可通过。 P. S.&#xff1a;测试代码均未展示头文件stdio.h的声明&#xff0c;使用时请自行添加。 博主主页&#xff1a;LiUEEEEE                        …
阅读更多...
试用笔记之-收钱吧安卓版演示源代码，收钱吧手机版感受

试用笔记之-收钱吧安卓版演示源代码，收钱吧手机版感受

首先下载&#xff1a; https://download.csdn.net/download/tjsoft/89499105 安卓手机安装 如果有收钱吧帐号输入收钱吧帐号和密码。 如果没有收钱吧帐号点我的注册 登录收钱吧帐号后就可以把手机当成收钱吧POS机用了&#xff0c;还可以扫客服的付款码哦 源代码技术交流QQ:42…
阅读更多...
Nuxt3 的生命周期和钩子函数（七）

Nuxt3 的生命周期和钩子函数（七）

title: Nuxt3 的生命周期和钩子函数&#xff08;七&#xff09; date: 2024/6/30 updated: 2024/6/30 author: cmdragon excerpt: 摘要&#xff1a;文章阐述了Nuxt3中Nitro生命周期钩子的使用&#xff0c;如nitro:config自定义配置、nitro:init注册构建钩子、nitro:build:be…
阅读更多...
Python自动化，实现自动登录并爬取商品数据，实现数据可视化

Python自动化，实现自动登录并爬取商品数据，实现数据可视化

关于如何使用Python自动化登录天 猫并爬取商品数据的指南&#xff0c;我们需要明确这是一个涉及多个步骤的复杂过程&#xff0c;且需要考虑到天猫的反爬虫策略。以下是一个简化的步骤指南&#xff1a; 步骤一&#xff1a;准备工作 环境准备&#xff1a;确保你的Python环境已经…
阅读更多...
数据沿袭是止痛药还是维生素？

数据沿袭是止痛药还是维生素？

首先&#xff0c;这在很大程度上取决于用户组织当前的使用案例及其成熟度。 在我看来&#xff0c;数据工程师喜欢查看数据流并对依赖关系有直观的了解&#xff0c;但他们最终真的会使用数据沿袭吗&#xff1f;使用频率是多少&#xff1f;具体用例是什么&#xff1f; 从我们的观…
阅读更多...
＜电力行业＞ - 《第12课：配电（2）》

＜电力行业＞ - 《第12课：配电（2）》

5 配网的指标 配电网与广大用户紧密联系&#xff0c;所以配电网是否合格还是十分重要的。 评判配电网的标准&#xff0c;主要有四个指标&#xff1a; 供电可靠性&#xff1a;供电可靠性是指针对用户连续供电的可靠程度。网损率&#xff1a;网损率可定义为电力网的电能损耗量与…
阅读更多...
问题-小技巧-专业版Win11怎么启动电脑的休眠模式？

问题-小技巧-专业版Win11怎么启动电脑的休眠模式？

专业版Win11怎么启动电脑的休眠模式&#xff1f; powercfg -a powercfg -hibernate on 启用管理员面板依次输入上述命令就可以了。
阅读更多...
短视频电商源码怎么选择

短视频电商源码怎么选择

随着移动互联网的迅猛发展&#xff0c;短视频电商成为了一种热门的商业模式。很多商家和创业者都希望能够快速搭建一个短视频电商平台来推广和销售自己的产品。然而&#xff0c;选择合适的短视频电商源码并不是一件容易的事情。在选择之前&#xff0c;有一些关键因素需要考虑。…
阅读更多...
推荐文章
最新文章

Copyright @ 2022~2023