CN111086001A - 一种足式机器人多模态感知的状态估计方法及系统 - Google Patents

一种足式机器人多模态感知的状态估计方法及系统 Download PDF

Info

Publication number
CN111086001A
CN111086001A CN201911358566.0A CN201911358566A CN111086001A CN 111086001 A CN111086001 A CN 111086001A CN 201911358566 A CN201911358566 A CN 201911358566A CN 111086001 A CN111086001 A CN 111086001A
Authority
CN
China
Prior art keywords
foot
robot
legged robot
imu
legged
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.)
Granted
Application number
CN201911358566.0A
Other languages
English (en)
Other versions
CN111086001B (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.)
Institute of Intelligent Manufacturing of Guangdong Academy of Sciences
Original Assignee
Guangdong Institute of Intelligent Manufacturing
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 Guangdong Institute of Intelligent Manufacturing filed Critical Guangdong Institute of Intelligent Manufacturing
Priority to CN201911358566.0A priority Critical patent/CN111086001B/zh
Publication of CN111086001A publication Critical patent/CN111086001A/zh
Application granted granted Critical
Publication of CN111086001B publication Critical patent/CN111086001B/zh
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/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
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/02—Sensing devices
    • 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/1605—Simulation of manipulator lay-out, design, modelling of manipulator
    • 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
    • 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

Landscapes

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

Abstract

本发明公开了一种足式机器人多模态感知的状态估计方法及系统，所述方法包括：基于IMU传感器测量足式机器人的加速度和角速度，并预测所述足式机器人的状态均值和协方差；基于关节编码器获取所述足式机器人中所有关节的角度位置，并计算所述足式机器人中每一只脚的脚点位置所对应的计量值与预测值；基于所述计量值与所述预测值，计算所述足式机器人的测量残差；基于所述测量残差和测量雅可比，对所述状态均值和所述协方差进行校正，获取所述足式机器人的最终状态。所述方法将提高足式机器人状态估计的精确度和稳定性。

Description

一种足式机器人多模态感知的状态估计方法及系统
技术领域
本发明涉及机器人领域，尤其涉及一种足式机器人多模态感知的状态评估方法及系统。
背景技术
足式机器人的研发灵感源于哺乳动物，是仿生学与机器人技术的重要体现，其环境适应性好、运动范围广、负载能力强，具备一定的自主作业的能力，能够实现崎岖山地运输、危险灾难救援、军事侦察等任务，已受到广泛的关注。但是本体的传感器精度有限，测量数据存在不确定性，需要对其状态进行良好的估计，以执行闭环控制指令和适应非结构环境。状态估计包括对机器人运动和特征的了解，在已知的环境中充分描述机器人随时间的运动，为了用不完美的传感器获得最好的估计，估计状态时记录数据的不确定性来确定估计结果的自信程度。传统的状态评估方法是通过结合机器人的动力学模型来完成状态估计与测量模型，利用动态模型给出预期的状态，并使用机器人上的各种传感器，提供机器人的一个或多个状态量，该方法面临着几个问题：1)动力学高度非线性，以及数值解的动态模型计算不准确；2)足式机器人的状态空间很大，不仅包括身体的姿态，还有脚部关节的位置与姿态；3)足式机器人通过多个间歇性与环境进行交互地面接触和撞击，使传感器更加嘈杂。
由此可见，传统的算法通常依赖于多个本体及外部传感器的数据融合来进行状态估计，然而外部传感器受到光强度、距离测量、声音幅度等特征因素的影响，其测量可靠性无法得到保障，降低了状态估计的精确度和稳定性。另外，由于足式机器人通过断续的脚-地接触与周围环境交互，在测量中也会引入额外的噪声，如何最大程度地降低噪声的影响也是一大挑战性难题。
发明内容
本发明的目的在于克服现有技术的不足，本发明提供了一种足式机器人多模态感知的状态估计方法及系统，利用扩展卡尔曼滤波器(EKF)，通过融合IMU和运动学信息实现状态估计，降低建模的复杂性，提高状态估计的效率和可靠性。
为了解决上述技术问题，本发明实施例提供了一种足式机器人多模态感知的状态估计方法，所述方法包括：
基于IMU传感器测量足式机器人的加速度和角速度，并预测所述足式机器人的状态均值和协方差；
基于关节编码器获取所述足式机器人中所有关节的角度位置，并计算所述足式机器人中每一只脚的脚点位置所对应的计量值与预测值；
基于所述计量值与所述预测值，获取所述足式机器人的测量残差；
基于所述测量残差和测量雅可比，对所述状态均值和所述协方差进行校正，获取所述足式机器人的最终状态。
可选的，所述基于IMU传感器测量足式机器人的加速度和角速度，并预测所述足式机器人的状态均值和协方差包括：
基于所述IMU传感器中的加速度计测量所述足式机器人的线性加速度；
基于所述IMU传感器中的陀螺仪测量所述足式机器人的角速度；
基于所述线性加速度和所述角速度，通过积分变换计算所述足式机器人的状态均值和协方差，并在本体坐标系表示出来。
可选的，所述足式机器人的状态均值包括本体位置、本体速度、本体姿态的四元数、各脚部位置、IMU加速度计偏置和IMU陀螺仪偏置。
可选的，预测所述足式机器人的本体位置为：
Figure BDA0002336597000000021
预测所述足式机器人的本体速度为：
Figure BDA0002336597000000022
预测所述足式机器人的本体姿态的四元数为：
Figure BDA0002336597000000023
预测所述足式机器人的各脚部位置为：
Figure BDA0002336597000000031
预测所述足式机器人的IMU加速度计偏置为：
Figure BDA0002336597000000032
预测所述足式机器人的IMU陀螺仪偏置为：
Figure BDA0002336597000000033
其中，Δt为时间步长，vt为t时刻下的本体速度，Rwb为世界坐标系与本体坐标系间的转换矩阵，at为t时刻下IMU测量的加速度，w为IMU测量的角速度，g为重力加速度。
可选的，计算所述足式机器人中每一只脚的脚点位置所对应的计量值为：
s＝fwd_Kin(α)
计算所述足式机器人中每一只脚的脚点位置所对应的预测值为：
Figure BDA0002336597000000034
获取所述足式机器人的测量残差为：
Figure BDA0002336597000000035
其中，α为所述足式机器人的关节角度，
Figure BDA0002336597000000036
为预测的旋转矩阵，
Figure BDA0002336597000000037
为预测的第i只脚的脚点位置，
Figure BDA0002336597000000038
为预测的所述足式机器人本体的质心位置。
另外，本发明实施例还提供一种足式机器人多模态感知的状态估计系统，所述系统包括：
预测模块，用于基于IMU传感器测量足式机器人的加速度和角速度，并预测所述足式机器人的状态均值和协方差；
计算模块，用于基于关节编码器获取所述足式机器人中所有关节的角度位置，并计算所述足式机器人中每一只脚的脚点位置所对应的计量值与预测值；
获取模块，用于基于所述计量值与所述预测值，获取所述足式机器人的测量残差；
校正模块，用于基于所述测量残差和测量雅可比，对所述状态均值和所述协方差进行校正，获取所述足式机器人的最终状态。
可选的，所述预测模块包括：
第一测量单元，用于基于所述IMU传感器中的加速度计测量所述足式机器人的线性加速度；
第二测量单元，用于基于所述IMU传感器中的陀螺仪测量所述足式机器人的角速度；
计算单元，用于基于所述线性加速度和所述角速度，通过积分变换计算所述足式机器人的状态均值和协方差，并在本体坐标系表示出来。
可选的，所述足式机器人的状态均值包括本体位置、本体速度、本体姿态的四元数、各脚部位置、IMU加速度计偏置和IMU陀螺仪偏置。
可选的，所述计算单元还用于预测所述足式机器人的本体位置为：
Figure BDA0002336597000000041
预测所述足式机器人的本体速度为：
Figure BDA0002336597000000042
预测所述足式机器人的本体姿态的四元数为：
Figure BDA0002336597000000043
预测所述足式机器人的各脚部位置为：
Figure BDA0002336597000000044
预测所述足式机器人的IMU加速度计偏置为：
Figure BDA0002336597000000045
预测所述足式机器人的IMU陀螺仪偏置为：
Figure BDA0002336597000000046
其中，Δt为时间步长，vt为t时刻下的本体速度，Rwb为世界坐标系与本体坐标系间的转换矩阵，at为t时刻下IMU测量的加速度，w为IMU测量的角速度，g为重力加速度。
可选的，所述计算模块还用于计算所述足式机器人中每一只脚的脚点位置所对应的计量值为：
s＝fwd_Kin(α)
计算所述足式机器人中每一只脚的脚点位置所对应的预测值为：
Figure BDA0002336597000000051
所述获取模块还用于获取所述足式机器人的测量残差为：
Figure BDA0002336597000000052
其中，α为所述足式机器人的关节角度，
Figure BDA0002336597000000053
为预测的旋转矩阵，
Figure BDA0002336597000000054
为预测的第i只脚的脚点位置，
Figure BDA0002336597000000055
为预测的所述足式机器人本体的质心位置。
在本发明实施例中，针对传统方法中存在的信号噪声大和信息融合复杂等问题，提出一种基于扩展卡尔曼滤波器(EKF)的足式机器人状态估计方法，只考虑足式机器人本体的IMU和关节编码器信息两种模态，通过融合IMU和运动学信息来实现状态估计，降低了建模的复杂性，极大地提高了状态估计的效率；通过利用脚部与地面的接触信息来抵消来自IMU和关节编码器的噪声传感器测量，将提高足式机器人状态估计的精确度和稳定性。所述方法具备可扩展性，通过修改状态向量的信息可直接扩展应用于其他类型的足式机器人，如双足步行机器人、六足爬行机器人等。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见的，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。
图1是本发明实施例提供的一种足式机器人多模态感知的状态估计方法的流程示意图；
图2是本发明实施例提供的一种足式机器人多模态感知的状态估计系统的结构组成示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例，都属于本发明保护的范围。
图1示出了本发明实施例中的一种足式机器人多模态感知的状态估计方法的流程示意图，所述方法包括如下步骤：
S101、基于IMU传感器测量足式机器人的加速度和角速度，并预测所述足式机器人的状态均值和协方差；
需要说明的是，所述IMU传感器包括加速度计和陀螺仪，所述加速度计用于测量所述足式机器人的线性加速度，所述陀螺仪用于测量所述足式机器人的角速度。
在本发明实施例中，假定所述足式机器人在t时刻的状态由状态均值和协方差组成，所述状态均值表示为六元组：
Figure BDA0002336597000000061
其中，
Figure BDA0002336597000000062
为世界坐标系的质心位置，
Figure BDA0002336597000000063
为世界坐标系的质心速度，
Figure BDA0002336597000000064
为由本体坐标系到世界坐标系的旋转，
Figure BDA0002336597000000065
为世界坐标系中第i只脚的位置，
Figure BDA0002336597000000066
为本体坐标系中加速度计的偏差，
Figure BDA0002336597000000067
为本体坐标系中陀螺仪的偏差。
通过所述加速度计和所述陀螺仪分别对应测量出所述足式机器人在IMU坐标系中的加速度值和角速度值，并利用适当的旋转和平移运算将所述加速度值和所述角速度值分别转换为在所述足式机器人本体坐标系中表示的加速度值
Figure BDA0002336597000000068
和角速度值
Figure BDA0002336597000000069
以供算法直接运用；再将所述加速度值
Figure BDA00023365970000000610
进行一次积分得到所述足式机器人的本体速度，将
Figure BDA00023365970000000611
进行二次积分得到所述足式机器人的本体位置；最后将所述角速度值
Figure BDA00023365970000000612
进行一次积分并转换为四元数得到所述足式机器人的本体姿态信息。
具体的，利用离散系统动力学方程预测所述足式机器人的状态均值中各参数值分别如下：
所述足式机器人的本体位置为：
Figure BDA00023365970000000613
所述足式机器人的本体速度为：
Figure BDA00023365970000000614
所述足式机器人的本体姿态的四元数为：
Figure BDA0002336597000000071
所述足式机器人的各脚部位置为：
Figure BDA0002336597000000072
所述足式机器人的IMU加速度计偏置为：
Figure BDA0002336597000000073
所述足式机器人的IMU陀螺仪偏置为：
Figure BDA0002336597000000074
其中，Δt为时间步长，vt为t时刻下的本体速度，Rwb为世界坐标系与本体坐标系间的转换矩阵，at为t时刻下IMU测量的加速度，w为IMU测量的角速度，g为重力加速度。
利用误差动力学模型预测所述足式机器人的协方差为：
Figure BDA0002336597000000075
其中，
Figure BDA0002336597000000076
为预测状态的协议差矩阵，
Figure BDA0002336597000000077
为当前状态的协方差矩阵，Ft为雅可比矩阵，
Figure BDA0002336597000000078
为雅可比矩阵的转置，Qt为高斯噪声协方差矩阵。
需要说明的是，所述状态均值向量的总维数为16+3N，状态的协方差是由一个大小为正的半定矩阵表示，大小为(15+3N)×(15+3N)，N为足式机器人的腿数。
S102、基于关节编码器获取所述足式机器人中所有关节的角度位置，并计算所述足式机器人中每一只脚的脚点位置所对应的计量值与预测值；
具体的，通过所述关节编码器获取所述足式机器人中所有关节的角度位置，并利用机器人的运动学原理，以腿为中心建立坐标系来估计每一只脚的位置，再利用适当的旋转和平移运算转换为所述足式机器人本体坐标系中表示。
假设所述足式机器人与地面接触的每一只脚在步态周期的一段时间内保持静止，在连续的时间间隙之间进行匹配，以计算增量运动。由于所述关节编码器的噪声比所述IMU传感器小，故利用正向运动学习计算出所述足式机器人中每一只脚的脚点位置所对应的计量值为：
s＝fwd_Kin(α)
计算出所述足式机器人中每一只脚的脚点位置所对应的预测值为：
Figure BDA0002336597000000081
其中，α为所述足式机器人的关节角度，
Figure BDA0002336597000000082
为预测的旋转矩阵，
Figure BDA0002336597000000083
为预测的第i只脚的脚点位置，
Figure BDA0002336597000000084
为预测的所述足式机器人本体的质心位置。
需要说明的是，为保证所述足式机器人的状态一致性，将通过插补的方法实现不同传感器信息的同步处理与降噪预处理，包括所述加速度计、所述陀螺仪和所述关节编码器。另外，所述足式机器人的脚步与地面的接触信息是从力矩传感器的力矩信号中提取出来的，可抵消来自所述IMU传感器和所述关节编码器的噪声传感器的影响。
S103、基于所述计量值与所述预测值，获取所述足式机器人的测量残差；
具体的，根据步骤S102，获取所述足式机器人的测量残差为：
Figure BDA0002336597000000085
S104、基于所述测量残差和测量雅可比，对所述状态均值和所述协方差进行校正，获取所述足式机器人的最终状态。
具体的，利用外部传感器(如运动捕捉装置)的信息融合得到真值后与步骤S101所得的预测值进行线性化计算，获取误差状态；基于所述误差状态，利用去除高阶项的方法计算测量雅可比；基于步骤S103中的所述测量残差和所述测量雅可比，计算状态修正值，并与所述预测值进行叠加，得到所述足式机器人的最终状态。
相应的，图2示出了本发明实施例中的一种足式机器人多模态感知的状态估计系统的结构组成示意图，所述系统包括：
预测模块201，用于基于IMU传感器测量足式机器人的加速度和角速度，并预测所述足式机器人的状态均值和协方差；
具体的，所述预测模块包括第一测量单元、第二测量单元和计算单元；其中，所述第一测量单元用于基于所述IMU传感器中的加速度计测量所述足式机器人的线性加速度；所述第二测量单元用于基于所述IMU传感器中的陀螺仪测量所述足式机器人的角速度；所述计算单元用于基于所述线性加速度和所述角速度，通过积分变换计算所述足式机器人的状态均值和协方差，并在本体坐标系表示出来。
需要说明的是，所述足式机器人的状态均值包括本体位置、本体速度、本体姿态的四元数、各脚部位置、IMU加速度计偏置和IMU陀螺仪偏置。
在本发明实施例中，所述计算单元还用于预测所述足式机器人的本体位置为：
Figure BDA0002336597000000091
预测所述足式机器人的本体速度为：
Figure BDA0002336597000000092
预测所述足式机器人的本体姿态的四元数为：
Figure BDA0002336597000000093
预测所述足式机器人的各脚部位置为：
Figure BDA0002336597000000094
预测所述足式机器人的IMU加速度计偏置为：
Figure BDA0002336597000000095
预测所述足式机器人的IMU陀螺仪偏置为：
Figure BDA0002336597000000096
其中，Δt为时间步长，vt为t时刻下的本体速度，Rwb为世界坐标系与本体坐标系间的转换矩阵，at为t时刻下IMU测量的加速度，w为IMU测量的角速度，g为重力加速度。
计算模块202，用于基于关节编码器获取所述足式机器人中所有关节的角度位置，并计算所述足式机器人中每一只脚的脚点位置所对应的计量值与预测值；
具体的，所述计算模块还用于计算所述足式机器人中每一只脚的脚点位置所对应的计量值为：
s＝fwd_Kin(α)
计算所述足式机器人中每一只脚的脚点位置所对应的预测值为：
Figure BDA0002336597000000097
其中，α为所述足式机器人的关节角度，
Figure BDA0002336597000000098
为预测的旋转矩阵，
Figure BDA0002336597000000099
为预测的第i只脚的脚点位置，
Figure BDA0002336597000000101
为预测的所述足式机器人本体的质心位置。
获取模块203，用于基于所述计量值与所述预测值，获取所述足式机器人的测量残差；
具体的，获取所述足式机器人的测量残差为：
Figure BDA0002336597000000102
校正模块204，用于基于所述测量残差和测量雅可比，对所述状态均值和所述协方差进行校正，获取所述足式机器人的最终状态。
在本发明实施例中，针对传统方法中存在的信号噪声大和信息融合复杂等问题，提出一种基于扩展卡尔曼滤波器(EKF)的足式机器人状态估计方法，只考虑足式机器人本体的IMU和关节编码器信息两种模态，通过融合IMU和运动学信息来实现状态估计，降低了建模的复杂性，极大地提高了状态估计的效率；通过利用脚部与地面的接触信息来抵消来自IMU和关节编码器的噪声传感器测量，将提高足式机器人状态估计的精确度和稳定性。所述方法具备可扩展性，通过修改状态向量的信息可直接扩展应用于其他类型的足式机器人，如双足步行机器人、六足爬行机器人等。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，ReadOnly Memory)、随机存取存储器(RAM，Random AccessMemory)、磁盘或光盘等。
另外，以上对本发明实施例所提供的一种足式机器人多模态感知的状态估计方法及系统进行了详细介绍，本文中应采用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种足式机器人多模态感知的状态估计方法，其特征在于，所述方法包括：
基于IMU传感器测量足式机器人的加速度和角速度，并预测所述足式机器人的状态均值和协方差；
基于关节编码器获取所述足式机器人中所有关节的角度位置，并计算所述足式机器人中每一只脚的脚点位置所对应的计量值与预测值；
基于所述计量值与所述预测值，获取所述足式机器人的测量残差；
基于所述测量残差和测量雅可比，对所述状态均值和所述协方差进行校正，获取所述足式机器人的最终状态。
2.根据权利要求1所述的状态评估方法，其特征在于，所述基于IMU传感器测量足式机器人的加速度和角速度，并预测所述足式机器人的状态均值和协方差包括：
基于所述IMU传感器中的加速度计测量所述足式机器人的线性加速度；
基于所述IMU传感器中的陀螺仪测量所述足式机器人的角速度；
基于所述线性加速度和所述角速度，通过积分变换计算所述足式机器人的状态均值和协方差，并在本体坐标系表示出来。
3.根据权利要求2所述的状态评估方法，其特征在于，所述足式机器人的状态均值包括本体位置、本体速度、本体姿态的四元数、各脚部位置、IMU加速度计偏置和IMU陀螺仪偏置。
4.根据权利要求3所述的状态评估方法，其特征在于，预测所述足式机器人的本体位置为：
Figure FDA0002336596990000011
预测所述足式机器人的本体速度为：
Figure FDA0002336596990000021
预测所述足式机器人的本体姿态的四元数为：
Figure FDA0002336596990000022
预测所述足式机器人的各脚部位置为：
Figure FDA0002336596990000023
预测所述足式机器人的IMU加速度计偏置为：
Figure FDA0002336596990000024
预测所述足式机器人的IMU陀螺仪偏置为：
Figure FDA0002336596990000025
其中，Δt为时间步长，vt为t时刻下的本体速度，Rwb为世界坐标系与本体坐标系间的转换矩阵，at为t时刻下IMU测量的加速度，w为IMU测量的角速度，g为重力加速度。
5.根据权利要求1所述的状态评估方法，其特征在于，计算所述足式机器人中每一只脚的脚点位置所对应的计量值为：
s＝fwd_Kin(α)
计算所述足式机器人中每一只脚的脚点位置所对应的预测值为：
Figure FDA0002336596990000026
获取所述足式机器人的测量残差为：
Figure FDA0002336596990000027
其中，α为所述足式机器人的关节角度，
Figure FDA0002336596990000028
为预测的旋转矩阵，
Figure FDA0002336596990000029
为预测的第i只脚的脚点位置，
Figure FDA00023365969900000210
为预测的所述足式机器人本体的质心位置。
6.一种足式机器人多模态感知的状态估计系统，其特征在于，所述系统包括：
预测模块，用于基于IMU传感器测量足式机器人的加速度和角速度，并预测所述足式机器人的状态均值和协方差；
计算模块，用于基于关节编码器获取所述足式机器人中所有关节的角度位置，并计算所述足式机器人中每一只脚的脚点位置所对应的计量值与预测值；
获取模块，用于基于所述计量值与所述预测值，获取所述足式机器人的测量残差；
校正模块，用于基于所述测量残差和测量雅可比，对所述状态均值和所述协方差进行校正，获取所述足式机器人的最终状态。
7.根据权利要求6所述的状态估计系统，其特征在于，所述预测模块包括：
第一测量单元，用于基于所述IMU传感器中的加速度计测量所述足式机器人的线性加速度；
第二测量单元，用于基于所述IMU传感器中的陀螺仪测量所述足式机器人的角速度；
计算单元，用于基于所述线性加速度和所述角速度，通过积分变换计算所述足式机器人的状态均值和协方差，并在本体坐标系表示出来。
8.根据权利要求7所述的状态评估系统，其特征在于，所述足式机器人的状态均值包括本体位置、本体速度、本体姿态的四元数、各脚部位置、IMU加速度计偏置和IMU陀螺仪偏置。
9.根据权利要求8所述的状态评估系统，其特征在于，
所述计算单元还用于预测所述足式机器人的本体位置为：
Figure FDA0002336596990000031
预测所述足式机器人的本体速度为：
Figure FDA0002336596990000032
预测所述足式机器人的本体姿态的四元数为：
Figure FDA0002336596990000033
预测所述足式机器人的各脚部位置为：
Figure FDA0002336596990000041
预测所述足式机器人的IMU加速度计偏置为：
Figure FDA0002336596990000042
预测所述足式机器人的IMU陀螺仪偏置为：
Figure FDA0002336596990000043
其中，Δt为时间步长，vt为t时刻下的本体速度，Rwb为世界坐标系与本体坐标系间的转换矩阵，at为t时刻下IMU测量的加速度，w为IMU测量的角速度，g为重力加速度。
10.根据权利要求1所述的状态评估系统，其特征在于，
所述计算模块还用于计算所述足式机器人中每一只脚的脚点位置所对应的计量值为：
s＝fwd_Kin(α)
计算所述足式机器人中每一只脚的脚点位置所对应的预测值为：
Figure FDA0002336596990000044
所述获取模块还用于获取所述足式机器人的测量残差为：
Figure FDA0002336596990000045
其中，α为所述足式机器人的关节角度，
Figure FDA0002336596990000046
为预测的旋转矩阵，
Figure FDA0002336596990000047
为预测的第i只脚的脚点位置，
Figure FDA0002336596990000048
为预测的所述足式机器人本体的质心位置。
CN201911358566.0A 2019-12-25 2019-12-25 一种足式机器人多模态感知的状态估计方法及系统 Active CN111086001B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911358566.0A CN111086001B (zh) 2019-12-25 2019-12-25 一种足式机器人多模态感知的状态估计方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911358566.0A CN111086001B (zh) 2019-12-25 2019-12-25 一种足式机器人多模态感知的状态估计方法及系统

Publications (2)

Publication Number Publication Date
CN111086001A true CN111086001A (zh) 2020-05-01
CN111086001B CN111086001B (zh) 2021-09-14

Family

ID=70397255

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911358566.0A Active CN111086001B (zh) 2019-12-25 2019-12-25 一种足式机器人多模态感知的状态估计方法及系统

Country Status (1)

Country Link
CN (1) CN111086001B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112947428A (zh) * 2021-02-02 2021-06-11 广东工业大学 一种四足机器人的移动控制方法及装置
CN113253748A (zh) * 2021-06-08 2021-08-13 深圳市优必选科技股份有限公司 并联构型仿人机器人的状态估计方法、装置、设备及介质
CN113358121A (zh) * 2021-06-10 2021-09-07 中国北方车辆研究所 一种电驱动昆虫构型足式机器人足地滑移估计方法
CN113405548A (zh) * 2021-06-10 2021-09-17 中国北方车辆研究所 一种电驱动哺乳构型足式机器人足地滑移估计方法
WO2024037155A1 (zh) * 2022-08-17 2024-02-22 腾讯科技（深圳）有限公司 用于足式机器人的状态估计方法、装置、计算机设备及计算机可读存储介质

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1361430A (zh) * 2000-12-23 2002-07-31 林清芳 增强的运动体定位和导航方法与系统
US20070083290A1 (en) * 2005-10-12 2007-04-12 Kenichiro Nagasaka Apparatus and method for computing operational-space physical quantity
CN101285686A (zh) * 2008-05-29 2008-10-15 中国农业大学 一种农业机械导航分级定位的方法和系统
EP2040036A2 (en) * 2007-09-18 2009-03-25 Honeywell International Inc. Method of personal navigation using stride vectoring
US20130131865A1 (en) * 2011-11-22 2013-05-23 Disney Enterprises, Inc. Kinematic and Dynamic Calibration Methods for Legged Robots with Force-Controlled Joints
US20160052574A1 (en) * 2014-08-25 2016-02-25 Google Inc. Natural Pitch and Roll
CN107289933A (zh) * 2017-06-28 2017-10-24 东南大学 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法
US9926025B1 (en) * 2014-08-25 2018-03-27 Boston Dynamics, Inc. Slip avoidance
CN108621161A (zh) * 2018-05-08 2018-10-09 中国人民解放军国防科技大学 基于多传感器信息融合的足式机器人本体状态估计方法
US20190079532A1 (en) * 2013-07-02 2019-03-14 Ubiquity Robotics, Inc. Versatile autonomous mobile platform with 3-d imaging system
CN109807911A (zh) * 2019-03-14 2019-05-28 湖南超能机器人技术有限公司 基于gnss、uwb、imu、激光雷达、码盘的室外巡逻机器人多环境联合定位方法
CN109857131A (zh) * 2019-03-11 2019-06-07 山东职业学院 一种足式机器人两足-四足姿态变换控制方法
CN110490273A (zh) * 2019-09-12 2019-11-22 河南牧业经济学院 噪声方差不精确建模的多传感器系统融合滤波算法
CN110561424A (zh) * 2019-07-28 2019-12-13 华南理工大学 基于多传感器混合滤波器的在线机器人运动学校准方法

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1361430A (zh) * 2000-12-23 2002-07-31 林清芳 增强的运动体定位和导航方法与系统
US20070083290A1 (en) * 2005-10-12 2007-04-12 Kenichiro Nagasaka Apparatus and method for computing operational-space physical quantity
EP2040036A2 (en) * 2007-09-18 2009-03-25 Honeywell International Inc. Method of personal navigation using stride vectoring
CN101285686A (zh) * 2008-05-29 2008-10-15 中国农业大学 一种农业机械导航分级定位的方法和系统
US20130131865A1 (en) * 2011-11-22 2013-05-23 Disney Enterprises, Inc. Kinematic and Dynamic Calibration Methods for Legged Robots with Force-Controlled Joints
US20190079532A1 (en) * 2013-07-02 2019-03-14 Ubiquity Robotics, Inc. Versatile autonomous mobile platform with 3-d imaging system
US9926025B1 (en) * 2014-08-25 2018-03-27 Boston Dynamics, Inc. Slip avoidance
US20160052574A1 (en) * 2014-08-25 2016-02-25 Google Inc. Natural Pitch and Roll
CN107289933A (zh) * 2017-06-28 2017-10-24 东南大学 基于mems传感器和vlc定位融合的双卡尔曼滤波导航装置和方法
CN108621161A (zh) * 2018-05-08 2018-10-09 中国人民解放军国防科技大学 基于多传感器信息融合的足式机器人本体状态估计方法
CN109857131A (zh) * 2019-03-11 2019-06-07 山东职业学院 一种足式机器人两足-四足姿态变换控制方法
CN109807911A (zh) * 2019-03-14 2019-05-28 湖南超能机器人技术有限公司 基于gnss、uwb、imu、激光雷达、码盘的室外巡逻机器人多环境联合定位方法
CN110561424A (zh) * 2019-07-28 2019-12-13 华南理工大学 基于多传感器混合滤波器的在线机器人运动学校准方法
CN110490273A (zh) * 2019-09-12 2019-11-22 河南牧业经济学院 噪声方差不精确建模的多传感器系统融合滤波算法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
PEI-CHUN LIN，ET AL.: "Sensor Data Fusion for Body State Estimation in a Hexapod Robot with Dynamical Gaits", 《IEEE TRANSACTIONS ON ROBOTICS》 *
PRIYANSHU AGARWAL，ET AL.: "State Estimation for Legged Robots: Consistent Fusion of Leg Kinematics and IMU", 《ROBOTICS: SCIENCE AND SYSTEMS VIII》 *
王聪伟: "基于扩展卡尔曼滤波的足式机器人运动速度估计研究", 《中国优秀硕士学位论文全文数据库（电子期刊）信息科技辑》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112947428A (zh) * 2021-02-02 2021-06-11 广东工业大学 一种四足机器人的移动控制方法及装置
CN112947428B (zh) * 2021-02-02 2023-12-22 广东工业大学 一种四足机器人的移动控制方法及装置
CN113253748A (zh) * 2021-06-08 2021-08-13 深圳市优必选科技股份有限公司 并联构型仿人机器人的状态估计方法、装置、设备及介质
CN113253748B (zh) * 2021-06-08 2022-02-22 深圳市优必选科技股份有限公司 并联构型仿人机器人的状态估计方法、装置、设备及介质
CN113358121A (zh) * 2021-06-10 2021-09-07 中国北方车辆研究所 一种电驱动昆虫构型足式机器人足地滑移估计方法
CN113405548A (zh) * 2021-06-10 2021-09-17 中国北方车辆研究所 一种电驱动哺乳构型足式机器人足地滑移估计方法
WO2024037155A1 (zh) * 2022-08-17 2024-02-22 腾讯科技（深圳）有限公司 用于足式机器人的状态估计方法、装置、计算机设备及计算机可读存储介质

Also Published As

Publication number Publication date
CN111086001B (zh) 2021-09-14

Similar Documents

Publication Publication Date Title
CN111086001B (zh) 一种足式机器人多模态感知的状态估计方法及系统
CN108621161B (zh) 基于多传感器信息融合的足式机器人本体状态估计方法
JP4876204B2 (ja) 小型姿勢センサ
CN113064433B (zh) 机器人迈步控制方法、装置、机器人控制设备及存储介质
Carpentier et al. Recent progress in legged robots locomotion control
KR100543709B1 (ko) 이동체의 회전 이동량 이용 방법과 장치 및 컴퓨터프로그램을 저장하는 컴퓨터로 읽을 수 있는 기록 매체
CN111949929B (zh) 一种多传感器融合的四足机器人运动里程计设计方法
CN112596534A (zh) 基于深度强化学习的四足机器人的步态训练方法、装置、电子设备及介质
Oriolo et al. Vision-based odometric localization for humanoids using a kinematic EKF
Vigne et al. State estimation for a legged robot with multiple flexibilities using imu s: A kinematic approach
Vigne et al. MOVIE: A velocity-aided IMU attitude estimator for observing and controlling multiple deformations on legged robots
CN110967017A (zh) 一种用于双移动机器人刚体协作搬运的协同定位方法
CN114102584A (zh) 一种小型高机动救援机器人行走及作业稳定控制方法
Masuya et al. Com motion estimation of a humanoid robot based on a fusion of dynamics and kinematics information
Benallegue et al. Lyapunov-stable orientation estimator for humanoid robots
Gür et al. Model-based proprioceptive state estimation for spring-mass running
CN105818145A (zh) 仿人机器人的分布式控制系统及方法
CN113119102B (zh) 基于浮动基飞轮倒立摆的仿人机器人建模方法和装置
CN112276950A (zh) 抗扰动模型训练、控制方法、装置、设备、机器人及介质
CN114137840B (zh) 足式机器人全局平衡控制方法、装置和足式机器人
Lowrey et al. Real-time state estimation with whole-body multi-contact dynamics: A modified UKF approach
Shi et al. An attitude estimation system for amphibious spherical robots
Lubbe et al. State estimation for a hexapod robot
Alai et al. New approach to center of mass estimation for humanoid robots based on sensor measurements and general lipm
Liu et al. State estimation of a heavy-duty hexapod robot with passive compliant ankles based on the leg kinematics and IMU data fusion

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
CP01 Change in the name or title of a patent holder
CP01 Change in the name or title of a patent holder

Address after: 510070 Building No. 15, compound 100, martyrs Road, Guangzhou, Guangdong

Patentee after: Institute of intelligent manufacturing, Guangdong Academy of Sciences

Address before: 510070 Building No. 15, compound 100, martyrs Road, Guangzhou, Guangdong

Patentee before: GUANGDONG INSTITUTE OF INTELLIGENT MANUFACTURING