CN111664851B - 基于序列优化的机器人状态规划方法、装置及存储介质 - Google Patents

Abstract

本发明提供了一种基于序列优化的机器人状态规划方法、装置及存储介质，方法包括：根据预设的初始状态重复执行随机序列生成步骤多次，获得随机序列集合；随机序列生成步骤包括：根据初始状态循环执行运动状态生成算法直至到达预设的目标状态或无法生成下一运动状态，获得随机序列；根据预设规则分别对每个随机序列进行评分，根据评分结果在随机序列集合中确定分数最高的随机序列为机器人的运动序列。本申请的技术方案，对运动过程整体进行考虑，规划六足机器人的状态序列，能够避免分周期规划带来的前后状态耦合影响，提高了六足机器人在复杂地形中的通行能力。

Description

基于序列优化的机器人状态规划方法、装置及存储介质

技术领域

本发明涉及机器人控制技术领域，具体而言，涉及一种基于序列优化的机器人状态规划方法、装置及存储介质。

背景技术

由于轮式机器人需要在连续的地面上才能运动，而足式机器人可以在离散的落足点地形中选择落足点进行运动，因此在面对复杂的野外环境时，足式机器人具有更好的通行能力。在众多足式机器人中，六足机器人凭借其强大的环境适应能力和较高的容错性脱颖而出，因其能够在一些危险的环境中，执行各种高难度的任务，而被广泛应用于工业、航天、军事和抢险救灾等领域，具有广阔的发展前景。

足式机器人从一个位置行走到另一个位置时，运动过程中的每一个位置都对应着足式机器人的一个状态，状态包括足式机器人的当前位置信息、位姿和落足点等，运动过程中包括多个离散的状态。现有的状态规划方法，在规划六足机器人的状态时，通常仅规划当前运动周期内六足机器人的下一状态，在六足机器人完成当前运动周期，运动到下一状态后，再继续规划六足机器人的下一运动周期中的目标状态，但是六足机器人的状态规划过程是一个马尔科夫过程，前面的规划结果会对后续的状态规划决策产生一定的影响，前后规划结果的耦合结果会影响六足机器人在复杂地形中的通行能力。

发明内容

本发明解决的问题是如何处理状态间的耦合关系，减少六足机器人在前面运动周期中的运动对后续状态规划的影响，提高六足机器人通过复杂地形的能力。

为解决上述问题，本发明提供一种基于序列优化的机器人状态规划方法、装置及存储介质。

第一方面，本发明提供了一种基于序列优化的机器人状态规划方法，所述方法包括：

根据预设的初始状态重复执行随机序列生成步骤多次，获得包括多个随机序列的随机序列集合；所述随机序列生成步骤包括：根据所述初始状态循环执行运动状态生成算法直至到达预设的目标状态或无法生成下一运动状态，获得包括所有运动状态的所述随机序列，其中所述运动状态生成算法为根据当前运动状态随机生成机器人的下一运动状态。

运动序列确定步骤，根据预设规则分别对每个所述随机序列进行评分，根据评分结果在所述随机序列集合中确定分数最高的所述随机序列为所述机器人的运动序列。

第二方面，本发明提供了一种基于序列优化的机器人状态规划装置，包括：

随机序列生成模块，用于根据预设的初始状态重复执行随机序列生成步骤多次，获得包括多个随机序列的随机序列集合；所述随机序列生成步骤包括：根据所述初始状态循环执行运动状态生成算法直至到达预设的目标状态或无法生成下一运动状态，获得包括所有运动状态的所述随机序列，其中，所述运动状态生成算法为根据当前运动状态随机生成机器人的下一运动状态。

运动序列确定模块，用于根据预设规则分别对每个所述随机序列进行评分，根据评分结果在所述随机序列集合中确定分数最高的所述随机序列为所述机器人的运动序列。

第三方面，本发明提供了一种基于序列优化的机器人状态规划装置，包括存储器和处理器。

所述存储器，用于存储计算机程序。

所述处理器，用于当执行所述计算机程序时，实现如上所述的基于序列优化的机器人状态规划方法。

第四方面，本发明提供了一种计算机可读存储介质，所述存储介质上存储有计算机程序，当所述计算机程序被处理器执行时，实现上所述的基于序列优化的机器人状态规划方法。

第五方面，本发明提供了一种六足机器人，包括机体和如上所述的基于序列优化的机器人状态规划装置，所述机器人状态规划装置的输出端与所述机体的通信接口连接。

本发明的基于序列优化的机器人状态规划方法、装置及存储介质的有益效果是：根据预设的初始状态循环执行运动状态生成算法，不断根据当前运动状态随机生成机器人的下一运动状态直至到达预设的目标状态或无法生成下一运动状态，获得包括生成的所有运动状态的随机序列，重复该过程多次，就可获得多条随机序列，初始状态为六足机器人在初始位置处的运动状态，目标状态为六足机器人在目标位置处的运动状态，将从初始状态到目标状态的整个过程结合起来生成随机序列，能够减少前面的状态规划结果对后续状态规划的影响。分别对每条随机序列进行评分，再在所有的随机序列中确定出最优序列，该最优序列就是六足机器人的运动序列。本申请的技术方案中，对运动过程整体进行考虑，来生成运动序列，能够避免分周期规划带来的前后耦合影响，减少了六足机器人在前面运动周期中的运动对后续运动的影响，提高了六足机器人在复杂地形中的通行能力。

附图说明

图1为本发明实施例的一种基于序列优化的机器人状态规划方法的流程示意图；

图2为本发明实施例的随机选取落足点的过程示意图；

图3为本发明实施例的一种图结构的示意图；

图4为本发明实施例的运动状态之间单向可达判断过程示意图；

图5为本发明实施例的一种随机序列尾运动状态示意图；

图6为本发明另一实施例的一种随机序列尾运动状态示意图；

图7为本发明实施例的一种第二待处理序列示意图；

图8为本发明实施例的第二待处理序列的截取过程示意图；

图9为本发明实施例的分段序列示意图；

图10为本发明实施例的一种窗口优化序列示意图；

图11为本发明实施例的一种运动优化序列示意图；

图12为本发明实施例的一种机体重心轨迹示意图；

图13为本发明实施例的一种摆动腿的足端轨迹示意图；

图14为本发明实施例的一种六足机器人的腿部支撑状态相序图；

图15为本发明实施例的一种六足机器人通过落足点区域的仿真示意图；

图16为在图15中的部分状态下六足机器人的俯视示意图；

图17为本发明实施例的一种基于序列优化的机器人状态规划装置的结构示意图。

具体实施方式

为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。本申请的实施例中以六足机器人为例详细说明基于序列优化的机器人状态规划方法。

如图1所示，本发明实施例提供的一种基于序列优化的机器人状态规划方法，所述方法包括：

步骤100，根据预设的初始状态重复执行随机序列生成步骤多次，获得包括多个随机序列的随机序列集合；所述随机序列生成步骤包括：根据所述初始状态循环执行运动状态生成算法直至到达预设的目标状态或无法生成下一运动状态，获得包括所有运动状态的所述随机序列，其中，所述运动状态生成算法为根据当前运动状态随机生成机器人的下一运动状态。

具体地，在已知当前运动状态的情况下，随机确定六足机器人的下一运动状态，从预设的初始状态开始，逐步向前确定六足机器人的下一运动状态，不断逼近预设的目标状态。

步骤200，运动序列确定步骤，包括根据预设规则分别对每个所述随机序列进行评分，根据评分结果在所述随机序列集合中确定分数最高的所述随机序列为所述机器人的运动序列。

需要说明的是，运动序列中的所有运动状态为离散的状态。

本实施例中，根据预设的初始状态循环执行运动状态生成算法，不断根据当前运动状态随机生成机器人的下一运动状态直至到达预设的目标状态或无法生成下一运动状态，获得包括生成的所有运动状态的随机序列，重复该过程多次，就可获得多条随机序列，初始状态为六足机器人在初始位置处的运动状态，目标状态为六足机器人在目标位置处的运动状态，将从初始状态到目标状态的整个过程结合起来生成随机序列，能够减少前面的状态规划结果对后续状态规划的影响。分别对每条随机序列进行评分，再在所有的随机序列中确定出最优序列，该最优序列就是六足机器人的运动序列。本申请的技术方案中，对运动过程整体进行考虑，来生成运动序列，能够避免分周期规划带来的前后耦合影响，减少了六足机器人在前面的运动周期中的运动对后续运动的影响，提高了六足机器人在复杂地形中的通行能力。

优选地，在已知当前运动状态的情况下，随机确定六足机器人的下一运动状态，下一运动状态包括确定六足机器人的下一支撑状态、下一前进方向、下一前进步长和下一落足点等。

确定下一支撑状态：选择六足机器人的除故障腿以外的腿为预选支撑腿，故障腿可包括当前运动状态下没有落足点的腿和具有故障的腿，根据预选支撑腿在预设的支撑状态表确定能够使六足机器人保持静态稳定的支撑状态集合，令支撑状态集合为S，集合S包括多个支撑状态，在集合S中随机选择六足机器人的下一支撑状态s，s∈S。

确定下一前进方向：以当前位置到目标位置的方向向量为基准，在该基准的标定方向范围内随机选取下一前进方向

确定下一前进步长：在下一支撑状态s下，根据六足机器人的物理参数确定在满足最小稳定裕度的情况下六足机器人在下一前进方向

上的最大移动步长

在

的范围内随机选取下一前进步长St。

确定下一落足点：根据当前运动状态对应的机体当前位置在下一前进方向

上移动下一前进步长St，根据六足机器人的腿部物理参数确定摆动腿的落足区域，结合六足机器人的视觉装置获取的落足点信息，确定落足区域的所有落足点，在所有落足点中随机选取摆动腿的目标落足点。如图2所示，靠左的虚线框为六足机器人机体重心移动前任一摆动腿的落足区域，K₁对应的折线段为该摆动腿摆动前的位姿，p_i为此时该摆动腿的足端位置；靠右的实线框为机体重心移动到机体重心下一位置时该摆动腿的落足区域，虚线框和实线框中的圆圈表示落足点，K₂对应的折线段为该摆动腿摆动后的位姿，p_e为此时该摆动腿的足端位置，即下一落足点，连接p_i和p_e的虚线即为该摆动腿的足端轨迹。箭头表示六足机器人的下一前进方向。

从初始状态开始，不断重复上述下一运动状态生成方法，直到到达目标状态或者连续生成的多个运动状态均显示六足机器人无法继续前进，获得一条随机序列seq，无法继续前进包括六足机器人的摆动腿没有落足点等情况，重复上述过程获得多条随机序列seq。并且，可采用单向可达性简化算法对随机序列seq的运动状态数量进行简化。单向可达性简化算法参见后文中对窗口优化序列进行简化部分的说明。

六足机器人在每个支撑状态下均能保持静态稳定，保持静态稳定的前提是在运动过程中支撑腿的数量大于或等于三，且机体重心在支撑多边形内，且能够满足标定的稳定裕度。具体而言，支撑多边形为支撑腿足端连线所组成的多边形，稳定裕度表示机体重心在支撑多边形的投影与支撑多边形任意一条边的距离，六足机器人要保持静态稳定需要稳定裕度大于或等于预设的阈值，该阈值的典型取值为0.1m。若依次对六足机器人的六条腿进行排序，支撑腿用1表示，摆动腿用0表示，则包含了所有可能的支撑状态的支撑状态表如表一所示：

表一支撑状态表

表一中支撑状态显示了该状态下的各条腿的状态，支撑状态示意图中的多边形即为支撑多边形，圆点即为机体重心在支撑多边形中的投影。

本优选的实施例中，可根据具体地形的需要自由选择支撑状态来确定步态，提取运动序列中各个运动状态下的支撑状态信息，就可确定出六足机器人在整个运动过程中步态，即六足机器人在整个运动过程中腿部之间的相位关系，相对于现有技术中采用的周期性的三足步态、四足步态或五足步态，能够充分发挥六足机器人的多足优势，大幅提高六足机器人应对复杂地形的能力。并且上述随机序列生成方法中没有倾向性策略的介入，对计算资源的消耗相对较小，能够在短时间内快速生成随机序列seq。

优选地，每条所述随机序列均对应有任务完成度，每个所述运动状态均对应有移动步长；所述步骤200包括：

对于所述随机序列集合中的每一个所述随机序列，根据所述随机序列对应的所有所述移动步长在所述随机序列集合中确定分数最高的所述随机序列为所述随机序列的平均步长。

具体地，计算所有移动步长的平均值，该平均值即为对应的随机序列的平均步长。

根据所述平均步长和对应的所述任务完成度分别对各条所述随机序列进行评分，根据评分结果确定所述运动序列。

具体地，将随机序列集合用Se表示，集合Se中可能既包括了到达目标位置的随机序列seq，也包括未到达目标位置的随机序列seq。

优选地，所述根据所述平均步长和对应的所述任务完成度分别对各条所述随机序列进行评分，根据评分结果在所述随机序列集合中确定分数最高的所述随机序列为所述运动序列包括：

采用第一公式确定各条所述随机序列的分数，所述第一公式为：

grade(seq)＝ω₁·AvgL_seq+ω₂·MC_seq，

其中，seq为任一所述随机序列，AvgL_seq为所述随机序列对应的所述平均步长，MC_seq为所述随机序列对应的所述任务完成度，取值范围为[0,1]，ω₁为平均步长的权重，ω₂为任务完成度的权重。

基于各条所述随机序列的分数，采用第二公式确定所述运动序列，所述第二公式为：

其中，seq₀为所述运动序列，Se为所述随机序列集合。

具体地，可根据具体情况自由选择ω₁和ω₂的大小，若ω₁的取值更大，则表示确定运动序列seq₀时，更倾向于选择平均步长更长的随机序列seq，六足机器人采用该运动序列seq₀时移动速度更快；若ω₂的取值更大，则表示确定运动序列seq₀时，更倾向于选择任务完成度更高的随机序列seq，六足机器人采用该运动序列seq₀时通行能力更强。

优选地，在所述步骤200之后，还包括序列优化步骤，所述序列优化步骤包括：

根据所有所述随机序列的所有所述运动状态构成图结构。

确定所述运动序列的尾运动状态为终止状态，并根据所述图结构生成从所述初始状态到所述终止状态的图优化运动序列，所述图优化运动序列为从所述初始状态到所述终止状态时遍历所述运动状态数量最少的序列。

具体地，如图3所示，图中每个黑点表示一个运动状态，每一行表示一条随机序列seq，图中显示了集合Se中所有随机序列seq的所有运动状态。需要说明的是，图中第一列的所有运动状态本质上都是初始状态，但是在图结构中对它们分别进行编号，它们之间互相可达。采用单向可达性判断算法将所有的运动状态连接成有向图。假设第八行代表运动序列seq₀，以运动序列seq₀的尾运动状态为终止状态，记为s_end，尾运动状态是对应序列中的最后一个运动状态，令第一列的所有初始状态组成的集合为S_begin。采用Bellman-Ford(贝尔曼-福特)算法在有向图中找到从初始状态s_begin到终止状态s_end的最短路径，即生成一条从初始状态s_begin到终止状态s_end过程中遍历运动状态数量最少的序列，该序列即为图优化运动序列，将该序列记为seq₁，其中s_begin∈S_begin。采用下式确定最终的初始状态：

其中，s_b0为最终的初始状态，BF(s_begin,s_end)为图优化运动序列seq₁包括的运动状态数量。图优化运动序列seq₁为一条以s_b0为起始状态，s_end为终止状态，通过Bellman-Ford最短路径算法找到的序列。

如图4所示，令两个运动状态分别为State₁和State₂，State₁对应的六足机器人的支撑多边形为图中多边形ABFDG,State₂对应的六足机器人的支撑多边形为图中多边形ABCDE，分别与多边形ABFDG和多边形ABCDE对应的虚线多边形均是为了满足稳定裕度留出余量后得到的多边形。P_c为六足机器人的机体重心在多边形ABFDG中的投影，P_cr为六足机器人的机体重心在多边形ABCDE中的投影。点A、B、D为两个支撑多边形的公共顶点。三角形ABD是公共顶点组成的多边形。Length表示六足机器人从State₁转至State₂时，机体重心的投影移动的距离。

采用单向可达性判断算法来判断两个运动状态是否单向可达，就是看运动状态等是否满足下面三个条件。

条件一：两个运动状态分别对应的支撑多边形的公共顶点大于或等于3，且两个机体重心投影P_c和P_cr均在公共顶点组成的多边形ABD内。

条件二：P_c在与多边形ABFDG对应的虚线多边形内，P_cr在与多边形ABCDE对应的虚线多边形内。

条件三：Length小于在运动状态State₁下六足机器人的物理参数所允许的机体重心最大移动步长，该最大移动步长至少满足最小稳定裕度的要求。

当同时满足上述三个条件时，即可判断从运动状态State₁到运动状态State₂单向可达。

本优选的实施例中，充分利用随机生成所有序列的所有运动状态，对运动序列seq₀进行图优化，能够简化运动序列seq₀的运动状态数量，有利于提高规划状态时的速度，并提高六足机器人的通行速度。

优选地，在根据所述图结构生成从所述初始状态到所述终止状态的图优化运动序列的步骤之后，所述序列优化步骤还包括：

步骤210，令所述图优化运动序列为第一待处理序列，将所述第一待处理序列的前k个所述运动状态组成的序列作为第一结果序列的一个子序列，并令所述第一待处理序列的第k+1个所述运动状态为所述初始状态，重复执行所述随机序列生成步骤多次，并再次执行所述运动序列确定步骤，或重复执行所述随机序列生成步骤多次，并依次执行所述运动序列确定步骤和所述序列优化步骤，获得图优化换位序列，其中，k大于零且小于N，N为所述图优化运动序列包括的所述运动状态的数量。

具体地，图优化运动序列seq₁有两种可能性：一种是图优化运动序列seq₁的终止状态到达了目标状态，能够使六足机器人完成通行任务；另一种是图优化运动序列seq₁的终止状态没有到达目标状态，六足机器人按照该图优化运动序列seq₁运动时无法到达目标位置，无法完成通行任务。

因此，先确定图优化运动序列seq₁的尾运动状态是否与目标状态相同，若相同，则该图优化运动序列seq₁对应第一种可能性，但是若没经过换位迭代优化就到达了目标状态，本实施例中认为没有对运动状态进行充分采样，此时k可为该图优化运动序列seq₁包括的运动状态数量的一半。

若不同，则该图优化运动序列seq₁对应第二种可能性，没有到达目标状态，k可为图优化运动序列seq₁的运动状态数量的80％。

将第k+1个运动状态当作初始状态，结合预设的目标状态返回步骤100，依次执行各步骤，就可得到从第k+1个运动状态到目标状态的图优化运动序列，即图优化换位序列。

步骤211，确定所述图优化换位序列的尾运动状态是否与所述目标状态相同，若相同，则转至步骤212；若不同，则将所述图优化换位序列作为所述第一待处理序列，返回步骤210。

步骤212，将所述图优化换位序列作为所述第一结果序列的另一个子序列，依次拼接所有的所述子序列，获得所述第一结果序列。

具体地，同样确定图优化换位序列的尾运动状态是否与目标状态相同，若不同，则返回步骤210，将图优化换位序列的前k个运动状态作为一个新的第一结果序列的子序列，将第k+1个运动状态当作初始状态，结合预设的目标状态返回步骤100，迭代多次直至到达目标状态或达到预设迭代次数。

若相同，此时已经过至少一轮换位迭代优化，因此将该图优化换位序列也作为第一结果序列的子序列，令第一结果序列为seq_out1，子序列根据获得的时间依次排序，依次拼接所有的子序列，就可得到第一结果序列seq_out1。

本优选的实施例中，对图优化运动序列进行多次换位迭代优化，使得到的第一结果序列的尾运动状态到达目标状态或尽可能逼近目标状态，可以提高第一结果序列的任务完成度，进而提高六足机器人在复杂地形中的通行能力。

优选地，令每个所述随机序列的尾运动状态对应的所述机器人的机体位置为尾状态位置；所述步骤100之后，所述序列优化步骤还包括:

步骤220，确定每个所述随机序列对应的所述尾状态位置是否均在预设范围内，若否，则将所述第一结果序列作为结果输出序列；若是，则转至步骤221。

具体地，运动状态与机器人的机体位置一一对应，如图5和图6所示，图中Begin表示初始状态对应的机器人机体的初始位置，End表示目标状态对应的机器人机体的目标位置，虚线圆圈为预设范围，直径d可为0.1m，虚线圆圈中的实线圆圈为各条随机序列seq对应的尾状态位置。如图5所示，若所有的尾状态位置均在虚线圆圈中，则表示六足机器人会出现受限情况。如图6所示，若所有的尾运动状态不在虚线圆圈中，则表示六足机器人不会出现受限情况。

步骤221，根据所有的所述尾状态位置确定受限位置，并根据所述受限位置确定脱陷位置。

具体地，根据所有尾状态位置的坐标确定平均值，令该平均值对应的位置受限位置，记为p_stuck，令目标位置为p_end，采用下式确定脱陷位置：

其中，p_out为脱陷位置，par₁为脱陷参数，一般设为0，par₂为地形参数，可根据地形的实际范围进行设置。

步骤222，令所述脱陷位置对应的所述运动状态为所述初始状态，重复执行所述随机序列生成步骤多次，生成多个脱陷随机序列。

具体地，脱陷成功后，将六足机器人在脱陷位置的运动状态作为初始状态，结合预设的目标状态，再次执行步骤100，就可得到从脱陷位置到目标位置的多个随机序列，即脱陷随机序列。

步骤232，确定每个所述脱陷随机序列对应的所述尾状态位置是否均在所述预设范围内，若否，则转至步骤233；若是，则改变所述受限位置，返回步骤221。

具体地，判断六足机器人是否脱陷成功，若所有脱陷随机序列的所有尾状态位置仍在预设范围内，即仍在图5中虚线圆圈中，认为六足机器人脱陷失败，则修改p_stuck、par₁和par₂的值，返回步骤221重新确定脱陷位置，重复脱陷过程。本实施例中可将p_stuck向初始位置收敛，par₁和par₂的值保持不变。若重复脱陷过程多次仍无法脱陷，则直接将第一结果序列作为输出结果序列并输出。

步骤233，确定从所述受限位置到所述脱陷位置的脱陷运动序列，并根据多个所述脱陷随机序列执行所述运动序列确定步骤，或依次执行所述运动序列确定步骤和所述序列优化步骤，获得第二结果序列。

具体地，脱陷成功后，记录六足机器人从受限位置到脱陷位置过程中的运动序列，即脱陷运动序列，并根据获得的多个脱陷随机序列转至步骤200，依次执行各步骤，重复运动序列确定过程和序列优化过程，获得第二结果序列，令第二结果序列为seq_out2。

步骤234，依次拼接所述第一结果序列、所述脱陷运动序列和所述第二结果序列，获得所述结果输出序列，令结果输出序列为seq_out。

本优选的实施例中，当六足机器人被困在稀疏落足点等复杂地形中无法继续前进时，通过上述脱陷步骤重新规划运动序列，尽可能避免陷入受限的境地，能够提高六足机器人在复杂地形中的通行能力。

优选地，在所述步骤234之后，所述序列优化步骤还包括：

步骤240，将所述结果输出序列作为第二待处理序列，采用预设窗宽的窗口依次截取所述第二待处理序列，获得多段依次排列的分段序列。

具体地，窗口优化的循环过程中，每次循环可采用的同一窗宽的窗口，也可采用不同窗宽的窗口。如图7所示为第二待处理序列，图中黑点为运动序列。如图8所示，采用窗口依次截取第二待处理序列，得到如图9所示的多条分段序列，令分段序列为s_duan，所有的分段序列组成了一个集合S_duan，s_duan∈S_duan。

步骤241，对于每一段所述分段序列，将所述分段序列的首运动状态作为所述初始状态，所述分段序列的尾运动状态作为所述目标状态，重复执行所述随机序列生成步骤多次，并再次执行所述运动序列确定步骤，或重复执行所述随机序列生成步骤多次，并依次执行所述运动序列确定步骤和所述序列优化步骤，获得每个所述分段序列的分段优化序列。

具体地，再次从步骤100开始依次执行各步骤，重复上述的运动序列确定过程和序列优化过程，对每个分段序列s_duan均进行优化。

步骤242，依次拼接所有的所述分段优化序列，获得所述窗口优化初始序列，将所述窗口优化初始序列作为所述第二待处理序列，返回步骤240，经过多次迭代，获得窗口优化序列。

具体地，根据窗口截取的顺序依次拼接所有的分段优化序列，就可得到窗口优化初始序列，将窗口优化初始序列作为第二待处理序列，返回步骤240，重复分段优化过程，进行迭代。本实施例中，每次迭代过程均采用不同窗宽的窗口。

令结果输出序列seq_out的运动状态个数为num_seq，则第一迭代过程中窗口的窗宽初始值可为num_seq/W₀，其中W₀为初始参数，可为6。图中窗口的窗口为5。然后，迭代过程中首先每次将窗宽提高为前一次窗宽的β₁倍，β₁可为2，直至窗宽大于β₁的运动状态个数，然后每次将窗宽缩小为前一次窗宽的1/β₂，直至窗宽小于运动状态个数，结束迭代过程，β₂可为3β₁。

本优选的实施例中，采用分治、融合的优化过程，通过分段优化可以减少序列中的运动状态数量，有利于提高后续状态规划速度，并提高六足机器人的通行速度。

优选地，在所述获得窗口优化序列的步骤之后，所述序列优化步骤还包括：

采用单向可达性简化算法对所述窗口优化序列进行简化，获得运动优化序列。

具体地，单向可达性简化算法就是根据单向可达性判断算法对序列进行简化。对于一个序列，找到序列中的单向可达的两个运动状态，两个运动状态之间相隔越远越好，删掉单向可达的两个运动状态之间的所有运动状态，从而减少运动状态的数量，简化序列。需要说明的是，在删除两个运动状态的其它运动状态后，需要对应修改两个运动状态的状态信息，例如需要修改前一运动状态的支撑状态、前进方向和前进步长等，以保证可以规划两个运动状态之间的运动轨迹。

如图10所示为单向可达性简化前的窗口优化序列，每个多边形对应一个运动状态，采用单向可达性简化算法简化后，得到如图11所示的单向可达性简化后的运动优化序列。可以看出，经过单向可达性简化算法简化后，序列中的运动状态大幅度减少。

本优选的实施中，通过单向可达性简化算法可以减少运动状态的数量，能够提高后续状态规划的速度，并且减少六足机器人的运动步骤，提高六足机器人的通行速度。

优选地，在所述获得运动优化序列的步骤之后，还包括：

基于多项式拟合的方法，根据所述运动优化序列的所有所述运动状态分别确定所述机器人的机体重心轨迹和摆动腿足端轨迹。

具体地，也可以在获得运动序列之后，就根据运动序列的所有运动状态来规划机体重心轨迹和摆动腿足端轨迹。本实施例中根据经过序列优化步骤后得到的运动优化序列规划六足机器人的运动轨迹，根据运动优化序列中每相邻的两个运动状态确定六足机器人在相邻的运动状态之间的分段运动轨迹，拼接所有的分段运动轨迹，就可得到六足机器人的从初始位置到目标位置的运动轨迹。控制六足机器人根据该运动轨迹进行运动，就可运动至目标位置。

一方面，如图12所示，图中X_WY_WZ_W为六足机器人的机体坐标系。对于任意两个相邻的运动状态，令前一运动状态对应的机体重心当前位置为^Wp_b1，该前一运动状态下，机体重心的速度为^Wv_b1、加速度为^Wa_b1和对应时间为t₁；令后一运动状态对应的机体重心下一位置为^Wp_b3，该后一运动状态下，机体重心的速度为^Wv_b3、加速度为^Wa_b3、对应时间为t₃。规划两个相邻运动状态之间的机体重心轨迹时的已知量如表二所示：

表二机体重心轨迹规划已知量

根据机体重心的位置和对应的时间建立机体重心轨迹的位置多项式，机体重心轨迹的位置多项式如下式所示：

^Wp_b＝A_b0+A_b1t+A_b2t²+A_b3t³+A_b4t⁴+A_b5t⁵，

其中，^Wp_b取^Wp_b1或^Wp_b3，^Wp_b1对应的t为t₁，^Wp_b3对应的t为t₃。

根据机体重心的速度和对应的时间建立机体重心轨迹的速度多项式，机体重心轨迹的速度多项式如下式所示：

^Wv_b＝A_b1+2A_b2t+3A_b3t²+4A_b4t³+5A_b5t⁴，

其中，^Wv_b取^Wv_b1或^Wv_b3，^Wv_b1对应的t为t₁，^Wv_b3对应的t为t₃。

根据机体重心的加速度和对应的时间建立机体重心轨迹的加速度多项式，机体重心轨迹的加速度多项式如下式所示：

^Wa_b＝2A_b2+6A_b3t+12A_b4t²+20A_b5t³，

其中，^Wa_b取^Wa_b1或^Wa_b3，^Wa_b1对应的t为t₁，^Wa_b3对应的t为t₃。

将各参数代入对应的多项式中，t₁可设为0，可得到如下的公式：

根据已知的^Wp_b1、^Wp_b3、^Wv_b1、^Wv_b3、^Wa_b1、^Wa_b3和t₃就可以得到多项式系数[A_b0,A_b1,A_b2,A_b3,A_b4,A_b5]^T，其中，^Wv_b1、^Wv_b3、^Wa_b1和^Wa_b3可为0，t₃可根据情况进行具体设置。

本优选的实施例中，在确定出多项式系数后，通过设定时间就可以得到机体重心从机体重心当前位置运动到机体重心下一位置过程中的任意位置，以及该位置对应的机体速度和加速度，确定机体重心轨迹。轨迹规划过程平滑，并且能够满足六足机器人的稳定性要求和能量消耗的约束。

另一方面，如图13所示，图中X_WY_WZ_W为六足机器人的机体坐标系。对于任意两个相邻的运动状态，令前一运动状态对应的摆动腿的足端当前位置为^Wp_r1，该前一运动状态下，摆动腿的足端在足端当前位置时的速度为^Wv_r1、加速度为^Wa_r1和对应时间为t₄；令后一运动状态对应的摆动腿的足端落足位置为^Wp_r3，该后一运动状态下，摆动腿的足端在足端落足位置时的速度为^Wv_r3、加速度为^Wa_r3和对应时间为t₆；并令摆动腿的足端在运动过程中的中间点位置为^Wp_r2，对应时间为t₅。规划两个相邻运动状态之间的足端轨迹时的已知量如表三所示：

表三足端轨迹规划已知量

根据摆动腿的足端位置和对应的时间建立摆动腿的足端位置多项式，足端位置多项式如下式所示：

^Wp_r＝A_r0+A_r1t+A_r2t²+A_r3t³+A_r4t⁴+A_r5t⁵，

其中，^Wp_r取^Wp_r1或^Wp_r2或^Wp_r3，^Wp_r对应的t为t₄，^Wp_r2对应的t为t₅，^Wp_r3对应的t为t₆。

足端位置可采用对应位置的坐标，本实施例中^Wp_r2的Z轴坐标为图6中的h。

根据摆动腿的足端运动时的速度和对应的时间建立摆动腿的足端速度多项式，足端速度多项式如下式所示：

^Wv_r＝A_r1+2A_r2t+3A_r3t²+4A_r4t³+5A_r5t⁴+6A_r6t⁵，

其中，^Wv_r取^Wv_r1或^Wv_r3，^Wv_r1对应的t为t₄，^Wv_r3对应的t为t₆。

根据摆动腿的足端运动时的加速度和对应的时间建立摆动腿的足端加速度多项式，足端加速度多项式如下式所示：

^Wa_r＝2A_r2+6A_r3t+12A_r4t²+20A_r5t³+30A_r6t⁴，

其中，^Wa_r取^Wa_r1或^Wa_r3，^Wa_r1对应的t为t₄，^Wa_r3对应的t为t₆。

将参数代入对应的多项式中，可将t₄设为0，得到如下的公式：

根据已知的^Wp_r1、^Wp_r2、^Wp_r3、^Wv_r1、^Wv_r3、^Wa_r1、^Wa_r3、t₅和t₆就可以得到多项式系数[A_r0,A_r1,A_r2,A_r3,A_r4,A_r5,A_r6]^T，其中，^Wv_r1、^Wv_r3、^Wa_r1和^Wa_r3可为0，t₅和t₆可根据情况进行具体设置。

本优选的实施例中，在确定出多项式系数后，通过设定时间就可以得到摆动腿的足端从足端当前位置运动到目标落足点过程中的任意位置，以及该位置对应的足端的速度和加速度，确定出足端轨迹。通过多项式拟合的方法规划足端轨迹，不仅能够保证位置轨迹曲线平滑连续，还能保证速度曲线和加速度曲线平滑连续，使得电机在控制摆动腿运动时不会发生抖动突变，并且能够节省能量。

下面以六足机器人通过落足点密度为4.8/m²的地形为例，对本发明实施例的一种基于序列优化的机器人状态规划方法做进一步的说明。

如图14所示的六足机器人的腿部支撑状态相序图，通过提取运动序列中各个运动状态下的支撑状态信息，就可确定出六足机器人在整个运动过程中步态，得到如图14所示的六足机器人在整个运动过程中腿部之间的相位关系，将六足机器人的六条腿依次命名为Leg1、Leg2、Leg3、Leg4、Leg5和Leg6，图中黑色部分表示对应的腿用作支撑腿，代表支撑相，白色部分表示对应的腿用作摆动腿，代表摆动相。在六足机器人的第一个运动周期中，Leg1、Leg2、Leg4、Leg5和Leg6用作支撑腿，Leg3用作摆动腿；在第二个运动周期中，Leg2、Leg3、Leg4和Leg6用作支撑腿，Leg1和Leg5用作摆动腿，以此类推。可知，采用本发明实施例的基于序列优化的机器人状态规划方法来规划状态时，每个运动状态下支撑状态是自由选取的，六足机器人在三足步态、四足步态和五足步态之间自由切换，能够根据不同的地形选择合适的步态模式，提高了在各种复杂地形中的通行能力。

如图15所示的六足机器人通过落足点密度为4.8/m²的地形时的仿真示意图，六足机器人实现了从坐标点[0,0]移动到坐标点[8.4,0]的任务。图15中从左到右，从上到下依次对应了六足机器人在运动过程中的12个运动状态，图中黑点为落足点，白色区域为无落足点区域，通过仿真结果可知，采用本申请的基于序列优化的机器人状态规划方法能够帮助六足机器人顺利通过稀疏落足点区域。图16显示了与图10中前8个运动状态分别对应的六足机器人的俯视示意图。

如图17所示，本发明实施例提供的一种基于序列优化的机器人状态规划装置，包括：

随机序列生成模块，用于根据预设的初始状态重复执行随机序列生成步骤多次，获得包括多个随机序列的随机序列集合；所述随机序列生成步骤包括：根据所述初始状态循环执行运动状态生成算法直至到达预设的目标状态或无法生成下一运动状态，获得包括所有运动状态的所述随机序列，其中，所述运动状态生成算法为根据当前运动状态随机生成机器人的下一运动状态。

运动序列确定模块，用于根据预设规则分别对每个所述随机序列进行评分，根据评分结果在所述随机序列集合中确定分数最高的所述随机序列为所述机器人的运动序列。

本发明另一实施例提供的一种基于序列优化的机器人状态规划装置包括存储器和处理器；所述存储器，用于存储计算机程序；所述处理器，用于当执行所述计算机程序时，实现如上所述的基于序列优化的机器人状态规划方法。该装置可为工控机和服务器等。

本发明再一实施例提供的一种计算机可读存储介质上存储有计算机程序，当所述计算机程序被处理器执行时，实现如上所述的基于序列优化的机器人状态规划方法。

本发明再一实施例提供的一种六足机器人，包括机体和如上所述的基于序列优化的机器人状态规划装置，所述机器人状态规划装置的输出端与所述机体的通信接口连接。所述机器人状态规划装置可设置在机体上。

本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)或随机存储记忆体(Random AccessMemory，RAM)等。在本申请中，所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本发明实施例方案的目的。另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以是两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

虽然本发明公开披露如上，但本发明公开的保护范围并非仅限于此。本领域技术人员在不脱离本发明公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。

Claims (11)

1.一种基于序列优化的机器人状态规划方法，其特征在于，包括：

根据预设的初始状态重复执行随机序列生成步骤多次，获得包括多个随机序列的随机序列集合；所述随机序列生成步骤包括：根据所述初始状态循环执行运动状态生成算法直至到达预设的目标状态或无法生成下一运动状态，获得包括所有运动状态的所述随机序列，其中，所述运动状态生成算法为根据当前运动状态随机生成机器人的下一运动状态；

运动序列确定步骤，根据预设规则分别对每个所述随机序列进行评分，根据评分结果在所述随机序列集合中确定分数最高的所述随机序列为所述机器人的运动序列；

其中，每条所述随机序列均对应有任务完成度，每个所述运动状态均对应有移动步长，对于所述随机序列集合中的每一个所述随机序列，根据所述随机序列对应的所有所述移动步长确定所述随机序列的平均步长，采用预设的权重对所述平均步长和所述任务完成度进行加权求和，获得每条所述随机序列的评分结果；

在所述根据评分结果在所述随机序列集合中确定分数最高的所述随机序列为所述机器人的运动序列之后，还包括序列优化步骤，所述序列优化步骤包括：

根据所有所述随机序列的所有所述运动状态构成图结构；

确定所述运动序列的尾运动状态为终止状态，并根据所述图结构生成从所述初始状态到所述终止状态的图优化运动序列，所述图优化运动序列为从所述初始状态到所述终止状态时遍历所述运动状态数量最少的序列。

2.根据权利要求1所述的基于序列优化的机器人状态规划方法，其特征在于，所述根据预设规则分别对每个所述随机序列进行评分，根据评分结果在所述随机序列集合中确定分数最高的所述随机序列为所述机器人的运动序列包括：

采用第一公式确定各条所述随机序列的分数，所述第一公式为：

grade(seq)＝ω₁·AvgL_seq+ω₂·MC_seq，

其中，seq为所述随机序列集合中任一所述随机序列，AvgL_seq为所述随机序列对应的所述平均步长，MC_seq为所述随机序列对应的所述任务完成度，ω₁为平均步长的权重，ω₂为任务完成度的权重；

基于各条所述随机序列的分数，采用第二公式确定所述运动序列，所述第二公式为：

其中，seq₀为所述运动序列，Se为所述随机序列集合。

3.根据权利要求1或2所述的基于序列优化的机器人状态规划方法，其特征在于，在根据所述图结构生成从所述初始状态到所述终止状态的图优化运动序列的步骤之后，所述序列优化步骤还包括：

步骤210，令所述图优化运动序列为第一待处理序列，将所述第一待处理序列的前k个所述运动状态组成的序列作为第一结果序列的一个子序列，并令所述第一待处理序列的第k+1个所述运动状态为所述初始状态，重复执行所述随机序列生成步骤多次，并再次执行所述运动序列确定步骤，或重复执行所述随机序列生成步骤多次，并依次执行所述运动序列确定步骤和所述序列优化步骤，获得图优化换位序列，其中，k大于零且小于N，N为所述图优化运动序列包括的所述运动状态的数量；

步骤211，确定所述图优化换位序列的尾运动状态是否与所述目标状态相同，若相同，则转至步骤212；若不同，则将所述图优化换位序列作为所述第一待处理序列，返回步骤210；

步骤212，将所述图优化换位序列作为所述第一结果序列的另一个子序列，依次拼接所有的所述子序列，获得所述第一结果序列。

4.根据权利要求3所述的基于序列优化的机器人状态规划方法，其特征在于，令每个所述随机序列的尾运动状态对应的所述机器人的机体位置为尾状态位置；在重复所述随机序列生成步骤多次，获得包括多个所述随机序列的随机序列集合之后，所述序列优化步骤还包括:

步骤220，确定每个所述随机序列对应的所述尾状态位置是否均在预设范围内，若否，则将所述第一结果序列作为结果输出序列；若是，则转至步骤221；

步骤221，根据所有的所述尾状态位置确定受限位置，并根据所述受限位置确定脱陷位置；

步骤222，令所述脱陷位置对应的所述运动状态为所述初始状态，重复执行所述随机序列生成步骤多次，生成多个脱陷随机序列；

步骤232，确定每个所述脱陷随机序列对应的所述尾状态位置是否均在所述预设范围内，若否，则转至步骤233；若是，则改变所述受限位置，返回步骤221；

步骤233，确定从所述受限位置到所述脱陷位置的脱陷运动序列，并根据多个所述脱陷随机序列执行所述运动序列确定步骤，或依次执行所述运动序列确定步骤和所述序列优化步骤，获得第二结果序列；

步骤234，依次拼接所述第一结果序列、所述脱陷运动序列和所述第二结果序列，获得所述结果输出序列。

5.根据权利要求4所述的基于序列优化的机器人状态规划方法，其特征在于，在所述步骤234之后，所述序列优化步骤还包括：

步骤240，将所述结果输出序列作为第二待处理序列，采用预设窗宽的窗口依次截取所述第二待处理序列，获得多段依次排列的分段序列；

步骤241，对于每一段所述分段序列，将所述分段序列的首运动状态作为所述初始状态，所述分段序列的尾运动状态作为所述目标状态，重复执行所述随机序列生成步骤多次，并再次执行所述运动序列确定步骤，或重复执行所述随机序列生成步骤多次，并依次执行所述运动序列确定步骤和所述序列优化步骤，获得每个所述分段序列的分段优化序列；

步骤242，依次拼接所有的所述分段优化序列，获得所述窗口优化初始序列，将所述窗口优化初始序列作为所述第二待处理序列，返回步骤240，经过多次迭代，获得窗口优化序列。

6.根据权利要求5所述的基于序列优化的机器人状态规划方法，其特征在于，在所述获得窗口优化序列的步骤之后，所述序列优化步骤还包括：

采用单向可达性简化算法对所述窗口优化序列进行简化，获得运动优化序列。

7.根据权利要求6所述的基于序列优化的机器人状态规划方法，其特征在于，在所述获得运动优化序列之后，还包括：

基于多项式拟合的方法，根据所述运动优化序列的所有所述运动状态分别确定所述机器人的机体重心轨迹和摆动腿足端轨迹。

8.一种基于序列优化的机器人状态规划装置，其特征在于，包括：

随机序列生成模块，用于根据预设的初始状态重复执行随机序列生成步骤多次，获得包括多个随机序列的随机序列集合；所述随机序列生成步骤包括：根据所述初始状态循环执行运动状态生成算法直至到达预设的目标状态或无法生成下一运动状态，获得包括所有运动状态的所述随机序列，其中，所述运动状态生成算法为根据当前运动状态随机生成机器人的下一运动状态；

运动序列确定模块，用于根据预设规则分别对每个所述随机序列进行评分，根据评分结果在所述随机序列集合中确定分数最高的所述随机序列为所述机器人的运动序列；

其中，每条所述随机序列均对应有任务完成度，每个所述运动状态均对应有移动步长，对于所述随机序列集合中的每一个所述随机序列，根据所述随机序列对应的所有所述移动步长确定所述随机序列的平均步长，采用预设的权重对所述平均步长和所述任务完成度进行加权求和，获得每条所述随机序列的评分结果；

运动序列优化模块，用于根据所有所述随机序列的所有所述运动状态构成图结构；确定所述运动序列的尾运动状态为终止状态，并根据所述图结构生成从所述初始状态到所述终止状态的图优化运动序列，所述图优化运动序列为从所述初始状态到所述终止状态时遍历所述运动状态数量最少的序列。

9.一种基于序列优化的机器人状态规划装置，其特征在于，包括存储器和处理器；

所述存储器，用于存储计算机程序；

所述处理器，用于当执行所述计算机程序时，实现如权利要求1至7任一项所述的基于序列优化的机器人状态规划方法。

10.一种计算机可读存储介质，其特征在于，所述存储介质上存储有计算机程序，当所述计算机程序被处理器执行时，实现如权利要求1至7任一项所述的基于序列优化的机器人状态规划方法。

11.一种六足机器人，其特征在于，包括机体和如权利要求9所述的基于序列优化的机器人状态规划装置，所述机器人状态规划装置的输出端与所述机体的通信接口连接。

Images