六足机器人的自由容错步态规划方法、装置及存储介质
技术领域
本发明涉及机器人控制技术领域，具体而言，涉及一种六足机器人的自由容错步态规划方法、装置及存储介质。
背景技术
由于轮式机器人需要在连续的地面上才能运动，而足式机器人可以在离散的落足点地形中选择落足点进行运动，因此在面对复杂的野外环境时，足式机器人具有更好的通行能力。在众多足式机器人中，六足机器人凭借其强大的环境适应能力和较高的容错性脱颖而出，因其能够在一些危险的环境中，执行各种高难度的任务，而被广泛应用于工业、航天、军事和抢险救灾等领域，具有广阔的发展前景。
步态规划是指足式机器人从一个位置行走到另一个位置时规划足式机器人腿部之间的相位关系。现有的步态规划方法，在规划六足机器人的步态时，通常仅考虑了六足机器人通过落足点密集的地形时的情况，运动过程中六足机器人的每条腿都能落足在落足点上。而在面对落足点稀疏的地形或者六足机器人的腿出现故障时，六足机器人的腿会出现在运动周期中没有落足点或无法落足的情况，此时现有技术没有很好的解决办法。
发明内容
本发明解决的问题是在六足机器人的腿出现故障或没有落足点时如何规划六足机器人的步态，提高六足机器人通过稀疏落足点地形的能力。
为解决上述问题，本发明提供一种六足机器人的自由容错步态规划方法、装置及存储介质。
第一方面，本发明提供了一种六足机器人的自由容错步态规划方法，所述方法包括如下步骤：
获取六足机器人的状态信息和地面落足点信息。
根据所述状态信息确定所述六足机器人的摆动腿组合和机体重心移动步长，所述摆动腿组合包括零个或至少一个摆动腿。
结合所述机体重心移动步长和所述状态信息分别确定所述六足机器人的机体重心轨迹和各个所述摆动腿的落足区域。
根据所述地面落足点信息确定各个所述摆动腿在对应的所述落足区域中的目标落足点，并根据所述状态信息确定各个所述摆动腿是否能落足到对应的所述目标落足点。
对于没有所述目标落足点或无法落足的所述摆动腿，控制所述摆动腿悬停在预定位置；对于能落足在对应的所述目标落足点的所述摆动腿，根据所述状态信息和所述目标落足点确定所述摆动腿的足端轨迹。
第二方面，本发明提供了一种六足机器人的自由容错步态规划装置，包括：
获取模块，用于获取六足机器人的状态信息和地面落足点信息。
第一处理模块，用于根据所述状态信息确定所述六足机器人的摆动腿组合和机体重心移动步长，所述摆动腿组合包括零个或至少一个摆动腿。
第一规划模块，用于结合所述机体重心移动步长和所述状态信息分别确定所述六足机器人的机体重心轨迹和各个所述摆动腿的落足区域。
第二处理模块，用于根据所述地面落足点信息确定各个所述摆动腿在对应的所述落足区域中的目标落足点，并根据所述状态信息确定各个所述摆动腿是否能落足到对应的所述目标落足点。
第二规划模块，用于对于没有所述目标落足点或无法落足的所述摆动腿，控制所述摆动腿悬停在预定位置；对于能落足在对应的所述目标落足点的所述摆动腿，根据所述状态信息和所述目标落足点确定所述摆动腿的足端轨迹。
第三方面，本发明提供了一种六足机器人的自由容错步态规划装置，包括存储器和处理器。
所述存储器，用于存储计算机程序。
所述处理器，用于当执行所述计算机程序时，实现如上所述的六足机器人的自由容错步态规划方法。
第四方面，本发明提供了一种计算机可读存储介质，所述存储介质上存储有计算机程序，当所述计算机程序被处理器执行时，实现如上所述的六足机器人的自由容错步态规划方法。
本发明的六足机器人的自由容错步态规划方法、装置及存储介质的有益效果是：首先根据获取的六足机器人的状态信息和地面落足点信息确定摆动腿和机体重心移动步长，摆动腿以外的六足机器人的腿就为支撑腿；然后根据机体重心移动步长和状态信息规划机体重心轨迹，并确定出各个摆动腿的目标落足点，同时判断各个摆动腿是否能落足在对应的目标落足点上。对于没有落足点或无法落足的摆动腿，控制该摆动腿悬停在预定位置，可以减少各个摆动腿之间的干扰，其中，无法落足的摆动腿包括因发生故障无法动作的摆动腿，没有落足点包括因环境恶劣导致摆动腿的落足区域内无可落足点等情况；对于能落足在对应的目标落足点上的摆动腿，规划该摆动腿的足端轨迹。最后，根据机体重心轨迹控制六足机器人的机体移动到下一位置，并根据足端轨迹控制各个能落足在对应的目标落足点的摆动腿进行运动，完成六足机器人在一个运动周期中的运动过程，重复上述步骤，经过多个运动周期后，六足机器人就可到达目标位置。本申请的技术方案中，对可落足在目标落足点的摆动腿规划足端轨迹，对出现没有目标落足点或无法落足等报错情况的摆动腿，控制其悬停在预定位置，完成六足机器人的步态规划，实现了对六足机器人腿部错误的容错处理，能够提高六足机器人面对稀疏落足点地形时的通行能力。
附图说明
图1为本发明实施例的一种六足机器人的自由容错步态规划方法的流程示意图；
图2为本发明实施例的机体重心轨迹示意图；
图3为本发明实施例的一种摆动腿落足过程示意图；
图4为本发明另一实施例的一种摆动腿落足过程示意图；
图5为本发明再一实施例的一种摆动腿落足过程示意图；
图6为本发明实施例的摆动腿的足端轨迹示意图；
图7为本发明实施例的摆动腿打滑或沦陷时的状态示意图；
图8为本发明实施例的摆动腿尝试脱陷的过程示意图；
图9为本发明实施例的一种六足机器人的腿部支撑状态相序图；
图10为本发明实施例的一种六足机器人通过落足点区域的仿真示意图；
图11为在图10中的部分状态下六足机器人的俯视示意图；
图12为本发明实施例的一种六足机器人的自由容错步态规划装置的结构示意图。
具体实施方式
为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。
需要说明的是，本文中的坐标系XWYWZW为六足机器人的机体坐标系。附图中箭头方向为六足机器人的机体运动方向，同时，要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。
如图1所示，本发明实施例提供的一种六足机器人的自由容错步态规划方法，所述方法包括如下步骤：
100，获取六足机器人的状态信息和地面落足点信息。
具体地，状态信息可包括六足机器人的机体位姿信息、腿部位姿信息和故障状态信息等，地面落足点信息可通过六足机器人的摄像头拍摄地面图片获得。
200，根据所述状态信息确定所述六足机器人的摆动腿组合和机体重心移动步长，所述摆动腿组合包括零个或至少一个摆动腿。
具体地，可根据状态信息在预设的支撑状态表中确定六足机器人的支撑状态，支撑状态对应着六足机器人的支撑腿组合和摆动腿组合。六足机器人移动时，通过支撑腿组合中的各个支撑腿支撑机体，摆动腿组合中的各个摆动腿向目标位置摆动落足。六足机器人还可能在某一运动周期内无法确定摆动腿组合，即摆动腿数量为零，支撑腿数量为六，此时控制六足机器人根据机体重心轨迹运动至下一位置，再根据在下一位置时的状态信息确定六足机器人的摆动腿组合，若此摆动腿组合中摆动腿数量仍为零，则六足机器人停止向前运动，若此摆动腿组合中包括有至少一个摆动腿，则继续规划各个摆动腿的足端轨迹。
300，结合所述机体重心移动步长和所述状态信息分别确定所述六足机器人的机体重心轨迹和各个所述摆动腿的落足区域。
具体地，摆动腿的落足区域可为摆动腿的足端可达工作区域，由于摆动腿的物理参数限制下摆动腿的可达区域。六足机器人在运动过程中，摆动腿向目标位置摆动时，机体重心也要相应前移。规划机体重心轨迹，控制六足机器人根据机体重心轨迹移动，能够使得六足机器人在运动过程中保持静态稳定。
400，根据所述地面落足点信息确定各个所述摆动腿在对应的所述落足区域中的目标落足点，并根据所述状态信息确定各个所述摆动腿是否能落足在对应的所述目标落足点。
具体地，在同时满足具有目标落足点且能够落足时，摆动腿才能准确落足到目标落足点上。
500，对于没有所述目标落足点或无法落足的所述摆动腿，控制所述摆动腿悬停在预定位置；对于能落足在对应的所述目标落足点的所述摆动腿，根据所述状态信息和所述目标落足点确定所述摆动腿的足端轨迹。
具体地，无法落足的摆动腿可包括具有故障的摆动腿等，预定位置可为靠近六足机器人机体的指定位置，能够减少各个摆动腿之间的干扰。
本实施例中，首先根据获取的六足机器人的状态信息和地面落足点信息确定摆动腿和机体重心移动步长，摆动腿以外的六足机器人的腿就为支撑腿；然后根据机体重心移动步长和状态信息规划机体重心轨迹，并确定出各个摆动腿的目标落足点，同时判断各个摆动腿是否能落足在对应的目标落足点上。对于没有落足点或无法落足的摆动腿，控制该摆动腿悬停在预定位置，可以减少各个摆动腿之间的干扰，其中，无法落足的摆动腿包括出现故障的摆动腿等；对于能落足在对应的目标落足点上的摆动腿，规划该摆动腿的足端轨迹。最后，根据机体重心轨迹控制六足机器人的机体移动到下一位置，并根据足端轨迹控制各个能落足在对应的目标落足点的摆动腿进行运动，完成六足机器人在一个运动周期中的运动过程，重复上述步骤，经过多个运动周期后，六足机器人就可到达目标位置。本申请的技术方案中，对可落足在目标落足点的摆动腿规划足端轨迹，对出现没有目标落足点或无法落足等报错情况的摆动腿，控制其悬停在预定位置，完成六足机器人的步态规划，实现了对六足机器人腿部错误的容错处理，能够提高六足机器人面对稀疏落足点地形时的通行能力。
优选地，所述状态信息包括所述六足机器人的故障信息；所述根据所述状态信息确定所述六足机器人的摆动腿组合和机体重心移动步长包括：
根据所述故障信息确定所述六足机器人的除故障腿以外的腿为预选支撑腿。
具体地，故障腿包括当前状态下无目标落足点的腿和具有故障的腿，故障腿无法支撑起六足机器人，因此选择除故障腿以外其它腿为预选支撑腿。
根据所述预选支撑腿与预设的支撑状态对应关系确定所述六足机器人的预选支撑状态，其中，所述预选支撑状态对应关系包括至少一个所述预选支撑状态以及与每个所述预选支撑状态分别对应的稳定裕度、机体最大前进步长和摆动腿预选组合。
具体地，预设的支撑状态对应关系可以为表格形式，也就是预设的支撑状态表，根据预设的支撑状态表自由组合多个预选支撑腿的状态，预选支撑腿的状态包括支撑和摆动，确定出所有可能的预选支撑状态，所有的预选支撑状态组成的集合就是预选支撑状态集合。
六足机器人在每个预选支撑状态下均能保持静态稳定，保持静态稳定的前提是在运动过程中支撑腿的数量大于或等于三，且机体重心在支撑多边形内，且能够满足标定的稳定裕度。具体而言，支撑多边形为支撑腿足端连线所组成的多边形，稳定裕度表示机体重心在支撑多边形的投影与支撑多边形任意一条边的距离，六足机器人要保持静态稳定需要稳定裕度大于或等于预设的阈值，该阈值的典型取值为0.1m。若依次对六足机器人的六条腿进行排序，支撑腿用1表示，摆动腿用0表示，则包含了所有可能的支撑状态的支撑状态表如表一所示：
表一 支撑状态表
表一中支撑状态显示了该状态下的各条腿的状态，支撑状态示意图中的多边形即为支撑多边形，圆点即为机体重心在支撑多边形中的投影。
需要说明的是，每个所述预选支撑状态分别对应的稳定裕度、机体最大前进步长和摆动腿预选组合没有在本实施例的支撑状态表中表示出来。
根据所述稳定裕度和所述机体最大前进步长确定所述六足机器人的稳定支撑状态，其中，所述稳定支撑状态对应的所述机体最大前进步长为所述机体重心移动步长，所述稳定支撑状态对应的所述摆动腿预选组合为所述摆动腿组合。
具体地，稳定裕度对应了六足机器人在运动过程中的稳定性，机体最大前进步长对应了六足机器人在运动过程中的行走速度。根据稳定裕度和机体最大前进步长来确定稳定支撑状态，可根据需要来自由选择对应的稳定支撑状态。能够使六足机器人在保持静态稳定的情况下，尽量提高通行速度。
本优选的实施例中，可根据具体地形的需要自由选择稳定支撑状态来规划步态，相对于现有技术中采用的周期性的三足步态、四足步态或五足步态，能够充分发挥六足机器人的多足优势，大幅提高六足机器人应对复杂地形的能力，并且能够在保持静态稳定的前提下，提升六足机器人的移动速度。
优选地，所述根据所述稳定裕度和所述机体最大前进步长确定所述六足机器人的稳定支撑状态包括：
采用第一公式确定各个所述预选支撑状态的评价函数，所述第一公式为：
其中，f(state)为所述评价函数，S为所述预选支撑状态集合，state为所述预选支撑状态集合中的任一所述预选支撑状态，BMstate为所述预选支撑状态对应的所述稳定裕度，
根据所述评价函数，采用第二公式确定所述稳定支撑状态，所述第二公式为：
其中，state0为所述稳定支撑状态。
具体地，运动过程中，ω1越大则表示六足机器人越稳定，ω2越大则表示六足机器人行走速度越快，因此，可根据具体需要来选择ω1和ω2的数值。例如：在落足点密集的地形中，六足机器人稳定度较好，可适当增大ω2，降低ω1，以提高六足机器人的行走速度；而在落足点稀疏的地形中，可适当增大ω1，降低ω2，适当牺牲行走速度的情况下，提升六足机器人通过落足点稀疏的地形的通行能力。
优选地，所述状态信息包括所述六足机器人的机体重心当前位置、各个所述摆动腿的设备参数和预设的机体运动参数；所述结合所述机体重心移动步长和所述状态信息确定所述六足机器人的机体重心轨迹和各个所述摆动腿的落足区域包括：
根据所述机体重心当前位置和所述机体重心移动步长确定所述六足机器人的机体重心下一位置；令机体重心位于所述机体重心下一位置，根据各个所述摆动腿的设备参数分别确定各个所述摆动腿的所述落足区域。
具体地，在机体重心当前位置上向前移动机体重心移动步长就可得到机体重心下一位置，假设六足机器人的机体重心运动到该机体重心下一位置，根据摆动腿的设备参数来确定此时该摆动腿的落足区域，设备参数包括摆动角度和摆动腿的长度等物理学参数。
基于多项式拟合的方法，根据所述机体重心当前位置、所述机体重心下一位置和所述机体运动参数确定所述机体重心轨迹。
具体地，如图2所示，机体重心轨迹规划就是在支撑腿组合支撑六足机器人的情况下，即支撑腿的足端位置不变的情况下，规划机体重心从机体重心当前位置到机体重心下一位置的轨迹。
本实施例中将机体重心轨迹视为直线，用五次多项式来规划机体重心轨迹，以保证机体重心位置、速度和加速度的连续光滑性。令机体重心当前位置为Wpb1，机体重心下一位置为Wpb3，预设的机体运动参数包括机体重心在机体重心当前位置时的速度Wvb1、加速度Wab1和对应时间t1，以及机体重心在机体重心下一位置时的速度Wvb3、加速度Wab3、对应时间t3。规划机体重心轨迹时的已知量如表二所示：
表二 机体重心轨迹规划已知量
根据机体重心的位置和对应的时间建立机体重心轨迹的位置多项式，机体重心轨迹的位置多项式如下式所示：
Wpb＝Ab0+Ab1t+Ab2t2+Ab3t3+Ab4t4+Ab5t5，
其中，Wpb为Wpb1或Wpb3，Wpb1对应的t为t1，Wpb3对应的t为t3。
Wpb1可为Wpx1或Wpy1或Wpz1，Wpx1为机体重心当前位置在机体坐标系中的XW轴坐标，Wpy1是机体重心当前位置在机体坐标系中的YW轴坐标，Wpz1为机体重心当前位置在机体坐标系中的ZW坐标。Wpb3可为Wpx3或Wpy3或Wpz3，Wpx3为机体重心下一位置在机体坐标系中的XW轴坐标，Wpy3是机体重心下一位置在机体坐标系中的YW轴坐标，Wpz3为机体重心下一位置在机体坐标系中的ZW轴坐标，本实施例中Wpz3等于Wpz1。
根据机体重心的速度和对应的时间建立机体重心轨迹的速度多项式，机体重心轨迹的速度多项式如下式所示：
Wvb＝Ab1+2Ab2t+3Ab3t2+4Ab4t3+5Ab5t4，
其中，Wvb为Wvb1或Wvb3，Wvb1对应的t为t1，Wvb3对应的t为t3。
Wvb1可为Wvx1或Wvy1或Wvz1，Wvx1为Wvb1的XW轴分量，Wvy1是Wvb1的YW轴分量，Wvz1为Wvb1的ZW轴分量。Wvb3可为Wvx3或Wvy3或Wvz3，Wvx3为Wvb3的XW轴分量，Wvy3是Wvb3的YW轴分量，Wvz3为Wvb3的ZW轴分量。
根据机体重心的加速度和对应的时间建立机体重心轨迹的加速度多项式，机体重心轨迹的加速度多项式如下式所示：
Wab＝2Ab2+6Ab3t+12Ab4t2+20Ab5t3，
其中，Wab为Wab1或Wab3，Wab1对应的t为t1，Wab3对应的t为t3。
Wab1可为Wax1或Way1或Waz1，Wax1为Wab1的XW轴分量，Way1是Wab1的YW轴分量，Waz1为Wab1的ZW轴分量。Wab3可为Wax3或Way3或Waz3，Wax3为Wab3的XW轴分量，Way3是Wab3的YW轴分量，Waz3为Wab3的ZW轴分量。
将各参数代入对应的多项式中，t1可设为0，可得到如下的公式：
根据已知的Wpb1、Wpb3、Wvb1、Wvb3、Wab1、Wab3和t3就可以得到多项式系数[Ab0,Ab1,Ab2,Ab3,Ab4,Ab5]T，其中，Wvb1、Wvb3、Wab1和Wab3可为0，t3可根据情况进行具体设置。
本优选的实施例中，在确定出多项式系数后，通过设定时间就可以得到机体重心从机体重心当前位置运动到机体重心下一位置过程中的任意位置，以及该位置对应的机体速度和加速度，确定机体重心轨迹。轨迹规划过程平滑，并且能够满足六足机器人的稳定性要求和能量消耗的约束。
优选地，所述状态信息还包括各个所述摆动腿的足端当前位置；所述根据所述地面落足点信息确定各个所述摆动腿在对应的所述落足区域中的目标落足点包括：
根据所述地面落足点信息确定各个所述落足区域中的落足点集合，所述落足点集合包括零个或至少一个所述预选落足点。
对于任一所述摆动腿，令所述落足点集合为P，根据第三公式确定选取所述目标落足点的代价函数，所述第三公式为：
其中，pi为所述摆动腿的所述足端当前位置，pe为所述摆动腿对应的所述预选落足点，
基于所述代价函数，根据第四公式确定各个所述摆动腿的所述目标落足点，所述第四公式为：
其中，p为所述摆动腿的所述目标落足点。
具体地，如图3所示，靠左的虚线框为机体重心移动前任一摆动腿的落足区域，K1对应的折线段为该摆动腿摆动前的位姿，pi为此时该摆动腿的足端位置；靠右的实线框为机体重心移动到机体重心下一位置时该摆动腿的落足区域，虚线框和实线框中的圆圈表示落足点，K2对应的折线段为该摆动腿摆动后的位姿，pe为此时该摆动腿的足端位置，即预选落足点，图4中将pe视为目标落足点，连接pi和pe的虚线即为该摆动腿的足端轨迹。箭头表示六足机器人的机体运动方向。
如图4所示，机体重心移动前对应的虚线框中有圆圈，而机体重心移动后对应的实线框中没有圆圈，即摆动腿在机体重心前移后的落足区域没有落足点，此时控制该摆动腿收回并悬停至预定位置，即图4中K2所示的位姿。
如图5所示，机体重心移动前对应的虚线框中没有圆圈，而机体重心移动后对应的实线框中有圆圈，即摆动腿在机体重心前移前对应的落足区域没有落足点，该摆动腿此时悬停在预定位置，即图5中K1所示的位姿。在机体重心前移后对应的落足区域有落足点，此时控制该摆动腿落足到对应的目标落足点pe。
优选地，所述故障信息包括腿部故障信息；根据所述状态信息确定各个所述摆动腿是否能落足在对应的所述目标落足点包括：根据所述腿部故障信息确定各个所述摆动腿是否为无法落足的摆动腿。
具体地，根据腿部故障信息确定各个摆动腿是否为故障腿，故障腿即为无法落足的摆动腿，腿部故障信息可包括驱动器故障、电机损毁、关节锁死和编码器等传感器失效等。
优选地，所述状态信息还包括预设的摆动腿运动参数；所述根据所述当前状态信息和所述目标落足点确定所述摆动腿的足端轨迹包括：
基于多项式拟合的方法，根据所述足端当前位置、所述目标落足点和所述摆动腿运动参数确定所述摆动腿的足端轨迹。
具体地，如图6所示，采用多阶连续的六次多项式来规划摆动腿的足端轨迹，令足端当前位置为Wpr1，目标落足点所在位置为Wpr3，摆动腿的足端在运动过程中的中间点位置为Wpr2。预设的摆动腿运动参数包括摆动腿的足端在足端当前位置时的速度Wvr1、加速度War1和对应时间t4，以及摆动腿的足端在目标落足点时的速度Wvr3、加速度War3、对应时间t6，并令中间点位置的时间为t5。规划机体重心轨迹时的已知量如表二所示：
表三 足端轨迹规划已知量
根据摆动腿的足端位置和对应的时间建立摆动腿的足端位置多项式，足端位置多项式如下式所示：
Wpr＝Ar0+Ar1t+Ar2t2+Ar3t3+Ar4t4+Ar5t5，
其中，Wpr为Wpr1或Wpr2或Wpr3，Wpr对应的t为t4，Wpr2对应的t为t5，Wpr3对应的t为t6。
同样的，Wpr1可为Wprx1或Wpry1或Wprz1，Wprx1为足端当前位置在机体坐标系中的XW轴坐标，Wpry1是足端当前位置在机体坐标系中的YW轴坐标，Wprz1为足端当前位置在机体坐标系中的ZW坐标。Wpr3可为Wprx3或Wpry3或Wprz3，Wprx3为目标落足点在机体坐标系中的XW轴坐标，Wpry3是目标落足点在机体坐标系中的YW轴坐标，Wprz3为目标落足点在机体坐标系中的ZW轴坐标，本实施例中Wprz1和Wprz3均为0。Wpr2可为Wprx2或Wpry2或Wprz2，Wprx2为中间点位置在机体坐标系中的XW轴坐标，Wpry2为中间点位置在机体坐标系中的YW轴坐标，Wprz2为中间点位置在机体坐标系中的ZW轴坐标，本实施例中Wprz2为摆动腿的抬足高度，即图6中的h。
根据摆动腿的足端运动时的速度和对应的时间建立摆动腿的足端速度多项式，足端速度多项式如下式所示：
Wvr＝Ar1+2Ar2t+3Ar3t2+4Ar4t3+5Ar5t4+6Ar6t5，
其中，Wvr为Wvr1或Wvr3，Wvr1对应的t为t4，Wvr3对应的t为t6。
同样的，Wvr1可为Wvr1的XW轴分量或YW轴分量或ZW轴分量，Wvr3可为Wvr3的XW轴分量或YW轴分量或ZW轴分量。
根据摆动腿的足端运动时的加速度和对应的时间建立摆动腿的足端加速度多项式，足端加速度多项式如下式所示：
War＝2Ar2+6Ar3t+12Ar4t2+20Ar5t3+30Ar6t4，
其中，War为War1或War3，War1对应的t为t4，War3对应的t为t6。
同样的，War1可为War1的XW轴分量或YW轴分量或ZW轴分量，War3可为War3的XW轴分量或YW轴分量或ZW轴分量。
将参数代入对应的多项式中，可将t4设为0，得到如下的公式：
根据已知的Wpr1、Wpr2、Wpr3、Wvr1、Wvr3、War1、War3、t5和t6就可以得到多项式系数[Ar0,Ar1,Ar2,Ar3,Ar4,Ar5,Ar6]T，其中，Wvr1、Wvr3、War1和War3可为0，t5和t6可根据情况进行具体设置。
本优选的实施例中，在确定出多项式系数后，通过设定时间就可以得到摆动腿的足端从足端当前位置运动到目标落足点过程中的任意位置，以及该位置对应的足端的速度和加速度，确定出足端轨迹。通过多项式拟合的方法规划足端轨迹，不仅能够保证位置轨迹曲线平滑连续，还能保证速度曲线和加速度曲线平滑连续，使得电机在控制摆动腿运动时不会发生抖动突变，并且能够节省能量。
优选地，在所述根据所述状态信息和所述目标落足点确定所述摆动腿的足端轨迹之后，所述方法还包括如下步骤：
根据所述机体重心轨迹运动控制所述六足机器人至所述机体重心下一位置，并根据所述足端轨迹控制所述摆动腿运动至对应的所述目标落足点。
若确定所述摆动腿在落足到对应的所述目标落足点时出现打滑或沉陷，则控制所述摆动腿尝试落足到与所述目标落足点相邻的所述预选落足点。
当确定多次尝试落足至所述预选落足点均无法平稳落足时，控制所述摆动腿悬停在预定位置。
具体地，控制摆动腿的足端落足到目标落足点时，当地面的特性较差时，如地面湿滑和较软等，足端可能会出现打滑或沦陷的情况。如图7所示，箭头方向为机体运动方向，K1为摆动腿摆动前的位姿，pi为此时该摆动腿的足端位置；K2为摆动腿摆动后落足的位姿，pe1为此时该摆动腿的足端位置；图中圆圈为可落足的落足点，黑点为无法落足的落足点，即会使摆动腿的足端打滑或沉陷的落足点。即图7中摆动腿从pi移动到pe1落足时，该摆动腿的足端会出现打滑或沉陷的情况。
此时，控制该摆动腿落足到与pe1相邻的其它落足点，其它落足点如图8所示的pe2，由于pe2同样是黑点，即无法落足的落足点，因此继续控制该摆动腿尝试落足到与pe2相邻的落足点，以此类推，尝试n次。n的大小可根据需要进行设定。n越大，该摆动腿落足的可能性就越大，六足机器人通过该地形的概率就越大，但是通行速度也就越慢。n越小，该摆动腿落足的可能性就越小，六足机器人通过该地形的概率也会降低。
若尝试n次，摆动腿始终无法稳定落足，则将该摆动腿视为出错腿，控制该摆动腿悬停至预定位置。其中，可设n大于落足区域中的落足点数量，即摆动腿无法落足时，挨个尝试落足区域中的所有落足点，直至该摆动腿稳定落足，或所有落足点均无法稳定落足，此时控制该摆动腿悬停至预定位置。
本优选的实施例中，在地形物理特性较差，摆动腿的足端出现易打滑或沉陷等情况时，控制摆动腿尝试落足到落足区域内的其它落足点，重新寻找目标落足点，对落足点进行纠错，能够提高摆动腿稳定落足的概率，进而提高六足机器人在复杂地形中的通过能力。
下面以六足机器人通过落足点密度为11.3/m2的地形为例，对本发明实施例的一种六足机器人的自由容错步态规划方法做进一步的说明。
如图9所示的六足机器人的腿部支撑状态相序图，将六足机器人的六条腿依次命名为Leg1、Leg2、Leg3、Leg4、Leg5和Leg6，图中黑色部分表示对应的腿用作支撑腿，白色部分表示对应的腿用作摆动腿。在六足机器人的第一个运动周期中，Leg1、Leg2、Leg4和Leg5用作支撑腿，Leg3和Leg6用作摆动腿；在第二个运动周期中，Leg3、Leg4和Leg6用作支撑腿，Leg1、Leg2和Leg5用作摆动腿，以此类推。可知，采用本发明实施例的六足机器人的自由容错步态规划方法来规划步态时，支撑状态是自由选取的，六足机器人在三足步态、四足步态和五足步态之间自由切换，能够根据不同的地形选择合适的步态模式，提高了在各种复杂地形中的通行能力。
如图10所示的六足机器人通过落足点密度为11.3/m2的地形时的仿真示意图，六足机器人实现了从坐标点[0,0]移动到坐标点[8.4,0]的任务。图10中从左到右，从上到下依次对应了六足机器人在运动过程中的15个状态，图中黑点为落足点，白色区域为无落足点区域，通过仿真结果可知，采用本申请的自由容错步态规划方法能够帮助六足机器人顺利通过稀疏落足点区域。图11显示了与图10中前9个状态分别对应的六足机器人的俯视示意图。
如图12所示，本发明实施例提供的一种六足机器人的自由容错步态规划装置，包括：
获取模块，用于获取六足机器人的状态信息和地面落足点信息。
第一处理模块，用于根据所述状态信息确定所述六足机器人的摆动腿组合和机体重心移动步长，所述摆动腿组合包括零个或至少一个摆动腿。
第一规划模块，用于结合所述机体重心移动步长和所述状态信息分别确定所述六足机器人的机体重心轨迹和各个所述摆动腿的落足区域。
第二处理模块，用于根据所述地面落足点信息确定各个所述摆动腿在对应的所述落足区域中的目标落足点，并根据所述状态信息确定各个所述摆动腿是否能落足到对应的所述目标落足点。
第二规划模块，用于对于没有所述目标落足点或无法落足的所述摆动腿，控制所述摆动腿悬停在预定位置；对于能落足在对应的所述目标落足点的所述摆动腿，根据所述状态信息和所述目标落足点确定所述摆动腿的足端轨迹。
本发明另一实施例提供的一种六足机器人的自由容错步态规划装置包括存储器和处理器；所述存储器，用于存储计算机程序；所述处理器，用于当执行所述计算机程序时，实现如上所述的六足机器人的自由容错步态规划方法。该装置可为工控机和服务器等。
本发明再一实施例提供的一种计算机可读存储介质上存储有计算机程序，当所述计算机程序被处理器执行时，实现如上所述的六足机器人的自由容错步态规划方法。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-Only Memory，ROM)或随机存储记忆体(Random AccessMemory，RAM)等。在本申请中，所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本发明实施例方案的目的。另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以是两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
虽然本发明公开披露如上，但本发明公开的保护范围并非仅限于此。本领域技术人员在不脱离本发明公开的精神和范围的前提下，可进行各种变更与修改，这些变更与修改均将落入本发明的保护范围。
