1.自主机器人近距离操作运动规划体系在研究(组图)

1.自主机器人近距离作业运动规划系统

在研究自主运动规划问题之前，首先需要建立一个比较完善的自主运动规划系统，然后以该系统为指导，对自主运动规划的各种具体问题进行深入研究。本节将根据自主机器人的思维方式、运动形式、任务行为等特点，建立相应的自主运动规划系统。根据机器人的数量和规模，自主运动规划分为两种规划系统：单机器人运动规划和多机器人协同运动规划。

1.1 单个自主机器人的规划系统

运动规划系统是自主控制系统中主控单元的核心部分，因此有必要首先研究自主控制系统及其主控单元的架构。

自自主控制技术研究以来，各种架构形式相继出现。目前，分布式架构在实践中应用广泛，各个功能模块作为一个相对独立的单元参与到整个系统中。随着人工智能技术的不断发展，基于多智能体的分布式架构逐渐成为主流。每个功能模块作为一个独立的代理参与整个自主控制过程。该架构的应用基本形式如图1所示。一方面，主控单元及测控干预处理、姿态控制系统、轨道控制系统、热控系统、能量系统、数据传输、载荷控制等功能子系统作为智能体相互独立，通过总线连接；另一方面，主控单元对整个系统进行整体规划，协调和管理各个子系统Agent的行为。TT&C干预处理Agent保证了地面系统对整个系统任何层面的控制和干预能力，可以接受上游任务级任务、具体飞行计划和底层控制指令；每个子系统Agent存储子系统的各种知识和控制算法，自主完成主控单元下发的任务规划，并将执行和自身健康等信息回传给主控单元，

图1 基于多智能体的分布式自主控制系统架构基本形式示意图

主控单元Agent采用主流的层次结构，层次分明，非常有利于实现。其基本结构如图2所示。主控制单元由三个基本结构组成：任务生成与调度、运动行为规划和控制命令生成。基本飞行任务从任务生成和调度层得到，具体的行为规划通过运动行为规划层得到，进而产生控制命令。该层获得最终的模块控制命令并将其发送给其他功能代理。每个功能代理向主控单元的状态检测系统发送状态信息，状态检测系统将任务执行情况和子系统状态反馈给任务生成调度层，以根据具体情况对任务进行规划和调整。遇到紧急情况时，也可以开启重规划模块，根据当前情况快速反应，快速生成行为计划，指导控制指令生成层获取应急控制指令。此外，地面控制系统在三个层面都具有干预能力。图2中虚线为主控单元的所有模块，虚线为运动规划系统，包括运动行为规划模块和再规划模块，也是运动的主要功能规划系统。以便根据具体情况规划和调整任务。遇到紧急情况时，也可以开启重规划模块，根据当前情况快速反应，快速生成行为计划，指导控制指令生成层获取应急控制指令。此外，地面控制系统在三个层面都具有干预能力。图2中虚线为主控单元的所有模块，虚线为运动规划系统，包括运动行为规划模块和再规划模块，也是运动的主要功能规划系统。以便根据具体情况规划和调整任务。遇到紧急情况时，也可以开启重规划模块，根据当前情况快速反应，快速生成行为计划，指导控制指令生成层获取应急控制指令。此外，地面控制系统在三个层面都具有干预能力。图2中虚线为主控单元的所有模块，虚线为运动规划系统，包括运动行为规划模块和再规划模块，也是运动的主要功能规划系统。可以根据当前情况快速反应，快速生成行为计划，指导控制指令生成层获取应急控制指令。此外，地面控制系统在三个层面都具有干预能力。图2中虚线为主控单元的所有模块，虚线为运动规划系统，包括运动行为规划模块和再规划模块，也是运动的主要功能规划系统。可以根据当前情况快速反应，快速生成行为计划，指导控制指令生成层获取应急控制指令。此外，地面控制系统在三个层面都具有干预能力。图2中虚线为主控单元的所有模块，虚线为运动规划系统，包括运动行为规划模块和再规划模块，也是运动的主要功能规划系统。

图2 主控单元基本结构示意图

在明确了自主控制系统及其主控单元的基本结构，以及运动规划系统在主控单元中的基本功能后，就可以建立运动规划系统的架构了。运动规划系统架构如图3所示。系统由规划器和再规划器两个执行单元组成，分别承担飞行任务的总体规划和突发事件应急处理的运动规划。当然，这两部分也可以理解为线下规划和线上规划。线下规划一般解决平时的预定飞行任务，线上规划一般解决突发性飞行任务。除了策划师，该系统还配备了知识领域模块，以特定语言描述相关知识。知识域包括行为域和模型域两部分。行为域用于存储服务系统的一般运动行为描述以及紧急情况下（如急停、转向等）某些运动行为的处理方法，模型域用于存储所需的模型知识用于规划，包括环境模型、装配模型、装配任务对象模型和任务模型等。

图3 运动规划系统架构示意图

1.2 多自主机器人协同规划系统

多智能体系统的群架构一般分为集中式和分散式两种基本结构，分散式结构又可以进一步分为层次结构和分布式结构。在集中式结构中，一个主控单元通常掌握环境和被控机器人的所有信息，利用规划算法对任务进行分解，分配给被控机器人，并组织它们完成任务。优点是理论清晰，实现更直观；缺点是容错性、灵活性和对环境的适应性较差，每个受控机器人存在通信瓶颈问题。与集中式结构相比，分散式结构虽然无法获得全局最优解，但由于其可靠性、灵活性和环境适应性强，越来越受到人们的青睐。分散式结构中的分布式结构没有主控单元，每个代理的地位都是平等的。通过代理之间的沟通和信息交换，达到协商的目的，达到最终的决定。但这种结构容易片面强调个人，造成资源占用。太多了，很难得到咨询结果。层次结构介于集中式和分布式之间。有主控单元，但不受主控单元控制。每个代理也有一定程度的自主权。

多自主机器人系统应采用分层结构，保证整个系统既适合统一领导，又能满足系统灵活性和速度的需要。多自主机器人协同规划架构如图4所示。根据层次结构建立两种工作模式。主控单元负责前期离线规划。首先得到协同任务，通过得到具体的行为运动规划。分布到各个子系统执行单元，相关知识域主要用于描述各个子系统协商规则的协商域直线搜索方法,无约束优化方法,约束优化方法，主控单元从外界获取环境信息，从各个子系统获取状态信息; 当遇到紧急情况时，当紧急任务发生变化或主控单元停止工作时，各子系统采用分布式结构，独立规划自己的运动行为，从自己的知识域中获取协商方法。外部环境信息由主控单元发送并结合自我感知。获取（当主控单元停止工作时，仅依靠自身感知获取信息），其他机器人信息的传递通过机器人之间的数据链路实现。外部环境信息由主控单元发送并结合自我感知。获取（当主控单元停止工作时，仅依靠自身感知获取信息），其他机器人信息的传递通过机器人之间的数据链路实现。外部环境信息由主控单元发送并结合自我感知。获取（当主控单元停止工作时，仅依靠自身感知获取信息），其他机器人信息的传递通过机器人之间的数据链路实现。

图4 多自主机器人协同规划架构示意图

2.路径规划研究

当被赋予特定任务时，如何规划机器人的运动将至关重要。机器人的规划由两部分组成：将底座移动到适合操作的位置和转动手臂关节完成操作。它由三个问题组成：基础点对点运动规划；联合空间规划；综合规划。

本章研究了几种常用的运动规划算法：图搜索法、RRT算法、人工势场法、BUG算法。并对一些算法的缺陷进行了一些改进。

2.1 图搜索方法

图搜索方法依靠已知的环境地图和地图中的障碍物信息来构建从起点到终点的可行路径。主要分为深度优先和广度优先两个方向。深度优先算法首先扩展了对深度较大的节点的搜索，可以快速得到一条可行的路径，但深度优先算法得到的第一条路径往往是一条较长的路径。广度优先算法优先扩展深度较小的节点，是一种类似波浪的搜索方法。广度优先算法搜索到的第一条路径是最短路径。

2.1.1 可视化查看方法

可见视图法由-Perez和1979提出，是机器人全局运动规划的经典算法。在视觉方法中，机器人用点来描述，障碍物用多边形来描述。起点

，目标

和多边形障碍物的顶点（设置

是所有障碍物的顶点的集合）进行组合连接，要求起点与障碍物的顶点之间的连接，目标点与障碍物的顶点之间的连接，以及障碍物的顶点与障碍物的顶点之间的连接。顶点不能穿过障碍物。物体，即直线是“可见的”。为图中的边分配权重以构建可见图

. 点集

,

是所有可见边的弧段的集合。然后使用一些优化算法从起点开始搜索

到目标点

，那么通过对这些直线的距离进行累加比较，就可以得到从起点到目标点的最短路径。

图 5 可见视图

可见，使用可见视图方法规划避障路径的主要目的是构建可见视图，而构建可见视图的关键在于判断障碍物顶点之间的可见性。判断主要分为两种情况，判断同一障碍物顶点间的可见性和判断不同障碍物顶点间的可见性。

在同一个障碍物内，相邻顶点可见（通常不考虑凹多边形障碍物中不相邻的顶点也可能可见），不相邻的顶点不可见，权重赋值为

.

不同障碍物之间顶点可见性的判断转化为判断顶点连接是否会与其他顶点连接相交的几何问题。如下图虚线所示，

,

障碍

,

顶点，但是

和

连接线与障碍物其他顶点的连接线相交，所以

,

之间不可见；而实线显示

和

连接线不与障碍物其他顶点的连接线相交，所以

,

之间可见。

图 6 顶点可见性判断

视觉法可以找到最短路径，但搜索时间长，缺乏灵活性，即一旦机器人的起点和目标点发生变化，重建视觉就很麻烦。可见视图法适用于多边形障碍物，不适用于圆形障碍物。切线法和图法改进了可视图法。切线图法用障碍物的切线表示圆弧，因此是从起点到目标点的最短路径图，移动机器人必须几乎接近障碍物行走。缺点是如果在控制过程中出现位置误差，机器人与障碍物发生碰撞的可能性会很大。图形方法表示具有尽可能远离障碍物和墙壁的路径的弧。

2.1.2 算法

该算法由荷兰计算机科学家 Wybe 发明，通过计算从初始点到自由空间任意点的最短距离可以得到全局最优路径。该算法从初始点开始计算4或8个周围点与初始点的距离，然后以新计算的距离点为计算点，计算其周围点与初始点的距离，使得计算像波前一样在自由空间中进行。传播直到到达目标点。这样就可以计算出机器人的最短路径。

该算法是经典的广度优先状态空间搜索算法，即算法会从初始点逐层搜索整个自由空间，直到到达目标点。这大大增加了计算时间和数据量。并且大量的搜索结果对机器人运动毫无用处。

详情请参考：路径规划-算法：

2.1.3 A*算法

为了解决算法效率低的问题，提出了A*算法作为启发式算法。该算法在广度优先的基础上增加了一个评价函数。

详情请参考：路径规划-A*算法：

2.2RRT算法

快速搜索随机树（RRT）算法是一种增量采样搜索方法，在应用中不需要任何参数调优，性能良好。它在不设置任何分辨率参数的情况下，逐步构建搜索树以逐渐提高分辨率。在极端情况下，搜索树会密集地填充整个空间，搜索树由许多较短的曲线或路径组成，以达到填充整个空间的目的。增量法构建的搜索树的方向取决于密集的采样序列。当序列为随机序列时，搜索树称为快速搜索随机树（Tree，RRT），无论序列是随机的还是确定的。两者都称为快速搜索密集树（RDT），

2.2.1 算法步骤

考虑环境中有静态障碍物的 2D 和 3D 工作空间。初始化一棵快速随机搜索树T，只包括根节点，即初始状态S。在自由空间中随机选取一个状态点

，遍历当前快速随机搜索树T，求T上的距离

最近的节点

，从控制输入集中考虑机器人的动态约束

选择输入

, 从状态

在一个控制周期后开始运行

达到一个新的状态

. 满足

和

控制输入

以获得最佳控制。新状态

加入到快速随机搜索树T中。按照这样得到的方法，不断产生新的状态，直到达到目标状态G。搜索树构建完成后，从目标点开始，逐个找到父节点，直到到达初始状态，即搜索树的根节点。

图 7 随机树构建过程

由于在搜索过程中考虑了机器人的动态约束，因此生成路径的可行性很好。然而，算法的随机性只导致概率完整性。

2.2.2 改进算法

等人的工作。为 RRT 方法奠定了基础。在采样策略方面，该方法控制机器人的随机运动，以一定的概率移动到最终目标；该方法分别对整个空间和目标点周围的空间进行采样；该方法通过增加随机步长来提高规划速度。也采用了双向规划的思想，衍生出各种算法。

基本的 RRT 算法收敛到最终姿势可能很慢。为了提高算法的效率和性能，需要不断地改进算法。例如，为了提高搜索效率，采用双向随机搜索树（Bi~RRT），从起点和目标点并行生成两棵RRT，直到两棵树相遇，算法收敛。由于该算法比原来的RRT算法收敛性更好，在目前的路径规划中很常见。所提出的粒子RRT算​​法考虑了地形的不确定性，保证了搜索树在不确定环境中的扩展。并且还提出了RRT-，大大提高了节点的扩展效率。在运动规划中，距离的定义非常复杂。研究了RRT生长过程中距离函数的持续学习算法，以降低距离函数对环境的敏感性。考虑到基本RRT规划器得到的路径长度一般为最优路径的1.3~1.5倍，英国的J.研究了变分法使其最优。Amna A 引入 KD 树作为二级数据结构，以加快搜索最接近从环境中取出的随机点的叶节点，降低搜索成本。该算法已广泛应用于动态障碍物、高维状态空间以及运动学和动力学等具有微分约束的环境中的运动规划。

改进RRT算法的详细内容请参考：路径规划——改进RRT算法：

2.3 滚动在线 RRT 算法

基本的 RRT 算法倾向于遍历整个自由空间，直到获得可行的路径，这使得它无法用于机器人在未知或动态环境中的在线运动规划。利用滚动规划的思想，可以对RRT算法进行改进，使其具备在线规划的能力。

2.3.1 滚动计划

当机器人在未知或动态环境中移动时，它只能在其传感器范围内的有限区域内检测环境信息。机器人利用局部信息进行局部运动规划，根据一定的评价标准获取局部目标。机器人到达局部目标后，再次进行新的局部规划。重复此过程，直到达到全球目标。

滚动规划算法的基本原理：

环境信息预测：在滚动的每一步，机器人根据视野内检测到的信息，或所有已知的环境信息，建立环境模型，包括设置已知区域的节点类型信息等；

局部滚动优化：将上述环境信息模型视为一个优化窗口。在此基础上，根据目标点的位置和具体的优化策略计算出下一个最优子目标，然后根据子目标和环境信息模型，选择局部规划算法，确定局部路径到子目标，执行当前策略，即按照规划的局部路径走几步，窗口会相应地向前滚动；

反馈信息修正：根据局部最优路径，驱动机器人行走一定路径后，机器人会检测到新的未知信息。此时，可以根据机器人在行走过程中检测到的新信息对原有的环境模型进行补充或修正，进行滚动。下一步的本地规划。

其中，局部子目标是在滚动窗口中找到一个全局目标的映射，它必须避开障碍物并满足一些优化指标。子目标的选择方法体现了全局优化的要求和局部有限信息约束之间的折衷，是在给定信息环境下试图实现全局优化的自然选择。

基于滚动窗口的路径规划算法依靠实时检测到的局部环境信息以滚动方式进行在线规划。在滚动的每一步，根据检测到的局部信息，采用启发式方法生成优化子目标，在当前滚动窗口内进行局部路径规划，然后执行当前策略（根据局部规划路径移动一步） )，随着滚动窗口前进，不断获取新的环境信息，从而在滚动中实现优化与反馈相结合。由于规划问题被压缩到滚动窗口中，与全局规划相比，计算量大大减少。

基于滚动窗口的路径规划算法具体步骤如下：

2.3.2 滚动在线RRT算法流程

在滚动窗口内，随机树从当前位置开始，在传感器范围内构建随机树。构造方法与基本的RRT算法一致。为了使全局环境中的随机树趋向于向目标方向生长，在运动规划中引入启发式信息以降低随机树的随机性，提高搜索效率。

制作

表示随机树中两个姿势节点之间的路径成本，

表示随机树中两个姿势节点之间的欧几里得距离。与 A* 算法类似，该算法为随机树中的每个节点定义了一个评估函数：

. 在

是一个随机节点

到树中的节点

所需的路径成本。

对于启发式评估函数，这里我们取随机节点

到目标点

距离是估计值，

. 所以

表示从节点

通过随机节点

到目标节点

路径估计。在滚动窗口中遍历随机树T，取评价函数最小值的节点

，有

. 这使得随机树沿着目标节点的评估值

向最小方向扩展。

由于在随机树生长中引入了面向目标的启发式评估因子，叶节点

总是选择离目标最近的节点，这会使随机树遇到局部最小值问题。所以随机树生长的新节点

需要克服这个问题，引导随机树更好地探索未知空间。

这里通过统计回归分析生成新节点，进一步增强RRT算法探索未知空间的能力，避免启发式评价因素造成的局部极小值。这个想法是探索以前访问过的空间是无用的，并且容易陷入局部最小值。引入回归分析（ ）是为了检查新节点与其他节点之间的关系，并使用回归函数约束，使随机树不探索它之前访问过的空间，从而避免局部最小值。

新的节点生成方法是遍历随机树，如果

与其父节点

距离小于

与扩展树上任何其他节点的距离，即

，该节点被选为随机树的新节点。下图说明了新节点的判断过程。

图8 新节点的判断

上图中的空心点是中间父节点的可能扩展。椭圆圈出的空心点表示新节点不满足回归函数的约束。其余两个未被圈出的空心节点与其父节点的距离小于该节点与随机树上任意节点的距离。这两个点可以成为随机树的一个新节点。

综上所述，滚动窗口中随机树构建的具体步骤如下：

初始化滚动窗口随机树T，T最初只包含初始位置S；

在滚动窗口的空闲空间中随机选择一个状态

;

根据最短路径思想找到树T中和

最近的节点

;

选择输入

，因此机器人状态由下式给出

到达

;

当然

是否符合回归分析，如果不符合，返回步骤4；

将要

作为随机树 T 的一个新节点，

记录在连接节点中

和

在边缘。

在滚动窗口状态空间中采样K次后，遍历随机树，根据启发式评估思想找到滚动窗口子目标

.

是当前滚动窗口中子树中评价函数最小的点。确定子目标后，机器人前进到子目标点，进行下一轮滚动RRT规划。重复此过程，直到到达目标点 G。

2.4 人工势场法

人工势场法是一种虚拟力的机器人运动规划方法。其基本思想是将目标和障碍物对机器人运动的影响具体化为一个人工势场。目标处的势能低，障碍物处的势能高。这个势差产生了目标对机器人的引力和障碍物对机器人的斥力，合力控制机器人沿着势场的负梯度方向向目标点移动。人工势场法计算方便，得到的路径安全平稳，但复杂的势场环境可能会在目标点之外产生局部极小点，从而阻碍机器人到达目标。为了解决人工势场法的局部极小点问题，学者们提出了各种改进方法。主要分为两个方向：一是构造合适的势函数，减少或避免局部极小值的出现；另一种是结合其他方法，使机器人在遇到局部最小值后离开局部最小值。前者一般需要全局地图信息，并且取决于障碍物的形状。环境复杂时难以应用。后者采用搜索法、多势场法和沿墙行走法使机器人离开局部极小点。搜索方法使用最好的优先，模拟退火，随机搜索等策略找到一个势场值低于局部最小点的点，使机器人继续运动。由于在未知环境中缺乏启发式信息，搜索方法的效率很低。多势场法构造多个具有相同全局最小点但局部最小点不同的势函数。当机器人落入局部最小点时，规划器切换势函数使机器人离开该点。但是这样的多重势场在未知环境中很难构建，这种方法可能会导致机器人返回到它所逃脱的局部最小值。由于局部极小点是由一个或多个障碍物的排斥势场和引力势场共同作用产生的，所以它的位置和障碍物一定相距不远。沿墙行走法就是利用这个距离让机器人遇到障碍物。到达局部极小点后，参照周围类似BUG算法的行为绕过产生局部极小点的障碍物，继续前进。该方法可靠性高，不依赖环境和障碍物形状的先验信息。参考类似于BUG算法的周边行为绕过产生局部极小点的障碍物继续前进。该方法可靠性高，不依赖环境和障碍物形状的先验信息。参考类似于BUG算法的周边行为绕过产生局部极小点的障碍物继续前进。该方法可靠性高，不依赖环境和障碍物形状的先验信息。

本节为机器人平移的在线运动规划构建人工势场直线搜索方法,无约束优化方法,约束优化方法，并通过沿墙行走的方法改进了基本的人工势场方法。

2.4.1 基本人工势场法

作用在机器人上的假想引力和斥力是势函数的负梯度，因此人工势函数应具有以下特点：

非负且连续可微；

排斥势越靠近障碍物越强；

引力势越接近目标位置，其强度越小。

空间中的合成势场是引力势场和排斥势场的总和：

在，

是目标产生的引力势场；

是每个障碍物产生的排斥势场之和，即：

.

这里构造了以下引力和排斥势函数：

在，

表示引力势的相对影响；

意味着第一个

障碍物排斥势的相对影响，