CN117008623A - 多足机器人的状态估计方法及相关设备 - Google Patents
多足机器人的状态估计方法及相关设备 Download PDFInfo
- Publication number
- CN117008623A CN117008623A CN202210609440.1A CN202210609440A CN117008623A CN 117008623 A CN117008623 A CN 117008623A CN 202210609440 A CN202210609440 A CN 202210609440A CN 117008623 A CN117008623 A CN 117008623A
- Authority
- CN
- China
- Prior art keywords
- current
- legged robot
- leg
- robot
- plane
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 154
- 238000004364 calculation method Methods 0.000 claims abstract description 147
- 230000008569 process Effects 0.000 claims abstract description 88
- 230000033001 locomotion Effects 0.000 claims abstract description 73
- 239000013598 vector Substances 0.000 claims description 107
- 230000001133 acceleration Effects 0.000 claims description 62
- 230000009471 action Effects 0.000 claims description 44
- 238000012545 processing Methods 0.000 claims description 25
- 239000011159 matrix material Substances 0.000 claims description 20
- 238000001914 filtration Methods 0.000 claims description 18
- 230000035772 mutation Effects 0.000 claims description 17
- 230000008859 change Effects 0.000 claims description 15
- 238000004590 computer program Methods 0.000 claims description 15
- 238000001514 detection method Methods 0.000 claims description 11
- 238000007499 fusion processing Methods 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 3
- 210000002414 leg Anatomy 0.000 description 355
- 230000000875 corresponding effect Effects 0.000 description 119
- 238000010586 diagram Methods 0.000 description 26
- 238000005516 engineering process Methods 0.000 description 11
- 230000006870 function Effects 0.000 description 10
- 230000001276 controlling effect Effects 0.000 description 4
- 238000012888 cubic function Methods 0.000 description 4
- 239000011800 void material Substances 0.000 description 4
- 230000005484 gravity Effects 0.000 description 3
- 230000007306 turnover Effects 0.000 description 3
- 241000238631 Hexapoda Species 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000001419 dependent effect Effects 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 239000000725 suspension Substances 0.000 description 2
- 241001265525 Edgeworthia chrysantha Species 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004422 calculation algorithm Methods 0.000 description 1
- 239000013256 coordination polymer Substances 0.000 description 1
- 230000002596 correlated effect Effects 0.000 description 1
- 238000013135 deep learning Methods 0.000 description 1
- 239000011521 glass Substances 0.000 description 1
- 210000004394 hip joint Anatomy 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000002452 interceptive effect Effects 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 210000000629 knee joint Anatomy 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 238000003058 natural language processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001960 triggered effect Effects 0.000 description 1
- 210000001364 upper extremity Anatomy 0.000 description 1
- 210000000689 upper leg Anatomy 0.000 description 1
Landscapes
- Manipulator (AREA)
Abstract
本申请公开一种多足机器人的状态估计方法及相关设备,方法包括:在多足机器人的空翻过程中,根据当前时刻采集到的多足机器人的传感信息,计算多足机器人的各条机械腿在当前时刻的位置,得到位置计算结果;根据历史得到的多足机器人在当前时刻的状态估计结果,观测多足机器人的各条机械腿在当前时刻的位置,得到位置观测结果;根据当前时刻所处的目标阶段对应的运动形态,确定状态估计所需的参考信息;参考信息指示:在状态估计时,分别对位置计算结果和位置观测结果的信任程度;根据参考信息、位置计算结果以及位置观测结果,实现对多足机器人在当前时刻的下一时刻的状态进行估计,并提升状态估计的准确性以及状态估计的适用性。
Description
技术领域
本申请涉及机器人技术领域,具体涉及机器人规划控制技术领域,尤其涉及一种多足机器人的状态估计方法及相关设备。
背景技术
目前,在多足机器人(如四足机器人)的空翻过程中,通常可使用动作捕捉(MotionCapture)手段来实时确定多足机器人的状态(如位置和姿态信息等);但在机器人领域众所周知,动作捕捉手段需在多足机器人以外的环境内假设全局摄像头,其一般用于在多足机器人真机调试过程中以得到感知数据的真值。也就是说,动作捕捉手段本质上是一种调试手段;其需约束多足机器人在特定的实验室区域内运行,离开实验室环境,动作捕捉手段便无法工作。
可见,动作捕捉手段只能用于实验室环境中,且其主要用于在多足机器人的空翻过程中,实时确定多足机器人在当前时刻的状态,无法对多足机器人在下一时刻的状态进行估计。基于此,如何在多足机器人的空翻过程中,对多足机器人在下一时刻的状态进行估计成为了研究热点。
发明内容
本申请实施例提供了一种多足机器人的状态估计方法及相关设备,可以实现对多足机器人在下一时刻的状态进行估计,并提升状态估计的准确性以及状态估计的适用性。
一方面,本申请实施例提供了一种多足机器人的状态估计方法,所述多足机器人在平面上执行空翻动作,且空翻过程被划分为连续的多个阶段,一个阶段对应所述多足机器人的一种运动形态,任一种运动形态用于指示所述多足机器人的各条机械腿在相应阶段中与所述平面之间的接触情况;所述方法包括:
在所述多足机器人的空翻过程中,根据当前时刻采集到的所述多足机器人的传感信息,计算所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置计算结果;
根据历史得到的所述多足机器人在所述当前时刻的状态估计结果,观测所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置观测结果;
从所述多个阶段中确定所述当前时刻所处的目标阶段,并根据所述目标阶段对应的运动形态,确定状态估计所需的参考信息;所述参考信息指示:在状态估计时,分别对所述位置计算结果和所述位置观测结果的信任程度;
根据所述参考信息、所述位置计算结果以及所述位置观测结果,对所述多足机器人在所述当前时刻的下一时刻的状态进行估计,得到所述多足机器人在所述下一时刻的状态估计结果。
另一方面,本申请实施例提供了一种多足机器人的状态估计装置,所述多足机器人在平面上执行空翻动作,且空翻过程被划分为连续的多个阶段,一个阶段对应所述多足机器人的一种运动形态,任一种运动形态用于指示所述多足机器人的各条机械腿在相应阶段中与所述平面之间的接触情况;所述装置包括:
处理单元,用于在所述多足机器人的空翻过程中,根据当前时刻采集到的所述多足机器人的传感信息,计算所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置计算结果;
所述处理单元,还用于根据历史得到的所述多足机器人在所述当前时刻的状态估计结果,观测所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置观测结果;
估计单元,用于从所述多个阶段中确定所述当前时刻所处的目标阶段,并根据所述目标阶段对应的运动形态,确定状态估计所需的参考信息;所述参考信息指示:在状态估计时,分别对所述位置计算结果和所述位置观测结果的信任程度;
所述估计单元,还用于根据所述参考信息、所述位置计算结果以及所述位置观测结果,对所述多足机器人在所述当前时刻的下一时刻的状态进行估计,得到所述多足机器人在所述下一时刻的状态估计结果。
再一方面,本申请实施例提供了一种计算机设备,所述计算机设备包括输入接口和输出接口,所述计算机设备还包括:
处理器,适于实现一条或多条指令;以及,
计算机存储介质,所述计算机存储介质存储有一条或多条指令,所述一条或多条指令适于由所述处理器加载并执行上述所提及的多足机器人的状态估计方法。其中,多足机器人的状态估计方法,所述多足机器人在平面上执行空翻动作,且空翻过程被划分为连续的多个阶段,一个阶段对应所述多足机器人的一种运动形态,任一种运动形态用于指示所述多足机器人的各条机械腿在相应阶段中与所述平面之间的接触情况。
示例性的,所述一条或多条指令适于由所述处理器加载并执行如下步骤:
在所述多足机器人的空翻过程中,根据当前时刻采集到的所述多足机器人的传感信息,计算所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置计算结果;
根据历史得到的所述多足机器人在所述当前时刻的状态估计结果,观测所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置观测结果;
从所述多个阶段中确定所述当前时刻所处的目标阶段,并根据所述目标阶段对应的运动形态,确定状态估计所需的参考信息;所述参考信息指示:在状态估计时,分别对所述位置计算结果和所述位置观测结果的信任程度;
根据所述参考信息、所述位置计算结果以及所述位置观测结果,对所述多足机器人在所述当前时刻的下一时刻的状态进行估计,得到所述多足机器人在所述下一时刻的状态估计结果。
再一方面,本申请实施例提供了一种计算机存储介质,所述计算机存储介质存储有一条或多条指令,所述一条或多条指令适于由处理器加载并执行上述所提及的多足机器人的状态估计方法。
再一方面,本申请实施例提供了一种计算机程序产品,该计算机程序产品包括计算机程序;所述计算机程序被处理器执行时,实现上述所提及的多足机器人的状态估计方法。
本申请实施例可在多足机器人在平面上执行空翻动作的过程中,根据当前时刻采集到的多足机器人的传感信息,计算多足机器人的各条机械腿在当前时刻的位置,并根据历史得到的多足机器人在当前时刻的状态估计结果,观测各条机械腿在当前时刻的位置,从而根据相应的位置计算结果和位置观测结果来实现对多足机器人在下一时刻的状态进行估计。并且,通过将空翻过程的多阶段划分运用到空翻状态估计中,可实现根据当前时刻所处的目标阶段对应的运动形态,确定位置计算结果和位置观测结果在状态估计时被信任的程度,从而使得在根据位置计算结果和位置观测结果估计多足机器人在下一时刻的状态时,可进一步参考这两个结果中的各结果被信任的程度,这样可提升状态估计的准确性。并且,本申请实施例可实现对任一环境中的多足机器人进行状态估计,其并不局限于实验室环境,因此可见本申请实施例还可提升状态估计的适用性。
附图说明
为了更清楚地说明本申请实施例技术方案,下面将对实施例描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1a是本申请实施例提供的一种三次样条差值的计算原理示意图;
图1b是本申请实施例提供的一种四足机器人的空翻过程的示意图;
图1c是本申请实施例提供的一种多足机器人的状态估计方案的方案原理的示意图;
图1d是本申请实施例提供的另一种多足机器人的状态估计方案的方案原理的示意图;
图1e是本申请实施例提供的一种对四足机器人进行简化所得到的二维平面模型中的虚拟机器人的示意图;
图1f是本申请实施例提供的一种空翻过程的阶段划分的示意图;
图2是本申请实施例提供的一种多足机器人的状态估计方法的流程示意图;
图3是本申请另一实施例提供的一种多足机器人的状态估计方法的流程示意图;
图4a是本申请实施例提供的一种在四足机器人的空翻过程中,四足机器人的质心在世界坐标系中的x方向上的位置的示意图;
图4b是本申请实施例提供的一种在四足机器人的空翻过程中,四足机器人的质心在世界坐标系中的z方向上的位置示意图;
图4c是本申请实施例提供的一种在四足机器人的空翻过程中,四足机器人的质心在世界坐标系中的俯仰角姿态的示意图;
图4d是本申请示例提供的一种在四足机器人的空翻过程中,四足机器人的质心在x方向和z方向的位置的示意图;
图4e是本申请实施例提供的一种时刻标记图;
图4f是本申请实施例提供的一种四足机器人的四条机械腿的足端位置的示意图;
图4g是本申请实施例提供的一种四足机器人的质心在世界坐标系中的x方向上的加速度的示意图;
图4h是本申请实施例提供的一种四足机器人的质心在世界坐标系中的z方向上的加速度的示意图;
图4i是本申请实施例提供的一种四足机器人的四个足端在世界坐标系下的受力情况的示意图;
图5a是本申请实施例提供的另一种在四足机器人的空翻过程中,四足机器人的质心在世界坐标系中的x方向上的位置的示意图;
图5b是本申请实施例提供的另一种在四足机器人的空翻过程中,四足机器人的质心在世界坐标系中的z方向上的位置示意图;
图5c是本申请实施例提供的另一种在四足机器人的空翻过程中,四足机器人的质心在世界坐标系中的俯仰角姿态的示意图;
图5d是本申请示例提供的另一种在四足机器人的空翻过程中,四足机器人的质心在x方向和z方向的位置的示意图;
图5e是本申请实施例提供的另一种时刻标记图;
图5f是本申请实施例提供的另一种四足机器人的四条机械腿的足端位置的示意图;
图5g是本申请实施例提供的另一种四足机器人的质心在世界坐标系中的x方向上的加速度的示意图;
图5h是本申请实施例提供的另一种四足机器人的质心在世界坐标系中的z方向上的加速度的示意图;
图5i是本申请实施例提供的另一种四足机器人的四个足端在世界坐标系下的受力情况的示意图;
图6是本申请实施例提供的一种多足机器人的状态估计装置的结构示意图;
图7是本申请实施例提供的一种计算机设备的结构示意图。
具体实施方式
下面将结合本申请实施例中的附图,对本申请实施例中的技术方案进行清楚、完整地描述。
在本申请实施例中,多足机器人是指基于多条机械腿进行移动的机器人,例如其可以是四足机器人、六足机器人等。此处的四足机器人是指除了身体之外,具有四条机械腿的机器人;六足机器人则是指除了身体之外,具有六条机械腿的机器人。多足机器人的每条机械腿可包括大腿和小腿,且每条机械腿可包括至少一个关节;除此之外,每条机械腿还可配置有三个电机,每条机械腿上的三个电机可以分别用来控制多足机器人的腿平面与身体的夹角、Hip关节(髋关节)的角度和Knee关节(膝关节)的角度。可见,整个四足机器人的姿态控制,可由控制每条机械腿上的三个电机,共计12个电机来实现;同理,整个六足机器人的姿态控制,可由控制每条机械腿上的三个电机,共计18个电机来实现。需要说明的是,多足机器人上还可配置各式各样的传感器,如IMU(Inertial Measurement Unit,惯性测量单元)传感器和关节角编码器等;其中,IMU传感器可以实时地提供多足机器人的加速度和姿态信息,关节角编码器可以实时地提供多足机器人的各个关节的关节角信息(如关节角的角度、角速度反馈值等)。
基于人工智能(Artificial Intelligence,AI)技术,可实现多足机器人在合理的规划下,在平面(如地面、桌面或其他水平面等)上执行并完成空翻动作。所谓的AI是利用数字计算机或者数字计算机控制的机器模拟、延伸和扩展人的智能,感知环境、获取知识并使用知识获得最佳结果的理论、方法、技术及应用系统;换句话说,AI是计算机科学的一个综合技术,其企图了解智能的实质,并生产出一种新的能以人类智能相似的方式做出反应的智能机器(例如机器人等)。相应的,AI技术是一门综合学科,涉及领域广泛,既有硬件层面的技术也有软件层面的技术;在硬件层面,其一般包括如传感器、专用人工智能芯片、云计算、分布式存储、大数据处理技术、操作/交互系统、机电一体化等技术,在软件层面,其主要包括计算机视觉技术、语音处理技术、自然语言处理技术以及机器学习/深度学习等几大方向。
具体的,空翻动作的完成可涉及机器人动力学模型的建立、基于机器人动力学模型的姿态和轨迹规划、以及基于规划轨迹的运动控制等过程。其中,规划和控制过程可根据多足机器人的姿态轨迹中所涉及的关键帧进行三次样条差值(Cubic SplineInterpolation)实现;所谓的三次样条差值就是把已知数据分割成若干段,每段构造一个三次函数,并且保证分段函数的衔接处具有0阶连续,一阶导数连续,二阶导数连续的性质(也就是光滑衔接)。示例性的,设已知数据包括(pa,va,ta)、(pb,tb)以及(pc,vc,tc)等三个数据,p表示位置,v表示速度,t表示时间,且将已知数据分割成两段为例,三次样条差值的计算原理可参见图1a所示:可先构造得到f1(t)和f2(t)两个三次函数,然后可基于这两个三次函数和已知数据建立等式方程组,从而通过该等式方程组求解出三次多项式系数(a0,a1,a2,a3,b0,b1,b2,b3);在求解出三次多项式系数后,后续便可利用这两个三次函数确定多足机器人在任一时刻所处的位置以及相应的速度,从而实现多足机器人的控制。
需要说明的是,本申请实施例对多足机器人所执行的空翻动作和完成空翻动作的过程不作限定,例如多足机器人执行的空翻动作可以是前空翻(即身体向前空翻)的动作,也可以是后空翻(即身体向后空翻)的动作等;并且,本申请实施例对于多足机器人的起跳点和落地点之间的相对高度不作限定,例如多足机器人在执行空翻动作时,可以是从平地起跳落到平地上,也可以是从高台起跳落到平地上,还可以是平地起跳落到高台上等。以多足机器人是四足机器人,并以四足机器人在合理的规划下完成后空翻的动作为例,其相应的空翻过程可示例性地参见图1b所示。通过图1b可知,四足机器人的四条机械腿可分为两条前机械腿(靠近机器人头部的机械腿)和两条后机械腿(远离机器人头部的机械腿);且在四足机器人在平面执行后空翻的动作时,四足机器人的两条前机械腿先于两条后机械腿离开平面。由于前空翻的动作与后空翻的动作是相反的,因此可理解的是,若四足机器人执行的是前空翻的动作,则四足机器人的两条后机械腿先于两条前机械腿离开平面。
为了实现对多足机器人在当前时刻的下一时刻的状态进行估计,并提升状态估计的准确性以及状态估计的适用性,本申请实施例提出了一种多足机器人的状态估计方案;所谓的当前时刻是指:在多足机器人的空翻过程中,随着时间的推进,最新到达的系统时刻。具体的,该状态估计方案可由一个计算机设备执行,此处的计算机设备可以是终端或服务器;或者,该状态估计方案也可由终端和服务器共同执行,对此不作限定。其中,终端可以是智能手机、电脑(如平板电脑、笔记本电脑、台式计算机等)、智能穿戴设备(如智能手表、智能眼镜)、智能语音交互设备、智能家电(如智能电视)、车载终端或飞行器等;服务器可以是独立的物理服务器,也可以是多个物理服务器构成的服务器集群或者分布式系统,还可以是提供云服务、云数据库、云计算、云函数、云存储、网络服务、云通信、中间件服务、域名服务、安全服务、CDN(Content Delivery Network,内容分发网络)、以及大数据和人工智能平台等基础云计算服务的云服务器,等等。进一步的,终端和服务器可位于区块链网络内或区块链网络外,对此不作限定;更进一步的,终端和服务器还可将内部所存储的任一数据上传至区块链网络进行存储,以防止内部所存储的数据被篡改,提升数据安全性。
在具体实现中,本申请实施例提出的状态估计方案的大致原理如下:首先,可调用多足机器人中的IMU传感器,采集多足机器人在当前时刻的加速度信息(其可包括多足机器人在多个方向(如竖直方向、水平方向)上的加速度)和当前姿态信息,以及调用关节角编码器确定多足机器人的各个关节在当前时刻的关节角信息(如关节角的角度、角速度反馈值等)。其次,可将当前姿态信息和关节角信息(如关节角的角度、角速度反馈值)汇入腿部里程计(Leg odometry)计算出位置计算结果(可采用y表示),该位置计算结果可包括:计算出的多足机器人的各条机械腿在当前时刻的位置。另外,可将加速度信息输入至状态空间观测器,以使得状态空间观测器可基于加速度信息和历史得到的多足机器人在当前时刻的状态估计结果输出位置观测结果(可采用ym表示),该位置观测结果可包括:观测出的多足机器人的各条机械腿在当前时刻的位置;其中,多足机器人在当前时刻的状态估计结果,可以是在当前时刻的上一时刻到达时,对多足机器人在当前时刻的状态进行估计所得到的,其可存储在向量\hatxk-1(可表示成)中或其他数据结构中,对此不作限定。然后,可根据位置计算结果和位置观测结果,对多足机器人在当前时刻的下一时刻的状态进行估计。具体的,可将位置计算结果和位置观测结果作为扩展卡尔曼滤波(Extended Kalman Filter,EKF)单元的输入,以通过该单元进行状态估计,从而得到多足机器人在下一时刻的状态估计结果,如图1c所示。所谓的扩展卡尔曼滤波是标准卡尔曼滤波(简称卡尔曼滤波)在非线性情形下的一种扩展形式,其可将非线性函数进行泰勒展开,省略高阶项,保留展开项的一阶项,以此来实现非线性函数线性化。可选的,也可将位置计算结果和位置观测结果作为卡尔曼滤波单元的输入或基于机器学习所得到的状态估计模型的输入,以通过卡尔曼滤波单元或状态估计模型进行状态估计,得到多足机器人在下一时刻的状态估计结果。其中,多足机器人在下一时刻的状态估计结果,可同时用于多足机器人的控制和下一次状态估计时的状态空间观测器的输入;也就是说,通过状态估计所得到的估计结果可用于多足机器人的反馈控制,从而形成闭环。
进一步的,经实践表明:在多足机器人的空翻过程中,多足机器人的各条机械腿与平面(如地面、桌面等)之间的接触情况会发生变化,使得多足机器人在空翻过程中可具有多种运动形态,如多足机器人的各条机械腿均未离开平面的形态,各条机械腿均离开平面的形态,以及部分机械腿离开平面的形态等。且当多足机器人的机械腿均未离开平面时,腿部里程计所输出的位置计算结果较为准确,其对于状态估计具有较高的参考价值;而当多足机器人的机械腿离开平面处于悬空时,腿部里程计所输出的位置计算结果的准确性会降低,其对于状态估计的参考价值较低,此时的状态估计可更加依赖于状态空间观测器所输出的位置观测结果。由此可见,在多足机器人的不同运动形态下,腿部里程计输出的位置计算结果以及状态空间观测器输出的位置观测结果的参考价值会发生变化。基于此,为了提升状态估计的准确性,本申请实施例还可将多足机器人的空翻过程划分为连续的多个阶段,一个阶段对应多足机器人的一种运动形态,任一种运动形态用于指示多足机器人的各条机械腿在相应阶段中与平面之间的接触情况;从而实现在得到位置计算结果以及位置观测结果后,还可根据当前时刻所处的目标阶段对应的运动形态,确定对当前时刻的下一时刻进行状态估计所需的参考信息,该参考信息可用于指示:在状态估计时,分别对位置计算结果和位置观测结果的信任程度。然后根据参考信息、位置计算结果以及位置观测结果,估计多足机器人在当前时刻的下一时刻的状态,如图1d所示。
在本申请实施例中,任一结果(如位置计算结果、位置观测结果等)对应的信任程度,可用于指示该任一结果的重要度,而重要度可指示计算机设备在进行状态估计以得到状态估计结果时依赖该任一结果的程度。并且,重要度和信任程度可成正相关,即任一结果对应的信任程度越高,该任一结果的重要度就越高,从而指示计算机设备在进行状态估计以得到状态估计结果时,依赖该任一结果的程度越高。例如,若针对位置计算结果的信任程度高于针对位置观测结果的信任程度,则表明位置计算结果比位置观测结果更加重要,从而可指示计算机设备在根据位置计算结果和位置观测结果进行状态估计时,相较于位置观测结果而言,更加倾向于依赖位置计算结果来实现状态估计,从而得到状态估计结果。
其中,对多足机器人的空翻过程进行阶段划分,以得到连续的多个阶段的方式如下:
以多足机器人为四足机器人为例,假设在空翻开始时刻,四足机器人的各条机械腿均站立在平面上,且在四足机器人的空翻过程中,四足机器人的左右两条前机械腿与平面的接触情况一致,左右两条后机械腿与平面的接触情况也一致。为便于描述,后续采用基准腿表示四足机器人在空翻时,先离开平面的两条机械腿,以及采用参考腿表示四足机器人在空翻时,后离开平面的两条机械腿。例如,若四足机器人执行的空翻动作是后空翻动作,则基于前述图1b所示的空翻过程可知,两条前机械腿先于两条后机械腿离开平面,那么此情况下的基准腿可用于表示四足机器人的两条前机械腿,参考腿可用于表示四足机器人的两条后机械腿;若四足机器人执行的空翻动作是前空翻动作,则考虑到两条后机械腿先于两条前机械腿离开平面,那么此情况下的基准腿可用于表示四足机器人的两条后机械腿,参考腿可用于表示四足机器人的两条前机械腿。基于此,对空翻过程进行阶段划分所得到的多个阶段可包括第一阶段、第二阶段、第三阶段以及第四阶段。其中:
第一阶段的起始时刻为空翻开始时刻,且结束时刻为基准腿离开平面的第一时刻。也就是说,第一阶段是从空翻开始时刻开始的,基准腿和参考腿同时接触平面,整个过程包括以参考腿作为位置点,基准腿发力蹬地的过程,多足机器人可受平面的反作用力,使得基准腿在接下来的阶段中会离开平面,仅有参考腿与平面接触,因此该第一阶段到基准腿离开平面的瞬间结束。
第二阶段的起始时刻为第一时刻,且结束时刻为基准腿和参考腿均离开平面的第二时刻。也就是说,第二阶段从基准腿离开平面的瞬间开始,整个过程包括多足机器人以参考腿与平面之间的接触点为圆心进行旋转的过程,到基准腿和参考腿同时离开平面的瞬间为止。
第三阶段的起始时刻为第二时刻,且结束时刻为基准腿和参考腿中的至少一项落到平面的第三时刻。也就是说,第三阶段从基准腿和参考腿同时离开平面的瞬间开始,整个过程包括多足机器人在空中使得质心的姿态旋转接近360度的过程,到基准腿和参考腿中的至少一项重新接触平面的瞬间为止。应理解的是,多足机器人可能是以身体倾斜的姿态从空中落到平面的,此情况下可能是基准腿先落到平面上的;或者,多足机器人也可能是以身体平行平面的姿态从空中落到平面的,此情况下是基准腿和参考腿同时落到平面上的。
第四阶段的起始时刻为第三时刻,且结束时刻为基准腿和参考腿均落到平面且处于稳定状态的第四时刻。也就是说,第四阶段从基准腿和参考腿中的至少一项落到平面的瞬间开始,整个过程包括基准腿和参考腿全部落到平面上待到稳定的过程,到基准腿和参考腿在平面达到稳定状态的瞬间为止。需说明的是,任一项腿处于稳定状态是指:该任一项腿不再晃动,或该任一项腿对应的关节电机力矩或反馈电流值不再变化,或关节电机力矩或反馈电流值的变化幅度小于幅度阈值等。
需要说明的是,本申请实施例可以是根据多足机器人的各条机械腿在空翻过程中与平面之间的接触情况,将多足机器人的空翻过程划分为上述所提及的多个阶段的;在此情况下,上述所提及的基准腿则是指四足机器人在空翻时,先离开平面的两条机械腿,参考腿则是指四足机器人在空翻时,后离开平面的两条机械腿。进一步的,为了方便描述,由于无论是规划、控制还是状态估计,此过程中的四足机器人可近似是在一个二维平面(x轴(水平轴)和z轴(竖直轴)所构成的平面)内运动;那么如果不考虑四足机器人的自身质量等的不对称,以及电机的控制误差等,四足机器人可以被简化为二维平面模型中的虚拟机器人进行控制和状态估计。参见图1e所示,虚拟机器人可包括一条虚拟前腿和一条虚拟后腿。其中,虚拟前腿是对四足机器人的两条前机械腿进行等效处理得到的,该虚拟前腿具体可以是通过将四足机器人的左右两侧前机械腿进行重合得到的;虚拟后腿是对四足机器人的两条后机械腿进行等效处理得到的,该虚拟后腿具体可以是通过将四足机器人的左右两侧后机械腿进行重合得到的。
若四足机器人被简化为二维平面模型中的虚拟机器人,则本申请实施例可以是根据虚拟机器人的各条腿在空翻过程中与平面之间的接触情况,将多足机器人的空翻过程划分为上述所提及的多个阶段;在此情况下,上述所提及的基准腿则是指对四足机器人在空翻时,先离开平面的两条机械腿进行等效处理所得到的腿,参考腿则是指对四足机器人在空翻时,后离开平面的两条机械腿进行等效处理所得到的腿。具体的,当四足机器人执行的空翻动作为后空翻的动作时,基准腿为虚拟前腿;当四足机器人执行的空翻动作为前空翻的动作时,基准腿为虚拟后腿。示例性的,以平面为地面,空翻动作为后空翻的动作为例,类似于上述图1b所示的过程,对空翻过程进行阶段划分所得到的多个阶段的示意图可参见图1f所示。由图1f可知,在此情况下,上述所提及的第一阶段、第二阶段、第三阶段以及第四阶段中的各阶段的定义可进一步如下:
第一阶段:虚拟机器人的虚拟前腿和虚拟后腿同时接触地面,整个过程包括虚拟机器人以虚拟后腿作为位置点,虚拟前腿发力蹬地的过程,虚拟机器人受地面的反作用力,在接下来的阶段会使得虚拟前腿会离开地面,仅有虚拟后腿与地面接触。该阶段到虚拟前腿离开地面的瞬间结束。在第一阶段中,二维平面模型中的虚拟机器人与地面的接触点数量(contact point)为2个。
第二阶段:该阶段从虚拟机器人的虚拟前腿离开地面的瞬间开始,整个过程包括虚拟机器人以虚拟后腿与地面之间的接触点为圆心旋转的过程,到虚拟机器人的虚拟前腿和虚拟后腿同时离开地面为止。在第二阶段中,二维平面模型中的虚拟机器人与地面的接触点数量为1个。
第三阶段:该阶段从虚拟机器人的虚拟前腿和虚拟后腿同时离开地面开始,包括虚拟机器人在空中质心的姿态旋转接近360度的过程,到虚拟机器人的某一条腿或几条腿接触地面为止。在第三阶段中,二维平面模型中的虚拟机器人与地面的接触点数量为0个。
第四阶段:该阶段从虚拟机器人的某一条腿或几条腿接触地面开始,包含虚拟机器人的虚拟前腿和虚拟后腿同时落在地面上待到稳定的过程。在第四阶段中,在虚拟前腿和虚拟后腿均落地后,二维平面模型中的虚拟机器人与地面的接触点数量为2个。
基于上述描述可知,本申请实施例所提出的状态估计方案可通过使用扩展卡尔曼滤波或其他方法,实现在多足机器人(如四足机器人)的空翻过程中,使用IMU传感器和各关节的关节角信息,实现多足机器人的自身状态估计,即实时预估出多足机器人在下一时刻的质心位置和姿态信息。并且,通过将空翻过程的多阶段划分运用到状态估计中,可实现根据当前时刻所处的目标阶段对应的运动形态,确定位置计算结果和位置观测结果在状态估计时被信任的程度,从而使得在根据位置计算结果和位置观测结果估计多足机器人在下一时刻的状态时,可进一步参考这两个结果中的各结果被信任的程度,这样可提升状态估计的准确性,从而为落地柔顺控制、空中姿态控制、落地后的位置和姿态定位等操作奠定基础。除此之外,该状态估计方案可实现对任一环境中的多足机器人进行状态估计,其并不局限于实验室环境,因此可见该状态估计方案具有较高的适用性。
基于上述状态估计方案的相关描述,本申请实施例提出了一种多足机器人的状态估计方法。该状态估计方法可由上述所提及的计算机设备(终端或服务器)执行,也可由终端和服务器共同执行;为便于阐述,后续均以计算机设备执行该状态估计方法为例进行说明。且在本申请实施例中,多足机器人在平面上执行空翻动作,且空翻过程被划分为连续的多个阶段,一个阶段对应多足机器人的一种运动形态,任一种运动形态用于指示多足机器人的各条机械腿在相应阶段中与平面之间的接触情况。请参见图2,该状态估计方法可包括以下步骤S201-S205:
S201,在多足机器人的空翻过程中,根据当前时刻采集到的多足机器人的传感信息,计算多足机器人的各条机械腿在所述当前时刻的位置,得到位置计算结果。
其中,传感器信息是指由多足机器人中的传感器所采集到的信息。由前述可知,多足机器人中的传感器可包括IMU传感器以及关节角编码器等,IMU传感器可用于实时采集多足机器人的姿态信息和加速度,关节角编码器可实时提供多足机器人的每条机械腿上的各个关节的关节角信息(如关节角度、角速度反馈值等);基于此,步骤S201所提及的传感信息可包括:通过IMU传感器采集到的多足机器人的当前姿态信息,以及通过关节角编码器采集到的多足机器人的各个关节的关节角信息。
在具体实现中,计算机设备可将当前时刻采集到的多足机器人的传感信息输入至腿部里程计中,以使腿部里程计根据传感信息对多足机器人的各条机械腿在当前时刻的位置进行计算,得到位置计算结果。其中,位置计算结果可至少包括:世界坐标系下的至少两个方向位置向量,不同的方向位置向量对应不同的坐标轴方向;一个方向位置向量用于指示:多足机器人的各条机械腿在相应坐标轴方向上的位置。当多足机器人被近似在一个二维平面(x轴(水平轴)和z轴(竖直轴)所构成的平面)内运动时,该至少两个方向位置向量可包括:与水平轴方向对应的方向位置向量(采用psx表示),以及与竖直轴方向对应的方向位置向量(采用psz表示)。可选的,考虑到多足机器人实际是在三维空间中运动的,因此至少两个方向位置向量还可包括:与垂直轴(y轴)方向对应的方向位置向量(采用psy表示),对此不作限定。可理解的是:psx用于指示各条机械腿在x轴方向上的位置,psz用于指示各条机械腿在z轴方向上的位置,psy用于指示各条机械腿在y轴方向上的位置;且当多足机器人是四足机器人时,一条机械腿对应一个维度,则psx、psz以及psy的维度均为4维。
其中,腿部里程计计算与水平轴方向对应的方向位置向量的方式包括:首先,可基于当前姿态信息计算旋转矩阵(可采用R1表示),所谓的旋转矩阵是指通过改变任一向量的方向,从而实现将该任一向量映射到机器人基坐标系中的矩阵;具体可根据当前姿态信息确定多足机器人的基座姿态角,根据该基座姿态角计算出旋转矩阵。以及,还可根据各个关节的关节角信息计算出参考位置向量(可采用prel表示),该参考位置向量用于指示:多足机器人的基座质心和各条机械腿的足端之间的相对位置。接着,可采用旋转矩阵将参考位置向量映射到机器人基坐标系中,得到目标位置向量;具体的,可采用旋转矩阵乘以参考位置向量,得到目标位置向量,采用pf表示目标位置向量,则pf=R1prel。另外,可获取多足机器人的基座质心在世界坐标系下的三维位置向量(采用p0表示)。然后,可对目标位置向量在水平轴方向上的分量(采用pf(x)表示),以及三维位置向量在水平轴方向上的分量(采用p0(x)表示)进行融合处理,得到水平轴方向对应的方向位置向量(采用psx表示);此处的融合处理可包括求和处理,即psx=p0(x)+pf(x)。
需要说明的是,腿部里程计计算与其他坐标轴(如竖直轴、垂直轴)方向对应的方向位置向量的方式,与计算与水平轴方向对应的方向位置向量的方式类似,在此不再赘述。并且,位置计算结果除了包括世界坐标系下的至少两个方向位置向量以外,还可包括机器人基坐标系下的足端位置向量(采用ps表示),以及机器人基坐标系下的足端速度向量(采用vs表示)等其他向量。其中,足端位置向量用于指示:多足机器人的各条机械腿的足端在机器人基坐标系下的三维位置;腿部里程计计算足端位置向量的方式可包括:对目标位置向量(pf)进行求反处理,得到足端位置向量。足端速度向量用于指示:多足机器人的各条机械腿的足端在机器人基坐标系下的三维速度;腿部里程计计算足端速度向量的方式可包括:对目标位置向量(pf)进行求导处理,并对求导结果进行求反处理,得到足端速度向量。应理解的是,当多足机器人是四足机器人时,ps和vs的维度均为12维。
基于上述描述,当位置计算结果同时包括机器人基坐标系下的足端位置向量(采用ps表示)、机器人基坐标系下的足端速度向量(采用vs表示)、与水平轴方向对应的方向位置向量(采用psx表示),以及与竖直轴方向对应的方向位置向量(采用psz表示)时,腿部里程计计算得到位置计算结果(y)的方式可参见下述公式1.1所示:
在上述公式1.1中,左下角标B代表机器人基坐标系下的定义,左下角标W表示世界坐标系下的定义;并且,(z)和(x)分别表示前面的向量在z轴和x轴上的分量。基于上述描述可知,本申请实施例不仅可将ps、vs以及psz添加至位置计算结果中,还可在位置计算结果中添加psx和对应的项,这样有助于提升在x轴方向上的状态估计的精准度。
应理解的是,由于位置计算结果中的每个信息(如ps、vs、psx、psz等)均包括了多足机器人的每条机械腿所对应的子向量,例如:ps包括了每条机械腿对应的子足端位置向量,任一子足端向量用于指示相应机械腿的足端在机器人基坐标系下的三维位置;vs包括了每条机械腿对应的子足端速度向量,任一子足端速度向量用于指示相应机械腿的足端在机器人基坐标系下的三维速度;psx包括了每条机械腿对应的x子方向位置向量,任一x子方向位置向量用于指示相应机械腿在x轴方向上的位置;psz包括了每条机械腿对应的z子方向位置向量,任一z子方向位置向量用于指示相应机械腿在z轴方向上的位置。那么,若从机械腿的维度对位置计算结果进行划分,则可认为位置计算结果包括各条机械腿的子位置计算结果,任一条机械腿的子位置计算结果可包括:位置计算结果中的每个信息中与任一条机械腿对应的子向量。
S202,根据历史得到的多足机器人在当前时刻的状态估计结果,观测多足机器人的各条机械腿在当前时刻的位置,得到位置观测结果。
应理解的是,在多足机器人的空翻过程中,状态估计是一个迭代的过程;即每当一个时刻到达时,计算机设备便可获取相应信息对多足机器人在该时刻的下一时刻的状态进行估计,从而得到相应的状态估计结果。基于此,多足机器人在当前时刻的状态估计结果,便是由计算机设备在当前时刻的上一时刻到达时,对多足机器人在当前时刻的状态进行估计所得到的。并且,多足机器人在当前时刻的状态估计结果可作为状态空间观测器中的状态向量;具体的,采用表示状态向量,则状态向量的定义可参见下述公式1.2所示:
在上述公式1.2中,p0和v0分别代表多足机器人的基座质心在世界坐标系下的三维位置向量和三维速度向量;p1,p2,p3以及p4分别表示多足机器人的四条机械腿的足端在世界坐标系下的位置向量。应理解的是,此处是以多足机器人为四足机器人为例,对状态向量进行示例性表示的,所以状态向量包含了p1,p2,p3以及p4这四个向量;若多足机器人为其它足数的机器人,则状态向量中位置向量的数量可作适应性调整。
在步骤S202的具体实现中,计算机设备可获取多足机器人在当前时刻的加速度信息,并将该加速度信息和多足机器人在当前时刻的状态估计结果输入至状态空间观测器中,以使得状态空间观测器根据该加速度信息和多足机器人在当前时刻的状态估计结果,观测多足机器人的各条机械腿在当前时刻的位置,得到位置观测结果。具体的,状态空间观测器可先采用内部的数学表达式(如下述公式1.3所示),根据加速度信息和多足机器人在当前时刻的状态估计结果,对多足机器人在当前时刻的状态进行观测,得到状态观测结果(采用表示);然后,采用下述公式1.4,根据该状态观测结果观测多足机器人的各条机械腿在当前时刻的位置,得到位置观测结果(ym)。
在上述公式1.3-1.4中,g表示重力加速度,a表示加速度信息,A、B和C这三个矩阵的形式可以如下所示:
其中,dt表示一个控制周期的时间,I为单位矩阵,0表示零矩阵,I和0的下角标分别表示单位矩阵和零矩阵的维数。C1,C2对应于将状态观测结果(采用表示)中的对应状态(即对应维度下的向量)选择成为输出ym的选择矩阵。G1,G2用于将状态观测结果(采用表示)中的p1,p2,p3以及p4等向量在x轴方向和z轴方向上的分量选择出来以构成ym。
S203,从多个阶段中确定当前时刻所处的目标阶段。
在一种具体实现中,由于多足机器人是按照动作轨迹规划序列来执行空翻动作,该动作轨迹规划序列通过规划多足机器人在空翻过程中,每条机械腿在每个时刻与平面之间的接触情况,使得空翻过程所涉及的各个阶段分别对应有相应的时间段;因此,计算机设备在执行步骤S203时,可将当前时刻与各个阶段对应的时间段进行匹配,将包含该当前时刻的时间段所对应的阶段确定为目标阶段。
另一种具体实现中,考虑到多足机器人在执行空翻动作时,实际的动作轨迹与规划的动作轨迹可能存在出入;在此情况下,若基于动作轨迹规划序列来确定当前时刻所处的目标阶段,可能会导致确定出的目标阶段不够准确。基于此,本申请实施例进一步提出了基于多足机器人实际发生的状态变化的反馈,来确定当前时刻所处的目标阶段的技术手段。具体的,计算机设备在执行步骤S203时,可获取多足机器人在当前时刻的当前状态反馈信息,该当前状态反馈信息是通过反馈多足机器人在当前时刻所处的状态生成的;并根据当前状态反馈信息,从多个阶段中确定出当前时刻所处的目标阶段。
S204,根据目标阶段对应的运动形态,确定状态估计所需的参考信息。
其中,参考信息可用于指示:在状态估计时,分别对位置计算结果和位置观测结果的信任程度。该参考信息可以是扩展卡尔曼滤波中的参数,也可以是卡尔曼滤波中的参数,还可以是状态估计模型中的参数等,对此不作限定;为便于阐述,后续主要以参考信息是扩展卡尔曼滤波中的参数为例进行说明。具体的,该参考信息可包括第一参数和第二参数;第一参数指示对位置计算结果的信任程度,第二参数指示对位置观测结果的信任程度。进一步的,由前述可知,位置计算结果可包括各条机械腿的子位置计算结果;那么相应的,第一参数可至少包括多个目标向量,且不同目标向量对应不同机械腿,任一目标向量用于指示对相应机械腿的子位置计算结果的信任程度。与位置计算结果类似,位置观测结果可包括各条机械腿的子位置观测结果;那么第二参数也可至少包括多个参考向量,且不同参考向量对应不同机械腿,任一参考向量用于指示对相应机械腿的子位置观测结果的信任程度。
经实践证明:对于腿部里程计所输出的位置计算结果中的ps和vs而言,在多足机器人的腿部不离开平面(如不离地)的时候,计算得到的ps和vs的准确性较高,而在腿部离开平面(如离地)处于悬空的时候,计算得到的ps和vs的准确性较低,其对于状态估计没有太多参考价值。因此,应合理选择扩展卡尔曼滤波的参数矩阵以实现:在多足机器人的腿部不离开平面时,增加对ps和vs的信任程度,即增加对位置计算结果的信任程度;在多足机器人的腿部离开平面的时候,减小对ps和vs的信任程度,即减小对位置计算结果的信任程度,更多地依赖于状态空间观测器所输出的位置观测结果。进一步的,还可根据各条机械腿与平面之间的接触情况,对位置计算结果中的各条机械腿的子位置计算结果进行区分对待。例如,以四足机器人执行后空翻的动作为例,由于在连续的多个阶段中的第二阶段中,四足机器人的两条前机械腿均已离开平面(如前腿离地),两条后机械腿均未离开平面(如后腿未离地),那么此时可以提升对两条后机械腿的子位置计算结果的信任程度,减小对两条前机械腿的子位置计算结果的信任程度。
基于此,以多足机器人为四足机器人为例,计算机设备执行步骤S204的具体实施方式可以是:若目标阶段对应的运动形态是四足机器人的全部机械腿均离开平面的形态,则按照对位置计算结果的信任程度小于对位置观测结果的信任程度的原则,选取第一参数和第二参数,以得到状态估计所需的参考信息。若目标阶段对应的运动形态是四足机器人的部分机械腿离开平面的形态,则按照对第一子位置计算结果的信任程度小于对第二子位置计算结果的信任程度的原则,选取第一参数和第二参数,以得到状态估计所需的参考信息。其中,第一子位置计算结果包括:离开平面的机械腿对应的子位置计算结果;第二子位置计算结果包括:将未离开平面的机械腿对应的子位置计算结果。由此可见,本申请实施例可在状态估计中,根据多足机器人是否与平面接触,与平面接触点的数量和平面具体接触了多足机器人的哪个部位,对状态估计算法进行调整,从而提升状态估计的准确性。
S205,根据参考信息、位置计算结果以及位置观测结果,对多足机器人在当前时刻的下一时刻的状态进行估计,得到多足机器人在下一时刻的状态估计结果。
在具体实现中,若参考信息为扩展卡尔曼滤波中的参数,则计算机设备在执行步骤S205时,可根据参考信息、位置计算结果以及位置观测结果,进行扩展卡尔曼滤波处理,得到多足机器人在下一时刻的状态估计结果。其中,扩展卡尔曼滤波处理的过程可包括如下公式1.5-1.10:
ye=y-ym 式1.5
Pm=APAT+Q 式1.6
S=CPmCT+R 式1.7
Kk=PmCTS-1 式1.8
在上述公式1.5-1.10中,ye表示位置计算结果y和位置观测结果ym之间的差值,A和C可参见前述公式1.3-1.4的相关描述,AT和CT分别表示A的转置矩阵和C的转置矩阵。Q和R是扩展卡尔曼滤波中的参数矩阵,分别对应对不同信息源的信任程度;其中,Q代表前述提及的参考信息中的第二参数,R代表前述提及的参考信息中的第一参数,即R指示对位置计算结果y的信任程度,Q指示对位置观测结果ym的信任程度。P是一个预置的矩阵参数,Pm、S、Kk以及均表示扩展卡尔曼滤波过程中所得到的中间参数,S-1表示S的逆矩阵,表示多足机器人在下一时刻的状态估计结果。并且,在得到多足机器人在下一时刻的状态估计结果后,还可对P进行更新以便于下次进行扩展卡尔曼滤波处理;P的更新公式可参见下述公式1.11-1.12所示:
在上述公式1.11-1.12中,表示对P进行更新的过程中所得到的中间参数,表示的转置矩阵。
需要说明的是,上述只是示例性地对步骤S205的具体实施方式进行了阐述,并非穷举。例如,若参考信息为卡尔曼滤波中的参数,则计算机设备在执行步骤S205时,可根据参考信息、位置计算结果以及位置观测结果,进行卡尔曼滤波处理,得到多足机器人在下一时刻的状态估计结果。又如,若参考信息是状态估计模型中的参数,则可调用状态估计模型根据参考信息、位置计算结果以及位置观测结果,对多足机器人在当前时刻的下一时刻的状态进行估计,得到多足机器人在下一时刻的状态估计结果,等等。
本申请实施例可在多足机器人在平面上执行空翻动作的过程中,根据当前时刻采集到的多足机器人的传感信息,计算多足机器人的各条机械腿在当前时刻的位置,并根据历史得到的多足机器人在当前时刻的状态估计结果,观测各条机械腿在当前时刻的位置,从而根据相应的位置计算结果和位置观测结果来实现对多足机器人在下一时刻的状态进行估计。并且,通过将空翻过程的多阶段划分运用到空翻状态估计中,可实现根据当前时刻所处的目标阶段对应的运动形态,确定位置计算结果和位置观测结果在状态估计时被信任的程度,从而使得在根据位置计算结果和位置观测结果估计多足机器人在下一时刻的状态时,可进一步参考这两个结果中的各结果被信任的程度,这样可提升状态估计的准确性。并且,本申请实施例可实现对任一环境中的多足机器人进行状态估计,其并不局限于实验室环境,因此可见本申请实施例还可提升状态估计的适用性。
基于上述图2所示的方法实施例的相关描述,本申请实施例还提出了图3所示的状态估计方法;在本申请实施例中,仍以计算机设备执行该状态估计方法为例进行说明。参见图3所示,该状态估计方法包括如下步骤S301-S306:
S301,在多足机器人的空翻过程中,根据当前时刻采集到的多足机器人的传感信息,计算多足机器人的各条机械腿在当前时刻的位置,得到位置计算结果。
S302,根据历史得到的多足机器人在当前时刻的状态估计结果,观测多足机器人的各条机械腿在当前时刻的位置,得到位置观测结果。
S303,获取多足机器人在当前时刻的当前状态反馈信息,该当前状态反馈信息是通过反馈多足机器人在当前时刻所处的状态生成的。
S304,根据当前状态反馈信息,从多个阶段中确定出当前时刻所处的目标阶段。
在步骤S304的一种具体实现中,计算机设备可根据当前状态反馈信息检测基准腿在当前时刻与平面之间的接触情况,以及参考腿在当前时刻与平面之间的接触情况,得到检测结果;基于检测结果从多个阶段中确定出当前时刻所处的目标阶段。具体的,若检测结果指示基准腿在当前时刻未与平面接触,参考腿在当前时刻与平面接触,则可确定当前时刻所处的目标阶段为第二阶段;若检测结果指示基准腿和参考腿在当前时刻均未与平面接触,则可确定当前时刻所处的目标阶段为第三阶段;若检测结果指示基准腿和参考腿在当前时刻均与平面接触,则表明当前时刻所处的目标阶段可能是第一阶段,也可能是第四阶段,那么此情况下,可进一步结合当前时刻的前一时刻所处的阶段来确定当前时刻所处的目标阶段。若前一时刻所处的阶段为第一阶段,则目标阶段为第一阶段,若前一时刻所处的阶段为第三阶段或第四阶段,则目标阶段为第四阶段。
下面对如何根据当前状态反馈信息检测基准腿在当前时刻与平面之间的接触情况,以及参考腿在当前时刻与平面之间的接触情况的几种实施方式进行阐述:
实施方式一:一般的,当多足机器人的腿部不接触平面(如不着地)而悬空时,腿部的负载仅为腿部的质量,由于多足机器人的腿部质量相对于整体的质量可以忽略,故负载较小,各关节的反馈电流值以及关节电机力矩相对较小。而当多足机器人的腿部接触平面(如触地)时,多足机器人的负载变成了自身全部的质量加上自身惯性作用下向下移动的等效惯性力,故负载较大,各关节的反馈电流值以及关节电机力矩相对较大。基于此,在检测到关节电机力矩或反馈电流值由较小值突然变大时,认为多足机器人从空中向平面(如地面)着陆。在检测到关节电机力矩或反馈电流由较大值突然变小时,认为多足机器人从平面(如地面)向空中运动,据此可以大致确定多足机器人足端离开平面的瞬间所对应的时刻,从而判断相应腿在当前时刻是否与平面接触。
基于此,计算机设备获取到的当前状态反馈信息可包括:基准腿的当前状态反馈值,以及所述参考腿的当前状态反馈值,任一状态反馈值包括相应腿对应的关节电机力矩或反馈电流值。其中,针对基准腿和参考腿中的任一项,根据当前状态反馈信息检测该任一项在当前时刻与平面之间接触情况的方式包括:获取任一项在当前时刻的上一时刻的历史状态反馈值,并从当前状态反馈信息中确定任一项的当前状态反馈值,从而可根据历史状态反馈值,判断任一项的当前状态反馈值是否存在突变。在本申请实施例中,当前状态反馈值存在突变是指当前状态反馈值和历史状态反馈值之间的差值大于预设差值;基于此,计算机设备可计算历史状态反馈值与任一项的当前状态反馈值之间的差值;若计算出的差值大于预设差值,则判定当前状态反馈值存在突变;若计算出的差值不大于预设差值,则判定当前状态反馈值不存在突变。例如,设历史状态反馈值为20,预设差值为50;若当前状态反馈值为100,则由于100减去20等于80,而80大于50,因此可认为当前状态反馈值存在突变;若当前状态反馈值为30,则由于30减去20等于10,而10小于50,因此可认为当前状态反馈值不存在突变。
若根据历史状态反馈值判定任一项的当前状态反馈值存在突变,且任一项的当前状态反馈值大于历史状态反馈值,则确定任一项在当前时刻与平面接触;若根据历史状态反馈值判定任一项的当前状态反馈值存在突变,且任一项的当前状态反馈值小于历史状态反馈值,则确定任一项在当前时刻未与平面接触。若根据历史状态反馈值判定任一项的当前状态反馈值不存在突变,则确定任一项在上一时刻与平面之间的接触情况;若任一项在上一时刻与平面接触,则确定任一项在当前时刻与平面接触;若任一项在上一时刻未与平面接触,则确定任一项在当前时刻未与平面接触。
实施方式二:可根据由外部视觉或者动作捕捉系统检测到的多足机器人质心高度和质心姿态,以及多足机器人的各关节角信息,计算多足机器人的足端与平面接触的时刻,从而判断相应腿在当前时刻是否与平面接触。基于此,计算机设备获取到的当前状态反馈信息可包括:多足机器人的质心高度、质心姿态、所述基准腿对应的当前关节角信息以及所述参考腿对应的当前关节角信息。其中,针对基准腿和参考腿中的任一项,根据当前状态反馈信息检测该任一项在当前时刻与平面之间的接触情况的方式包括:根据质心高度、质心姿态以及任一项对应的当前关节角信息,计算任一项距离所述平面的高度;若计算出的高度小于或等于高度阈值(如数值0或0.5等),则确定任一项在当前时刻与平面接触;若计算出的高度大于高度阈值,则确定任一项在当前时刻未与平面接触。
实施方式三:可通过足底触觉传感器来判断相应腿在当前时刻是否与平面接触。基于此,计算机设备获取到的当前状态反馈信息可包括:基准腿对应的当前足底触觉反馈值,以及参考腿对应的当前足底触觉反馈值,足底触觉反馈值是通过相应腿的足底触觉传感器生成的。并且,任一足底触觉传感器在检测到相应腿与所述平面接触时,生成第一数值作为足底触觉反馈值,在检测到相应腿未与所述平面接触时,生成第二数值作为足底触觉反馈值;此处的第一数值和第二数值可根据实际需求设定,例如第一数值为数值1,第二数值为数值0,或者第一数值为数值0,第二数值为数值1等。其中,针对基准腿和参考腿中的任一项,根据当前状态反馈信息检测该任一项在当前时刻与平面之间的接触情况的方式包括:从当前状态反馈信息中获取任一项对应的当前足底触觉反馈值;若获取到的当前足底触觉反馈值为第一数值,则确定任一项在当前时刻与平面接触;若获取到的当前足底触觉反馈值为第二数值,则确定任一项在当前时刻未与平面接触。
应理解的是,上述只是示例性地阐述了步骤S304的一种具体实现,并非穷举。例如,经实践表明:当多足机器人在平面稳定站立时,IMU传感器所采集到的多足机器人在z方向的加速度为一倍的重力加速度数值g;当多足机器人在空中完全失重状态下时,IMU传感器所采集到的多足机器人在z方向的加速度为接近于0;当多足机器人在平面足部用力踩踏平面准备飞起的过程,以及落地后向平面缓冲的过程中,IMU传感器所采集到的多足机器人在z方向的加速度均大于一倍的重力加速度g。据此可知,在多个阶段的任意两个相邻阶段中,从前一阶段进入到后一阶段的时刻,多足机器人在竖直方向上的加速度发生突变。基于此,本申请实施例还提出了步骤S304的另一种具体实现。
在步骤S304的另一种具体实现中,计算机设备获取到的当前状态反馈信息可包括:多足机器人在竖直方向上的当前加速度;那么计算机设备可获取在当前时刻的上一时刻,多足机器人在竖直方向上的历史加速度;若根据历史加速度判定当前加速度发生突变,则将多个阶段中与参考阶段相邻的下一阶段,确定为当前时刻所处的目标阶段;若根据历史加速度判定当前加速度未发生突变,则将参考阶段确定为当前时刻所处的目标阶段。在本申请实施例中,当前加速度存在突变是指当前加速度和历史加速度之间的差值大于差值阈值;基于此,计算机设备可计算历史加速度与当前加速度之间的差值;若计算出的差值大于差值阈值,则判定当前加速度发生突变;若计算出的差值不大于差值阈值,则判定当前加速度未发生突变。例如,设历史加速度为2,差值阈值为5;若当前加速度为9,则由于9减去2等于7,而7大于5,因此可认为当前加速度发生突变;若当前加速度为4,则由于4减去2等于2,而2小于5,因此可认为当前加速度未发生突变。
可选的,由于多足机器人的空翻过程是连续的,针对任意相邻的两个时刻而言,这两个时刻要么处于同一个阶段,要么分别处于两个连续的阶段;因此,计算机设备还可先将当前时刻的上一时刻所处的阶段确定为参考阶段,并判断参考阶段是否为多个阶段中的最后一个阶段。若参考阶段为多个阶段中的最后一个阶段,则表明当前时刻必然是处于最后一个阶段的,那么此时可直接将最后一个阶段确定为当前时刻所处的目标阶段;若参考阶段为多个阶段中除最后一个阶段以外的阶段,则可触发执行步骤S303,以便于通过步骤S304根据该当前状态反馈信息确定当前时刻所处的目标阶段。通过此处理方式,可实现在已知上一时刻处于最后一个阶段的情况下,快速地确定出当前时刻所处的目标阶段,在提升处理效率的同时,还可节省处理资源。
在此情况下,步骤S304还可具体如下几种具体实现:
若当前状态反馈信息包括:基准腿的当前状态反馈值以及所述参考腿的当前状态反馈值,则由于基准腿和参考腿中的任一项与平面之间的接触情况发生变化时,任一项的状态反馈值可发生突变;因此步骤S304的另一种具体实现还可以包括:从基准腿和参考腿中,确定当参考阶段的结束时刻到达时,与平面之间的接触情况需发生变化的目标腿;获取目标腿在上一时刻的历史状态反馈值,并根据历史状态反馈值判断目标腿的当前状态反馈值是否发生突变;若是,则将多个阶段中与参考阶段相邻的下一阶段,确定为当前时刻所处的目标阶段;若否,则将参考阶段确定为当前时刻所处的目标阶段。
若当前状态反馈信息包括:多足机器人的质心高度、质心姿态、基准腿对应的当前关节角信息以及参考腿对应的当前关节角信息,则步骤S304的另一种具体实现还可以包括:根据质心高度、质心姿态以及基准腿对应的当前关节角信息,计算基准腿距离平面的第一高度;根据质心高度、质心姿态以及基准腿对应的当前关节角信息,计算参考腿距离平面的第二高度;根据第一高度和第二高度,从多个阶段中确定出当前时刻所处的目标阶段。具体的,若第一高度和第二高度均等于零,则将多个阶段中的第一阶段,确定为当前时刻所处的目标阶段;若第一高度大于零,且第二高度等于零,则将多个阶段中的第二阶段,确定为当前时刻所处的目标阶段;若第一高度和第二高度均大于零,则将多个阶段中的第三阶段,确定为当前时刻所处的目标阶段;若第一高度等于零,且第二高度大于零,则可将多个阶段中的第四阶段,确定为当前时刻所处的目标阶段。
若当前状态反馈信息包括:基准腿对应的当前足底触觉反馈值,以及参考腿对应的当前足底触觉反馈值,则步骤S304的另一种具体实现还可以包括:若根据当前状态反馈信息确定基准腿和参考腿均与平面接触,则将多个阶段中的第一阶段,确定为当前时刻所处的目标阶段;若根据当前状态反馈信息确定基准腿未与平面接触,且参考腿与平面接触,则将多个阶段中的第二阶段,确定为当前时刻所处的目标阶段;若根据当前状态反馈信息确定基准腿和参考腿均未与平面接触,则将多个阶段中的第三阶段,确定为当前时刻所处的目标阶段;若根据当前状态反馈信息确定基准腿与平面接触,且参考腿未与平面接触,则将多个阶段中的第四阶段,确定为当前时刻所处的目标阶段。
S305,根据目标阶段对应的运动形态,确定状态估计所需的参考信息,参考信息为扩展卡尔曼滤波中的参数。
S306,根据参考信息、位置计算结果以及位置观测结果,进行扩展卡尔曼滤波处理,得到多足机器人在下一时刻的状态估计结果。
本申请实施例可在多足机器人在平面上执行空翻动作的过程中,根据当前时刻采集到的多足机器人的传感信息,计算多足机器人的各条机械腿在当前时刻的位置,并根据历史得到的多足机器人在当前时刻的状态估计结果,观测各条机械腿在当前时刻的位置,从而根据相应的位置计算结果和位置观测结果来实现对多足机器人在下一时刻的状态进行估计。并且,通过将空翻过程的多阶段划分运用到空翻状态估计中,可实现根据当前时刻所处的目标阶段对应的运动形态,确定位置计算结果和位置观测结果在状态估计时被信任的程度,从而使得在根据位置计算结果和位置观测结果估计多足机器人在下一时刻的状态时,可进一步参考这两个结果中的各结果被信任的程度,这样可提升状态估计的准确性。并且,本申请实施例可实现对任一环境中的多足机器人进行状态估计,其并不局限于实验室环境,因此可见本申请实施例还可提升状态估计的适用性。
为了可以说明上述图2或图3所示的状态估计方法的效果,本申请实施例将上述图2或图3所示的状态估计方法应用到了四足机器人的一次空翻过程中,从而得到了图4a-图4c所示的状态估计结果。其中:
(1)图4a表示在四足机器人的空翻过程中,四足机器人的质心在世界坐标系中的x方向上的位置。其中,图4a中的虚线11表示在四足机器人的空翻过程中,通过结合基于四足机器人实际发生的状态变化的反馈来划分每个时刻所处的阶段这一技术手段,对四足机器人的质心在世界坐标系中的x方向上的位置进行估计所得到的状态估计值;点虚线12表示在四足机器人的空翻过程中,通过依靠基于四足机器人的动作轨迹规划序列中的规划的足端与地面接触情况划分每个时刻所处的阶段这一技术手段,对四足机器人的质心在世界坐标系中的x方向上的位置进行估计所得到的状态估计值;实线13表示在四足机器人的空翻过程中,通过动作捕捉系统对四足机器人的质心在世界坐标系中的x方向上的位置进行实时捕捉所得到的数据结果(即质心在世界坐标系中的x方向上的位置的真值)。由图4a所示的结果可知,在世界坐标系的x方向上,在合理划分阶段的情况下所得到的虚线11所示的状态估计值,与动作捕捉系统采集到的真值非常相近。
(2)图4b表示在四足机器人的空翻过程中,四足机器人的质心在世界坐标系中的z方向上的位置。其中,图4b中的虚线14表示在四足机器人的空翻过程中,通过结合基于四足机器人实际发生的状态变化的反馈来划分每个时刻所处的阶段这一技术手段,对四足机器人的质心在世界坐标系中的z方向上的位置进行估计所得到的状态估计值;点虚线15表示在四足机器人的空翻过程中,通过依靠基于四足机器人的动作轨迹规划序列中的规划的足端与地面接触情况划分每个时刻所处的阶段这一技术手段,对四足机器人的质心在世界坐标系中的z方向上的位置进行估计所得到的状态估计值;实线16表示在四足机器人的空翻过程中,通过动作捕捉系统对四足机器人的质心在世界坐标系中的z方向上的位置进行实时捕捉所得到的数据结果(即质心在世界坐标系中的z方向上的位置的真值)。由图4b所示的结果可知,在世界坐标系的z方向上,在合理划分阶段的情况下所得到的虚线14所示的状态估计值,与动作捕捉系统采集到的真值非常相近。
(3)图4c表示在四足机器人的空翻过程中,四足机器人的质心在世界坐标系中的pitch(俯仰角)姿态。其中,图4c中的虚线17表示在四足机器人的空翻过程中,通过结合基于四足机器人实际发生的状态变化的反馈来划分每个时刻所处的阶段这一技术手段,对四足机器人的质心在世界坐标系中的pitch姿态进行估计所得到的状态估计值;实线18表示在四足机器人的空翻过程中,通过动作捕捉系统对四足机器人的质心在世界坐标系中的pitch姿态进行实时捕捉所得到的数据结果(即质心在世界坐标系中的pitch姿态的真值)。由图4c所示的结果可知,在世界坐标系的pitch转动方向上,在合理划分阶段的情况下所得到的虚线17所示的状态估计值,与动作捕捉系统采集到的真值非常相近。
进一步的,基于上述图4a-图4c,通过将四足机器人的质心在x方向和z方向的位置单独拿出来,组成x,z图,可得到图4d所示的示意图(横坐标表示在x方向的位置,纵坐标表示在z方向上的位置);其中,图4d中的虚线为状态估计结果,实线为动作捕捉系统所采集到的数据结果。通过对图4d所示的数据进行分析可见,落地时刻:相较于动作捕捉系统采集到的数据结果而言,状态估计结果在x方向上的误差为2.5cm,在z方向上的误差为-11.5cm。在涉及多阶段的、高动态、与环境强交互的场景(类似前后空翻等动作)时,该精确度为一个较小值。
基于上述图4a-图4c的相关描述可知,相对于基于动作轨迹规划序列来进行划分每个时刻所处的阶段的这一技术手段而言,基于四足机器人实际发生的状态变化的反馈来划分每个时刻所处的阶段这一技术手段可使得阶段划分更加合理性。示例性的,本申请实施例通过对基于四足机器人实际发生的状态变化的反馈得到的阶段划分时刻进行标记,得到了图4e所示的时刻标记图。其中,按照从左往右的顺序,第一个竖线为第一阶段和第二阶段的分界线,第二个竖线是第二阶段和第三阶段的分界线,第三个竖线是第三阶段和第四阶段的分界线。应理解的是,这些对应的划分时刻同时以数线的方式表示在了其他附图中,例如前述提及的图4a-图4c中。
进一步的,由前述图2或图3所示的状态估计方法的相关描述可知,在状态估计过程中,四足机器人的质心位置、质心姿态、加速度以及足端受力情况等信息也会发生变化,如图4f-图4i所示。其中,图4f表示基于动作捕捉系统得到的质心位置和关节编码器测量的关节角信息计算出来的四足机器人的4条机械腿的足端位置,用于作为一个参考判断四足机器人的腾空、落地状态和相应的阶段划分。图4g是四足机器人的质心在世界坐标系中的x方向上的加速度,用于作为一个参考判断四足机器人的腾空、落地状态和相应的阶段划分。图4h是四足机器人的质心在世界坐标系中的z方向上的加速度,用于作为一个参考判断四足机器人的腾空、落地状态和相应的阶段划分。图4i是四足机器人的四个足端在世界坐标系下的受力情况,该受力情况可由基于各关节角的电流反馈值和四足机器人的各关节姿态得到的雅克比矩阵计算得到,用于作为一个参考判断四足机器人的腾空、落地状态和相应的阶段划分。
需要说明的是:在实际应用场景中,因为IMU的信号融合一定是需要一定的时间的,不可能瞬时获得,同时系统软件架构和通讯方面的传输也造成了延时;本申请实施例为了说明状态估计方法本身的精度情况,在得到上述图4a-图4i时,去除了IMU的延时造成的误差。并且,考虑到动作捕捉系统与IMU初始角存在偏差,原因是动作捕捉系统是以开机标定时的姿态作为姿态原点的,但是IMU是根据重力方向和地磁等信号融合,得到的是四足机器人相对于地球的绝对姿态。本申请实施例的后续结果针对这一情况,将这一静态误差进行了消除,由于绝对姿态信息影响旋转矩阵,从而影响状态估计中所涉及的位置计算结果的准确性,因此通过消除静态误差这一措施可以提升状态估计结果与动作捕捉系统所捕捉到的状态结果之间的吻合程度。
另外实测结果显示:在实验系统中,IMU数据与动作捕捉系统采集到的数据相比,IMU数据慢了10ms,即IMU数据的延时为10ms。基于此,本申请实施例还给出了未去除IMU延时的情况下所得到的相关附图,如图5a-图5i所示。应理解的是,图5a-图5i中的每个图的数据类型和曲线标记方法与前述图4a-图4i一致,在此不再赘述;且通过对图5d所示的数据进行分析可见,落地时刻:相较于动作捕捉系统采集到的数据结果而言,状态估计结果在x方向上的误差为10.6cm,在z方向上的误差为-11.9cm。
基于上述多足机器人的状态估计方法实施例的描述,本申请实施例还公开了一种多足机器人的状态估计装置;所述状态估计装置可以是运行于计算机设备中的一个计算机程序(包括程序代码),且该状态估计装置可以执行图2或图3所示的方法流程中的各个步骤。其中,所述多足机器人在平面上执行空翻动作,且空翻过程被划分为连续的多个阶段,一个阶段对应所述多足机器人的一种运动形态,任一种运动形态用于指示所述多足机器人的各条机械腿在相应阶段中与所述平面之间的接触情况。请参见图6,所述状态估计装置可以运行如下单元:
处理单元601,用于在所述多足机器人的空翻过程中,根据当前时刻采集到的所述多足机器人的传感信息,计算所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置计算结果;
所述处理单元601,还用于根据历史得到的所述多足机器人在所述当前时刻的状态估计结果,观测所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置观测结果;
估计单元602,用于从所述多个阶段中确定所述当前时刻所处的目标阶段,并根据所述目标阶段对应的运动形态,确定状态估计所需的参考信息;所述参考信息指示:在状态估计时,分别对所述位置计算结果和所述位置观测结果的信任程度;
所述估计单元602,还用于根据所述参考信息、所述位置计算结果以及所述位置观测结果,对所述多足机器人在所述当前时刻的下一时刻的状态进行估计,得到所述多足机器人在所述下一时刻的状态估计结果。
在一种实施方式中,处理单元601在用于根据当前时刻采集到的所述多足机器人的传感信息,计算所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置计算结果时,可具体用于:
将当前时刻采集到的所述多足机器人的传感信息输入至腿部里程计中,以使所述腿部里程计根据所述传感信息对所述多足机器人的各条机械腿在所述当前时刻的位置进行计算,得到位置计算结果;
其中,所述位置计算结果包括:世界坐标系下的至少两个方向位置向量,不同的方向位置向量对应不同的坐标轴方向;一个方向位置向量用于指示:所述各条机械腿在相应坐标轴方向上的位置。
另一种实施方式中,所述至少两个方向位置向量包括:与水平轴方向对应的方向位置向量;所述多足机器人的每条机械腿上包括至少一个关节;所述传感信息包括:所述多足机器人的当前姿态信息以及所述多足机器人的各个关节的关节角信息;
其中,所述腿部里程计计算与所述水平轴方向对应的方向位置向量的方式包括:
基于所述当前姿态信息计算旋转矩阵,并根据所述各个关节的关节角信息计算出参考位置向量,所述参考位置向量用于指示:所述多足机器人的基座质心和所述各条机械腿的足端之间的相对位置;
采用所述旋转矩阵将所述参考位置向量映射到机器人基坐标系中,得到目标位置向量;以及,获取所述多足机器人的基座质心在世界坐标系下的三维位置向量;
对所述目标位置向量在水平轴方向上的分量,以及所述三维位置向量在水平轴方向上的分量进行融合处理,得到所述水平轴方向对应的方向位置向量。
另一种实施方式中,所述多足机器人为四足机器人,且在空翻开始时刻,所述四足机器人的各条机械腿均站立在所述平面上;其中,所述多个阶段包括第一阶段、第二阶段、第三阶段以及第四阶段;
所述第一阶段的起始时刻为空翻开始时刻,且结束时刻为基准腿离开所述平面的第一时刻;所述基准腿用于表示所述四足机器人在空翻时,先离开所述平面的两条机械腿;
所述第二阶段的起始时刻为所述第一时刻,且结束时刻为所述基准腿和参考腿均离开所述平面的第二时刻;所述参考腿用于表示所述四足机器人在空翻时,后离开所述平面的两条机械腿;
所述第三阶段的起始时刻为所述第二时刻,且结束时刻为所述基准腿和所述参考腿中的至少一项落到所述平面的第三时刻;
所述第四阶段的起始时刻为所述第三时刻,且结束时刻为所述基准腿和所述参考腿均落到所述平面且处于稳定状态的第四时刻。
另一种实施方式中,估计单元602在用于从所述多个阶段中确定所述当前时刻所处的目标阶段时,可具体用于:
获取所述多足机器人在所述当前时刻的当前状态反馈信息,所述当前状态反馈信息是通过反馈所述多足机器人在所述当前时刻所处的状态生成的;
根据所述当前状态反馈信息,从所述多个阶段中确定出所述当前时刻所处的目标阶段。
另一种实施方式中,估计单元602在用于根据所述当前状态反馈信息,从所述多个阶段中确定出所述当前时刻所处的目标阶段时,可具体用于:
根据所述当前状态反馈信息检测所述基准腿在所述当前时刻与所述平面之间的接触情况,以及所述参考腿在所述当前时刻与所述平面之间的接触情况,得到检测结果;
基于所述检测结果从所述多个阶段中确定出所述当前时刻所处的目标阶段。
另一种实施方式中,所述当前状态反馈信息包括:所述基准腿的当前状态反馈值,以及所述参考腿的当前状态反馈值,任一状态反馈值包括相应腿对应的关节电机力矩或反馈电流值;
其中,针对所述基准腿和所述参考腿中的任一项,估计单元602在用于根据所述当前状态反馈信息检测所述任一项在所述当前时刻与所述平面之间的接触情况时,可具体用于:
获取所述任一项在所述当前时刻的上一时刻的历史状态反馈值,并从所述当前状态反馈信息中确定所述任一项的当前状态反馈值;
若根据所述历史状态反馈值判定所述任一项的当前状态反馈值存在突变,且所述任一项的当前状态反馈值大于所述历史状态反馈值,则确定所述任一项在所述当前时刻与所述平面接触;
若根据所述历史状态反馈值判定所述任一项的当前状态反馈值存在突变,且所述任一项的当前状态反馈值小于所述历史状态反馈值,则确定所述任一项在所述当前时刻未与所述平面接触。
另一种实施方式中,估计单元602还可用于:
若根据所述历史状态反馈值判定所述任一项的当前状态反馈值不存在突变,则确定所述任一项在所述上一时刻与所述平面之间的接触情况;
若所述任一项在所述上一时刻与所述平面接触,则确定所述任一项在所述当前时刻与所述平面接触;
若所述任一项在所述上一时刻未与所述平面接触,则确定所述任一项在所述当前时刻未与所述平面接触。
另一种实施方式中,所述当前状态反馈信息包括:所述多足机器人的质心高度、质心姿态、所述基准腿对应的当前关节角信息以及所述参考腿对应的当前关节角信息;
其中,针对所述基准腿和所述参考腿中的任一项,估计单元602在用于根据所述当前状态反馈信息检测所述任一项在所述当前时刻与所述平面之间的接触情况时,可具体用于:
根据所述质心高度、所述质心姿态以及所述任一项对应的当前关节角信息,计算所述任一项距离所述平面的高度;
若计算出的高度小于或等于高度阈值,则确定所述任一项在所述当前时刻与所述平面接触;
若计算出的高度大于所述高度阈值,则确定所述任一项在所述当前时刻未与所述平面接触。
另一种实施方式中,所述当前状态反馈信息包括:所述基准腿对应的当前足底触觉反馈值,以及所述参考腿对应的当前足底触觉反馈值,足底触觉反馈值是通过相应腿的足底触觉传感器生成的;任一足底触觉传感器在检测到相应腿与所述平面接触时,生成第一数值作为足底触觉反馈值,在检测到相应腿未与所述平面接触时,生成第二数值作为足底触觉反馈值;
其中,针对所述基准腿和所述参考腿中的任一项,估计单元602在用于根据所述当前状态反馈信息检测所述任一项在所述当前时刻与所述平面之间的接触情况时,可具体用于:
从所述当前状态反馈信息中获取所述任一项对应的当前足底触觉反馈值;
若获取到的当前足底触觉反馈值为第一数值,则确定所述任一项在所述当前时刻与所述平面接触;
若获取到的当前足底触觉反馈值为第二数值,则确定所述任一项在所述当前时刻未与所述平面接触。
另一种实施方式中,所述当前状态反馈信息包括:所述多足机器人在竖直方向上的当前加速度;其中,在所述多个阶段的任意两个相邻阶段中,从前一阶段进入到后一阶段的时刻,所述多足机器人在竖直方向上的加速度发生突变;
相应的,估计单元602在用于根据所述当前状态反馈信息,从所述多个阶段中确定出所述当前时刻所处的目标阶段时,可具体用于:
获取在所述当前时刻的上一时刻,所述多足机器人在所述竖直方向上的历史加速度;
若根据所述历史加速度判定所述当前加速度发生突变,则将所述多个阶段中与所述参考阶段相邻的下一阶段,确定为所述当前时刻所处的目标阶段;
若根据所述历史加速度判定所述当前加速度未发生突变,则将所述参考阶段确定为所述当前时刻所处的目标阶段。
另一种实施方式中,所述参考信息包括第一参数和第二参数;所述第一参数指示对所述位置计算结果的信任程度,所述第二参数指示对所述位置观测结果的信任程度;
其中,所述位置计算结果包括所述各条机械腿的子位置计算结果,所述第一参数至少包括多个目标向量,且不同目标向量对应不同机械腿,任一目标向量用于指示对相应机械腿的子位置计算结果的信任程度;
相应的,估计单元602在用于根据所述目标阶段对应的运动形态,确定状态估计所需的参考信息时,可具体用于:
若所述目标阶段对应的运动形态是所述四足机器人的全部机械腿均离开所述平面的形态,则按照对所述位置计算结果的信任程度小于对所述位置观测结果的信任程度的原则,选取第一参数和第二参数,以得到状态估计所需的参考信息;
若所述目标阶段对应的运动形态是所述四足机器人的部分机械腿离开所述平面的形态,则按照对第一子位置计算结果的信任程度小于对第二子位置计算结果的信任程度的原则,选取第一参数和第二参数,以得到状态估计所需的参考信息。
其中,所述第一子位置计算结果包括:离开所述平面的机械腿对应的子位置计算结果;所述第二子位置计算结果包括:将未离开所述平面的机械腿对应的子位置计算结果。
另一种实施方式中,所述参考信息为扩展卡尔曼滤波中的参数;相应的,估计单元602在用于根据所述参考信息、所述位置计算结果以及所述位置观测结果,对所述多足机器人在所述当前时刻的下一时刻的状态进行估计,得到所述多足机器人在所述下一时刻的状态估计结果时,可具体用于:
根据所述参考信息、所述位置计算结果以及所述位置观测结果,进行扩展卡尔曼滤波处理,得到所述多足机器人在下一时刻的状态估计结果。
根据本申请的另一个实施例,图6所示的状态估计装置中的各个单元可以分别或全部合并为一个或若干个另外的单元来构成,或者其中的某个(些)单元还可以再拆分为功能上更小的多个单元来构成,这可以实现同样的操作,而不影响本申请的实施例的技术效果的实现。上述单元是基于逻辑功能划分的,在实际应用中,一个单元的功能也可以由多个单元来实现,或者多个单元的功能由一个单元实现。在本申请的其它实施例中,基于状态估计装置也可以包括其它单元,在实际应用中,这些功能也可以由其它单元协助实现,并且可以由多个单元协作实现。
根据本申请的另一个实施例,可以通过在包括中央处理单元(CPU)、随机存取存储介质(RAM)、只读存储介质(ROM)等处理元件和存储元件的例如计算机的通用计算设备上运行能够执行如图2或图3中所示的相应方法所涉及的各步骤的计算机程序(包括程序代码),来构造如图6中所示的状态估计装置设备,以及来实现本申请实施例的状态估计方法。所述计算机程序可以记载于例如计算机可读记录介质上,并通过计算机可读记录介质装载于上述计算设备中,并在其中运行。
本申请实施例可在多足机器人在平面上执行空翻动作的过程中,根据当前时刻采集到的多足机器人的传感信息,计算多足机器人的各条机械腿在当前时刻的位置,并根据历史得到的多足机器人在当前时刻的状态估计结果,观测各条机械腿在当前时刻的位置,从而根据相应的位置计算结果和位置观测结果来实现对多足机器人在下一时刻的状态进行估计。并且,通过将空翻过程的多阶段划分运用到空翻状态估计中,可实现根据当前时刻所处的目标阶段对应的运动形态,确定位置计算结果和位置观测结果在状态估计时被信任的程度,从而使得在根据位置计算结果和位置观测结果估计多足机器人在下一时刻的状态时,可进一步参考这两个结果中的各结果被信任的程度,这样可提升状态估计的准确性。并且,本申请实施例可实现对任一环境中的多足机器人进行状态估计,其并不局限于实验室环境,因此可见本申请实施例还可提升状态估计的适用性。
基于上述方法实施例以及装置实施例的描述,本申请实施例还提供一种计算机设备。请参见图7,该计算机设备至少包括处理器701、输入接口702、输出接口703以及计算机存储介质704。其中,计算机设备内的处理器701、输入接口702、输出接口703以及计算机存储介质704可通过总线或其他方式连接。计算机存储介质704可以存储在计算机设备的存储器中,所述计算机存储介质704用于存储计算机程序,所述计算机程序包括程序指令,所述处理器701用于执行所述计算机存储介质704存储的程序指令。处理器701(或称CPU(Central Processing Unit,中央处理器))是计算机设备的计算核心以及控制核心,其适于实现一条或多条指令,具体适于加载并执行一条或多条指令从而实现相应方法流程或相应功能。
在一个实施例中,本申请实施例所述的处理器701可以用于对多足机器人进行一系列的状态估计,具体包括:在所述多足机器人的空翻过程中,根据当前时刻采集到的所述多足机器人的传感信息,计算所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置计算结果;根据历史得到的所述多足机器人在所述当前时刻的状态估计结果,观测所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置观测结果;从所述多个阶段中确定所述当前时刻所处的目标阶段,并根据所述目标阶段对应的运动形态,确定状态估计所需的参考信息;所述参考信息指示:在状态估计时,分别对所述位置计算结果和所述位置观测结果的信任程度;根据所述参考信息、所述位置计算结果以及所述位置观测结果,对所述多足机器人在所述当前时刻的下一时刻的状态进行估计,得到所述多足机器人在所述下一时刻的状态估计结果,等等。
本申请实施例还提供了一种计算机存储介质(Memory),所述计算机存储介质是计算机设备中的记忆设备,用于存放程序和数据。可以理解的是,此处的计算机存储介质既可以包括计算机设备中的内置存储介质,当然也可以包括计算机设备所支持的扩展存储介质。计算机存储介质提供存储空间,该存储空间存储了计算机设备的操作系统。并且,在该存储空间中还存放了适于被处理器701加载并执行的一条或多条的指令,这些指令可以是一个或一个以上的计算机程序(包括程序代码)。需要说明的是,此处的计算机存储介质可以是高速RAM存储器,也可以是非不稳定的存储器(non-volatile memory),例如至少一个磁盘存储器;可选的,还可以是至少一个位于远离前述处理器的计算机存储介质。
在一个实施例中,可由处理器加载并执行计算机存储介质中存放的一条或多条指令,以实现上述有关图2或图3所示的方法实施例中的相应步骤;具体实现中,计算机存储介质中的一条或多条指令可由处理器加载并执行如下步骤:
在所述多足机器人的空翻过程中,根据当前时刻采集到的所述多足机器人的传感信息,计算所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置计算结果;
根据历史得到的所述多足机器人在所述当前时刻的状态估计结果,观测所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置观测结果;
从所述多个阶段中确定所述当前时刻所处的目标阶段,并根据所述目标阶段对应的运动形态,确定状态估计所需的参考信息;所述参考信息指示:在状态估计时,分别对所述位置计算结果和所述位置观测结果的信任程度;
根据所述参考信息、所述位置计算结果以及所述位置观测结果,对所述多足机器人在所述当前时刻的下一时刻的状态进行估计,得到所述多足机器人在所述下一时刻的状态估计结果。
在一种实施方式中,在根据当前时刻采集到的所述多足机器人的传感信息,计算所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置计算结果时,所述一条或多条指令可由处理器加载并具体执行:
将当前时刻采集到的所述多足机器人的传感信息输入至腿部里程计中,以使所述腿部里程计根据所述传感信息对所述多足机器人的各条机械腿在所述当前时刻的位置进行计算,得到位置计算结果;
其中,所述位置计算结果包括:世界坐标系下的至少两个方向位置向量,不同的方向位置向量对应不同的坐标轴方向;一个方向位置向量用于指示:所述各条机械腿在相应坐标轴方向上的位置。
另一种实施方式中,所述多足机器人为四足机器人,且在空翻开始时刻,所述四足机器人的各条机械腿均站立在所述平面上;其中,所述多个阶段包括第一阶段、第二阶段、第三阶段以及第四阶段;
所述第一阶段的起始时刻为空翻开始时刻,且结束时刻为基准腿离开所述平面的第一时刻;所述基准腿用于表示所述四足机器人在空翻时,先离开所述平面的两条机械腿;
所述第二阶段的起始时刻为所述第一时刻,且结束时刻为所述基准腿和参考腿均离开所述平面的第二时刻;所述参考腿用于表示所述四足机器人在空翻时,后离开所述平面的两条机械腿;
所述第三阶段的起始时刻为所述第二时刻,且结束时刻为所述基准腿和所述参考腿中的至少一项落到所述平面的第三时刻;
所述第四阶段的起始时刻为所述第三时刻,且结束时刻为所述基准腿和所述参考腿均落到所述平面且处于稳定状态的第四时刻。
另一种实施方式中,在从所述多个阶段中确定所述当前时刻所处的目标阶段时,所述一条或多条指令可由处理器加载并具体执行:
获取所述多足机器人在所述当前时刻的当前状态反馈信息,所述当前状态反馈信息是通过反馈所述多足机器人在所述当前时刻所处的状态生成的;
根据所述当前状态反馈信息,从所述多个阶段中确定出所述当前时刻所处的目标阶段。
另一种实施方式中,在根据所述当前状态反馈信息,从所述多个阶段中确定出所述当前时刻所处的目标阶段时,所述一条或多条指令可由处理器加载并具体执行:
根据所述当前状态反馈信息检测所述基准腿在所述当前时刻与所述平面之间的接触情况,以及所述参考腿在所述当前时刻与所述平面之间的接触情况,得到检测结果;
基于所述检测结果从所述多个阶段中确定出所述当前时刻所处的目标阶段。
另一种实施方式中,所述当前状态反馈信息包括:所述基准腿的当前状态反馈值,以及所述参考腿的当前状态反馈值,任一状态反馈值包括相应腿对应的关节电机力矩或反馈电流值;
其中,针对所述基准腿和所述参考腿中的任一项,在根据所述当前状态反馈信息检测所述任一项在所述当前时刻与所述平面之间的接触情况时,所述一条或多条指令可由处理器加载并具体执行:
获取所述任一项在所述当前时刻的上一时刻的历史状态反馈值,并从所述当前状态反馈信息中确定所述任一项的当前状态反馈值;
若根据所述历史状态反馈值判定所述任一项的当前状态反馈值存在突变,且所述任一项的当前状态反馈值大于所述历史状态反馈值,则确定所述任一项在所述当前时刻与所述平面接触;
若根据所述历史状态反馈值判定所述任一项的当前状态反馈值存在突变,且所述任一项的当前状态反馈值小于所述历史状态反馈值,则确定所述任一项在所述当前时刻未与所述平面接触。
另一种实施方式中,所述一条或多条指令可由处理器加载并具体执行:
若根据所述历史状态反馈值判定所述任一项的当前状态反馈值不存在突变,则确定所述任一项在所述上一时刻与所述平面之间的接触情况;
若所述任一项在所述上一时刻与所述平面接触,则确定所述任一项在所述当前时刻与所述平面接触;
若所述任一项在所述上一时刻未与所述平面接触,则确定所述任一项在所述当前时刻未与所述平面接触。
另一种实施方式中,所述当前状态反馈信息包括:所述多足机器人的质心高度、质心姿态、所述基准腿对应的当前关节角信息以及所述参考腿对应的当前关节角信息;
其中,针对所述基准腿和所述参考腿中的任一项,在根据所述当前状态反馈信息检测所述任一项在所述当前时刻与所述平面之间的接触情况时,所述一条或多条指令可由处理器加载并具体执行:
根据所述质心高度、所述质心姿态以及所述任一项对应的当前关节角信息,计算所述任一项距离所述平面的高度;
若计算出的高度小于或等于高度阈值,则确定所述任一项在所述当前时刻与所述平面接触;
若计算出的高度大于所述高度阈值,则确定所述任一项在所述当前时刻未与所述平面接触。
另一种实施方式中,所述当前状态反馈信息包括:所述基准腿对应的当前足底触觉反馈值,以及所述参考腿对应的当前足底触觉反馈值,足底触觉反馈值是通过相应腿的足底触觉传感器生成的;任一足底触觉传感器在检测到相应腿与所述平面接触时,生成第一数值作为足底触觉反馈值,在检测到相应腿未与所述平面接触时,生成第二数值作为足底触觉反馈值;
其中,针对所述基准腿和所述参考腿中的任一项,在根据所述当前状态反馈信息检测所述任一项在所述当前时刻与所述平面之间的接触情况时,所述一条或多条指令可由处理器加载并具体执行:
从所述当前状态反馈信息中获取所述任一项对应的当前足底触觉反馈值;
若获取到的当前足底触觉反馈值为第一数值,则确定所述任一项在所述当前时刻与所述平面接触;
若获取到的当前足底触觉反馈值为第二数值,则确定所述任一项在所述当前时刻未与所述平面接触。
另一种实施方式中,所述当前状态反馈信息包括:所述多足机器人在竖直方向上的当前加速度;其中,在所述多个阶段的任意两个相邻阶段中,从前一阶段进入到后一阶段的时刻,所述多足机器人在竖直方向上的加速度发生突变;
相应的,在根据所述当前状态反馈信息,从所述多个阶段中确定出所述当前时刻所处的目标阶段时,所述一条或多条指令可由处理器加载并具体执行:
获取在所述当前时刻的上一时刻,所述多足机器人在所述竖直方向上的历史加速度;
若根据所述历史加速度判定所述当前加速度发生突变,则将所述多个阶段中与所述参考阶段相邻的下一阶段,确定为所述当前时刻所处的目标阶段;
若根据所述历史加速度判定所述当前加速度未发生突变,则将所述参考阶段确定为所述当前时刻所处的目标阶段。
另一种实施方式中,所述参考信息包括第一参数和第二参数;所述第一参数指示对所述位置计算结果的信任程度,所述第二参数指示对所述位置观测结果的信任程度;
其中,所述位置计算结果包括所述各条机械腿的子位置计算结果,所述第一参数至少包括多个目标向量,且不同目标向量对应不同机械腿,任一目标向量用于指示对相应机械腿的子位置计算结果的信任程度;
相应的,在根据所述目标阶段对应的运动形态,确定状态估计所需的参考信息时,所述一条或多条指令可由处理器加载并具体执行:
若所述目标阶段对应的运动形态是所述四足机器人的全部机械腿均离开所述平面的形态,则按照对所述位置计算结果的信任程度小于对所述位置观测结果的信任程度的原则,选取第一参数和第二参数,以得到状态估计所需的参考信息;
若所述目标阶段对应的运动形态是所述四足机器人的部分机械腿离开所述平面的形态,则按照对第一子位置计算结果的信任程度小于对第二子位置计算结果的信任程度的原则,选取第一参数和第二参数,以得到状态估计所需的参考信息。
其中,所述第一子位置计算结果包括:离开所述平面的机械腿对应的子位置计算结果;所述第二子位置计算结果包括:将未离开所述平面的机械腿对应的子位置计算结果。
另一种实施方式中,所述参考信息为扩展卡尔曼滤波中的参数;相应的,在根据所述参考信息、所述位置计算结果以及所述位置观测结果,对所述多足机器人在所述当前时刻的下一时刻的状态进行估计,得到所述多足机器人在所述下一时刻的状态估计结果时,所述一条或多条指令可由处理器加载并具体执行:根据所述参考信息、所述位置计算结果以及所述位置观测结果,进行扩展卡尔曼滤波处理,得到所述多足机器人在下一时刻的状态估计结果。
本申请实施例可在多足机器人在平面上执行空翻动作的过程中,根据当前时刻采集到的多足机器人的传感信息,计算多足机器人的各条机械腿在当前时刻的位置,并根据历史得到的多足机器人在当前时刻的状态估计结果,观测各条机械腿在当前时刻的位置,从而根据相应的位置计算结果和位置观测结果来实现对多足机器人在下一时刻的状态进行估计。并且,通过将空翻过程的多阶段划分运用到空翻状态估计中,可实现根据当前时刻所处的目标阶段对应的运动形态,确定位置计算结果和位置观测结果在状态估计时被信任的程度,从而使得在根据位置计算结果和位置观测结果估计多足机器人在下一时刻的状态时,可进一步参考这两个结果中的各结果被信任的程度,这样可提升状态估计的准确性。并且,本申请实施例可实现对任一环境中的多足机器人进行状态估计,其并不局限于实验室环境,因此可见本申请实施例还可提升状态估计的适用性。
需要说明的是,根据本申请的一个方面,还提供了一种计算机程序产品或计算机程序,该计算机程序产品或计算机程序包括计算机指令,该计算机指令存储在计算机可读存储介质中。计算机设备的处理器从计算机可读存储介质读取该计算机指令,处理器执行该计算机指令,使得该计算机设备执行上述图2或图3所示的方法实施例方面的各种可选方式中提供的方法。应理解的是,以上所揭露的仅为本申请较佳实施例而已,当然不能以此来限定本申请之权利范围,因此依本申请权利要求所作的等同变化,仍属本申请所涵盖的范围。
Claims (18)
1.一种多足机器人的状态估计方法,其特征在于,所述多足机器人在平面上执行空翻动作,且空翻过程被划分为连续的多个阶段,一个阶段对应所述多足机器人的一种运动形态,任一种运动形态用于指示所述多足机器人的各条机械腿在相应阶段中与所述平面之间的接触情况;方法包括:
在所述多足机器人的空翻过程中,根据当前时刻采集到的所述多足机器人的传感信息,计算所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置计算结果;
根据历史得到的所述多足机器人在所述当前时刻的状态估计结果,观测所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置观测结果;
从所述多个阶段中确定所述当前时刻所处的目标阶段,并根据所述目标阶段对应的运动形态,确定状态估计所需的参考信息;所述参考信息指示:在状态估计时,分别对所述位置计算结果和所述位置观测结果的信任程度;
根据所述参考信息、所述位置计算结果以及所述位置观测结果,对所述多足机器人在所述当前时刻的下一时刻的状态进行估计,得到所述多足机器人在所述下一时刻的状态估计结果。
2.如权利要求1所述的方法,其特征在于,所述根据当前时刻采集到的所述多足机器人的传感信息,计算所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置计算结果,包括:
将当前时刻采集到的所述多足机器人的传感信息输入至腿部里程计中,以使所述腿部里程计根据所述传感信息对所述多足机器人的各条机械腿在所述当前时刻的位置进行计算,得到位置计算结果;
其中,所述位置计算结果包括:世界坐标系下的至少两个方向位置向量,不同的方向位置向量对应不同的坐标轴方向;一个方向位置向量用于指示:所述各条机械腿在相应坐标轴方向上的位置。
3.如权利要求2所述的方法,其特征在于,所述至少两个方向位置向量包括:与水平轴方向对应的方向位置向量;所述多足机器人的每条机械腿上包括至少一个关节;所述传感信息包括:所述多足机器人的当前姿态信息以及所述多足机器人的各个关节的关节角信息;
其中,所述腿部里程计计算与所述水平轴方向对应的方向位置向量的方式包括:
基于所述当前姿态信息计算旋转矩阵,并根据所述各个关节的关节角信息计算出参考位置向量,所述参考位置向量用于指示:所述多足机器人的基座质心和所述各条机械腿的足端之间的相对位置;
采用所述旋转矩阵将所述参考位置向量映射到机器人基坐标系中,得到目标位置向量;以及,获取所述多足机器人的基座质心在世界坐标系下的三维位置向量;
对所述目标位置向量在水平轴方向上的分量,以及所述三维位置向量在水平轴方向上的分量进行融合处理,得到所述水平轴方向对应的方向位置向量。
4.如权利要求1-3任一项所述的方法,所述多足机器人为四足机器人,且在空翻开始时刻,所述四足机器人的各条机械腿均站立在所述平面上;其中,所述多个阶段包括第一阶段、第二阶段、第三阶段以及第四阶段;
所述第一阶段的起始时刻为空翻开始时刻,且结束时刻为基准腿离开所述平面的第一时刻;所述基准腿用于表示所述四足机器人在空翻时,先离开所述平面的两条机械腿;
所述第二阶段的起始时刻为所述第一时刻,且结束时刻为所述基准腿和参考腿均离开所述平面的第二时刻;所述参考腿用于表示所述四足机器人在空翻时,后离开所述平面的两条机械腿;
所述第三阶段的起始时刻为所述第二时刻,且结束时刻为所述基准腿和所述参考腿中的至少一项落到所述平面的第三时刻;
所述第四阶段的起始时刻为所述第三时刻,且结束时刻为所述基准腿和所述参考腿均落到所述平面且处于稳定状态的第四时刻。
5.如权利要求4所述的方法,其特征在于,所述四足机器人被简化为二维平面模型中的虚拟机器人,所述虚拟机器人包括一条虚拟前腿和一条虚拟后腿;
其中,所述虚拟前腿是对所述四足机器人的两条前机械腿进行等效处理得到的,所述虚拟后腿是对所述四足机器人的两条后机械腿进行等效处理得到的;
当所述四足机器人执行的空翻动作为后空翻的动作时,所述基准腿为所述虚拟前腿;当所述四足机器人执行的空翻动作为前空翻的动作时,所述基准腿为所述虚拟后腿。
6.如权利要求4所述的方法,其特征在于,所述从所述多个阶段中确定所述当前时刻所处的目标阶段,包括:
获取所述多足机器人在所述当前时刻的当前状态反馈信息,所述当前状态反馈信息是通过反馈所述多足机器人在所述当前时刻所处的状态生成的;
根据所述当前状态反馈信息,从所述多个阶段中确定出所述当前时刻所处的目标阶段。
7.如权利要求6所述的方法,其特征在于,所述根据所述当前状态反馈信息,从所述多个阶段中确定出所述当前时刻所处的目标阶段,包括:
根据所述当前状态反馈信息检测所述基准腿在所述当前时刻与所述平面之间的接触情况,以及所述参考腿在所述当前时刻与所述平面之间的接触情况,得到检测结果;
基于所述检测结果从所述多个阶段中确定出所述当前时刻所处的目标阶段。
8.如权利要求7所述的方法,其特征在于,所述当前状态反馈信息包括:所述基准腿的当前状态反馈值,以及所述参考腿的当前状态反馈值,任一状态反馈值包括相应腿对应的关节电机力矩或反馈电流值;
其中,针对所述基准腿和所述参考腿中的任一项,根据所述当前状态反馈信息检测所述任一项在所述当前时刻与所述平面之间的接触情况的方式,包括:
获取所述任一项在所述当前时刻的上一时刻的历史状态反馈值,并从所述当前状态反馈信息中确定所述任一项的当前状态反馈值;
若根据所述历史状态反馈值判定所述任一项的当前状态反馈值存在突变,且所述任一项的当前状态反馈值大于所述历史状态反馈值,则确定所述任一项在所述当前时刻与所述平面接触;
若根据所述历史状态反馈值判定所述任一项的当前状态反馈值存在突变,且所述任一项的当前状态反馈值小于所述历史状态反馈值,则确定所述任一项在所述当前时刻未与所述平面接触。
9.如权利要求8所述的方法,其特征在于,所述方法还包括:
若根据所述历史状态反馈值判定所述任一项的当前状态反馈值不存在突变,则确定所述任一项在所述上一时刻与所述平面之间的接触情况;
若所述任一项在所述上一时刻与所述平面接触,则确定所述任一项在所述当前时刻与所述平面接触;
若所述任一项在所述上一时刻未与所述平面接触,则确定所述任一项在所述当前时刻未与所述平面接触。
10.如权利要求7所述的方法,其特征在于,所述当前状态反馈信息包括:所述多足机器人的质心高度、质心姿态、所述基准腿对应的当前关节角信息以及所述参考腿对应的当前关节角信息;
其中,针对所述基准腿和所述参考腿中的任一项,根据所述当前状态反馈信息检测所述任一项在所述当前时刻与所述平面之间的接触情况的方式,包括:
根据所述质心高度、所述质心姿态以及所述任一项对应的当前关节角信息,计算所述任一项距离所述平面的高度;
若计算出的高度小于或等于高度阈值,则确定所述任一项在所述当前时刻与所述平面接触;
若计算出的高度大于所述高度阈值,则确定所述任一项在所述当前时刻未与所述平面接触。
11.如权利要求7所述的方法,其特征在于,所述当前状态反馈信息包括:所述基准腿对应的当前足底触觉反馈值,以及所述参考腿对应的当前足底触觉反馈值,足底触觉反馈值是通过相应腿的足底触觉传感器生成的;任一足底触觉传感器在检测到相应腿与所述平面接触时,生成第一数值作为足底触觉反馈值,在检测到相应腿未与所述平面接触时,生成第二数值作为足底触觉反馈值;
其中,针对所述基准腿和所述参考腿中的任一项,根据所述当前状态反馈信息检测所述任一项在所述当前时刻与所述平面之间的接触情况的方式,包括:
从所述当前状态反馈信息中获取所述任一项对应的当前足底触觉反馈值;
若获取到的当前足底触觉反馈值为第一数值,则确定所述任一项在所述当前时刻与所述平面接触;
若获取到的当前足底触觉反馈值为第二数值,则确定所述任一项在所述当前时刻未与所述平面接触。
12.如权利要求6所述的方法,其特征在于,所述当前状态反馈信息包括:所述多足机器人在竖直方向上的当前加速度;其中,在所述多个阶段的任意两个相邻阶段中,从前一阶段进入到后一阶段的时刻,所述多足机器人在竖直方向上的加速度发生突变;
所述根据所述当前状态反馈信息,从所述多个阶段中确定出所述当前时刻所处的目标阶段,包括:
获取在所述当前时刻的上一时刻,所述多足机器人在所述竖直方向上的历史加速度;
若根据所述历史加速度判定所述当前加速度发生突变,则将所述多个阶段中与所述参考阶段相邻的下一阶段,确定为所述当前时刻所处的目标阶段;
若根据所述历史加速度判定所述当前加速度未发生突变,则将所述参考阶段确定为所述当前时刻所处的目标阶段。
13.如权利要求4所述的方法,其特征在于,所述参考信息包括第一参数和第二参数;所述第一参数指示对所述位置计算结果的信任程度,所述第二参数指示对所述位置观测结果的信任程度;
其中,所述位置计算结果包括所述各条机械腿的子位置计算结果,所述第一参数至少包括多个目标向量,且不同目标向量对应不同机械腿,任一目标向量用于指示对相应机械腿的子位置计算结果的信任程度;
所述根据所述目标阶段对应的运动形态,确定状态估计所需的参考信息,包括:
若所述目标阶段对应的运动形态是所述四足机器人的全部机械腿均离开所述平面的形态,则按照对所述位置计算结果的信任程度小于对所述位置观测结果的信任程度的原则,选取第一参数和第二参数,以得到状态估计所需的参考信息;
若所述目标阶段对应的运动形态是所述四足机器人的部分机械腿离开所述平面的形态,则按照对第一子位置计算结果的信任程度小于对第二子位置计算结果的信任程度的原则,选取第一参数和第二参数,以得到状态估计所需的参考信息;
其中,所述第一子位置计算结果包括:离开所述平面的机械腿对应的子位置计算结果;所述第二子位置计算结果包括:将未离开所述平面的机械腿对应的子位置计算结果。
14.如权利要求1所述的方法,其特征在于,所述参考信息为扩展卡尔曼滤波中的参数;所述根据所述参考信息、所述位置计算结果以及所述位置观测结果,对所述多足机器人在所述当前时刻的下一时刻的状态进行估计,得到所述多足机器人在所述下一时刻的状态估计结果,包括:
根据所述参考信息、所述位置计算结果以及所述位置观测结果,进行扩展卡尔曼滤波处理,得到所述多足机器人在下一时刻的状态估计结果。
15.一种多足机器人的状态估计装置,其特征在于,所述多足机器人在平面上执行空翻动作,且空翻过程被划分为连续的多个阶段,一个阶段对应所述多足机器人的一种运动形态,任一种运动形态用于指示所述多足机器人的各条机械腿在相应阶段中与所述平面之间的接触情况;装置包括:
处理单元,用于在所述多足机器人的空翻过程中,根据当前时刻采集到的所述多足机器人的传感信息,计算所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置计算结果;
所述处理单元,还用于根据历史得到的所述多足机器人在所述当前时刻的状态估计结果,观测所述多足机器人的各条机械腿在所述当前时刻的位置,得到位置观测结果;
估计单元,用于从所述多个阶段中确定所述当前时刻所处的目标阶段,并根据所述目标阶段对应的运动形态,确定状态估计所需的参考信息;所述参考信息指示:在状态估计时,分别对所述位置计算结果和所述位置观测结果的信任程度;
所述估计单元,还用于根据所述参考信息、所述位置计算结果以及所述位置观测结果,对所述多足机器人在所述当前时刻的下一时刻的状态进行估计,得到所述多足机器人在所述下一时刻的状态估计结果。
16.一种计算机设备,包括输入接口和输出接口,其特征在于,还包括:
处理器,适于实现一条或多条指令;以及,计算机存储介质;
所述计算机存储介质存储有一条或多条指令,所述一条或多条指令适于由所述处理器加载并执行如权利要求1-14任一项所述的多足机器人的状态估计方法。
17.一种计算机存储介质,其特征在于,所述计算机存储介质存储有一条或多条指令,所述一条或多条指令适于由处理器加载并执行如权利要求1-14任一项所述的多足机器人的状态估计方法。
18.一种计算机程序产品,其特征在于,所述计算机程序产品包括计算机程序;所述计算机程序被处理器执行时,实现如权利要求1-14任一项所述的多足机器人的状态估计方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210609440.1A CN117008623A (zh) | 2022-05-31 | 2022-05-31 | 多足机器人的状态估计方法及相关设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210609440.1A CN117008623A (zh) | 2022-05-31 | 2022-05-31 | 多足机器人的状态估计方法及相关设备 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN117008623A true CN117008623A (zh) | 2023-11-07 |
Family
ID=88575028
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210609440.1A Pending CN117008623A (zh) | 2022-05-31 | 2022-05-31 | 多足机器人的状态估计方法及相关设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117008623A (zh) |
-
2022
- 2022-05-31 CN CN202210609440.1A patent/CN117008623A/zh active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111506100B (zh) | 多足机器人关节控制方法、装置和多足机器人 | |
CN107314778B (zh) | 一种相对姿态的标定方法、装置及系统 | |
CN113534828B (zh) | 质心位置确定方法、装置、足式机器人及存储介质 | |
CN112783175B (zh) | 质心轨迹确定方法、装置、足式机器人、设备及介质 | |
US20240312061A1 (en) | High-precision odometry estimation method based on double-layer filtering framework | |
US20220258356A1 (en) | Visual perception device based spatial calibration method and apparatus for robot body coordinate system, and storage medium | |
CN110068326B (zh) | 姿态计算方法、装置、电子设备以及存储介质 | |
CN108748220B (zh) | 一种基于惯性测量单元的脚-机跟随控制方法及控制系统 | |
CN113031639A (zh) | 机器人步长处理方法、装置、机器人控制设备及存储介质 | |
CN113858207B (zh) | 人形机器人的倒地控制方法、装置、电子设备及存储介质 | |
CN112757301B (zh) | 机器人抗扰动控制方法、装置、电子设备及存储介质 | |
CN112276950B (zh) | 抗扰动模型训练、控制方法、装置、设备、机器人及介质 | |
CN114161402A (zh) | 机器人稳定控制方法、模型构建方法、装置和机器人 | |
CN113119102B (zh) | 基于浮动基飞轮倒立摆的仿人机器人建模方法和装置 | |
WO2024037155A1 (zh) | 用于足式机器人的状态估计方法、装置、计算机设备及计算机可读存储介质 | |
WO2024021744A1 (zh) | 一种控制足式机器人的方法、装置、电子设备、计算机可读存储介质、计算机程序产品及足式机器人 | |
WO2024021767A1 (zh) | 控制足式机器人的方法、装置、设备、足式机器人、计算机可读存储介质和计算机程序产品 | |
CN117008623A (zh) | 多足机器人的状态估计方法及相关设备 | |
CN104699987B (zh) | 一种手臂惯性式动作捕捉数据融合方法 | |
US20220274254A1 (en) | Barycenter position determining method and apparatus, legged robot, and storage medium | |
CN113485403B (zh) | 机器人的步态规划方法、装置、处理设备及存储介质 | |
CN113954080B (zh) | 机器人的转向行走轨迹规划方法、装置、设备及介质 | |
CN114454981A (zh) | 双足机器人的弹跳运动控制方法、装置及双足机器人 | |
CN113589817A (zh) | 足式机器人落脚控制方法、装置、电子设备及存储介质 | |
CN113465822A (zh) | 基于多刚体机器人的质心位置计算方法、装置及介质 |
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 |