基于蒙特卡洛树搜索算法的机器人状态规划方法
技术领域
本发明涉及机器人控制技术领域，具体而言，涉及一种基于蒙特卡洛树搜索算法的机器人状态规划方法。
背景技术
由于轮式机器人需要在连续的地面上才能运动，而足式机器人可以在离散的落足点地形中选择落足点进行运动，因此在面对复杂的野外环境时，足式机器人具有更好的通行能力。在众多足式机器人中，六足机器人凭借其强大的环境适应能力和较高的容错性脱颖而出，因其能够在一些危险的环境中，执行各种高难度的任务，而被广泛应用于工业、航天、军事和抢险救灾等领域，具有广阔的发展前景。
足式机器人从一个位置运动到另一个位置时，运动过程中的每一个位置都对应足式机器人的一个状态，状态包括足式机器人的当前位置信息、位姿和落足点等，运动过程中包括多个离散的状态。现有的机器人状态规划方法，在规划六足机器人的状态时，通常仅规划当前运动周期内六足机器人的下一状态，在六足机器人完成当前运动周期，运动到下一状态后，再规划下一运动周期的目标状态，但是六足机器人的状态规划过程是一个马尔科夫过程，前面的状态规划结果会对后续的状态规划决策产生一定的影响，前后规划结果的耦合结果会影响六足机器人在复杂地形中的通行能力。
发明内容
本发明解决的问题是如何处理状态间的耦合关系，减少六足机器人在前面运动周期中的状态对后续状态规划的影响，提高六足机器人通过复杂地形的能力。
为解决上述问题，本发明提供一种基于蒙特卡洛树搜索算法(Monte Carlo TreeSearch，MCTS)的机器人状态规划方法，具体包括基于MCTS的机器人状态规划方法、装置及存储介质。
第一方面，本发明提供了一种基于蒙特卡洛树搜索算法的机器人状态规划方法，包括：
获取机器人的初始状态和目标状态。
以所述初始状态为起始节点，采用蒙特卡洛树搜索算法扩展蒙特卡洛树，直至生成的目标节点到达所述目标状态。
根据所述起始节点到所述目标节点的所有节点确定所述机器人的状态序列。
第二方面，本发明提供了一种基于蒙特卡洛树搜索算法的机器人状态规划装置，包括：
获取模块，用于获取机器人的初始状态和目标状态。
处理模块，用于以所述初始状态为起始节点，采用蒙特卡洛树搜索算法扩展蒙特卡洛树，直至生成的目标节点到达所述目标状态。
输出模块，用于根据所述起始节点到所述目标节点的所有节点确定所述机器人的状态序列。
第三方面，本发明提供了一种基于蒙特卡洛树搜索算法的机器人状态规划装置，包括存储器和处理器。
所述存储器，用于存储计算机程序。
所述处理器，用于当执行所述计算机程序时，实现如上所述的基于蒙特卡洛树搜索算法的机器人状态规划方法。
第四方面，本发明提供了一种计算机可读存储介质，所述存储介质上存储有计算机程序，当所述计算机程序被处理器执行时，实现如上所述的基于蒙特卡洛树搜索算法的机器人状态规划方法。
第五方面，本发明提供了一种六足机器人，包括机体和如权利要求13所述的基于蒙特卡洛树搜索算法的机器人状态规划装置，所述机器人状态规划装置与所述机体通信连接。
本发明的基于蒙特卡洛树搜索算法的机器人状态规划方法的有益效果是：将预设的初始状态视为蒙特卡洛树的起始节点，采用蒙特卡洛树搜索算法构建和扩展蒙特卡洛树，使得蒙特卡洛树的节点不断逼近预设的目标状态，当节点到达目标状态时，遍历目标状态到初始状态的所有节点，按照初始状态到目标状态的顺序将所有节点依次排列，就可得到六足机器人从初始状态到目标状态的状态序列。本申请的技术方案中，采用蒙特卡洛树搜索算法对运动过程整体状态进行规划，生成整个运动过程的状态序列，能够避免分周期规划带来的前后耦合影响，降低了六足机器人在前面运动周期中的运动对后续运动的影响，提高了六足机器人在复杂地形中的通行能力。
附图说明
图1为本发明实施例的六足机器人的结构示意图；
图2为本发明实施例的支撑多边形和放缩的支撑多边形的示意图；
图3为本发明实施例的一种基于蒙特卡洛树搜索算法的机器人状态规划方法的流程示意图；
图4为本发明实施例的状态Φ0的状态空间示意图；
图5为本发明实施例的一种扩展蒙特卡洛树的流程示意图；
图6为本发明另一实施例的扩展蒙特卡洛树至标定深度的示意图；
图7为本发明另一实施例的扩展蒙特卡洛树的流程示意图；
图8为本发明另一实施例的分值指标示意图；
图9为本发明另一实施例的局部目标距离示意图；
图10为本发明实施例的一种离散落足点地形示意图；
图11为本发明实施例的六足机器人通过图10所示地形的仿真示意图；
图12为本发明实施例的一种六足机器人的腿部支撑状态相序图；
图13为本发明实施例的一种中间无落足点地形示意图；
图14为本发明实施例的六足机器人通过图13所示地形的仿真示意图；
图15为本发明实施例的一种连续壕沟地形示意图；
图16为本发明实施例的六足机器人通过图15所示地形的仿真示意图；
图17为本发明实施例的一种基于蒙特卡洛树搜索算法的机器人状态规划装置。
具体实施方式
为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。
需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。本申请的实施例中以六足机器人为例详细说明本申请的基于蒙特卡洛树搜索算法(Monte Carlo Tree Search，MCTS)的机器人状态规划方法，具体涉及基于MCTS的机器人状态规划方法、装置及存储介质。
首先对六足机器人的状态等信息进行详细说明，如图1所示，图1(a)为六足机器人的结构示意图，图1(b)为六足机器人结构的俯视示意图。六足机器人状态表示为
六足机器人在运动过程中要保持静态稳定，保持静态稳定的条件包括：六足机器人的支撑腿的数量大于或等于三，且机体重心在支撑多边形内，且能够满足标定的稳定裕度。具体而言，支撑多边形为支撑腿足端连线所组成的多边形，稳定裕度表示机体重心在支撑多边形的投影与支撑多边形任意一条边的距离，六足机器人满足标定的稳定裕度即稳定裕度大于预设的阈值，令该阈值为BM0，该阈值的典型取值为0.1m。
静态稳定裕度：SM表示六足机器人的静态稳定裕度。静态稳定裕度也可以称之为绝对静态稳定裕度，表示六足机器人的机体重心在支撑多边形的投影到支撑多边形边的最短距离，如图1所示，六足机器人保持静态稳定需要SM大于BM0。
放缩的支撑多边形：支撑多边形为六足机器人的支撑腿的足端与水平面上的交点顺次连接构成的凸多边形，如图1和图2所示的实线多边形。支撑多边形用于衡量机器人的稳定性。如果机器人的重心的水平投影落在支撑多边形内，则称六足机器人为稳定的。在六足机器人运动时，重心如果离支撑多边形的边缘太过靠近，仍然是比较危险的，容易侧翻，因此在规划过程中需要留一定余量。在本申请中，对支撑多边形往重心方向进行等距缩放，构成放缩的支撑多边形。
首先计算多边形的质心，图2中实线多边形为支撑腿数量为5的一个支撑多边形，虚线多边形为放缩的支撑多边形。定义支撑多边形质心的坐标为(xc,yc)，支撑腿足端的坐标为(xi,yi)(i＝1,2…n)，其中对于六足机器人n小于或等于6，则六足机器人质心的计算公式如下：
其中，A表示支撑多边形的面积，由如下公式确定：
确定好多边形质心位置后，往重心方向等距缩小支撑多边形，得到图2中虚线多边形表示的放缩的支撑多边形。BM0越大，支撑多边形缩小程度越大，六足机器人运动时稳定性越高，但会使得落足点和支撑状态的选择更少，进而使得六足机器人在复杂地形中的通行能力降低。
支撑状态向量：
表一支撑状态表
表一中支撑状态显示了该状态下的各条腿的状态，支撑状态示意图中的多边形即为支撑多边形，圆点即为机体重心在支撑多边形中的投影。
腿部状态向量：
错误腿：包括由于地面环境原因没有落足点的腿和出现故障的腿。当某条腿为错误腿时，运动过程中将其作为摆动腿。
运动学裕度余量：Ki(P)表示六足机器人任一条腿i的足端的运动学裕度余量,Ki(P)＝KMi，其中，KMi表示从足端沿着六足机器人机体运动的反方向到达工作空间边界的距离。腿i不能超过工作空间边界，因为超过工作空间边界，该腿就会被锁死不能继续工作。
机体允许前进量：AA表示六足机器人的机体允许前进量，机体允许前进量为六足机器人的机体重心(COG)沿着机体前进方向移动到放缩的支撑多边形的边的距离。
最大移动步长：MSL为六足机器人的最大移动步长,最大移动步长为六足机器人沿着前进方向，在保持稳定的基础上能够移动的最大距离，可由以下公式确定：MSL＝min(KMi,AA)。
如图3所示，本发明实施例提供的一种基于蒙特卡洛树搜索算法的机器人状态规划方法，包括：
步骤100，获取机器人的初始状态和目标状态。
步骤200，以所述初始状态为起始节点，采用蒙特卡洛树搜索算法扩展蒙特卡洛树，直至生成的目标节点到达所述目标状态。
具体地，蒙特卡洛树搜索算法是一种通过在决策空间中抽取随机样本，并根据结构构建蒙特卡洛树在特定领路找到最佳决策的方法。
步骤300，根据所述起始节点到所述目标节点的所有节点确定所述机器人的状态序列。
本实施例中，将预设的初始状态视为蒙特卡洛树的起始节点，采用蒙特卡洛树搜索算法构建和扩展蒙特卡洛树，使得蒙特卡洛树的节点不断逼近预设的目标状态，当节点到达目标状态时，遍历目标状态到初始状态的所有节点，按照初始状态到目标状态的顺序将所有节点依次排列，就可得到六足机器人从初始状态到目标状态的状态序列。本申请的技术方案中，采用蒙特卡洛树搜索算法对整个运动过程的状态进行规划，生成整个运动过程的状态序列，能够避免分周期规划带来的前后耦合影响，降低了六足机器人在前面运动周期中的运动对后续运动的影响，提高了六足机器人在复杂地形中的通行能力。
需要说明的是，状态序列中的所有状态为离散的状态。
优选地，所述采用蒙特卡洛树搜索算法扩展蒙特卡洛树，直至生成的目标节点到达所述目标状态包括：
步骤211，以所述起始节点为根节点，在已建立的当前蒙特卡洛树中从所述根节点开始向下递归选择子节点，直至到达所述叶子节点。
具体地，在已建立的蒙特卡洛树中，首先选择根节点的子节点，再选择该子节点的子节点，依次循环，直至到达蒙特卡洛树的叶子节点。
步骤212，确定所述叶子节点对应的状态是否为所述目标状态，若是，则转至步骤216；若否，则转至步骤213。
步骤213，在所述叶子节点后随机创建一个扩展节点。
具体地，蒙特卡洛树中的节点分为两种，一种为完全展开的节点，表示该节点的所有子节点均被访问过，另一种为未完全展开的节点，表示该节点至少有一个未被访问过的子节点。机器人的每个状态都对应有一个下一状态集合，蒙特卡洛树中的节点也对应有一个子节点集合，未完全展开的节点也可表示为该节点对应的状态的下一状态集合与该节点的子节点集合的差集非空，即该节点对应的状态的下一状态集合中的所有状态未全部在蒙特卡洛树中显示出来。
在叶子节点后随机创建一个未被访问过的节点，该未被访问过的节点即为扩展节点，可在叶子节点对应的下一状态集合中随机选择一个状态作为扩展节点。
步骤214，从所述扩展节点开始循环进行状态仿真，直至仿真得到的状态满足预设的第一终止条件。
需要说明的是，扩展节点会加入到蒙特卡洛树中，而仿真得到的节点并没有加入到蒙特卡洛树中，是虚拟的仿真过程。
步骤215，根据仿真结果回溯更新所述扩展节点到所述根节点的所有节点，并返回步骤211。
具体地，仿真结果包括成功或失败等，例如仿真得到的状态到达目标状态，或没有到达目标状态，根据该结果对扩展节点到根节点的所有节点的信息进行更新。在搜索的一开始，仅有根节点，根节点的所有子节点均是未被访问过的节点，则随机选择一个子节点作为扩展节点，进行仿真，并根据仿真结果回溯更新该子节点和根节点，重复选择未展开的节点重复上述过程，就可构建出蒙特卡洛树。
步骤216，令所述叶子节点为所述目标节点，遍历所述目标节点到所述起始节点的所有节点。
具体地，从该叶子节点向上回溯，遍历叶子节点到起始节点的所有节点，将遍历的所有节点按照起始节点到叶子节点的顺序依次排列，组成序列，就得到机器人的状态序列。
本优选的实施例中，采用蒙特卡洛树搜索算法能够在运动过程中的繁杂的状态信息中，快速生成到达目标状态的状态序列，能够显著提高六足机器人在复杂地形中的通行能力。
优选地，每个节点都对应有被访问的总次数和仿真成功参数两个参数，所述步骤211包括：
对于所述蒙特卡洛树中的任一节点，根据上限置信区间(Upper ConfidenceBound，UCB)算法分别确定所述蒙特卡洛树中所述节点各个子节点的分值，所述上限置信区间算法由第一公式表示，所述第一公式为：
其中，Φj为所述蒙特卡洛树中的任一节点，Φj'为节点Φj的一个子节点，UCB为子节点Φj'的的所述分值，Nvisit(Φj)为节点Φj的所述被访问的总次数，Nvisit(Φj')为子节点Φj'的所述被访问的总次数，Nsuccess(Φj')为子节点Φj'的所述仿真成功参数，C为平衡系数。
具体地，对于任一节点Φj，令节点Φj与其子孙节点组成的集合为Sj，所述仿真成功参数为集合Sj中仿真结果为成功的节点个数，对于任一节点，从该节点开始循环进行状态仿真，若仿真距离大于预设的仿真视野，则该节点的仿真结果为成功，否则，为该节点的仿真结果为失败。C值越小则搜索速度越快，选择节点越倾向于历史分值更好的节点，C值越大搜索速度越慢，更倾向于广度搜索。
从所述根节点开始，循环选择所述分值最大的子节点，直至到达所述叶子节点。
具体地，从根节点开始，先计算根节点的各个子节点的分值，选择其中分值最高的子节点，再计算该子节点的各个子节点的分值，依次类推。
本优选的实施例中，上限置信区间算法能够在探索和利用之间进行平衡，通过利用过去扩展和仿真的经验，和探索未展开的节点，能够在保持扩展速度的同时，积极探索通行能力更好的路径。
优选地，每个节点分别对应有一个子节点集合，所述子节点集合包括多个子节点，每个仿真得到的状态都对应有所述机器人的前进步长和仿真距离，所述步骤214包括：
根据所述扩展节点循环迭代随机仿真算法，直至仿真得到的仿真状态满足所述第一终止条件，所述随机仿真算法包括在当前节点的所述子节点集合中随机确定所述当前节点的子节点。
其中，每个所述仿真状态分别对应有仿真距离和所述机器人的移动步长，所述仿真距离为所述仿真状态到所述扩展节点之间的所有状态的数量。
具体地，根据扩展节点循环迭代随机仿真算法时，每个状态对应有一个动作集合，动作集合包括机器人的多个动作，包括六足机器人的机体动作和各条腿的动作等。假设任一状态Φ0为扩展节点，则在状态Φ0的动作集合A(Φ0)中随机选择一个动作，记选取的动作为a，a∈A(Φ0)，则六足机器人经过动作a后的下一个状态Φn可记为Φn＝f(Φ0,a)，Φn∈AS(Φ0)，AS(Φ0)为状态Φ0的下一备选状态集合。
如图4所示，某一状态的下一备选状态集合也为该状态的状态空间，对于状态Φ0，其可能的下一状态即状态Φ0的状态空间确定过程包括：在预设的支撑状态表中确定六足机器人的重心在放缩的支撑多边形中的支撑状态，令获得的n种支撑状态组成的集合为S。对于任一支撑状态s∈S，可以确定六足机器人在对应的前进方向MD上的最大前进步长MSs,MD，将最大前进步长MSs,MD离散为MSs,MD/3,MSs,MD×2/3,MSs,MD三种不同长度的步长，令所有步长组成的集合为L。对于任一长度的步长l∈L，在支撑状态s下，六足机器人各摆动腿的落足点组合有ml,s种。定义六足机器人任一状态的所有下一备选状态组成的下一备选状态集合为AS(Φ0)，该状态的下一备选状态的数量为Nalternative(Φ0)，可由第二公式确定，第二公式为：
在当前节点的子节点集合中随机选择当前节点的子节点即在当前状态的状态空间中随机选择下一状态。
本优选的实施例中，在当前节点的子节点集合中随机选取下一子节点，能够提高仿真速度。
优选地，所述机器人的状态包括所述机器人的旋转矩阵
步骤2141，获取所述机器人当前的所述机体位置
具体地，可通过六足机器人的定位装置确定其在世界坐标系中的机体位置
步骤2142，根据所述设备信息确定所述机器人在下一运动周期中的支撑状态和移动步长，所述支撑状态包括支撑腿组合和摆动腿组合，所述摆动腿组合包括零个或至少一个摆动腿，根据所述支撑腿组合和所述摆动腿组合确定所述机器人的所述支撑状态向量cF。
具体地，设备信息包括故障信息，根据故障信息确定六足机器人的除故障腿以外的腿为预选支撑腿，根据预选支撑腿与预设的支撑状态对应关系确定六足机器人的预选支撑状态，其中，预选支撑状态对应关系包括至少一个预选支撑状态以及与每个预选支撑状态分别对应的稳定裕度、机体最大前进步长和摆动腿预选组合，支撑状态对应关系可以为表格形式，也就是表1所示的支撑状态表。根据稳定裕度和机体最大前进步长采用第三公式确定六足机器人的各个预选支撑状态的评价函数，第三公式为：
其中，f(state)为评价函数，S为预选支撑状态集合，预选支撑状态集合为所有的预选支撑状态组成的集合，state为预选支撑状态集合中的任一预选支撑状态，BMstate为该预选支撑状态对应的稳定裕度，
根据所述评价函数，采用第四公式确定所述稳定支撑状态，第四公式为：
其中，state0为稳定支撑状态。稳定支撑状态对应的机体最大前进步长为六足机器人的移动步长，稳定支撑状态对应的摆动腿预选组合为所述摆动腿组合，除摆动腿外的其它腿为支撑腿组合。
六足机器人还可能在某一运动周期内无法确定摆动腿组合，即摆动腿数量为零，支撑腿数量为六，此时控制六足机器人根据机体重心轨迹运动至下一位置，再根据在下一位置时的状态信息确定六足机器人的摆动腿组合，若此摆动腿组合中摆动腿数量仍为零，则六足机器人停止向前运动，若此摆动腿组合中包括有至少一个摆动腿，则继续规划六足机器人的下一状态。
步骤2143，结合所述移动步长和所述设备信息分别确定所述机器人的机体重心轨迹和各个所述摆动腿的落足区域。
具体地，设备信息包括六足机器人的各个摆动腿的设备参数和预设的机体运动参数。在机体位置上向前运动移动步长就可得到机体重心下一位置，令六足机器人的机体重心位于机体重心下一位置，根据各个摆动腿的设备参数分别确定各个摆动腿的落足区域，设备参数包括摆动角度和摆动腿的长度等物理学参数。基于多项式拟合的方法，根据当前位置、机体重心下一位置和机体运动参数确定所述机体重心轨迹。
步骤2144，根据所述地面落足点信息确定各个所述摆动腿在对应的所述落足区域中的目标落足点，根据所述设备信息和各个所述摆动腿的所述目标落足点确定所述腿部状态向量tF、所述摆动腿落足点向量
具体地，根据地面落足点信息确定各个落足区域中的落足点集合，述落足点集合包括零个或至少一个所述预选落足点。
对于任一摆动腿，令落足点集合为P，根据第三公式确定选取目标落足点的代价函数，第五公式为：
其中，pi为所述摆动腿的足端当前位置，pe为所述摆动腿对应的预选落足点，
基于所述代价函数，根据第四公式确定各个摆动腿的目标落足点，所述第四公式为：
其中，p为所述摆动腿的目标落足点。
根据故障信息和目标落足点确定六足机器人的腿是否为错误腿，错误腿为没有目标落足点的腿和出现故障的腿，令错误腿的值为1，正常腿的值为0，确定腿部状态向量tF。对于错误腿以外有目标落足点的摆动腿，各个摆动腿的目标落足点组成摆动腿落足点向量
基于多项式拟合的方法，根据所述足端当前位置、所述目标落足点和所述摆动腿运动参数确定所述摆动腿的足端轨迹。
步骤2145，根据所述旋转矩阵
具体地，根据各个参数确定六足机器人的当前状态，控制机器人运动到下一位置，重复上述过程，确定六足机器人的下一状态，依次类推。
本优选的实施例中，分周期逐步确定六足机器人的下一状态，仿真得到路径能够提高六足机器人在运动过程中的稳定性。
优选地，所述第一终止条件包括连续多个生成的状态对应的所述机器人的移动步长为0，所述仿真距离大于预设的仿真视野和生成的状态到达所述目标状态中的至少一项。
具体地，第一终止条件中，连续Nstop个仿真或迭代得到的状态对应的所述机器人的前进步长为0即六足机器人在仿真过程中连续卡死，Nstop值太小，可能导致某些只是暂时卡死但长时间能够通行的路径丢失。仿真得到的终止状态对应的所述仿真距离大于预设的仿真视野即六足机器人在仿真过程中行进距离超过预设的仿真视野SH，所述终止状态到达所述目标状态即六足机器人在仿真过程中到达目标状态对应的位置，SH值越大表示考虑的越远，获得的状态序列对应的六足机器人的通行能力就越强，但搜索时间也会越高，因此，可在保证通过性的情况下，尽可能的更小的SH值。
优选地，所述步骤215包括：
比对所述仿真距离和预设的所述仿真视野，当所述仿真距离大于或等于所述仿真视野时，回溯更新所述扩展节点到所述根节点的所有节点的所述被访问的总次数和所述仿真成功参数。
当所述仿真距离小于所述仿真视野时，回溯更新所述扩展节点到所述根节点的所有节点的所述被访问的总次数。
具体地，从扩展节点依次向上回溯父节点直到根节点，对每一个节点的被访问的总次数和仿真成功参数进行更新，令Φ为扩展节点到根节点之间的任一节点，更新时，其被访问的总次数Nvisit(Φ)+1，其仿真成功参数Nsuccess(Φ)+Δscore，其中：
仿真视野SH小于扩展节点到目标状态对应节点的距离,pass表示仿真距离大于或等于仿真视野SH；notpass表示仿真距离小于仿真视野SH。
优选地，如图5所示，令所述初始状态对应的节点为起始节点，所述采用蒙特卡洛树搜索算法扩展蒙特卡洛树，直至生成的目标节点到达所述目标状态包括：
步骤221，以所述起始节点为根节点，确定所述根节点的子节点集合，所述子节点集合包括所述根节点的所有子节点。
具体地，如图5(a)所示，令起始节点Φk为根节点，将根节点Φk的所有子节点展开，可采用上述步骤213选取扩展节点的方法展开根节点Φk的所有子节点，该根节点Φk的子节点集合为AS(Φk)。
步骤222，对于所述子节点集合中的所有所述子节点，分别从每一个所述子节点开始循环进行状态仿真，直至仿真得到的仿真状态满足预设的第二终止条件，令所述仿真状态对应的所有节点中仿真距离最大的节点为当前终止节点。
具体地，对于任一子节点Φ1∈AS(Φk)，可采用步骤214的具体仿真方法进行仿真，令从节点Φ1为起点进行仿真的仿真距离为d(Φ1)，仿真距离为仿真起点到终止节点的节点数量，令从节点Φ1为起点仿真得到的所有节点的集合为T(Φ1)。本实施例中，第二终止条件包括仿真得到的连续多个节点对应的六足机器人的移动步长为0，即六足机器人卡死，和/或仿真得到节点到达目标状态。如图5(b)所示，图中X所示为终止节点。
步骤223，从所述当前终止节点开始依次向上回溯父节点直至到达所述起始节点，确定所述起始节点到所述当前终止节点的所有节点组成的序列为当前的主分支。
具体地，选择仿真距离最大的节点为当前终止节点，遍历当前终止节点到起始节点的所有节点，将所有节点按照从起始节点到终止节点的顺序进行排列构成序列，该序列就为当前的主分支。本实施例中，将当前的主分支中的当前终止节点到起始节点之间的所有节点都扩展到蒙特卡洛树中。如图5(b)所示，起始节点到当前终止节点的所有节点组成的序列为当前主分支。
步骤224，确定所述当前终止节点对应的所述仿真状态是否到达所述目标状态，若是，则转至步骤226；若否，则转至步骤225。
步骤225，在当前的所述主分支中选择一个节点为根节点，返回步骤221。
具体地，如图5(c)所示，仿真得到的当前终止节点未到达目标状态，则在当前的主分支上选择朝向起始节点方向上的与当前终止节点相邻的下一节点Φn为根节点，返回步骤221，展开该根节点的所有子节点，d(Φni)为该根节点的子节点ni进行仿真对应的仿真距离，确定出仿真距离最大的新终止节点，再次遍历新终止节点到起始节点的所有节点，更新主分支。可在选择节点Φn为根节点时，将节点Φn到起始节点的部分主分支保留，则从新终止节点向上遍历节点时，就可仅遍历到根节点，将节点Φn为起点展开和仿真得到的所有节点与部分主分支结合，生成新的主分支。
步骤226，令所述终止节点为所述目标节点，遍历所述目标节点到所述起始节点的所有节点。
本可选的实施例中，在确定根节点的子节点后，直接根据各个子节点进行仿真，相较于在蒙特卡洛树中递归选择子节点直至到达叶子节点，省去了搜索蒙特卡洛树中节点的过程，将仿真获得的状态加入到蒙特卡洛树的主分支中，不断迭代更新该主分支，进而不断逼近目标状态，充分利用了仿真结果，能够快速生成通过当前地形的状态序列，大幅提高生成状态序列的速度，提高效率。并且通过仿真找到容易使六足机器人受困的位置，在主分支中选择新的根节点重新进行仿真，直到仿真状态通过该容易受困的位置，使得最终生成的状态序列能够提高六足机器人在当前地形中的通行能力。
优选地，令所述起始节点到终止节点的长度为第一长度，当前的所述主分支的长度为第二长度，所述步骤225包括：
步骤2251，按照从所述当前终止节点到所述起始节点的顺序依次选择当前的所述主分支中的节点为所述根节点。
步骤2252，对于每次确定的所述根节点，依次执行步骤221和步骤222，得到新终止节点。
步骤2253，确定所述新终止节点对应的所述第一长度是否大于所述第二长度，若是，则将所述当前终止节点更新为所述新终止节点，转至步骤223；若否，则返回步骤2251。
具体地，按照终止节点到起始节点的顺序，依次选取当前主分支中的节点为根节点，对于每次选取的根节点，依次执行步骤221和步骤222，对根节点的所有子节点进行展开，再根据每一个子节点进行仿真，确定出仿真距离最大的节点为新终止节点，确定新终止节点对应的第一长度是否大于当前主分支的第二长度。若第一长度小于或等于第二长度，则在当前主分支中，往起始节点的方向选择与当前根节点相邻的下一个节点为根节点，重复上述步骤，直至得到的新终止节点对应的第一长度大于当前主分支的第二长度，则将起始节点到新终止节点的所有节点组成的序列作为新的主分支。如图5(d)和图5(e)所示，节点Φn为根节点，仿真得到的终止节点对应的第一长度不大于当前主分支的第二长度，因此在指向起始节点Φk的方向上，选择与节点Φn相邻的下一节点ΦI为根节点，依次执行步骤221和步骤222，确定出仿真距离最大的新终止节点ΦI,f，此时新终止节点ΦI,f对应的第一长度大于当前主分支的第二长度，因此遍历新终止节点ΦI,f到起始节点的所有节点，更新主分支。
第一长度为终止节点到起始节点包括的所有节点的数量，第二长度为当前的主分支包括的所有节点的数量。
需要说明的是，由于当前终止节点未到达目标状态时，表示在当前终止节点的基础上已无法继续仿真，因此在选择新的根节点时，按照当前终止节点到起始节点的方向，在当前主分支中，从与当前终止节点相邻的下一节点开始依次选择新的根节点。
本优选的实施例中，当前终止节点未到达目标状态，无法继续仿真时，表示当前终止节点对应的位置容易使六足机器人受困，通过在主分支中回退重新选择根节点，再次进行仿真的方法，尝试通过该容易受困区域，使得最终生成的状态序列能够提高六足机器人在复杂地形中的通行能力。仅在仿真得到的新终止节点到起始节点的长度大于当前主分支的长度时，更新当前主分支，能够保证每次更新后的主分支，终止节点都更接近目标状态，提高了生成状态序列的速度，效率更高。
优选地，如图6和图7所示，所述采用蒙特卡洛树搜索算法扩展蒙特卡洛树，直至生成的目标节点到达所述目标状态包括：
步骤231，以所述起始节点为根节点，采用蒙特卡洛树搜索算法扩展蒙特卡洛树的节点至标定深度，并根据预设的打分规则确定所述蒙特卡洛树中所有节点的分值。
具体地，如图6所示，可将扩展蒙特卡洛树的节点至标定深度视为在标定窗宽的窗口中构建蒙特卡洛树。可采用前文中扩展蒙特卡洛树的方法扩展蒙特卡洛树至标定深度，先根据根节点在已构建的蒙特卡洛树中递归选择子节点多次，再对当前扩展后的蒙特卡洛树的每一个叶子节点进行状态仿真多次，直至获得仿真结束节点，确定仿真结束节点到根节点的长度，该长度为从仿真结束节点回溯到根节点遍历的所有节点的个数，该长度等于标定深度。也可如步骤213所述，在叶子节点后，创建每个叶子节点的扩展节点，再从叶子节点开始进行仿真。
步骤232，根据各个节点的分值确定所述根节点的最佳子节点，令所述最佳子节点为根节点，返回步骤231，迭代多次，直至扩展得到的所述目标节点对应的状态到达所述目标状态。
具体地，如图7所示，根据各个节点的分值选择根节点的最佳子节点，保留该最佳子节点和其子树，剪去最佳子节点和其子树外的其它枝干，并以该最佳子节点为根节点，重复上述步骤，直至生成的蒙特卡洛树的节点到达目标状态。
本可选的实施例中，每次仅扩展标定深度的蒙特卡洛树，确定当前根节点的子节点后，再继续进行扩展，相较于扩展蒙特卡洛树至无法扩展后，再在整个蒙特卡洛树中直接确定序列，能够减少搜索节点的时间，大幅提高状态序列的生成速度。在每次生成的标定深度的蒙特卡洛树中确定当前根节点的子节点，再重新扩展蒙特卡洛树，相较于仿真随机生成状态，根据仿真得到的所有状态直接确定状态序列，在扩展蒙特卡洛树的同时，选定子节点，实时确定初始状态到当前状态的序列，实现了对蒙特卡洛树的优化，能够对繁杂的状态信息进行简化，减少最终生成的状态序列中的状态数量，使得最终生成的状态序列能够在保证六足机器人通行能力的情况下，提高通行速度。
优选地，所述采用蒙特卡洛树搜索算法扩展蒙特卡洛树的节点至标定深度包括：
从所述根节点开始向下递归选择子节点直至截止节点，所述截止节点到所述根节点的深度为第三长度，再从所述截止节点开始循环进行状态仿真，直至仿真距离达到第四长度，所述第三长度和第四长度之和为所述标定深度。
具体地，如图8所示，可使每次仿真的仿真距离，即第四长度保持一致，令第四长度为NSimStepNum，便于后续确定各个节点的分值。本实施例中，仿真得到节点并未实际扩展到蒙特卡洛树中，是虚拟的仿真。
优选地，每个节点分别对应有所述机器人在对应状态下的移动步长和静态稳定裕度，所述根据预设的打分规则确定所述蒙特卡洛树中所有节点的分值包括：
根据所述蒙特卡洛树中每个节点的所述移动步长和所述静态稳定裕度，确定状态仿真得到的所有节点的第一平均移动步长，所述截止节点到所述根节点之间的所有节点的第二平均移动步长和平均静态稳定裕度，和所述截止节点到所述截止节点的父节点的当前移动步长。
具体地，如图8所示，令截止节点为q，从截止节点q仿真固定距离NSimStepNum得到的所有节点的第一平均移动步长为Jq,SimStepL。仿真距离越长，第一平均步长越长，Jq,SimStepL越大，表示六足机器人在截止节点q有更好的通过性。
令Jq,StepExp为截止节点q到根节点的所有节点的第二平均移动步长，该值越大，能够让算法倾向于收敛出移动速度更快的序列。定义从截止节点q到根节点的所有节点组成的集合为Cq，定义集合Cq中包括的元素个数为nt，定义集合Cq中任一节点t与其父节点的步长为st,对于根节点r,sr为0。Jq,StepExp可由如下公式确定：
令Jq,marginExp为截止节点q到根节点的所有节点的平均静态稳定裕度，该值越大，能够让算法倾向于收敛出运动更稳定的序列。SMt为集合Cq中任一节点t的静态稳定裕度，则Jq,marginExp由如下公式确定：
令Jq,disToPar为截止节点q的父节点到截止节点q的当前移动步长，通过该值能够防止算法反复访问前进距离极小的节点。
对所述第一平均移动步长、所述第二平均移动步长、所述平均静态稳定裕度和所述当前移动步长进行加权求和，确定所述截止节点的奖励值。
具体地，对Jq,SimStepL、Jq,StepExp、Jq,marginExp和Jq,disToPar进行加权求和，获得截止节点q的奖励值，如下公式所示：
Jq＝ωq,SimStepLJq,SimStepL+ωq,StepExpJq,StepExp+ωq,marginExp·Jq,marginExp+ωq,disToParJq,disToPar，
其中，Jq为截止节点q的奖励值，ωq,SimStepL为第一平均移动步长的权重参数，ωq,StepExp为第二平均移动步长的权重参数，ωq,marginExp为平均静态稳定裕度的权重参数，ωq,disToPar为当前移动步长的权重参数。
根据所述截止节点的奖励值回溯更新所述截止节点到所述根节点的所有节点的奖励值，根据各个节点的奖励值确定各个节点的分值。
具体地，从截止节点q依次向上回溯更新截止节点q的所有祖先节点直至根节点，令截止节点q的所有祖先组成的集合为Si，对任意节点j∈Si，其奖励值为Xj，节点j的奖励值更新公式可由下式表示：
比对截止节点q的奖励值Jq和节点j的奖励值Xj，当Jq＞Xj时，将节点j的奖励值更新为Jq；当Jq≤Xj时，节点j的奖励值保持不变。针对六足机器人的状态序列规划，仅需找到一条状态指标更好的序列，所以衡量一颗子树的好坏，由该子树的最佳子节点进行表征更好。若取整棵树的均值进行衡量，那么值较低的节点会降低最佳子节点的值，这样会使得在稀疏落足点地形中，难以找到到达目标状态的序列。
回溯更新时，还将截止节点q到根节点的所有节点的被访问的总次数都加一。可根据上文中所述的上限置信区间算法确定各个节点的分值，根据如下公式确定根节点的各个子节点的分值，公式如下：
其中，UCB为子节点Φj'的分值，Nvisit(Φj)仍为蒙特卡洛树中任一节点Φj的所述被访问的总次数，Nvisit(Φj')仍为节点Φj的子节点Φj'的所述被访问的总次数，而将仿真成功参数替换成子节点Φj'的奖励值X(Φj')，C为平衡系数，本实施例中可取为0。
如图9所示，Sd为标定深度，当蒙特卡洛树中某一节点到根节点的距离与从该节点进行仿真的仿真距离之和超过标定深度，该回合停止扩展蒙特卡洛树。当遇到障碍物时，到目标位置的距离会收缩，如图9所示，椭圆图形对应的为障碍物，对应的标定深度缩小，当标定深度减小到非常小的预设阈值时，整个算法停止，不再进行任何动作。
下面以六足机器人分别通过如图10所示的离散落足点地形、如图13所示的中间无落足点地形和如图15所示的连续壕沟地形为例，对本发明实施例的一种基于蒙特卡洛树搜索算法的机器人状态规划方法做进一步的说明。需要说明的是，下述示例中采用的是每次仅扩展蒙特卡洛树至标定深度的方法。
图10中白色区域为落足点区域，黑色区域为无落足点区域，图11中从左到右，从上到下依次对应了六足机器人通过图10所示的离散落足点地形时的12个运动状态。从图11的仿真结果可知，采用本申请的基于蒙特卡洛树搜索算法的机器人状态规划方法，六足机器人能够顺利通过离散落足点的地形。
图12所示的六足机器人的腿部支撑状态相序图，通过提取状态序列中各个运动状态下的支撑状态向量，就可确定出六足机器人在整个运动过程中步态，得到如图12所示的六足机器人在整个运动过程中腿部之间的相位关系，将六足机器人的六条腿依次命名为Leg1、Leg2、Leg3、Leg4、Leg5和Leg6，图13中黑色部分表示对应的腿用作支撑腿，代表支撑相，白色部分表示对应的腿用作摆动腿，代表摆动相。在六足机器人的第一个运动周期中，Leg1、Leg2、Leg3、Leg4、Leg5和Leg6都用作支撑腿，表示六足机器人第一运动周期并没有移动；在第二个运动周期中，Leg1、Leg3、Leg4和Leg5用作支撑腿，Leg2和Leg6用作摆动腿，以此类推。可知，采用本发明实施例的基于蒙特卡洛树搜索算法的机器人状态规划方法来规划机器人状态时，每个运动状态下支撑状态是自由选取的，六足机器人在三足步态、四足步态和五足步态之间自由切换，能够根据不同的地形选择合适的步态模式，提高了六足机器人在各种复杂地形中的通行能力。
图13中白色区域为落足点区域，黑色区域为无落足点区域，地面中间连续无落足点。图14中从左到右，从上到下依次对应了六足机器人通过图13所示的中间无落足点地形时的12个运动状态。从图14的仿真结果可知，采用本申请的基于蒙特卡洛树搜索算法的机器人状态规划方法，六足机器人在运动过程中仅采用了4条腿，无落足点的两条腿，始终悬停在预定位置，顺利通过了该中间无落足点的地形。
图15中白色区域为落足点区域，黑色区域为无落足点区域，该地形具有多个连续的壕沟，且壕沟的宽度不同。图16中从左到右，从上到下依次对应了六足机器人通过图15所示的连续壕沟地形时的12个运动状态。从图16的仿真结果可知，采用本申请的基于蒙特卡洛树搜索算法的机器人状态规划方法，六足机器人不断切换步长和步态，顺利通过了连续壕沟的地形。
如图17所示，本发明实施例提供的一种基于蒙特卡洛树搜索算法的机器人状态规划装置，包括：
获取模块，用于获取机器人的初始状态和目标状态。
处理模块，用于以所述初始状态为根节点，采用蒙特卡洛树搜索算法扩展蒙特卡洛树，直至生成的目标节点到达所述目标状态。
输出模块，用于根据所述根节点到所述目标节点的所有节点确定所述机器人的状态序列。
本发明另一实施例提供的一种基于蒙特卡洛树搜索算法的机器人状态规划装置包括存储器和处理器；所述存储器，用于存储计算机程序；所述处理器，用于当执行所述计算机程序时，实现如上所述的基于蒙特卡洛树搜索算法的机器人状态规划方法。该装置可为工控机和服务器等。
本发明再一实施例提供的一种计算机可读存储介质上存储有计算机程序，当所述计算机程序被处理器执行时，实现如上所述的基于蒙特卡洛树搜索算法的机器人状态规划方法。
本发明再一实施例提供的一种六足机器人，包括机体和如上所述的基于蒙特卡洛树搜索算法的机器人状态规划装置，所述机器人状态规划装置与所述机体通信连接。所述机器人状态规划装置可设置在机体上。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)或随机存储记忆体(Random AccessMemory，RAM)等。在本申请中，所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。
虽然本发明公开披露如上，但本发明公开的保护范围并非仅限于此。本领域技术人员在不脱离本发明公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。
