CN113119102A - 基于浮动基飞轮倒立摆的仿人机器人建模方法和装置 - Google Patents

Abstract

本发明实施例公开了一种基于浮动基飞轮倒立摆的仿人机器人建模方法和装置，该方法包括：基于第一正向运动学函数将仿人机器人的真实关节空间正向映射至浮动基飞轮倒立摆的关节空间；基于第二正向运动学函数将浮动基飞轮倒立摆的关节空间正向映射至浮动基飞轮倒立摆的工作空间；基于第二反向运动学函数将所述工作空间逆向映射至所述关节空间；基于第一反向运动学函数将所述关节空间逆向映射至所述真实关节空间。本发明的技术方案基于浮动基飞轮倒立摆对仿人机器人进行建模，该模型不仅能反映机器人的上身姿态，也能反映浮动基和仿人机器人的脚底姿态等，基于此模型对该仿人机器人可以进行完全运动控制，控制效果好，鲁棒性好等。

Description

基于浮动基飞轮倒立摆的仿人机器人建模方法和装置

技术领域

本发明涉及仿人机器人技术领域，尤其涉及一种基于浮动基飞轮倒立摆的仿人机器人建模方法和装置。

背景技术

双足仿人机器人是具有极高自由度的一种特殊机器人。一个能够完成全向和全地形行走的双足仿人机器人需要至少12个自由度。其多自由度和非线性的特点为规划和控制带来极大挑战。双足仿人机器人的工作空间控制需要考虑其动力学和运动学模型的计算。

目前，其动力学和运动学模型的计算主要有两种技术路线：1.从全动力学角度通过牛顿欧拉或者欧拉拉格朗日方法直接计算双足仿人机器人所有连杆的动力学方程，并通过全运动学的角度通过几何或者解析的方法直接计算双足仿人机器人的运动学方程；2.将双足仿人机器人简化成为更为简单的虚拟模型，在简单的虚拟模型上进行运动控制，而真实的仿人机器人通过复制虚拟模型的动态实现运动，常见的简化模型有线性倒立摆模型、弹性倒立摆模型等等。

然而，针对上述提到的两种方案均存在一些问题。例如，对于第一种方案，该双足仿人机器人的每一个关节就需要至少十二个动力学参数，如包含质量、质心、惯性张量和摩擦系数等等，而这些参数往往难于直接测量或者辨识而导致其计算并不准确。而对于第二种方案，首先，在简化模型中，由于线性倒立摆模型、弹性倒立摆模型忽略机器人上身的姿态，这为双足仿人机器人的上身姿态控制带来很大问题。其次，线性倒立摆模型的衍生模型往往假设倒立摆摆杆的支点固定于地面，因此未考虑地面倾斜等情况下对脚底姿态的控制问题，这使得简化模型的控制效果不好、鲁棒性较差等等。

发明内容

有鉴于此，本发明目的是为了克服现有技术中的不足，提供一种基于浮动基飞轮倒立摆的仿人机器人建模方法和装置。

本发明的一实施例提供一种基于浮动基飞轮倒立摆的仿人机器人建模方法，包括：

基于第一正向运动学函数将仿人机器人的真实关节空间正向映射至浮动基飞轮倒立摆的关节空间；

基于第二正向运动学函数将所述浮动基飞轮倒立摆的关节空间正向映射至所述浮动基飞轮倒立摆的工作空间；

基于第二反向运动学函数将所述浮动基飞轮倒立摆的工作空间逆向映射至所述浮动基飞轮倒立摆的关节空间；

基于第一反向运动学函数将所述浮动基飞轮倒立摆的关节空间逆向映射至所述仿人机器人的真实关节空间。

进一步地，在上述的基于浮动基飞轮倒立摆的仿人机器人建模方法中，所述浮动基飞轮倒立摆的关节空间中的向量由第一旋转自由度、第二旋转自由度、直线自由度和浮动基构成；基于所述第一正向运动学函数的正向映射的过程，包括：

根据所述仿人机器人的关节位置计算所述仿人机器人从腰到脚踝的长度，将所述直线自由度映射为所述从腰到脚踝的长度；

根据所述仿人机器人的关节姿态计算所述仿人机器人的脚踝姿态和脚底姿态，将所述第一旋转自由度映射为所述脚踝姿态，并将所述浮动基映射为所述脚底姿态；

将所述脚踝姿态与所述脚底姿态相加，并将所述第二旋转自由度映射为所述相加的结果与所述腰姿态之间的差值。

进一步地，在上述的基于浮动基飞轮倒立摆的仿人机器人建模方法中，所述仿人机器人的关节位姿的各关节的位置及姿态的获取，包括：

通过惯性测量单元测量以获取所述仿人机器人的腰姿态，并将所述仿人机器人的腰作为运动链原点；基于运动链规则根据所述腰姿态和对应的关节角度计算所述仿人机器人对应关节的位置及姿态；

所述运动链规则的计算公式为：

其中，R_k代表所述仿人机器人第k种关节姿态；IMU为所述惯性测量单元测量的角度；RPY()为欧拉角到旋转矩阵的转换公式；Rot(q_i)表示对第i个关节角度q_i进行旋转变换。

进一步地，在上述的基于浮动基飞轮倒立摆的仿人机器人建模方法中，基于所述第一反向运动学函数的逆向映射的过程，包括：

将所述浮动基飞轮倒立摆的关节空间中的向量在笛卡尔坐标系下进行xz平面和yz平面投影，分别得到横滚方向和俯仰方向的平面分量；

基于分别构建的横滚方向的和俯仰方向的几何约束函数将所述平面分量映射到所述真实关节空间下对应的坐标点，以获取所述仿人机器人的腰位姿、左腿位姿和右腿位姿；

根据所述腰位姿坐标、所述左腿位姿坐标和所述右腿位姿坐标利用逆运动学求解方法求解所述仿人机器人的各关节角度。

进一步地，在上述的基于浮动基飞轮倒立摆的仿人机器人建模方法中，所述浮动基飞轮倒立摆的关节空间中的向量表示为(θ₁，θ₂，θ₃，γ)，其中，θ₁，θ₂，θ₃和γ依次表示第一旋转自由度、直线自由度、第二旋转自由度和浮动基；所述真实关节空间中的三维姿态向量由欧拉角构成，表示为(χ，ψ，ω)；当所述仿人机器人双脚落地时，所述横滚方向的几何约束函数为：

χ_W＝θ_3,roll

χ_L＝θ_1,roll

χ_R＝θ_1,roll；

z_L＝θ_2,roll在z轴投影+Δl_z,L

z_R＝θ_2,roll在z轴投影+Δl_z,R

其中，Δl_z,L+Δl_z,R等于所述真实关节空间中该仿人机器人的左腿和右腿在z方向的高度差；θ_1,roll、θ_2,roll和θ_3,roll分别为θ₁、θ₂和θ₃在横滚方向的平面分量；χ_W、χ_L和χ_R分别为该仿人机器人的腰、左腿和右腿在x方向的角度；z_L和z_R分别为该仿人机器人的左腿和右腿在z方向的高度。

进一步地，在上述的基于浮动基飞轮倒立摆的仿人机器人建模方法中，所述浮动基飞轮倒立摆的关节空间的向量表示为(θ₁，θ₂，θ₃，γ)，所述浮动基飞轮倒立摆的工作空间的坐标表示为(φ₁，φ₂，y_com，z_com)，其中，φ₁代表摆杆与z轴的夹角；φ₂代表飞轮与z轴的夹角；y_com代表质心位置；z_com代表质心高度；

所述第二正向运动学函数为：

y_com＝θ₂cos(φ₁)

z_com＝θ₂sin(φ₁)

所述第二反向运动学函数为：

θ₃＝φ₂-φ₁

进一步地，在上述的基于浮动基飞轮倒立摆的仿人机器人建模方法中，还包括：

将所述仿人机器人的各关节的质量之和作为所述浮动基飞轮倒立摆的飞轮质量，并基于合质心原理计算所述浮动基飞轮倒立摆的质心位置和质心高度；所述质心位置y_com和所述质心高度z_com分别为：

其中，M为飞轮质量；n为该仿人机器人的关节数量；m_i为该仿人机器人第i个关节的质量；y_i为该仿人机器人第i个关节在y方向的质心坐标；z_i为该仿人机器人第i个关节在z方向的质心坐标。

进一步地，在上述的基于浮动基飞轮倒立摆的仿人机器人建模方法中，还包括：

利用平行轴原理将所述仿人机器人的各关节的惯量平移至合质心坐标系后相加，得到所述浮动基飞轮倒立摆的惯量；

其中，所述浮动基飞轮倒立摆的惯量为：

其中，I_ci表示将第i个关节的惯性张量矩阵平移到合质心后的惯性张量矩阵。

本发明的另一实施例提出一种基于浮动基飞轮倒立摆的仿人机器人建模装置，包括：

关节空间正向映射模块，用于基于第一正向运动学函数将仿人机器人的真实关节空间正向映射至浮动基飞轮倒立摆的关节空间；

工作空间正向映射模块，用于基于第二正向运动学函数将所述浮动基飞轮倒立摆的关节空间正向映射至所述浮动基飞轮倒立摆的工作空间；

工作空间逆向映射模块，用于基于第二反向运动学函数将所述浮动基飞轮倒立摆的工作空间逆向映射至所述浮动基飞轮倒立摆的关节空间；

关节空间逆向映射模块，用于基于第一反向运动学函数将所述浮动基飞轮倒立摆的关节空间逆向映射至所述仿人机器人的真实关节空间。

本发明的又一实施例提出一种仿人机器人，基于好述的基于浮动基飞轮倒立摆的仿人机器人建模方法得到的模型进行运动控制。

本发明的再一实施例提出一种计算机可读存储介质，其存储有计算机程序，在所述计算机程序被执行时，实施上所述的基于浮动基飞轮倒立摆的仿人机器人建模方法。

本发明的实施例具有如下优点：

本发明实施例的技术方案提出基于浮动基飞轮倒立摆对仿人机器人进行建模，通过该模型不仅能反映机器人的上身姿态，也能反映浮动基和仿人机器人的脚底姿态等，基于此模型对该仿人机器人可以进行完全运动控制，控制效果好，鲁棒性好等。

附图说明

为了更清楚地说明本发明的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对本发明保护范围的限定。在各个附图中，类似的构成部分采用类似的编号。

图1示出了本发明实施例的具有十二自由度的仿人机器人的简化模型；

图2示出了本发明实施例的基于浮动基飞轮倒立摆的仿人机器人建模方法的第一流程示意图；

图3示出了本发明实施例的基于浮动基飞轮倒立摆的仿人机器人建模方法的第一阶段的映射示意图；

图4示出了本发明实施例的基于浮动基飞轮倒立摆的仿人机器人建模方法的第二阶段的映射示意图；

图5示出了本发明实施例的基于浮动基飞轮倒立摆的虚拟关节空间的解耦示意图；

图6示出了本发明实施例的基于浮动基飞轮倒立摆的仿人机器人双脚落地的约束示意图；

图7示出了本发明实施例的基于浮动基飞轮倒立摆的第一阶段的正向映射的流程图；

图8示出了本发明实施例的基于浮动基飞轮倒立摆的仿人机器人建模装置的结构示意图。

主要元件符号说明：

10-仿人机器人建模装置；110-关节空间正向映射模块；120-工作空间正向映射模块；130-工作空间逆向映射模块；140-关节空间逆向映射模块。

具体实施方式

下面将结合本发明实施例中附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

通常在此处附图中描述和示出的本发明实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本发明的实施例的详细描述并非旨在限制要求保护的本发明的范围，而是仅仅表示本发明的选定实施例。基于本发明的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。

在下文中，可在本发明的各种实施例中使用的术语“包括”、“具有”及其同源词仅意在表示特定特征、数字、步骤、操作、元件、组件或前述项的组合，并且不应被理解为首先排除一个或更多个其它特征、数字、步骤、操作、元件、组件或前述项的组合的存在或增加一个或更多个特征、数字、步骤、操作、元件、组件或前述项的组合的可能性。

此外，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

除非另有限定，否则在这里使用的所有术语(包括技术术语和科学术语)具有与本发明的各种实施例所属领域普通技术人员通常理解的含义相同的含义。所述术语(诸如在一般使用的词典中限定的术语)将被解释为具有与在相关技术领域中的语境含义相同的含义并且将不被解释为具有理想化的含义或过于正式的含义，除非在本发明的各种实施例中被清楚地限定。

图1示出了本发明中的仿人机器人的简化模型，共有12个自由度(亦称关节)，分别是左右腿各6个，其中，髋部3个，膝部1个，踝部1个，由于腰部实质为连接上半身和左右髋关节的固件，不具备转动功能，因此可将其视为刚体，并且该仿人机器人的上躯体姿态与腰姿态相同。此外，在仿人机器人的上躯体安装有惯性测量单元IMU，该IMU可以用于测量仿人机器人上身的俯仰角度、横滚角度及偏航角。应当理解，该仿人机器人的各连杆具有其对应的质量和长度。而本发明中所述的关节角度表示的是连杆与其母连杆之间的旋转夹角，例如，q₄则表示右小腿和右大腿之间的夹角等。

下面基于该仿人机器人进行基于浮动基飞轮倒立摆的仿人机器人建模方法的具体说明。

实施例1

请参照图2，本实施例提出一种基于浮动基飞轮倒立摆的仿人机器人建模方法，可应用于仿人机器人的简化模型构建。通过本实施例的基于浮动基飞轮倒立摆的建模方法可对该仿人机器人进行完全运动控制，并达到较好的控制效果及鲁棒性等。

本实施例中，该仿人机器人建模方法主要分为两个阶段：第一阶段是从将真实的仿人机器人的关节空间映射到虚拟的浮动基飞轮倒立摆的关节空间；第二阶段则是将该浮动基飞轮倒立摆的关节空间映射到该浮动基飞轮倒立摆的工作空间。

如图2所示，该基于浮动基飞轮倒立摆的建模方法主要包括以下步骤：

步骤S10，基于第一正向运动学函数将仿人机器人的真实关节空间正向映射至浮动基飞轮倒立摆的关节空间。

对于上述步骤S10，可通过第一正向运动学函数FK1来描述该仿人机器人的真实关节空间到浮动基飞轮倒立摆的关节空间的映射关系，即为上述第一阶段的正向映射过程。可以理解，该第一阶段的正向映射过程主要是将该仿人机器人各连杆的位置及姿态与关节角度之间的关系通过一个简化的浮动基飞轮倒立摆模型来描述，以便进行运动学分析和控制。

如图3所示，在仿人机器人的真实关节空间中，其关节角度构成的空间坐标向量可描述为：Q＝[q₁ q₂ … q₁₂]^T，其中，q_i代表第i个关节角度，i＝1，2，...，12。而通过对应的该关节角度并基于运动链规则(也称链乘法则)可计算得到仿人机器人各关节的位置及姿态，也称为关节位姿。

例如，可通过安装在仿人机器人上躯体的惯性测量单元IMU来获取该仿人机器人的上躯体姿态，由于将该仿人机器人的上躯体视为刚体，故该上躯体姿态也视为该仿人机器人的腰姿态。然后，以腰为运动链的原点并基于从IMU到对应关节的运动链即可计算得到对应关节的姿态及位置。优选地，可通过如下公式计算该仿人机器人某一部位关节的姿态：

其中，m＝1，2，…,12；R_k代表该仿人机器人第k种关节姿态；IMU为该惯性测量单元IMU测量的欧拉角度；RPY()为欧拉角到旋转矩阵的转换公式；Rot(q_i)表示对第i个关节角度q_i进行旋转变换。

而在该浮动基飞轮倒立摆的关节空间中，空间坐标向量可描述为：Θ＝[θ₁ θ₂ θ₃γ]^T，其中，θ₁和θ₃分别代表第一旋转自由度和第二旋转自由度，θ₂代表直线自由度，γ代表浮动基。

在一个实施例中，为实现如图3所示的左边的真实关节空间到右边的虚拟关节空间的正向映射，可采用如下映射关系：令该第一旋转自由度θ₁映射为该仿人机器人的脚踝姿态；令直线自由度θ₂映射为该仿人机器人的腿长，即从腰到脚踝的长度；令该浮动基γ映射为该仿人机器人的腿底姿态；令该第二旋转自由度θ₃映射为该仿人机器人的腿与腰之间的相对姿态，即该脚踝姿态和脚底姿态之和与腰姿态之间的差值。

可以理解，若采用上述的映射关系，在获取到该仿人机器人对应关节的位置及姿态后，即可根据该映射关系确定该第一正向运动学函数FK1。当然，也可以采用其他的映射关系，在此并不作限定。

步骤S20，基于第二正向运动学函数将该浮动基飞轮倒立摆的关节空间正向映射至该浮动基飞轮倒立摆的工作空间。

对于上述步骤S20，可通过第二正向运动学函数FK2来描述该仿人机器人的真实关节空间到浮动基飞轮倒立摆的关节空间的正向映射关系，即为第二阶段的正向映射过程。可以理解，该第二阶段的正向映射过程主要是将由飞轮和倒立摆摆杆等构成的该浮动基飞轮倒立摆模型从虚拟的关节空间转换到工作空间进行描述，以实现动力学分析及控制。

在一种实施例中，如图4所示，在该浮动基飞轮倒立摆的工作空间中，其坐标可表示为：Φ＝[φ₁ φ₂ y_com z_com]，其中，φ₁表示该浮动基飞轮倒立摆的摆杆与z轴之间的夹角；φ₂表示该浮动基飞轮倒立摆的飞轮与z轴之间的夹角；y_com表示质心位置；z_com表示质心高度。而该浮动基飞轮倒立摆的关节空间中，空间坐标可表示为：Θ＝[θ₁ θ₂ θ₃ γ]^T。于是，基于几何原理可得到如下所示的第二正向运动学函数FK2：

y_com＝θ₂cos(φ₁)

z_com＝θ₂sin(φ₁)

可以理解，上述的第二运动学函数FK2为该第二正向运动学函数的一种示例，若上述工作空间中的各变量的表示含义发生变化，则相应的几何关系也将发生变化。

通过上述步骤S10可将该仿人机器人的真实关节空间映射到简化的浮动基飞轮倒立摆的关节空间进行运动学分析，并进一步通过上述步骤S20则将该浮动基飞轮倒立摆的关节空间映射到工作空间进行动力学分析。对于仿人机器人而言，在进行正向映射的同时还需要考虑逆向映射过程，从而实现完整的闭环反馈控制。

步骤S30，基于第二反向运动学函数将该浮动基飞轮倒立摆的工作空间逆向映射至该浮动基飞轮倒立摆的关节空间。

对于上述步骤S30，可通过第二反向运动学函数IK2来描述该仿人机器人的真实关节空间到浮动基飞轮倒立摆的关节空间的逆向映射关系，即为上述第二阶段的逆向映射过程。

示范性地，若在该浮动基飞轮倒立摆的工作空间中，其坐标表示为：Φ＝[φ₁ φ₂y_com z_com]，而该浮动基飞轮倒立摆的关节空间中，空间坐标可表示为：Θ＝[θ₁ θ₂ θ₃ γ]^T，基于上述第二正向运动学函数FK2的逆向求解，可得到如下所示的第二反向运动学函数IK2：

θ₃＝φ₂-φ₁

步骤S40，基于第一反向运动学函数将该浮动基飞轮倒立摆的关节空间逆向映射至该仿人机器人的真实关节空间。

对于上述步骤S40，可通过第一反向运动学函数IK1来描述该仿人机器人的真实关节空间到浮动基飞轮倒立摆的关节空间的逆向映射关系，即为上述第一阶段的逆向映射过程。

示范性地，在该浮动基飞轮倒立摆的关节空间中，空间坐标向量表示为：Θ＝[θ₁θ₂ θ₃ γ]^T，而真实关节空间的向量Q＝[q₁ q₂ … q₁₂]^T，为实现由低维空间到高维空间的映射，可先对该低维空间的向量Θ进行解耦以得到平面向量分量，再基于平面向量分量进行高维空间的坐标映射。

对于仿人机器人而言，由低维的平面空间向高维的立体空间进行映射时，还需要建立相应方向上的几何约束函数。然后，基于构建的几何约束函数将得到的平面分量映射到该真实关节空间下对应的坐标点，以获取该仿人机器人的腰位姿、左腿位姿和右腿位姿等主要部位的位姿坐标。最后，根据获取的腰位姿坐标、左腿位姿坐标和右腿位姿坐标这三个部位的位姿基于解析逆运动学求解出该仿人机器人的各关节角度，即得到上述向量Q。

例如，可通过将该虚拟关节空间的向量Θ在笛卡尔坐标系下的xz平面和yz平面进行投影，得到解耦后的平面分量Θ_roll和Θ_pitch。如图5所示，当对该虚拟关节空间中的向量Θ进行解耦后，可得到平面分量Θ_roll＝[θ_1,roll θ_2,roll θ_3,roll γ_roll]^T及平面分量Θ_pitch＝[θ_1,pitch θ_2,pitch θ_3,pitch γ_pitch]^T。

可以理解，上述的投影得到的平面分量能够通过RPY角与旋转矩阵的关系以及反正切函数计算得到具体的数值。以γ_roll为例，其在xz平面和yz平面的分量分别为：

其中，r_i,j代表旋转矩阵中的第i行，第j列元素。

然后，在横滚(Roll)方向和俯仰(Pitch)方向建立相应的几何约束函数。例如，如图6所示，当仿人机器人双脚落地时，在横滚(Roll)方向应满足如下约束：

χ_W＝θ_3,roll

χ_L＝θ_1,roll

χ_R＝θ_1,roll；

z_L＝θ_2,roll在z轴投影+Δl_z,L

z_R＝θ_2,roll在z轴投影+Δl_z,R

其中，Δl_z,L+Δl_z,R等于所述真实关节空间中该仿人机器人的左腿和右腿在z方向的高度差；θ_1,roll、θ_2,roll和θ_3,roll分别为θ₁、θ₂和θ₃在横滚方向的平面分量；χ_W、χ_L和χ_R分别为该仿人机器人的腰、左腿和右腿在x方向的角度；z_L和z_R分别为该仿人机器人的左腿和右腿在z方向的高度。

之后，利用计算得到的这些姿态角来更新该仿人机器人的位姿坐标，其中，该仿人机器人的腰位姿、左腿位姿和右腿位姿分别表示为P_W、P_L和P_R；如可通过解析解的逆运动学求解方法来求解该仿人机器人的各个关节角度，即有：

χ_W→P_W

χ_L,z_L→P_L

χ_R,z_R→P_R

Q＝解析逆运动学(P_W,P_L,P_R)。

可以理解，第一阶段的逆向映射为第一阶段的正向映射的逆过程，该第二阶段的逆向映射为第二阶段的正向映射的逆过程，也就是说，通过设置该浮动基飞轮倒立摆的工作空间的向量坐标，基于上述两个阶段的逆向映射可以实现对真实的仿人机器人的位置及姿态的控制。同样，当该仿人机器人在测量得到欧拉角后，基于上述两个阶段的正向映射可以反映到该工作空间进行分析等。

本实施例的基于浮动基飞轮倒立摆的仿人机器人建模方法通过提出简化的浮动基飞轮倒立摆模型对该将仿人机器人进行运动学及动力学分析，不仅考虑了真实的仿人机器人的上躯体姿态，较好地解决了上躯体姿态的控制问题；同时还考虑了浮动基问题，相对于现有的采用线性倒立摆模型及其衍生模型往往假设倒立摆摆杆的支点固定于地面等方案，本实施例提出的模型考虑了在实际应用中对该仿人机器人在如跳跃等状态下没有固定基底的脚底姿态的控制问题等。由上述可知，基于本实施例提出的模型能够完成对双足仿人机器人的完全运动控制，且控制效果、鲁棒性好等等。

实施例2

请参照图7，基于上述实施例1的方法，在本实施例的基于浮动基飞轮倒立摆的仿人机器人建模方法中，对于上述步骤S10中的第一阶段的正向映射过程，以上述的映射关系作进一步举例说明：

子步骤S101，根据仿人机器人的关节位置向量计算仿人机器人从腰到脚踝的长度，将直线自由度映射为该从腰到脚踝的长度。

子步骤S102，根据仿人机器人的关节姿态向量计算仿人机器人的脚踝姿态和脚底姿态，将第一旋转自由度映射为该脚踝姿态，并将浮动基映射为该脚底姿态。

子步骤S103，将该脚踝姿态与该脚底姿态相加，并将第二旋转自由度映射为该相加后的结果与该腰姿态之间的差值。

示范性地，求解该仿人机器人的各关节姿态时，可定义当k＝1时，R₁为该仿人机器人的腰姿态；当k＝2时，R₂为该仿人机器人的脚踝姿态；当k＝3时，R₃为该仿人机器人的脚底姿态。以R₂为例，根据上述姿态公式有：

其中，χ_m为IMU测得的横滚(roll)角，ψ_m为IMU测得的俯仰(pitch)角，ω_m为IMU测得的偏航(yaw)角。可以理解，其他关节的姿态同样可根据上述公式计算，故在此不再详述。对于各关节的位置的获取，可根据该仿人机器人各关节之间的距离及对应的关节角度基于运动链原理进行计算，优选地，可将该仿人机器人的腰作为运动链的原点。

当然，若根据该仿人机器人的关节位姿求解该浮动基飞轮倒立摆的关节空间中的向量的各参数，可通过将各关节位姿分解为三维位置向量和三维姿态向量。通常地，关节位姿通过一个六维向量P表示，如下所示：P＝[x y z χ ψ ω]，其中，x，y，z为笛卡尔坐标系下的位置坐标；χ，ψ，ω则为基于旋转-俯仰-偏摆(RPY)定义的欧拉角。例如，P_W、P_L和P_R分别代表该仿人机器人的腰位姿、左脚位姿和右脚位姿。然后，将分解的三维位置向量用于对该直线自由度θ₂的计算；利用三维姿态向量计算浮动基γ、第一旋转自由度θ₁和第二旋转自由度θ₃。

在一种实施例中，该方法还包括：将仿人机器人的各关节的质量之和作为该浮动基飞轮倒立摆的飞轮质量，并基于合质心原理计算该浮动基飞轮倒立摆的质心位置和质心高度。

示范性地，质心位置y_com和质心高度z_com分别为：

其中，M为飞轮质量；n为该仿人机器人的关节数量；m_i为该仿人机器人第i个关节的质量；y_i为该仿人机器人第i个关节在y方向的质心坐标；z_i为该仿人机器人第i个关节在z方向的质心坐标。

在一种实施例中，该方法还包括：利用平行轴原理将仿人机器人的各关节的惯量平移至合质心坐标系后相加，得到该浮动基飞轮倒立摆的惯量。

示范性地，该浮动基飞轮倒立摆的惯量为：

其中，I_ci表示将第i个关节的惯性张量矩阵平移到合质心后的惯性张量矩阵。

其中，对于惯性张量矩阵中惯性矩的平移，以y轴为例，该平行轴定理可表示为：

其中，I_gi是该仿人机器人的关节i的以质心为中心，以笛卡尔坐标系为方向的惯性张量；上标yy代表y轴惯性矩；x_i代表该仿人机器人的关节i的质心到合质心在x方向上的距离，z_i代表关节i的质心到合质心在z方向上的距离。由于x轴和z轴的惯性矩的平移原理类似，故在此不再详述。

其中，对于惯性张量矩阵中惯性积的平移，以xy轴为例，该平行轴定理可表示为：

其中，上标xy代表x轴与y轴的惯性积；y_i代表该仿人机器人的关节i的质心到合质心在y方向上的距离。由于xz轴和yz轴的惯性积的平移原理类似，故在此不再详述。

实施例3

请参照图8，基于上述实施例1的方法，本实施例提出一种基于浮动基飞轮倒立摆的仿人机器人建模装置10，包括：

关节空间正向映射模块110，用于基于第一正向运动学函数将仿人机器人的真实关节空间正向映射至浮动基飞轮倒立摆的关节空间；

工作空间正向映射模块120，用于基于第二正向运动学函数将所述浮动基飞轮倒立摆的关节空间正向映射至所述浮动基飞轮倒立摆的工作空间；

工作空间逆向映射模块130，用于基于第二反向运动学函数将所述浮动基飞轮倒立摆的工作空间逆向映射至所述浮动基飞轮倒立摆的关节空间；

关节空间逆向映射模块140，用于基于第一反向运动学函数将所述浮动基飞轮倒立摆的关节空间逆向映射至所述仿人机器人的真实关节空间。

可以理解，本实施例的装置对应于上述实施例1的方法，上述实施例1的可选项同样适用于本实施例，故在此不再详述。

本发明的另一实施例还提出一种仿人机器人，该仿人机器人将通过基于上述的基于浮动基飞轮倒立摆的仿人机器人建模方法得到的模型进行运动控制。

本发明的又一实施例还提供了一种计算机可读存储介质，其存储有计算机程序，在所述计算机程序被实施时，执行上述的基于浮动基飞轮倒立摆的仿人机器人建模方法。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和结构图显示了根据本发明的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，结构图和/或流程图中的每个方框、以及结构图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本发明各个实施例中的各功能模块或单元可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或更多个模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是智能手机、个人计算机、服务器、或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本发明的保护范围之内。

Claims (11)

1.一种基于浮动基飞轮倒立摆的仿人机器人建模方法，其特征在于，包括：

基于第一正向运动学函数将仿人机器人的真实关节空间正向映射至浮动基飞轮倒立摆的关节空间；

基于第二正向运动学函数将所述浮动基飞轮倒立摆的关节空间正向映射至所述浮动基飞轮倒立摆的工作空间；

基于第二反向运动学函数将所述浮动基飞轮倒立摆的工作空间逆向映射至所述浮动基飞轮倒立摆的关节空间；

基于第一反向运动学函数将所述浮动基飞轮倒立摆的关节空间逆向映射至所述仿人机器人的真实关节空间。

2.根据权利要求1所述的方法，其特征在于，所述浮动基飞轮倒立摆的关节空间中的向量由第一旋转自由度、第二旋转自由度、直线自由度和浮动基构成；基于所述第一正向运动学函数的正向映射的过程，包括：

根据所述仿人机器人的关节位置计算所述仿人机器人从腰到脚踝的长度，将所述直线自由度映射为所述从腰到脚踝的长度；

根据所述仿人机器人的关节姿态计算所述仿人机器人的脚踝姿态和脚底姿态，将所述第一旋转自由度映射为所述脚踝姿态，并将所述浮动基映射为所述脚底姿态；

将所述脚踝姿态与所述脚底姿态相加，并将所述第二旋转自由度映射为所述相加的结果与所述腰姿态之间的差值。

3.根据权利要求2所述的方法，其特征在于，所述仿人机器人的关节位姿的各关节的位置及姿态的获取，包括：

通过惯性测量单元测量以获取所述仿人机器人的腰姿态，并将所述仿人机器人的腰作为运动链原点；基于运动链规则根据所述腰姿态和对应的关节角度计算所述仿人机器人对应关节的位置及姿态；

所述运动链规则的计算公式为：

其中，R_k代表所述仿人机器人第k种关节姿态；IMU表示所述惯性测量单元测量的角度；RPY()为欧拉角到旋转矩阵的转换公式；Rot(q_i)表示对第i个关节角度q_i进行旋转变换。

4.根据权利要求2所述的方法，其特征在于，基于所述第一反向运动学函数的逆向映射的过程，包括：

将所述浮动基飞轮倒立摆的关节空间中的向量在笛卡尔坐标系下进行xz平面和yz平面投影，分别得到横滚方向和俯仰方向的平面分量；

基于分别构建的横滚方向的和俯仰方向的几何约束函数将所述平面分量映射到所述真实关节空间下对应的坐标点，以获取所述仿人机器人的腰位姿坐标、左腿位姿坐标和右腿位姿坐标；

根据所述腰位姿坐标、所述左腿位姿坐标和所述右腿位姿坐标利用逆运动学求解方法求解所述仿人机器人的各关节角度。

5.根据权利要求4所述的方法，其特征在于，所述浮动基飞轮倒立摆的关节空间中的向量表示为(θ₁，θ₂，θ₃，γ)，其中，θ₁，θ₂，θ₃和γ依次表示第一旋转自由度、直线自由度、第二旋转自由度和浮动基；所述真实关节空间中的三维姿态向量由欧拉角构成，表示为(χ，ψ，ω)；当所述仿人机器人双脚落地时，所述横滚方向的几何约束函数为：

其中，Δl_z,L+Δl_z,R等于所述真实关节空间中该仿人机器人的左腿和右腿在z方向的高度差；θ_1,roll、θ_2,roll和θ_3,roll分别为θ₁、θ₂和θ₃在横滚方向的平面分量；χ_W、χ_L和χ_R分别为该仿人机器人的腰、左腿和右腿在x方向的角度；z_L和z_R分别为该仿人机器人的左腿和右腿在z方向的高度。

6.根据权利要求5所述的方法，其特征在于，所述浮动基飞轮倒立摆的关节空间的向量表示为(θ₁，θ₂，θ₃，γ)，所述浮动基飞轮倒立摆的工作空间的坐标表示为(φ₁，φ₂，y_com，z_com)，其中，φ₁代表摆杆与z轴的夹角；φ₂代表飞轮与z轴的夹角；y_com代表质心位置；z_com代表质心高度；

所述第二正向运动学函数为：

所述第二反向运动学函数为：

7.根据权利要求6所述的方法，其特征在于，还包括：

将所述仿人机器人的各关节的质量之和作为所述浮动基飞轮倒立摆的飞轮质量，并基于合质心原理计算所述浮动基飞轮倒立摆的质心位置和质心高度；所述质心位置y_com和所述质心高度z_com分别为：

其中，M为飞轮质量；n为该仿人机器人的关节数量；m_i为该仿人机器人第i个关节的质量；y_i为该仿人机器人第i个关节在y方向的质心坐标；z_i为该仿人机器人第i个关节在z方向的质心坐标。

8.根据权利要求7所述的方法，其特征在于，还包括：

利用平行轴原理将所述仿人机器人的各关节的惯量平移至合质心坐标系后相加，得到所述浮动基飞轮倒立摆的惯量；

所述浮动基飞轮倒立摆的惯量为：

其中，I_ci表示将第i个关节的惯性张量矩阵平移到合质心后的惯性张量矩阵。

9.一种基于浮动基飞轮倒立摆的仿人机器人建模装置，其特征在于，包括：

关节空间正向映射模块，用于基于第一正向运动学函数将仿人机器人的真实关节空间正向映射至浮动基飞轮倒立摆的关节空间；

工作空间正向映射模块，用于基于第二正向运动学函数将所述浮动基飞轮倒立摆的关节空间正向映射至所述浮动基飞轮倒立摆的工作空间；

工作空间逆向映射模块，用于基于第二反向运动学函数将所述浮动基飞轮倒立摆的工作空间逆向映射至所述浮动基飞轮倒立摆的关节空间；

关节空间逆向映射模块，用于基于第一反向运动学函数将所述浮动基飞轮倒立摆的关节空间逆向映射至所述仿人机器人的真实关节空间。

10.一种仿人机器人，其特征在于，基于根据权利要求1至8中任一项所述的基于浮动基飞轮倒立摆的仿人机器人建模方法得到的模型进行运动控制。

11.一种计算机可读存储介质，其特征在于，其存储有计算机程序，在所述计算机程序被执行时，实施根据权利要求1至8中任一项所述的基于浮动基飞轮倒立摆的仿人机器人建模方法。

Images