一种基于模型预测控制的机器人轨迹跟踪系统的制作方法

1.本发明属于机器人控制技术领域，具体涉及为一种基于模型预测控制的机器人轨迹跟踪系统，本产品解决了且大部分控制算法都没有考虑执行器饱和约束问题。使用模型预测控制算法，实现轨迹跟踪的功能。


背景技术：

2.随着人类社会的进步，人们对各种环境的而不断探索，例如海底探测、抗震救灾、场景侦查等对人类安全有危险或不方便人类直接到达的场所，越来越需要可移动机器人的协助，研究移动机器人，对科学技术的发展起到推动作用，成为目前的热门研究方向之一。移动机器人可以通过传感器检测周围的环境信息，并根据环境做出相应的调整和变化。可移动的机器人是一门交叉融合的学科，涉及到控制、计算机、材料、机械等领域。
3.此外，移动机器人经常在运动过程中受到很多约束，例如执行器饱和约束、移动范围约束等。如果只是简单的忽略掉这些约束，则可能导致移动机器人的控制性能变差甚至损坏控制器。目前，针对移动机器人轨迹跟踪提出了很多控制算法，控制器的设计较为复杂，对移动机器人的参数也有不同的要求，而且大部分控制算法都没有考虑执行器饱和约束问题。而在实际应用中，由于受到物理器件的限制，移动机器人的输出力矩是有界的，因此所设计的控制器需要考虑饱和约束问题。本发明主要针对存在饱和约束问题的移动机器人进行了轨迹跟踪控制的研究，并通过仿真实验验证了所提算法的有效性。


技术实现要素：

4.为于解决在实际应用中，由于受到物理器件的限制，移动机器人的输出力矩是有界的，因此所设计的控制器需要考虑饱和约束问题。本发明的目的是提供一种基于模型预测控制的机器人轨迹跟踪系统，具有能够有效处理饱和约束问题，并实现轨迹跟踪的功能。并通过以下技术方案来实现：
5.一种基于模型预测控制的机器人轨迹跟踪系统，包括了一个主体为圆柱形的差速移动机器人，包括两个驱动轮和两个万向轮。如图2所示。其特征在于，先针对所述机器人进行数学建模，包括机器人的坐标系建模和运动学建模。根据机器人的运动学特性，将机器人的动力学模型进行线性化。然后针对线性化的模型设计模型预测控制的控制器部分，设定所述机器人的参考轨迹，进行仿真验证，实现良好的轨迹跟踪效果。
6.建立机器人的数学模型，所述机器人主体部分包括圆柱形底座，两个驱动轮和两个万向轮。针对此模型进行坐标系建模。坐标系建模方式如下：
7.在全局坐标系中，机器人的位姿设为：
8.φ1＝[x
1 y
1 θ1]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0009]
式中，φ1为机器人全局坐标系的位姿，(x
1 y1)为机器人处在全局坐标系上的坐标，θ1表示机器人在全局坐标系中运动的角度。
[0010]
同理，机器人在局部坐标系中的位姿为：
[0011]
φ2＝[x
2 y
2 θ2]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0012]
式中，φ2为机器人在局部坐标系中的位姿，(x
2 y2)为机器人在局部坐标系上的坐标，θ2表示机器人在局部坐标系中的角度。
[0013]
通过对机器人位置图的分析，结合以上两式的计算，可以得到在局部坐标系中，机器人的参考点在全局坐标系中的位姿为：
[0014][0015]
式中，φ3为在全局坐标系下机器人参考点的位姿，(x
3 y3)为在全局坐标系下，机器人参考点的坐标，θ3为在全局坐标系下，机器人参考点的角度。
[0016]
运动学建模如下：
[0017]
机器人的运动学模型和约束为：
[0018][0019]
式中，可设机器人的输入矩阵为设一个变换矩阵为
[0020]
故式(4)可简写为：
[0021]
根据差速机器人的特性可得：
[0022][0023]
将式(6)代入(5)，可得到由机器人的运动学方程为：
[0024][0025]
1、根据权利要求3所述的一种基于模型预测控制的机器人轨迹跟踪系统，其特征在于：对所给出的动力学模型进行线性化处理。如对式可表示为式中的x为
状态量(x
1 y
1 ω)，使用前向差分法得系统离散化的线性模型为：
[0026][0027]
式中式中，t表示采样周期，k表示时间间隔。
[0028]
针对离散化处理后的模型，进行模型预测控制的控制器设计，采用二次规划的方式，二次规划的一般形式为：
[0029]
min z
t
qz+c
tzꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)
[0030]
式中z
t
qz为二次型，c
t
z为线性。
[0031]
设系统的代价函数为：
[0032][0033]
式中，表示误差的加权和，表示输入的加权和，表示最末位的误差。其中的q和r分别为对角矩阵。
[0034]
经过迭代计算可以得到控制器
[0035][0036]
本发明使用模型预测控制算法进行机器人的轨迹跟踪，由图2所示，通过模型预测、滚动优化还有反馈矫正，对机器人模型进行优化求解，进行控制器设计，对需要考虑控制器饱和约束问题上得到解决，并且具有良好的轨迹跟踪效果。
[0037]
综上所述,由于采用了上述技术方案,本发明的有益效果是：
[0038]
1、本发明采用模型预测控制算法进行机器人的轨迹跟踪，实现了机器人轨迹跟踪系统的设计，并有效的改善了控制器的饱和约束问题。
[0039]
2、通过对系统未来状态的预测，求解优化问题获得的最优解，将最优解的第一个元素作用于系统，使得系统可以实现轨迹跟踪的效果，且控制系统简单，动态性能优良。
附图说明
[0040]
图1为本发明使用的流程示意图。
[0041]
图2为本发明使用的几何示意图。
[0042]
图3为本发明使用的模型预测控制过程图。
[0043]
图4为本发明的增加代价函数的轨迹跟踪仿真图。
[0044]
图5为本发明使用的增加代价函数的误差图。
[0045]
图6为本发明的基于mpc和滑模控制轨迹跟踪仿真图。
[0046]
图7为本发明基于mpc和滑模控制的误差图。
[0047]
图8为本发明的增加双闭环控制的轨迹跟踪仿真图。
[0048]
图9为本发明使用的增加双闭环控制的误差图。
具体实施方式
[0049]
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本发明进行进一步详细说明。应当理解,此处所描述的具体实施例仅用以解释本发明,并不用于限定本发明。
[0050]
一种基于基于模型预测控制的机器人轨迹跟踪系统，其特征在于，包括如下步骤：
[0051]
s1、建立该机器人的坐标系模型；
[0052]
s2、根据坐标系模型建立机器人的运动学模型；
[0053]
s3、将所给的运动学模型进行线性化处理；
[0054]
s4、设计模型预测控制的控制器；
[0055]
s5、根据设计的控制器调整参数，并进行仿真验证。
[0056]
模型预测控制有三个特征，分别为模型预测、滚动优化还有反馈矫正。其基本原理，就是在当前的时刻k,可以预测未来的n个时刻的状态量，在求解出符合约束条件的最小控制量之后，将当前时刻下的控制量中的第一个元素uk施加于被控对象上,因为预测的模型很难完美的描述现实的系统，而且现实的系统当中可能会有扰动，所以当实施uk之后，在下一个k+1时刻，将控制区间和预测空间向未来移动一个时刻，然后再次进行预测计算，这就是一个滚动优化的过程。这样在每一步的时候都要进行一下最优化的计算。
[0057]
所以模型预测控制的过程可以分为三点：第一点要对系统未来n个时刻的状态进行预测，第二点对于优化问题求解并获得出最优解，第三点要把求出的最优解中的第一个元素再次作用于系统，如此滚动反复进行，每个未来状态的初试值都是目前状态的测量值，所以需要每个采样时刻的测量值。随着时间的不断推移，系统也在不断的进行求解，得到每个时刻的最优解。
[0058]
在全局坐标系中，机器人的位姿设为：
[0059]
φ1＝[x
1 y 1
θ]
t1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(1)
[0060]
式中，φ1为机器人全局坐标系的位姿，(x
1 y1)为机器人处在全局坐标系上的坐标，θ1表示机器人在全局坐标系中运动的角度。
[0061]
同理，机器人在局部坐标系中的位姿为：
[0062]
φ2＝[x
2 y
2 θ2]
t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)
[0063]
式中，φ2为机器人在局部坐标系中的位姿，(x
2 y2)为机器人在局部坐标系上的坐标，θ2表示机器人在局部坐标系中的角度。
[0064]
通过对机器人位置图的分析，结合以上两式的计算，可以得到在局部坐标系中，机器人的参考点在全局坐标系中的位姿为：
[0065][0066]
式中，φ3为在全局坐标系下机器人参考点的位姿，(x
3 y3)为在全局坐标系下，机器人参考点的坐标，θ3为在全局坐标系下，机器人参考点的角度。
[0067]
设机器人的运动学模型和约束为：
[0068][0069]
式中，可设机器人的输入矩阵为设一个变换矩阵为
[0070]
故式(4)可简写为：故式(4)可简写为：
[0071]
可以将其简化为一种更简洁的形式：
[0072][0073]
式中的x为状态量(x
1 y
1 ω)，则可以建立机器人的线性误差模型：
[0074][0075]
将(6)式在(x
e ue)附近做泰勒展开，且忽略高次项可得：
[0076][0077]
两式相减可得：
[0078][0079]
式中，
[0080]
而根据差速机器人的特性可得：
[0081][0082]
将其写成矩阵形式可以表示为：
[0083][0084]
将式(2-12)代入(2-5)，可得到由左右线速度控制的运动学方程为：
[0085][0086]
使用前向差分法得系统离散化的线性模型为：
[0087][0088]
式中式中，t表示采样周期，k表示时间间隔。
[0089]
对于模型预测控制而言，有很多种方式可以对其进行最优化，本文选择一种常用的二次规划的方式。
[0090]
二次规划的一般形式为：
[0091]
min z
t
qz+c
tzꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9) 式中z
t
qz为二次型，c
t
z为线性。
[0092]
已经建立了机器人的离散化模型如下：
[0093][0094]
式中，为状态变量，为输入变量，二者均为矩阵形式，即：
[0095][0096][0097]
所以在k时刻，模型的输入可以表示为在k+1时刻，输入可以表示为以此类推到任意时刻的输入为最后到式中后面的k表示在k时刻做的预测，前面的k+i表示在k时刻预测到的相应时刻的输入值是多少。一直持续到第项，其中的n表示预测区间。同理，对于预测的系统状态也是如此，预测到的状态从到为了简化，可以将输入值和状态值用向量进行表示：
[0098][0099][0100]
假设系统的输出y＝x，系统的参考值r＝0，则可以得到系统的误差为：
[0101]
e＝y-r＝x-0＝x
[0102]
则可以设系统的代价函数为：
[0103][0104]
式中，表示误差的加权和，表示输入的加权和，表示最末位的误差。其中的q和r分别为对角矩阵，如下所示：
[0105][0106][0107]
由于现在的代价函数j中，既有输入变量又有误差值而最后的控制器是控制输入，所以还需经过一定的变换，将误差量消除掉。
[0108]
对于系统的状态量，可以有如下的变化：
[0109][0110][0111][0112]
经过多次迭代，可以得到：
[0113][0114]
由(4-7)式可得，上述的迭代式可以简化为：
[0115][0116]
式中，设
[0117]
所以将代价函数j展开可得：
[0118][0119]
展开化简可得：
[0120][0121]
式中，
[0122]
在控制时还需要对系统的输入量和增量进行约束，进而得出控制器函数以及其约束条件的公式如下：
[0123][0124]
以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
[0125]
实施例1
[0126]
s1、建立该机器人的坐标系模型；
[0127]
s2、根据坐标系模型建立机器人的运动学模型；
[0128]
s3、将所给的运动学模型进行线性化处理；
[0129]
s4、设计模型预测控制的控制器；
[0130]
s5、根据设计的控制器调整参数，并进行仿真验证。
[0131]
对比例1
[0132]
为了验证本发明加入终端代价函数的模型预测控制对移动机器人轨迹跟踪控制的有效性，与针对移动机器人运动学模型所设计的未加入终端代价函数的模型预测控制进行仿真对比结果。如图4和图5所示。
[0133]
假设机器人的参考轨迹为：x(t)＝cos(0.5t)y(t)＝sin(0.5t)θ(t)＝0.5t，机器
人的期望速度和角速度为：v＝0.5m/s，ω＝0.5rad/s。
[0134]
仿真中的参数为：假设机器人的初始位置为：预测时域为：n＝5，采样周期为：t＝0.1 q＝diag(4 10 0.2)r＝diag(0.2 0.2)。
[0135]
通过分析实验结果可得出以下结论，在实施例1中通过观察图中两种控制策略可以发现增加终端代价函数后移动机器人进行轨迹跟踪时效果更好。从图中可以看出本章所提算法能够在2s内完成轨迹跟踪，因此，通过对比可以发现增加终端代价函数的模型预测控制不仅能够进行精确地轨迹跟踪而且收敛的速度也比较快。
[0136]
实施例2
[0137]
s1、建立该机器人的坐标系模型；
[0138]
s2、根据坐标系模型建立机器人的运动学模型；
[0139]
s3、将所给的运动学模型进行线性化处理；
[0140]
s4、设计模型预测控制的控制器；
[0141]
s5、根据设计的控制器调整参数，并进行仿真验证。
[0142]
对比例2、为了验证本发明的模型预测控制对移动机器人轨迹跟踪控制的有效性，与针对移动机器人运动学模型所设计的滑膜控制控制进行仿真对比结果。结果如图6和图7所示。
[0143]
仿真中的参数为：假设机器人的初始位置为：预测时域为：n＝5，采样周期为：t＝0.1 q＝diag(1 2 0.4)r＝diag(1 1)。
[0144]
通过观察图6的实验结果可以得到以下结论：在实施例2中通过对比可以发现基于模型预测控制的模型不仅能够进行轨迹跟踪而且收敛的速度也比较快。从图7中可以看出两种算法的跟踪误差都逐渐趋近于0。其中，本发明所提的控制算法能够在4s内完成轨迹跟踪。因此，模型预测控制算法更能体现其控制算法的优越性。
[0145]
实施例3
[0146]
s1、建立该机器人的坐标系模型；
[0147]
s2、根据坐标系模型建立机器人的运动学模型；
[0148]
s3、将所给的运动学模型进行线性化处理；
[0149]
s4、设计模型预测控制的控制器；
[0150]
s5、根据设计的控制器调整参数，并进行仿真验证。
[0151]
对比例3、
[0152]
为了验证本发明的模型预测控制对移动机器人轨迹跟踪控制的有效性，与针对移动机器人双闭环控制使用模型预测控制完成轨迹跟踪进行仿真对比。结果如图8和图9所示。
[0153]
假设机器人的参考轨迹为：x(t)＝cos(0.2t)y(t)＝sin(0.2t)θ(t)＝0.2t，机器人的期望速度和角速度为：v＝0.2m/s，ω＝0.2rad/s。
[0154]
仿真中的参数为：假设机器人的初始位置为：预测时域
为：n＝5，采样周期为：t＝0.1s q＝diag(1 2)r＝diag(0.8 1)。
[0155]
通过观察图8和图9的实验结果可以得到以下结论：两种算法的跟踪误差都逐渐趋近于0。其中，可以发现基于模型预测控制的模型不仅能够进行轨迹跟踪而且收敛的速度也比较快。因此，模型预测控制算法更能体现其控制算法的优越性。
[0156]
以上所述实施例仅表达了本发明的具体实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明保护范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，其他任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。