一种机器人姿态控制方法、装置、可读存储介质及机器人

摘要

本申请属于机器人技术领域，尤其涉及一种机器人姿态控制方法、装置、计算机可读存储介质及机器人。所述方法构建机器人的虚拟模型，所述虚拟模型包括所述机器人的飞轮倒立摆模型以及所述机器人的脚底面与水平面所成夹角；基于所述虚拟模型，对所述机器人进行外环闭环反馈、内环抗外界位置扰动、内环抗外界速度扰动以及内环抗外界力扰动的姿态控制。通过本申请，提出了一种全新的虚拟模型，在机器人的飞轮倒立摆模型的基础上又增加了对于机器人脚底面与水平面夹角的考虑，能够充分反应出机器人上身姿态、质心以及脚板姿态等这些对于机器人的平衡以及姿态控制极为关键的要素，基于这一虚拟模型进行姿态控制，具有更好的控制效果和更强的鲁棒性。

权利要求

1.一种机器人姿态控制方法，其特征在于，包括：构建机器人的虚拟模型，所述虚拟模型包括所述机器人的飞轮倒立摆模型以及所述机器人的脚底面与水平面所成夹角；

基于所述虚拟模型，对所述机器人进行外环闭环反馈、内环抗外界位置扰动、内环抗外界速度扰动以及内环抗外界力扰动的姿态控制。

2.根据权利要求1所述的机器人姿态控制方法，其特征在于，所述构建机器人的虚拟模型包括：构建所述虚拟模型的关节空间和工作空间；

构建从所述机器人的关节空间到所述虚拟模型的关节空间的正运动学映射关系；

构建从所述虚拟模型的关节空间到所述虚拟模型的工作空间的正运动学映射关系；

构建从所述虚拟模型的工作空间到所述虚拟模型的关节空间的逆运动学映射关系；

构建从所述虚拟模型的关节空间到所述机器人的关节空间的逆运动学映射关系。

3.根据权利要求1所述的机器人姿态控制方法，其特征在于，所述对所述机器人进行外环闭环反馈、内环抗外界位置扰动、内环抗外界速度扰动以及内环抗外界力扰动的姿态控制包括：通过预设的闭环反馈控制器输出控制力；

通过预设的导纳控制器将所述控制力转化为所述虚拟模型的工作空间的位姿控制量；

通过预设的第二逆向映射模块将所述位姿控制量转化为所述虚拟模型的关节空间的第一关节控制量；

通过预设的第一逆向映射模块将所述第一关节控制量转化为所述机器人的关节空间的第二关节控制量；

根据所述第二关节控制量对所述机器人进行姿态控制。

4.根据权利要求3所述的机器人姿态控制方法，其特征在于，所述对所述机器人进行外环闭环反馈、内环抗外界位置扰动、内环抗外界速度扰动以及内环抗外界力扰动的姿态控制还包括：通过预设的零空间模块进行抗外界位置扰动的内环反馈控制；

通过预设的外力估计模块进行对外力的估计并抵消外界力扰动；

通过预设的惯性测量传感器测量所述机器人的腰部姿态的变化量；

通过预设的第一正向映射模块将所述机器人的腰部姿态的变化量转化为所述虚拟模型的关节空间的第一关节变化量；

通过预设的第二正向映射模块将所述第一关节变化量转化为所述虚拟模型的工作空间的第二关节变化量。

5.根据权利要求4所述的机器人姿态控制方法，其特征在于，所述通过预设的零空间模块进行抗外界位置扰动的内环反馈控制包括：将所述第一关节变化量进行解耦，得到水平面夹角变化量和关节角变化量；

使用预设的雅克比矩阵计算所述关节角变化量的零空间的基向量；

根据所述水平面夹角变化量、所述关节角变化量和所述零空间的基向量计算所述虚拟模型的关节空间的关节控制偏移量。

6.根据权利要求2所述的机器人姿态控制方法，其特征在于，所述构建所述虚拟模型的关节空间和工作空间包括：根据下式构建所述虚拟模型的关节空间的坐标向量：Θ=[θ1,θ2,θ3,γ]T

其中，θ1为所述虚拟模型的摆杆与脚面所成夹角，θ2为所述虚拟模型的摆杆长度，θ3为所述虚拟模型的飞轮与摆杆所成夹角，γ为所述虚拟模型的脚底面与水平面所成夹角，Θ为所述虚拟模型的关节空间的坐标向量。

7.根据权利要求2所述的机器人姿态控制方法，其特征在于，所述构建所述虚拟模型的关节空间和工作空间包括：根据下式构建所述虚拟模型的工作空间的坐标向量：Φ=[φ1,φ2,ycom,zcom]T

其中，φ1为所述虚拟模型的摆杆与笛卡尔坐标系z轴所成夹角，φ2为所述虚拟模型的飞轮在笛卡尔坐标系下的姿态，ycom和zcom为所述虚拟模型的质心在笛卡尔坐标系下的位置，Φ为所述虚拟模型的工作空间的坐标向量。

8.一种机器人姿态控制装置，其特征在于，包括：虚拟模型构建模块，用于构建机器人的虚拟模型，所述虚拟模型包括所述机器人的飞轮倒立摆模型以及所述机器人的脚底面与水平面所成夹角；

机器人控制模块，用于基于所述虚拟模型，对所述机器人进行外环闭环反馈、内环抗外界位置扰动、内环抗外界速度扰动以及内环抗外界力扰动的姿态控制。

9.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至7中任一项所述的机器人姿态控制方法的步骤。

10.一种机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7中任一项所述的机器人姿态控制方法的步骤。

说明书

一种机器人姿态控制方法、装置、可读存储介质及机器人

技术领域

[0001] 本申请属于机器人技术领域，尤其涉及一种机器人姿态控制方法、装置、计算机可读存储介质及机器人。

背景技术

[0002] 现有技术在对双足仿人机器人进行姿态控制时，一般都需要首先对其进行建模。机器人的建模方法可以分为全动力学模型方法和降阶模型方法。全动力学模型对机器人所有的关节和连杆进行建模，规划各个关节的运动和出力。降阶模型方法有线性倒立摆模型、弹性倒立摆模型等等。通过建模将双足仿人机器人简化成为更为简单的虚拟模型，在简单的虚拟模型上进行运动控制，而真实的仿人机器人通过复制虚拟模型的状态实现运动。

[0003] 目前，支持全动力学模型的双足仿人机器人，其硬件要求高，往往需要力控关节，大扭矩的力控电机直接带来成本的上升和关节控制的难度，不易普及。而线性倒立摆模型的衍生模型往往假设倒立摆摆杆的支点固定于地面，仅仅能够反应机器人质心与支撑点，这使得通过简化模型进行机器人姿态控制的效果不好、鲁棒性较差。

发明内容

[0004] 有鉴于此，本申请实施例提供了一种机器人姿态控制方法、装置、计算机可读存储介质及机器人，以解决现有的通过简化模型进行机器人姿态控制的效果不好、鲁棒性较差的问题。

[0005] 本申请实施例的第一方面提供了一种机器人姿态控制方法，可以包括：

[0006] 构建机器人的虚拟模型，所述虚拟模型包括所述机器人的飞轮倒立摆模型以及所述机器人的脚底面与水平面所成夹角；

[0007] 基于所述虚拟模型，对所述机器人进行外环闭环反馈、内环抗外界位置扰动、内环抗外界速度扰动以及内环抗外界力扰动的姿态控制。

[0008] 进一步地，所述构建机器人的虚拟模型可以包括：

[0009] 构建所述虚拟模型的关节空间和工作空间；

[0010] 构建从所述机器人的关节空间到所述虚拟模型的关节空间的正运动学映射关系；

[0011] 构建从所述虚拟模型的关节空间到所述虚拟模型的工作空间的正运动学映射关系；

[0012] 构建从所述虚拟模型的工作空间到所述虚拟模型的关节空间的逆运动学映射关系；

[0013] 构建从所述虚拟模型的关节空间到所述机器人的关节空间的逆运动学映射关系。

[0014] 进一步地，所述对所述机器人进行外环闭环反馈、内环抗外界位置扰动、内环抗外界速度扰动以及内环抗外界力扰动的姿态控制可以包括：

[0015] 通过预设的闭环反馈控制器输出控制力；

[0016] 通过预设的导纳控制器将所述控制力转化为所述虚拟模型的工作空间的位姿控制量；

[0017] 通过预设的第二逆向映射模块将所述位姿控制量转化为所述虚拟模型的关节空间的第一关节控制量；

[0018] 通过预设的第一逆向映射模块将所述第一关节控制量转化为所述机器人的关节空间的第二关节控制量；

[0019] 根据所述第二关节控制量对所述机器人进行姿态控制。

[0020] 进一步地，所述对所述机器人进行外环闭环反馈、内环抗外界位置扰动、内环抗外界速度扰动以及内环抗外界力扰动的姿态控制还可以包括：

[0021] 通过预设的零空间模块进行抗外界位置扰动的内环反馈控制；

[0022] 通过预设的外力估计模块进行对外力的估计并抵消外界力扰动；

[0023] 通过预设的惯性测量传感器测量所述机器人腰部姿态的变化量；

[0024] 通过预设的第一正向映射模块将所述机器人腰部姿态的变化量转化为所述虚拟模型的关节空间的第一关节变化量；

[0025] 通过预设的第二正向映射模块将所述第一关节变化量转化为所述虚拟模型的工作空间的第二关节变化量。

[0026] 进一步地，所述通过预设的零空间模块进行抗外界位置扰动的内环反馈控制可以包括：

[0027] 将所述第一关节变化量进行解耦，得到水平面夹角变化量和关节角变化量；

[0028] 使用预设的雅克比矩阵计算所述关节角变化量的零空间的基向量；

[0029] 根据所述水平面夹角变化量、所述关节角变化量和所述零空间的基向量计算所述虚拟模型的关节空间的关节控制偏移量。

[0030] 本申请实施例的第二方面提供了一种机器人姿态控制装置，可以包括实现上述任一种机器人姿态控制方法的步骤的功能模块。

[0031] 本申请实施例的第三方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述任一种机器人姿态控制方法的步骤。

[0032] 本申请实施例的第四方面提供了一种机器人，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述任一种机器人姿态控制方法的步骤。

[0033] 本申请实施例的第五方面提供了一种计算机程序产品，当计算机程序产品在机器人上运行时，使得机器人执行上述任一种机器人姿态控制方法的步骤。

[0034] 本申请实施例与现有技术相比存在的有益效果是：本申请实施例构建机器人的虚拟模型，所述虚拟模型包括所述机器人的飞轮倒立摆模型以及所述机器人的脚底面与水平面所成夹角；基于所述虚拟模型，对所述机器人进行外环闭环反馈、内环抗外界位置扰动、内环抗外界速度扰动以及内环抗外界力扰动的姿态控制。通过本申请实施例，提出了一种全新的虚拟模型，在机器人的飞轮倒立摆模型的基础上又增加了对于机器人脚底面与水平面夹角的考虑，从模型的角度看，该虚拟模型能够充分反应出机器人上身姿态、质心以及脚板姿态等这些对于机器人的平衡以及姿态控制极为关键的要素。基于这一虚拟模型，通过对所述机器人进行外环闭环反馈、内环抗外界位置扰动、内环抗外界速度扰动以及内环抗外界力扰动等方式，实现了完整的机器人质心和姿态的约束，完成了一个能够保持机器人平衡状态和姿态状态的控制系统框架，具有更好的控制效果和更强的鲁棒性。

附图说明

[0035] 为了更清楚地说明本申请实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

[0036] 图1为虚拟模型的示意图；

[0037] 图2为构建机器人的虚拟模型的示意流程图；

[0038] 图3为机器人的关节空间与虚拟模型的关节空间的示意图；

[0039] 图4为虚拟模型的关节角的确定过程示意图；

[0040] 图5为虚拟模型的关节空间到机器人的关节空间的计算过程示意图；

[0041] 图6为脚支撑的偏摆方向约束示例的示意图；

[0042] 图7为零空间的内环抗扰动控制示意图；

[0043] 图8为机器人抗外力与位置扰动控制的系统框图；

[0044] 图9为坐标系的含义与变换关系的示意图；

[0045] 图10为本申请实施例中一种机器人姿态控制装置的一个实施例结构图；

[0046] 图11为本申请实施例中一种机器人的示意框图。

具体实施方式

[0047] 为使得本申请的发明目的、特征、优点能够更加的明显和易懂，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，下面所描述的实施例仅仅是本申请一部分实施例，而非全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本申请保护的范围。

[0048] 应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。

[0049] 还应当理解，在此本申请说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本申请。如在本申请说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。

[0050] 还应当进一步理解，在本申请说明书和所附权利要求书中使用的术语“和/或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。

[0051] 如在本说明书和所附权利要求书中所使用的那样，术语“如果”可以依据上下文被解释为“当...时”或“一旦”或“响应于确定”或“响应于检测到”。类似地，短语“如果确定”或“如果检测到[所描述条件或事件]”可以依据上下文被解释为意指“一旦确定”或“响应于确定”或“一旦检测到[所描述条件或事件]”或“响应于检测到[所描述条件或事件]”。

[0052] 另外，在本申请的描述中，术语“第一”、“第二”、“第三”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

[0053] 当在户外环境中，机器人需要在高低起伏的路面稳定行走并保持上身姿态与平衡。如果需要机器人在移动的平台中进行服务，例如汽车、轮船和飞机等正在行进的交通工具中，机器人则更需要在颠簸的浮动平面上保持上身姿态与平衡。因此，双足仿人机器人的平衡与姿态的控制问题是机器人在各种人类建立的复杂环境中服务所需要解决的首要技术问题。一个能够完成全向和全地形行走的双足仿人机器人需要至少12个自由度。在如上所述的环境中，完成高自由度和非线性机器人的运动规划和控制并保持机器人姿态和平衡是技术上的极大挑战。从控制的角度，机器人的平衡和姿态保持可以理解为机器人的质心和姿态始终维持在规划的状态（位置、速度和加速度），而如果机器人的质心和姿态达到了一个非期望的状态，则机器人有可能回失稳，且机器人的姿态并非期望姿态。从时域的角度，机器人维持平衡和姿态的状态可以分为静态的维持和动态的维持。静态的维持一般指机器人与环境间的相对速度变化非常低的情况下，机器人可以保持平衡，例如机器人的质心对地面的投影始终在脚所构成的支撑多边形内，而机器人的姿态保持，多为位置上的顺从，例如通过视觉传感器估计前方地形起伏，并令机器人脚面提前做好对地形的适应。而当机器人与环境的相对速度很高的情况下，则需要进行动态的保持。可以采用的方法包含考虑足底支反力、零力矩点和机器人质心加速度之间的关系，通过角动量和线动量来控制机器人的平衡，而机器人动态的姿态维持则可以依靠反馈控制来实现。

[0054] 在本申请实施例中，基于降阶模型方法这一技术路线，提出基于机器人脚底面与水平面夹角加上由机器人自身所简化的飞轮倒立摆模型所构成的虚拟模型，该模型既能反应机器人上身姿态，也能反映机器人足部姿态。并根据这个虚拟模型，通过惯性测量传感器（Inertial Measurement Unit，IMU）对机器人脚底面与水平面夹角进行位置估计，实现静态平衡与姿态保持。其次，通过基于零空间的内环速度抗扰动控制，以实现动态平衡与位置保持。再次，通过对加速度的估计，实现完整的机器人质心和姿态的约束。最后通过位置闭环，提升系统稳定性，完成一个能够保持机器人平衡状态和姿态状态的控制系统框架。

[0055] 为了能够同时对机器人的质心和机器人的姿态进行控制，可以首先构建所述机器人的虚拟模型，使该模型可以反映这两个被控对象的状态。在本申请实施例中，优选建立如图1所示的虚拟模型，由图中可以看出，该虚拟模型由机器人脚底面与水平面所成夹角加上由机器人自身所简化的飞轮倒立摆模型所构成。

[0056] 所述虚拟模型的构建过程具体可以包括如图2所示的步骤：

[0057] 步骤S201、构建所述虚拟模型的关节空间。

[0058] 具体地，可以根据下式构建所述虚拟模型的关节空间的坐标向量：

[0059] Θ=[θ1, θ2, θ3,γ]T（1）

[0060] 其中，θ1为所述虚拟模型的摆杆与脚面所成夹角，θ2为所述虚拟模型的摆杆长度，θ3为所述虚拟模型的飞轮与摆杆所成夹角，γ为所述虚拟模型的脚底面与水平面所成夹角，θ1、θ3和γ是旋转自由度，θ2是直线自由度，T为转置符号，Θ为所述虚拟模型的关节空间的坐标向量。

[0061] 步骤S202、构建所述虚拟模型的工作空间。

[0062] 构建所述虚拟模型的目的在于质心状态和姿态状态这两个控制目标，因此可以定义所述虚拟模型的工作空间主要围绕这两个控制目标进行，具体地，可以根据下式构建所述虚拟模型的工作空间的坐标向量：

[0063] Φ=[φ1 ,φ2 , ycom , zcom]T（2）

[0064] 其中，φ1为所述虚拟模型的摆杆与笛卡尔坐标系z轴所成夹角，φ2为所述虚拟模型的飞轮在笛卡尔坐标系下的姿态，ycom和zcom为所述虚拟模型的质心在笛卡尔坐标系下的位置，Φ为所述虚拟模型的工作空间的坐标向量。

[0065] 值得注意的是，如果需要对姿态的状态进行控制，则可以直接利用φ2的信息。如果需要控制质心的状态，则既可以直接利用ycom和zcom，也可以通过控制φ1和θ2的状态来控制质心，因为存在如式（3a）和式（3b）所示的关系：

[0066] zcom =θ2sin(φ1)      （3a）

[0067] ycom =θ2cos(φ1)     （3b）

[0068] 在质量属性方面，可以将所述机器人各连杆的质量集总在一起成为飞轮质量，如式（4）所示：

[0069] （4）

[0070] 其中，M为所述飞轮质量，n为所述机器人的关节数量，mi为所述机器人的关节i（1≤i≤n）的质量。

[0071] 简化飞轮倒立摆的质心位置可以根据合质心公式计算得到，以y轴为例，如式（5）所示：

[0072]             (5)

[0073] 其中，yi为所述机器人的关节i的质心坐标。

[0074] 简化飞轮倒立摆的惯量可以通过平行轴定理，将各个机器人关节的惯量平移到质心坐标系下再相加得到：

[0075] （6）

[0076] 其中，Ici是将关节i的惯性张量矩阵平移到合质心后的惯性张量矩阵，其中惯性矩的平移方式，以y轴为例，如式（7）所示：

[0077] Iciyy = Igiyy +mi (xi2+ zi2)    （7）

[0078] 其中，Igi是所述机器人的关节i以质心为中心，与笛卡尔坐标系各轴对齐的惯性张量，上标yy代表y轴惯性矩，xi为所述机器人的关节i质心到合质心的x方向距离，zi为所述机器人的关节i质心到合质心的z方向距离。

[0079] 惯性积的平移方式，以xy轴为例，如式（8）所示：

[0080] Icixy = Igixy +mixiyi（8）

[0081] 其中，上标xy代表x轴与y轴的惯性积。yi代表所述机器人的关节i质心到合质心的y方向距离。

[0082] 步骤S203、构建从所述机器人的关节空间到所述虚拟模型的关节空间的正运动学映射关系。

[0083] 实现真实12自由度双足机器人的关节空间到虚拟模型的关节空间的简化，需要正运动学映射算法，如图3所示。以一般可以进行全向行走的双足仿人机器人为例，假定其具有12个自由度，其中每条腿髋关节为RZ、RX、RY三个自由度，膝关节为RY一个自由度，踝关节为RY、RX两个自由度，则此机器人的关节空间的坐标向量可表示为：

[0084] （9）

[0085] 其中，Q为所述机器人的关节空间的坐标向量，q1、q2、…、q12分别为Q在12个自由度上的分量。

[0086] 对于笛卡尔坐标系下的点，应用齐次变换矩阵描述其位姿，如式（10）所示：

[0087] （10）

[0088] 其中，P代表空间中某点的位姿，R∈R3×3代表这一点的姿态，p∈R3×1代表这一点的位置。假设飞轮与摆杆所成夹角θ3仅与给定机器人腰部的姿态有关，则根据RPY所规定的旋转顺序，则可以把θ3的旋转矩阵写为：

[0089] （11）

[0090] 其中，下标wheel代表飞轮的姿态，下标waist代表腰部的给定姿态，下标roll代表横滚方向，下标pitch代表俯仰方向。根据式（11），可以根据Rwheel的第2行第2个元素（rwaist2,2）和第2行第3个元素（rwaist2,3）确定θ3,roll的值，根据Rwheel的第1行第1个元素（rwaist1,1）和第3行第1个元素（rwaist3,1）确定θ3,pitch的值，如下式所示：

[0091] θ3,roll =atan2(-rwaist2,3, rwaist2,2)   （12a）

[0092] θ3,pitch =atan2(-rwaist3,1, rwaist1,1)    （12b）

[0093] 需要注意的是，这里仅考虑横滚和俯仰方向，如果给定腰部姿态存在偏摆方向，则需要将Rwaist前乘偏摆方向变换矩阵（Rwaistyaw）的转置，如下式所示：

[0094] （13）

[0095] 利用IMU传感器测量的姿态Rimu和位置pimu，可以从腰部到脚底板的正向运动学算法来计算虚拟模型的关节空间γ、θ1和θ2。

[0096] 首先，通过运动链分别计算左右脚的位姿PL和PR，以左腿为例，如下式所示：

[0097] （14）

[0098] 其中，Ri,L代表关节i的旋转矩阵，pi,L代表关节i的空间位置。

[0099] 图4所示为虚拟模型的关节角γ的确定过程示意图，具体地，可以根据左右脚与地面的支撑状态来确定γroll和γpitch：

[0100] （15a）

[0101] （15b）

[0102] 其中，α为0到1之间的实数，与规划有关。俯仰方向与横滚方向与之类似。

[0103] 然后，可以根据式（14）中的位置p来计算θ2，此时同样需要用到α且判断过程与图4所示类似，其公式为：

[0104] （16）

[0105] 最后，通过几个旋转矩阵确定θ1，有：

[0106] （17）

[0107] 再通过与公式（12）类似的方式求解θ1。

[0108] 步骤S204、构建从所述虚拟模型的关节空间到所述虚拟模型的工作空间的正运动学映射关系。

[0109] 虚拟模型关节空间到虚拟模型工作空间的正运动学算法同样需要通过旋转矩阵来实现。首先ϕ1的旋转矩阵可以写为：

[0110] （18）

[0111] 类似的，可以求取ϕ2的旋转矩阵，如：

[0112] （19）

[0113] 然后，再采用与式（12a）和式（12b）类似的方式，通过atan2()函数来求得各自角度。而质心位置，则可以根据式（3a）和式（3b）求得。

[0114] 步骤S205、构建从所述虚拟模型的工作空间到所述虚拟模型的关节空间的逆运动学映射关系。

[0115] 当在虚拟模型的工作空间进行质心和姿态的状态控制后，还需要将控制量返回到虚拟模型的关节空间，再进一步映射到真实机器人的关节空间中去。虚拟模型的工作空间到虚拟模型的关节空间的逆运动学算法是对步骤S204中各式的求逆。

[0116] θ3旋转矩阵可写为：

[0117] （20）

[0118] 虚拟模型关节角θ1旋转矩阵可写为：

[0119] （21）

[0120] 虚拟模型关节角θ2为支点到质心的距离，γ一般不可控。

[0121] 步骤S206、构建从所述虚拟模型的关节空间到所述机器人的关节空间的逆运动学映射关系。

[0122] 如图5所示，虚拟模型的关节空间到真实12自由度双足机器人的关节空间的逆运动学映射算法是对步骤S203的求逆。

[0123] 首先，可以根据θ1和θ3的旋转矩阵确定腰和脚的相对姿态，如：

[0124] （22a）

[0125] （22b）

[0126] 再通过θ2和α来确定腿长，并最终获得真实机器人的工作空间参数，再带入逆运动学的解析解中求得12个关节角。需要注意的是，从低维空间映射到高维空间，需要在几何上进行约束。例如，双脚支撑的偏摆方向约束如图6所示。

[0127] 在构建完成所述虚拟模型之后，则可以基于所述虚拟模型，对所述机器人进行姿态控制。

[0128] 当机器人脚面与其所立的平面（地面、斜坡）平行时，可以认为虚拟模型的关节角γ即为其所立平面的倾斜角度，可以通过式（15a）和式（15b）并基于IMU传感器对机器人所立平面的倾斜角度的估计。

[0129] 得到地面的位置有助于完成倾斜平面上机器人的静态平衡，此处也可以基于IMU传感器对机器人所立平面的相对运动速度进行估计。以上计算得到的平面倾斜角度同样为RPY旋转顺序的角度，在分别得到俯仰角和偏摆角后，可以通过微分的形式获得偏摆角速度和俯仰角速度，如：

[0130] （22c）

[0131] （22d）

[0132] 其中，s代表傅里叶变换后的s域传递函数，c代表送一个接近于0的非负实数，需要注意的是，在本申请实施例中，在变量上方加一点为其一阶微分，在变量上方加两点为其二阶微分。对速度的估计可以辅助控制进行动态平衡与姿态状态控制。

[0133] 以上所描述的差分方法，带有RPY的旋转顺序，其坐标变换如前面所描述的，均为矩阵相乘的形式，因此具有耦合性，不利有后续零空间的计算以及后续基于零空间的内环抗扰动控制策略的应用。在本申请实施例中可以将其解耦，使速度变为可以直接线性相加的方式而非矩阵相乘的方式，将角速度从先绕x轴转动、再绕y轴转动，最后绕z轴转动的顺序，变成绕固定的绝对世界（笛卡尔）坐标系，x轴、y轴和z轴各自转动，且相互独立，互不影响。在获得RPY的角速度之后，可以通过一个变换矩阵将其解耦。

[0134] （23）

[0135] 其中， 代表刚体绕固定世界坐标轴旋转的速度，下标代表坐标轴。

[0136] 解耦的另一种方法是通过雅可比矩阵。在仅考虑姿态时，将IMU测量的俯仰和偏摆作为额外两个自由度再加上各个实际关节角所构成的几何雅可比矩阵（以下所提到的雅可比矩阵均为几何雅可比矩阵）记为 ，则有

[0137] （24）

[0138] 通过以上方法，空间内的立体模型可以投影到xz平面和yz平面，形成两个相互独立的平面虚拟模型，方便后续方法的实施。

[0139] 为了完成控制目标的全状态控制，除了位置和速度的信息，还需要完成加速度的估计。基于IMU传感器的对于机器人所立平面的相对运动加速度的估计可以在RPY速度的基础上通过差分的形式获得，再根据式（25）进行解耦：

[0140] （25）

[0141] 或者直接通过雅可比矩阵来计算：

[0142] （26）

[0143] 机器人的运动学方程可以写为：

[0144] （27）

[0145] 其中X代表机器人的r维工作空间，q代表机器人的n维关节空间。当r

[0146] （28）

[0147] 根据零空间定义，假设雅可比矩阵J存在零空间，令：

[0148] （29）

[0149] 则有：

[0150] （30）

[0151] 由此可见，只要在关节空间上，每个关节叠加的速度满足零空间基向量的要求，则其不对工作空间造成影响。此处可以通过对雅可比矩阵J使用奇异值分解(Singular Value Decomposition，SVD)的方法获得零空间的基向量，再通过式（31）找到满足式（28）的速度偏移量：

[0152] （31）

[0153] 其中，代表零空间的基向量。

[0154] 在本申请实施例中，可以利用虚拟模型关节角γ和θ1对工作空间ϕ1的冗余，通过调整θ1的速度使其与γ的速度满足零空间定义，实现ϕ1的速度不受γ速度变化的影响，也即质心的状态不受γ速度变化的影响。通过这种方式，完成零空间的一种对质心速度的内环抗扰动控制策略。

[0155] 如图7所示，将所述虚拟模型的关节空间的关节变化量Θγ（记为第一关节变化量）进行解耦，得到水平面夹角变化量γ和关节角变化量Θ，然后使用预设的雅克比矩阵计算所述关节角变化量的零空间的基向量 ，最后根据所述水平面夹角变化量、所述关节角变化量和所述零空间的基向量按照式（31）计算所述虚拟模型的关节空间的关节控制偏移量 。

[0156] 进一步地，本申请实施例中还可以利用虚拟模型工作空间ϕ1对工作空间ϕ2的冗余，或者说是关节空间γ、θ1和θ3对工作空间ϕ2的冗余，通过调整θ3的速度使其与γ和θ1的速度满足零空间定义，实现ϕ2的速度不受γ和θ1速度变化的影响，也即质心的状态不受γ速度变化的影响。通过这种方式，完成零空间的一种对姿态速度的内环抗扰动控制策略。

[0157] 机器人通过脚板对地面施加的力和地面对机器人脚板施加的力互为相反力，通过质心加速度的变化，可以对机器人所受到的外力进行估计，根据牛顿定律，物体运动加速度的产生满足下式：

[0158] （32）

[0159] 其中，下标c代表未知的外力，下标d代表已知的规划力。

[0160] 已知的规划力产生的规划力矩满足下式：

[0161] （33）

[0162] 通过估计出的加速度信息am，可以进一步估计外力Fc：

[0163] （34）

[0164] 在机器人学中，从关节空间到工作空间力的映射可以通过雅可比矩阵实现，如下式所示：

[0165] （35）

[0166] 因此，令机器人关节比规划力矩τd少出τc的扭矩，即可抵消外力的Fc影响。力控机器人可以直接将关节转矩作为指令发送给机器人关节，位置控制的关节则须利用导纳控制器，先将力指令转化成位置指令，再发送给位置控制的关节执行。

[0167] 根据以上算法，形成各自模块并组合后形成的基于虚拟模型对所述机器人进行外环闭环反馈、内环抗外界位置扰动、内环抗外界速度扰动以及内环抗外界力扰动的姿态控制的机器人平衡与姿态控制框架如图8所示。其中力矩τe为作用于机器人的外力，γ为机器人受外力所用而引起的位置变化，外力τe及其引起的位置变化γ往往难以被直接测量，因此需要采用估计的方式获得其变化，图8中Q、Θ、和Φ之间的关系如图9所示。

[0168] 如图8所示，在虚拟模型工作空间Φ中做闭环反馈控制，通过设计反馈控制器实现的控制目标如式（36）所示：

[0169] （36）

[0170] 即对于外界任何速度扰动的影响，机器人的实际质心状态于姿态状态始终保持于其相应规划状态一致。其中，符号 代表规划量，符号 代表控制指令。

[0171] 如图8所示，在对机器人的控制过程中，通过预设的闭环反馈控制器（Ctrl）输出控制力 ；通过预设的导纳控制器（Adm）将所述控制力转化为所述虚拟模型的工作空间的位姿控制量 ；通过预设的第二逆向映射模块（IK2）将所述位姿控制量转化为所述虚拟模型的关节空间的第一关节控制量 ，其中，IK2实现的即为前文所描述的虚拟模型的工作空间到虚拟模型的关节空间的逆运动学算法；通过预设的第一逆向映射模块（IK1）将所述第一关节控制量转化为所述机器人的关节空间的第二关节控制量 ，其中，IK1实现的即为前文所描述的虚拟模型的关节空间到机器人的关节空间的逆运动学算法；根据所述第二关节控制量对所述机器人（Robot）进行姿态控制。

[0172] 在这一过程中，还可以通过预设的零空间模块（Null Space）进行抗外界位置扰动的内环反馈控制，也即实现如图7所示的过程；通过预设的外力估计模块（External Force Estimator）进行对外力的估计并抵消外界力扰动；通过预设的惯性测量传感器（IMU）测量所述机器人腰部姿态的变化量；通过预设的第一正向映射模块（FK1）将所述机器人腰部姿态的变化量转化为所述虚拟模型的关节空间的第一关节变化量，其中，FK1实现的即为前文所描述的真实12自由度双足机器人的关节空间到虚拟模型的关节空间的逆运动学算法；通过预设的第二正向映射模块（FK2）将所述第一关节变化量转化为所述虚拟模型的工作空间的第二关节变化量，其中，FK2实现的即为前文所描述的虚拟模型的关节空间到虚拟模型的工作空间的正运动学算法。IK2模块和所述外力估计模块的使用需要对脚底板对空间的姿态进行估计，分离模块（Selector）仅用于将γp信号从向量Θγ中分离出来。

[0173] 通过这样的方式，对机器人所受外界位置状态和外界力的扰动估计，并且利用零空间以及导纳等相关的内环抗扰动策略，既通过内环控制降低了机器人质心和位置状态对外界的灵敏度，也能够提升外环位置反馈闭环控制系统的稳定行，并且提高机器人针对外界扰动抵消动作的响应。

[0174] 综上所述，本申请实施例构建机器人的虚拟模型，所述虚拟模型包括所述机器人的飞轮倒立摆模型以及所述机器人的脚底面与水平面所成夹角；基于所述虚拟模型，对所述机器人进行外环闭环反馈、内环抗外界位置扰动、内环抗外界速度扰动以及内环抗外界力扰动的姿态控制。通过本申请实施例，提出了一种全新的虚拟模型，在机器人的飞轮倒立摆模型的基础上又增加了对于机器人脚底面与水平面夹角的考虑，从模型的角度看，该虚拟模型能够充分反应出机器人上身姿态、质心以及脚板姿态等这些对于机器人的平衡以及姿态控制极为关键的要素。基于这一虚拟模型，通过对所述机器人进行外环闭环反馈、内环抗外界位置扰动、内环抗外界速度扰动以及内环抗外界力扰动等方式，实现了完整的机器人质心和姿态的约束，完成了一个能够保持机器人平衡状态和姿态状态的控制系统框架，具有更好的控制效果和更强的鲁棒性。

[0175] 应理解，上述实施例中各步骤的序号的大小并不意味着执行顺序的先后，各过程的执行顺序应以其功能和内在逻辑确定，而不应对本申请实施例的实施过程构成任何限定。

[0176] 对应于上文实施例所述的一种机器人姿态控制方法，图10示出了本申请实施例提供的一种机器人姿态控制装置的一个实施例结构图。

[0177] 本实施例中，一种机器人姿态控制装置可以包括：

[0178] 虚拟模型构建模块1001，用于构建机器人的虚拟模型，所述虚拟模型包括所述机器人的飞轮倒立摆模型以及所述机器人的脚底面与水平面所成夹角；

[0179] 机器人控制模块1002，用于基于所述虚拟模型，对所述机器人进行外环闭环反馈、内环抗外界位置扰动、内环抗外界速度扰动以及内环抗外界力扰动的姿态控制。

[0180] 进一步地，所述虚拟模型构建模块可以包括：

[0181] 空间构建子模块，用于构建所述虚拟模型的关节空间和工作空间；

[0182] 第一映射关系构建子模块，用于构建从所述机器人的关节空间到所述虚拟模型的关节空间的正运动学映射关系；

[0183] 第二映射关系构建子模块，用于构建从所述虚拟模型的关节空间到所述虚拟模型的工作空间的正运动学映射关系；

[0184] 第三映射关系构建子模块，用于构建从所述虚拟模型的工作空间到所述虚拟模型的关节空间的逆运动学映射关系；

[0185] 第四映射关系构建子模块，用于构建从所述虚拟模型的关节空间到所述机器人的关节空间的逆运动学映射关系。

[0186] 进一步地，所述机器人控制模块可以包括：

[0187] 控制力输出子模块，用于通过预设的闭环反馈控制器输出控制力；

[0188] 第一转化子模块，用于通过预设的导纳控制器将所述控制力转化为所述虚拟模型的工作空间的位姿控制量；

[0189] 第二转化子模块，用于通过预设的第二逆向映射模块将所述位姿控制量转化为所述虚拟模型的关节空间的第一关节控制量；

[0190] 第三转化子模块，用于通过预设的第一逆向映射模块将所述第一关节控制量转化为所述机器人的关节空间的第二关节控制量；

[0191] 姿态控制子模块，用于根据所述第二关节控制量对所述机器人进行姿态控制。

[0192] 进一步地，所述机器人控制模块还可以包括：

[0193] 内环反馈控制子模块，用于通过预设的零空间模块进行抗外界位置扰动的内环反馈控制；

[0194] 外界力抵消子模块，用于通过预设的外力估计模块进行对外力的估计并抵消外界力扰动；

[0195] 姿态测量子模块，用于通过预设的惯性测量传感器测量所述机器人腰部姿态的变化量；

[0196] 第四转化子模块，用于通过预设的第一正向映射模块将所述机器人腰部姿态的变化量转化为所述虚拟模型的关节空间的第一关节变化量；

[0197] 第五转化子模块，用于通过预设的第二正向映射模块将所述第一关节变化量转化为所述虚拟模型的工作空间的第二关节变化量。

[0198] 进一步地，所述内环反馈控制子模块可以包括：

[0199] 解耦单元，用于将所述第一关节变化量进行解耦，得到水平面夹角变化量和关节角变化量；

[0200] 基向量计算单元，用于使用预设的雅克比矩阵计算所述关节角变化量的零空间的基向量；

[0201] 关节控制偏移量计算单元，用于根据所述水平面夹角变化量、所述关节角变化量和所述零空间的基向量计算所述虚拟模型的关节空间的关节控制偏移量。

[0202] 所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的装置，模块和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

[0203] 在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

[0204] 图11示出了本申请实施例提供的一种机器人的示意框图，为了便于说明，仅示出了与本申请实施例相关的部分。

[0205] 如图11所示，该实施例的机器人11包括：处理器110、存储器111、存储在所述存储器111中并可在所述处理器110上运行的计算机程序112、传感器113以及关节执行器114。所述处理器110执行所述计算机程序112时实现上述各个机器人姿态控制方法实施例中的步骤。或者，所述处理器110执行所述计算机程序112时实现上述各装置实施例中各模块/单元的功能。所述传感器113包括前述方法实施例中提及的IMU传感器，用于测量所述机器人11腰部姿态的变化量。所述关节执行器114用于根据前述方法实施例中提及的第二关节控制量对所述机器人11进行姿态控制。

[0206] 示例性的，所述计算机程序112可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器111中，并由所述处理器110执行，以完成本申请。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序112在所述机器人11中的执行过程。

[0207] 本领域技术人员可以理解，图11仅仅是机器人11的示例，并不构成对机器人11的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述机器人11还可以包括输入输出设备、网络接入设备、总线等。

[0208] 所述处理器110可以是中央处理单元(Central Processing Unit，CPU)，还可以是其它通用处理器、数字信号处理器  (Digital Signal Processor，DSP)、专用集成电路 (Application Specific Integrated Circuit，ASIC)、现场可编程门阵列(Field-Programmable Gate Array，FPGA) 或者其它可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

[0209] 所述存储器111可以是所述机器人11的内部存储单元，例如机器人11的硬盘或内存。所述存储器111也可以是所述机器人11的外部存储设备，例如所述机器人11上配备的插接式硬盘，智能存储卡（Smart Media Card, SMC），安全数字（Secure Digital, SD）卡，闪存卡（Flash Card）等。进一步地，所述存储器111还可以既包括所述机器人11的内部存储单元也包括外部存储设备。所述存储器111用于存储所述计算机程序以及所述机器人11所需的其它程序和数据。所述存储器111还可以用于暂时地存储已经输出或者将要输出的数据。

[0210] 所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

[0211] 在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

[0212] 本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。

[0213] 在本申请所提供的实施例中，应该理解到，所揭露的装置/机器人和方法，可以通过其它的方式实现。例如，以上所描述的装置/机器人实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。

[0214] 所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

[0215] 另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

[0216] 所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读存储介质中。基于这样的理解，本申请实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

[0217] 以上所述实施例仅用以说明本申请的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围，均应包含在本申请的保护范围之内。