CN112917477B - 多自由度机器人静态环境运动规划方法 - Google Patents

Abstract

本发明公开了多自由度机器人静态环境运动规划方法，涉及机器人运动规划技术领域。本发明包括以下步骤：先在理线状态下搭建***，根据机器人运动的自由度建立中间姿态点形成的路径，之后检测这些路径的合法性，并集合实际工作将空间分割、建立坐标系，最后对合法路径进行障碍检测，并以真值表的形式输出安全路径。本发明通过真值表的实现适用于任意层级存储，改写数据较为灵活，且避免了因额外引入解码带来额外实时性开销的问题。

Description

多自由度机器人静态环境运动规划方法

技术领域

本发明属于机器人运动规划技术领域，特别涉及多自由度机器人静态环境运动规划方法。

背景技术

多自由度机器人的关节多，自由度高。全局规划算法：使用传统的数学建模的模型计算方法进行运动规划对算力需求增大。在需要实时控制的场合，对控制器的性能需求极大，往往不能满足。局部规划算法：只对空间的部分随机进行采样，可以大大缓解算力瓶颈问题。局部规划算法常见的有快速扩展随机树法(Rapidly Exploring Random Tree，RRT)和随机路标法(Probabilistic RoadMap，PRM)。

杜克大学智能机器人实验室于2016年提出了基于随机路标法的运动规划，基于空间分割法的碰撞检测方案。与传统方案不同的是，他们选定了少量但非常高效的路径，在离线状态下，让机械臂执行路径并记录碰撞到的栅格。以逻辑门电路的方式映射碰撞到的栅格和路径。对每个路径生成碰撞检测单元(collision detection unit，CDU)。以ASIC方式固化CDU产生运动规划专用芯片。实时执行时，当输入障碍物数据后，所有CDU可以并行进行碰撞检测得出所有路径的碰撞检测结果。例如使用4096个栅格编码空间，则一个路径的CDU为4096输入，1输出的微***。对于一个由1024条路径，空间由4096个栅格组成的检测***。

杜克大学的方案存在的问题是，ASIC固化CDU的方式实现周期过长，成本高，一旦完成，在应用改变时(需要修改机械臂模型，工作场景的情况)无法修改专用芯片的CDU，不具备灵活性。

中科大方案在杜克大学方案基础上使用RAM作为CDU的主要功能单元进行流片，实际使用时将碰撞数据以八叉树的形式压缩保存于外部并载入RAM中并进行解压缩使用，这种方法可以初步解决灵活性问题，但额外引入解码带来额外实时性开销的问题。

发明内容

本发明的目的在于提供多自由度机器人静态环境运动规划方法，解决现有的问题。

为解决上述技术问题，本发明是通过以下技术方案实现的：

本发明为多自由度机器人静态环境运动规划方法，包括以下步骤：

步骤S01：通过随机采样在机械臂的配置空间获取大量姿态点，为随机中间姿态点；

步骤S02：判断随机中间姿态点是否合法，并保留合法的随机中间姿态点；

步骤S03：遍历所有姿态点，连接满足要求的随机中间姿态点形成路径；

步骤S04：检查所形成的路径，确保不会与静态的环境或机械臂自身发生碰撞，形成合法路径集合；由于此时为***搭建阶段离线完成，对实时性无要求，在高性能服务器中采用任意高精度长耗时的碰撞检测算法；

步骤S05：在未来***的常见工作场景下离线执行运动规划，统计所有路径的使用频率；由于此时为离线完成，对实时性无要求，其中的碰撞检测在高性能服务器采用任意高精度长耗时的碰撞检测算法；

步骤S06：从合法路径集合中筛选指定数量的路径用于碰撞栅格的编码，优先选择使用频率高的，路径数量由***对成功率的要求；

步骤S07：根据坐标系的坐标把空间分割为若干个栅格；采用笛卡尔坐标系4096个栅格，同时根据场景，也可以采用其他坐标系，包括但不限于水平截面极坐标系和球坐标系；

步骤S08：在离线环境中分别执行每条路径，记录该路径执行过程中触碰到的栅格，由于此时为离线完成，对实时性无要求，其中的碰撞检测可以在高性能服务器采用任意高精度长耗时的碰撞检测算法；

步骤S09：构造CDU，根据上述步骤获得的碰撞数据，对每条路径产生真值表；以1024条路径为例，则产生了4096个输入，1024个输出的真值表，并将真值表写入***；

步骤S10：获取传感器数据，得到障碍物的占用栅格，获取数据的方式，包括但不限于点云传感器或图像传感器加机器视觉或模型法；

步骤S11：将障碍物的栅格作为真值表的输入，得到输出；

步骤S12：使用基于图论的搜索算法，包括但不限于A*算法或Dijkstra算法，搜索一条从起始姿态点到目标姿态点的线路。

优选地，所述S01-S09为***搭建步骤，且在离线状态下完成。

优选地，所述步骤S10-S12为***执行步骤，且在在线状态下完成。

优选地，所述步骤S07中的坐标系为笛卡尔坐标系。

优选地，所述步骤S07的栅格包括三种类型，分别为：

一、机械臂可以到达，且障碍物会出现的区域；

二、机械臂可以到达，但障碍物不会出现的区域；

三、障碍物会出现，但机械臂无法到达的区域。

在上述三类区域中，仅有一区域需要进行编码，在编码时应该尽可能减少对二、三区域的覆盖，并保证能够完全覆盖一区域。

优选地，所述步骤S11中，输出的真值表的结果为路径与当前障碍物发生了碰撞或路径依旧安全两种情况。

本发明具有以下有益效果：

本发明通过真值表的实现适用于任意层级存储，改写数据较为灵活，且避免了因额外引入解码带来额外实时性开销的问题。

当然，实施本发明的任一产品并不一定需要同时达到以上所述的所有优点。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将结合附图对实施例描述所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例的机械臂-码垛占用栅格在线并行碰撞检测原理图；

图2为本发明实施例的路径离散化和匀速执行示意图；

图3为本发明实施例的三角函数解算加速模块原理图；

图4为本发明实施例的机械臂A正运动学解算模块逻辑结构图；

图5为本发明实施例的两个线段最短距离求解示意图；

图6为本发明实施例的连杆最短距离计算模块示意图；

图7为本发明实施例的机械臂-机械臂碰撞判定模块示意图；

图8为本发明实施例的离散静态姿态生成模块示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。

本发明为多自由度机器人静态环境运动规划方法，包括以下步骤：

步骤S01：通过随机采样在机械臂的配置空间获取大量姿态点，为随机中间姿态点；

步骤S02：判断随机中间姿态点是否合法，并保留合法的随机中间姿态点；

步骤S03：遍历所有姿态点，连接满足要求的随机中间姿态点形成路径；

步骤S04：检查所形成的路径，确保不会与静态的环境或机械臂自身发生碰撞，形成合法路径集合；由于此时为***搭建阶段离线完成，对实时性无要求，在高性能服务器中采用任意高精度长耗时的碰撞检测算法；

步骤S05：在未来***的常见工作场景下离线执行运动规划，统计所有路径的使用频率；由于此时为离线完成，对实时性无要求，其中的碰撞检测在高性能服务器采用任意高精度长耗时的碰撞检测算法；

步骤S06：从合法路径集合中筛选指定数量的路径用于碰撞栅格的编码，优先选择使用频率高的，路径数量由***对成功率的要求；

步骤S07：根据坐标系的坐标把空间分割为若干个栅格；采用笛卡尔坐标系4096个栅格，同时根据场景，也可以采用其他坐标系，包括但不限于水平截面极坐标系和球坐标系；

步骤S08：在离线环境中分别执行每条路径，记录该路径执行过程中触碰到的栅格，由于此时为离线完成，对实时性无要求，其中的碰撞检测可以在高性能服务器采用任意高精度长耗时的碰撞检测算法；

步骤S09：构造CDU，根据上述步骤获得的碰撞数据，对每条路径产生真值表；以1024条路径为例，则产生了4096个输入，1024个输出的真值表，并将真值表写入***；

步骤S10：获取传感器数据，得到障碍物的占用栅格，获取数据的方式，包括但不限于点云传感器或图像传感器加机器视觉或模型法；

步骤S11：将障碍物的栅格作为真值表的输入，得到输出；

步骤S12：使用基于图论的搜索算法，包括但不限于A*算法或Dijkstra算法，搜索一条从起始姿态点到目标姿态点的线路。

优选地，所述S01-S09为***搭建步骤，且在离线状态下完成。

优选地，所述步骤S10-S12为***执行步骤，且在在线状态下完成。

优选地，所述步骤S07中的坐标系为笛卡尔坐标系。

优选地，所述步骤S07的栅格包括三种类型，分别为：

一、机械臂可以到达，且障碍物会出现的区域；

二、机械臂可以到达，但障碍物不会出现的区域；

三、障碍物会出现，但机械臂无法到达的区域。

在上述三类区域中，仅有一区域需要进行编码，在编码时应该尽可能减少对二、三区域的覆盖，并保证能够完全覆盖一区域。

优选地，所述步骤S11中，输出的真值表的结果为路径与当前障碍物发生了碰撞或路径依旧安全两种情况。

实施例一：

本发明以有6个自由度的机械臂为例，每个维度分贝表示1个关节的状态。

在基于随机路标法的运动规划中，碰撞检测占用了大部分的运算资源。在固定的碰撞检测时间内，需要进行碰撞检测的次数将显著影响规划的效率。在“Lazy策略”下，算法将在搜索路径时只对选中的路径进行碰撞检测，而不是对所有路径进行碰撞检测，这将减少碰撞检测次数。

由于采样的中间姿态点是随机产生，与采样的时间、状态、次数无关，PRM算法搜索的随机姿态点在***设计阶段预先采样生成。每次运动规划使用同一个采样集合。通过离线采样和筛选，主动剔除一部分姿态点，包括远离其它合法姿态的点或超出任务需求的低效率姿态点。通过对随机采样姿态点的预先离线选优，提升PRM实时执行的搜索效率。

运动规划算法prmsort(P^S，P^D)如下：

定义运动规划算法为prmsort(P^S，P^D)。其中规划的起始姿态点为P^S，规划的目标姿态点为P^D。

首先对连接P^S和P^D的进行路径预查找(Presort(P^S，P^D))，确定从P^S到P^D的直达路径在路径集合库中，并且路径不会与静态码垛发生碰撞。如果路径在此阶段预查找通过，进一步进行动态路径碰撞检测(Collideroute(P^S，P^D))，检测执行这条路径时是否会发生机械臂-机械臂之间动态的碰撞。如果路径合法，则返回用路径点序列[P^S，P^D]表示的线路。

若起点和终点之间的直达路径不能使用，则不断在随机中间姿态点集合库中随机获取一个中间姿态并使用同样的方法判断P^S与P^M _i+1之间的路径是否合法。若路径合法，则迭代调用运动规划算法prmsort(P^M _i+1，P^D)，尝试搜索P^M _i+1，P^D之间的路径，并产生路径[P^S，P^M _i+1，…，P^D]。否则，在迭代次数耗尽或者触发超时条件(TIMEOUT)时跳出循环并返回搜索失败。

路径预查找阶段***需要判断所选中的两个姿态点是否能够形成路径，且形成的路径是否与码垛发生碰撞。

路径预查找算法Presort(E^B，E^E)如下：

定义路径预查找算法为Presort(E^B，E^E)。其中机械臂路径起点E^B；机械臂路径终点为E^E。

首先路径集合库中所有的路径与码垛进行碰撞检测(CollideGrid())，并标记出安全的路径。接着在路径集合中寻找连接E^B，E^E的路径(Route(E^B，E^E))。如果找到了，则判定这条路径符合要求且不会与码垛发生碰撞。否则，这条路径可能因为不满足要求而未加入路径集合库或者与码垛发生了碰撞。

随机路标搜索中使用的随机中间姿态点集合库和路径预查找中使用的路径集合库在运动规划中发挥着非常重要的作用。通过产生索引表，路径和姿态点相互配合工作。

集合库的生成包括：随机中间姿态点离线生成，路径离线生成与筛选。虽然机械臂需要完成的任务是变化的，但它的起始姿态点和目标姿态点的终端位置是有限的。通过遍历所有起始姿态点和目标姿态点，对机械臂姿态解逆运动学，获得所有必须的起始姿态点和目标姿态点。

随机中间姿态点是连接起始姿态点和目标姿态点的桥梁。在一次运动规划中机械臂将从起始姿态点运动到若干个随机中间姿态点，并最终运动到目标姿态点。适当远离起始姿态点和目标姿态点的随机中间姿态点，能够绕开障碍物，提高搜索的成功率，在机械臂过于靠近障碍物时依然能够找到路径。

在采样中，先采样机械臂的末端位置，将末端位置和方位角约束在允许范围内。再通过解逆运动学获得机械臂六个关节的姿态，并剔除不合法的姿态，完成随机采样中间姿态点集合库的生成。

如所示，机械臂-码垛碰撞检测原理为：

在***搭建阶段，将需要检测的空间分割成栅格的形式，并将每个栅格进行编号索引。在离线的仿真器中，依次执行每一条路径集合库中的路径，记录路径执行过程中与机械臂发生碰撞的栅格。每条路径使用发生碰撞的栅格编码表示。碰撞数据保存在组合逻辑电路中，成为一个碰撞检测单元(Collision Detection Unit，CDU)。

上级***通过识别码垛的状态，将码垛转换成对应的占用栅格，作为静态码垛碰撞检测加速器的障碍物输入。

在实时运行时，将码垛占用栅格编码与所有待测路径的栅格索引并行做逻辑“与”运算。当路径占用栅格和码垛占用栅格有重合时，认为碰撞发生，这条路径在本次运动规划中为不合法路径。由于实时碰撞检测过程是所有路径并行进行的，因此执行速度极快。

在空间中根据坐标划分栅格，每个栅格根据自身的位置按顺序编码进行索引。在离线仿真环境中，通过自动化脚本的控制，机械臂依次执行每条需要编码的路径，并记录碰撞到栅格的索引号。使用发生碰撞的栅格编号记录每一条路径，生成CDU。如：当执行第10条路径会与栅格1、栅格3发生碰撞时，路径10的CDU表示为：

Edge₁₀＝G1丨G3；

码垛占用栅格在线推导方法为：

物流小车上码垛的占用栅格产生于上级***：在清空物流小车上货物之前，堆码的货物是在原有基础上不断增长的。对于放置的物品，其大小和位置均为已知的，因此上级***能够在分拣堆码过程中不断累积获得当前码垛所占用的栅格，并根据当前物流小车的位置生成栅格的索引序列。若当前状态下码垛占用了栅格2、栅格3，则障碍物表示为：

G₁＝0

G₂＝0

obs_t＝G₃＝0

G₄＝0

{...

如图1所示，在进行实时静态码垛碰撞检测时，将上级***输入的障碍物栅格并行输入到1024个CDU中，判断路径是否会与码垛发生碰撞。所有CDU输出为“0”的路径为合法路径，用以参与后续的随机路标搜索。

动态路径碰撞检测原理为：

在一个机械臂(后称规划机械臂)进行运动规划和执行路径时，作为障碍物的另一个机械臂(后称障碍机械臂)是动态运动的，几乎每时每刻它的姿态都在发生变化。由于执行的任务存在差异，两个机械臂不能同时进行运动规划。本***在规划完成后将在规划机械臂的执行路径打上起始时间戳和结束时间戳。在两个机械臂交替变换角色过程中，规划和启动时间更早的机械臂路线将有更高的优先级并成为障碍物，新规划的路线必须主动避免与更早规划的路线发生碰撞。同时，根据时间戳能够精确计算障碍机械臂在某一个时刻的姿态，并用于进行离散姿态的碰撞检测。

以动态连续的机器人路径离散为32个静态的机器人姿态(后称离散静态姿态)进行碰撞检测。以一定的步进值，计算未来若干个时间点规划机械臂和障碍机械臂姿态，将离散静态姿态组合进行静态机械臂-机械臂碰撞检测，并通过增大安全距离的阈值，来补偿离散检测带来的安全性损失。当所有离散姿态均被判断为安全时，则判定该路径是安全的。

为了避免6个关节转动速度差异导致执行路径过程中的姿态与碰撞检测过程中的姿态不一致，约定检测和执行一条路径时，所有关节均同时启动，同时到达目标位置，即全程以匀速完成整条路径。

在离散姿态的碰撞检测中，使用改进的包围盒法，将两个静态的机械臂各连杆视为空间中的线段，通过机械臂的关节状态，求解各连杆在笛卡尔空间中的坐标。归纳碰撞检测的过程为计算空间中两个线段之间的最短距离d，通过判断d是否满足两个连杆半径和安全阈值的约束，从而判断两个机械臂的连杆是否发生碰撞。

当两个机械臂的所有连杆对均没有发生碰撞时，则认为两个机械臂在当前姿态下不会发生碰撞。

动态路径碰撞检测流程算法如下：

定义路径碰撞检测算法为Collideroute(E^B，E^E)。其中规划机械臂路径起点为E^B，规划机械臂路径终点为E^E。

根据EB和EE计算获得32个离散静态姿态及相应的预测时间戳/>根据预测时间戳求得障碍机械臂在相同时刻正在执行路线中的离散静态姿态/>

将32组离散静态姿态进行静态机械臂-机械臂碰撞检测：首先预先求解各个关节的三角函数(sinθ，cosθ)；其次根据正运动学方程计算各个连杆的端点坐标(LD)；最后求得8组连杆的最短距离(d)并判断这组姿态是否发生碰撞(PoseState)。

在获得32组离散姿态的碰撞状态后，判断当前路径是否与障碍机械臂正在执行的路径发生碰撞。最后返回碰撞检测结果(RouteState)。

动态路径碰撞检测加速器由多个基本功能单元构成。部分基本功能单元将使用硬件形式实现于FPGA中，利用硬件的并行性和DSP(Digital Signal Processor)Slice加速关键运算的执行速度，从而加速动态路径碰撞检测。为了便于软件实现与硬件实现交换数据，在设计时，各个基本功能单元主要使用Q格式交换数据。

基本功能单元的设计包括：三角函数解算模块设计，正运动学解算模块设计，连杆最短距离计算模块设计，机械臂-机械臂碰撞判定模块设计，离散静态姿态生成模块设计，动态路径碰撞判定模块设计。

三角函数解算模块：

在正运动学解算之前，输入的各个关节的弧度需要预先转换为对应的三角函数值。求解三角函数较为经济的做法是将正弦表保存于RAM中，使用查表法获得结果。

机械臂的各个关节行程具有上下限，利用三角函数的周期性，奇偶性，以及正弦余弦函数的相位差，消耗很少的RAM即可完成正弦余弦函数的求解。模块设计原理如图2所示，并在后文中用M₁表示。

M₁的硬件加速器在顶层模块状态机的控制下进行工作，需要7个时钟周期完成基本功能：

在第一个状态(S₀)，判断输入的各个关节弧度(θ_i)所在取值区间，通过相位移动将θ_i对准到RAM上对应正弦、余弦(sθ_i、cθ_i)的地址；在之后的S₁，S₂，S₃，S₄，S₅状态，使用流水线方式依次将各个关节对应的正弦、余弦地址传入双端口RAM中，并在S₂，S₃，S₄，S₅，S₆状态读出查表结果；最后根据θ_i所在区间校正结果的正负号。

M₁每次输入1个机械臂的5个关节弧度值，使用16-bit，Q13格式，能够表示-4rad～+3.9999rad之间的弧度，覆盖了机械臂各个关节的行程。M1输出结果为对应关节的正弦和余弦值，使用8-bit，Q7格式，能表示范围从-1～+0.9921875之间的三角函数值。

正运动学解算模块：

正运动学解算能够将机械臂从关节空间映射为笛卡尔空间。通过正运动学计算，得到机械臂5个连杆节点的坐标。其中机械臂A的正运动学解算模块设计原理如图3所示，并在后文中用M₂表示。

如图3所示，圆框表示执行的计算，方框内文字表示相应值的计算结果，s表示正弦，c表示余弦。LD_iX，LD_iY，LD_iZ表示所需要求解的连杆端点的笛卡尔坐标。

M₂的硬件加速器在顶层模块状态机的控制下进行工作，需要4个时钟周期完成基本功能：

其中LD₀，LD₁的笛卡尔坐标为常量，直接产生结果。M₂根据***使用六自由度机械臂的D-H参数，化简计算过程，调整计算顺序，共用部分预先计算结果，充分利用Artix-7系列FPGA的DSP Slice的功能结构，在不影响电路时序的前提下依次求解LD₂，LD₃，LD₄坐标。

M₂输入为1个机械臂求解正运动学所需的10个三角函数值，使用8-bit，Q7格式进行表示。M₂输出为该机械臂4个连杆的5个端点的笛卡尔坐标，以16-bit，Q3格式表示，能够表示-1024mm～+1024mm的笛卡尔坐标。

连杆最短距离计算模块：

连杆最短距离计算模块(M₃)将参与检测的两个连杆视为空间中的两个线段(L_s，L_t)，其中L_s的起点和终点分别为s₀、s₁，L_t的起点和终点分别为t₀、t₁。使用空间解析几何原理，求解出两个线段的最短距离。求解过程如下：

在空间中定义两条直线l_s、l_t分别为线段L_s(s₀-s₁)、L_t(t₀-t₁)所在直线。两条线段的最短距离点分别为s_x、t_x。方向向量分别为：

令直线上最短距离两点为s_j、t_j，他们对应标量为s_c、t_c则有：

令

则有：

当两直线不重合且不平行，所在直线为两条直线的唯一的公垂线，即：

进而可求得:

令

可得：

当ac-d2＝0时，两直线平行，从直线上任取一点作为公垂线即可求得另一条直线上的公垂线垂足。

对于两线段在空间中的位置，最短距离点所在位置需要分9种情况进行讨论：当求得s_c或t_c小于0时，s_j或t_j在线段起点之外，则s₀或t₀为最短距离点；当s_c或t_c大于1时，s_j或t_j在线段终点之外，则s₁或t₁为最短距离点；当s_c或t_c在0和1之间时，两条直线公垂线的垂足s_j、t_j即为两条线段的最短距离点s_x、t_x。

计算模型如图4所示，

两线段最短距离为：d(L_s,L_t)＝|s_x-t_x|。

模块如图5所示，M₃的硬件加速器在顶层模块状态机的控制下进行工作，需要13个时钟周期完成基本功能：

在前6个周期，M₃根据输入两个连杆的坐标，进行向量计算，并初步判断两个连杆最短距离点的位置关系。在之后的3个周期，若最短距离点在线段上，M₃并行进行6位精度的除法运算，计算获得最短距离点在线段的位置。在最后的4个周期，M₃计算出两个最短距离点的具体坐标，并计算得到最短距离的平方用于后续机械臂-机械臂碰撞判定。

M₃输入为2个需要判定连杆的4个端点的笛卡尔坐标，使用16-bit，Q3格式表示。

M₃输出为降低精度后使用18-bit表示的最短距离的平方。

机械臂-机械臂碰撞判定模块：

在获得8组连杆对的最短距离后，根据预先设定的碰撞检测阈值，能够判定连杆是否发生碰撞。模块原理如图6所示，并在后文中用M₄表示。

M₄的硬件加速器在顶层模块状态机的控制下进行工作，需要1个时钟周期完成基本功能：

在接收到8组连杆对的最短距离后，M₄根据连杆的半径及安全阈值，将各个最短距离与相应的阈值比较。将比较结果做逻辑“或”运算。仅当所有连杆对距离均为合法

(输出逻辑0)时，静态机械臂-机械臂碰撞检测才为合法(输出逻辑0)。

M₄输入为8个最短距离的平方，使用18-bit表示。M₄输出为1-bit布尔值表示是否发生碰撞。

离散静态姿态生成模块：

在动态路径碰撞检测过程中，离散静态姿态生成模块需要根据路径起点和路径终点产生32个规划机械臂的离散静态姿态；根据预测的时间戳，推算对应时刻障碍机械臂所处的姿态。最后将产生的离散静态姿态组合进行静态机械臂-机械臂碰撞检测。通过离散静态姿态之间的碰撞检测，间接检测动态路径的安全性。模块原理如图7所示，并在后文中用M₅表示。

M₅的硬件加速器在顶层模块状态机的控制下根据需求使用类似流水线的方式进行工作。M₅每个时钟周期计算产生一个规划机械臂静态离散姿态，并生成对应的预测时间戳。根据时间戳，M₅使用路线的开始时间戳和结束时间戳计算出相应的时间偏置作为查找表的地址，并在下一个周期读出一个障碍机械臂离散静态姿态用于检测。

为了后续检测的进行，在完成运动规划后，M₅需要把规划后最终路线的开始时间戳、结束时间戳以及各个静态离散姿态点写入查找表。

M₅在工作时需要输入规划机械臂的路径起点E^B和路径终点E^E的5个关节弧度值，使用16-bit，Q13格式表示。同时为了预测每个离散静态姿态的时间戳，路径起点的时间戳需要以32-bit格式输入。M₅每次输出2组离散静态姿态，使用16-bit，Q13格式表示。

M₅在初始化状态需要输入不多于700个离散静态姿态，使用16-bit，Q13格式表示。同时为了计算障碍机械臂的查找地址，还需要输入整条规划路线的开始时间戳和结束时间戳，分别使用32-bit表示。

动态路径碰撞判定模块：

在获得32组静态机械臂-机械臂碰撞检测结果之后，如果有任意一组发生碰撞，整个路径将被动态路径碰撞判定模块认为发生了碰撞。模块设计如图8所示，并在后文中用M₆表示。

M₆的硬件加速器在顶层模块状态机的控制下进行工作，需要1个时钟周期完成基本功能：

在32组碰撞检测结果均返回后，M₆对它们进行逻辑“或”运算，仅当所有返回结果均为逻辑0(不碰撞时)，输出结果才为逻辑0(不碰撞)。

M₆输入为32-bit布尔值表示各姿态是否发生碰撞。M₆输出为1-bit布尔值表示该路径是否发生碰撞。

本实施例中对物品分拣机器人***中的六自由度机械臂运动规划算法进行了分析和设计；根据***应用场景，基于随机路标法改进和设计了运动规划算法流程，设计了路径预查找算法、设计了静态码垛碰撞检测加速器和动态路径碰撞检测加速器。

路径预查找算法的设计中，将离线选优并生成集合库，提高了规划效率；将静态码垛的碰撞检测融入到了运动规划的路径预查找中，在完成路径搜索的同时完成静态码垛碰撞检测。

在静态码垛碰撞检测加速器的设计中，采用了改进的空间分割策略：使用栅格编码路径的方式，将大量计算工作离线完成，并保存于CDU中；通过实时检测的并行执行，快速确定所有路径是否合法。

在动态路径碰撞检测加速器的设计中，采用了改进的包围盒法：将动态运动的机械臂根据时间戳求出各个时刻静态姿态；将静态的机械臂连杆模型简化后视为空间中的线段；通过求两条线段在空间中的最短距离来判定两个机械臂是否发生碰撞。动态路径碰撞检测加速器由若干基本功能单元组成，它们包括：三角函数解算模块、正运动学解算模块、连杆最短距离计算模块、机械臂-机械臂碰撞判定模块、离散静态姿态生成模块、动态路径碰撞判定模块。动态路径碰撞检测加速器主要通过硬件的并行性和DSP Slice提供性能加速；在检测路径是否安全时采用“Lazy策略”，仅检测选中的路径是否合法。

在本说明书的描述中，参考术语“一个实施例”、“示例”、“具体示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制。

Claims (5)

1.多自由度机器人静态环境运动规划方法，其特征在于，包括以下步骤：

步骤S01：通过随机采样在机械臂的配置空间获取大量姿态点，为随机中间姿态点；

步骤S02：判断随机中间姿态点是否合法，并保留合法的随机中间姿态点；

步骤S03：遍历所有姿态点，连接满足要求的随机中间姿态点形成路径；

步骤S04：检查所形成的路径，确保不会与静态的环境或机械臂自身发生碰撞，形成合法路径集合；

步骤S05：在未来***的常见工作场景下离线执行运动规划，统计所有路径的使用频率；

步骤S06：从合法路径集合中筛选指定数量的路径用于碰撞栅格的编码，优先选择使用频率高的路径，路径数量由***对成功率的要求决定；

步骤S07：根据坐标系的坐标把空间分割为若干个栅格；

步骤S08：在离线环境中分别执行每条路径，记录该路径执行过程中触碰到的栅格；

步骤S09：构造CDU，根据上述步骤获得的碰撞数据，对每条路径产生真值表；

步骤S10：获取传感器数据，得到障碍物的占用栅格；

步骤S11：将障碍物的栅格作为真值表的输入，得到输出；

步骤S12：使用基于图论的搜索算法，搜索一条从起始姿态点到目标姿态点的线路。

2.根据权利要求1所述的多自由度机器人静态环境运动规划方法，其特征在于，所述S01-S09为***搭建步骤，且在离线状态下完成。

3.根据权利要求2所述的多自由度机器人静态环境运动规划方法，其特征在于，所述步骤S10-S12为***执行步骤，且在在线状态下完成。

4.根据权利要求1所述的多自由度机器人静态环境运动规划方法，其特征在于，所述步骤S07的栅格包括三种类型，分别为：

一、机械臂可以到达，且障碍物会出现的区域；

二、机械臂可以到达，但障碍物不会出现的区域；

三、障碍物会出现，但机械臂无法到达的区域。

5.根据权利要求1所述的多自由度机器人静态环境运动规划方法，其特征在于，所述步骤S11中，输出的真值表的结果为路径与当前障碍物发生了碰撞或路径依旧安全两种情况。