CN113305830B - 一种基于人体姿态控制的仿人机器人动作系统及控制方法 - Google Patents

Abstract

本发明公开了一种基于人体姿态控制的仿人机器人动作系统及控制方法；系统，包括操作者、上位机、机器人本体；控制方法，步骤如下：对机器人本体各个舵机进行逐一标号，记录机器人姿态信息，如

以及

操作者打开各个陀螺仪蓝牙，逐一连接至上位机，操作者做出姿态变化，各陀螺仪进行空间定位。上位机记录此时的

和

将人体姿态信息进行整合，求出人体姿态与机器人姿态之间的转换矩阵，并与动作组数据库中的标准转换矩阵依次进行相关系数计算；搜索出最优相关系数，并读取动作组数据库中对应存储的舵机信息，将存储的舵机信息编译成控制指令；仿人机器人接收动作控制指令，执行相应的关节运动，各舵机达到预定角度，完成仿人机器人动作控制。

Description

一种基于人体姿态控制的仿人机器人动作系统及控制方法

技术领域

本发明涉及一种基于人体姿态控制的仿人机器人动作系统及控制方法，更确切地说，本 发明涉及一种利用九轴MEMS陀螺仪记录人体姿态信息实现仿人机器人动作控制的系统及 方法。

背景技术

机器人技术在近些年得到了迅速发展，已对世界各个国家的经济、文化、军事等各个方 面产生了重要影响，仿人机器人是时下机器人研究领域的一个重要的研究热点，仿人机器人 是一个包含多个自由度的复杂系统，由于其本体的自由度很高，使得开发人员对其进行编程 控制具有一定的难度，其性能好坏主要取决于开发人员的编程技能、用户的操作技能、以及 广泛经验的可用性等因素。

仿人机器人的动作控制是仿人机器人研究中的重要领域之一。仿人机器人动作控制的应 用领域有很多，如在商业领域作为接待员或形象代言人，在军事领域可以代替士兵执行任务， 以及在科技教育领域中参与如机器人示教、机器人避障以及足球比赛等活动。

目前仿人机器人的动作控制最主要的方法分为以下两类：

一、直接调节机器人各个部位的舵机角度使其动作发生改变。但这种方法存在一定的 缺点，如：不具备很好的实时性，多个舵机需要一一进行角度编程，效率低下。

二、使用Kinect体感传感器对人体骨骼节点进行捕捉，将人体的骨骼信息转换为机器 人舵机角度控制指令，使机器人动作发生变化。但这种方法也具备一定的缺点：如前期准备 工作过于麻烦，需要准备摄像头进行人体动作捕捉，对环境与设备有一定要求。

本发明将九轴MEMS陀螺仪固定在人体各个关节，利用陀螺仪空间定位技术，能够准确 的描绘人体各个部位的空间运动轨迹并转换为机器人舵机控制指令，本发明不仅实现了基于 人体姿态控制的机器人动作控制，又有效的控制了成本。综上所述，本发明所述的一种基于 人体姿态控制的仿人机器人动作系统及控制方法具有一定的合理性与优越性。

发明内容

本发明所要解决的技术问题是克服了现有的机器人动作控制技术方面的不足，提供了一 种基于人体姿态控制的仿人机器人动作系统及控制方法，利用该方法使得机器人的动作控制 变得更加简单，可以让机器人对操作者的身体动作进行跟随，此方法对操作者的要求不高， 只需佩戴搭配的陀螺仪就可以完成对机器人的动作编程，并且可以对机器人进行流畅的动作 控制。

一种基于人体姿态控制的仿人机器人动作系统，包括操作者、上位机、机器人本体；所 述上位机与机器人本体的数据传输直接使用蓝牙通讯模块进行数据传输。

进一步地，所述的操作者身体上固定陀螺仪，陀螺仪位置包括操作者的额头处，操作者 上半身的左右肩关节外侧、肘关节外侧以及腕关节外侧，操作者下半身的左右髋关节外侧、 膝关节外侧以及踝关节外侧；

操作者佩戴的陀螺仪个数与机器人本体的自由度相同。

机器人本体具备13个自由度。

进一步地，所述陀螺仪为九轴MEMS陀螺仪，所述九轴MEMS陀螺仪由一个三轴加速度传感器、一个三轴陀螺仪组成的感应芯片MPU6050，以及一个三轴磁传感器RMG144组 成；

陀螺仪空间坐标系，以重力方向为Z轴的反方向，以磁力方向为Y轴正方向，取正交与 这两个方向的唯一方向为X轴，并且以右手定理确定X轴的正方向。

进一步地，所述机器人本体内部模块包括：主控模块、蓝牙通讯模块、网卡模块、运动 控制模块、n自由度舵机模块和位置传感器模块；

所述主控模块包括电源模块、硬盘模块、CPU模块、内存模块；电源模块实现机器人稳 定的供电和充电功能；硬盘模块实现存储内部程序及数据；CPU模块实现运行控制程序功能； 内存模块实现临时存储数据功能；

所述蓝牙通讯模块与网卡模块实现与上位机的数据传输，所述位置传感器模块实现记录 机器人各关节空间位置，所述运动控制模块实现存储控制指令模块，并集成动作组指令数据 库；所述n自由度舵机模块实现机器人的动作执行。

一种基于人体姿态控制的仿人机器人动作系统的控制方法，包括如下步骤：

步骤1，首先对机器人本体各个舵机进行逐一标号，并记录下机器人本体平衡位置下的 质心位置

各个舵机的转动角度

以及各个关节的空间位置

步骤2，操作者打开各个陀螺仪蓝牙，逐一连接至上位机，操作者做出姿态变化，对各 陀螺仪进行空间定位。

步骤3，上位机记录此时的磁场指向向量

以及定位出来的终止位置的平面指向角

和俯仰角

以及空间位移向量

和终止动作位置的三轴角速度

步骤4，将人体姿态信息进行整合，求出人体姿态与机器人本体姿态之间的转换矩阵， 与动作组数据库中的转换矩阵依次进行相关系数计算。

步骤5，搜索出最优相关系数，并读取动作组数据库中对应存储的舵机信息，将存储的 舵机信息编译成控制指令。

步骤6，机器人本体接收控制指令，执行相应的关节运动。

步骤7，机器人各个舵机达到预定的角度，完成机器人本体的动作控制。

步骤1中的机器人本体平衡位置下的质心位置按如下公式计算：

其中M为仿人机器人质量，n为机器人的自由度数目，m_j为仿人机器人各个连杆的质 量，c_j表示以支撑平衡点为原点的坐标系下，从原点指向连杆的空间向量；向量c_j由机器人 本体关节角度经正向运动学计算得到。

进一步地，向量c_j的计算以机器人本体的左侧髋关节和左侧膝关节为例，假设这两个 关节的空间向量分别为k₁＝(x₁,y₁,z₁)、k₂＝(x₂,y₂,z₂)，c₁为左侧髋关节和左侧膝 关节间连杆的向量，则有:

步骤三，以佩戴在操作者身上某一关节的九轴MEMS陀螺仪为例，

ψ，θ，

计算过程如下：

三轴磁传感器固定在九轴MEMS陀螺仪中，地磁场为三轴磁传感器的参考方向，当九轴 MEMS陀螺仪发生空间变化时，即指向任何方向时，地磁场产生的磁场强度会分配在三轴磁 传感器的三轴上，其中：

H_x：地磁场强度向量在传感器X轴上的分量；

H_y：地磁场强度向量在传感器Y轴上的向量；

H_z：地磁场强度向量在传感器Z轴上的向量；

假设操作者所在的初始位置的磁场强度向量为M₁＝(H_x1,H_y1,H_z1),操作者所在的终止位 置的磁场强度向量M₂＝(H_x2,H_y2,H_z2)，则磁场强度指向向量

可表示为：

其中，X＝H_x1-H_x2,Y＝H_y1-H_y2,Z＝H_z1-H_z2；

通过终止位置的H_x2、H_y2、H_z2,可以计算出平面指向角ψ和俯仰角θ：

根据建立的空间坐标系，三轴加速度传感器读取的每个轴的加速度通过积分运算算出当 前速度值，并再根据积分运算算出每个轴的位移量，通过此方法得到初始位置空间坐标 (x₁,y₁,z₁)和终止位置(x₂,y₂,z₂)的空间坐标，得到空间位移向量

并记录终止位置的三轴陀螺仪采集到的三轴角速度

三轴角速度在九轴 MEMS陀螺仪中的三轴陀螺仪中可以实时读取，所以可以直接记录。

步骤4中所述求出人体姿态与机器人本体姿态之间的转换矩阵，包括：

建立人体姿态信息矩阵S，其中：

B_i(X,Y,Z)代表磁场强度指向向量，ψ_i代表平面指向角，θ_i代表俯仰角，d_i(x,y,z)代表空 间指向向量，w_i代表三轴角速度；

建立机器人本体姿态信息矩阵R，其中：

g_i(x,y,z)代表机器人质心信息，α_i代表舵机角度，l_i(x,y,z)代表仿人机器人关节坐 标信息。

机器人本体姿态与人体姿态的转换过程由如下公式表示：

即C_T＝RS^-1，其中，R和S均为非方阵，S^-1为S的广义逆矩阵，C_i与C_T的计算方法相同。

由此可得出机器人本体姿态与人体姿态的转换矩阵C_T，并与动作组数据库中的转换矩阵C_i依次进行相关系数计算；相关系数的计算公式如下：

其中，m和n分别代表转换矩阵C_T与C_i的行数和列数，

与

分别代表矩阵C_T与C_i的均值；相关系数r的值越接近1，代表此时的转换矩阵C_T与数据库转换矩阵C_i越具备相关性。

步骤5中所述搜索出最优相关系数的方法为：将转换矩阵C_T与数据库中存储的各个转换 矩阵C_i依次进行相关系数计算，并比较各相关矩阵r_i值，选择最大的相关系数r_max作为输出；

所述的动作组数据库的建立过程如下：

对机器人本体各个舵机进行标号，并对机器人本体进行动作调整，将机器人本体调整到 理想动作，操作者对此时的机器人本体动作进行复现，通过对记录下来的机器人本体姿态信 息和人体姿态信息进行计算，得到对应的转换矩阵，重复上述此步骤多次，记录下每一次的 转换矩阵，将得到的所有转换矩阵中相同位置的元素提取并作均值处理，处理得到后的矩阵 记为标准的转换矩阵。

将标准的转换矩阵和对应的机器人本体舵机信息(舵机1…n角度)存入数据库中，以此 类推，完成机器人本体动作组数据库的建立。

与现有技术相比本发明的有益效果是：

1.本发明所述的一种基于人体姿态控制的仿人机器人动作系统及控制方法可以用于教育 领域，可以有效提高示教效率，此方法记录数据的实时性高，利用人体姿态信息，可以保证 仿人机器人动作跟随过程的稳定性。

2.本发明所述的一种基于人体姿态控制的仿人机器人动作系统及控制方法的优势在于当 仿人机器人接收到控制指令后，动作执行模块(即舵机)按照指令中的角度执行即可，不需 要编程人员对每个舵机进行单独编程，机器人的执行结果是可预期的。

3.本发明所述的一种基于人体姿态控制的仿人机器人动作系统及控制方法使用陀螺仪空 间定位技术，可以实现人体姿态的准确定位，对环境和设备要求低，且节约成本，此方法将 各个动作组储存进数据库中，进行实时调用，能使机器人根据人的动作示范做出相同或相似 的动作。

4.本发明所述的一种基于人体姿态控制的仿人机器人动作系统及控制方法使人机交互更 加自然、简单，且生成的动作组序列更具有亲和力、自然性。此方法在实际应用中上手快， 可操作性强，为非接触式的人机交互技术提供了一种新的思路。

附图说明

下面结合附图对本发明作进一步的说明：

图1为本发明所述的一种基于人体姿态控制的仿人机器人动作系统及控制方法的结构原 理示意框图；

图2为本发明所述的一种基于人体姿态控制的仿人机器人动作系统及控制方法的陀螺仪 人体佩戴方法；

图3为本发明所述的一种基于人体姿态控制的仿人机器人动作系统及控制方法所使用的 陀螺仪示意图与空间坐标系示意图；

图4为本发明所述的一种基于人体姿态控制的仿人机器人动作系统及控制方法的机器人 内部模块示意图；

图5为本发明所述的一种基于人体姿态控制的仿人机器人动作系统及控制方法的流程框 图；

图6为本发明所述的一种基于人体姿态控制的仿人机器人动作系统及控制方法的机器人 动作组指令数据库示意图；

图中：1.佩戴九轴MEMS陀螺仪的操作员，2.一号蓝牙模块，3.计算机，4.二号蓝牙模块， 5.仿人机器人，6.九轴MEMS陀螺仪，7.绑带。

具体实施方式

下面结合附图对本发明作详细的描述：

参阅图1，本发明所述的一种基于人体姿态控制的仿人机器人动作系统包括操作员、上 位机、机器人本体。上位机与机器人的数据传输直接使用蓝牙通讯模块进行数据传输。

参阅图2，本发明所述的一种基于人体姿态控制的仿人机器人动作系统的固定于人体上 的陀螺仪位置包括人体的额头处，上半身的左右肩关节外侧，肘关节外侧以及腕关节外侧， 下半身的左右髋关节外侧，膝关节外侧以及踝关节外侧，人体佩戴的陀螺仪个数应与机器人 的自由度相同，图2以一个具备13自由度的仿人机器人为例，图示了陀螺仪人体佩戴方法。

参阅图3，本发明所述的一种基于人体姿态控制的仿人机器人动作系统的陀螺仪示意图 如图所示，所述的九轴MEMS陀螺仪由一个三轴加速度传感器与一个三轴陀螺仪组成的感应 芯片MPU6050，以及一个三轴磁传感器RMG144组成。陀螺仪空间坐标系示意图如图所示， 以重力方向为Z轴的反方向，以磁力方向为Y轴正方向，取正交与这两个方向的唯一方向为 X轴，并且以右手定理确定X轴的正方向。

参阅图4，本发明所述的一种基于人体姿态控制的仿人机器人动作系统及控制方法的机 器人内部模块包括以下部分：主控模块、蓝牙通讯模块、网卡模块、运动控制模块、n自由 度舵机模块，其中主控模块包括电源模块、显卡模块、硬盘模块、CPU模块、内存模块。电 源模块实现机器人稳定的供电和充电功能；硬盘模块实现存储内部程序及数据；CPU模块实 现运行控制程序功能；内存模块实现临时存储数据功能；蓝牙通讯模块与网卡模块实现与上 位机的数据传输，位置传感器模块实现记录机器人各关节空间位置，运动控制模块实现存储 控制指令模块，并集成动作组指令数据库，n自由度舵机模块实现机器人的动作执行。

参阅图5，本发明所述的一种基于人体姿态控制的仿人机器人动作系统的控制方法的流 程框图，包括如下步骤：

步骤1，首先对机器人本体各个舵机进行逐一标号，并记录下机器人本体平衡位置下的 质心位置

各个舵机的转动角度

以及各个关节的空间位置

步骤2，操作者打开各个陀螺仪蓝牙，逐一连接至上位机，操作者做出姿态变化，对各 陀螺仪进行空间定位。

步骤3，上位机记录此时的磁场指向向量

以及定位出来的终止位置的平面指向角

和俯仰角

以及空间位移向量

和终止动作位置的三轴角速度

步骤4，将人体姿态信息进行整合，求出人体姿态与机器人本体姿态之间的转换矩阵， 与动作组数据库中的转换矩阵依次进行相关系数计算。

步骤5，搜索出最优相关系数，并读取动作组数据库中对应存储的舵机信息，将存储的 舵机信息编译成控制指令。

步骤6，机器人本体接收控制指令，执行相应的关节运动。

步骤7，机器人各个舵机达到预定的角度，完成机器人本体的动作控制。

其中，步骤1中的仿人机器人平衡姿态下的质心位置可按如下公式计算：

其中M为仿人机器人质量，n为机器人的自由度数目，m_j为仿人机器人各个连杆的质 量，c_j表示以支撑平衡点为原点的坐标系下，从原点指向连杆的空间向量。向量c_j可由仿人 机器人关节角度经正向运动学计算得到。

向量c_j的计算以仿人机器人的左侧髋关节和左侧膝关节为例，假设这两个关节的空间 向量分别为k₁＝(x₁,y₁,z₁)、k₂＝(x₂,y₂,z₂)，c₁为左侧髋关节和左侧膝关节间连杆 的向量，则有:

步骤3中，三轴磁传感器固定在九轴MEMS陀螺仪中，地磁场为三轴磁传感器的参考方 向，当九轴MEMS陀螺仪发生空间变化时，即指向任何方向时，地磁场产生的作用会分配在 三轴磁传感器的三轴上，其中：

H_x：地磁场强度向量在传感器X轴上的分量；

H_y：地磁场强度向量在传感器Y轴上的向量；

H_z：地磁场强度向量在传感器Z轴上的向量；

假设操作者所在的初始位置的磁场强度向量为M₁＝(H_x1,H_y1,H_z1),操作者所在的终止位 置的磁场强度向量M₂＝(H_x2,H_y2,H_z2)，则磁场强度指向向量

可表示为：

其中，X＝H_x1-H_x2,Y＝H_y1-H_y2,Z＝H_z1-H_z2。

通过终止位置的H_x2、H_y2、H_z2,可以计算出平面指向角ψ和俯仰角θ：

根据建立的空间坐标系，三轴加速度传感器读取的每个轴的加速度通过积分运算算出当 前速度值，并再根据积分运算算出每个轴的位移量，即可以通过此方法得到初始位置空间坐 标(x₁,y₁,z₁)和终止位置(x₂,y₂,z₂的空间坐标，故可以得到空间位移向量

并记录终止位置三轴陀螺仪采集到的角速度

步骤4中，将上述信息进行整合步骤为建立人体姿态信息矩阵S，其中：

B_i(X,Y,Z)代表磁场指向向量，ψ_i代表平面指向角，θ_i代表俯仰角，d_i(x,y,z)代表空间指 向向量，w_i代表角速度。

建立机器人姿态信息矩阵R，其中：

g_i(x,y,z)代表机器人质心信息，α_i代表舵机角度，l_i(x,y,z)代表仿人机器人关节坐标信 息。

故机器人姿态与人体姿态的转换过程可以由如下公式表示：

即C_T＝RS^-1，其中，R和S均为非方阵，S^-1为S的广义逆矩阵，C_i与C_T的计算方法相同。

由此可得出机器人姿态与人体姿态的转换矩阵C_T，并与动作组数据库中的转换矩阵C_i依 次进行相关系数计算。相关系数的计算公式如下：

其中，m和n分别代表转换矩阵C_T与C_i的行数和列数，

与

分别代表矩阵C_T与C_i的均 值。相关系数r的值越接近1，代表此时的转换矩阵C_T与数据库转换矩阵C_i越具备相关性。

步骤5中，进行最优相关系数搜索，搜索出最优相关系数的方法为：将转换矩阵C_T与数 据库中存储的各个转换矩阵C_i依次进行相关系数计算，并比较各相关矩阵r_i值，选择最大的 相关系数r_max作为输出。

参阅图6，本发明所述的一种基于人体姿态控制的仿人机器人动作系统的控制方法的机 器人动作组指令数据库的建立过程如下：

首先，对机器人各个舵机进行标号，并对机器人进行动作调整，将机器人调整到理想动 作，操作者对此时的仿人机器人动作进行复现，通过对记录下来的机器人姿态信息和人体姿 态信息进行计算，可以得到对应的转换矩阵，重复上述此步骤多次，记录下每一次的转换矩 阵，将得到的所有转换矩阵中相同位置的元素提取并作均值处理，处理得到后的矩阵记为标 准的转换矩阵。

将标准的转换矩阵和对应的机器人舵机信息(舵机1…n角度)存入数据库中，以此类推， 完成机器人动作组指令数据库的建立。

本发明所述的一种基于人体姿态控制的仿人机器人动作系统及控制方法的工作原理为：

操作者佩戴好九轴MEMS陀螺仪后，演示姿态动作，绑定在操作者身上各位置的陀螺仪 进行空间定位，一号蓝牙通讯模块将此时的人体姿态信息传输给上位机，上位机记录下人体 的姿态信息，开始计算此时的转换矩阵C_T，并与构建的数据库中的转换矩阵C_i依次进行相关 系数计算，搜索出最优相关系数，记录此时动作组数据库中的转换矩阵，读取转换矩阵对应 存储的舵机信息，将存储的舵机信息编译成控制指令。通过二号蓝牙通讯模块发送给仿人机 器人，机器人接收关节运动指令，执行相应的关节运动，实现机器人跟随人体动作的姿态控 制。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉 本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任 何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。同时本说明书中未作详细 描述的内容均属于本领域技术人员公知的现有技术。

Claims (7)

1.一种基于人体姿态控制的仿人机器人动作系统的控制方法，其特征在于，系统包括操作者、上位机、机器人本体；所述上位机与机器人本体的数据传输直接使用蓝牙通讯模块进行数据传输，运用系统进行控制方法，包括如下步骤：

步骤1，首先对机器人本体各个舵机进行逐一标号，并记录下机器人本体平衡位置下的质心位置

各个舵机的转动角度

以及各个关节的空间位置

步骤2，操作者打开各个陀螺仪蓝牙，逐一连接至上位机，操作者做出姿态变化，对各陀螺仪进行空间定位；

步骤3，上位机记录此时的磁场指向向量

以及定位出来的终止位置的平面指向角

和俯仰角

以及空间位移向量

和终止动作位置的三轴角速度

步骤4，将人体姿态信息进行整合，求出人体姿态与机器人本体姿态之间的转换矩阵，与动作组数据库中的转换矩阵依次进行相关系数计算；

步骤4中所述求出人体姿态与机器人本体姿态之间的转换矩阵，包括：

建立人体姿态信息矩阵S，其中：

B_i(X,Y,Z)代表磁场强度指向向量，ψ_i代表平面指向角，θ_i代表俯仰角，d_i(x,y,z)代表空间指向向量，w_i代表三轴角速度；

建立机器人本体姿态信息矩阵R，其中：

g_i(x,y,z)代表机器人质心信息，α_i代表舵机角度，l_i(x,y,z)代表仿人机器人关节坐标信息；

机器人本体姿态与人体姿态的转换过程由如下公式表示：

即C_T＝RS^-1，其中，R和S均为非方阵，S^-1为S的广义逆矩阵，C_i与C_T的计算方法相同；

由此可得出机器人本体姿态与人体姿态的转换矩阵C_T，并与动作组数据库中的转换矩阵C_i依次进行相关系数计算；相关系数的计算公式如下：

其中，m和n分别代表转换矩阵C_T与C_i的行数和列数，

与

分别代表矩阵C_T与C_i的均值；相关系数r的值越接近1，代表此时的转换矩阵C_T与数据库转换矩阵C_i越具备相关性；

步骤5，搜索出最优相关系数，并读取动作组数据库中对应存储的舵机信息，将存储的舵机信息编译成控制指令；

步骤5中所述搜索出最优相关系数的方法为：将转换矩阵C_T与数据库中存储的各个转换矩阵C_i依次进行相关系数计算，并比较各相关矩阵r_i值，选择最大的相关系数r_max作为输出；

所述的动作组数据库的建立过程如下：

对机器人本体各个舵机进行标号，并对机器人本体进行动作调整，将机器人本体调整到理想动作，操作者对此时的机器人本体动作进行复现，通过对记录下来的机器人本体姿态信息和人体姿态信息进行计算，得到对应的转换矩阵，重复上述此步骤多次，记录下每一次的转换矩阵，将得到的所有转换矩阵中相同位置的元素提取并作均值处理，处理得到后的矩阵记为标准的转换矩阵；

将标准的转换矩阵和对应的机器人本体舵机信息存入数据库中，以此类推，完成机器人本体动作组数据库的建立；

步骤6，机器人本体接收控制指令，执行相应的关节运动；

步骤7，机器人各个舵机达到预定的角度，完成机器人本体的动作控制。

2.根据权利要求1所述的一种基于人体姿态控制的仿人机器人动作系统的控制方法，其特征在于：

步骤1中的机器人本体平衡位置下的质心位置按如下公式计算：

其中M为仿人机器人质量，n为机器人的自由度数目，m_j为仿人机器人各个连杆的质量，c_j表示以支撑平衡点为原点的坐标系下，从原点指向连杆的空间向量；向量c_j由机器人本体关节角度经正向运动学计算得到。

3.根据权利要求2所述的一种基于人体姿态控制的仿人机器人动作系统的控制方法，其特征在于：

以佩戴在操作者身上某一关节的九轴MEMS陀螺仪为例，

ψ，θ，

计算过程如下：

三轴磁传感器固定在九轴MEMS陀螺仪中，地磁场为三轴磁传感器的参考方向，当九轴MEMS陀螺仪发生空间变化时，即指向任何方向时，地磁场产生的磁场强度会分配在三轴磁传感器的三轴上，其中：

H_x：地磁场强度向量在传感器X轴上的分量；

H_y：地磁场强度向量在传感器Y轴上的向量；

H_z：地磁场强度向量在传感器Z轴上的向量；

假设操作者所在的初始位置的磁场强度向量为M₁＝(H_x1,H_y1,H_z1),操作者所在的终止位置的磁场强度向量M₂＝(H_x2,H_y2,H_z2)，则磁场强度指向向量

可表示为：

其中，X＝H_x1-H_x2,Y＝H_y1-H_y2,Z＝H_z1-H_z2；

通过终止位置的H_x2、H_y2、H_z2,可以计算出平面指向角ψ和俯仰角θ：

根据建立的空间坐标系，三轴加速度传感器读取的每个轴的加速度通过积分运算算出当前速度值，并再根据积分运算算出每个轴的位移量，通过此方法得到初始位置空间坐标(x₁,y₁,z₁)和终止位置(x₂,y₂,z₂)的空间坐标，得到空间位移向量

并记录终止位置的三轴陀螺仪采集到的三轴角速度

4.根据权利要求3所述的一种基于人体姿态控制的仿人机器人动作系统的控制方法，其特征在于：

向量c_j的计算以机器人本体的左侧髋关节和左侧膝关节为例，假设这两个关节的空间向量分别为k₁＝(x₁,y₁,z₁)、k₂＝(x₂,y₂,z₂)，c₁为左侧髋关节和左侧膝关节间连杆的向量，则有:

5.根据权利要求1所述的一种基于人体姿态控制的仿人机器人动作系统的控制方法，其特征在于：

所述的操作者身体上固定陀螺仪，陀螺仪位置包括操作者的额头处，操作者上半身的左右肩关节外侧、肘关节外侧以及腕关节外侧，操作者下半身的左右髋关节外侧、膝关节外侧以及踝关节外侧；

操作者佩戴的陀螺仪个数与机器人本体的自由度相同。

6.根据权利要求5所述的一种基于人体姿态控制的仿人机器人动作系统的控制方法，其特征在于：

所述陀螺仪为九轴MEMS陀螺仪，所述九轴MEMS陀螺仪由一个三轴加速度传感器、一个三轴陀螺仪组成的感应芯片MPU6050，以及一个三轴磁传感器RMG144组成；

陀螺仪空间坐标系，以重力方向为Z轴的反方向，以磁力方向为Y轴正方向，取正交与这两个方向的唯一方向为X轴，并且以右手定理确定X轴的正方向。

7.根据权利要求5所述的一种基于人体姿态控制的仿人机器人动作系统的控制方法，其特征在于：

所述机器人本体内部模块包括：主控模块、蓝牙通讯模块、网卡模块、运动控制模块、n自由度舵机模块和位置传感器模块；

所述主控模块包括电源模块、硬盘模块、CPU模块、内存模块；电源模块实现机器人稳定的供电和充电功能；硬盘模块实现存储内部程序及数据；CPU模块实现运行控制程序功能；内存模块实现临时存储数据功能；

所述蓝牙通讯模块与网卡模块实现与上位机的数据传输，所述位置传感器模块实现记录机器人各关节空间位置，所述运动控制模块实现存储控制指令模块，并集成动作组指令数据库；所述n自由度舵机模块实现机器人的动作执行。

Images