WO2022134143A1 - 一种机器人状态估计方法、装置、可读存储介质及机器人 - Google Patents

一种机器人状态估计方法、装置、可读存储介质及机器人 Download PDF

Info

Publication number
WO2022134143A1
WO2022134143A1 PCT/CN2020/140560 CN2020140560W WO2022134143A1 WO 2022134143 A1 WO2022134143 A1 WO 2022134143A1 CN 2020140560 W CN2020140560 W CN 2020140560W WO 2022134143 A1 WO2022134143 A1 WO 2022134143A1
Authority
WO
WIPO (PCT)
Prior art keywords
robot
coordinate system
force
foot
moment point
Prior art date
Application number
PCT/CN2020/140560
Other languages
English (en)
French (fr)
Inventor
葛利刚
刘益彰
陈春玉
谢铮
熊友军
Original Assignee
深圳市优必选科技股份有限公司
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 深圳市优必选科技股份有限公司 filed Critical 深圳市优必选科技股份有限公司
Publication of WO2022134143A1 publication Critical patent/WO2022134143A1/zh
Priority to US18/144,160 priority Critical patent/US20230271656A1/en

Links

Images

Classifications

    • 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
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B25J13/085—Force or torque sensors
    • 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/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
    • 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/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
    • 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/1633—Programme controls characterised by the control loop compliant, force, torque control, e.g. combined with position control
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01L—MEASURING FORCE, STRESS, TORQUE, WORK, MECHANICAL POWER, MECHANICAL EFFICIENCY, OR FLUID PRESSURE
    • G01L5/00—Apparatus for, or methods of, measuring force, work, mechanical power, or torque, specially adapted for specific purposes
    • G01L5/16—Apparatus for, or methods of, measuring force, work, mechanical power, or torque, specially adapted for specific purposes for measuring several components of force
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01M—TESTING STATIC OR DYNAMIC BALANCE OF MACHINES OR STRUCTURES; TESTING OF STRUCTURES OR APPARATUS, NOT OTHERWISE PROVIDED FOR
    • G01M1/00—Testing static or dynamic balance of machines or structures
    • G01M1/12—Static balancing; Determining position of centre of gravity
    • G01M1/122—Determining position of centre of gravity

Landscapes

  • Engineering & Computer Science (AREA)
  • Mechanical Engineering (AREA)
  • Robotics (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Chemical & Material Sciences (AREA)
  • Transportation (AREA)
  • Combustion & Propulsion (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Human Computer Interaction (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Manipulator (AREA)

Abstract

一种机器人状态估计方法、装置、计算机可读存储介质及机器人。所述方法包括：获取机器人的左脚受力信息和右脚受力信息（S101）；根据所述左脚受力信息和所述右脚受力信息计算所述机器人在世界坐标系下的双足零力矩点（S102）；基于预设的线性倒立摆模型计算所述机器人的质心位置（S103）。该方法预先构建一种全新的线性倒立摆模型，将机器人的双足零力矩点作为该模型的支撑点，充分考虑了双足零力矩点的位置变化对质心位置的影响，基于这一模型可以得到更加准确的估计结果。

Description

一种机器人状态估计方法、装置、可读存储介质及机器人
本申请要求于2020年12月24日在中国专利局提交的、申请号为202011547547.5的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
技术领域
本申请属于机器人技术领域，尤其涉及一种机器人状态估计方法、装置、计算机可读存储介质及机器人。
背景技术
在双足机器人的运动控制过程中，常常需要对质心位置等机器人状态进行估计，以便据此对机器人进行更加精准的控制。但在现有技术中，一般都是基于机器人的脚踝支撑位置来对质心位置进行估计，估计结果的准确性较差。
技术问题
有鉴于此，本申请实施例提供了一种机器人状态估计方法、装置、计算机可读存储介质及机器人，以解决现有的机器人状态估计方法存在的准确性较差的问题。
技术解决方案
本申请实施例的第一方面提供了一种机器人状态估计方法，可以包括：
获取机器人的左脚受力信息和右脚受力信息；
根据所述左脚受力信息和所述右脚受力信息计算所述机器人在世界坐标系下的双足零力矩点；
基于预设的线性倒立摆模型计算所述机器人的质心位置，其中，所述线性倒立摆模型的支撑点为所述双足零力矩点。
进一步地，所述基于预设的线性倒立摆模型计算所述机器人的质心位置，可以包括：
获取所述机器人的本体姿态角；
根据所述本体姿态角计算由所述机器人的本体坐标系到所述世界坐标系的位姿变换矩阵；
获取所述机器人的质心规划位置；
根据所述质心规划位置、所述位姿变换矩阵和所述双足零力矩点计算所述机器人的质心位置。
进一步地，所述根据所述左脚受力信息和所述右脚受力信息计算所述机器人在世界坐标系下的双足零力矩点，可以包括：
根据所述左脚受力信息计算所述机器人的左脚零力矩点；
根据所述右脚受力信息计算所述机器人的右脚零力矩点；
获取第一位置和第二位置，所述第一位置为所述机器人的左脚六维力坐标系原点在所述世界坐标系中的位置，所述第二位置为所述机器人的右脚六维力坐标系原点在所述世界坐标系中的位置；
根据所述第一位置、所述第二位置、所述左脚零力矩点和所述右脚零力矩点计算所述双足零力矩点。
进一步地，在基于预设的线性倒立摆模型计算所述机器人的质心位置之后，所述方法还可以包括：
根据第一质心位置和第二质心位置进行位置差分，得到所述机器人的质心速度；所述第一质心位置为所述机器人在当前时刻的质心位置，所述第二质心位置为所述机器人在上一时刻的质心位置。
本申请实施例的第二方面提供了一种机器人状态估计装置，可以包括：
受力信息获取模块，用于获取机器人的左脚受力信息和右脚受力信息；
双足零力矩点计算模块，用于根据所述左脚受力信息和所述右脚受力信息计算所述机器人在世界坐标系下的双足零力矩点；
质心位置计算模块，用于基于预设的线性倒立摆模型计算所述机器人的质心位置，其中，所述线性倒立摆模型的支撑点为所述双足零力矩点。
进一步地，所述质心位置计算模块可以包括：
本体姿态角获取子模块，用于获取所述机器人的本体姿态角；
位姿变换矩阵计算子模块，用于根据所述本体姿态角计算由所述机器人的本体坐标系到所述世界坐标系的位姿变换矩阵；
质心规划位置获取子模块，用于获取所述机器人的质心规划位置；
质心位置计算子模块，用于根据所述质心规划位置、所述位姿变换矩阵和所述双足零力矩点计算所述机器人的质心位置。
进一步地，所述双足零力矩点计算模块可以包括：
左脚零力矩点计算子模块，用于根据所述左脚受力信息计算所述机器人的左脚零力矩点；
右脚零力矩点计算子模块，用于根据所述右脚受力信息计算所述机器人的右脚零力矩点；
位置获取子模块，用于获取第一位置和第二位置，所述第一位置为所述机器人的左脚 六维力坐标系原点在所述世界坐标系中的位置，所述第二位置为所述机器人的右脚六维力坐标系原点在所述世界坐标系中的位置；
双足零力矩点计算子模块，用于根据所述第一位置、所述第二位置、所述左脚零力矩点和所述右脚零力矩点计算所述双足零力矩点。
进一步地，所述机器人状态估计装置还可以包括：
质心速度计算模块，用于根据第一质心位置和第二质心位置进行位置差分，得到所述机器人的质心速度；所述第一质心位置为所述机器人在当前时刻的质心位置，所述第二质心位置为所述机器人在上一时刻的质心位置。
本申请实施例的第三方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述任一种机器人状态估计方法的步骤。
本申请实施例的第四方面提供了一种机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述任一种机器人状态估计方法的步骤。
本申请实施例的第五方面提供了一种计算机程序产品，当计算机程序产品在机器人上运行时，使得机器人执行上述任一种机器人状态估计方法的步骤。
有益效果
本申请实施例与现有技术相比存在的有益效果是：本申请实施例获取机器人的左脚受力信息和右脚受力信息；根据所述左脚受力信息和所述右脚受力信息计算所述机器人在世界坐标系下的双足零力矩点；基于预设的线性倒立摆模型计算所述机器人的质心位置。通过本申请实施例，预先构建了一种全新的线性倒立摆模型，将机器人的双足零力矩点作为该模型的支撑点，充分考虑了双足零力矩点的位置变化对质心位置的影响，基于这一模型可以得到更加准确的估计结果。
附图说明
为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1为本申请实施例中一种机器人状态估计方法的一个实施例流程图；
图2为世界坐标系的示意图；
图3为六维力坐标系的示意图；
图4为根据左脚受力信息和右脚受力信息计算机器人在世界坐标系下的双足零力矩点的示意流程图；
图5为以六维力坐标系原点为支撑点的线性倒立摆模型的示意图；
图6为以双足零力矩点为支撑点的线性倒立摆模型的示意图；
图7为将机器人的本体姿态误差近似为线性倒立摆模型绕双足零力矩点的转动角度的示意图；
图8为基于线性倒立摆模型计算机器人的质心位置的示意流程图；
图9为本申请实施例中一种机器人状态估计装置的一个实施例结构图；
图10为本申请实施例中一种机器人的示意框图。
本发明的实施方式
为使得本申请的发明目的、特征、优点能够更加的明显和易懂，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，下面所描述的实施例仅仅是本申请一部分实施例，而非全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本申请保护的范围。
应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
还应当理解，在此本申请说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本申请。如在本申请说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。
还应当进一步理解，在本申请说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
如在本说明书和所附权利要求书中所使用的那样，术语“如果”可以依据上下文被解释为“当...时”或“一旦”或“响应于确定”或“响应于检测到”。类似地，短语“如果确定”或“如果检测到[所描述条件或事件]”可以依据上下文被解释为意指“一旦确定”或“响应于确定”或“一旦检测到[所描述条件或事件]”或“响应于检测到[所描述条件或事件]”。
另外，在本申请的描述中，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。
为叙述简便起见，如无特殊说明，本申请实施例中所提及的机器人均为双足机器人。
请参阅图1，本申请实施例中一种机器人状态估计方法的一个实施例可以包括：
步骤S101、获取机器人的左脚受力信息和右脚受力信息。
为了便于叙述，首先对本申请实施例中所使用的坐标系进行说明。
图2所示为世界坐标系的示意图(俯视图)，世界坐标系的坐标原点为机器人的两脚中心，x轴正方向为机器人的行进方向，y轴正方向为机器人的左侧方向，也即将行进方向逆时针旋转90度后所指向的方向，z轴正方向为垂直向上的方向。
图3所示为世界坐标系和六维力坐标系的对比示意图(后视图)，六维力坐标系的坐标原点为六维力传感器的中心，x轴正方向、y轴正方向和z轴正方向分别为六维力传感器的前向、左向和向上的方向。为了便于区分，此处将左脚和右脚的两个六维力坐标系分别记为左脚六维力坐标系和右脚六维力坐标系。
在本申请实施例中，可以在机器人的双足和腿部连接的部位分别设置六维力传感器，为了便于区分，此处将左脚和右脚的两个六维力传感器分别记为左脚六维力传感器和右脚六维力传感器，通过这些传感器即可获取机器人的左脚受力信息和右脚受力信息。其中，左脚受力信息包括在左脚六维力坐标系的三个坐标轴上的力分量以及力矩分量，右脚受力信息包括在右脚六维力坐标系的三个坐标轴上的力分量以及力矩分量。
此处将所述左脚受力信息记为：[F lx,F ly,F lz,T lx,T ly,T lz]。其中，F lx为所述左脚受力信息在所述左脚六维力坐标系的x轴上的力的分量，F ly为所述左脚受力信息在所述左脚六维力坐标系的y轴上的力的分量，F lz为所述左脚受力信息在所述左脚六维力坐标系的z轴上的力的分量，T lx为所述左脚受力信息在所述左脚六维力坐标系的x轴上的力矩分量，T ly为所述左脚受力信息在所述左脚六维力坐标系的y轴上的力矩分量，T lz为所述左脚受力信息在所述左脚六维力坐标系的z轴上的力矩分量。
将所述右脚受力信息记为：[F rx,F ry,F rz,T rx,T ry,T rz]。其中，F rx为所述右脚受力信息在所述右脚六维力坐标系的x轴上的力的分量，F ry为所述右脚受力信息在所述右脚六维力坐标系的y轴上的力的分量，F rz为所述右脚受力信息在所述右脚六维力坐标系的z轴上的力的分量，T rx为所述右脚受力信息在所述右脚六维力坐标系的x轴上的力矩分量，T ry为所述右脚受力信息在所述右脚六维力坐标系的y轴上的力矩分量，T rz为所述右脚受力信息在所述右脚六维力坐标系的z轴上的力矩分量。
步骤S102、根据所述左脚受力信息和所述右脚受力信息计算所述机器人在世界坐标系下的双足零力矩点(Zero Moment Point，ZMP)。
如图4所示，步骤S102具体可以包括如下过程：
步骤S1021、根据所述左脚受力信息计算所述机器人的左脚ZMP。
具体地，可以根据下式计算所述左脚ZMP：
Figure PCTCN2020140560-appb-000001
Figure PCTCN2020140560-appb-000002
p l＝[p lx,p ly,0]
其中，p lx为所述左脚ZMP在所述左脚六维力坐标系的x轴上的坐标，p ly为所述左脚ZMP在所述左脚六维力坐标系的y轴上的坐标，d l为所述左脚六维力传感器与地面之间的高度，p l为所述左脚ZMP。
步骤S1022、根据所述右脚受力信息计算所述机器人的右脚ZMP。
具体地，可以根据下式计算所述右脚ZMP：
Figure PCTCN2020140560-appb-000003
Figure PCTCN2020140560-appb-000004
p r＝[p rx,p ry,0]
其中，p rx为所述右脚ZMP在所述右脚六维力坐标系的x轴上的坐标，p ry为所述右脚ZMP在所述右脚六维力坐标系的y轴上的坐标，d r为所述右脚六维力传感器与地面之间的高度，p r为所述右脚ZMP。
步骤S1023、获取第一位置和第二位置。
所述第一位置为所述机器人的左脚六维力坐标系原点在所述世界坐标系中的位置，将其记为：[lfoot x,lfoot y,lfoot z]，其中，lfoot x为所述左脚六维力坐标系原点在所述世界坐标系的x轴上的坐标，lfoot y为所述左脚六维力坐标系原点在所述世界坐标系的y轴上的坐标，lfoot z为所述左脚六维力坐标系原点在所述世界坐标系的z轴上的坐标。
所述第二位置为所述机器人的右脚六维力坐标系原点在所述世界坐标系中的位置，将其记为：[rfoot x,rfoot y,rfoot z]，其中，rfoot x为所述右脚六维力坐标系原点在所述世界坐标系的x轴上的坐标，rfoot y为所述右脚六维力坐标系原点在所述世界坐标系的y轴上 的坐标，rfoot z为所述右脚六维力坐标系原点在所述世界坐标系的z轴上的坐标。
步骤S1024、根据所述第一位置、所述第二位置、所述左脚ZMP和所述右脚ZMP计算所述双足ZMP。
具体地，可以根据下式计算所述双足ZMP：
Figure PCTCN2020140560-appb-000005
Figure PCTCN2020140560-appb-000006
P[3]＝[p x,p y,0]
其中，p x为所述双足ZMP在所述世界坐标系的x轴上的坐标，p y为所述双足ZMP在所述世界坐标系的y轴上的坐标，P[3]为所述双足ZMP。
需要注意的是，以上的计算过程均默认六维力坐标系的各坐标轴方向与世界坐标系保持一致。若不一致，则需要对单脚的ZMP进行相应的旋转变换之后再进行双足ZMP的计算，此处不再赘述。
步骤S103、基于预设的线性倒立摆模型(Linear Inverted Pendulum Model，LIPM)计算所述机器人的质心位置。
近似化是处理复杂系统的一种有效方法，在双足机器人步态规划中，常常采用简化模型的方式来进行步态的生成，最常用的简化模型是LIMP。基于以下三个假设条件，双足机器人就可以简化为一个LIMP：第一，假定机器人的所有质量集中于其质心位置；第二，假设机器人腿部无质量；第三，假设质心只有水平方向上的运动。图5所示为传统的LIMP的示意图，其中，左侧为双足模型，左脚支撑，右脚摆动。右侧为简化模型，以六维力坐标系原点为支撑点，简化为LIMP。
在LIMP中，描述倒立摆运动状态的两个重要物理量分别是质心的位置和质心的速度，能否准确估计出质心当前的位置和速度，是双足平衡控制中至关重要的一点。然而多数的估计方法中没有考虑ZMP的位置变化对质心状态估计带来的影响，而通常直接基于脚踝支撑位置进行估计。由于ZMP的变化范围有限(支撑区域内)，以及本体姿态角度偏差有限(基本在±5度范围内)，因此这种传统的估计方法近似准确，却不能充分描述出当前机器人的状态。
为了实现双足机器人更准确的状态估计，本申请实施例提出了一种以双足ZMP为支撑点的全新的LIMP，图6所示即为该LIMP的示意图，基于这一模型，在机器人步态的支撑期，LIPM的支撑点位置会随着双足ZMP位置的变化而变化。如图7所示，在将双足机器人简化为以双足ZMP为支撑点的LIMP之后，可以将其本体姿态误差近似为LIMP绕双足 ZMP的转动角度。
在本申请实施例中，可以基于这一全新的LIMP来进行机器人状态的估计，综合考虑双足ZMP位置变化和本体姿态变化的影响，从而提高双足机器人状态估计的准确性。具体的估计过程如图8所示：
步骤S1031、获取所述机器人的本体姿态角。
具体地，可以通过安装在机器人本体上的惯性测量单元(Inertial Measurement Unit，IMU)来获取所述本体姿态角，将其记为Pose body[3]，且Pose body[3]＝[roll,pitch,yaw]，其中，roll为横滚角，pitch为俯仰角，yaw为偏航角。
步骤S1032、根据所述本体姿态角计算由所述机器人的本体坐标系到所述世界坐标系的位姿变换矩阵。
所述本体坐标系的坐标原点为机器人的质心，x轴正方向、y轴正方向和z轴正方向分别为机器人躯干的前向、左向和向上的方向。将所述位姿变换矩阵记为T body2world，则可以根据下式计算所述位姿变换矩阵：
T body2world＝R z(yaw)R y(pitch)R x(roll)
其中：
Figure PCTCN2020140560-appb-000007
Figure PCTCN2020140560-appb-000008
Figure PCTCN2020140560-appb-000009
步骤S1033、获取所述机器人的质心规划位置。
所述质心规划位置可以根据实际情况进行设置，本申请实施例对此不作具体限定。此处将所述质心规划位置记为Com plan[3]，且Com plan[3]＝[x d,y d,z d]，其中，x d,y d,z d分别为所述质心规划位置在所述本体坐标系的x，y，z轴上的坐标。
步骤S1034、根据所述质心规划位置、所述位姿变换矩阵和所述双足ZMP计算所述机器人的质心位置。
具体地，可以根据下式计算所述机器人的质心位置：
Com measure[3]＝P[3]+T body2world*(Com plan[3]-P[3])
其中，Com measure[3]为所述机器人的质心位置，且Com measure[3]＝[x m,y m,z m]，其中，x m,y m,z m分别为该质心位置在所述世界坐标系的x，y，z轴上的坐标。
进一步地，在本申请实施例中，还可以根据第一质心位置和第二质心位置进行位置差分，得到所述机器人的质心速度。其中，所述第一质心位置为所述机器人在当前时刻的质心位置，所述第二质心位置为所述机器人在上一时刻的质心位置。
具体地，可以根据下式计算所述质心速度：
Vel measure[3] now＝(Com measure[3] now-Com measure[3] last)/t
其中，Com measure[3] now为所述第一质心位置，Com measure[3] last为所述第二质心位置，t为两次质心位置估计的时间间隔，Vel measure[3] now为所述质心速度。考虑到传感器噪声的影响，还可以进一步对差分求解得到的质心速度进行滤波处理，以得到更加准确的质心速度。
综上所述，本申请实施例获取机器人的左脚受力信息和右脚受力信息；根据所述左脚受力信息和所述右脚受力信息计算所述机器人在世界坐标系下的双足ZMP；基于预设的线性倒立摆模型计算所述机器人的质心位置。通过本申请实施例，预先构建了一种全新的线性倒立摆模型，将机器人的双足ZMP作为该模型的支撑点，充分考虑了双足ZMP的位置变化对质心位置的影响，基于这一模型可以得到更加准确的估计结果。
应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。
对应于上文实施例所述的一种机器人状态估计方法，图9示出了本申请实施例提供的一种机器人状态估计装置的一个实施例结构图。
本实施例中，一种机器人状态估计装置可以包括：
受力信息获取模块901，用于获取机器人的左脚受力信息和右脚受力信息；
双足零力矩点计算模块902，用于根据所述左脚受力信息和所述右脚受力信息计算所述机器人在世界坐标系下的双足零力矩点；
质心位置计算模块903，用于基于预设的线性倒立摆模型计算所述机器人的质心位置，其中，所述线性倒立摆模型的支撑点为所述双足零力矩点。
进一步地，所述质心位置计算模块可以包括：
本体姿态角获取子模块，用于获取所述机器人的本体姿态角；
位姿变换矩阵计算子模块，用于根据所述本体姿态角计算由所述机器人的本体坐标系到所述世界坐标系的位姿变换矩阵；
质心规划位置获取子模块，用于获取所述机器人的质心规划位置；
质心位置计算子模块，用于根据所述质心规划位置、所述位姿变换矩阵和所述双足零力矩点计算所述机器人的质心位置。
进一步地，所述双足零力矩点计算模块可以包括：
左脚零力矩点计算子模块，用于根据所述左脚受力信息计算所述机器人的左脚零力矩点；
右脚零力矩点计算子模块，用于根据所述右脚受力信息计算所述机器人的右脚零力矩点；
位置获取子模块，用于获取第一位置和第二位置，所述第一位置为所述机器人的左脚六维力坐标系原点在所述世界坐标系中的位置，所述第二位置为所述机器人的右脚六维力坐标系原点在所述世界坐标系中的位置；
双足零力矩点计算子模块，用于根据所述第一位置、所述第二位置、所述左脚零力矩点和所述右脚零力矩点计算所述双足零力矩点。
进一步地，所述机器人状态估计装置还可以包括：
质心速度计算模块，用于根据第一质心位置和第二质心位置进行位置差分，得到所述机器人的质心速度；所述第一质心位置为所述机器人在当前时刻的质心位置，所述第二质心位置为所述机器人在上一时刻的质心位置。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的装置，模块和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
图10示出了本申请实施例提供的一种机器人的示意框图，为了便于说明，仅示出了与本申请实施例相关的部分。
如图10所示，该实施例的机器人10包括：处理器100、存储器101以及存储在所述存储器101中并可在所述处理器100上运行的计算机程序102。所述处理器100执行所述计算机程序102时实现上述各个机器人状态估计方法实施例中的步骤，例如图1所示的步骤S101至步骤S103。或者，所述处理器100执行所述计算机程序102时实现上述各装置实施例中各模块/单元的功能，例如图9所示模块901至模块903的功能。
示例性的，所述计算机程序102可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器101中，并由所述处理器100执行，以完成本申请。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序102在所述机器人10中的执行过程。
本领域技术人员可以理解，图10仅仅是机器人10的示例，并不构成对机器人10的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述机器人10还可以包括输入输出设备、网络接入设备、总线等。
所述处理器100可以是中央处理单元(Central Processing Unit，CPU)，还可以是其它通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述存储器101可以是所述机器人10的内部存储单元，例如机器人10的硬盘或内存。所述存储器101也可以是所述机器人10的外部存储设备，例如所述机器人10上配备的插接式硬盘，智能存储卡(Smart Media Card，SMC)，安全数字(Secure Digital，SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器101还可以既包括所述机器人10的内部存储单元也包括外部存储设备。所述存储器101用于存储所述计算机程序以及所述机器人10所需的其它程序和数据。所述存储器101还可以用于暂时地存储已经输出或者将要输出的数据。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
在本申请所提供的实施例中，应该理解到，所揭露的装置/机器人和方法，可以通过其它的方式实现。例如，以上所描述的装置/机器人实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读存储介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读存储介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读存储介质不包括电载波信号和电信信号。
以上所述实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。

Claims (10)

  1. 一种机器人状态估计方法，其特征在于，包括：
    获取机器人的左脚受力信息和右脚受力信息；
    根据所述左脚受力信息和所述右脚受力信息计算所述机器人在世界坐标系下的双足零力矩点；
    基于预设的线性倒立摆模型计算所述机器人的质心位置，其中，所述线性倒立摆模型的支撑点为所述双足零力矩点。
  2. 根据权利要求1所述的机器人状态估计方法，其特征在于，所述基于预设的线性倒立摆模型计算所述机器人的质心位置，包括：
    获取所述机器人的本体姿态角；
    根据所述本体姿态角计算由所述机器人的本体坐标系到所述世界坐标系的位姿变换矩阵；
    获取所述机器人的质心规划位置；
    根据所述质心规划位置、所述位姿变换矩阵和所述双足零力矩点计算所述机器人的质心位置。
  3. 根据权利要求2所述的机器人状态估计方法，其特征在于，所述根据所述质心规划位置、所述位姿变换矩阵和所述双足零力矩点计算所述机器人的质心位置，包括：
    根据下式计算所述机器人的质心位置：
    Com measure[3]＝P[3]+T body2world*(Com plan[3]-P[3])
    其中，Com plan[3]为所述质心规划位置，T body2world为所述位姿变换矩阵，P[3]为所述双足零力矩点，Com measure[3]为所述机器人的质心位置。
  4. 根据权利要求1所述的机器人状态估计方法，其特征在于，所述根据所述左脚受力信息和所述右脚受力信息计算所述机器人在世界坐标系下的双足零力矩点，包括：
    根据所述左脚受力信息计算所述机器人的左脚零力矩点；
    根据所述右脚受力信息计算所述机器人的右脚零力矩点；
    获取第一位置和第二位置，所述第一位置为所述机器人的左脚六维力坐标系原点在所述世界坐标系中的位置，所述第二位置为所述机器人的右脚六维力坐标系原点在所述世界坐标系中的位置；
    根据所述第一位置、所述第二位置、所述左脚零力矩点和所述右脚零力矩点计算所述双足零力矩点。
  5. 根据权利要求4所述的机器人状态估计方法，其特征在于，所述根据所述左脚受力信息计算所述机器人的左脚零力矩点，包括：
    根据下式计算所述左脚零力矩点：
    Figure PCTCN2020140560-appb-100001
    Figure PCTCN2020140560-appb-100002
    p l＝[p lx,p ly,0]
    其中，p lx为所述左脚零力矩点在所述左脚六维力坐标系的x轴上的坐标，p ly为所述左脚零力矩点在所述左脚六维力坐标系的y轴上的坐标，F lx为所述左脚受力信息在所述左脚六维力坐标系的x轴上的力的分量，F ly为所述左脚受力信息在所述左脚六维力坐标系的y轴上的力的分量，F lz为所述左脚受力信息在所述左脚六维力坐标系的z轴上的力的分量，T lx为所述左脚受力信息在所述左脚六维力坐标系的x轴上的力矩分量，T ly为所述左脚受力信息在所述左脚六维力坐标系的y轴上的力矩分量，d l为所述机器人的左脚六维力传感器与地面之间的高度，p l为所述左脚零力矩点；
    所述根据所述右脚受力信息计算所述机器人的右脚零力矩点，包括：
    根据下式计算所述右脚零力矩点：
    Figure PCTCN2020140560-appb-100003
    Figure PCTCN2020140560-appb-100004
    p r＝[p rx,p ry,0]
    其中，p rx为所述右脚零力矩点在所述右脚六维力坐标系的x轴上的坐标，p ry为所述右脚零力矩点在所述右脚六维力坐标系的y轴上的坐标，F rx为所述右脚受力信息在所述右脚六维力坐标系的x轴上的力的分量，F ry为所述右脚受力信息在所述右脚六维力坐标系的y轴上的力的分量，F rz为所述右脚受力信息在所述右脚六维力坐标系的z轴上的力的分量，T rx为所述右脚受力信息在所述右脚六维力坐标系的x轴上的力矩分量，T ry为所述右脚受力信息在所述右脚六维力坐标系的y轴上的力矩分量，d r为所述机器人的右脚六维力传感器与地面之间的高度，p r为所述右脚零力矩点。
  6. 根据权利要求5所述的机器人状态估计方法，其特征在于，所述根据所述第一位置、所述第二位置、所述左脚零力矩点和所述右脚零力矩点计算所述双足零力矩点，包括：
    根据下式计算所述双足零力矩点：
    Figure PCTCN2020140560-appb-100005
    Figure PCTCN2020140560-appb-100006
    P[3]＝[p x,p y,0]
    其中，[lfoot x,lfoot y,lfoot z]为所述第一位置，[rfoot x,rfoot y,rfoot z]为所述第二位置，p x为所述双足零力矩点在所述世界坐标系的x轴上的坐标，p y为所述双足零力矩点在所述世界坐标系的y轴上的坐标，P[3]为所述双足零力矩点。
  7. 根据权利要求1至6中任一项所述的机器人状态估计方法，其特征在于，在基于预设的线性倒立摆模型计算所述机器人的质心位置之后，还包括：
    根据第一质心位置和第二质心位置进行位置差分，得到所述机器人的质心速度；所述第一质心位置为所述机器人在当前时刻的质心位置，所述第二质心位置为所述机器人在上一时刻的质心位置。
  8. 一种机器人状态估计装置，其特征在于，包括：
    受力信息获取模块，用于获取机器人的左脚受力信息和右脚受力信息；
    双足零力矩点计算模块，用于根据所述左脚受力信息和所述右脚受力信息计算所述机器人在世界坐标系下的双足零力矩点；
    质心位置计算模块，用于基于预设的线性倒立摆模型计算所述机器人的质心位置，其中，所述线性倒立摆模型的支撑点为所述双足零力矩点。
  9. 一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7中任一项所述的机器人状态估计方法的步骤。
  10. 一种机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7中任一项所述的机器人状态估计方法的步骤。
PCT/CN2020/140560 2020-12-24 2020-12-29 一种机器人状态估计方法、装置、可读存储介质及机器人 WO2022134143A1 (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
US18/144,160 US20230271656A1 (en) 2020-12-24 2023-05-05 Robot state estimation method, computer-readable storage medium, and legged robot

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN202011547547.5A CN112744313B (zh) 2020-12-24 2020-12-24 一种机器人状态估计方法、装置、可读存储介质及机器人
CN202011547547.5 2020-12-24

Related Child Applications (1)

Application Number Title Priority Date Filing Date
US18/144,160 Continuation US20230271656A1 (en) 2020-12-24 2023-05-05 Robot state estimation method, computer-readable storage medium, and legged robot

Publications (1)

Publication Number Publication Date
WO2022134143A1 true WO2022134143A1 (zh) 2022-06-30

Family

ID=75647478

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2020/140560 WO2022134143A1 (zh) 2020-12-24 2020-12-29 一种机器人状态估计方法、装置、可读存储介质及机器人

Country Status (3)

Country Link
US (1) US20230271656A1 (zh)
CN (1) CN112744313B (zh)
WO (1) WO2022134143A1 (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113359792A (zh) * 2021-05-27 2021-09-07 深圳市优必选科技股份有限公司 一种双足机器人控制方法、装置、双足机器人及存储介质
CN113253748B (zh) * 2021-06-08 2022-02-22 深圳市优必选科技股份有限公司 并联构型仿人机器人的状态估计方法、装置、设备及介质
CN113858207B (zh) * 2021-10-27 2023-06-09 乐聚(深圳)机器人技术有限公司 人形机器人的倒地控制方法、装置、电子设备及存储介质
CN114227761B (zh) * 2021-12-29 2023-07-14 之江实验室 一种双足机器人外界推力检测装置及其检测方法
CN114486045A (zh) * 2022-01-24 2022-05-13 之江实验室 一种双足机器人足底受力状态检测系统及方法

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0856457B1 (en) * 1997-01-31 2002-05-08 Honda Giken Kogyo Kabushiki Kaisha Gait generation system of legged mobile robot
KR20100097881A (ko) * 2009-02-27 2010-09-06 숭실대학교산학협력단 이족 로봇의 안정된 보행을 위한 칼만 필터의 지엠피 상태예측을 통한 균형제어 기법
CN103149933A (zh) * 2013-02-27 2013-06-12 南京邮电大学 基于闭环控制的仿人机器人全向行走方法
CN105511465A (zh) * 2015-12-02 2016-04-20 歌尔声学股份有限公司 一种双足机器人的步态控制方法和装置
US20190070729A1 (en) * 2017-09-04 2019-03-07 Honda Motor Co., Ltd. Desired zmp trajectory generating device
CN110286679A (zh) * 2019-06-24 2019-09-27 南京理工大学 基于线性倒立摆模型的机器人步态规划方法
CN111015653A (zh) * 2019-12-13 2020-04-17 深圳市优必选科技股份有限公司 机器人控制方法、装置、计算机可读存储介质及机器人

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR100709556B1 (ko) * 2005-10-19 2007-04-20 한국과학기술연구원 인간형 로봇의 보행 제어 방법
KR101182288B1 (ko) * 2010-08-26 2012-09-14 한국과학기술원 로봇의 걸음새 생성기
CN103042526A (zh) * 2013-01-22 2013-04-17 北京理工大学 仿人机器人控制单脚支撑期支撑脚板的方法和装置
CN110244791B (zh) * 2019-07-11 2020-05-15 北京理工大学 一种双足机器人足部力和力矩跟随控制方法
CN110371213A (zh) * 2019-07-12 2019-10-25 沈阳城市学院 一种双足机器人步行规划和控制方法
CN111736615B (zh) * 2020-08-04 2020-12-15 深圳市优必选科技股份有限公司 一种步态规划方法、装置、计算机可读存储介质及机器人

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP0856457B1 (en) * 1997-01-31 2002-05-08 Honda Giken Kogyo Kabushiki Kaisha Gait generation system of legged mobile robot
KR20100097881A (ko) * 2009-02-27 2010-09-06 숭실대학교산학협력단 이족 로봇의 안정된 보행을 위한 칼만 필터의 지엠피 상태예측을 통한 균형제어 기법
CN103149933A (zh) * 2013-02-27 2013-06-12 南京邮电大学 基于闭环控制的仿人机器人全向行走方法
CN105511465A (zh) * 2015-12-02 2016-04-20 歌尔声学股份有限公司 一种双足机器人的步态控制方法和装置
US20190070729A1 (en) * 2017-09-04 2019-03-07 Honda Motor Co., Ltd. Desired zmp trajectory generating device
CN110286679A (zh) * 2019-06-24 2019-09-27 南京理工大学 基于线性倒立摆模型的机器人步态规划方法
CN111015653A (zh) * 2019-12-13 2020-04-17 深圳市优必选科技股份有限公司 机器人控制方法、装置、计算机可读存储介质及机器人

Also Published As

Publication number Publication date
US20230271656A1 (en) 2023-08-31
CN112744313B (zh) 2022-04-15
CN112744313A (zh) 2021-05-04

Similar Documents

Publication Publication Date Title
WO2022134143A1 (zh) 一种机器人状态估计方法、装置、可读存储介质及机器人
US10852139B2 (en) Positioning method, positioning device, and robot
CN111015653B (zh) 机器人控制方法、装置、计算机可读存储介质及机器人
WO2022105020A1 (zh) 机器人控制方法、装置、计算机可读存储介质及机器人
CN111098300B (zh) 一种机器人平衡控制方法、装置、可读存储介质及机器人
CN107636420B (zh) 用于行人航位推算的技术
WO2022134144A1 (zh) 一种机器人质心规划方法、装置、可读存储介质及机器人
WO2022135011A1 (zh) 合质心状态获取方法、装置、仿人机器人和可读存储介质
WO2020221307A1 (zh) 一种运动物体的追踪方法和装置
WO2022247116A1 (zh) 质心轨迹生成方法、装置、计算机可读存储介质及机器人
CN112731953A (zh) 机器人控制方法、装置、计算机可读存储介质及机器人
CN109724602A (zh) 一种基于硬件fpu的姿态解算系统及其解算方法
CN113146638B (zh) 质心位姿估计方法、装置、计算机可读存储介质及机器人
CN109903330A (zh) 一种处理数据的方法和装置
WO2022105023A1 (zh) 机器人控制方法、装置、计算机可读存储介质及机器人
WO2022121003A1 (zh) 机器人控制方法、装置、计算机可读存储介质及机器人
CN111208809A (zh) 机器人步态规划方法及终端设备
CN109141411A (zh) 定位方法、定位装置、移动机器人及存储介质
CN113031579A (zh) 双足机器人的行走控制方法、装置及双足机器人
CN109866217B (zh) 机器人里程定位方法、装置、终端设备及计算机存储介质
CN112363196B (zh) 车辆属性确定方法、装置、存储介质和电子设备
WO2023142353A1 (zh) 一种位姿预测方法及装置
CN115727871A (zh) 一种轨迹质量检测方法、装置、电子设备和存储介质
CN112157651B (zh) 位姿确定方法、装置及机器人
CN114147717A (zh) 机器人运动轨迹估计方法、装置、控制器及存储介质

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 20966705

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 20966705

Country of ref document: EP

Kind code of ref document: A1