一种路径跟踪控制方法、系统及计算机可读存储介质与流程

1.本发明涉及技术领域，尤其涉及一种路径跟踪控制方法、系统及计算机可读存储介质。


背景技术：

2.路径跟踪是无人车研究方向的一项关键技术。路径跟踪控制方法是指能够使无人车按照预设路径，安全稳定行驶的控制方法。
3.比例积分微分控制，简称pid控制，是最早发展起来的控制策略之一，由于其算法简单、鲁棒性好和可靠性高，被广泛应用于工业过程控制，至今仍有90％左右的控制回路具有pid结构。
4.简单的说，根据给定值和实际输出值构成控制偏差，将偏差按比例、积分和微分通过线性组合构成控制量，对被控对象进行控制。常规pid控制器作为一种线性控制器。
5.在进行移动机器人的控制时，pid控制不够精准。


技术实现要素：

6.有鉴于此，本发明要解决的技术问题是提供一种路径跟踪控制方法、系统及计算机可读存储介质，能够对移动机器人进行精准控制。
7.本发明的技术方案是这样实现的：
8.一种路径跟踪控制方法，包括以下步骤：
9.s1、建立移动机器人的运动学模型和状态空间方程；
10.s2、由系统状态变量和控制输入，计算矩阵系数；
11.s3、建立cost
‑
function目标函数，获取在预测周期内使目标函数最小情况下的状态反馈控制器；
12.s4、得到下一个预测周期内的最优控制输入并执行s3。
13.优选的，所述s1中，被控系统的状态方程为：
[0014][0015]
u(t)＝k(t)x(t)
[0016]
其中x(t)表示机器人的实时位姿，可由坐标和角度表示为
[0017]
u(t)表示机器人的控制量，由舵轮的速度和角度表示
[0018]
k(t)为状态反馈控制器；
[0019]
则系统的控制状态方程表示为：
[0020][0021]
输入量是观测值是
[0022]
优选的，所述移动机器人舵轮速度v
wheel
、舵轮角度α
wheel
、运动中心至舵轮的距离l；车体角速度：w＝v
wheel
*sin(α
wheel
)/l
[0023]
车体运动中心速度：v
car
＝w*l/tanθ＝v
wheel
*cos(α
wheel
)
[0024]
则：
[0025][0026][0027][0028]
得到
[0029][0030]
对于非线性系统进行线性化。
[0031]
将分别对和进行一阶泰勒展开得到矩阵系数a和b。
[0032]
建立cost
‑
function目标函数，求预测周期内，使目标函数最小情况下的状态反馈控制器k(t)；建立cost
‑
function目标函数，寻找一个最优控制函数u(t), 使得系统从给定的状态x0出发转移到目标控制末态x
tf
时，使得系统指标最优
[0033][0034]
其中q(t)、r(t)分别为x(t)、u(t)的控制权重，q(t)为半正定矩阵、r(t) 为正定矩阵。将(2)式代入(6)式，得到
[0035][0036]
根据黎卡提方法
[0037]
k(t)＝r
‑1(t)b
t
(t)p(t)
ꢀꢀ
(8)
[0038]
a
t
(t)p(t) p(t)a(t) q(t)
‑
p(t)b(t)r
‑1(t)b
t
(t)p(t)＝0
ꢀꢀ
(9)
[0039]
k(t)的取值和p(t)有关，而p(t)是假设的一个量，p(t)只要使得(9)式成立即可。
[0040]
p(t)求得后，根据(8)式即可求得使cost
‑
function目标函数最小时的反馈矩阵 k(t)。得到下一个预测周期内的最优控制输入u(t)根据(2)式u(t)＝k(t)
·
x(t)，可以求得
最终的控制量，即下一个周期舵轮的速度和角度，转步骤3。
[0041]
本发明还提出了路径跟踪控制系统，包括：
[0042]
建立单元，用于建立移动机器人的运动学模型和状态空间方程；
[0043]
计算单元，用于由系统状态变量和控制输入，计算矩阵系数；
[0044]
获取单元，用于建立cost
‑
function目标函数，获取在预测周期内使目标函数最小情况下的状态反馈控制器；
[0045]
执行单元，用于得到下一个预测周期内的最优控制输入并启动所述获取单元。
[0046]
本发明还提出了路径跟踪控制装置，包括存储器和处理器；所述存储器，用于存储计算机程序；所述处理器，用于当执行所述计算机程序时，实现上述所述的路径跟踪控制方法。
[0047]
本发明还提出了计算机可读存储介质，其特征在于，所述存储介质上存储有计算机程序，当所述计算机程序被处理器执行时，实现上述所述的路径跟踪控制方法。
[0048]
本发明提出的路径跟踪控制方法、系统及计算机可读存储介质，相比于传统的pid控制，采用lqr最优控制的思想，使得控制更加精准。
附图说明
[0049]
图1为本发明实施例提出的路径跟踪控制方法的流程图；
[0050]
图2为本发明实施例提出的路径跟踪控制系统的结构框图；
[0051]
图3为本发明实施例提出的路径跟踪控制方法的车辆模型图。
具体实施方式
[0052]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0053]
如图1所示，本发明实施例提出了一种路径跟踪控制方法，包括以下步骤：
[0054]
s101、建立移动机器人的运动学模型和状态空间方程；
[0055]
s102、由系统状态变量和控制输入，计算矩阵系数；
[0056]
s103、建立cost
‑
function目标函数，获取在预测周期内使目标函数最小情况下的状态反馈控制器；
[0057]
s104、得到下一个预测周期内的最优控制输入并执行s103。
[0058]
可见，本发明实施例提出的路径跟踪控制方法、系统及计算机可读存储介质，相比于传统的pid控制，采用lqr最优控制的思想，使得控制更加精准。
[0059]
本发明实施例具体步骤如下：
[0060]
建立移动机器人的运动学模型，及状态空间方程；
[0061]
已知被控系统的状态方程为：
[0062][0063]
u(t)＝k(t)x(t)
ꢀꢀ
(2)
[0064]
其中x(t)表示机器人的实时位姿，可由坐标和角度表示为
[0065]
u(t)表示机器人的控制量，由舵轮的速度和角度表示
[0066]
k(t)为状态反馈控制器。
[0067]
则系统的控制状态方程可表示为：
[0068][0069]
输入量是观测值是
[0070]
由系统状态变量和控制输入，求出矩阵系数a(t)和b(t)
[0071]
已知车辆模型如图3；移动机器人舵轮速度v
wheel
、舵轮角度α
wheel
、运动中心至舵轮的距离l；车体角速度：w＝v
wheel
*sin(α
wheel
)/l
[0072]
车体运动中心速度：v
car
＝w*l/tanθ＝v
wheel
*cos(α
wheel
)
[0073]
则：
[0074][0075]
将(4)式代入(1)式整理得到：
[0076][0077]
需要对于非线性系统进行线性化。
[0078]
将分别对和进行一阶泰勒展开，可以求得(1)式中的矩阵系数a和b。
[0079]
建立cost
‑
function目标函数，求预测周期内，使目标函数最小情况下的状态反馈控制器k(t)
[0080]
建立cost
‑
function目标函数，寻找一个最优控制函数u(t),使得系统从给定的状态x0出发转移到目标控制末态x
tf
时，使得系统指标最优
[0081][0082]
其中q(t)、r(t)分别为x(t)、u(t)的控制权重，q(t)为半正定矩阵、r(t)为正定矩阵。将(2)式代入(6)式，得到
[0083][0084]
根据黎卡提方法：
[0085]
k(t)＝r
‑1(t)b
t
(t)p(t)
ꢀꢀ
(8)
[0086]
a
t
(t)p(t) p(t)a(t) q(t)
‑
p(t)b(t)r
‑1(t)b
t
(t)p(t)＝0
ꢀꢀ
(9)
[0087]
k(t)的取值和p(t)有关，而p(t)是假设的一个量，p(t)只要使得(9)式成立即可。p(t)求得后，根据(8)式即可求得使cost
‑
function目标函数最小时的反馈矩阵k(t)。得到下一个预测周期内的最优控制输入u(t)根据(2)式 u(t)＝k(t)
·
x(t)，可以求得最终的控制量，即下一个周期舵轮的速度和角度，转步骤3。
[0088]
如图2所示，本发明还提出了路径跟踪控制系统，包括：
[0089]
建立单元21，用于建立移动机器人的运动学模型和状态空间方程；
[0090]
计算单元22，用于由系统状态变量和控制输入，计算矩阵系数；
[0091]
获取单元23，用于建立cost
‑
function目标函数，获取在预测周期内使目标函数最小情况下的状态反馈控制器；
[0092]
执行单元24，用于得到下一个预测周期内的最优控制输入并启动所述获取单元。
[0093]
本发明实施例还提出了一种路径跟踪控制装置，包括存储器和处理器；所述存储器，用于存储计算机程序；所述处理器，用于当执行所述计算机程序时，实现上述路径跟踪控制方法。
[0094]
本发明实施例还提出了计算机可读存储介质，所述存储介质上存储有计算机程序，当所述计算机程序被处理器执行时，实现上述所述的路径跟踪控制方法。
[0095]
通过以上的实施方式的描述，所属领域的技术人员可以清楚地了解到本技术可借助软件加必需的通用硬件的方式来实现，当然也可以通过专用硬件包括专用集成电路、专用cpu、专用存储器、专用元器件等来实现。一般情况下，凡由计算机程序完成的功能都可以很容易地用相应的硬件来实现，而且，用来实现同一功能的具体硬件结构也可以是多种多样的，例如模拟电路、数字电路或专用电路等。但是，对本技术而言更多情况下软件程序实现是更佳的实施方式。基于这样的理解，本技术的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在可读取的存储介质中，如计算机的软盘、u盘、移动硬盘、rom、ram、磁碟或者光盘等，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本技术各个实施例的方法。
[0096]
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时，可以全部或部分地以计算机程序产品的形式实现。计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行计算机程序指令时，全部或部分地产生按照本技术实施例的流程或功能。计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一计算机可读存储介质传输，例如，计算机指令可以从一个网站站点、计
算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线 (dsl))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。计算机可读存储介质可以是计算机能够存储的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，dvd)、或者半导体介质(例如固态硬盘(solidstatedisk，ssd))等。
[0097]
最后需要说明的是：以上所述仅为本发明的较佳实施例，仅用于说明本发明的技术方案，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所做的任何修改、等同替换、改进等，均包含在本发明的保护范围内。