亚洲鞋业资讯网,最新潮鞋资讯分享!

微信号:weixin888

机器人控制算法——移动机器人横向控制最优控制LQR算法

时间:2024-04-01 11:03人气: 编辑:亚洲鞋业资讯网

1.Introduction
LQR (外文名linear quadratic regulator)即线性二次型调节器,LQR可得到状态线性反馈的最优控制规律,易于构成闭环最优控制。LQR最优控制利用廉价成本可以使原系统达到较好的性能指标(事实也可以对不稳定的系统进行整定) ,而且方法简单便于实现 ,同时利用 Matlab 强大的功能体系容易对系统实现仿真。
之前已经在自动驾驶汽车上工程落地,参见自动驾驶——最优控制算法(LQR)工程化总结
与上次不同的是,此次被控对象是移动机器人,两轮差速模型,差速模型原理参见另一篇机器人控制算法——两轮差速驱动运动模型
2. Implment LQR Algorithm for Differential Drive Motion

车辆运动学模型是根据车辆的几何关系建立的,不考虑影响车辆运动的力。以车辆后轴中点作为研究对象,在惯性坐标系OXY下,车辆运动状态可用车辆坐标(x,y)和车身航向角。v为车辆纵向行驶速度,如下图所示。

则车辆运动学模型为:

其中,u1和u2分别是车辆的纵向和横向控制输入。
设车辆行驶的目标点位姿为
,则在车辆目标点处建立局部相对坐标系,如图2所示的坐标系变换示意图,则车辆当前位置相对目标位置的横纵向偏差和航向角偏差满足下列几何关系:

其中,
分别为车辆的纵向偏差、横向偏差和航向角偏差。
根据公式(1)和公式(2)可得车辆在相对坐标系下的运动学模型为:

公式(3)中包含了纵向控制u1(v)和横向控制u2(w),但是本文我们只考虑横向控制。我们需要把横向控制和纵向控制解耦合。
根据(3)可得解耦后的车辆横向运动学模型为:

我们需要进一步将公式16进行线性化处理,利用高等数学极限知识,当x->0,sin(x)~x,故有:

至此我们得到了机器人两轮差速动力学模型。

3. LQR Implement
在本部分,我们将使用LQR控制算法计算最优控制规律。

  1. 对矩阵A和B离散化处理

    B_D = T * B
    A_D = (I+0.5AT)(I-0.5A*T)^(-1)
 matrix_A_D    = (matrix_I - 0.5 * ts * matrix_A)^(-1) * (matrix_I + 0.5 * ts * matrix_A);
 matrix_B_D    = matrix_B * ts;

2.设定Q和R矩阵,Q应是2X2矩阵,R应是1X1维。
3.Riccati 方程求解:

My Related Works
2. 自动驾驶-LQR工程实现(调研)
3. 自动驾驶-动力学模型-状态空间法A矩阵----c语言实现【附Github源码链接】
4. 自动驾驶——最优控制算法(LQR)工程化总结

Reference
1.基于车辆运动学模型的路径跟踪仿真及程序

如果觉得写得还不错,可以关注一下我的微信公众号,最新动态会分享在此。

标签: [db:TAGS]  
相关资讯
热门频道

热门标签

官方微信官方微博百家号

网站简介 | 意见反馈 | 联系我们 | 法律声明 | 广告服务

Copyright © 2002-2024 欧亿●平台●官网-OE SPORTS 版权所有 网站地图 备案号:粤06094508号