CN109491389B - 一种具有速度约束的机器人轨迹跟踪方法 - Google Patents

Abstract

本发明公开了一种具有速度约束的机器人轨迹跟踪方法，包括（1）设置人运动区域、起始点、障碍物；（2）进行路径规划，先用改进的快速探索随机树算法得出路径，再对路径进行优化和曲线拟合，得到一条光滑的路径；（3）根据机器人的参数，建立速度约束；（4）建立***预测模型，根据路径节点预测下一时刻的最优输入速度；（5）重复执行步骤(4)，求出最优输入速度；（6）根据最优速度输入控制机器人进行运动。该方法可以实现机器人沿路径进行快速、平稳地移动，且能够十分准确地拟合所规划的路径，误差小、鲁棒性强。

Description

一种具有速度约束的机器人轨迹跟踪方法

技术领域

本发明涉及一种机器人轨迹跟踪方法，具体涉及一种对全向移动机器人的速度进行控制的轨迹跟踪方法。

背景技术

移动机器人如今在各个领域都受到了广泛的研究，它不仅在工业、航天、公共事业等方面得到了应用，而且在服务、生活领域也有了很大发展。与传统的非完整约束的双驱动轮式机器人相比，具有完整约束的全向移动机器人(OMR)的轮子可以同时和分开旋转，以实现任何方向的平移和复杂的弧形运动，所以引起了研究人员的极大关注。全向移动机器人具有灵活、建模简单、可操纵性好等优点，主要研究方面集中在点对点稳定、路径规划、轨迹跟踪、速度控制等方面。

模型预测控制(MPC)算法最早是工业过程控制领域的一种计算机控制算法，现在作为一种反馈控制策略被广泛讨论。MPC算法主要针对多变量、约束对象，原理如下：在每个采样时刻，根据***的测量信息，在线求解一个有限时域开环优化问题，可以求得当前时刻和以后的控制序列，但是只取第一个控制变量来控制当前时刻，在下一时刻，重复此过程，用新的测量信息处理新的优化问题。MPC算法凭借它只注重模型功能、不注重模型形式，运用滚动优化减小不确定性，拥有反馈校正，算法易拓展等特点得到了广泛应用。目前，将MPC算法应用于机器人运动控制也十分有效。然而，在运用模型预测控制算法进行机器人的轨迹跟踪过程中，对机器人的速度进行控制是一个重点。如果没有对速度进行约束，那么机器人会在跟踪的每个点处静止下来，再在下一时刻获得速度，这样它的速度会一直发生变化，而在进行速度控制之后，机器人可以实现速度的稳定，符合实际需求。

发明内容

为解决背景技术中全向移动机器人轨迹跟踪的问题，本发明公开了一种具有速度约束的全向移动机器人轨迹跟踪方法。假设已知机器人运动的起点和终点以及起始点之间存在的障碍，在机器人运动之前先通过快速探索随机树算法得到一条连结起始点的无碰撞的可行路径，将路径信息输入到模型预测控制算法中。根据机器人的实际信息和想施加的速度约束信息，建立预测模型。根据所建立的预测模型和该时刻的路径信息，预测出下一时刻的最优速度，由预测的速度进行机器人的移动。在下一时刻，重复预测步骤，由此得到每个时刻机器人的速度。因为在模型中加入了速度约束，所以可以实现速度的稳定。

技术方案：为了实现上述目的，本发明采用如下技术方案：一种具有速度约束的机器人轨迹跟踪方法，包括如下步骤：

(1)设置人运动区域、起始点、障碍物；

(2)进行路径规划，先用改进的快速探索随机树算法得出路径，再对路径进行优化和曲线拟合，得到一条光滑的路径；

(3)根据机器人的三个轮子的速度界限和机器人的运动范围，建立速度约束；

(4)建立***预测模型，根据路径节点预测下一时刻的最优输入速度；

(5)重复执行步骤(4)，求出每一时刻的最优输入速度；

(6)根据最优速度输入控制机器人进行运动。

快速探索随机树(RRT)算法是一种通过在空间内采样来随机构建树的路径规划方法，它具有概率完备性。它通过在无障碍空间内随机取点生成树，树在原则上向大部分未探测区域生长，从而可以占据大部分自由空间，最后找出一条从起始点到终点的可行路径。本发明中使用的是它的改进算法——RRT*，上述步骤(2)中的改进的快速探索随机树算法如下：

定义全局环境X，其中障碍空间Xobstacle，无障碍空间Xfree＝X/Xobstacle，假设障碍空间的障碍是事先已知的且静止的；起点qinit和终点qend位于无障碍空间内；要规划的路径是树T的一部分，树由一个个采样点V和采样点之间的连线E构建形成，其中采样点称为叶子节点，连线称为树枝；

在X中取随机点qrand，找到T中与qrand最近的节点qnear，连结形成向量Vector；在Vector上从qnear开始取步长step，形成新节点qnew；若qnew与qnear连线通过碰撞检测发现发生了碰撞，则放弃此次生长；

在qnew附近建立节点集合，以qnew为圆心，预设的R为半径作圆所产生的圆形空间，比较将落入圆内的树节点和qnew连结后树的累积代价，选择代价最小的节点作为qnew的父节点qnear，加入树中；

假设新加入树的节点qnew为父节点，计算落入以它为半径的圆内节点的代价；选择代价最小的节点qmin，将它与qnew连结，并删除原来与qmin连结的枝；qmin作为qnew的子节点加入树中；

重复上述步骤，得到一系列叶子节点；当某一节点与终点qend的距离小于步长时，连结两点，若之间无碰撞，则找到了一条从起点qinit到终点qend的可行路径。

上述步骤(4)中的***预测模型由以下方法获得：

全向移动机器人的运动学模型表示为

其中

是OMR在笛卡尔坐标中的状态，

是OMR在世界坐标中的状态，H是变换矩阵，θ_c为转向角；

根据全向轮的模型，得到

其中矩阵R由全向轮的参数得到，决定了OMR的全向性，[v₁ v₂ v₃]^T为机器人三个轮子的速度；

OMR的逆运动学方程为

令S＝R^-1，则

推导出连续时间***方程

对公式(1)离散化，得到

其中T为采样时间；

将其转换为离散状态空间模型，并得到X(k+1)＝AX(k)+BU(k)，其中X(k+1)为下一时刻的状态，X(k)为该时刻的状态，U(k)为速度的变化量，A、B为系数矩阵。

输出模型为Y(k)＝CX(k)+DU(k)，其中C、D为系数矩阵。根据滚动优化的理论，在某一时刻的输入无法影响该时刻的输出，所以矩阵D为0矩阵，则Y(k)＝CX(k)。

所以预测模型为

上述步骤(5)中的最优输入速度由以下方法得到：

假设控制输入为Δu(k_i),Δu(k_i+1),...,Δu(k_i+N_p-1)，共N_p个，

状态为x(k_i+1|k_i),x(k_i+2|k_i),...,x(k_i+N_c|k_i),...,x(k_i+N_p|k_i)；

由状态空间模型，得

其中A、B、C为不同的系数矩阵。

定义向量

Y＝[y(k_i+1|k_i)y(k_i+2|k_i)y(k_i+3|k_i)...y(k_i+N_p|k_i)]^T

ΔU＝[Δu(k_i)Δu(k_i+1)Δu(k_i+2)...Δu(k_i+N_c-1)]^T

所以预测输出Y＝Fx(k_i)+ΦΔU，其中

假设设定点信号r(k_i)，且k_i时刻的闭环性能调节参数为r_ω；令

和转换矩阵

所以代价函数

通过最小化成本函数，获得最优控制输入，即最小化预测***输出和期望参考输出之间的差异；

考虑输入约束Δu^min≤Δu(k)≤Δu^max和输出约束Y^min≤Fx(k_i)+ΦΔU≤Y^max；

由于存在约束，无法获得优化问题的解析解，使用数值优化方法来解决；

对于具有约束的MPC算法，简化后的目标函数是二次的，动态方程和时域约束是线性的，因此它是二次规划(QP)问题。带约束的二次规划的形式是

二次规划问题通过调用函数库来解决，然后获得最优控制输入。我们为QP算法添加约束，包括上限/下限约束和参考约束，以便速度可以稳定在一个值附近，从而实现速度控制。

本发明具有以下优点：

(1)利用RRT*算法，可以快速得到一条连结起点到终点的无障碍的路径，该路径接近最优路径。

(2)通过模型预测控制算法，可以使全向移动机器人快速的沿路径进行移动，所运动的轨迹和RRT*算法得到的路径的误差很小。

(3)通过改进模型，在其中加入速度约束，可以使机器人保持稳定的速度进行运动，符合实际情况。

附图说明

图1是本发明的轨迹跟踪方法流程图。

图2是本发明的路径规划结果示意图。

图3是基于本发明模拟机器人运动的速度曲线仿真(不具有速度约束)。

图4是基于本发明模拟机器人运动的速度曲线仿真(具有速度约束)。

图5是基于本发明模拟机器人的运动轨迹仿真。

图6是基于本发明模拟机器人的运动误差仿真。

具体实施方式

下面结合附图并通过具体实施例对本发明作进一步详述，以下实施例只是描述性的，不是限定性的，不能以此限定本发明的保护范围。

一种具有速度约束的机器人轨迹跟踪方法，流程图如图1所示，具体包括以下步骤：包括如下步骤：

(1)设置人运动区域、起始点、障碍物；

(2)进行路径规划，先用改进的快速探索随机树算法得出路径，再对路径进行优化和曲线拟合，得到一条光滑的路径；

(3)根据机器人的三个轮子的速度界限和机器人的运动范围，建立速度约束；

(4)建立***预测模型，根据路径节点预测下一时刻的最优输入速度；

(5)重复执行步骤(4)，求出每一时刻的最优输入速度；

(6)根据最优速度输入控制机器人进行运动。

通过改进的快速探索随机树算法得到一条连结起始点的无碰撞的可行路径；

设定机器人所运动的空间范围X(具体实施中运动范围为21m*21m，但不限于此)、起点qinit和终点qend(具体实施中起点为(0m,0m)，终点为(20m,20m)，但不限于此)，并随机设定障碍物的数量和大小，将障碍物的形状设置为圆形，为图2中黑色区域，白色区域为机器人的可运动区域。忽略机器人的大小和形状，将其视为质点。经过RRT*算法的规划得到的路径为图中蓝色节点所组成的曲线，之后再进行优化，将可以连结并且无碰撞的点连结起来，使路径的长度减小，最后经过曲线拟合后，得到一条光滑的曲线，为图中红色曲线。

全向移动机器人的运动学模型为

根据全向轮的模型，我们得到

其中矩阵R的秩为3，它表明机器人可以实现全向移动，θ_c为转向角。

所以OMR的逆运动学方程为

令S＝R^-1，则

因此我们可以推导出连续时间***方程

对上式离散化，得到

其中T为采样时间(具体实施中T为0.02s，但不限于此)。

将其转换为离散状态空间模型，并得到X(k+1)＝AX(k)+BU(k)，即

其中X(k+1)为下一时刻的状态，X(k)为该时刻的状态，U(k)＝[Δv₁ Δv₂ Δv₃]^T为速度的变化量,r为机器人轮子的半径。

假设控制输入为Δu(k_i),Δu(k_i+1),...,Δu(k_i+N_p-1)共N_p个，

状态为x(k_i+1|k_i),x(k_i+2|k_i),...,x(k_i+N_c|k_i),...,x(k_i+N_p|k_i)。

由状态空间模型，得

其中A、B、C为不同的系数矩阵。

定义向量

Y＝[y(k_i+1|k_i)y(k_i+2|k_i)y(k_i+3|k_i)...y(k_i+N_p|k_i)]^T

ΔU＝[Δu(k_i)Δu(k_i+1)Δu(k_i+2)...Δu(k_i+N_c-1)]^T

所以预测输出Y＝Fx(k_i)+ΦΔU，其中

假设设定点信号r(k_i)，且k_i时刻的闭环性能调节参数为r_ω。令

和转换矩阵

所以代价函数

因此，通过最小化成本函数，可以获得最优控制输入，即，最小化预测***输出和期望参考输出之间的差异。

考虑输入约束Δu^min≤Δu(k)≤Δu^max(具体实施中输入约束为机器人每个车轮的旋转速度范围[-2m/s，2m/s]，但不限于此)和输出约束Y^min≤Fx(k_i)+ΦΔU≤Y^max(具体实施中输出约束为运动区域范围[-20m，20m]，但不限于此)。

接下来通过调用函数库来解决QP问题，然后可以获得最优控制输入。

本发明的实施例如下：

本实施例中，仿真区域(0m,0m)和(21m,21m)之间的矩形区域，机器人的起点为(0m,0m)，目标终点为(20m,20m)，障碍物的中心坐标分别为(5m,5m)、(6m,13m)、(10m,8.5m)、(15m,14m)、(16m,5m)，其半径分别为2m、2.5m、2m、3.5m、3m。由优化的RRT*算法所规划的路径如图2所示，为一条平滑的曲线。

实例进行了轨迹跟踪，其各个输入速度如图3和图4所示。上部子图是机器人的实际运动速度，中间子图是机器人在x和y方向上的速度分量，下部子图是机器人的角速度。图3是没有速度控制的结果。可以看出，机器人在各个节点处获得一定的速度，然后慢慢收敛到零，并在下一节点处获得新的初速度。这种情况实际上只能实现多个点对点稳定，并没有全局速度控制。而且，当机器人在实际行走时，机器人会经常停下来，对机器人造成损害，不符合实际情况。图4是经过速度控制的结果，可以看出，机器人在零时刻获得初速度，然后保持稳定运动，并且运动的时间相比图3大大减少。机器人在x和y方向上的速度曲线及其角速度曲线都是平滑曲线，这说明了速度控制的有效性。

实例所进行的轨迹跟踪的结果如图5所示，图中粗线为优化的RRT*算法所规划出的路径，细线为机器人的实际行走轨迹。可以看出，机器人的运动曲线很好地拟合了所规划出的路径，两条曲线基本实现了重合。路径和实际运动曲线的误差如图6所示，可以看出，误差一般在-0.04m和0.04m之间，最大误差不超过0.07m。

本领域内的技术人员可以对本发明进行改动或变型的设计但不脱离本发明的思想和范围。因此，如果本发明的这些修改和变型属于本发明权利要求及其等同的技术范围之内，则本发明也意图包含这些改动和变型在内。

Claims (1)

1.一种具有速度约束的机器人轨迹跟踪方法，其特征在于，包括如下步骤：

(1)设置人运动区域、起始点、障碍物；

(2)进行路径规划，先用改进的快速探索随机树算法得出路径，再对路径进行优化和曲线拟合，得到一条光滑的路径；

(3)根据机器人的三个轮子的速度界限和机器人的运动范围，建立速度约束；

(4)建立***预测模型，根据路径节点预测下一时刻的最优输入速度；

(5)重复执行步骤(4)，求出每一时刻的最优输入速度；

(6)根据最优速度输入控制机器人进行运动；

所述步骤(2)中的改进的快速探索随机树算法如下：

定义全局环境X，其中障碍空间Xobstacle，无障碍空间Xfree＝X/Xobstacle，假设障碍空间的障碍是事先已知的且静止的；起点qinit和终点qend位于无障碍空间内；要规划的路径是树T的一部分，树由一个个采样点V和采样点之间的连线E构建形成，其中采样点称为叶子节点，连线称为树枝；

在X中取随机点qrand，找到T中与qrand最近的节点qnear，连结形成向量Vector；在Vector上从qnear开始取步长step，形成新节点qnew；若qnew与qnear连线通过碰撞检测发现发生了碰撞，则放弃此次生长；

在qnew附近建立节点集合，以qnew为圆心，预设的R为半径作圆所产生的圆形空间，比较将落入圆内的树节点和qnew连结后树的累积代价，选择代价最小的节点作为qnew的父节点qnear，加入树中；

假设新加入树的节点qnew为父节点，计算落入以它为半径的圆内节点的代价；选择代价最小的节点qmin，将它与qnew连结，并删除原来与qmin连结的枝；qmin作为qnew的子节点加入树中；

重复上述步骤，得到一系列叶子节点；当某一节点与终点qend的距离小于步长时，连结两点，若之间无碰撞，则找到了一条从起点qinit到终点qend的可行路径；

所述步骤(4)中的***预测模型由以下方法获得：

全向移动机器人的运动学模型表示为

其中

是OMR在笛卡尔坐标中的状态，

是全向移动机器人在世界坐标中的状态，H是变换矩阵，θ_c为转向角；

根据全向轮的模型，得到

其中矩阵R由全向轮的参数得到，决定了全向移动机器人的全向性，[v₁ v₂ v₃]^T为机器人三个轮子的速度；

OMR的逆运动学方程为

令S＝R^-1，则

推导出连续时间***方程

对公式(1)离散化，得到

其中T为采样时间；

将其转换为离散状态空间模型，并得到X(k+1)＝AX(k)+BU(k)，其中X(k+1)为下一时刻的状态，X(k)为该时刻的状态，U(k)为速度的变化量，A、B为系数矩阵；

输出模型为Y(k)＝CX(k)+DU(k)，其中C、D为系数矩阵；根据滚动优化的理论，在某一时刻的输入无法影响该时刻的输出，所以矩阵D为零矩阵，则Y(k)＝CX(k)；

所以预测模型为

所述步骤(5)中的最优输入速度由以下方法得到：

假设控制输入为Δu(k_i),Δu(k_i+1),...,Δu(k_i+N_p-1)，共N_p个，

状态为x(k_i+1|k_i),x(k_i+2|k_i),...,x(k_i+N_c|k_i),...,x(k_i+N_p|k_i)；

由步骤(4)得到的状态空间模型

得

定义向量

Y＝[y(k_i+1|k_i) y(k_i+2|k_i) y(k_i+3|k_i) ... y(k_i+N_p|k_i)]^T

ΔU＝[Δu(k_i) Δu(k_i+1) Δu(k_i+2) ... Δu(k_i+N_c-1)]^T

所以预测输出Y＝Fx(k_i)+ΦΔU，其中

假设设定点信号r(k_i)，且k_i时刻的闭环性能调节参数为r_ω；令

和转换矩阵

所以代价函数

通过最小化代价函数，获得最优控制输入，即最小化预测***输出和期望参考输出之间的差异；

考虑输入约束Δu^min≤Δu(k)≤Δu^max和输出约束Y^min≤Fx(k_i)+ΦΔU≤Y^max；

由于存在约束，无法获得优化问题的解析解，使用数值优化方法来解决；

带约束的二次规划的形式是

二次规划问题通过调用函数库来解决，然后获得最优控制输入。

Images