基于全向移动机器人运动学建模的误差模型预测控制方法与流程

本发明涉及全向移动机器人、轨迹跟踪、误差模型预测控制等领域，特别是涉及一种基于轨迹跟踪误差全向移动机器人运动学建模的误差模型预测控制方法。

背景技术：

轨迹跟踪控制是为了解决如何将移动机器人的实际位置和方向收敛到期望的参考轨迹的问题。该领域在多智能体协作如多机器人编队和多机器人轨迹规划等方面具有广阔的应用前景。近年来，对轮式移动机器人的轨迹跟踪控制的研究逐渐增多。普通轮式差动驱动移动机器人仅具有两个自由度，狭窄的环境会导致其灵活性和可操作性下降。

与传统的差动驱动机器人相比，全向移动机器人(omr)能够实现沿任意方向的平移而不旋转。此外，omr还能够有效地适应小的工作空间并获得高精度的轨迹。因此，它们被广泛部署在拥挤的地区和紧凑的环境中，如仓库、走廊和船舱。omr由于其优越的移动性而越来越受欢迎。在不久的将来，采用omr来处理大量任务也指日可待。目前已有人提出一种具有三个全向轮的对称配置的omr。杰西瓦尔多提出了三个全向轮的omr的滤波史密斯预测器。虽然这种三轮式omr结构简单，但其稳定性并不令人满意。

而配备有四个麦克纳姆轮的omr则显示出相对较好的稳定性。omr复杂的轮系结构显然增加了与运动控制相关的难度。yanwen提出了一种四轮全向移动机器人，并设计了基于滑移动力学模型的路径跟踪控制器。同时，ehsan提出了四轮全向移动机器人的pi-模糊控制器。此外，vlantis提出了一种具有四个麦克纳姆轮的新型omr(fm-omr)。

fm-omr具有全向移动、稳定性好、承载能力强等优点，在工业上得到了广泛的应用。对于非完整约束系统，刚性约束有可能降低系统的控制性能和稳定性。mpc(模型预测控制)在处理具有各种约束的复杂线性和非线性系统方面具有卓越的性能，并且在以前的工作中得到了广泛的关注。mpc已经被广泛用于移动机器人、拟人机器人、无人驾驶飞行器。

技术实现要素：

本发明的目的是针对现有技术的不足，提供了fm-omr的运动学模型和四个车轮之间的速度约束关系。考虑四个麦克纳姆轮速度之间的约束关系，在运动学模型的基础上，进一步制定跟踪误差运动学模型。基于轨迹跟踪误差运动学模型的empc方案在求解非完整约束问题的同时可以解决其速度约束问题。所述方法优点突出，参数物理意义明确，适用性较强。

本发明采用如下技术方案实现：

基于全向移动机器人运动学建模的误差模型预测控制方法，所述方法包括以下步骤：

s11、建立fm-omr四个麦克纳姆轮之间的速度约束运动学模型；

s12、建立fm-omr的跟踪误差运动学模型；

s13、针对fm-omr的轨迹跟踪问题，针对跟踪误差运动学模型，设计结合速度约束方程的误差模型预测控制器；

s14、根据设计的误差模型预测控制器调整全向移动机器人之间的有效轨迹跟踪参数，使全向移动机器人之间的跟踪误差维持不变。

进一步地，s11步骤所述的fm-omr四个麦克纳姆轮之间的速度约束运动学模型构建包括：

s111、首先对fm-omr的结构进行描述；

s112、其次对fm-omr的速度约束进行描述：

将车轮的转速表示为车轮的平移速度表示为而vir，i＝1，2，3，4，是自由移动辊的切向速度，类似地，我们可以将机器人框架中轮子的合成速度表示为：

v1x＝v1m++v1rcosα1v1y＝v1rsinα1

v2x＝v2m+v2rcosα2v2y＝v2rsinα2

v3x＝v3m+v3rcosα3v3y＝v3rsinα3

v4x＝v4m+v4rcosα4v4y＝v4rsinα4

其中α1＝α3＝45，α2＝α4＝-45，此外，由于轮速与机器人本体的速度是刚性连接的，因此轮速与机器人本体的速度相关，因此，这种相关性可以建立为：

v1x＝vx-l1wv1y＝vy+l2w

v2x＝vx+l1wv2y＝vy+l2w

v3x＝vx-l1wv3y＝vy-l2w

v4x＝vx+l1wv4y＝vy-l2w

因此，可以获得fm-omr机体的速度以及四个轮子的角速度的关系如下：

其中r是如下雅可比矩阵：

其中矩阵r不是方阵，因此没有逆矩阵，然而，广义逆矩阵可以表示为：

因此可以得到：

因此，由上面获得：

同样的，可以从上式得到以下等式：

ω1+ω2＝ω3+ω4

调整四个麦克纳姆轮各自的速度，可以实现在给定的平面内期望的全向运动，因此，每个轮子的约束产生作用于整个机体，四个麦克纳姆轮之间的速度约束关系由ω1+ω2＝ω3+ω4表示，车身速度与四个轮子角速度之间的关系已经得出，机器人本体的速度可以表示如下：

其中，

s113、最后构建fm-omr的运动学模型：

从上述式子，可以得出非完整fm-omr的运动学模型如下：

进一步地，s12步骤所述的fm-omr跟踪误差运动学建模具体过程为：

s121、确定领导者rr以及跟随者r的物理参数，其中对于领导者小车，有：

其中，

s122、根据领导者以及跟随者在世界坐标系下的位置和角度计算rr在r坐标系下的相对位置和角度(xe,ye,θe)：

其中，(x,y,θ)以及(xr,yr,θr)是分别都是对应于r和rr的惯性协同坐标系；

s123、根据所得的(xe,ye,θe)计算其导数，建立其导数与输入[u1,u2,u3]之间的状态方程，所述导数表示为：

其中，

a＝w1r+w2r+w3r+w4r

b＝w1r-w2r-w3r+w4r

c＝w1+w2+w3+w4

d＝w1-w2-w3+w4

其中，a、b是rr四个车轮角速度的线性组合，c、d是r四个车轮角速度的线性组合；

则综上四个麦克纳姆轮之间具有速度约束的轨迹跟踪误差运动学模型为：

其中，

u3＝wr-w；

s124、通过控制输入[u1,u2,u3]，误差状态量[xe,ye,θe]可以收敛到原点。

进一步地，步骤s13中，设计结合速度约束方程的误差模型预测控制器时，连续时变系统用离散时间模型的形式重新表示，每个特征点的离散时间状态空间模型可以表示为：

ze(k+1)＝l(k)ze(k)+m(k)u(k)

其中，表示输入向量，表示状态量，参数n和m表示状态和输入变量的数量；根据采样定理，给定连续时间系统可以用采样周期t离散化，因此，上式可以写成如下形式：

所述状态空间模型中，为了导出给定预测层np的最优控制序列，损失函数可以表示为：

其中，δu(k)表示输入向量的增量，有δu(k)＝u(k)-u(k-1)，ze(k+j|k)和△u(k+j|k)表示基于当前时间k，在时间k+j,上的预测变量，此外，nc代表控制层，np代表预测层，最后，q和p表示适当的加权矩阵；

从以上式子，可以得出：

定义了如下形式的预测向量：

其中，np＝3，nc＝2，且对于所有k≥0时刻，约束表示如下：

预测状态可以表示为如下形式：

其中，

最终，基于约束条件下代价函数的empc算法可通过解以下约束来对问题进行优化：

这取决于：

进一步地，s13步骤所述的误差模型预测控制器中实时的empc算法通过以下步骤实现：

s131、选择适当的控制器参数，包括nc,np,q,p,t,t1；

其中，nc代表控制层参数，np代表预测层参数；q,p表示适当的加权矩阵；t为采样周期，t1为控制周期。

s132、解优化问题得到输入增量：

s133、计算得到k时刻时的输入向量；

s134、将计算的u(k)输入到系统中，更新状态向量以及ze的信息；

s135、根据u(k)解出四个电机的角速度[w1,w2,w3,w4]；

s136、如果则k＝k+1，重新回到步骤s132，否则退出。

进一步地，所述误差模型预测控制器采用机器人操作系统ros的仿真平台进行仿真验证。

进一步地，所述仿真平台为一台装有机器人操作系统(ros)的pc电脑，该电脑系统是ubuntu14.04，其中数据可视化平台rviz是ros中一款可视化仿真软件。

相比于现有技术方法，本发明的优点和有益效果是：

1、建立了fm-omr的运动学模型和四个车轮之间的速度约束关系。在运动学模型的基础上，进一步制定根据四个麦克纳姆轮速度之间的约束关系下的跟踪误差运动学模型；

2、基于轨迹跟踪误差运动学模型的empc(误差模型预测控制)方案在求解非完整约束的同时可以解决上述速度约束问题；

3、fm-omr具有全方位、稳定性好、承载能力强等优点，在工业上得到了广泛的应用。本发明提出的控制方法也适用于各种fm-omr，从而解决了实际工程问题。

附图说明

此处说明的附图用来提供本发明实施例的进一步讲解，构成本申请的一部分，并不构成本发明实施例的限定。在附图中：

图1为全向移动机器人结构示意图。

图2为两个小车坐标系以及参数定义图。

图3为fm-omr的物理参数示意图。

图4为曲线参考轨迹跟踪仿真实验图。

图5为曲线跟踪实验中相关误差。

图6为四个轮子的角速度和跟随者小车r的四个麦克纳姆轮之间的矢量约束。

图7为前两轮子的角速度之和、后两轮子角速度之和与跟随者小车r的四个麦克纳姆轮之间的矢量约束。

图8为机器人操作系统(ros)中通过rviz软件实验的仿真轨迹图。

图9为fm-omr跟随者r的物理参数图。

图10为fm-omr领导者rr的物理参数图。

具体实施方式

为使本发明的目的、技术方案及优点更加清楚明白，下面结合附图和具体实施例对本发明的发明目的作进一步详细地描述，实施例不能在此一一赘述，但本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。

实施例

麦克纳姆轮是一种性能优越、应用广泛的全向操纵轮，它由均匀分布在中心轮毂周围的小辊子组成。麦克纳姆轮具有三个自由度，即绕轮轴旋转、绕辊子轴线旋转以及在轮和地面之间滚动。车轮的转动由电机驱动，而辊子则由地面上的摩擦力驱动。因此，当电机驱动轮子旋转时，轮子将沿着垂直于驱动轴的方向前进，而围绕轮子的辊子将沿着各自的轴线前进。

基于全向移动机器人运动学建模的误差模型预测控制方法，所述方法包括以下步骤：

s11、建立fm-omr四个麦克纳姆轮之间的速度约束运动学模型；

s12、建立fm-omr的跟踪误差运动学模型；

s13、针对fm-omr的轨迹跟踪问题，针对跟踪误差运动学模型，设计结合速度约束方程的误差模型预测控制器；

s14、根据设计的误差模型预测控制器调整全向移动机器人之间的有效轨迹跟踪参数，使全向移动机器人之间的跟踪误差维持不变。

具体地，s11步骤所述的fm-omr四个麦克纳姆轮之间的速度约束运动学模型构建包括：

s111、首先对fm-omr的结构进行描述；

s112、其次对fm-omr的速度约束进行描述：

将车轮的转速表示为车轮的平移速度表示为而vir，i＝1，2，3，4，是自由移动辊的切向速度，类似地，我们可以将机器人框架中轮子的合成速度表示为：

v1x＝v1m+v1rcosα1v1y＝v1rsinα1

v2x＝v2m+v2rcosα2v2y＝v2rsinα2

v3x＝v3m+v3rcosα3v3y＝v3rsinα3

v4x＝v4m+v4rcosα4v4y＝v4rsinα4

其中α1＝α3＝45，α2＝α4＝-45，此外，由于轮速与机器人本体的速度是刚性连接的，因此轮速与机器人本体的速度相关，因此，这种相关性可以建立为：

v1x＝vx-l1wv1y＝vy+l2w

v2x＝vx+l1wv2y＝vy+l2w

v3x＝vx-l1wv3y＝vy-l2w

v4x＝vx+l1wv4y＝vy-l2w

因此，可以获得fm-omr机体的速度以及四个轮子的角速度的关系如下：

其中r是如下雅可比矩阵：

其中矩阵r不是方阵，因此没有逆矩阵，然而，广义逆矩阵可以表示为：

因此可以得到：

因此，由上面获得：

同样的，可以从上式得到以下等式：

ω1+ω2＝ω3+ω4

调整四个麦克纳姆轮各自的速度，可以实现在给定的平面内期望的全向运动，因此，每个轮子的约束产生作用于整个机体，四个麦克纳姆轮之间的速度约束关系由ω1+ω2＝ω3+ω4表示，车身速度与四个轮子角速度之间的关系已经得出，机器人本体的速度可以表示如下：

其中，

s113、最后构建fm-omr的运动学模型：

从上述式子，可以得出非完整fm-omr的运动学模型如下：

具体地，s12步骤所述的fm-omr跟踪误差运动学建模具体过程为：

s121、确定领导者rr以及跟随者r的物理参数，其中对于领导者小车，有：

s122、根据领导者以及跟随者在世界坐标系下的位置和角度计算rr在r坐标系下的相对位置和角度(xe,ye,θe)：

其中，(x,y,θ)以及(xr,yr,θr)是分别都是对应于r和rr的惯性协同坐标系；

s123、根据所得的(xe,ye,θe)计算其导数，建立其导数与输入[u1,u2,u3]之间的状态方程，所述导数表示为：

其中，

a＝w1r+w2r+w3r+w4r

b＝w1r-w2r-w3r+w4r

c＝w1+w2+w3+w4

d＝w1-w2-w3+w4

其中，a、b是rr四个车轮角速度的线性组合，c、d是r四个车轮角速度的线性组合；

则综上四个麦克纳姆轮之间具有速度约束的轨迹跟踪误差运动学模型为：

其中，

u3＝wr-w；

s124、通过控制输入[u1,u2,u3]，误差状态量[xe,ye,θe]可以收敛到原点。

具体地，步骤s13中，mpc(模型预测控制)过程可以看成是寻找损失函数的最优解。在该搜索空间的每个采样实例处，n个未来采样实例的输出可从系统模型获得，而损失函数是通过预测和真实系统状态输出之间的估计误差得到的。因此，通过最小化损失函数，导出即时n个采样实例的最优控制输入。值得强调的是，只有第一控制向量被当作系统的输入，这个过程是迭代执行的，并且在每次迭代中实现优化，直到获得最优解。

设计结合速度约束方程的误差模型预测控制器时，连续时变系统用离散时间模型的形式重新表示，每个特征点的离散时间状态空间模型可以表示为：

ze(k+1)＝l(k)ze(k)+m(k)u(k)

其中，表示输入向量，表示状态量，参数n和m表示状态和输入变量的数量；根据采样定理，给定连续时间系统可以用采样周期t离散化，因此，上式可以写成如下形式：

所述状态空间模型中，为了导出给定预测层np的最优控制序列，损失函数可以表示为：

其中，δu(k)表示输入向量的增量，有δu(k)＝u(k)-u(k-1)。ze(k+j|k)和△u(k+j|k)表示基于当前时间k，在时间k+j,上的预测变量，此外，nc代表控制层，np代表预测层，最后，q和p表示适当的加权矩阵，t为采样周期；

从以上式子，可以得出：

定义了如下形式的预测向量：

其中，np＝3，nc＝2。且对于所有k≥0时刻，约束表示如下：

其中状态量ze的最大值和最小值输入向量的最大值和最小值输入向量的增量δu(k)的最小值和最大值均为某一给定的常数；

预测状态可以表示为如下形式：

其中，

最终，基于约束条件下代价函数的empc算法可通过解以下约束来对问题进行优化：

这取决于：

具体地，s13步骤所述的误差模型预测控制器中实时的empc算法通过以下步骤实现：

s131、选择适当的控制器参数，包括nc,np,q,p,t,t1；其中，nc代表控制层参数，np代表预测层参数；q,p表示适当的加权矩阵；t为采样周期，t1为控制周期。

s132、解优化问题得到输入增量：

s133、计算得到k时刻时的输入向量；

s134、将计算的u(k)输入到系统中，更新状态向量以及ze的信息；

s135、根据u(k)解出四个电机的角速度[w1,w2,w3,w4]；

s136、如果则k＝k+1，重新回到步骤s132，否则退出。

具体地，所述误差模型预测控制器采用机器人操作系统ros的仿真平台进行仿真验证。所述仿真平台为一台装有机器人操作系统(ros)的pc电脑，该电脑系统是ubuntu14.04，其中数据可视化平台rviz是ros中一款可视化仿真软件。

如图1所示，考虑一台移动机器人，装备有四个麦克纳姆轮，轮子沿着机器人平台的两侧成对安装，并且相对于其质心是均匀的。因此麦克纳姆轮小车可以实现在平面上以三个自由度运动，即沿x、y轴平移和围绕z轴旋转。fm-omr的物理参数如图9所示。

如图2所示，定义了两个小车的坐标系以及参数。可以根据已知参数代入已建立的运动学模型中。

如图3所示，为了验证所提出的跟踪误差模型适用于各种类型的fm-omr。使用两个具有不同参数的fm-omr进行轨迹跟踪控制。rr是领导者fm-omr，其轨迹随时间变化。r是跟随者fm-omr，跟随rr的轨迹行走。r的参数与图9中的参数相同。因此，r的运动学模型也类似可以用上述式子描述。rr的主要物理参数如图10所示。其他参数设置与r类似，其中约束等式也适用于rr，有如下等式：ω1r+ω2r＝ω3r+ω4r。

在两个速度约束的fm-omr运动学模型基础上建立跟踪误差运动学模型。控制输入[u1,u2,u3]，使得误差状态量[xe,ye,θe]可以收敛到原点。

基于约束条件下代价函数的empc可以通过解以下约束来对问题进行优化：

这取决于：

其中，我们取：

在其实时形式中，empc的优化问题可以使用滚动时域的方法解决。在此方法中，仅将给定输入序列的前三维元素视为系统的输入，丢弃所有其他元素。然后，在下一个时间段里，对于新的输出重复这个过程。

如图4至图7所示，仿真中控制器参数定义为：nc＝2,np＝3,q＝400i,p＝200i。跟随者小车r的参数为rw＝0.08m,l1＝0.32m,l2＝0.30m。领导者小车rr的参数为rwr＝0.05m,l3＝0.22m,l4＝0.20m。具体的参数解释如图9，图10所示。图4、图5分别表示曲线轨迹追踪实验、相关的跟踪误差；图6、图7表示四个轮子的角速度和跟随者小车r的四个麦克纳姆轮之间的矢量约束。图4的仿真实验中使用所提出的empc控制方法跟踪曲线参考轨迹。领导者小车生成参考轨迹的参数定义为：

如图8所示，在机器人操作系统(ros)中通过rviz软件上的仿真实验验证了本发明所提出的基于轨迹跟踪误差全向移动机器人运动学建模的误差模型预测控制方法。

本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。