CN117572768A - 足式机器人规划与控制方法、装置、机器人及存储介质 - Google Patents

足式机器人规划与控制方法、装置、机器人及存储介质 Download PDF

Info

Publication number
CN117572768A
CN117572768A CN202311511514.9A CN202311511514A CN117572768A CN 117572768 A CN117572768 A CN 117572768A CN 202311511514 A CN202311511514 A CN 202311511514A CN 117572768 A CN117572768 A CN 117572768A
Authority
CN
China
Prior art keywords
foot
collision
leg
constraint
robot
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.)
Pending
Application number
CN202311511514.9A
Other languages
English (en)
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.)
Tsinghua University
Original Assignee
Tsinghua University
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 Tsinghua University filed Critical Tsinghua University
Priority to CN202311511514.9A priority Critical patent/CN117572768A/zh
Publication of CN117572768A publication Critical patent/CN117572768A/zh
Pending legal-status Critical Current

Links

Classifications

    • 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
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

  • Engineering & Computer Science (AREA)
  • Health & Medical Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Computation (AREA)
  • Medical Informatics (AREA)
  • Software Systems (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Manipulator (AREA)

Abstract

本申请涉及机器人技术领域，特别涉及一种足式机器人规划与控制方法、装置、机器人及存储介质，其中，方法包括：根据足式机器人的运动状态和每条腿的下一时刻摆动参数确定每条腿的足端参考状态，对于下一时刻开始摆动的腿，根据足端动力学模型和离散碰撞模型规划具有碰撞意识的摆动腿轨迹；根据运动状态、参考状态序列和足端参考状态对足式机器人进行全身控制，其中，对于在规划上应为支撑腿、且该支撑腿未触地，根据全身动力学模型和离散碰撞模型进行具有碰撞意识的全身控制。由此，解决了相关技术中足式机器人碰撞的被动柔性缓冲有限，算法控制计算量大、控制精度较低，使得机器人硬件磨损程度较高、使用寿命较短，无法满足实际需要等问题。

Description

足式机器人规划与控制方法、装置、机器人及存储介质
技术领域
本申请涉及机器人技术领域，特别涉及一种足式机器人规划与控制方法、装置、机器人及存储介质。
背景技术
足式机器人发展迅速，许多足式机器人已经不再局限于静态行走，而是朝着更快、更灵敏、更鲁棒、更安全、更多功能等方向发展。当足式机器人执行高动态运动时，其足底与地面的碰撞问题不容忽视。
相关技术中，可以从硬件方面和控制算法方面对足式机器人进行调节。在硬件方面，可以选择在机器人的足底安装具有一定柔性的脚垫，或者在机器人的驱动关节处安装弹簧阻尼等缓冲装置，还可以采用VIA(Variable ImpedanceActuators，可变阻抗执行器)或VSA(Variable Stiffness Actuators，可变刚度执行器)提供期望的被动机械阻抗，在机械层面实现改变关节阻抗的效果，从而可以根据实际需求调节缓冲效果；在控制算法方面，可以在运动规划中让足底与地面的接触速度接近于0。
然而，相关技术的方法存在一定不足：硬件方面，材料的引入会对机器人系统响应频率产生影响，且由于材料的刚度、阻尼、惯量特性是固定的，因而无法根据实际需求调节缓冲效果；使用VIA或VSA时整体结构过于复杂，体积和重量都较大，很难将这种执行器应用于需要做高动态运动的足式机器人身上。控制算法方面，足式机器人进行高动态的运动时，摆动腿的实际运动轨迹与规划轨迹可能存在明显误差，从而造成足底提前触地或者滞后触地，可能会产生巨大的足底力冲击，使得关节的零位漂移，加速硬件老化等。
发明内容
本申请提供一种足式机器人规划与控制方法、装置、机器人及存储介质，以解决相关技术中足式机器人碰撞的被动柔性缓冲有限，算法控制计算量大、控制精度较低，使得机器人硬件磨损程度较高、使用寿命较短，无法满足实际应用需要等问题。
本申请第一方面实施例提供一种足式机器人规划与控制方法，包括以下步骤：获取足式机器人的运动指令和运动状态；根据所述运动指令和所述运动状态生成所述足式机器人的参考状态序列和每条腿的下一时刻摆动参数；根据所述运动状态和每条腿的下一时刻摆动参数确定每条腿的足端参考状态，对于下一时刻开始摆动的腿，根据足端动力学模型和离散碰撞模型规划具有碰撞意识的摆动腿轨迹，使得提前碰撞后产生的影响在目标约束范围内；根据所述运动状态、所述参考状态序列和所述足端参考状态对所述足式机器人进行全身控制，其中，对于在规划上应为支撑腿、且该支撑腿未触地，根据全身动力学模型和离散碰撞模型进行具有碰撞意识的全身控制，使得滞后碰撞后产生的影响在目标约束范围内。
可选地，所述足端动力学模型为：
其中，为第l条腿足端在惯性系I中的线加速度，Λc,l为操作空间中的质量矩阵，hc,l为包含了操作空间中的科里奥利力、向心力和重力的项，是驱动关节的力矩τj映射到操作空间后的足端驱动力；所述离散碰撞模型为：
其中，是在碰撞法向量上的投影算子，是碰撞面的单位法向量；分别是物体1碰撞前和碰撞后的速度，分别是物体2碰撞前和碰撞后的速度，cr为恢复系数。
可选地，所述根据足端动力学模型和离散碰撞模型规划具有碰撞意识的摆动腿轨迹，包括：获取预先制定的状态量和目标约束条件；根据所述足端动力学模型、所述预先制定的状态量和所述目标约束条件构建下一时刻开始摆动的腿的连续时间轨迹优化问题；对所述连续时间轨迹优化问题进行离散化，得到摆动腿足端离散时间轨迹优化问题，求解该问题得到具有碰撞意识的摆动腿轨迹。
可选地，所述预先制定的状态量包括摆动腿足端的初始状态、摆动腿足端的落地状态、摆动的持续时间和摆动腿足端的期望最大离地高度中的一个或多个，所述目标约束条件包括状态方程等式约束、驱动力不等式约束、轨迹高度不等式约束、初始状态等式约束、终端状态等式约束、碰撞意识不等式约束和末段速度方向不等式约束中的一个或多个。
可选地，所述连续时间轨迹优化问题的目标函数为：
其中，和是维度各不相同的权重矩阵(对角矩阵居多)；||a||Q＝aTQa表示权重矩阵Q下向量a的加权模长；反映了摆动腿足端落地状态的任务，和分别是已知的摆动腿足端终端点位置和线速度；是最小化整个摆动过程的控制量(即线加速度)，t0＝0,tF＝TSW，TSW为摆动的持续时间；反映了摆动腿离地最大高度任务，th1和th2两个参数指定了达到最大高度的时间段，满足t0≤th1≤th2≤tF；zc是状态量的第3个分量，即状态量中表示足端离地高度的分量；所述摆动腿足端离散时间轨迹优化问题的目标函数为：
fmin≤Λcuk+hc≤fmax,k＝0,1,…,N-1，
zc,min≤Szxk≤zc,max,k＝1,2,…,N-1，
其中，是由各帧的控制量组成的向量； 是维度各不相同的对角权重矩阵；当前时刻足端状态(包括位置和速度)反馈参考的终端足端状态(包括位置和速度) 是足端线速度的选择矩阵；和为人为指定的维持摆动高度的起始帧与终止帧，满足kimp是开启碰撞意识约束和末端速度方向约束的起始帧，满足0≤kimp≤N。
可选地，所述全身控制的优化问题为：
s.t.lbj≤Cjχ≤ubj,j＝1,2,…,nconstraint，
其中，Ai是任务矩阵，bi是任务向量，Cj是约束矩阵，lbj和ubj是约束下边界和上边界，Wi为权重矩阵，ntask为任务的数量，nconstraint为约束的数量。
可选地，所述全身控制同时处理的任务包括躯干轨迹跟踪任务、足端轨迹跟踪任务、足底力跟踪任务、最小化关节力矩变化量任务、最小化足底力变化量任务中的多个；所述具有碰撞意识的全身控制同时处理的约束包括：浮动基动力学等式约束、足底力不等式约束、关节输出力矩饱和不等式约束、关节转速饱和不等式约束、关节输出功率饱和不等式约束和碰撞意识约束中的多个。
可选地，所述根据所述运动状态、所述参考状态序列和所述足端参考状态对所述足式机器人进行全身控制，包括：若规划上应为摆动腿，则设置足端轨迹跟踪任务为跟踪所述具有碰撞意识的摆动腿轨迹、所述足底力跟踪任务的目标足底力为预设值、所述足底力不等式约束中足底力约束为预设值和禁用所述碰撞意识约束；若规划上应为支撑腿、且该支撑腿触地，则设置所述足端轨迹跟踪任务的目标线加速度为预设值、所述足底力跟踪任务的目标足底力为MPC(Model Predictive Control，模型预测控制)或其他模块提供的足底力、足底力约束为摩擦锥约束和禁用所述碰撞意识约束；若规划上应为支撑腿、且该支撑腿未触地，则设置所述足端轨迹跟踪任务的目标为跟踪一个指向撞击面的速度、所述足底力跟踪任务的目标足底力为预设值、所述足底力约束为预设值和启用所述碰撞意识约束。
可选地，所述全身动力学模型为：
其中，是广义质量矩阵，是包含了科里奥利力、向心力和重力的项，表示驱动关节的输出力矩；0n×m表示大小为n×m的零矩阵；和Fc分别是每个支撑腿的接触雅可比矩阵堆叠而成的增广雅可比矩阵和地面对每个支撑腿足底的反作用力堆叠而成的增广足底力；qg为广义关节空间位置，为广义关节空间速度，为广义关节空间加速度；所述碰撞意识约束矩阵和向量的表达式为：
其中，分别是人为指定的可以允许的碰撞冲量的下限和上限，δt表达当前时刻与下一时刻的时间间隔。
本申请第二方面实施例提供一种足式机器人规划与控制装置，包括：获取模块，用于获取足式机器人的运动指令和运动状态；生成模块，用于根据所述运动指令和所述运动状态生成所述足式机器人的参考状态序列和每条腿的下一时刻摆动参数；规划模块，用于根据所述运动状态和每条腿的下一时刻摆动参数确定每条腿的足端参考状态，对于下一时刻开始摆动的腿，根据足端动力学模型和离散碰撞模型规划具有碰撞意识的摆动腿轨迹，使得提前碰撞后产生的影响在目标约束范围内；控制模块，用于根据所述运动状态、所述参考状态序列和所述足端参考状态对所述足式机器人进行全身控制，其中，对于在规划上应为支撑腿、且该支撑腿未触地，根据全身动力学模型和离散碰撞模型进行具有碰撞意识的全身控制，使得滞后碰撞后产生的影响在目标约束范围内。
可选地，所述足式机器人规划与控制装置的足端动力学模型为：
其中，为第l条腿足端在惯性系I中的线加速度，Λc,l为操作空间中的质量矩阵，hc,l为包含了操作空间中的科里奥利力、向心力和重力的项，是驱动关节的力矩τj映射到操作空间后的足端驱动力；所述离散碰撞模型为：
其中，是在碰撞法向量上的投影算子，是碰撞面的单位法向量；分别是物体1碰撞前和碰撞后的速度，分别是物体2碰撞前和碰撞后的速度，cr为恢复系数。
可选地，所述规划模块进一步用于：获取预先制定的状态量和目标约束条件；根据所述足端动力学模型、所述预先制定的状态量和所述目标约束条件构建下一时刻开始摆动的腿的连续时间轨迹优化问题；对所述连续时间轨迹优化问题进行离散化，得到摆动腿足端离散时间轨迹优化问题，求解该问题得到具有碰撞意识的摆动腿轨迹。
可选地，所述规划模块的预先制定的状态量包括摆动腿足端的初始状态、摆动腿足端的落地状态、摆动的持续时间和摆动腿足端的期望最大离地高度中的一个或多个，所述目标约束条件包括状态方程等式约束、驱动力不等式约束、轨迹高度不等式约束、初始状态等式约束、终端状态等式约束、碰撞意识不等式约束和末段速度方向不等式约束中的一个或多个。
可选地，所述规划模块的连续时间轨迹优化问题的目标函数为：
其中，和是维度各不相同的权重矩阵(对角矩阵居多)；||a||Q＝aTQa表示权重矩阵Q下向量a的加权模长；反映了摆动腿足端落地状态的任务，和分别是已知的摆动腿足端终端点位置和线速度；是最小化整个摆动过程的控制量(即线加速度)，t0＝0,tF＝TSW，TSW为摆动的持续时间；反映了摆动腿离地最大高度任务，th1和th2两个参数指定了达到最大高度的时间段，满足t0≤th1≤th2≤tF。zc是状态量的第3个分量，即状态量中表示足端离地高度的分量；所述摆动腿足端离散时间轨迹优化问题的目标函数为：
fmin≤Λcuk+hc≤fmax,k＝0,1,…,N-1，
zc,min≤Szxk≤zc,max,k＝1,2,…,N-1，
其中，是由各帧的控制量组成的向量； 是维度各不相同的对角权重矩阵；当前时刻足端状态(包括位置和速度)反馈参考的终端足端状态(包括位置和速度) 是足端线速度的选择矩阵；和为人为指定的维持摆动高度的起始帧与终止帧，满足kimp是开启碰撞意识约束和末端速度方向约束的起始帧，满足0≤kimp≤N。
可选地，所述足式机器人规划与控制装置的全身控制的优化问题为：
s.t.lbj≤Cjχ≤ubj,j＝1,2,…,nconstraint，
其中，Ai是任务矩阵，bi是任务向量，Cj是约束矩阵，lbj和ubj是约束下边界和上边界，Wi为权重矩阵，ntask为任务的数量，nconstraint为约束的数量。
可选地，所述全身控制同时处理的任务包括躯干轨迹跟踪任务、足端轨迹跟踪任务、足底力跟踪任务、最小化关节力矩变化量任务、最小化足底力变化量任务中的多个；所述具有碰撞意识的全身控制同时处理的约束包括：浮动基动力学等式约束、足底力不等式约束、关节输出力矩饱和不等式约束、关节转速饱和不等式约束、关节输出功率饱和不等式约束和碰撞意识约束中的多个。
可选地，所述根据所述运动状态、所述参考状态序列和所述足端参考状态对所述足式机器人进行全身控制，包括：若规划上应为摆动腿，则设置足端轨迹跟踪任务为跟踪所述具有碰撞意识的摆动腿轨迹、所述足底力跟踪任务的目标足底力为预设值、所述足底力不等式约束中足底力约束为预设值和禁用所述碰撞意识约束；若规划上应为支撑腿、且该支撑腿触地，则设置所述足端轨迹跟踪任务的目标线加速度为预设值、所述足底力跟踪任务的目标足底力为MPC或其他模块提供的足底力、足底力约束为摩擦锥约束和禁用所述碰撞意识约束；若规划上应为支撑腿、且该支撑腿未触地，则设置所述足端轨迹跟踪任务的目标为跟踪一个指向撞击面的速度、所述足底力跟踪任务的目标足底力为预设值、所述足底力约束为预设值和启用所述碰撞意识约束。
可选地，所述足式机器人规划与控制装置的全身动力学模型为：
其中，是广义质量矩阵，是包含了科里奥利力、向心力和重力的项，表示驱动关节的输出力矩；0n×m表示大小为n×m的零矩阵；和Fc分别是每个支撑腿的接触雅可比矩阵堆叠而成的增广雅可比矩阵和地面对每个支撑腿足底的反作用力堆叠而成的增广足底力；qg为广义关节空间位置，为广义关节空间速度，为广义关节空间加速度；所述碰撞意识约束矩阵和向量的表达式为：
其中，分别是人为指定的可以允许的碰撞冲量的下限和上限，δt表达当前时刻与下一时刻的时间间隔。
本申请第三方面实施例提供一种足式机器人，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序，以实现如上述实施例所述的足式机器人规划与控制方法。
本申请第四方面实施例提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行，以用于实现如上述实施例所述的足式机器人规划与控制方法。
由此，本申请至少具有如下有益效果：
本申请实施例可以进行足端动力学模型简化和离散碰撞模型的轨迹优化，对提前碰撞问题从摆动过程进行规划，使得提前碰撞后产生的影响在目标约束范围内；同时可以对足式机器人全身动力学模型和离散碰撞模型进行全身控制，对滞后问题从控制环节进行处理，使得滞后碰撞后产生的影响在目标约束范围内；由于同时对提前和滞后两方面碰撞进行考虑，因此本申请实施例可以降低足底与地面碰撞瞬间产生的冲击，减少碰撞的影响，进而提升机器人运动状态稳定性，提高机器人硬件使用寿命。
本申请附加的方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本申请的实践了解到。
附图说明
本申请上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：
图1为本申请实施例的一种足式机器人规划与控制方法的流程图；
图2为本申请实施例的具有碰撞意识的四足机器人步态控制系统示意图；
图3为本申请实施例的具有碰撞意识的全身控制的决策树示意图；
图4为本申请实施例的足式机器人规划与控制装置的示例图；
图5为本申请实施例的足式机器人的结构示意图。
具体实施方式
下面详细描述本申请的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，旨在用于解释本申请，而不能理解为对本申请的限制。
近几年来，足式机器人领域发展迅速，越来越多成熟稳定的方法被应用到足式机器人的运动规划与控制中，许多研究机构的足式机器人已经不再局限于静态行走，而是朝着更快、更灵敏、更鲁棒、更安全、更多功能等方向发展。当足式机器人执行高动态运动时，一些原本低动态运动情况下可以忽视的问题就变得不可忽视了，比如足式机器人在高速奔跑时足底与地面的碰撞问题。足式机器人的移动是依靠支撑腿的不断切换来实现的，每一步行走都需要足底与地面发生接触，这是有目的的与环境发生碰撞，也是足式机器人与环境交互时常见的碰撞问题。控制策略不当可能会使得足底与地面发生剧烈碰撞，从而影响之后的机器人运动状态，使机器人失去平衡，造成机器人跌倒，甚至有可能造成硬件使用寿命减短或损毁。
相关技术中，可以从硬件和控制算法两个方面提出足式机器人碰撞问题的解决方案。
在硬件方面，可以选择在机器人的足底安装具有一定柔性的脚垫，或者在机器人的驱动关节处安装弹簧阻尼等缓冲装置，还可以采用VIA或VSA。VIA和VSA可以提供期望的被动机械阻抗，在机械层面实现改变关节阻抗的效果，从而可以根据实际需求调节缓冲效果；其中，为机器人安装缓冲装置的做法通常称为被动柔顺方法。
在控制算法方面，为了让足式机器人在行走时足底与地面的碰撞对机器人状态的负面影响尽可能地小，可以在运动规划中让足底与地面的接触速度接近于0。当小型或轻型足式机器人在平坦的地形上慢速行走时，控制器的性能通常能够保证摆动腿的实际运动轨迹与规划轨迹基本一致，因此方案效果较好。当地形凹凸不平时，可以使用相机或激光雷达等视觉传感器来估计地形，根据估计的地形规划出让足底与地面的接触速度接近于0的足底运动轨迹，从而避免足底因提前触地或滞后触地造成的碰撞冲击。
然而，相关技术中的方法存在一定限制。
在硬件方面，被动柔顺方法可以在一定程度上减小碰撞带来的影响，但也存在一些问题。首先，这些被动柔性材料的引入会影响机器人系统的响应频率，如果控制算法没有将这一影响考虑在内，容易使得机器人在运动过程中产生不稳定的震动；另一方面，被动柔顺方法的缓冲效果是有限的，当缓冲装置的材料确定后，它的刚度、阻尼、惯量特性也确定了，因而无法根据实际需求调节缓冲效果；使用VIA或VSA的方法时，可以使用2个电机，其中一个电机独立控制关节的机械刚度，而另一个电机产生扭矩，整体结构过于复杂，体积和重量都较大，很难将这种执行器应用于需要做高动态运动的足式机器人身上。
在控制算法方面，当足式机器人进行高速奔跑等高动态的运动时，摆动腿的实际运动轨迹与规划轨迹可能存在明显误差，从而造成足底提前触地或者滞后触地；其中，滞后触地时，当足底尚未落地，但规划上需要它起到支撑躯干的作用时，这条腿会迅速踩到地上，造成剧烈碰撞，在碰撞瞬间产生巨大的足底力冲击；对于重型的足式机器人而言，这种碰撞造成的冲击力往往更大。当足式机器人高速奔跑时，剧烈的冲击有可能造成关节的零位漂移，足底长期频繁的碰撞有可能加速硬件老化。
综上，相关技术中的控制策略将碰撞时的冲击力视为一种扰动，将重点放在碰撞后机器人的平衡控制问题上，而忽视了这种频繁的、有目的的碰撞对机器人硬件寿命的影响。
针对上述背景技术中提到的问题，本申请提供了一种足式机器人规划与控制方法，下面参考附图描述本申请实施例的足式机器人规划与控制方法、装置、机器人及存储介质。
具体而言，图1为本申请实施例所提供的一种足式机器人规划与控制方法的流程示意图。
如图1所示，该足式机器人规划与控制方法包括以下步骤：
在步骤S101中，获取足式机器人的运动指令和运动状态。
其中，驱动指令可以包括用户输入的运动速度指令、方向指令等，运动状态可以包括机器人躯干状态、每条腿足端的状态及每条腿的触底状态等。
可以理解的是，本申请实施例可以使用至少一种方式对足式机器人的运动指令和运动状态进行获取；本申请实施例的足式机器人的执行器都是具有力控能力的电机，具备的传感器包括IMU(Inertial Measurement Unit，惯性测量单元)、电机角度码盘、电机力矩传感器等，以下实施例中，将以此为基础进行阐述。
举例而言，图2为本申请实施例的具有碰撞意识的四足机器人步态控制系统框图，该框图既适用于仿真也适用于实物。整个控制框架主要由5个模块组成，分别是状态估计(State Estimator)、粗略规划(Rough Planning)、SRB-MPC(Single Rigid Body ModelPredictive Control，单刚体模型预测控制)、IA-TO(Impact-aware TrajectoryOptimization，具有碰撞意识的轨迹优化)和IA-WBC(Impact-aware Whole-body Control，具有碰撞意识的全身控制)。该系统以较高的系统运行(例如500Hz、1kHz等)，IA-WBC运行在同频率上，SRB-MPC运行在40Hz左右，IA-TO中的优化问题仅在每次有某条腿需要摆动前计算一次。以下实施例中，将以上述系统为例进行阐述。
具体而言，如图2所示，本申请实施例可以根据读取到的IMU数据和电机状态信息，估计出足式机器人的躯干状态；其中，IMU数据可以包括三维欧拉角、三维角速度和三维线加速度等，电机的状态信息可以包括电机旋转角度、转速和反馈力矩等，足式机器人的躯干状态可以包括躯干的点位置、姿态单位四元数、线速度和角速度等，每条腿足端的状态可以包括点位置和线速度；本申请实施例可以使用一种EKF(Extended Kalman Filter，扩展卡尔曼滤波器)估计躯干的状态，对此不做具体限定。
本申请实施例的状态估计会根据不同的需求传递相应信息，如图2所示，对于SRB-MPC，传递的只有躯干状态；对于IA-TO，传递的是躯干状态、每条腿足端的状态以及关节状态；对于IA-WBC，传递的是躯干状态、每条腿足端的状态、关节状态以及每条腿的触地状态；对于粗略规划模块，可以传递躯干状态和每条腿足端状态；以上传递过程将在以下实施例中具体阐述，此处不再赘述。
在步骤S102中，根据运动指令和运动状态生成足式机器人的参考状态序列和每条腿的下一时刻摆动参数。
其中，参考状态序列可以是从当前时刻起到未来一段时间内的一系列参考虑；参考状态序列可以有躯干参考状态序列、支撑腿足端的参考位置序列、参考接触状态序列以及MPC参考帧间隔序列等；摆动参数包括摆动腿足端的初始状态(初始的点位置和线速度)、落地状态(落地时的点位置和线速度)、摆动的持续时间和摆动腿足端的期望最大离地高度等。
可以理解的是，本申请实施例可以利用获取的运动指令和运动状态生成足式机器人的参考状态序列和每条腿的下一时刻摆动参数；本申请实施例可以使用至少一种方式对参考状态序列和下一时刻摆动参数进行生成，比如可以利用获取的信息进行运动规划等。
具体而言，如图2所示，粗略规划模块可以根据状态估计的反馈信息以及用户(例如用遥控器)输入的运动速度指令(包括前后行走速度、左右侧向平动速度)和方向指令(即偏航角速度)对四足机器人进行一个粗略的运动规划。粗略规划模块需要为SRB-MPC提供参考状态序列，其中，参考状态序列的个数与SRB-MPC的预测帧数相同。粗略规划模块还需要为IA-TO提供每条腿下一次摆动的落脚点位置、摆动持续时间以及当前参考接触状态。
在步骤S103中，根据运动状态和每条腿的下一时刻摆动参数确定每条腿的足端参考状态，对于下一时刻开始摆动的腿，根据足端动力学模型和离散碰撞模型规划具有碰撞意识的摆动腿轨迹，使得提前碰撞后产生的影响在目标约束范围内。
可以理解的是，本申请实施例可以根据获取的运动状态、足式机器人每条腿下一时刻摆动参数确定每条腿的足端参考状态；当存在下一时刻开始摆动的腿时，利用足端动力学模型、离散碰撞模型对摆动腿轨迹进行具有碰撞意识的规划，使得摆动腿运动时提前碰撞后产生的影响在目标约束范围内。
需要说明的是，由于地面的轻微起伏、状态估计的误差以及控制上的误差等因素导致足式机器人的摆动腿足端在规划的落地时间点之前与地面发生接触，这就是提前碰撞；而这种非预期的提前接触往往带来非预期的碰撞冲击。这一问题其实可以在规划上解决，即规划一段具有碰撞意识的摆动腿足端运动轨迹，使得即便发生提前碰撞，其碰撞冲击也在预期范围内。由此，本申请实施例可以构建一个可以应用于足式机器人摆动腿的轨迹优化问题，其中，采用的机器人动力学模型是操作空间中摆动腿足端的线性化的动力学模型，而离散碰撞模型是离散碰撞动力学模型。该轨迹优化问题中，预测时间窗口取决于规划好的足端摆动时间，即预先指定的。为了让机器人上的计算机能够在线进行轨迹优化，相应的优化问题被设计为易于求解的QP(Quadratic Programming，二次规划)问题。
具体而言，如图2所示，粗略规划模块可以为IA-TO提供每条腿下一次摆动的落脚点位置、摆动持续时间以及当前参考接触状态；IA-TO模块接收状态估计的反馈信息以及粗略规划提供的参考信息，为每条腿足端生成合适的参考状态(包括参考的点位置和线速度)。SRB-MPC模块可以根据粗略规划提供的参考信息求解一个模型预测控制中的优化问题，得到最优躯干参考状态和最优参考足底力，这些都直接传递给IA-WBC；SRB-MPC可以每隔一帧左右的时间求解一次优化问题，当它不需要求解时，其输出量不更新，即输出数据是零阶保持的。
对于支撑腿，其参考状态就是保持在地面不动；对于即将开始摆动的腿，求解具有碰撞意识的轨迹优化问题；对于已经在摆动中的腿，对轨迹优化的最优解进行插值以得到当前时刻的参考状态。其中，IA-TO模块仅在有某条腿需要摆动前进行一次轨迹优化的求解，而其他时间的计算量是很少的；IA-TO模块最终会将每条腿足端的参考状态连同粗略规划提供的参考接触状态一并传递给IA-WBC。
在本申请实施例中，根据足端动力学模型和离散碰撞模型规划具有碰撞意识的摆动腿轨迹，包括：获取预先制定的状态量和目标约束条件；根据足端动力学模型、预先制定的状态量和目标约束条件构建下一时刻开始摆动的腿的连续时间轨迹优化问题；对连续时间轨迹优化问题进行离散化，得到摆动腿足端离散时间轨迹优化问题，求解该问题得到具有碰撞意识的摆动腿轨迹。
其中，预先指定的量可以包括：摆动腿足端的初始状态，它包括了初始的点位置和线速度；摆动腿足端的落地状态，它包括了落地时的点位置和线速度；摆动的持续时间摆动腿足端的期望最大离地高度
本申请实施例的预先制定的状态量包括摆动腿足端的初始状态、摆动腿足端的落地状态、摆动的持续时间和摆动腿足端的期望最大离地高度中的一个或多个，目标约束条件包括状态方程等式约束、驱动力不等式约束、轨迹高度不等式约束、初始状态等式约束、终端状态等式约束、碰撞意识不等式约束和末段速度方向不等式约束中的一个或多个。
可以理解的是，每一条腿在开始摆动前都需要求解一个轨迹优化问题以得到最优的足端摆动轨迹。考虑到实际机器人上的计算机算力有限，足端摆动过程中不再重复求解同一条腿的轨迹优化问题。其中，由于每条腿的轨迹优化问题的表达式没有本质差异，出于表达上的简便性考虑，本申请实施例在不存在歧义的情况下将省去所有用于表示腿的序号的下标l。
具体而言，本申请实施例可以将摆动腿足端线加速度作为控制量，即而状态量选为足端的点位置和线速度，即以此为基础，下面对本申请实施例的目标函数、约束条件及离散时间轨迹优化问题进行描述：
一、目标函数
在本申请实施例中，足端动力学模型为：
其中，为第l条腿足端在惯性系I中的线加速度，Λc,l操作空间中的质量矩阵，hc,l为包含了操作空间中的科里奥利力、向心力和重力的项，是驱动关节的力矩τj映射到操作空间后的足端驱动力；本申请实施例得到足端动力模型的过程可以具体如下：
本申请实施例可以设第l条腿足端在惯性系I中的点位置、线速度和线加速度分别为则操作空间中足端的动力学方程为：
其中，分别是驱动关节的力矩τj映射到操作空间后的足端驱动力和地面对足端的反作用力(后者仅在与地面发生接触时存在)；而Λc,l(qg)为操作空间中的质量矩阵，是包含了操作空间中的科里奥利力、向心力和重力的项。
考虑到足式机器人即使进行动态运动，其摆动腿大部分时候都处于躯干下方的某些范围内，本申请实施例可以认为在单次摆动的过程中腿部的构型变化范围并不是特别大，因此Λc,l(qg)可以被近似认为与构型无关，视为常数矩阵；实验证明，非线性项在腿部摆动过程中的变化其实并不大，同样可以被近似地视为常数向量。
由此，可以得到简化后的足端动力学模型：
将第l条腿的足底视为物体1，将地面视为物体2，考虑到地面的质量远远大于机器人的质量，且地面在碰撞前后的速度都为零，由此可以进一步得到本申请实施例的离散碰撞模型为：
其中，是在碰撞法向量上的投影算子，是碰撞面的单位法向量；分别是物体1碰撞前和碰撞后的速度，分别是物体2碰撞前和碰撞后的速度；cr为恢复系数，其定义为
在本申请实施例中，连续时间轨迹优化问题的目标函数为：
其中，和是维度各不相同的权重矩阵(对角矩阵居多)；||a||Q＝aTQa表示权重矩阵Q下向量a的加权模长；反映了摆动腿足端落地状态的任务，和分别是已知的摆动腿足端终端点位置和线速度；是最小化整个摆动过程的控制量(即线加速度)，t0＝0,tF＝TSW，TSW为摆动的持续时间；反映了摆动腿离地最大高度任务，th1和th2两个参数指定了达到最大高度的时间段，满足t0≤th1≤th2≤tF。zc是状态量的第3个分量，即状态量中表示足端离地高度的分量。
二、约束条件
(1)状态方程等式约束：由于控制量选为了足端线加速度，因此状态方程非常容易表达；令
则系统的状态方程可以写为：
(2)驱动力约束：由于实际执行器的输出力矩存在下限和上限，在操作空间中即对应的足端驱动力下限和上限由此驱动力约束可以写为：
fmin≤Λcu(t)+hc≤fmax,fort∈[t0,tF]。
(3)轨迹高度约束：为了避免优化出来的轨迹低于地面或者高于期望的最大离地高度，需要对轨迹的高度进行约束，即
zc,min≤Szx(t)≤zc,max,fort∈[t0,tF]，
其中，是轨迹高度的上下限；是用于选出状态第3个分量的选择矩阵。
(4)初始状态约束：摆动腿足端轨迹优化中的初始状态应该是足端离地前一刻的状态，因此初始状态约束为
其中，分别是摆动腿足端在求解轨迹优化问题前最新的反馈点位置和线速度。
(5)终端状态约束：尽管目标函数中已经体现了终端状态的任务，但是其无法保证优化出来的足端轨迹末端离地高度为零，即可能导致轨迹末端悬在空中或者扎入地下，因此需要一个终端状态约束，即
其中，和分别是轨迹末端的参考高度和竖直方向参考速度，其中，而是相应的选择矩阵，即
需要说明的是，本申请实施例的终端状态约束不关心状态中水平方向的分量，这是因为在约束条件较多的情况下水平方向的期望终端状态可能是达不到的，强制实施水平方向的终端状态约束可能会导致优化问题无解。
(6)碰撞意识约束：为了让摆动腿在落地前充分减速到足够安全的速度，本申请实施例可以在轨迹的末段考虑碰撞意识约束：假设碰撞过程从tc时刻开始持续到tc+δt时刻结束，对等号两边同时积分，有：
由于碰撞前后瞬间机器人构型几乎不变，即同时碰撞过程十分短暂，即δt→0，此时来自有限力(如重力、科里奥利力和向心力等)的脉冲在此极限下都会消失掉，只留下碰撞产生的冲量(Impulse)；因此上述公式在δt→0时变成：
其中，表示地面对足端的冲量；将该式代入离散碰撞模型，即可得到碰撞意识约束：
其中，是人为选定的可以容许的碰撞冲量下限和上限；timp是人为选定的开启碰撞意识约束的时间，满足t0≤timp≤tF。
可以理解的是，碰撞意识约束公式可以表示摆动腿足端在即将落地前，也就是[timp,tF]这时间段内，如果由于某些原因发生了提前碰撞，则碰撞所造成的冲量应该在该式的约束范围内；其中，该约束不能在摆动的全程开启，否则整个摆动过程会很缓慢，无法实现正常的奔跑运动。
(7)末段速度方向约束：如果摆动腿足端在即将落地前的速度方向是接近水平的，那么由于足端高度控制存在误差，可能造成足底提前触地，导致摆动腿足端没有落在规划好的落脚点位置上，从而可能影响机器人质心的稳定性。由此，本申请实施例可以设计足端末段速度方向约束，使得摆动腿足端落地前的速度与碰撞面的单位法向量-n(负号即对应方向朝内)的夹角θc在某一指定范围内，即：
其中，是人为指定的可以允许的最大夹角，上式非线性表达式，可以通过缩放的方式得到线性的表达式；根据向量的范数和范数的关系，有：
由此可以得到改写后的凡满足该改写后式的解都满足改写前式，因此可以将改写后的式子作为TO的末段速度方向约束，可以进一步写成：
其中，是的各个分量；通过遍历所有绝对值符号内的情况，上式可以进一步写成等价的8条不等式，即
但是，在摆动腿落地前一小段时间内通常都有该式中前4行通常是多余的，因此最终末段速度方向约束可以简化为：
其中，
三、离散时间轨迹优化问题
可以理解的是，为了便于在计算机中求解上述连续时间轨迹优化问题，本申请实施例还可以进行离散化；其中，本申请实施例可以使用至少一种离散化的方式，例如，目标函数中的积分项可以用有限项求和代替、状态方程约束可以用忽略高阶项的泰勒展开公式代替等。
在本申请实施例中，设摆动过程按时间平均分成N段，即共有N帧，帧间隔为Δt＝Tsw/N。令tk＝t0+k(tF-t0)/N,k＝1,2,…,N，则可以简记xk＝x(tk)，uk＝u(tk)；可以得到摆动腿足端离散时间轨迹优化问题的目标函数为：
fmin≤Λcuk+hc≤fmax,k＝0,1,…,N-1，
zc,min≤Szxk≤zc,max,k＝1,2,…,N-1，
其中，是由各帧的控制量组成的向量； 是维度各不相同的对角权重矩阵；当前时刻足端状态(包括位置和速度)反馈参考的终端足端状态(包括位置和速度) 是足端线速度的选择矩阵；和为人为指定的维持摆动高度的起始帧与终止帧，满足kimp是开启碰撞意识约束和末端速度方向约束的起始帧，满足0≤kimp≤N。
可以理解的是，由于很多研究中的轨迹优化问题都是高度非线性的，通常都是非凸优化问题，使用一些专业的非线性求解器虽然可以找到这些轨迹优化问题的局部最优解，但是无法保证找到全局最优解；本申请实施例的轨迹优化问题是一个QP问题，属于典型的凸优化问题，故只要其存在非空解集，则优化器找到的最优解必然是全局最优解。
对前述的摆动腿足端离散时间轨迹优化问题的目标函数可以使用一些QP求解器来求解。例如，本申请实施例可以使用基于C++的开源库qpOASES，需要将QP问题转换成标准形式：
s.t.lb≤CU≤ub，
在求解出上式的最优解后，得到的只是N帧的最优控制量U*，还需要利用它得到最优的位置和速度轨迹。将上述N行离散动力学方程写成关于初始状态和优化变量的线性表达式，即
其中，而AQP和BQP分别是：
由此，将U*代入上述线性表达式就可以得到相应的N帧最优位置和速度。
由于后续的控制程序可能在任意的时刻都需要相应的参考轨迹，此时就需要通过插值的方式，将N个离散的点变成连续的曲线；其中，本申请实施例可以使用至少一种方式实现插值；为了最大化计算效率，本申请实施例可以使用最简单的线性插值。
举例而言，设从摆动腿离开地面开始，此时已经过了tsw的时间，tsw∈[0,Tsw]，而距离此刻最近的前一帧是第k帧，对应的最优位置为则利用线性插值方法，此刻的最优参考位置应为：
同理可以得到相应的最优参考速度
需要说明的是，这种插值方式无法保证速度曲线的时间积分就是位置曲线，要做到这点需要更高次的插值函数；此处只是为了最大化计算效率而牺牲一些插值精度。
在步骤S104中，根据运动状态、参考状态序列和足端参考状态对足式机器人进行全身控制，其中，对于在规划上应为支撑腿、且该支撑腿未触地，根据全身动力学模型和离散碰撞模型进行具有碰撞意识的全身控制，使得滞后碰撞后产生的影响在目标约束范围内。
可以理解的是，如图2所示，本申请实施例可以根据状态估计的反馈信息、SRB-MPC和IA-TO模块的参考信息，求解WQP(Weighted Quadratic Programming，加权二次规划)并计算出最优关节力矩指令，对足式机器人进行全身控制，并对滞后碰撞问题进行处理。
需要说明的是，滞后碰撞指的是由于某些原因(例如地面的轻微起伏、状态估计的误差以及控制上的误差等)在原计划的时间点上足端尚未与地面发生接触，则足端与地面的接触必然晚于原计划的时间点；如果没有合适的控制策略，这种非预期的滞后碰撞也很容易导致非预期的碰撞冲击。滞后碰撞问题与提前碰撞问题有着本质的不同，即滞后碰撞是发生在规划的摆动过程之外的，既无法准确获知足底与地面的距离(本来距离应该为零才对)，也无法确定什么时候才与地面发生接触(本来应该按时发生碰撞才对)，需要在控制环节才能得到有效解决。由此，基于一步预测控制的核心思想，本申请实施例可以使用一种碰撞意识的WBC(Whole Body Control，全身控制)方法。
具体而言，以下实施例中将以四足机器人为例，对本申请实施例的WBC方法进行具体阐述；其中，碰撞意识约束并非时刻开启，在不同的情况下，全身控制的任务目标和约束设置都有所不同，由此，本申请实施例可以用一种如图3所示的决策树的形式来表达；本申请实施例的碰撞意识的全身控制具体如下：
一、基于加权二次规划的全身控制
可以理解的是，为了让全身控制处理好碰撞意识约束等不等式约束，全身控制的实现应该选择基于优化的方法；同时，考虑到实际机器人上的计算资源非常有限，需要同时运行的程序(例如通信、控制算法等)较多，本申请实施例可以采用基于WQP实现的全身控制方法。
在本申请实施例中，全身控制的优化问题为：
s.t.lbj≤Cjχ≤ubj,j＝1,2,…,nconstraint，
其中，Ai是任务矩阵，bi是任务向量，Cj是约束矩阵，lbj和ubj是约束下边界和上边界，Wi为权重矩阵，ntask为任务的数量，nconstraint为约束的数量。
可以理解的是，在WQP中，所有需要完成的任务都以Aiχ-bi的形式表示在目标函数中，通过权重矩阵Wi来调整任务相对优先级；所有约束都表达成lbj≤cjχ≤ubj的形式，对于等式约束，可以通过令lbj＝ubj来实现。本申请实施例中，WBC的优化变量选择为广义关节空间加速度和四条腿足端受到的作用力堆叠而成的增广足底力Fc，即 其中，nχ＝nq+nF。
当四足机器人处于原地站立状态时，本申请实施例对应的足式机器人的全身动力学模型为：
其中，是广义质量矩阵，是包含了科里奥利力、向心力和重力的项，表示驱动关节的输出力矩；0n×m表示大小为n×m的零矩阵；和Fc分别是每个支撑腿的接触雅可比矩阵堆叠而成的增广雅可比矩阵和地面对每个支撑腿足底的反作用力堆叠而成的增广足底力；qg为广义关节空间位置，为广义关节空间速度，为广义关节空间加速度。
通过求解上式的QP问题，可以得到最优解χ*；结合完整动力学方程，可以很容易计算出最优的关节力矩指
其中，是驱动关节的选择矩阵，和分别是由状态估计提供的广义关节空间位置和速度反馈。
二、任务设置
在本申请实施例中，全身控制同时处理的任务包括躯干轨迹跟踪任务、足端轨迹跟踪任务、足底力跟踪任务、最小化关节力矩变化量任务、最小化足底力变化量任务中的多个；具有碰撞意识的全身控制同时处理的约束包括：浮动基动力学等式约束、足底力不等式约束、关节输出力矩饱和不等式约束、关节转速饱和不等式约束、关节输出功率饱和不等式约束和碰撞意识约束中的多个。
具体而言，(1)躯干跟踪任务：躯干轨迹跟踪任务是让四足机器人运动的基本任务，不可或缺。在本申请实施例中，可以使用一种由MPC(Model Predictive Control，模型预测控制)提供的躯干参考运动轨迹，包括了惯性系I下的躯干最优参考点位置最优参考线速度最优Z-Y-X欧拉角以及浮动基系B下的躯干最优参考角速度
在WBC中，先根据MPC提供的最优参考轨迹和状态估计提供的反馈状态进行PD控制，得到躯干在惯性系I下的线加速度指令和在浮动基系B下的角加速度指令再设计相应的任务完成这些指令。其中，PD控制为：
其中，分别是PD增益系数矩阵，它们都是对角矩阵；是根据参考Z-Y-X欧拉角和反馈Z-Y-X欧拉角计算出的表示姿态误差的轴角。
由于的前6行对应了浮动基座的线加速度和角加速度，因此躯干轨迹跟踪任务相应的矩阵和向量分别写为：
(2)足端轨迹跟踪任务：足端轨迹跟踪任务也是四足机器人运动的基本任务，不可或缺。对于第l条腿的足端而言，根据(不含旋转的)加速度级的运动学，有：
因而第l条腿的足端轨迹跟踪子任务相应的矩阵和向量分别可以写为：
l＝1,2,3,4，
其中，是第l条腿足端在惯性系I下的线加速度指令，当这条腿处于不同状态时，指令不同。
举例而言，当这条腿为支撑腿时，足端应该固定在地面不动，即当它本应该作为支撑腿但是尚未触地(滞后触地)时，为跟踪一个指向碰撞面的速度的微分控制：
其中，是人为指定的期望碰撞速度，是前文提到过的碰撞面单位法向量，指向碰撞面以外；当这条腿作为摆动腿时，由PD控制得出：
其中，是PD增益系数矩阵，它们都是对角矩阵，为上述实施例计算得到的摆动腿足端最优参考点位置和线速度。
需要说明的是，本申请实施例四条腿的足端轨迹跟踪子任务合起来才是完整的足端轨迹跟踪任务，由此，相应的矩阵和向量分别可以写为：
(3)足底力跟踪任务：四足机器人的质心线动量和质心角动量除了受到重力的影响以外，还受到地面对足端的反作用力(即足底力)的影响；进行高动态运动时，合适的足底力非常重要。
本申请实施例可以提出一种利用单刚体模型MPC优化具有参考价值的足底力的方式，其在WBC中需要设置相应的任务来完成足底力的跟踪。
由于本申请实施例中使用的四足机器人足端并没有力/力矩传感器，因此在WBC中难以实现足底力反馈控制，只能将足底力跟踪任务设置为单纯的前馈力任务，相应的矩阵和向量分别可以写为：
其中，当第l条腿已经与地面接触并作为支撑腿时，它应该执行MPC提供的参考足底力，因此而当这条腿是摆动腿或者在规划上是支撑腿但尚未触地(即滞后触地)时，这条腿的足端应该不受外力，因此
(4)最小化关节力矩变化量任务：由于实际的电机力控带宽有限，太剧烈的输出力矩变化是实机无法实现的，当实际电机无法按照WBC的最优解来执行动作时，控制误差容易增大；由此，本申请实施例可以在WBC中将“减少关节力矩的变化量”一事纳入考虑，使得实际电机更容易按照WBC的最优解来执行动作。
设上一个控制周期中WBC计算出的最优关节力矩指令为在此次控制周期中的最小化关节力矩变化量任务的矩阵和向量分别可以写为：
需要说明的是，该任务与前述的躯干轨迹跟踪任务、足端轨迹跟踪任务和足底力跟踪任务存在潜在矛盾，因此本申请实施例中，该任务的权重不能太大，否则会影响到前述任务的执行效果。
(4)最小化足底力变化量任务：为了让足底力的变化更平滑(从而让质心的状态变化更平滑)，本申请实施例可以在WBC中考虑“减少足底力变化量”的任务。
设上一个控制周期中WBC计算出的最优足底力为在此次控制周期中的最小化足底力变化量任务的矩阵和向量分别可以写为：
需要说明的是，同理，该任务与前述的躯干轨迹跟踪任务、足端轨迹跟踪任务和足底力跟踪任务存在潜在矛盾，因此该任务的权重不能太大，否则会影响到其他任务的执行效果。
三、约束设置
(1)浮动基动力学等式约束：四足机器人的躯干不具有直接驱动力和力矩，因此完整运动方程的前6行中不存在关节输出力矩，这6行就是浮动基动力学方程，是必须要满足的等式约束，相应的矩阵和向量分别为：
其中，是浮动基的选择矩阵。
(2)足底力不等式约束：为了避免支撑腿的足端与地面发生相对滑动，足底力中平行于接触面的分量大小和垂直于接触面的分量大小的比值应该小于摩擦系数，即第l条腿的足底力Fc,l应该满足如下约束才能避免打滑：
||(I3×3-nnT)Fc,l||2≤μ|nTFc,l|，
其中，μ是摩擦系数。
可以理解的是，上式描述了一个圆锥形区间，因此这种避免足底打滑的约束也称为摩擦锥约束；由于式中含有范数，是非线性表达式，为了方便WBC优化问题的求解，本申请实施例可以对其进行简化。
本申请实施例可以首先对该式进行线性化处理，即，使用四棱锥代替了圆锥；其次，假定了四足机器人通常行走在坡度接近于零的平地上，因而可以将Fc,l的Z轴分量当成垂直于接触面的分量；由此可以将上式改写为：
需要说明的是，足底力是地面对足底的反作用力，因此其Z轴分量总是大于零。为了避免优化出特别巨大的足底力造成硬件损伤，本申请实施例还可以对Fc,l的Z轴分量大小进行限制；由此，第l条腿的足底力不等式约束可以写为：
其中，是人为指定的Z轴方向最大足底力：当这条腿已经是支撑腿时，是某个事先指定好的正数；而当这条腿是摆动腿或者规划上应为支撑腿但尚未落地(滞后落地)时，这条腿的足底力应该为零，因此设置本申请实施例中，上式还可以等价地用矩阵的形式表达，即：
05×1≤CFFc,l≤ubF，
其中，
最终四条腿的足底力约束合起来才是完整的足底力不等式约束，相应的矩阵和向量分别可以写为：
lb2＝020×1，
(3)关节输出力矩饱和不等式约束：实际的电机性能有限，即使是在堵转的状态下其输出力矩也存在上限。WBC中需要考虑实际电机的输出力矩限制，才能更好地协调各个关节以同时完成多个期望的任务。由此，关节输出力矩饱和不等式约束的矩阵和向量分别可以写为：
其中，τj,min,τj,max分别是关节输出力矩的下限和上限。
(4)关节转速饱和不等式约束：与关节输出力矩饱和不等式约束同理，即使是在空载的情况下，实际电机的转速也存在上限，WBC中必须考虑实际电机的转速限制，才能更好地协调各个关节以同时完成多个期望的任务。
对关节转速在t时刻进行泰勒展开，并忽略高于加速度的高阶项，本申请实施例可以对进行线性预测：
在同一个控制周期内，本申请实施例可以将最新反馈的关节转速当作当前时刻的关节转速并用优化变量中的关节加速度当作当前时刻的关节加速度从而利用上式就可以用优化变量线性表示出t+δt时刻的关节转速，由此，关节转速饱和不等式约束相应的矩阵和向量分别为：
其中，分别是关节转速的下限和上限。
(5)关节输出功率饱和不等式约束：与关节输出力矩饱和不等式约束同理，实际电机的输出功率也存在上限，WBC中必须考虑实际电机的输出功率限制，才能更好地协调各个关节以同时完成多个期望的任务。考虑到环境也有可能对电机做功，本申请实施例不约束关节的输入功率，可以只约束关节的输出功率。
对第α个关节，设其输出力矩为输出转速为则该关节的输出功率为设nj个关节的输出功率为则有：
其中，关节的转速可以用代入，而关节的输出力矩可以用优化变量线性表示出来，从而关节输出功率饱和不等式约束相应的矩阵和向量分别为：
其中，是关节输出功率的上限。
(6)碰撞意识约束：WBC中的碰撞意识约束与TO中的碰撞意识约束的思想是类似的，并且采用的离散碰撞模型也是相同的，即离散碰撞动力学模型。
可以理解的是，在WBC中，假设碰撞就发生在下一时刻(即t+δt时刻)，本申请实施例可以先利用泰勒展开公式可以预测下一时刻碰前足端线速度，然后预测碰撞所造成的足底线速度突变，再预测碰撞所造成的冲量，最后在当前控制周期内约束这一冲量。由此，WBC优化出来的关节加速度就能够使得足端的线速度足够小，以至于如果碰撞真的发生在下一时刻，则碰撞所造成的冲量就在WBC的碰撞意识约束范围内；如果下一时刻并未发生碰撞，则机器人表现出来的效果为足端的运动速度明显减小。
具体而言，对于第l条腿，利用机器人的运动学和泰勒展开公式可以用优化变量中的关节加速度线性预测t+δt时刻的碰前足端线速度
将上式分别代入离散碰撞动力学模型和即可用优化变量中的关节加速度线性表示出预测的碰撞所造成的足底线速度突变
用优化变量中的关节加速度线性表示出预测的碰撞冲量ιc,l,next：
由此，得到碰撞意识约束，其矩阵和向量的表达式为：
其中，分别是人为指定的可以允许的碰撞冲量的下限和上限，δt表达当前时刻与下一时刻的时间间隔。
可以理解的是，当这条腿在规划上是摆动腿或者它已经与地面发生接触并且成为支撑腿时，这条腿就不需要碰撞意识约束；反之，当这条腿在规划上应为支撑腿但是尚未落地(即滞后落地)时，这条腿就需要开启相应的碰撞意识约束。
具体而言，根据运动状态、参考状态序列和足端参考状态对足式机器人进行全身控制，包括：若规划上应为摆动腿，则设置足端轨迹跟踪任务为跟踪具有碰撞意识的摆动腿轨迹、足底力跟踪任务的目标足底力为预设值、足底力不等式约束中足底力约束为预设值和禁用碰撞意识约束；若规划上应为支撑腿、且该支撑腿触地，则设置足端轨迹跟踪任务的目标线加速度为预设值、足底力跟踪任务的目标足底力为MPC或其他模块提供的足底力、足底力约束为摩擦锥约束和禁用碰撞意识约束；若规划上应为支撑腿、且该支撑腿未触地，则设置足端轨迹跟踪任务的目标为跟踪一个指向撞击面的速度、足底力跟踪任务的目标足底力为预设值、足底力约束为预设值和启用碰撞意识约束。
其中，可以为足底力跟踪任务提供目标足底力的模块可以有上述MPC模块等；足底力跟踪任务的目标足底力、不等式约束中足底力约束、跟踪任务的目标线加速度的预设值可以为0；本申请实施例中，以预设值为0进行阐述。
需要说明的是，本申请实施例还可以进一步地对碰撞造成的关节空间中的影响进行预测，比如可以对碰撞冲力映射到各个关节上得到碰撞对关节造成的力矩影响进行预测等；但是，通常情况下只要在操作空间中做到碰撞造成的冲量足够小，则碰撞在关节空间中造成的影响也不会很大。为了实现WBC计算效率的最大化，本申请实施例的碰撞意识约束仅限于操作空间中，与上述实施例中的轨迹优化方法一致。
四、全身控制的实现决策
可以理解的是，本申请实施例的WBC中，始终不变的任务设置有3个，即躯干轨迹跟踪任务、最小化关节力矩变化量任务与最小化足底力变化量任务；始终不变的约束设置有4个，即浮动基动力学等式约束、电机输出力矩饱和不等式约束、电机输出转速饱和不等式约束及电机输出功率饱和不等式约束；其他任务和约束在不同情况下的设置是不同的，分别是足端轨迹跟踪任务、足底力跟踪任务、足底力不等式约束及碰撞意识约束。
需要说明的是，如图3所示，当某条腿在规划上是摆动腿时，如果WBC在该腿作为摆动腿时开启对应的碰撞意识约束，则有两种情况：一是由于TO中已经有了碰撞意识约束，该腿足端的参考线速度本身就很小，此时WBC优化出来的最优解并不在对应的碰撞意识约束边界上，即WBC中对应的碰撞意识约束并不起作用；另一种情况是WBC优化出来的最优解就在对应的碰撞意识约束边界上，即该约束起作用了，导致该腿足端的运动速度比TO给出的参考线速度还要低，从而使得该腿足端无法在TO指定的触地时间点上完成触地，即出现了滞后落地。由此可以得到，当某条腿在规划上是摆动腿时，如果WBC中开启了该腿的碰撞意识约束，则或者不起作用，或者起到负面作用；因此WBC中的碰撞意识约束只有在出现滞后落地时才会开启。
进一步地，当某条腿在规划上应为支撑腿但尚未检测到触地(即滞后落地)时，足端轨迹跟踪任务的目标为跟踪一个指向撞击面的速度-vimpn，其中，vimp不应该设置得太小，否则会滞后落地很久；但无需考虑vimp被设置得太大时导致撞击速度太大，碰撞意识约束可以将足端速度限制住。本申请实施例的足式机器人的每一条腿都可以根据如图3所示的决策树进行判断，从而确定所有的任务和约束的表达式，即确定WQP的表达式。
综上，根据本申请实施例提出的足式机器人规划与控制方法，可以进行足端动力学模型简化和离散离散碰撞模型的轨迹优化，对提前碰撞问题从摆动过程进行规划，使得提前碰撞后产生的影响在目标约束范围内；同时可以对足式机器人全身动力学模型和离散碰撞模型进行全身控制，对滞后问题从控制环节进行处理，使得滞后碰撞后产生的影响在目标约束范围内；由于同时对提前和滞后两方面碰撞进行考虑，因此本申请实施例可以降低足底与地面碰撞瞬间产生的冲击，减少碰撞的影响，进而提升机器人运动状态稳定性，提高机器人硬件使用寿命。
其次参照附图描述根据本申请实施例提出的足式机器人规划与控制装置。
图4是本申请实施例的足式机器人规划与控制装置的方框示意图。
如图4所示，该足式机器人规划与控制装置10包括：获取模块100、生成模块200、规划模块300和控制模块400。
其中，获取模块100，用于获取足式机器人的运动指令和运动状态；生成模块200，用于根据运动指令和运动状态生成足式机器人的参考状态序列和每条腿的下一时刻摆动参数；规划模块300，用于根据运动状态和每条腿的下一时刻摆动参数确定每条腿的足端参考状态，对于下一时刻开始摆动的腿，根据足端动力学模型和离散碰撞模型规划具有碰撞意识的摆动腿轨迹，使得提前碰撞后产生的影响在目标约束范围内；控制模块400，用于根据运动状态、参考状态序列和足端参考状态对足式机器人进行全身控制，其中，对于在规划上应为支撑腿、且该支撑腿未触地，根据全身动力学模型和离散碰撞模型进行具有碰撞意识的全身控制，使得滞后碰撞后产生的影响在目标约束范围内。
在本申请实施例中，足式机器人规划与控制装置的足端动力学模型为：
其中，为第l条腿足端在惯性系I中的线加速度，Λc,l操作空间中的质量矩阵，hc,l为包含了操作空间中的科里奥利力、向心力和重力的项，是驱动关节的力矩τj映射到操作空间后的足端驱动力；离散碰撞模型为：
其中，是在碰撞法向量上的投影算子，是碰撞面的单位法向量；分别是物体1碰撞前和碰撞后的速度，分别是物体2碰撞前和碰撞后的速度，cr为恢复系数。
在本申请实施例中，规划模块进一步用于：获取预先制定的状态量和目标约束条件；根据足端动力学模型、预先制定的状态量和目标约束条件构建下一时刻开始摆动的腿的连续时间轨迹优化问题；对连续时间轨迹优化问题进行离散化，得到摆动腿足端离散时间轨迹优化问题，求解该问题得到具有碰撞意识的摆动腿轨迹。
在本申请实施例中，规划模块的预先制定的状态量包括摆动腿足端的初始状态、摆动腿足端的落地状态、摆动的持续时间和摆动腿足端的期望最大离地高度中的一个或多个，目标约束条件包括状态方程等式约束、驱动力不等式约束、轨迹高度不等式约束、初始状态等式约束、终端状态等式约束、碰撞意识不等式约束和末段速度方向不等式约束中的一个或多个。
在本申请实施例中，规划模块的连续时间轨迹优化问题的目标函数为：
其中，和是维度各不相同的权重矩阵(对角矩阵居多)；||a||Q＝aTQa表示权重矩阵Qa下向量a的加权模长；反映了摆动腿足端落地状态的任务，和分别是已知的摆动腿足端终端点位置和线速度；是最小化整个摆动过程的控制量(即线加速度)，t0＝0,tF＝TSW，TSW为摆动的持续时间；反映了摆动腿离地最大高度任务，th1和th2两个参数指定了达到最大高度的时间段，满足t0≤th1≤th2≤tF。zc是状态量的第3个分量，即状态量中表示足端离地高度的分量。
摆动腿足端离散时间轨迹优化问题的目标函数为：
fmin≤Λcuk+hc≤fmax,k＝0,1,…,N-1，
zc,min≤szxk≤zc,max,k＝1,2,…,N-1，
其中，是由各帧的控制量组成的向量； 是维度各不相同的对角权重矩阵；当前时刻足端状态(包括位置和速度)反馈参考的终端足端状态(包括位置和速度) 是足端线速度的选择矩阵；和为人为指定的维持摆动高度的起始帧与终止帧，满足kimp是开启碰撞意识约束和末端速度方向约束的起始帧，满足0≤kimp≤N。
在本申请实施例中，足式机器人规划与控制装置的全身控制的优化问题为：
s.t.lbj≤Cjχ≤ubj,j＝1,2,…,nconstraint，
其中，Ai是任务矩阵，bi是任务向量，Cj是约束矩阵，lbj和ubj是约束下边界和上边界，Wi为权重矩阵，ntask为任务的数量，nconstraint为约束的数量。
在本申请实施例中，全身控制同时处理的任务包括躯干轨迹跟踪任务、足端轨迹跟踪任务、足底力跟踪任务、最小化关节力矩变化量任务、最小化足底力变化量任务中的多个；具有碰撞意识的全身控制同时处理的约束包括：浮动基动力学等式约束、足底力不等式约束、关节输出力矩饱和不等式约束、关节转速饱和不等式约束、关节输出功率饱和不等式约束和碰撞意识约束中的多个。
在本申请实施例中，根据运动状态、参考状态序列和足端参考状态对足式机器人进行全身控制，包括：若规划上应为摆动腿，则设置足端轨迹跟踪任务为跟踪具有碰撞意识的摆动腿轨迹、足底力跟踪任务的目标足底力为预设值、足底力不等式约束中足底力约束为预设值和禁用碰撞意识约束；若规划上应为支撑腿、且该支撑腿触地，则设置足端轨迹跟踪任务的目标线加速度为预设值、足底力跟踪任务的目标足底力为MPC或其他模块提供的足底力、足底力约束为摩擦锥约束和禁用碰撞意识约束；若规划上应为支撑腿、且该支撑腿未触地，则设置足端轨迹跟踪任务的目标为跟踪一个指向撞击面的速度、足底力跟踪任务的目标足底力为预设值、足底力约束为预设值和启用碰撞意识约束。
在本申请实施例中，足式机器人规划与控制装置的全身动力学模型为：
其中，是广义质量矩阵，是包含了科里奥利力、向心力和重力的项，表示驱动关节的输出力矩；0n×m表示大小为n×m的零矩阵；和Fc分别是每个支撑腿的接触雅可比矩阵堆叠而成的增广雅可比矩阵和地面对每个支撑腿足底的反作用力堆叠而成的增广足底力；qg为广义关节空间位置，为广义关节空间速度，为广义关节空间加速度。
碰撞意识约束矩阵和向量的表达式为：
其中，分别是人为指定的可以允许的碰撞冲量的下限和上限，δt表达当前时刻与下一时刻的时间间隔。
需要说明的是，前述对足式机器人规划与控制方法实施例的解释说明也适用于该实施例的足式机器人规划与控制装置，此处不再赘述。
根据本申请实施例提出的足式机器人规划与控制装置，可以进行足端动力学模型简化和离散离散碰撞模型的轨迹优化，对提前碰撞问题从摆动过程进行规划，使得提前碰撞后产生的影响在目标约束范围内；同时可以对足式机器人全身动力学模型和离散碰撞模型进行全身控制，对滞后问题从控制环节进行处理，使得滞后碰撞后产生的影响在目标约束范围内；由于同时对提前和滞后两方面碰撞进行考虑，因此本申请实施例可以降低足底与地面碰撞瞬间产生的冲击，减少碰撞的影响，进而提升机器人运动状态稳定性，提高机器人硬件使用寿命。
图5为本申请实施例提供的足式机器人的结构示意图。该足式机器人可以包括：
存储器501、处理器502及存储在存储器501上并可在处理器502上运行的计算机程序。
处理器502执行程序时实现上述实施例中提供的足式机器人规划与控制方法。
进一步地，足式机器人还包括：
通信接口503，用于存储器501和处理器502之间的通信。
存储器501，用于存放可在处理器502上运行的计算机程序。
存储器501可能包含高速RAM(Random Access Memory，随机存取存储器)存储器，也可能还包括非易失性存储器，例如至少一个磁盘存储器。
如果存储器501、处理器502和通信接口503独立实现，则通信接口503、存储器501和处理器502可以通过总线相互连接并完成相互间的通信。总线可以是ISA(IndustryStandard Architecture，工业标准体系结构)总线、PCI(Peripheral ComponentInterconnect，外部设备互连)总线或EISA(Extended Industry Standard Architecture，扩展工业标准体系结构)总线等。总线可以分为地址总线、数据总线、控制总线等。为便于表示，图5中仅用一条粗线表示，但并不表示仅有一根总线或一种类型的总线。
可选的，在具体实现上，如果存储器501、处理器502及通信接口503，集成在一块芯片上实现，则存储器501、处理器502及通信接口503可以通过内部接口完成相互间的通信。
处理器502可能是一个CPU(Central Processing Unit，中央处理器)，或者是ASIC(Application Specific Integrated Circuit，特定集成电路)，或者是被配置成实施本申请实施例的一个或多个集成电路。
本申请实施例还提供一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如上的足式机器人规划与控制方法。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本申请的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不是必须针对的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任一个或N个实施例或示例中以合适的方式结合。此外，在不相互矛盾的情况下，本领域的技术人员可以将本说明书中描述的不同实施例或示例以及不同实施例或示例的特征进行结合和组合。
此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本申请的描述中，“N个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。
流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或N个用于实现定制逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本申请的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本申请的实施例所属技术领域的技术人员所理解。
应当理解，本申请的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，N个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。如，如果用硬件来实现和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列，现场可编程门阵列等。
本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。
尽管上面已经示出和描述了本申请的实施例，可以理解的是，上述实施例是示例性的，不能理解为对本申请的限制，本领域的普通技术人员在本申请的范围内可以对上述实施例进行变化、修改、替换和变型。

Claims (12)

1.一种足式机器人规划与控制方法，其特征在于，包括以下步骤：
获取足式机器人的运动指令和运动状态；
根据所述运动指令和所述运动状态生成所述足式机器人的参考状态序列和每条腿的下一时刻摆动参数；
根据所述运动状态和每条腿的下一时刻摆动参数确定每条腿的足端参考状态，对于下一时刻开始摆动的腿，根据足端动力学模型和离散碰撞模型规划具有碰撞意识的摆动腿轨迹，使得提前碰撞后产生的影响在目标约束范围内；
根据所述运动状态、所述参考状态序列和所述足端参考状态对所述足式机器人进行全身控制，其中，对于在规划上应为支撑腿、且该支撑腿未触地，根据全身动力学模型和离散碰撞模型进行具有碰撞意识的全身控制，使得滞后碰撞后产生的影响在目标约束范围内。
2.根据权利要求1所述的足式机器人规划与控制方法，其特征在于，所述足端动力学模型为：
其中，为第l条腿足端在惯性系I中的线加速度，Λc,l为操作空间中的质量矩阵，hc,l为包含了操作空间中的科里奥利力、向心力和重力的项，是驱动关节的力矩τj映射到操作空间后的足端驱动力；
所述离散碰撞模型为：
其中，是在碰撞法向量上的投影算子，是碰撞面的单位法向量；分别是物体1碰撞前和碰撞后的速度，分别是物体2碰撞前和碰撞后的速度，cr为恢复系数。
3.根据权利要求1或2所述的足式机器人规划与控制方法，其特征在于，所述根据足端动力学模型和离散碰撞模型规划具有碰撞意识的摆动腿轨迹，包括：
获取预先制定的状态量和目标约束条件；
根据所述足端动力学模型、所述预先制定的状态量和所述目标约束条件构建下一时刻开始摆动的腿的连续时间轨迹优化问题；
对所述连续时间轨迹优化问题进行离散化，得到摆动腿足端离散时间轨迹优化问题，求解该问题得到具有碰撞意识的摆动腿轨迹。
4.根据权利要求3所述的足式机器人规划与控制方法，其特征在于，所述预先制定的状态量包括摆动腿足端的初始状态、摆动腿足端的落地状态、摆动的持续时间和摆动腿足端的期望最大离地高度中的一个或多个，所述目标约束条件包括状态方程等式约束、驱动力不等式约束、轨迹高度不等式约束、初始状态等式约束、终端状态等式约束、碰撞意识不等式约束和末段速度方向不等式约束中的一个或多个。
5.根据权利要求3所述的足式机器人规划与控制方法，其特征在于，所述连续时间轨迹优化问题的目标函数为：
其中，和是维度各不相同的权重矩阵(对角矩阵居多)；||a||Q＝aTQa表示权重矩阵Q下向量a的加权模长；反映了摆动腿足端落地状态的任务，和分别是已知的摆动腿足端终端点位置和线速度；是最小化整个摆动过程的控制量(即线加速度)，t0＝0,tF＝TSW，TSW为摆动的持续时间；反映了摆动腿离地最大高度任务，th1和th2两个参数指定了达到最大高度的时间段，满足t0≤th1≤th2≤tF；zc是状态量的第3个分量，即状态量中表示足端离地高度的分量；
所述摆动腿足端离散时间轨迹优化问题的目标函数为：
fmin≤Λcuk+hc≤fmax,k＝0,1,…,N-1，
zc,min≤Szxk≤zc,max,k＝1,2,…,N-1，
其中，是由各帧的控制量组成的向量； 是维度各不相同的对角权重矩阵；当前时刻足端状态(包括位置和速度)反馈参考的终端足端状态(包括位置和速度) 是足端线速度的选择矩阵；和为人为指定的维持摆动高度的起始帧与终止帧，满足kimp是开启碰撞意识约束和末端速度方向约束的起始帧，满足0≤kimp≤N。
6.根据权利要求1所述的足式机器人规划与控制方法，其特征在于，所述全身控制的优化问题为：
s.t.lbj≤Cjχ≤ubj,j＝1,2,…,nconstraint，
其中，Ai是任务矩阵，bi是任务向量，Cj是约束矩阵，lbj和ubj是约束下边界和上边界，Wi为权重矩阵，ntask为任务的数量，nconstraint为约束的数量。
7.权利要求1所述的足式机器人规划与控制方法，其特征在于，所述全身控制同时处理的任务包括躯干轨迹跟踪任务、足端轨迹跟踪任务、足底力跟踪任务、最小化关节力矩变化量任务、最小化足底力变化量任务中的多个；所述具有碰撞意识的全身控制同时处理的约束包括：浮动基动力学等式约束、足底力不等式约束、关节输出力矩饱和不等式约束、关节转速饱和不等式约束、关节输出功率饱和不等式约束和碰撞意识约束中的多个。
8.根据权利要求7所述的足式机器人规划与控制方法，其特征在于，所述根据所述运动状态、所述参考状态序列和所述足端参考状态对所述足式机器人进行全身控制，包括：
若规划上应为摆动腿，则设置足端轨迹跟踪任务为跟踪所述具有碰撞意识的摆动腿轨迹、所述足底力跟踪任务的目标足底力为预设值、所述足底力不等式约束中足底力约束为预设值和禁用所述碰撞意识约束；
若规划上应为支撑腿、且该支撑腿触地，则设置所述足端轨迹跟踪任务的目标线加速度为预设值、所述足底力跟踪任务的目标足底力为MPC或其他模块提供的足底力、足底力约束为摩擦锥约束和禁用所述碰撞意识约束；
若规划上应为支撑腿、且该支撑腿未触地，则设置所述足端轨迹跟踪任务的目标为跟踪一个指向撞击面的速度、所述足底力跟踪任务的目标足底力为预设值、所述足底力约束为预设值和启用所述碰撞意识约束。
9.根据权利要求1所述的足式机器人规划与控制方法，其特征在于，所述全身动力学模型为：
其中，是广义质量矩阵，是包含了科里奥利力、向心力和重力的项，表示驱动关节的输出力矩；0n×m表示大小为n×m的零矩阵；和Fc分别是每个支撑腿的接触雅可比矩阵堆叠而成的增广雅可比矩阵和地面对每个支撑腿足底的反作用力堆叠而成的增广足底力；qg为广义关节空间位置，为广义关节空间速度，为广义关节空间加速度；
所述碰撞意识约束矩阵和向量的表达式为：
其中，分别是人为指定的可以允许的碰撞冲量的下限和上限，δt表达当前时刻与下一时刻的时间间隔。
10.一种足式机器人规划与控制装置，其特征在于，包括：
获取模块，用于获取足式机器人的运动指令和运动状态；
生成模块，用于根据所述运动指令和所述运动状态生成所述足式机器人的参考状态序列和每条腿的下一时刻摆动参数；
规划模块，用于根据所述运动状态和每条腿的下一时刻摆动参数确定每条腿的足端参考状态，对于下一时刻开始摆动的腿，根据足端动力学模型和离散碰撞模型规划具有碰撞意识的摆动腿轨迹，使得提前碰撞后产生的影响在目标约束范围内；
控制模块，用于根据所述运动状态、所述参考状态序列和所述足端参考状态对所述足式机器人进行全身控制，其中，对于在规划上应为支撑腿、且该支撑腿未触地，根据全身动力学模型和离散碰撞模型进行具有碰撞意识的全身控制，使得滞后碰撞后产生的影响在目标约束范围内。
11.一种足式机器人，其特征在于，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述程序，以实现如权利要求1-9任一项所述的足式机器人规划与控制方法。
12.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行，以用于实现如权利要求1-9任一项所述的足式机器人规划与控制方法。
CN202311511514.9A 2023-11-13 2023-11-13 足式机器人规划与控制方法、装置、机器人及存储介质 Pending CN117572768A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311511514.9A CN117572768A (zh) 2023-11-13 2023-11-13 足式机器人规划与控制方法、装置、机器人及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311511514.9A CN117572768A (zh) 2023-11-13 2023-11-13 足式机器人规划与控制方法、装置、机器人及存储介质

Publications (1)

Publication Number Publication Date
CN117572768A true CN117572768A (zh) 2024-02-20

Family

ID=89887338

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311511514.9A Pending CN117572768A (zh) 2023-11-13 2023-11-13 足式机器人规划与控制方法、装置、机器人及存储介质

Country Status (1)

Country Link
CN (1) CN117572768A (zh)

Similar Documents

Publication Publication Date Title
CN111558941B (zh) 浮动基动力学前馈控制方法、装置和多足机器人
JP2024512235A (ja) 脚車輪型ロボットの制御方法、装置、脚車輪型ロボット及び機器
JP7324932B2 (ja) 動的計画コントローラ
JP5034235B2 (ja) 制御システム及び制御方法、並びにコンピュータ・プログラム
US20210162589A1 (en) Systems and methods for learning agile locomotion for multiped robots
Yi et al. Online learning of a full body push recovery controller for omnidirectional walking
Kim et al. Computationally-robust and efficient prioritized whole-body controller with contact constraints
WO2007127723A2 (en) Control of robots from human motion descriptors
JPWO2003011535A1 (ja) 脚式移動ロボット及びその動作制御方法
JP6781101B2 (ja) 非線形システムの制御方法、二足歩行ロボットの制御装置、二足歩行ロボットの制御方法及びそのプログラム
Omran et al. Optimal task space control design of a Stewart manipulator for aircraft stall recovery
CN116551669A (zh) 仿人机器人动态跳跃与平衡控制方法、电子设备、介质
WO2024021744A1 (zh) 一种控制足式机器人的方法、装置、电子设备、计算机可读存储介质、计算机程序产品及足式机器人
Katayama et al. Whole-body model predictive control with rigid contacts via online switching time optimization
JP7221833B2 (ja) 非線形モデル予測制御装置
WO2024021767A1 (zh) 控制足式机器人的方法、装置、设备、足式机器人、计算机可读存储介质和计算机程序产品
US20230191600A1 (en) Motion control method, robot, and computer-readable storage medium
Xie et al. Gait optimization and energy-based stability for biped locomotion using large-scale programming
WO2023165177A1 (zh) 构建机器人的控制器的方法、机器人的运动控制方法、装置以及机器人
CN117572768A (zh) 足式机器人规划与控制方法、装置、机器人及存储介质
Faraji et al. Compliant and adaptive control of a planar monopod hopper in rough terrain
Abdolmaleki et al. Omnidirectional walking with a compliant inverted pendulum model
CN116736748B (zh) 构建机器人的控制器的方法和机器人
Shen et al. Implementation of a robust dynamic walking controller on a miniature bipedal robot with proprioceptive actuation
KR102570962B1 (ko) 로봇 제어 장치 및 이의 동작 방법

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