CN112091984B - 双足机器人的步态纠偏方法、装置和计算机设备 - Google Patents

双足机器人的步态纠偏方法、装置和计算机设备 Download PDF

Info

Publication number
CN112091984B
CN112091984B CN202011284461.8A CN202011284461A CN112091984B CN 112091984 B CN112091984 B CN 112091984B CN 202011284461 A CN202011284461 A CN 202011284461A CN 112091984 B CN112091984 B CN 112091984B
Authority
CN
China
Prior art keywords
pose
biped robot
current
gait
supporting
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202011284461.8A
Other languages
English (en)
Other versions
CN112091984A (zh
Inventor
麻星星
陈春玉
葛利刚
王鸿舸
黄明强
周江琛
刘益彰
谢铮
熊友军
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Ubtech Robotics Corp
Original Assignee
Ubtech Robotics Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Ubtech Robotics Corp filed Critical Ubtech Robotics Corp
Priority to CN202011284461.8A priority Critical patent/CN112091984B/zh
Publication of CN112091984A publication Critical patent/CN112091984A/zh
Priority to PCT/CN2020/140576 priority patent/WO2022105025A1/zh
Application granted granted Critical
Publication of CN112091984B publication Critical patent/CN112091984B/zh
Priority to US17/314,039 priority patent/US11878426B2/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B25J9/1666—Avoiding collision or forbidden zones
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1615—Programme controls characterised by special kind of manipulator, e.g. planar, scara, gantry, cantilever, space, closed chain, passive/active joints and tendon driven manipulators
    • B25J9/162—Mobile manipulator, movable base with manipulator arm mounted on it
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1653—Programme controls characterised by the control loop parameters identification, estimation, stiffness, accuracy, error analysis
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D57/00—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track
    • B62D57/02—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members
    • B62D57/032—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members with alternately or sequentially lifted supporting base and legs; with alternately or sequentially lifted feet or skid
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V40/00—Recognition of biometric, human-related or animal-related patterns in image or video data
    • G06V40/20—Movements or behaviour, e.g. gesture recognition
    • G06V40/23—Recognition of whole body movements, e.g. for sport training
    • G06V40/25—Recognition of walking or running movements, e.g. gait recognition
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00—Program-control systems
    • G05B2219/30—Nc systems
    • G05B2219/40—Robotics, robotics mapping to robotics vision
    • G05B2219/40264—Human like, type robot arm

Abstract

本申请提供了一种双足机器人的步态纠偏方法、装置和计算机设备，在运动过程中，系统获取双足机器人当前支撑脚的当前支撑位姿，然后根据当前支撑位姿和下一步预设的理想支撑位姿，计算得到支撑脚之间的相对位姿。系统根据两步支撑脚之间的相对位姿和双足机器人站立时的初始状态时的左右踝关节的关节距离，计算得到下一步的修正步态参数。最后，系统控制下一支撑脚按照修正步态参数进行移动。在运动过程中，系统实时监测双足机器人的落脚点，并据此按照预设算法对双足机器人下一步的步态参数进行调整，从而实现在运动中实时跟踪双足机器人的理想落脚位置，避免运动误差造成的预设路径偏移，进而保证多双足机器人编队在运动过程中的协同性。

Description

双足机器人的步态纠偏方法、装置和计算机设备
技术领域
本申请涉及机器人运动技术领域，特别涉及一种双足机器人的步态纠偏方法、装置和计算机设备。
背景技术
双足机器人是最有希望成为在人类生活和工作的环境中服务于人的机器人类型，其一直是机器人领域的研究热点。在双足机器人共同协作的场景中，机器人与机器人之间的相对距离可能会非常小。一方面，如果只关心双足机器人的终点精度而不对行走过程进行位置的闭环，将会使机器人与外界发生碰撞。另一方面，如果机器人所使用的纠偏算法只针对运动路径的终点进行纠偏，将无法保证机器人达到止步条件所消耗的时间（即机器人运动到终点的所使用的步数），从而无法满足多机器人编队在时间线上的一致性，有可能会导致机器人与机器人之间发生碰撞。
发明内容
本申请的主要目的为提供一种双足机器人的步态纠偏方法、装置和计算机设备，旨在解决现有双足机器人在运动过程中容易因为误差偏离预设路径导致无法满足多机器人编队在时间线上的一致性的弊端。
为实现上述目的，本申请提供了一种双足机器人的步态纠偏方法，包括：
获取所述双足机器人当前支撑脚的当前支撑位姿；
根据所述当前支撑位姿和所述双足机器人下一步预设的理想支撑位姿，计算得到所述双足机器人支撑脚之间的相对位姿，所述支撑脚之间的相对位姿为所述下一步预设的理想支撑位姿相对于所述当前支撑位姿的位姿；
根据所述支撑脚之间的相对位姿和所述双足机器人初始状态时的左右踝关节的关节距离，计算得到所述双足机器人下一步的步态参数；
控制所述双足机器人的下一支撑脚按照所述下一步的步态参数进行运动。
进一步的，所述获取所述双足机器人当前支撑脚的当前支撑位姿的步骤，包括：
获取所述双足机器人的质心在全局坐标系中的质心位姿，以及所述当前支撑脚在质心坐标系中的支撑位姿，所述质心坐标系是以所述双足机器人的质心为坐标原点构建的坐标系，所述全局坐标系对应所述双足机器人当前所处的环境全局；
根据所述支撑位姿和所述质心位姿，通过坐标系变换得到所述当前支撑脚在所述全局坐标系中的所述当前支撑位姿。
进一步的，所述质心为所述双足机器人的两个髋关节之间连线的中点，所述获取所述当前支撑脚在质心坐标系中的支撑位姿的步骤，包括：
获取所述当前支撑脚的髋关节角度、膝关节角度和踝关节角度；
根据所述髋关节角度、所述膝关节角度、所述踝关节角度、所述当前支撑脚的大腿长度、所述当前支撑脚的小腿长度以及所述双足机器人的两个髋关节之间的距离，解析得到所述当前支撑脚在所述质心坐标系中的支撑位姿。
进一步的，所述控制所述双足机器人的下一支撑脚按照所述下一步的步态参数进行运动的步骤，包括：
根据所述下一步预设的理想步态参数对所述下一步的步态参数进行限幅处理，得到修正步态参数；
控制所述下一支撑脚按照所述修正步态参数进行运动。
进一步的，所述根据所述下一步预设的理想步态参数对所述下一步的步态参数进行限幅处理，得到修正步态参数的步骤之后，包括：
判断所述下一支撑脚按照所述修正步态参数运动后，是否会发生碰撞；
若所述下一支撑脚按照所述修正步态参数运动后会发生碰撞，则根据与所述下一支撑脚发生碰撞的物体的位姿，修正所述修正步态参数，得到二次修正步态参数；
控制所述下一支撑脚按照所述二次修正步态参数进行运动，以避免发生碰撞。
进一步的，步态参数包括前向位移、侧向位移和转角位移，所述根据所述当前支撑位姿和所述双足机器人下一步预设的理想支撑位姿，计算得到所述双足机器人支撑脚之间的相对位姿的步骤，包括：
分别将所述当前支撑位姿和所述下一步预设的理想支撑位姿代入第一算法中，计算得到所述支撑脚之间的相对位姿，所述第一算法包括：
Figure 549739DEST_PATH_IMAGE001
，
Figure 543102DEST_PATH_IMAGE002
，
Figure DEST_PATH_IMAGE003
，其中，x1、y1、θ1为所述当前支撑位姿，依次表征在所述全局坐标系中x方向上的位移、y方向上的位移和转角；x2、y2、θ2为所述下一步预设的理想支撑位姿，依次表征在所述全局坐标系中x方向上的位移、y方向上的位移和转角；x3、y3、θ3依次为所述支撑脚之间的相对位姿的侧向位移、前向位移和转角位移。
进一步的，所述根据所述支撑脚之间的相对位姿和所述双足机器人初始状态时的左右踝关节的关节距离，计算得到所述双足机器人下一步的步态参数的步骤，包括：
识别所述当前支撑脚是否为右脚；
若所述当前支撑脚为右脚，则将所述支撑脚之间的相对位姿和所述关节距离代入第二算法中，计算得到所述下一步的步态参数，所述第二算法包括：
Figure 890907DEST_PATH_IMAGE004
，
Figure DEST_PATH_IMAGE005
，
Figure 96761DEST_PATH_IMAGE006
，其中，x4、y4、θ4依次为所述下一步的步态参数的侧向位移、前向位移和转角位移，a为所述关节距离；
若所述当前支撑脚为左脚，则将所述支撑脚之间的相对位姿和所述关节距离代入第三算法中，计算得到所述下一步的步态参数，所述第三算法包括：
Figure DEST_PATH_IMAGE007
，
Figure 961424DEST_PATH_IMAGE008
，
Figure 860110DEST_PATH_IMAGE006
。
本申请还提供了一种双足机器人的步态纠偏装置，包括：
获取模块，用于获取所述双足机器人当前支撑脚的当前支撑位姿；
第一计算模块，用于根据所述当前支撑位姿和所述双足机器人下一步预设的理想支撑位姿，计算得到所述双足机器人支撑脚之间的相对位姿，所述支撑脚之间的相对位姿为所述下一步预设的理想支撑位姿相对于所述当前支撑位姿的位姿；
第二计算模块，用于根据所述支撑脚之间的相对位姿和所述双足机器人初始状态时的左右踝关节的关节距离，计算得到所述双足机器人下一步的步态参数；
控制模块，用于控制所述双足机器人的下一支撑脚按照所述下一步的步态参数进行运动。
进一步的，所述获取模块，包括：
获取单元，用于获取所述双足机器人的质心在全局坐标系中的质心位姿，以及所述当前支撑脚在质心坐标系中的支撑位姿，所述质心坐标系是以所述双足机器人的质心为坐标原点构建的坐标系，所述全局坐标系对应所述双足机器人当前所处的环境全局；
变换单元，用于根据所述支撑位姿和所述质心位姿，通过坐标系变换得到所述当前支撑脚在所述全局坐标系中的所述当前支撑位姿。
进一步的，所述质心为所述双足机器人的两个髋关节之间连线的中点，所述获取单元，包括：
获取子单元，用于获取所述当前支撑脚的髋关节角度、膝关节角度、踝关节角度；
解析子单元，用于根据所述髋关节角度、所述膝关节角度、所述踝关节角度、所述当前支撑脚的大腿长度、所述当前支撑脚的小腿长度以及所述双足机器人的两个髋关节之间的距离，解析得到所述当前支撑脚在所述质心坐标系中的支撑位姿。
进一步的，所述控制模块，包括：
限幅单元，用于根据所述下一步预设的理想步态参数对所述下一步的步态参数进行限幅处理，得到修正步态参数；
第一控制单元，用于控制所述下一支撑脚按照所述修正步态参数进行运动。
进一步的，所述控制模块，还包括：
判断单元，用于判断所述下一支撑脚按照所述修正步态参数运动后，是否会发生碰撞；
修正单元，用于若所述下一支撑脚按照所述修正步态参数运动后会发生碰撞，则根据与所述下一支撑脚发生碰撞的物体的位姿，修正所述修正步态参数，得到二次修正步态参数；
第二控制单元，用于控制所述下一支撑脚按照所述二次修正步态参数进行运动，以避免发生碰撞。
进一步的，位姿包括前向位移、侧向位移和转角位移，所述第一计算模块，包括：
第一计算单元，用于分别将所述当前支撑位姿和所述下一步预设的理想支撑位姿代入第一算法中，计算得到所述支撑脚之间的相对位姿，所述第一算法包括：
Figure 695211DEST_PATH_IMAGE001
，
Figure 970334DEST_PATH_IMAGE002
，
Figure 348226DEST_PATH_IMAGE003
，其中，x1、y1、θ1为所述当前支撑位姿，依次表征在所述全局坐标系中x方向上的位移、y方向上的位移和转角；x2、y2、θ2为所述下一步预设的理想支撑位姿，依次表征在所述全局坐标系中x方向上的位移、y方向上的位移和转角；x3、y3、θ3依次为所述支撑脚之间的相对位姿的侧向位移、前向位移和转角位移。
进一步的，所述第二计算模块，包括：
识别单元，用于识别所述当前支撑脚是否为右脚；
第二计算单元，用于若所述当前支撑脚为右脚，则将所述支撑脚之间的相对位姿和所述关节距离代入第二算法中，计算得到所述下一步的步态参数，所述第二算法包括：
Figure 417813DEST_PATH_IMAGE004
，
Figure 756522DEST_PATH_IMAGE005
，
Figure 569757DEST_PATH_IMAGE006
，其中，x4、y4、θ4依次为所述下一步的步态参数的侧向位移、前向位移和转角位移，a为所述关节距离；
第三计算单元，用于若所述当前支撑脚为左脚，则将所述支撑脚之间的相对位姿和所述关节距离代入第三算法中，计算得到所述下一步的步态参数，所述第三算法包括：
Figure 802155DEST_PATH_IMAGE007
，
Figure 42643DEST_PATH_IMAGE008
，
Figure 852336DEST_PATH_IMAGE006
。
本申请还提供一种计算机设备，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器执行所述计算机程序时实现上述任一项所述方法的步骤。
本申请还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述任一项所述的方法的步骤。
本申请中提供的一种双足机器人的步态纠偏方法、装置和计算机设备，在运动过程中，系统获取双足机器人当前支撑脚的当前支撑位姿，然后根据当前支撑位姿和双足机器人下一步预设的理想支撑位姿，计算得到双足机器人支撑脚之间的相对位姿。系统根据支撑脚之间的相对位姿和双足机器人初始状态时的左右踝关节的关节距离，计算得到双足机器人下一步的步态参数。最后，系统控制双足机器人的下一支撑脚按照下一步的步态参数进行移动。在运动过程中，系统实时监测双足机器人的落脚点（即当前支撑脚的当前支撑位姿），并据此按照预设算法对双足机器人下一步的步态参数进行调整，从而实现在运动中实时纠正双足机器人的步态参数，避免运动误差造成的预设路径偏移，进而保证多双足机器人编队在运动过程中的协同性和时间线上的一致性。
附图说明
图1是本申请一实施例中双足机器人的步态纠偏方法步骤示意图；
图2是本申请一实施例中双足机器人在整个运动过程中的步态纠偏流程图；
图3是本申请一实施例中双足机器人的步态纠偏装置整体结构框图；
图4是本申请一实施例的计算机设备的结构示意框图。
本申请目的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
参照图1，本申请一实施例中提供了一种双足机器人的步态纠偏方法，包括：
S1:获取所述双足机器人当前支撑脚的当前支撑位姿；
S2:根据所述当前支撑位姿和所述双足机器人下一步预设的理想支撑位姿，计算得到所述双足机器人支撑脚之间的相对位姿，所述支撑脚之间的相对位姿为所述下一步预设的理想支撑位姿相对于所述当前支撑位姿的位姿；
S3:根据所述支撑脚之间的相对位姿和所述双足机器人初始状态时的左右踝关节的关节距离，计算得到所述双足机器人下一步的步态参数；
S4:控制所述双足机器人的下一支撑脚按照所述下一步的步态参数进行运动。
本实施例中，系统对应双足机器人当前所处的环境全局构建有全局坐标系，全局坐标系为固定不动（即不会随着双足机器人的运动而改变位置）双足机器人在当前所处的环境全局内按照预先设定的运动路径进行移动，因此全局坐标系可以以双足机器人在环境全局中的运动路径的起点为坐标系原点，双足机器人在环境全局中的运动路径在全局坐标系中以对应的点坐标表示。双足机器人上设置有相机，相机与双足机器人的质心（质量中心）之间的位置关系固定，即两者之间的坐标转换关系固定。因此，系统可以通过双足机器人运动时相机所拍摄得到的环境图像，计算得到相机在全局坐标系中的位姿（位姿包括在全局坐标系中y方向的位移、在全局坐标系中x方向的位移，转双足机器人的支撑脚在z方向上的转角），进而根据相机与双足机器人的质心之间的坐标转换关系，计算得到质心在全局坐标系中的位姿，即质心位姿。进一步的，系统内还构建有质心坐标系，质心坐标系是以双足机器人的质心为坐标系原点进行构建，随着双足机器人的运动而运动（质心坐标系的原点在双足机器人上，因此会随双足机器人的运动而改变其在全局坐标系中的位置）。本实施例中，双足机器人的质心设定为双足机器人的两个髋关节连线所在的中点位置。双足机器人的质心到当前支撑脚（当前支撑脚为双足机器人当前受力的脚，具体可参照人体走路时双脚的受力）的运动学变换可以当做多根连杆（即质心到当前支撑脚所对应的髋关节、当前支撑脚的大腿、小腿简易看成是连杆）连在一起，质心和当前支撑脚分别在双足机器人的首尾连杆的两端。系统通过各个连杆（双足机器人的关节）当前的角度得到当前支撑脚与质心之间的位置关系，从而得到当前支撑脚在质心坐标系中的支撑位姿。然后，系统根据当前支撑脚在质心坐标系中的支撑位姿、质心在全局坐标系中的质心位姿，通过坐标系转换得到当前支撑脚在全局坐标系中的当前支撑位姿。系统根据当前支撑位姿和双足机器人下一步预设的理想支撑位姿，计算得到双足机器人支撑脚之间的相对位姿。其中，双足机器人下一步预设的理想支撑位姿为双足机器人原先预设的运动路径中，下一步运动后双足机器人的下一支撑脚应该落脚的位姿；双足机器人支撑脚之间的相对位姿为双足机器人的下一支撑脚在下一步预设的理想支撑位姿的落脚点，相对于当前支撑脚在当前支撑位姿的落脚点的位姿。然后，系统根据双足机器人支撑脚之间的相对位姿和双足机器人处于初始状态时的左右踝关节的关节距离，计算得到双足机器人下一步的步态参数。在得到下一步的步态参数后，系统控制双足机器人的下一支撑脚按照下一步的步态参数进行运动，从而使得双足机器人的下一支撑脚落在预先期望的落脚点。本实施例中，通过在双足机器人下一步的支撑脚落脚前，根据预先规划的理想落脚点（即下一步预设的理想支撑位姿）和当前的实际落脚点进行相应的计算，从而对双足机器人的下一步的步态参数进行修正，实现误差补充，避免下一支撑脚的落脚点偏离预先规划的运动路径。
进一步的，所述获取所述双足机器人当前支撑脚的当前支撑位姿的步骤，包括：
S101:获取所述双足机器人的质心在全局坐标系中的质心位姿，以及所述当前支撑脚在质心坐标系中的支撑位姿，所述质心坐标系是以所述双足机器人的质心为坐标原点构建的坐标系，所述全局坐标系对应所述双足机器人当前所处的环境全局；
S102:根据所述支撑位姿和所述质心位姿，通过坐标系变换得到所述当前支撑脚在所述全局坐标系中的所述当前支撑位姿。
本实施例中，系统内部构建有全局坐标系和质心坐标系，其中，全局坐标系对应双足机器人当前所在的环境全局，可以以双足机器人预设的运动路径的起点为坐标系原点，用于表现双足机器人运动时在环境全局中的位置变化（即双足机器人在全局坐标系中的位姿可以表征双足机器人在环境全局中的位置）；质心坐标系是以双足机器人的质心为坐标系原点构建的坐标系，本实施例中双足机器人的质心为两个髋关节之间连线的中点。双足机器人上固定设置有相机，相机与双足机器人的质心之间的位置关系固定，即两者之间的坐标转换关系固定。因此，系统可以通过双足机器人运动时相机所拍摄得到的环境图像，计算得到相机在全局坐标系中的位姿，进而根据相机与双足机器人的质心之间的坐标转换关系，计算得到质心在全局坐标系中的位姿，即质心位姿。双足机器人的质心到当前支撑脚的运动学变换可以当做多根连杆（即质心到当前支撑脚所对应的髋关节、当前支撑脚的大腿、小腿简易看成是连杆）连在一起，质心和当前支撑脚分别在双足机器人的首尾连杆的两端。系统通过各个连杆（双足机器人的关节）当前的角度得到当前支撑脚与质心之间的位置关系，从而得到当前支撑脚在质心坐标系中的支撑位姿。具体地，系统获取当前支撑脚的髋关节角度、膝关节角度和踝关节；然后根据髋关节角度、膝关节角度、踝关节角度、当前支撑脚的大腿长度、当前支撑脚的小腿长度以及双足机器人的两个髋关节之间的距离，解析得到当前支撑脚在质心坐标系中的支撑位姿。然后，系统根据当前支撑脚在质心坐标系中的支撑位姿、质心在全局坐标系中的质心位姿，通过坐标系转换得到当前支撑脚在全局坐标系中的当前支撑位姿。
进一步的，所述质心为所述双足机器人的两个髋关节之间连线的中点，所述获取所述当前支撑脚在质心坐标系中的支撑位姿的步骤，包括：
S1011:获取所述当前支撑脚的髋关节角度、膝关节角度和踝关节角度；
S1012:根据所述髋关节角度、所述膝关节角度、所述踝关节角度、所述当前支撑脚的大腿长度、所述当前支撑脚的小腿长度以及所述双足机器人的两个髋关节之间的距离，解析得到所述当前支撑脚在所述质心坐标系中的支撑位姿。
本实施例中，双足机器人的质心为两个髋关节之间连线的中点，系统预先以质心为坐标系原点构建质心坐标系。双足机器人运动时，系统会自动记录双足机器人的两个脚的各关节的角度变化。系统自动获取当前支撑脚的髋关节角度和膝关节角度，其中，当前支撑脚包括大腿和小腿，髋关节角度与当前支撑脚的大腿与髋关节之间的夹角（或者说是大腿与躯干连接点之间的夹角），膝关节角度为当前支撑脚的大腿和小腿之间的夹角，踝关节角度为当前支撑脚的小腿和脚掌之间的夹角。由于质心坐标系的原点是两个髋关节之间连线的中点，因此可以根据双足两个髋关节之间的距离确定当前支撑脚所在的髋关节的位姿。在得到当前支撑脚所在的髋关节的位姿，系统可以根据髋关节角度和大腿长度，得到膝关节在质心坐标系中的位姿。进一步的，系统根据膝关节角度、小腿长度和踝关节，可以计算得到当前支撑脚的脚部在质心坐标系中的位姿，该位姿即为支撑位姿。本实施例中，大腿、小腿的长度可由开发人员预先测量后保存在系统内部数据库内，需要时可直接调用。
进一步的，所述控制所述双足机器人的下一支撑脚按照所述下一步的步态参数进行运动的步骤，包括：
S401:根据所述下一步预设的理想步态参数对所述下一步的步态参数进行限幅处理，得到修正步态参数；
S402:控制所述下一支撑脚按照所述修正步态参数进行运动。
本实施例中，系统可以根据支撑脚之间的相对位姿解析得到双足机器人的下一支撑脚在下一步运动中的步态参数。而为了避免计算值（即下一步的步态参数）与期望值（即理想步态参数，理想步态参数与下一步预设的理想支撑位姿相对应）之间具有太大的偏差，因此系统根据下一步预设的理想支撑位姿对下一步的步态参数进行限幅处理，得到修正步态参数。比如，理想步态参数为（4，5，15°），x和y方向的偏差限制在1cm, 转角方向的偏差限制在1°；如果下一步的步态参数为（2，6，18°），则限幅处理后的修正步态参数为（3，6，16°）。系统直接控制双足机器人的下一支撑脚按照修正步态参数进行运动，比如修正步态参数为（3，6，16°），则控制双足机器人的下一支撑脚往x方向移动3cm、往y方向移动6cm、并且在x方向的夹角为16°。
进一步的，所述根据所述下一步预设的理想步态参数对所述下一步的步态参数进行限幅处理，得到修正步态参数的步骤之后，包括：
S4021:判断所述下一支撑脚按照所述修正步态参数运动后，是否会发生碰撞；
S4022:若所述下一支撑脚按照所述修正步态参数运动后会发生碰撞，则根据与所述下一支撑脚发生碰撞的物体的位姿，修正所述修正步态参数，得到二次修正步态参数；
S4023:控制所述下一支撑脚按照所述二次修正步态参数进行运动，以避免发生碰撞。
本实施例中，系统在得到修正步态参数后，即对应得到了限幅后的双足机器人的下一支撑脚下一步的落脚点。系统可以通过双足机器人上的相机拍摄或者红外扫描等方式，查看当前环境中与修正步态参数所对应的位置是否有其他物体（比如障碍物或者其他机器人），从而判断双足机器人的下一支撑脚按照修正步态参数运动后，是否会与其他物体发生碰撞。如果系统查看到当前环境中与修正步态参数所对应的位置放置有其他物体，则判定双足机器人的下一支撑脚按照修正步态参数运动后会发生碰撞。此时，系统可以根据与下一支撑脚发生碰撞的物体的位姿，对修正步态参数进行再次修正，以使下一支撑脚在下一步的落脚点避开碰撞物体，得到二次修正步态参数。具体地，系统可以根据碰撞物体所占区域的位姿，对修正步态参数进行修正。比如碰撞物体所占区域的位姿为（4—6，9—12，0°），修正步态参数为（5，10，16°），则可以修正为二次修正步态参数（3，8，16°），从而避开碰撞物体所在的区域。系统控制双足机器人的下一支撑脚按照二次修正步态参数进行运动，以避免发生碰撞。
进一步的，步态参数包括前向位移、侧向位移和转角位移，所述根据所述当前支撑位姿和所述双足机器人下一步预设的理想支撑位姿，计算得到所述双足机器人支撑脚之间的相对位姿的步骤，包括：
S201:分别将所述当前支撑位姿和所述下一步预设的理想支撑位姿代入第一算法中，计算得到所述支撑脚之间的相对位姿，所述第一算法包括：
Figure DEST_PATH_IMAGE009
，
Figure 203683DEST_PATH_IMAGE010
，
Figure 165954DEST_PATH_IMAGE011
，其中，x1、y1、θ1为所述当前支撑位姿，依次表征在所述全局坐标系中x方向上的位移、y方向上的位移和转角；x2、y2、θ2为所述下一步预设的理想支撑位姿，依次表征在所述全局坐标系中x方向上的位移、y方向上的位移和转角；x3、y3、θ3依次为所述支撑脚之间的相对位姿的侧向位移、前向位移和转角位移。
本实施例中，系统在计算双足机器人支撑脚之间的相对位姿的过程中，分别将当前支撑位姿和下一步预设的理想支撑位姿代入第一算法中进行计算。其中，第一算法包括有三个计算公式，分别为：
Figure 842923DEST_PATH_IMAGE012
，
Figure 15279DEST_PATH_IMAGE002
，
Figure 170316DEST_PATH_IMAGE003
。x1、y1、θ1为当前支撑位姿的坐标值，依次表征在全局坐标系中x方向上的位移、y方向上的位移和转角；x2、y2、θ2为下一步预设的理想支撑位姿的坐标值，依次表征在所述全局坐标系中x方向上的位移、y方向上的位移和转角；x3、y3、θ3为支撑脚之间的相对位姿，分别对应支撑脚之间的相对位姿中的侧向位移、前向位移和转角位移。本实施例中，系统基于当前支撑位姿和下一步预设的理想支撑位姿，通过第一算法能够准确预测得到双足机器人的下一支撑脚在产生偏差后的支撑脚之间的相对位姿。
进一步的，所述根据所述支撑脚之间的相对位姿和所述双足机器人初始状态时的左右踝关节的关节距离，计算得到所述双足机器人下一步的步态参数的步骤，包括：
S301:识别所述当前支撑脚是否为右脚；
S302:若所述当前支撑脚为右脚，则将所述支撑脚之间的相对位姿和所述关节距离代入第二算法中，计算得到所述下一步的步态参数，所述第二算法包括：
Figure 501941DEST_PATH_IMAGE004
，
Figure 84232DEST_PATH_IMAGE005
，
Figure 743883DEST_PATH_IMAGE006
，其中x4、y4、θ4依次为所述下一步的步态参数的侧向位移、前向位移和转角位移，a为所述关节距离；
S303:若所述当前支撑脚为左脚，则将所述支撑脚之间的相对位姿和所述关节距离代入第三算法中，计算得到所述下一步的步态参数，所述第三算法包括：
Figure 312399DEST_PATH_IMAGE007
，
Figure 373896DEST_PATH_IMAGE008
，
Figure 392667DEST_PATH_IMAGE006
。
本实施例中，系统根据支撑脚之间的相对位姿，以及双足机器人处于初始状态时的左右踝关节的关节距离，通过第二算法或第三算法预测得到双足机器人下一支撑脚在下一步移动时对应的步态参数（即下一步的步态参数）。具体地，系统首先识别双足机器人的当前支撑脚是否为右脚。如果当前支撑脚为右脚，则调用第二算法，并将支撑脚之间的相对位姿和关节距离代入第二算法中进行计算。其中，第二算法包括三个计算公式：
Figure 539615DEST_PATH_IMAGE004
，
Figure 161089DEST_PATH_IMAGE013
，
Figure 77093DEST_PATH_IMAGE014
，x4、y4、θ4为下一步的步态参数，a为关节距离（关节距离的具体值可由开发人员预先测量后录入系统数据库内进行保存）。如果当前支撑脚为左脚，则系统调用第三算法，并将支撑脚之间的相对位姿和关节距离代入第三算法中进行计算。其中，第三算法包括三个计算公式：
Figure 266765DEST_PATH_IMAGE007
，
Figure 901009DEST_PATH_IMAGE015
，
Figure 802538DEST_PATH_IMAGE014
。本实施例中，系统基于上一步计算得到的双足机器人在支撑脚之间的相对位姿和关节距离，通过第二算法或第三算法的计算，能够对双足机器人在每一步移动时的步态参数的误差进行补偿，从而避免导航数据的误差对双足机器人的运动产生较大影响，避免运动误差造成的预设路径偏移。
参照图2，本实施例中，双足机器人在运动过程中的完整流程为：系统实时计算，获取双足机器人的相机在全局坐标系中的位姿，然后通过坐标系变换计算出双足机器人的质心在全局坐标系的位姿。系统在双足机器人的落脚瞬间通过运动学求解得到当前支撑脚的当前支撑位姿，然后实时判断双足机器人的当前支撑脚为左脚或右脚，从而识别到双足机器人在下一步将要落下的脚为（即下一支撑脚）为右脚或左脚，进而调取对应的算法，计算得到双足机器人支撑脚之间的相对位姿。系统根据支撑脚之间的相对位姿反算得到下一支撑脚在下一步对应的步态参数（即下一步的步态参数），并对下一步的步态参数进行限幅，得到修正步态参数。系统在双足机器人进入双足支撑期瞬间下发修正步态参数，从而确保机器人在抬腿的时候所需要的参数已经设定好了，使得双足机器人的下一支撑脚可以按照下发的步态参数进行运动。在双足机器人运动过程中，系统迭代循环上述的处理步骤，从而实现对双足机器人每一步的运动参数的误差补偿。
本实施例提供的一种双足机器人的步态纠偏方法，在运动过程中，系统获取双足机器人当前支撑脚的当前支撑位姿，然后根据当前支撑位姿和双足机器人下一步预设的理想支撑位姿，计算得到双足机器人支撑脚之间的相对位姿。系统根据支撑脚之间的相对位姿和双足机器人初始状态时的左右踝关节的关节距离，计算得到双足机器人下一步的步态参数。最后，系统控制双足机器人的下一支撑脚按照下一步的步态参数进行移动。在运动过程中，系统实时监测双足机器人的落脚点（即当前支撑脚的当前支撑位姿），并据此按照预设算法对双足机器人下一步的步态参数进行调整，从而实现在运动中实时纠正双足机器人的步态参数，避免运动误差造成的预设路径偏移，进而保证多双足机器人编队在运动过程中的协同性和时间线上的一致性。
参照图3，本申请一实施例中还提供了一种双足机器人的步态纠偏装置，包括：
获取模块1，用于获取所述双足机器人当前支撑脚的当前支撑位姿；
第一计算模块2，用于根据所述当前支撑位姿和所述双足机器人下一步预设的理想支撑位姿，计算得到所述双足机器人支撑脚之间的相对位姿，所述支撑脚之间的相对位姿为所述下一步预设的理想支撑位姿相对于所述当前支撑位姿的位姿；
第二计算模块3，用于根据所述支撑脚之间的相对位姿和所述双足机器人初始状态时的左右踝关节的关节距离，计算得到所述双足机器人下一步的步态参数；
控制模块4，用于控制所述双足机器人的下一支撑脚按照所述下一步的步态参数进行运动。
进一步的，所述获取模块1，包括：
获取单元，用于获取所述双足机器人的质心在全局坐标系中的质心位姿，以及所述当前支撑脚在质心坐标系中的支撑位姿，所述质心坐标系是以所述双足机器人的质心为坐标原点构建的坐标系，所述全局坐标系对应所述双足机器人当前所处的环境全局；
变换单元，用于根据所述支撑位姿和所述质心位姿，通过坐标系变换得到所述当前支撑脚在所述全局坐标系中的所述当前支撑位姿。
进一步的，所述质心为所述双足机器人的两个髋关节之间连线的中点，所述获取单元，包括：
获取子单元，用于获取所述当前支撑脚的髋关节角度、膝关节角度、踝关节角度；
解析子单元，用于根据所述髋关节角度、所述膝关节角度、所述踝关节角度、所述当前支撑脚的大腿长度、所述当前支撑脚的小腿长度以及所述双足机器人的两个髋关节之间的距离，解析得到所述当前支撑脚在所述质心坐标系中的支撑位姿。
进一步的，所述控制模块4，包括：
限幅单元，用于根据所述下一步预设的理想步态参数对所述下一步的步态参数进行限幅处理，得到修正步态参数；
第一控制单元，用于控制所述下一支撑脚按照所述修正步态参数进行运动。
进一步的，所述控制模块4，还包括：
判断单元，用于判断所述下一支撑脚按照所述修正步态参数运动后，是否会发生碰撞；
修正单元，用于若所述下一支撑脚按照所述修正步态参数运动后会发生碰撞，则根据与所述下一支撑脚发生碰撞的物体的位姿，修正所述修正步态参数，得到二次修正步态参数；
第二控制单元，用于控制所述下一支撑脚按照所述二次修正步态参数进行运动，以避免发生碰撞。
进一步的，位姿包括前向位移、侧向位移和转角位移，所述第一计算模块2，包括：
第一计算单元，用于分别将所述当前支撑位姿和所述下一步预设的理想支撑位姿代入第一算法中，计算得到所述支撑脚之间的相对位姿，所述第一算法包括：
Figure 573048DEST_PATH_IMAGE001
，
Figure 933622DEST_PATH_IMAGE002
，
Figure 179796DEST_PATH_IMAGE003
，其中，x1、y1、θ1为所述当前支撑位姿，依次表征在所述全局坐标系中x方向上的位移、y方向上的位移和转角；x2、y2、θ2为所述下一步预设的理想支撑位姿，依次表征在所述全局坐标系中x方向上的位移、y方向上的位移和转角；x3、y3、θ3依次为所述支撑脚之间的相对位姿的侧向位移、前向位移和转角位移。
进一步的，所述第二计算模块3，包括：
识别单元，用于识别所述当前支撑脚是否为右脚；
第二计算单元，用于若所述当前支撑脚为右脚，则将所述支撑脚之间的相对位姿和所述关节距离代入第二算法中，计算得到所述下一步的步态参数，所述第二算法包括：
Figure 18439DEST_PATH_IMAGE004
，
Figure 643455DEST_PATH_IMAGE005
，
Figure 50297DEST_PATH_IMAGE006
，其中，x4、y4、θ4依次为所述下一步的步态参数的侧向位移、前向位移和转角位移，a为所述关节距离；
第三计算单元，用于若所述当前支撑脚为左脚，则将所述支撑脚之间的相对位姿和所述关节距离代入第三算法中，计算得到所述下一步的步态参数，所述第三算法包括：
Figure 393554DEST_PATH_IMAGE007
，
Figure 301467DEST_PATH_IMAGE008
，
Figure 46569DEST_PATH_IMAGE006
。
本实施例中，步态纠偏装置中各模块、单元、子单元用于对应执行与上述双足机器人的步态纠偏方法中的各个步骤，其具体实施过程在此不做详述。
本实施例提供的一种双足机器人的步态纠偏装置，在运动过程中，系统获取双足机器人当前支撑脚的当前支撑位姿，然后根据当前支撑位姿和双足机器人下一步预设的理想支撑位姿，计算得到双足机器人支撑脚之间的相对位姿。系统根据支撑脚之间的相对位姿和双足机器人初始状态时的左右踝关节的关节距离，计算得到双足机器人下一步的步态参数。最后，系统控制双足机器人的下一支撑脚按照下一步的步态参数进行移动。在运动过程中，系统实时监测双足机器人的落脚点（即当前支撑脚的当前支撑位姿），并据此按照预设算法对双足机器人下一步的步态参数进行调整，从而实现在运动中实时纠正双足机器人的步态参数，避免运动误差造成的预设路径偏移，进而保证多双足机器人编队在运动过程中的协同性和时间线上的一致性。
参照图4，本申请实施例中还提供一种计算机设备，该计算机设备可以是服务器，其内部结构可以如图3所示。该计算机设备包括通过系统总线连接的处理器、存储器、网络接口和数据库。其中，该计算机设计的处理器用于提供计算和控制能力。该计算机设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统、计算机程序和数据库。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该计算机设备的数据库用于存储第一算法等数据。该计算机设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种双足机器人的步态纠偏方法。
上述处理器执行上述双足机器人的步态纠偏方法的步骤：
S1:获取所述双足机器人当前支撑脚的当前支撑位姿；
S2:根据所述当前支撑位姿和所述双足机器人下一步预设的理想支撑位姿，计算得到所述双足机器人支撑脚之间的相对位姿，所述支撑脚之间的相对位姿为所述下一步预设的理想支撑位姿相对于所述当前支撑位姿的位姿；
S3:根据所述支撑脚之间的相对位姿和所述双足机器人初始状态时的左右踝关节的关节距离，计算得到所述双足机器人下一步的步态参数；
S4:控制所述双足机器人的下一支撑脚按照所述下一步的步态参数进行运动。
进一步的，所述获取所述双足机器人当前支撑脚的当前支撑位姿的步骤，包括：
S101:获取所述双足机器人的质心在全局坐标系中的质心位姿，以及所述当前支撑脚在质心坐标系中的支撑位姿，所述质心坐标系是以所述双足机器人的质心为坐标原点构建的坐标系，所述全局坐标系对应所述双足机器人当前所处的环境全局；
S102:根据所述支撑位姿和所述质心位姿，通过坐标系变换得到所述当前支撑脚在所述全局坐标系中的所述当前支撑位姿。
进一步的，所述质心为所述双足机器人的两个髋关节之间连线的中点，所述获取所述当前支撑脚在质心坐标系中的支撑位姿的步骤，包括：
S1011:获取所述当前支撑脚的髋关节角度、膝关节角度和踝关节角度；
S1012:根据所述髋关节角度、所述膝关节角度、所述踝关节角度、所述当前支撑脚的大腿长度、所述当前支撑脚的小腿长度以及所述双足机器人的两个髋关节之间的距离，解析得到所述当前支撑脚在所述质心坐标系中的支撑位姿。
进一步的，所述控制所述双足机器人的下一支撑脚按照所述下一步的步态参数进行运动的步骤，包括：
S401:根据所述下一步预设的理想步态参数对所述下一步的步态参数进行限幅处理，得到修正步态参数；
S402:控制所述下一支撑脚按照所述修正步态参数进行运动。
进一步的，所述根据所述下一步预设的理想步态参数对所述下一步的步态参数进行限幅处理，得到修正步态参数的步骤之后，包括：
S4021:判断所述下一支撑脚按照所述修正步态参数运动后，是否会发生碰撞；
S4022:若所述下一支撑脚按照所述修正步态参数运动后会发生碰撞，则根据与所述下一支撑脚发生碰撞的物体的位姿，修正所述修正步态参数，得到二次修正步态参数；
S4023:控制所述下一支撑脚按照所述二次修正步态参数进行运动，以避免发生碰撞。
进一步的，步态参数包括前向位移、侧向位移和转角位移，所述根据所述当前支撑位姿和所述双足机器人下一步预设的理想支撑位姿，计算得到所述双足机器人支撑脚之间的相对位姿的步骤，包括：
S201:分别将所述当前支撑位姿和所述下一步预设的理想支撑位姿代入第一算法中，计算得到所述支撑脚之间的相对位姿，所述第一算法包括：
Figure 873580DEST_PATH_IMAGE009
，
Figure 438553DEST_PATH_IMAGE016
，
Figure 618999DEST_PATH_IMAGE011
，其中，x1、y1、θ1为所述当前支撑位姿，依次表征在所述全局坐标系中x方向上的位移、y方向上的位移和转角；x2、y2、θ2为所述下一步预设的理想支撑位姿，依次表征在所述全局坐标系中x方向上的位移、y方向上的位移和转角；x3、y3、θ3依次为所述支撑脚之间的相对位姿的侧向位移、前向位移和转角位移。
进一步的，所述根据所述支撑脚之间的相对位姿和所述双足机器人初始状态时的左右踝关节的关节距离，计算得到所述双足机器人下一步的步态参数的步骤，包括：
S301:识别所述当前支撑脚是否为右脚；
S302:若所述当前支撑脚为右脚，则将所述支撑脚之间的相对位姿和所述关节距离代入第二算法中，计算得到所述下一步的步态参数，所述第二算法包括：
Figure 359553DEST_PATH_IMAGE017
，
Figure 232831DEST_PATH_IMAGE018
，
Figure 550680DEST_PATH_IMAGE019
，其中x4、y4、θ4依次为所述下一步的步态参数的侧向位移、前向位移和转角位移，a为所述关节距离；
S303:若所述当前支撑脚为左脚，则将所述支撑脚之间的相对位姿和所述关节距离代入第三算法中，计算得到所述下一步的步态参数，所述第三算法包括：
Figure 534816DEST_PATH_IMAGE007
，
Figure 379144DEST_PATH_IMAGE008
，
Figure 157745DEST_PATH_IMAGE006
。
本申请一实施例还提供一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时实现一种双足机器人的步态纠偏方法，所述双足机器人的步态纠偏方法具体为：
S1:获取所述双足机器人当前支撑脚的当前支撑位姿；
S2:根据所述当前支撑位姿和所述双足机器人下一步预设的理想支撑位姿，计算得到所述双足机器人支撑脚之间的相对位姿，所述支撑脚之间的相对位姿为所述下一步预设的理想支撑位姿相对于所述当前支撑位姿的位姿；
S3:根据所述支撑脚之间的相对位姿和所述双足机器人初始状态时的左右踝关节的关节距离，计算得到所述双足机器人下一步的步态参数；
S4:控制所述双足机器人的下一支撑脚按照所述下一步的步态参数进行运动。
进一步的，所述获取所述双足机器人当前支撑脚的当前支撑位姿的步骤，包括：
S101:获取所述双足机器人的质心在全局坐标系中的质心位姿，以及所述当前支撑脚在质心坐标系中的支撑位姿，所述质心坐标系是以所述双足机器人的质心为坐标原点构建的坐标系，所述全局坐标系对应所述双足机器人当前所处的环境全局；
S102:根据所述支撑位姿和所述质心位姿，通过坐标系变换得到所述当前支撑脚在所述全局坐标系中的所述当前支撑位姿。
进一步的，所述质心为所述双足机器人的两个髋关节之间连线的中点，所述获取所述当前支撑脚在质心坐标系中的支撑位姿的步骤，包括：
S1011:获取所述当前支撑脚的髋关节角度、膝关节角度和踝关节角度；
S1012:根据所述髋关节角度、所述膝关节角度、所述踝关节角度、所述当前支撑脚的大腿长度、所述当前支撑脚的小腿长度以及所述双足机器人的两个髋关节之间的距离，解析得到所述当前支撑脚在所述质心坐标系中的支撑位姿。
进一步的，所述控制所述双足机器人的下一支撑脚按照所述下一步的步态参数进行运动的步骤，包括：
S401:根据所述下一步预设的理想步态参数对所述下一步的步态参数进行限幅处理，得到修正步态参数；
S402:控制所述下一支撑脚按照所述修正步态参数进行运动。
进一步的，所述根据所述下一步预设的理想步态参数对所述下一步的步态参数进行限幅处理，得到修正步态参数的步骤之后，包括：
S4021:判断所述下一支撑脚按照所述修正步态参数运动后，是否会发生碰撞；
S4022:若所述下一支撑脚按照所述修正步态参数运动后会发生碰撞，则根据与所述下一支撑脚发生碰撞的物体的位姿，修正所述修正步态参数，得到二次修正步态参数；
S4023:控制所述下一支撑脚按照所述二次修正步态参数进行运动，以避免发生碰撞。
进一步的，步态参数包括前向位移、侧向位移和转角位移，所述根据所述当前支撑位姿和所述双足机器人下一步预设的理想支撑位姿，计算得到所述双足机器人支撑脚之间的相对位姿的步骤，包括：
S201:分别将所述当前支撑位姿和所述下一步预设的理想支撑位姿代入第一算法中，计算得到所述支撑脚之间的相对位姿，所述第一算法包括：
Figure 962889DEST_PATH_IMAGE009
，
Figure 626083DEST_PATH_IMAGE020
，
Figure 200284DEST_PATH_IMAGE011
，其中，x1、y1、θ1为所述当前支撑位姿，依次表征在所述全局坐标系中x方向上的位移、y方向上的位移和转角；x2、y2、θ2为所述下一步预设的理想支撑位姿，依次表征在所述全局坐标系中x方向上的位移、y方向上的位移和转角；x3、y3、θ3依次为所述支撑脚之间的相对位姿的侧向位移、前向位移和转角位移。
进一步的，所述根据所述支撑脚之间的相对位姿和所述双足机器人初始状态时的左右踝关节的关节距离，计算得到所述双足机器人下一步的步态参数的步骤，包括：
S301:识别所述当前支撑脚是否为右脚；
S302:若所述当前支撑脚为右脚，则将所述支撑脚之间的相对位姿和所述关节距离代入第二算法中，计算得到所述下一步的步态参数，所述第二算法包括：
Figure 415365DEST_PATH_IMAGE021
，
Figure 301281DEST_PATH_IMAGE022
，
Figure 892799DEST_PATH_IMAGE019
，其中x4、y4、θ4依次为所述下一步的步态参数的侧向位移、前向位移和转角位移，a为所述关节距离；
S303:若所述当前支撑脚为左脚，则将所述支撑脚之间的相对位姿和所述关节距离代入第三算法中，计算得到所述下一步的步态参数，所述第三算法包括：
Figure 321507DEST_PATH_IMAGE007
，
Figure 707489DEST_PATH_IMAGE008
，
Figure 359662DEST_PATH_IMAGE006
。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储与一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的和实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可以包括只读存储器（ROM）、可编程ROM（PROM）、电可编程ROM（EPROM）、电可擦除可编程ROM（EEPROM）或闪存。易失性存储器可包括随机存取存储器（RAM）或者外部高速缓冲存储器。作为说明而非局限，RAM通过多种形式可得，诸如静态RAM（SRAM）、动态RAM（DRAM）、同步DRAM（SDRAM）、双速据率SDRAM（SSRSDRAM）、增强型SDRAM（ESDRAM）、同步链路（Synchlink）DRAM（SLDRAM）、存储器总线（Rambus）直接RAM（RDRAM）、直接存储器总线动态RAM（DRDRAM）、以及存储器总线动态RAM（RDRAM）等。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其它变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、装置、物品或者方法不仅包括那些要素，而且还包括没有明确列出的其它要素，或者是还包括为这种过程、装置、物品或者方法所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、装置、物品或者方法中还存在另外的相同要素。
以上所述仅为本申请的优选实施例，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其它相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (9)

1.一种双足机器人的步态纠偏方法，其特征在于，包括：
获取所述双足机器人当前支撑脚的当前支撑位姿；
根据所述当前支撑位姿和所述双足机器人下一步预设的理想支撑位姿，计算得到所述双足机器人的支撑脚之间的相对位姿，所述支撑脚之间的相对位姿为所述下一步预设的理想支撑位姿相对于所述当前支撑位姿的位姿；
根据所述支撑脚之间的相对位姿和所述双足机器人初始状态时的左右踝关节的关节距离，计算得到所述双足机器人下一步的步态参数；
控制所述双足机器人的下一支撑脚按照所述下一步的步态参数进行运动；
所述控制所述双足机器人的下一支撑脚按照所述下一步的步态参数进行运动的步骤，包括：
根据所述下一步预设的理想步态参数对所述下一步的步态参数进行限幅处理，得到修正步态参数；
控制所述下一支撑脚按照所述修正步态参数进行运动。
2.根据权利要求1所述的双足机器人的步态纠偏方法，其特征在于，所述获取所述双足机器人当前支撑脚的当前支撑位姿的步骤，包括：
获取所述双足机器人的质心在全局坐标系中的质心位姿，以及所述当前支撑脚在质心坐标系中的支撑位姿，所述质心坐标系是以所述双足机器人的质心为坐标原点构建的坐标系，所述全局坐标系对应所述双足机器人当前所处的环境全局；
根据所述支撑位姿和所述质心位姿，通过坐标系变换得到所述当前支撑脚在所述全局坐标系中的所述当前支撑位姿。
3.根据权利要求2所述的双足机器人的步态纠偏方法，其特征在于，所述质心为所述双足机器人的两个髋关节之间连线的中点，所述获取所述当前支撑脚在质心坐标系中的支撑位姿的步骤，包括：
获取所述当前支撑脚的髋关节角度、膝关节角度和踝关节角度；
根据所述髋关节角度、所述膝关节角度、所述踝关节角度、所述当前支撑脚的大腿长度、所述当前支撑脚的小腿长度以及所述双足机器人的两个髋关节之间的距离，解析得到所述当前支撑脚在所述质心坐标系中的支撑位姿。
4.根据权利要求1所述的双足机器人的步态纠偏方法，其特征在于，所述根据所述下一步预设的理想步态参数对所述下一步的步态参数进行限幅处理，得到修正步态参数的步骤之后，包括：
判断所述下一支撑脚按照所述修正步态参数运动后，是否会发生碰撞；
若所述下一支撑脚按照所述修正步态参数运动后会发生碰撞，则根据与所述下一支撑脚发生碰撞的物体的位姿，修正所述修正步态参数，得到二次修正步态参数；
控制所述下一支撑脚按照所述二次修正步态参数进行运动，以避免发生碰撞。
5.根据权利要求2所述的双足机器人的步态纠偏方法，其特征在于，步态参数包括前向位移、侧向位移和转角位移，所述根据所述当前支撑位姿和所述双足机器人下一步预设的理想支撑位姿，计算得到所述双足机器人的支撑脚之间的相对位姿的步骤，包括：
分别将所述当前支撑位姿和所述下一步预设的理想支撑位姿代入第一算法中，计算得到所述支撑脚之间的相对位姿，所述第一算法包括：
Figure 897435DEST_PATH_IMAGE001
Figure 558224DEST_PATH_IMAGE002
，其中，x1、y1、θ1为所述当前支撑位姿，依次表征在所述全局坐标系中x方向上的位移、y方向上的位移和转角；x2、y2、θ2为所述下一步预设的理想支撑位姿，依次表征在所述全局坐标系中x方向上的位移、y方向上的位移和转角；x3、y3、θ3依次为所述支撑脚之间的相对位姿的侧向位移、前向位移和转角位移。
6.根据权利要求5所述的双足机器人的步态纠偏方法，其特征在于，所述根据所述支撑脚之间的相对位姿和所述双足机器人初始状态时的左右踝关节的关节距离，计算得到所述双足机器人下一步的步态参数的步骤，包括：
识别所述当前支撑脚是否为右脚；
若所述当前支撑脚为右脚，则将所述支撑脚之间的相对位姿和所述关节距离代入第二算法中，计算得到所述下一步的步态参数，所述第二算法包括：
Figure 638175DEST_PATH_IMAGE003
，其中，x4、y4、θ4依次为所述下一步的步态参数的侧向位移、前向位移和转角位移，a为所述关节距离；
若所述当前支撑脚为左脚，则将所述支撑脚之间的相对位姿和所述关节距离代入第三算法中，计算得到所述下一步的步态参数，所述第三算法包括：
Figure 742529DEST_PATH_IMAGE004
。
7.一种双足机器人的步态纠偏装置，其特征在于，包括：
获取模块，用于获取所述双足机器人当前支撑脚的当前支撑位姿；
第一计算模块，用于根据所述当前支撑位姿和所述双足机器人下一步预设的理想支撑位姿，计算得到所述双足机器人支撑脚之间的相对位姿，所述支撑脚之间的相对位姿为所述下一步预设的理想支撑位姿相对于所述当前支撑位姿的位姿；
第二计算模块，用于根据所述支撑脚之间的相对位姿和所述双足机器人初始状态时的左右踝关节的关节距离，计算得到所述双足机器人下一步的步态参数；
控制模块，用于控制所述双足机器人的下一支撑脚按照所述下一步的步态参数进行运动；
所述控制模块，包括：
限幅单元，用于根据所述下一步预设的理想步态参数对所述下一步的步态参数进行限幅处理，得到修正步态参数；
第一控制单元，用于控制所述下一支撑脚按照所述修正步态参数进行运动。
8.一种计算机设备，包括存储器和处理器，所述存储器中存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至6中任一项所述方法的步骤。
9.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至6中任一项所述的方法的步骤。
CN202011284461.8A 2020-11-17 2020-11-17 双足机器人的步态纠偏方法、装置和计算机设备 Active CN112091984B (zh)

Priority Applications (3)

Application Number Priority Date Filing Date Title
CN202011284461.8A CN112091984B (zh) 2020-11-17 2020-11-17 双足机器人的步态纠偏方法、装置和计算机设备
PCT/CN2020/140576 WO2022105025A1 (zh) 2020-11-17 2020-12-29 双足机器人的步态纠偏方法、装置和计算机设备
US17/314,039 US11878426B2 (en) 2020-11-17 2021-05-06 Biped robot gait control method and robot and computer readable storage medium using the same

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011284461.8A CN112091984B (zh) 2020-11-17 2020-11-17 双足机器人的步态纠偏方法、装置和计算机设备

Publications (2)

Publication Number Publication Date
CN112091984A CN112091984A (zh) 2020-12-18
CN112091984B true CN112091984B (zh) 2021-04-20

Family

ID=73784740

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011284461.8A Active CN112091984B (zh) 2020-11-17 2020-11-17 双足机器人的步态纠偏方法、装置和计算机设备

Country Status (3)

Country Link
US (1) US11878426B2 (zh)
CN (1) CN112091984B (zh)
WO (1) WO2022105025A1 (zh)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112091984B (zh) * 2020-11-17 2021-04-20 深圳市优必选科技股份有限公司 双足机器人的步态纠偏方法、装置和计算机设备
CN112711259B (zh) * 2020-12-28 2024-04-02 深圳市优必选科技股份有限公司 动态生成脚印集方法、装置、存储介质及双足机器人
CN113002659B (zh) * 2021-04-21 2022-02-08 乐聚(深圳)机器人技术有限公司 双足机器人的姿态测量方法、装置、设备及存储介质
CN113246125B (zh) * 2021-04-30 2022-09-16 深圳市优必选科技股份有限公司 机器人控制方法、装置、计算机可读存储介质及机器人
CN113253727A (zh) * 2021-05-13 2021-08-13 乐聚(深圳)机器人技术有限公司 一种机器人的轨迹确定方法、装置、设备及存储介质
CN113830197B (zh) * 2021-11-23 2022-04-05 之江实验室 一种应用于双足机器人动态行走的平衡控制方法
CN114432098A (zh) * 2022-01-27 2022-05-06 中山大学附属第一医院 一种基于模型的步态矫正装置
CN117572877A (zh) * 2024-01-16 2024-02-20 科大讯飞股份有限公司 一种双足机器人步态控制方法、装置、存储介质及设备

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4834200A (en) * 1986-12-15 1989-05-30 Agency Of Industrial Science & Technology Method and apparatus for dynamic walking control of robot
CN105511465A (zh) * 2015-12-02 2016-04-20 歌尔声学股份有限公司 一种双足机器人的步态控制方法和装置
JP2016117114A (ja) * 2014-12-19 2016-06-30 株式会社ジェイテクト 二足歩行ロボットの制御方法
CN109987169A (zh) * 2017-12-29 2019-07-09 深圳市优必选科技有限公司 双足机器人的步态控制方法、装置、终端设备及介质
CN110315543A (zh) * 2019-07-29 2019-10-11 北京理工大学 一种双足机器人步态生成与优化方法

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US5337235A (en) * 1992-03-12 1994-08-09 Honda Giken Kogyo Kabushiki Kaisha Locomotion control system for legged mobiled robot
WO2005068136A1 (ja) * 2004-01-13 2005-07-28 Honda Motor Co., Ltd. 移動ロボットの歩容生成装置
KR101493384B1 (ko) * 2008-12-22 2015-02-13 삼성전자 주식회사 로봇 및 그 균형 제어방법
KR20120069924A (ko) * 2010-12-21 2012-06-29 삼성전자주식회사 보행 로봇 및 그 제어 방법
CN105608309B (zh) * 2015-12-11 2019-08-02 杭州南江机器人股份有限公司 一种双足机器人步行规划和控制方法
CN110243360B (zh) * 2018-03-08 2022-02-22 深圳市优必选科技有限公司 机器人在运动区域的地图构建及定位方法
CN112091984B (zh) * 2020-11-17 2021-04-20 深圳市优必选科技股份有限公司 双足机器人的步态纠偏方法、装置和计算机设备

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4834200A (en) * 1986-12-15 1989-05-30 Agency Of Industrial Science & Technology Method and apparatus for dynamic walking control of robot
JP2016117114A (ja) * 2014-12-19 2016-06-30 株式会社ジェイテクト 二足歩行ロボットの制御方法
CN105511465A (zh) * 2015-12-02 2016-04-20 歌尔声学股份有限公司 一种双足机器人的步态控制方法和装置
CN109987169A (zh) * 2017-12-29 2019-07-09 深圳市优必选科技有限公司 双足机器人的步态控制方法、装置、终端设备及介质
CN110315543A (zh) * 2019-07-29 2019-10-11 北京理工大学 一种双足机器人步态生成与优化方法

Also Published As

Publication number Publication date
WO2022105025A1 (zh) 2022-05-27
CN112091984A (zh) 2020-12-18
US20220152827A1 (en) 2022-05-19
US11878426B2 (en) 2024-01-23

Similar Documents

Publication Publication Date Title
CN112091984B (zh) 双足机器人的步态纠偏方法、装置和计算机设备
CN111377004B (zh) 一种双足机器人步态控制方法以及双足机器人
US8392346B2 (en) Reinforcement learning system
JP4480843B2 (ja) 脚式移動ロボット及びその制御方法、並びに、脚式移動ロボット用相対移動測定センサ
US20230055206A1 (en) Legged robot motion control method, apparatus, and device, and medium
CN112964196A (zh) 三维扫描方法、系统、电子装置和计算机设备
JP6498597B2 (ja) 移動ロボットの制御装置
CN112698650A (zh) 仿人机器人的类人步态控制方法、装置、设备及存储介质
US7363109B2 (en) Backlash compensation control method, backlash compensation controller and backlash compensation control program
CN112051797A (zh) 足式机器人运动控制方法、装置、设备及介质
CN111438688B (zh) 机器人校正方法、装置、计算机设备及存储介质
US8942847B2 (en) Mobile apparatus and robot, and control system thereof
CN112405504B (zh) 外骨骼机器人
US20230076589A1 (en) Controlling foot landing points and step order of legged robot based on foot contact force
JP2024502726A (ja) 外骨格を動かす方法
CN113002659B (zh) 双足机器人的姿态测量方法、装置、设备及存储介质
Bertrand et al. Closed-form solutions of inverse kinematic models for the control of a biped robot with 8 active degrees of freedom per leg
Chen et al. Lower-body control of humanoid robot NAO via Kinect
US7904203B2 (en) Apparatus and method for robot control
US8761926B2 (en) Apparatus and method for robot control
CN113467484B (zh) 双足机器人步态控制方法、装置和计算机设备
Ficht et al. Fast whole-body motion control of humanoid robots with inertia constraints
WO2022141268A1 (zh) 一种仿人机器人控制方法、计算机设备及存储介质
CN113253748B (zh) 并联构型仿人机器人的状态估计方法、装置、设备及介质
KR100927572B1 (ko) 칼만 필터를 이용한 2족보행로봇의 균형제어시스템 및 그제어방법

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant