基于目标跟踪控制策略的单移动机器人最优巡回控制方法

摘要

一种基于目标跟踪控制策略的单移动机器人最优巡回控制方法，包括地图构建、任务描述、拓扑构建、路径寻优、任务分解、目标跟踪控制和位姿信息反馈7个步骤。首先，基于线性时序逻辑理论，将构建的加权切换系统与线性时序逻辑任务公式相融合，构建任务可行网络拓扑；然后，采用基于Dijkstra算法的路径寻优方法搜索出最优路径，并将环境中对应最优路径上的离散坐标点存储在缓存中，之后按序发送给目标跟踪控制器；然后，采用目标跟踪控制方法，根据机器人与目标点之间距离和角度的偏差，给出控制量控制机器人向目标点运行；最后，在机器人运行过程中，采用全局摄像头实时获取机器人当前的位置坐标和朝向信息，并反馈给目标跟踪控制器，从而实现闭环控制。

说明书

技术领域

本系统涉及计算机图像处理、路径规划、移动机器人控制、通信等领域，尤其涉及一种基于目标跟踪 控制策略的单移动机器人最优巡回控制方法。

背景技术

指示、规划、控制机器人以代替人类工作是目前工业自动控制领域的难点之一，近年来已受到了广泛 的关注。机器人控制的主要目标是采用高级的近似人类逻辑的语言向机器人指示任务，使机器人在客观限 制条件下自主将高级语言转换成低级的原始命令来完成任务，其中原始命令包括根据反馈控制器建立输入 输出之间的对应关系，根据通信协议完成机器人个体之间或机器人与服务器之间的信息交换等，客观限制 条件包括机器人自身的限制及外界物理环境对机器人的限制，机器人自身的限制包括机械限制、能量限制、 计算能力及感知能力限制、通信范围的限制等，环境限制包括各种形状的障碍物及动态目标等。

对单移动机器人的控制包括上层的路径规划和基于规划路径的底层运动控制。传统的机器人路径规划 方法主要针对“从A到B并避开障碍物”这种简单的点到点任务，而无法完成诸如“顺序访问A、B两点， 然后巡回C、D两点”这类包含复杂巡回任务的最优路径规划。此外，传统的机器人运动控制方法主要集中 在模糊控制、基于视觉的导航控制、学习控制、基于红外循迹控制等方法。其中，模糊控制需要根据经验 和实验数据来制定控制策略，精度不高，自适应能力有限；基于视觉的导航控制需要实时采集并处理图像 数据，处理的数据量大，对于光源、算法和硬件配置的效率要求较高；学习控制通用性差，机器学习与参 数的调整是一个较为复杂的过程，性能较差；基于红外寻迹的移动机器人控制算法对光线有着特别的要求， 而且需要给定红外识别对象，适用范围局限。基于目标跟踪控制策略的单移动机器人最优巡回控制方法的 机器人运动控制采用了目标跟踪控制策略，区别于诸如模糊控制等传统的机器人控制策略，目标跟踪控制 策略控制具有精度高、适用范围广、扩展性强、实现方便等优点，使得移动机器人能够实现最优路径的准 确跟踪，从而更有效地完成复杂的巡回任务。

基于目标跟踪控制策略的单移动机器人最优巡回控制方法可以广泛的应用于如机器人排障、工厂等特 殊环境的巡回监测、火灾救灾等实际应用中，实现机器人替人，从而降低任务风险，减少损失，提高工作 效率。

发明内容

本发明要克服现有机器人路径规划及控制方法存在的上述缺点，提出一种基于目标跟踪控制策略的、 高精度、适应性广、高效的单移动机器人最优巡回控制方法，包括基于线性时序逻辑的上层路径寻优和基 于寻优路径的底层运动控制。首先，上层采用基于线性时序逻辑的最优路径规划方法规划出满足任务需求 的最优路径，然后底层采用目标跟踪控制策略实现路径跟踪控制，同时结合实时的机器人位姿反馈信息(机 器人的位置坐标和朝向信息)及时校准机器人的运动，实现闭环控制。

本发明解决其技术问题所采用的技术方案是：

一种基于目标跟踪控制策略的单移动机器人最优巡回控制方法，包括地图构建、任务描述、拓扑构建、 路径寻优、任务分解、目标跟踪控制和位姿信息反馈7个步骤，其特征在于：首先，对输入的环境信息进 行建模，构建加权切换系统模型，同时输入描述任务需求的线性时序任务公式，并进行转换，输出任务图 表；其次，将加权切换系统模型和任务图表进行融合，构建任务可行网络拓扑；接着，采用基于Dijkstra 算法的最优路径搜索算法在任务可行网络拓扑上搜索出满足任务需求的最优路径，并将任务可行网络拓扑 上的最优路径映射回加权切换系统模型，获得环境中满足任务需求的最优路径；之后，将实际环境中对应 最优路径上的离散坐标点存储在缓存中，并结合当前任务完成情况，按序读取缓存中的目标点；然后，采 用目标跟踪控制策略，结合目标点坐标信息和实时反馈的机器人位置与朝向信息，根据机器人与目标点之 间的距离和角度的偏差给出控制量，控制机器人完成最优路径跟踪；最后，实时采集机器人当前运行环境 的图像信息，通过色块识别的方法，采用UDP通信的方式，反馈回机器人当前的位置坐标和朝向信息，具 体实现步骤如下：

步骤1，地图构建

将机器人当前运行环境构建为加权切换系统模型；将移动机器人所在的运行环境划分为有限多个方块 区域，每个独立的区域用字母q_i∈Q表示，其中i∈{1,2,3,...}，Q是区域的集合，Q₀表示机器人的初始位 置，机器人在环境中的移动对应区域的变化，表示为→；从一个区域到相邻区域的移动距离或是机器人移 动的成本(权值)，表示为ω，移动机器人在特定区域所执行的任务表示为π_i∈Π，其中i∈{1,2,3,...}，Π是 所有任务的集合；如此构成一个转换系统模型，用字母T表示：T＝(Q,Q₀,→,Π,ω)；

步骤2，任务描述

采用线性时序逻辑语言来描述指定任务，以线性时序任务公式的形式表达任务需求，并将任务公式转 换为任务图表；线性时序逻辑语言产生的表达式基于切换系统的任务集合π_i∈Π，线性时序逻辑语言的语 法包括如下：线性时序逻辑语言的谓词包括(非)、∨(或)、∧(与)、□(全局)、◇(最终)、U(直到)；用线性 时序逻辑语言表述的任务需求用字母φ表示，φ是由切换系统模型中的子任务π_i∈Π和谓词所组成的表达 式：假设φ₁和φ₂为两个任务需求，φ₁表示不能执行φ₁，φ₁∧φ₂表示执行φ₂同时避免执行φ₁，φ₁∧(□φ₂) 表示执行φ₁和φ₂并最终执行φ₂，(◇□φ₁)∧(◇□φ₂)表示循环执行φ₁和φ₂；采用线性时序逻辑语言表述的移动 机器人巡回任务为：

(◇□O)∧(◇□q₁)∧(◇□q₂)∧(◇□q₃)∧...

其中，O为机器人运行环境中需要全局避开的区域，q_i∈Q为环境中需要巡回的节点；在获得任务表达式 后，采用LTL2BA工具箱将任务表达式φ转换成对应的Büchi自动机，Büchi自动机是可判断输入的路径 序列是否满足任务需求的图表，用字母B表示：B＝(S,S₀,∑_B,δ_B,F)，其中S是有限状态集合，是 初始状态集合，∑_B是输入字母表，δ_B是转换函数，是最终状态集合；Büchi自动机可以保证路径满 足任务需求；

步骤3，拓扑构建

将加权切换系统模型和任务图表相融合构建任务可行网络拓扑；把输入的加权切换系统模型T与任务 图表B作笛卡尔乘积，得到任务可行网络拓扑，用字母P表示，即P＝T×B，其中T＝(Q,Q₀,→,Π,ω)， B＝(S,S₀,∑_B,δ_B,F)，P＝(S_P,S_p0,→_P,F_P)，S_P＝Q×S为有限状态集合，S_p0＝Q₀×S₀为初始状态集合， 为转换函数，F_P＝Q×F为最终状态集合；在任务可行网络拓扑上所包含的路径，既满足指 定的任务需求，又符合机器人当前的运行环境；

步骤4，路径寻优

采用基于Dijkstra算法的最优路径搜索算法，根据线性时序逻辑任务公式中要求访问节点的出现顺序， 在任务可行网络拓扑上搜索出最优路径，然后任务可行网络拓扑上的最优路径映射回加权切换系统模型 上，获得实际环境中的最优路径；其中，搜索所得的最优路径为一个机器人运行环境中最优路径上对应的 离散坐标点集；

步骤5，任务分解

将步骤4所得的最优路径存入缓存，并结合当前任务完成情况(已访问的点)，按序将离散坐标点集中 的坐标信息发送给目标跟踪控制器，用以控制机器人实现路径跟踪，直到遍历离散坐标点集中的所有点， 以确保机器人完成完整的路径跟踪；

步骤6，目标跟踪控制

结合步骤5给出的目标点坐标(x^*,y^*)和实时反馈的机器人当前位置坐标(x,y)，根据两轮差动机器人 运动学特性，可以得到机器人到目标点的距离偏差r为：

$r=\sqrt{{(x^{*}-x)}^2+{(y^{*}-y)}^2}$

根据全局摄像头实时采集的机器人上前后两个不同颜色的色块中心点坐标信息，换算得到坐标系X轴 的正方向按顺时针旋转到机器人正面朝向所经过的角度θ，从而可以得到机器人与目标点之间的角度偏差 φ为：

其中，可表示为：

根据上述获得的机器人与目标点之间的距离偏差r和角度偏差φ，采用具有如下形式的控制器，获得 机器人的线速度和转动速度

υ＝K₁rcos(φ)

ω＝-K₁sin(φ)cos(φ)-K₂φ

其中，K₁和K₂为控制参数，υ为机器人向前运行的速度，ω表示机器人转向时的角速度；然后，再根据 两轮差动机器人的运动学特性得到移动机器人左右两轮的控制量为：

${\upsilon }_L=\frac{\upsilon }{2\pi }-\frac{d}{4\pi }\omega$

${\upsilon }_R=\frac{\upsilon }{2\pi }+\frac{d}{4\pi }\omega$

其中，υ_L和υ_R分别表示机器人的左右两轮速度，d为机器人左右两轮之间的距离；在控制量υ_L和υ_R的 作用下，移动机器人朝着给定的目标点运行。

由上述υ_L和υ_R两式可以看出υ部分引导机器人向目标点运动，对保证机器人电机的正常运行至关重 要；ω部分控制机器人正面朝向目标点，即将φ逐渐调整为0；K₂可调节机器人转向的灵敏度，因此当K₁和K₂差值过大时机器人将产生很高的角速度，在有限的采样速率下，将导致控制量不能及时修正而造成 超调，导致机器人蛇形前进，影响控制效果，因此控制参数K₁和K₂取值的差值不可过大。此外，目标跟 踪控制器能够控制机器人实现目标跟踪，也就意味着，在有限的时间内，机器人到目标点的距离r和机器 人与目标点之间的角度偏差φ能够收敛到0。因此，构造李雅普诺夫能量函数：

$V=\frac{1}{2}(r^2+{\phi }^2)$

求导可得：

$\stackrel{·}{V}=r\stackrel{·}{r}+\phi \stackrel{·}{\phi }=r(-v\cos \phi )+\phi (\omega +\frac{v}{r}\sin \phi )$

其中，和由对r和φ两式求导所得：

$\left(\begin{array}{c}\stackrel{·}{r}\\ \stackrel{·}{\phi }\end{array}\right)=\left(\begin{array}{cc}-\cos \phi & 0\\ \frac{1}{r}\sin \phi & 1\end{array}\right)\left(\begin{array}{c}\upsilon \\ \omega \end{array}\right)$

根据李雅普诺夫稳定性判据可知当K₁>0且K₂>0时，即理论能上只要控制参数K₁和K₂为 正，则所设计的目标跟踪控制策略就可以引导机器人至目标点；

步骤7，位姿信息反馈

在机器人运行过程中，采用全局相机实时采集机器人当前运行环境的图像信息，通过图像处理记录并 显示机器人的运行轨迹，并以色块识别的方法将机器人当前的位置坐标信息和朝向信息通过通信协议UDP 反馈给步骤6中给出的目标跟踪控制器，从而实现闭环控制，其中，机器人的朝向通过获取机器人上两个 不同颜色的色块的中心坐标来换算得到；此外，当机器人运动到当前给定的目标点时，重复执行步骤5， 给出下一个目标点的坐标，实现分段目标跟踪，最终达到路径跟踪的效果。

本发明的技术构思为：本发明针对巡回类任务，通过地图构建模块、任务描述模块、拓扑构建模块、 路径寻优模块、位姿信息反馈模块和目标跟踪控制器模块各模块的设计，采用基于线性时序逻辑的最优路 径规划方法规划出满足任务需求的最优路径，然后采用目标跟踪控制方法实现路径跟踪控制，达到了巡回 类任务的需求。

本发明的优点是：采用了线性时序逻辑语言来描述复杂任务需求，将任务以线性时序逻辑公式的形式 表达，更贴近人类自然语言，简洁方便；采用基于线性时序逻辑的路径规划方法，能够搜索出既满足线性 时序逻辑公式，又符合机器人当前运行环境的最优路径；采用目标跟踪控制方法实现移动机器人的路径跟 踪控制，区别于传统的控制方法，精度更高，适用范围更广，且实现方便；采用分段目标跟踪的方法最终 达到路径跟踪的效果，使得机器人最终的实际路径和理想的最优路径偏差较小；采用一台全局摄像头，通 过色块识别的方法同时获取机器人的位置和朝向信息，便捷且误差较小，同时能够记录并显示机器人实际 的运动轨迹。本发明可以广泛的应用于诸如火灾救灾、狭小空间搬运、排障等危险的实际应用中，实现机 器人替人，减少损失，降低任务风险。

附图说明

图1是本发明的系统结构框架

图2是本发明系统设计总体框架

图3是本发明闭环目标跟踪控制方法的基本框图

图4是本发明目标跟踪控制方法流程图

图5是本发明加权切换系统示例

图6是本发明可行网络拓扑示例

图7是本发明所采用的两轮差动机器人运动学特性示意图

图8(a)是本发明机器人与目标点之间距离变化曲线图

图8(b)是本发明机器人与目标点之间的角度变化曲线图

图8(c)是本发明机器人左右两轮速度变化仿真结果示意图

图9是本发明位姿信息反馈模块定位程序显示界面

图10是本发明实验室地图

图11是本发明实验案例结果示意图

具体实施方式

以下结合附图对本发明的技术方案作进一步描述。本发明实施例用来说明本发明，而不是对本发明进 行限制，在本发明的精神和权利要求的保护范围内，对本发明做出的任何修改和改变，都落入本发明的保 护范围。

图1为基于目标跟踪控制策略的单移动机器人最优巡回控制方法结构框图，图2为基于目标跟踪控制策 略的单移动机器人最优巡回控制方法实施例的总体框架，它包括一台网络摄像机、一台图像处理服务器、 一台路径规划计算机、两轮差动机器人，以及机器人运行的实验平台。

本实施例中以图5所示的加权切换系统为例。

步骤1，地图构建

将环境区域划分为5个部分Q1＝{q₀,q₁,q₂,q₃,q₄}，其中Q1₀＝q₀表示机器人的初始位置，机器人在 环境中的移动对应区域的变化，表示为→。从一个区域到相邻区域的移动距离或是机器人移动的成本(权 值)，如图5中箭头上的数字，表示为ω1。移动机器人在特定区域所执行的任务表示为π_i∈Π1，其中 i∈{1,2,3,...}，Π是所有任务的集合。如此构成一个转换系统模型，用字母T1表示：T1＝(Q1,Q1₀,→,Π1,ω1)；

步骤2，任务描述

采用线性时序逻辑语言来描述指定任务，以线性时序任务公式的形式表达任务需求，并将任务公式转 换为任务图表。以图5所示加权切换系统为例，任务要求巡回访问{q₁,q₄}两点，任务描述模块2用线性时 序逻辑表达式φ3将任务表示为：

φ3＝(◇□q₁)∧(◇□q₄)

采用LTL2BA工具箱将表达式转换成对应的Büchi自动机B1输出至拓扑构建模块3。

步骤3，拓扑构建

将T1和B1作笛卡尔乘积，得到任务可行网络拓扑，用字母P1表示，即P1＝T1×B1。如图6所示即 为所得的任务可行网络拓扑P1。

步骤4，路径寻优

采用基于Dijkstra算法的最优路径搜索算法，在输入的任务可行网络拓扑上搜索出满足任务需求的最优 路径。以图6所示的任务可行网络拓扑为例，路径寻优模块4搜索所得最优路径为：

(q₀,s₀)→(q₁,s₀)→(q₂,s₁)→(q₄,s₁)→(q₃,s₂)→(q₁,s₀)→...

其中(q₁,s₀)→(q₂,s₁)→(q₄,s₁)→(q₃,s₂)→(q₁,s₀)→...为巡回部分。将其映射回图5所示的加权切换 系统上的最优路径为：

q₀→q₁→q₂→q₄→q₃→q₁→q₂→...

其中，q₁→q₂→q₄→q₃→q₁→q₂→...为巡回部分，该路径既满足任务φ3的需求，又符合图5所示 的机器人运行环境。

步骤5，任务分解

将步骤4所得最优路径对应的的坐标点集{q₀,q₁,q₂,q₄,q₃,q₁,q₂,q₄,...}存入缓存中，并结合当前任务 完成情况(已访问的坐标点)，按序将坐标点集中的坐标发送给目标跟踪控制器。

步骤6，目标跟踪控制

结合步骤5给出的目标点坐标(x^*,y^*)和实时反馈的机器人当前位置坐标(x,y)，根据两轮差动机器人 运动学特性，可以得到机器人到目标点的距离偏差r为：

$r=\sqrt{{(x^{*}-x)}^2+{(y^{*}-y)}^2}$

根据全局摄像头实时采集的机器人上前后两个不同颜色的色块中心点坐标信息，换算得到坐标系X轴 的正方向按顺时针旋转到机器人正面朝向所经过的角度θ，从而可以得到机器人与目标点之间的角度偏差 φ为：

其中，可表示为：

根据上述获得的机器人与目标点之间的距离偏差r和角度偏差φ，采用如下控制策略：

υ＝K₁rcos(φ)

ω＝-K₁sin(φ)cos(φ)-K₂φ

其中，K₁和K₂为控制参数，υ为机器人向前运行的速度，ω表示机器人转向时的角速度；然后，再根据 两轮差动机器人的运动学特性得到移动机器人左右两轮的控制量为：

${\upsilon }_L=\frac{\upsilon }{2\pi }-\frac{d}{4\pi }\omega$

${\upsilon }_R=\frac{\upsilon }{2\pi }+\frac{d}{4\pi }\omega$

其中，υ_L和υ_R分别为目标跟踪控制模块输出的机器人左右两轮速度，d为机器人左右两轮之间的距离。υ_L和υ_R控制移动机器人朝着给定的目标点运动。

本例中根据机器人当前运行的环境取K₁＝8，K₂＝10。图8所示即为目标跟踪控制方法的仿真结果 图，其中图8(a)为机器人与目标点之间距离的变化示意图，图8(b)为机器人与目标点之间角度的变化示意图， 图8(c)为机器人左右两轮速度的变化示意图。

步骤7，位姿信息反馈

在机器人运行过程中，采用全局相机实时采集机器人当前运行环境的图像信息，通过图像处理记录并 显示机器人的运行轨迹，并以色块识别的方法将机器人当前的位置坐标信息和朝向信息通过通信协议UDP 反馈给步骤6中给出的目标跟踪控制器，实现闭环控制；其中，机器人的朝向通过获取机器人上两个不同 颜色的色块的中心坐标来换算得到；此外，当机器人运动到当前给定的目标点时，重复执行步骤5，给出 下一个目标点的坐标，实现分段目标跟踪，最终达到路径跟踪的效果。图9即为位姿信息反馈模块定位程 序显示界面，其中定位程序通过色块识别来定位机器人当前的坐标，左边栏用来选取需要定位的色块，右 上角区域为图像实时采集区域，右下角区域为所选色块坐标变化曲线记录区域。

以图10所示的实验室地图为例，阴影部分为障碍区域，贴有黄蓝色块的物体为机器人，黄色为机器人 的正面，白色节点为选取的加权切换系统节点，节点之间的距离即为加权切换系统的权值，任意节点之间 都是连通的。给定任务“巡回访问A、B、C、D、E五个节点”。基于目标跟踪控制策略的单移动机器人最 优巡回控制方法最终获得的路径如图11所示。

实验显示基于目标跟踪控制策略的单移动机器人最优巡回控制方法能够规划出既满足任务需求，又符 合环境信息的最优路径，并能够较为精确的控制机器人完成最优路径跟踪。