机器人位姿估计方法、装置、仿人机器人及存储介质
技术领域
本申请涉及机器人技术领域,具体而言,涉及一种机器人位姿估计方法、装置、仿人机器人及存储介质。
背景技术
仿人机器人具有在现实空间中行走以及操作的能力,在对仿人机器人进行控制时,可以从仿人机器人的本体结构、执行器件、控制策略以及状态估计等多个维度进行优化,从而提升仿人机器人的控制效果。
其中,在仿人机器人的状态估计中,仿人机器人的腰部点的状态估计对仿人机器人有着较大的影响,腰部点的状态估计不仅可以精确且客观地描述仿人机器人在世界坐标系中的状态,而且还可以为仿人机器人的控制提供高质量反馈信号,从而保证仿人机器人的控制效果。
然而,目前针对仿人机器人腰部点的状态估计方法精度较低,使得仿人机器人的控制效果较差。
发明内容
本申请的目的在于提供一种机器人位姿估计方法、装置、仿人机器人及存储介质,能够提高仿人机器人腰部状态参数的估计精度。
为了实现上述目的,本申请采用的技术方案如下:
第一方面,本申请提供一种机器人位姿估计方法,应用于仿人机器人,所述方法包括:
对获得的所述仿人机器人的腰部姿态参数以及脚底运动参数进行处理,得到所述仿人机器人的腰部中心点的测量姿态参数;
根据所述腰部姿态参数,计算所述仿人机器人的腰部中心点的预测姿态参数;
将所述测量姿态参数和所述预测姿态参数进行融合,得到所述仿人机器人的腰部中心点的估计姿态参数。
第二方面,本申请提供一种机器人位姿估计装置,应用于仿人机器人,所述装置包括:
处理模块,用于对获得的所述仿人机器人的腰部姿态参数以及脚底运动参数进行处理,得到所述仿人机器人的腰部中心点的测量姿态参数;
所述处理模块还用于,根据所述腰部姿态参数,计算所述仿人机器人的腰部中心点的预测姿态参数;
估计模块,用于将所述测量姿态参数和所述预测姿态参数进行融合,得到所述仿人机器人的腰部中心点的估计姿态参数。
第三方面,本申请提供一种仿人机器人,所述仿人机器人包括存储器,用于存储一个或多个程序;处理器;当所述一个或多个程序被所述处理器执行时,实现上述的机器人位姿估计方法。
第四方面,本申请提供一种计算机可读存储介质,其上存储有计算机程序,该计算机程序被处理器执行时实现上述的机器人位姿估计方法。
本申请提供的一种机器人位姿估计方法、装置、仿人机器人及存储介质,通过对获得的仿人机器人的腰部姿态参数以及脚底运动参数进行处理,得到仿人机器人的腰部中心点的测量姿态参数;以及根据腰部姿态参数,计算仿人机器人的腰部中心点的预测姿态参数;从而将测量姿态参数和预测姿态参数进行融合,得到仿人机器人的腰部中心点的估计姿态参数;如此,采用融合运动学解算以及IMU预测结果的方式得到仿人机器人的腰部中心点的估计姿态参数,能够避免仅采用IMU预测结果时出现的漂移现象,也能够避免仅采用运动学解算时出现的噪声,提高了仿人机器人腰部状态参数的估计精度。
为使本申请的上述目的、特征和优点能更明显易懂,下文特举较佳实施例,并配合所附附图,作详细说明如下。
附图说明
为了更清楚地说明本申请的技术方案,下面将对实施例中所需要使用的附图作简单地介绍,应当理解,以下附图仅示出了本申请的某些实施例,因此不应被看作是对范围的限定,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其它相关的附图。
图1示出本申请提供的仿人机器人的一种示意性结构框图;
图2示出本申请提供的机器人位姿估计方法的一种示意性流程框图;
图3示出本申请提供的一种示意性应用场景图;
图4示出图2中步骤201的子步骤的一种示意性流程框图;
图5示出脚底接触点位示意图;
图6示出图2中步骤203的子步骤的一种示意性流程框图;
图7示出图2中步骤205的子步骤的一种示意性流程框图;
图8示出本申请提供的机器人位姿估计装置的一种示意性结构框图。
图中:100-仿人机器人;101-存储器;102-处理器;103-通信接口;300-机器人位姿估计装置;301-处理模块;302-估计模块。
具体实施方式
为使本申请的目的、技术方案和优点更加清楚,下面将结合本申请的一些实施例中的附图,对本申请中的技术方案进行清楚、完整地描述,显然,所描述的实施例是本申请一部分实施例,而不是全部的实施例。通常在此处附图中描述和示出的本申请的组件可以以各种不同的配置来布置和设计。
因此,以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围,而是仅仅表示本申请选定的一些实施例。基于本申请中的一部分实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本申请保护的范围。
应注意到:相似的标号和字母在下面的附图中表示类似项,因此,一旦某一项在一个附图中被定义,则在随后的附图中不需要对其进行进一步定义和解释。同时,在本申请的描述中,术语“第一”、“第二”等仅用于区分描述,而不能理解为指示或暗示相对重要性。
需要说明的是,在本文中,诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
在例如上述针对仿人机器人的腰部状态估计的场景中,一些可能的实施方式为基于运动学建模解算或者是基于IMU(Inertial measurement unit,惯性测量单元)信号进行预测;比如,在基于运动学建模进行解算的方案中,可以在仿人机器人内部设置多个传感器,以采集仿人机器人的姿态信息,从而基于建立的运动学模型,计算出仿人机器人的腰部中心点的运动状态,以实现对仿人机器人包括腰部中心点在内的位姿状态估计;或者是,在基于IMU信号预测的方案中,利用设置在仿人机器人上的IMU,获得仿人机器人的线加速度信号,从而通过预设的预测模型预测出腰部点的运动状态,从而实现对仿人机器人的位姿状态估计。
然而,上述针对仿人机器人的位姿状态估计策略精度较低,比如上述的基于运动学解算的方案,由于传感器的测量值存在大量噪声信息,尤其是当仿人机器人落地时的冲击与震荡等产生的尖峰噪声,导致估计的腰部状态存在大量的噪声信息,精度较低;又例如上述的基于IMU信号预测的方案,该方案的精度主要依赖于预测模型的准确性,当预测模型与实际的机器人轨迹偏离较多时,腰部状态的估计会随着时间的积累出现漂移现象,精度自然也会受到影响。
因此,基于上述缺陷,本申请提供的一种可能的实现方式为:通过对获得的仿人机器人的腰部姿态参数以及脚底运动参数进行处理,得到仿人机器人的腰部中心点的测量姿态参数;以及根据腰部姿态参数,计算仿人机器人的腰部中心点的预测姿态参数;从而将测量姿态参数和预测姿态参数进行融合,得到仿人机器人的腰部中心点的估计姿态参数;如此,不仅能够避免仅采用IMU预测结果时出现的漂移现象,也能够避免仅采用运动学解算时出现的噪声,从而提高仿人机器人腰部状态参数的估计精度。
下面结合附图,对本申请的一些实施方式作详细说明。在不冲突的情况下,下述的实施例及实施例中的特征可以相互组合。
请参阅图1,图1示出本申请提供的仿人机器人100的一种示意性结构框图,在一些实施例中,仿人机器人100可以包括存储器101、处理器102和通信接口103,该存储器101、处理器102和通信接口103相互之间直接或间接地电性连接,以实现数据的传输或交互。例如,这些元件相互之间可通过一条或多条通讯总线或信号线实现电性连接。
存储器101可用于存储软件程序及模块,如本申请提供的机器人位姿估计装置对应的程序指令/模块,处理器102通过执行存储在存储器101内的软件程序及模块,从而执行各种功能应用以及数据处理,进而执行本申请提供的机器人位姿估计方法的步骤。该通信接口103可用于与其他节点设备进行信令或数据的通信。
其中,存储器101可以是,但不限于,随机存取存储器(Random Access Memory,RAM),只读存储器(Read Only Memory,ROM),可编程只读存储器(Programmable Read-OnlyMemory,PROM),可擦除只读存储器(Erasable Programmable Read-Only Memory,EPROM),电可擦除可编程只读存储器(Electric Erasable Programmable Read-Only Memory,EEPROM)等。
处理器102可以是一种集成电路芯片,具有信号处理能力。该处理器102可以是通用处理器,包括中央处理器(Central Processing Unit,CPU)、网络处理器(NetworkProcessor,NP)等;还可以是数字信号处理器(Digital Signal Processing,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现场可编程门阵列(Field-Programmable Gate Array,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
可以理解的是,图1所示的结构仅为示意,仿人机器人100还可以包括比图1中所示更多或者更少的组件,或者具有与图1所示不同的配置。图1中所示的各组件可以采用硬件、软件或其组合实现。
下面以图1所示的仿人机器人作为示意性执行主体为例,对本申请提供的机器人位姿估计方法进行示例性说明。
请参阅图2,图2示出本申请提供的机器人位姿估计方法的一种示意性流程框图,该机器人位姿估计方法可以包括以下步骤:
步骤201,对获得的仿人机器人的腰部姿态参数以及脚底运动参数进行处理,得到仿人机器人的腰部中心点的测量姿态参数;
步骤203,根据腰部姿态参数,计算仿人机器人的腰部中心点的预测姿态参数;
步骤205,将测量姿态参数和预测姿态参数进行融合,得到仿人机器人的腰部中心点的估计姿态参数。
在一些实施例中,结合图3所示,仿人机器人可以预先建立世界坐标系{G}、预先根据仿人机器人的腰部中心点建立腰部坐标系{W}、以及预先根据仿人机器人的脚底建立的脚底坐标系{F}。
其中,在一些实施例中,世界坐标系的原点可以为预先配置的点;腰部坐标系的原点可以是预先确定的仿人机器人的腰部中心点,该腰部中心点可以是仿人机器人腰部附近设置的点,或者该腰部坐标系的原点还可以是仿人机器人腰部位置的几何中心点;脚底坐标系的原点可以为仿人机器人脚底的中心点。
并且,在一些实施例中,根据仿人机器人包含的脚的数量的不同,可以设置多个脚底坐标系,比如在图3的示意图中,该仿人机器人配置有两条腿,且配置有两只脚,则可以建立两个脚底坐标系,且两个脚底坐标系的原点可以分别为两只脚脚底的中心点。
另外,在一些实施例中,可以在仿人机器人的腰部设置一IMU,比如该IMU可以设置在仿人机器人的腰部中心点,通过该IMU可以采集仿人机器人的腰部姿态参数,比如仿人机器人的腰部中心点在世界坐标系下的姿态角、姿态角速度以及该仿人机器人的腰部中心点在腰部坐标系下的线加速度等;示例性地,该仿人机器人的腰部中心点在世界坐标下的姿态角可以记为腰部姿态角,该仿人机器人的腰部中心点在世界坐标下的姿态角速度可以记为腰部姿态角速度,该仿人机器人的腰部中心点在腰部坐标系下的线加速度可以记为第一腰部线加速度。
并且,在一些实施例中,可以在仿人机器人的各个关节位置设置检测传感器,以获得仿人机器人各个关节位置的例如位移参数、角速度参数等信息;例如,可以在仿人机器人的腿部安装位移传感器,以采集仿人机器人腿部的各个关节点的信息,比如采集仿人机器人的脚底位移参数、脚底角速度参数等脚底运动参数。
如此,在对仿人机器人进行位姿估计时,仿人机器人可以先对获得的仿人机器人的腰部姿态参数以及脚底运动参数进行处理,比如采用运动学解算的方式对腰部姿态参数以及测量姿态参数进行处理,以得到仿人机器人的腰部中心点的测量姿态参数,该测量姿态参数可以表征利用测得的仿人机器人的测量参数计算出的姿态参数。
另外,仿人机器人还可以根据获得的仿人机器人的腰部姿态参数,采用例如坐标换算并结合运动学解算等方式,计算该仿人机器人的腰部中心点的预测姿态参数,该预测姿态参数可以表征利用测得的仿人机器人的测量参数预测出的仿人机器人的姿态参数。
需要说明的是,上述步骤201与步骤203并无必然的先后执行顺序,可以是先执行步骤201再执行步骤203,也可以是先执行步骤203再执行步骤201,本申请对此不进行限制;比如,在本申请其他一些可能的实施例中,步骤201还可以与步骤203一起被执行。
接下来,仿人机器人可以将上述得到的测量姿态参数和预测姿态参数进行融合,比如将两者进行滤波融合,从而得到该仿人机器人的腰部中心点的估计姿态参数。
可见,基于上述设计,本申请提供的机器人位姿估计方法,通过对获得的仿人机器人的腰部姿态参数以及脚底运动参数进行处理,得到仿人机器人的腰部中心点的测量姿态参数;以及根据腰部姿态参数,计算仿人机器人的腰部中心点的预测姿态参数;从而将测量姿态参数和预测姿态参数进行融合,得到仿人机器人的腰部中心点的估计姿态参数;如此,采用融合运动学解算以及IMU预测结果的方式得到仿人机器人的腰部中心点的估计姿态参数,能够避免仅采用IMU预测结果时出现的漂移现象,也能够避免仅采用运动学解算时出现的噪声,提高了仿人机器人腰部状态参数的估计精度。
其中,在一些实施方式中,在计算仿人机器人的测量姿态参数时,一般是默认仿人机器人的脚底板与地面接触,从而根据预先设置的脚底坐标系的原点位置坐标,结合采集得到的脚底运动参数进行解算处理,以得到仿人机器人的测量姿态参数。
然而,在一些可能的应用场景中,仿人机器人的脚底板可能并非是与地面完全接触,比如仿人机器人可能出现踮脚等情况,此时仿人机器人的脚底板即出现翘起的情形,导致仿人机器人的脚底板并没有与地面完全接触,从而导致仿人机器人的脚底中心发生变化,计算的测量姿态参数存在误差。
因此,在图2的基础上,请参阅图4,图4示出图2中步骤201的子步骤的一种示例性流程图,作为一种可能的实施方式,步骤201可以包括以下子步骤:
步骤201-1,利用腰部姿态参数及脚底运动参数进行运动学解算,得到仿人机器人的脚底接触点坐标;
步骤201-3,根据脚底接触点坐标、脚底运动参数及腰部姿态参数进行运动学解算,得到仿人机器人的腰部中心点的测量姿态参数。
在一些实施例中,考虑到仿人机器人可能出现例如踮脚等脚底板出现翘起的情况,在执行步骤201时,仿人机器人可以先利用腰部姿态参数及脚底运动参数进行运动学解算,得到仿人机器人的脚底与地面的接触点在世界坐标系下的脚底接触点坐标。
比如,在一些实施例中,上述的脚底运动参数可以包括仿人机器人的脚底位移参数,仿人机器人在进行运动学解算得到仿人机器人的脚底接触点坐标时,可以先利用腰部姿态角以及脚底位移参数进行运动学解算,得到仿人机器人的脚底姿态角;该脚底姿态角为仿人机器人的脚底在世界坐标系下的姿态角,可以包括脚底横滚角、俯仰角以及偏航角。
接下来,仿人机器人可以根据预设的姿态配置策略,在该姿态配置策略中查找与脚底姿态角对应初始接触点坐标,该初始接触点坐标为仿人机器人的脚底与地面的接触点在脚底坐标系中的坐标。
其中,该姿态配置策略包括有多种脚底姿态下该接触点在脚底坐标系下的坐标;比如,结合图5所示,在建立脚底坐标系{G}时,可以将脚底的中心点(图5中的点⑨)作为脚底坐标系的原点,按照仿人机器人脚底的尺寸,则例如图5所示,可以预先配置出在脚底中确定的多个接触点在脚底坐标系中的坐标。
示例性地,结合图5所示,该姿态配置策略可以表示如下:
当脚底姿态角的横滚角小于零且俯仰角大于零,则脚底与地面的接触点为点①,且初始接触点坐标表示为
当脚底姿态角的横滚角大于零且俯仰角大于零,则脚底与地面的接触点为点②,且初始接触点坐标表示为
当脚底姿态角的横滚角大于零且俯仰角小于零,则脚底与地面的接触点为点③,且初始接触点坐标表示为
当脚底姿态角的横滚角小于零且俯仰角小于零,则脚底与地面的接触点为点④,且初始接触点坐标表示为
当脚底姿态角的横滚角等于零且俯仰角大于零,则脚底与地面的接触点为点⑤,且初始接触点坐标表示为
当脚底姿态角的横滚角大于零且俯仰角等于零,则脚底与地面的接触点为点⑥,且初始接触点坐标表示为
当脚底姿态角的横滚角等于零且俯仰角小于零,则脚底与地面的接触点为点⑦,且初始接触点坐标表示为
当脚底姿态角的横滚角小于零且俯仰角等于零,则脚底与地面的接触点为点⑧,且初始接触点坐标表示为
当脚底姿态角的横滚角等于零且俯仰角等于零,则脚底与地面的接触点为点⑨,且初始接触点坐标表示为
然后,仿人机器人可以将初始接触点坐标从脚底坐标系转换到世界坐标系中,得到脚底接触点坐标。
其中,需要指出的是,在初始状态下,仿人机器人的脚底一般是贴合地面的,在该状态下,脚底坐标系与世界坐标系一般是平行的;假定初始状态下脚底坐标系在世界坐标系中的位置表示为
则脚底接触点坐标可以表示为:
基于此,仿人机器人可以利用上述获得的脚底接触点坐标,结合上述的脚底运动参数以及腰部姿态参数进行运动学解算,得到仿人机器人的腰部中心点的测量姿态参数;如此,可以使仿人机器人在计算测量姿态参数时,能够考虑到仿人机器人的脚底与地面并不贴合等情况导致的计算误差,进而提高测量姿态参数的计算精度。
其中,在一些实施例中,上述利用IMU采集获得的仿人机器人的腰部姿态参数,可以包括仿人机器人的腰部中心点在世界坐标系下的腰部姿态角以及腰部姿态角速度;上述利用运动学解算得到的测量姿态参数可以包括仿人机器人的测量位置坐标以及测量线速度。
其中,对于仿人机器人的测量位置坐标,可以利用仿人机器人的腰部姿态角,并结合上述计算得到的脚底接触点坐标、以及脚底运动参数中所包含的仿人机器人的脚底位移参数进行计算获得。
比如,在一些实施例中,仿人机器人在执行步骤201-3时,仿人机器人可以先利用仿人机器人的腰部姿态角以及脚底位移参数,采用例如上述的实施方式进行运动学解算,以得到仿人机器人的脚底姿态角。
然后,仿人机器人可以根据上述得到的脚底接触点坐标、脚底姿态角以及脚底位移参数进行运动学解算,得到腰部中心点的测量位置坐标。
示例性地,仿人机器人可以根据脚底接触点坐标以及脚底姿态角进行坐标换算,从而得到脚底在世界坐标系中的位置坐标,其中,采用的计算公式可以满足如下:
式中,
为脚底坐标系{F}转换到世界坐标系{G}的第一姿态矩阵,其值可通过脚底姿态角计算得到。
然后,仿人机器人可以根据脚底在世界坐标系中的位置坐标、第一姿态矩阵以及脚底位移参数进行运动学解算,从而得到腰部中心点在世界坐标系下的测量位置坐标。
另外,对于仿人机器人的测量线速度,可以利用仿人机器人的腰部姿态角速度,并结合上述计算得到的脚底接触点坐标、以及脚底运动参数中所包含的仿人机器人的脚底角速度参数进行计算获得。
比如,仿人机器人可以先利用腰部姿态角速度以及脚底角速度参数进行运动学解算处理,得到仿人机器人的姿态角速度;其中,姿态角速度可以分为横滚方向上的横滚角速度、俯仰方向上的俯仰角速度以及偏航方向上的偏航角速度。
接下来,仿人机器人可以根据脚底接触点坐标、脚底姿态角、姿态角速度以及脚底角速度参数进行运动学解算处理,得到腰部中心点的测量线速度。
示例性地,仿人机器人可以先根据脚底接触点坐标、上述的根据脚底姿态角计算出的第一姿态矩阵、以及姿态角速度,计算出脚底在世界坐标系下的线速度,其中,采用的计算公式可以满足如下:
式中,
为脚底在世界坐标系下的线速度,
为姿态角速度对应的叉乘反对称矩阵,其形式表示如下:
式中,
表示姿态角速度,
分别为姿态角速度包括的横滚角速度、俯仰角速度和偏航角速度。
然后,仿人机器人可以根据得到的脚底在世界坐标系下的线速度、姿态角速度以及脚底角速度参数进行运动学解算,从而得到腰部中心点在世界坐标系下的线速度。
另外,在例如上述的实施例中,上述利用IMU采集获得的仿人机器人的腰部姿态参数可以包括该腰部中心点在世界坐标系下的腰部姿态角以及在腰部坐标系下的第一腰部线加速度;于是,仿人机器人在执行步骤203时,可以基于获得的腰部中心点的腰部姿态角以及第一腰部线加速度,计算得到腰部中心点的预测姿态参数。
示例性地,在图2的基础上,请参阅图6,图6示出图2中步骤203的子步骤的一种示意性流程框图,作为一种可能的实施方式,步骤203可以包括以下子步骤:
步骤203-1,根据腰部姿态角,将第一腰部线加速度从腰部坐标系转换为世界坐标系下的第二腰部线加速度;
步骤203-3,根据第二线腰部加速度、腰部中心点的历史线速度以及腰部中心点的历史位置坐标,计算腰部中心点的预测线速度以及预测位置坐标。
在一些实施例中,仿人机器人的预测姿态参数可以包括预测位置坐标以及预测线速度;对应地,仿人机器人的估计姿态参数可以包括估计位置坐标以及估计线速度。
另外,在一些实施例中,仿人机器人可以按照预设的时间间隔ΔT,每间隔时间ΔT则执行本申请提供的机器人位姿估计方法,计算出对应时刻的估计姿态参数。
其中,仿人机器人可以保存计算出的每一个估计姿态参数,从而利用保存的第k-1个时刻的估计时刻参数进行迭代计算,得到第k个时刻的估计时刻参数,k为大于1的正整数。
另外,当间隔时间ΔT设置的足够小时,仿人机器人的腰部中心点在连续的两个计算时刻间的运动可以视为匀加速过程,因此,本申请可以基于匀加速模型计算仿人机器人的腰部中心点的估计姿态参数。
如此,仿人机器人在执行步骤203时,可以先根据腰部姿态角,采用与上述计算第一姿态矩阵
相同的计算方式,计算出第二姿态矩阵,该第二姿态矩阵
为腰部坐标系转换到世界坐标系的姿态矩阵,从而根据得到的第二姿态矩阵
将第一腰部线加速度从腰部坐标系转换为世界坐标系下的第二腰部线加速度;其中,采用的计算公式可以满足如下:
式中,
为腰部坐标系{W}转换到世界坐标系{G}的第二姿态矩阵,
为第二腰部线加速度,
为第一腰部线加速度,g
G为重力加速度在世界坐标系{G}下的表达。
接下来,仿人机器人可以根据第二腰部线加速度、腰部中心点的历史线速度以及腰部中心点的历史位置坐标,计算腰部中心点的预测线速度以及预测位置坐标。
其中,如上所述,仿人机器人可以保存每一次计算出的估计姿态参数,则在计算第k个时刻的估计姿态参数时,第k-1个时刻的估计姿态参数则可以作为第k个时刻的历史估计参数;对应地,第k-1个时刻的估计线速度则为第k个时刻的历史线速度,第k-1个时刻的估计位置坐标则为第k个时刻的历史位置坐标;也就是说,本申请中的历史线速度为腰部中心点在历史时刻对应估计的线速度,历史位置坐标为腰部中心点在历史时刻对应估计的位置坐标。
另外,作为一种可能的实施方式,计算得到预测姿态参数的公式可以满足如下:
式中,
表示预测姿态参数,
表示预测位置坐标,
表示预测线速度;
表示历史位置坐标,
表示历史线速度;
表示第二腰部线加速度;A表示设定的状态转移矩阵;B表示设定的状态控制矩阵。
比如在上述的第k-1个时刻与第k个时刻两者的时间间隔为ΔT的示例中,A和B可以分别表示为:
另外,在图2的基础上,请参阅图7,图7示出图2中步骤205的子步骤的一种示意性流程框图,作为一种可能的实施方式,步骤205可以包括以下子步骤:
步骤205-1,根据在历史时刻对应的历史卡尔曼参数,计算在当前时刻的卡尔曼增益;
步骤205-3,利用卡尔曼增益对测量姿态参数和预测姿态参数进行处理,得到仿人机器人的腰部中心点的估计姿态参数。
在例如上述的将仿人机器人的腰部中心点运动视为匀加速过程的模型中,仿人机器人不仅可以连续保存每一个估计姿态参数,还可以保存每一个计算估计姿态参数时刻的卡尔曼参数,以使仿人机器人可以利用保存的第k-1个时刻的卡尔曼参数,计算第k个时刻的卡尔曼增益。
比如,仿人机器人在计算当前时刻的卡尔曼增益时,采用的计算公式可以满足如下:
Kk=P′kHT(HP′kHT+R)-1
式中,Kk表示卡尔曼增益;P′k表示卡尔曼滤波器输出的预测值和估计值之间的误差协方差矩阵;H表示设定的观测矩阵;R表示设定的测量噪声协方差矩阵;
其中,卡尔曼滤波器输出的预测值和估计值之间的误差协方差矩阵的计算公式满足如下:
P′k=APk-1AT+Q
式中,Pk-1表示历史卡尔曼参数,A表示根据设定的状态转移矩阵,即上述示例的ΔT;Q表示设定的系统噪声协方差矩阵。
于是,仿人机器人可以利用上述计算得到的卡尔曼增益,对上述得到的测量姿态参数和预测姿态参数进行处理,从而得到仿人机器人的腰部中心点的估计姿态参数。
示例性地,计算得到估计姿态参数的公式满足如下:
式中,
表示估计姿态参数,其维度为6×1,本申请中
为估计位置坐标,
为估计线速度;
表示预测姿态参数;K
k表示卡尔曼增益;Z
k表示测量姿态参数;H表示设定的观测矩阵。
其中,本申请所述的方案中,观测的仿人机器人的参数包括三维空间中的位置坐标以及线速度,因此,该观测矩阵H可以表示如下:
另外,如上所述,仿人机器人在计算第k个时刻的估计姿态参数时,不仅可以保存第k个时刻的估计姿态参数,用作第k+1个时刻的历史估计参数,还可以保存第k个时刻的卡尔曼参数,用于计算第k+1个时刻的卡尔曼增益。
因此,在一些实施例中,仿人机器人还可以根据上述的当前时刻的卡尔曼增益,计算当前时刻的卡尔曼参数,并保存该当前时刻的卡尔曼参数。
其中,示例性地,仿人机器人计算当前时刻的卡尔曼参数的公式满足如下:
Pk=(I-KkH)P′k
式中,Pk表示当前时刻的卡尔曼参数,I表示单位矩阵。
另外,需要说明的是,当仿人机器人配置有至少两条腿时,仿人机器人采用不同的腿着力,对仿人机器人的腰部姿态参数的估计也会产生影响。
因此,在一些实施例中,仿人机器人在执行步骤201时,可以对获得的仿人机器人的腰部姿态参数以及至少两条腿中的支撑腿对应的脚底运动参数进行处理,得到仿人机器人的腰部中心点的测量姿态参数;其中,该支撑腿为至少两条腿中用于支撑仿人机器人着地的腿。
也就是说,仿人机器人在执行步骤201时,仿人机器人是利用支撑仿人机器人着地的腿计算测量姿态参数;如此,可以使仿人机器人计算得到的估计姿态参数与实际的情况更为符合,对仿人机器人的腰部姿态参数的估计精度更高。
其中,在一些实施例中,当仿人机器人存在多条支撑腿时,仿人机器人可以先根据每一条支撑腿计算出一个初始估计参数,然后再综合所有的初始估计参数求出仿人机器人的估计姿态参数。
另外,需要说明的是,支撑腿可以由仿人机器人接收其他一些设备的指示信息确定,也可以由仿人机器人通过执行一些判断算法确定。
比如,以图3所示的包括两条腿的仿人机器人为例,可以在仿人机器人的两条腿处分别安装六维力传感器,分别采集仿人机器人两条腿分别在Z方向的力。
在判断支撑腿时:
当左腿的Z向力超过右腿的Z向力一定阈值,则可以判定仿人机器人的左腿属于支撑腿;
当右腿的Z向力超过左腿的Z向力一定阈值,则可以判定仿人机器人的右腿属于支撑腿;
当右腿的Z向力与左腿的Z向力之间的绝对值小于或等于该阈值,则可以判定仿人机器人的两条腿均为支撑腿。
另外,基于与本申请提供的上述机器人位姿估计方法相同的发明构思,请参阅图8,本申请还提供一种机器人位姿估计装置300,该机器人位姿估计装置300可以包括处理模块301及估计模块302;其中:
处理模块301,用于对获得的仿人机器人的腰部姿态参数以及脚底运动参数进行处理,得到仿人机器人的腰部中心点的测量姿态参数;
处理模块301还用于,根据腰部姿态参数,计算仿人机器人的腰部中心点的预测姿态参数;
估计模块302,用于将测量姿态参数和预测姿态参数进行融合,得到仿人机器人的腰部中心点的估计姿态参数。
可选地,作为一种可能的实施方式,处理模块301在对获得的仿人机器人的腰部姿态参数以及脚底运动参数进行处理,得到仿人机器人的腰部中心点的测量姿态参数时,具体用于:
利用腰部姿态参数及脚底运动参数进行运动学解算,得到仿人机器人的脚底接触点坐标;其中,脚底接触点坐标为仿人机器人的脚底与地面的接触点在世界坐标系下的坐标;
根据脚底接触点坐标、脚底运动参数及腰部姿态参数进行运动学解算,得到仿人机器人的腰部中心点的测量姿态参数。
可选地,作为一种可能的实施方式,腰部姿态参数包括腰部中心点在世界坐标系下的腰部姿态角、腰部姿态角速度;
处理模块301在根据脚底接触点坐标、脚底运动参数及腰部姿态参数进行运动学解算,得到仿人机器人的腰部中心点的测量姿态参数时,具体用于:
利用腰部姿态角、脚底接触点坐标以及脚底运动参数进行运动学解算处理,得到腰部中心点的测量位置坐标;
利用腰部姿态角速度、脚底接触点坐标以及脚底运动参数进行运动学解算处理,得到腰部中心点的测量线速度;
其中,测量姿态参数包括测量位置坐标以及测量线速度。
可选地,作为一种可能的实施方式,脚底运动参数包括仿人机器人的脚底位移参数;
处理模块301在利用腰部姿态参数及脚底运动参数进行运动学解算,得到仿人机器人的脚底接触点坐标时,具体用于:
利用腰部姿态参数以及脚底位移参数进行运动学解算,得到仿人机器人的脚底姿态角;
在预设的姿态配置策略中查找与脚底姿态角对应的初始接触点坐标;其中,初始接触点坐标为仿人机器人的脚底与地面的接触点在脚底坐标系中的坐标,脚底坐标系为根据仿人机器人的脚底所建立的坐标系,姿态配置策略包括有多种脚底姿态下接触点在脚底坐标系下的坐标;
将初始接触点坐标从脚底坐标系转换到世界坐标系中,得到脚底接触点坐标。
可选地,作为一种可能的实施方式,腰部姿态参数包括腰部中心点在世界坐标系下的腰部姿态角以及在腰部坐标系下的第一腰部线加速度;腰部坐标系为根据腰部中心点建立的坐标系;
处理模块301在根据腰部姿态参数,计算仿人机器人的腰部中心点的预测姿态参数时,具体用于:
根据腰部姿态角,将第一腰部线加速度从腰部坐标系转换为世界坐标系下的第二腰部线加速度;
根据第二腰部线加速度、腰部中心点的历史线速度以及腰部中心点的历史位置坐标,计算腰部中心点的预测线速度以及预测位置坐标;其中,历史线速度为腰部中心点在历史时刻对应估计的线速度,历史位置坐标为腰部中心点在历史时刻对应估计的位置坐标;预测姿态参数包括预测线速度以及预测位置坐标。
可选地,作为一种可能的实施方式,估计模块302在将测量姿态参数和预测姿态参数进行融合,得到仿人机器人的腰部中心点的估计姿态参数时,具体用于:
根据在历史时刻对应的历史卡尔曼参数,计算在当前时刻的卡尔曼增益;
利用卡尔曼增益对测量姿态参数和预测姿态参数进行处理,得到仿人机器人的腰部中心点的估计姿态参数。
可选地,作为一种可能的实施方式,仿人机器人包括至少两条腿;
处理模块301在对获得的仿人机器人的腰部姿态参数以及脚底运动参数进行处理,得到仿人机器人的腰部中心点的测量姿态参数时,具体用于:
对获得的仿人机器人的腰部姿态参数以及至少两条腿中的支撑腿对应的脚底运动参数进行处理,得到仿人机器人的腰部中心点的测量姿态参数;其中,支撑腿为至少两条腿中用于支撑仿人机器人着地的腿。
在本申请所提供的实施例中,应该理解到,所揭露的装置和方法,也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的,例如,附图中的流程图和框图显示了根据本申请的一些实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上,流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分,所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。
也应当注意,在有些作为替换的实现方式中,方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如,两个连续的方框实际上可以基本并行地执行,它们有时也可以按相反的顺序执行,这依所涉及的功能而定。
也要注意的是,框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合,可以用执行规定的功能或动作的专用的基于硬件的系统来实现,或者可以用专用硬件与计算机指令的组合来实现。
另外,在本申请的一些实施例中的各功能模块可以集成在一起形成一个独立的部分,也可以是各个模块单独存在,也可以两个或两个以上模块集成形成一个独立的部分。
所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时,可以存储在一个计算机可读取存储介质中。基于这样的理解,本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来,该计算机软件产品存储在一个存储介质中,包括若干指令用以使得一台计算机设备(可以是个人计算机,服务器,或者网络设备等)执行本申请的一些实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器、随机存取存储器、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述仅为本申请的部分实施例而已,并不用于限制本申请,对于本领域的技术人员来说,本申请可以有各种更改和变化。凡在本申请的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本申请的保护范围之内。
对于本领域技术人员而言,显然本申请不限于上述示范性实施例的细节,而且在不背离本申请的精神或基本特征的情况下,能够以其它的具体形式实现本申请。因此,无论从哪一点来看,均应将实施例看作是示范性的,而且是非限制性的,本申请的范围由所附权利要求而不是上述说明限定,因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本申请内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。