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

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

A Trajectory Tracking Method for Robots with Velocity Constraints

The invention discloses a trajectory tracking method for robots with speed constraints, which includes: (1) setting human motion area, starting point and obstacles; (2) path planning, obtaining a path by using improved fast search random tree algorithm, then optimizing and curve fitting to obtain a smooth path; (3) establishing speed constraints according to the parameters of robots; (4) Establish a system prediction model to predict the optimal input speed at the next moment according to the path nodes; (5) repeat the execution steps (4) to find the optimal input speed; (6) control the robot according to the optimal input speed. This method can realize the robot moving along the path quickly and smoothly, and can accurately fit the planned path, with small error and strong robustness.

【技术实现步骤摘要】
一种具有速度约束的机器人轨迹跟踪方法
本专利技术涉及一种机器人轨迹跟踪方法，具体涉及一种对全向移动机器人的速度进行控制的轨迹跟踪方法。
技术介绍
移动机器人如今在各个领域都受到了广泛的研究，它不仅在工业、航天、公共事业等方面得到了应用，而且在服务、生活领域也有了很大发展。与传统的非完整约束的双驱动轮式机器人相比，具有完整约束的全向移动机器人(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的全向性，[v1v2v3]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(ki),Δu(ki+1),...,Δu(ki+Np-1)，共Np个，状态为x(ki+1|ki),x(ki+2|ki),...,x(ki+Nc|ki),...,x(ki+Np|ki)；由状态空间模型，得其中A、B、C为不同的系数矩阵。定义向量Y＝[y(ki+1|ki)y(ki+2|ki)y(ki+3|ki)...y(ki+Np|ki)]TΔU＝[Δu(ki)Δu(ki+1)Δu(ki+2)...Δu(ki+Nc-1)]T所以预测输出Y＝Fx(ki)+ΦΔU，其中假设设定点信号r(ki)，且ki时刻的闭环性能调节参数为rω；令和转换矩阵所以代价函数通过最小化成本函数，获得最优控制输入，即最小化预测系统输出和期望参考输出之间的差异；考虑输入约束Δumin≤Δu(k)≤Δumax和输出约束Ymin≤Fx(ki)+ΦΔU≤Ymax；由于存在约束，无法获得优化问题的解析解，使用数值优化方法来解决；对于具有约束的MPC算法，简化后的目标函数是二次的，动态方程和时域约束是线性的，因此它是二次规划(QP)问题。带约束的二次规划的形式是二次规划问题通过调用函数库来解决，然后获得最优控制输入。我们为QP算法添加约束，包括上限/下限约束和参考约束，以便速度可以稳定在一个值附近，从而实现速度控制。本专利技术具有以下优点：(1)利用RRT*算法，可以快速得到一条连结起点到终点的无障碍的路径，该路径接近最优路径。(2)通过模型预测控制算法，可以使全向移动机器人快速的沿路径进行移动，所运动的轨迹和RRT*算法得到的路径的误差很小。(3)通过改进模型，在其中加入速度约束，可以使机器人保持稳定的速度进行运动，符合实际情况。附图说明图1是本专利技术的轨迹跟踪方法本文档来自技高网...

【技术保护点】
1.一种具有速度约束的机器人轨迹跟踪方法，其特征在于，包括如下步骤：(1)设置人运动区域、起始点、障碍物；(2)进行路径规划，先用改进的快速探索随机树算法得出路径，再对路径进行优化和曲线拟合，得到一条光滑的路径；(3)根据机器人的三个轮子的速度界限和机器人的运动范围，建立速度约束；(4)建立系统预测模型，根据路径节点预测下一时刻的最优输入速度；(5)重复执行步骤(4)，求出每一时刻的最优输入速度；(6)根据最优速度输入控制机器人进行运动。

【技术特征摘要】
1.一种具有速度约束的机器人轨迹跟踪方法，其特征在于，包括如下步骤：(1)设置人运动区域、起始点、障碍物；(2)进行路径规划，先用改进的快速探索随机树算法得出路径，再对路径进行优化和曲线拟合，得到一条光滑的路径；(3)根据机器人的三个轮子的速度界限和机器人的运动范围，建立速度约束；(4)建立系统预测模型，根据路径节点预测下一时刻的最优输入速度；(5)重复执行步骤(4)，求出每一时刻的最优输入速度；(6)根据最优速度输入控制机器人进行运动。2.根据权利要求1所述的一种具有速度约束的机器人轨迹跟踪方法，其特征在于，所述步骤(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的可行路径。3.根据权利要求1所述的一种具有速度约束的机器人轨迹跟踪方法，其特征在于，所述步骤(4)中的...

【专利技术属性】
技术研发人员：刘小峰，李金明，
申请(专利权)人：河海大学常州校区，
类型：发明
国别省市：江苏,32