CN105415396A

CN105415396A - 一种关节级联系统的运动参数检测方法及关节级联系统

Info

Publication number: CN105415396A
Application number: CN201510961471.3A
Authority: CN
Inventors: 李凤玲; 刘永涛; 赵铁军; 周尤
Original assignee: Qingdao Hailankf Equipment Co Ltd
Current assignee: Qingdao Hailankf Equipment Co Ltd
Priority date: 2015-12-18
Filing date: 2015-12-18
Publication date: 2016-03-23

Abstract

本发明实施例提供一种关节级联系统的运动参数检测方法及关节级联系统，其中方法基于关节级联系统，系统包括：多个连杆，设置于各连杆上的运动姿态传感器，连接各关联连杆的电机，和处理装置；所述方法应用于处理装置，所述方法包括：获取运动姿态传感器所感应的运动姿态数据；根据所述运动姿态数据分析关节级联系统的运动参数。本发明实施例可实现关节级联系统的运动参数检测，且无对于使用要求的局限，可被普遍应用。

Description

一种关节级联系统的运动参数检测方法及关节级联系统

技术领域

本发明涉及运动仿生模型技术领域，具体涉及一种关节级联系统的运动参数检测方法及关节级联系统。

背景技术

关节级联系统也称为多关节串联系统，多关节表示由多个关节相互连接的整体系统，当一个关节连接的连杆发生运动时，会导致相关关节同样发生位置姿态的变化，关节级联系统最常见的为人体与动物连杆模型，以及多足动物运动生理结构，图1示出了常见的2支撑点和4支撑点情况下的关节级联系统的结构，可参照。

对关节级联系统的运动进行检测，对于双足以及多足生物体的运动仿生模拟具有重要意义；关节级联系统的运动检测主要是对关节级联系统的运动参数进行检测，诸如关节级联系统的连杆运动位置的检测、关节级联系统的重心检测、关节级联系统的位移检测等；通过检测关节级联系统的连杆运动位置、重心、位移等运动参数，可实现对关节级联系统的移动、转向、静止等运动控制调整。

目前，关节级联系统的运动参数检测方式主要是通过光学传感器(如摄像头)实现，通过在关节级联系统的运动场地安装不同角度的多个摄像头，可追踪关节级联系统的关键位置的标记点，从而根据摄像头拍摄的图像数据分析出关节级联系统的运动参数。

本发明的发明人在研究过程中发现，采用光学传感器检测关节级联系统的运动参数的方式，虽然可直接以摄像头拍摄的图像数据获得关节级联系统的绝对运动轨迹，进而分析出运动轨迹相应的运动参数，所得到的运动参数具有一定的精度；但是，光学传感器对于光线、场地的要求较高，在使用要求上具有一定的局限性，并无法普遍应用；因此提供一种新的关节级联系统的运动参数检测方法，以解决使用要求的局限性，使得关节级联系统的运动参数检测能够被普遍应用，成为本领域技术人员需要考虑的问题。

发明内容

有鉴于此，本发明实施例提供一种关节级联系统的运动参数检测方法及关节级联系统，以解决关节级联系统的运动参数检测过程中，所存在的使用要求局限的问题，以使得关节级联系统的运动参数检测能够被普遍应用。

为实现上述目的，本发明实施例提供如下技术方案：

一种关节级联系统的运动参数检测方法，基于一种关节级联系统，所述关节级联系统包括：多个连杆，设置于各连杆上的运动姿态传感器，连接各关联连杆的电机，和处理装置；

所述方法应用于处理装置，所述方法包括：

获取运动姿态传感器所感应的运动姿态数据；

根据所述运动姿态数据分析关节级联系统的运动参数。

其中，所述运动参数包括：关节级联系统的连杆的运动位置；所述方法还包括：

对运动姿态传感器进行初始姿态校准，使得连杆处于初始姿态的初始连杆向量与运动姿态传感器的测量坐标系对齐；

所述获取运动姿态传感器所感应的运动姿态数据包括：

获取连杆运动时，连杆运动的连杆姿态欧拉角，所述连杆姿态欧拉角包括：连杆运动的俯仰角θ，连杆运动的横滚角φ，和连杆运动的航向角ψ；

所述根据所述运动姿态数据分析关节级联系统的运动参数包括：

根据所述连杆姿态欧拉角生成连杆运动所对应的三维旋转矩阵R，R为

[\begin{matrix} \cos θ \cos ψ & \sin φ \sin θ \cos ψ - \cos φ \sin ψ & \cos φ \sin θ \cos ψ + \sin φ \sin ψ \\ \cos θ \sin ψ & \sin φ \sin θ \sin ψ + \cos φ \cos ψ & \cos φ \sin θ \sin ψ - \sin φ \cos ψ \\ - \sin θ & \sin φ \cos θ & \cos φ \cos θ \end{matrix}];

将连杆处于初始姿态的初始连杆向量乘以R，得到连杆运动所对应的连杆向量，所述连杆向量表示连杆的运动位置。

其中，所述对运动姿态传感器进行初始姿态校准包括：

在进行初始姿态校准时，将连杆的向量V(输出)-向量V(补偿)，得到连杆处于初始姿态的初始连杆向量。

其中，所述关节级联系统还包括：设置于关节级联系统的支撑点下方的压力分布传感器；所述运动参数包括：关节级联系统的重心；

所述获取运动姿态传感器所感应的运动姿态数据包括：

获取关节级联系统重心沿x轴方向上的加速度和重心沿y轴方向上的加速度

确定关节级联系统的零力矩点(x_zmp，y_zmp，0)；

根据公式计算x_g，根据公式计算y_g，其中，关节级联系统重心的表达式为(x_g，y_g，z_g)，z_g为常量。

其中，所述关节级联系统还包括：设置于关节级联系统的支撑点下方的压力分布传感器；所述运动参数包括：关节级联系统的位移；

所述获取运动姿态传感器所感应的运动姿态数据包括：

获取各接触点的各轴旋转角度速率；

检测各接触点的总旋转角度速率，总旋转角度速率＝sqrt(X旋转角度速率*X旋转角度速率+Y旋转角度速率*Y旋转角度速率+Z旋转角度速率*Z旋转角度速率)；

根据接触点的总旋转角度速率确定落地点，其中，总旋转角度速率从大于阈值到小于阈值变化时，接触点的运动正处在落地点，总旋转角度速率从小于阈值到大于阈值变化时，接触点的运动则正处在离地点；

根据连杆之间的层次关系，通过落地点的连杆位置推算，落地点与另一接触点之间关联的各连杆的位置；

根据落地点与另一接触点之间关联的各连杆的位置，计算接触点当前落地所对应的位移。

本发明实施例还提供一种关节级联系统，包括：多个连杆，设置于各连杆上的运动姿态传感器，连接各关联连杆的电机，和处理装置；

其中，处理装置，用于获取运动姿态传感器所感应的运动姿态数据；根据所述运动姿态数据分析关节级联系统的运动参数。

其中，所述运动参数包括：关节级联系统的连杆的运动位置；

所述处理装置还用于，对运动姿态传感器进行初始姿态校准，使得连杆处于初始姿态的初始连杆向量与运动姿态传感器的测量坐标系对齐；

所述处理装置在获取运动姿态传感器所感应的运动姿态数据，根据所述运动姿态数据分析关节级联系统的运动参数的方面，具体用于：

获取连杆运动时，连杆运动的连杆姿态欧拉角，所述连杆姿态欧拉角包括：连杆运动的俯仰角θ，连杆运动的横滚角φ，和连杆运动的航向角ψ；根据所述连杆姿态欧拉角生成连杆运动所对应的三维旋转矩阵R，R为

[\begin{matrix} \cos θ \cos ψ & \sin φ \sin θ \cos ψ - \cos φ \sin ψ & \cos φ \sin θ \cos ψ + \sin φ \sin ψ \\ \cos θ \sin ψ & \sin φ \sin θ \sin ψ + \cos φ \cos ψ & \cos φ \sin θ \sin ψ - \sin φ \cos ψ \\ - \sin θ & \sin φ \cos θ & \cos φ \cos θ \end{matrix}];

其中，所述处理装置在对运动姿态传感器进行初始姿态校准的方面具体用于，在进行初始姿态校准时，将连杆的向量V(输出)-向量V(补偿)，得到连杆处于初始姿态的初始连杆向量。

所述处理装置具体用于，获取关节级联系统重心沿x轴方向上的加速度和重心沿y轴方向上的加速度确定关节级联系统的零力矩点(x_zmp，y_zmp，0)；根据公式计算x_g，根据公式计算y_g，其中，关节级联系统重心的表达式为(x_g，y_g，z_g)，z_g为常量。

所述处理装置具体用于，获取各接触点的各轴旋转角度速率；检测各接触点的总旋转角度速率，总旋转角度速率＝sqrt(X旋转角度速率*X旋转角度速率+Y旋转角度速率*Y旋转角度速率+Z旋转角度速率*Z旋转角度速率)；

基于上述技术方案，本发明实施例中关节级联系统的关节是由电机组成，电机的运动可使得关节具有三个转动的自由度，并且通过设置于连杆上的运动姿态传感器在关节运动时，采集关节级联系统的运动姿态数据，可使得反应关节级联系统的运动参数的运动数据得以获取，通过分析运动姿态传感器采集的运动数据则可检测出关节级联系统的运动参数；由于关节级联系统中由电机组成关节，运动姿态传感器设置于连杆上，分析关节级联系统的运动参数的源数据均可通过集合于关节级联系统的本体结构上的设备采集，进而由处理装置分析得出运动参数，因此，本发明实施例可在任意场地实现关节级联系统的运动参数检测，对于应用场地并无限制，由此解决了关节级联系统的运动参数检测过程中，对于使用要求的局限问题，使得关节级联系统的运动参数检测能够被普遍应用。

附图说明

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

图1为2支撑点和4支撑点情况下的关节级联系统的结构示意图；

图2为本发明实施例提供的关节级联系统的运动参数检测方法的流程图；

图3为本发明实施例提供的关节级联系统的结构示意图；

图4为本发明实施例提供的关节级联系统的运动参数检测方法的另一流程图；

图5为N-POSE情况下关节级联系统的示意图；

图6为连杆“确定点”和“推出点”的示意图；

图7为本发明实施例提供的关节级联系统的另一结构示意图；

图8为本发明实施例提供的关节级联系统的运动参数检测方法的再一流程图；

图9为零点力矩所在点的示意图；

图10为本发明实施例提供的关节级联系统的运动参数检测方法的又一流程图；

图11为人体步行的过程示意图；

图12为脚从“晃动”到“静止”状态转换的示意图；

图13为足部压力分布示意图；

图14为关节级联系统位移轨迹的推算示意图；

图15为连续步行情况下关节级联系统位移轨迹的推算示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图2为本发明实施例提供的关节级联系统的运动参数检测方法的流程图，该方法基于图3所示的关节级联系统，在描述运动参数检测方法前，本发明实施例先对关节级联系统的结构改进进行介绍；

参照图3，本发明实施例提供的关节级联系统可以包括：多个连杆1，设置于各连杆上的运动姿态传感器2，连接各关联连杆的电机3，和处理装置4；

其中，运动姿态传感器2为加速度、陀螺仪以及其他辅助的传感器通过数据融合，获得稳定姿态数据的系统装置；运动姿态传感器可以采用微型惯性运动姿态测量设备，这类装置一般叫做OMU(orientationmeasurementunit，定向测量单元)；

在本发明实施例中，连接连杆的关节主要由电机3组成，由电机3组成关节可使得关节具有三个转动的自由度，该三个转动的自由度可以三维姿态向量V(roll，pitch，yaw)表示；

需要说明的是，连杆之间具有父子层次关系，每一段连杆具有层次结构上的父连杆和子连杆(肢端连杆除外)；关节级联系统每一时刻的姿态和运动情况可以看作是由以上层次关系的连杆结构中的每一关节处，在以一个特定的姿态角度同时在转动；可选的，可根据实际情况限定每个关节转动的范围；

处理装置4为具有数据处理功能的设备，可选用处理芯片、控制器等实现；处理装置4主要获取运动姿态传感器2所感应的运动姿态数据，从而分析出关节级联系统的运动参数；运动姿态传感器2可与处理装置4有线或无线连接；

基于图3所示关节级联系统的结构，图2所示方法可应用于处理装置4中，参照图2，本发明实施例提供的关节级联系统的运动参数检测方法可以包括如下步骤：

步骤S100、获取运动姿态传感器所感应的运动姿态数据；

步骤S110、根据所述运动姿态数据分析关节级联系统的运动参数。

本发明实施例提供的关节级联系统的运动参数检测方法，基于一种关节级联系统，该关节级联系统包括：多个连杆，设置于各连杆上的运动姿态传感器，连接各关联连杆的电机，和处理装置；在进行关节级联系统的运动参数检测时，处理装置可获取运动姿态传感器所感应的运动姿态数据，从而根据所获取的运动姿态数据可分析出关节级联系统的运动参数，实现对关节级联系统的运动参数检测。

可以看出，本发明实施例中关节级联系统的关节是由电机组成，电机的运动可使得关节具有三个转动的自由度，并且通过设置于连杆上的运动姿态传感器在关节运动时，采集关节级联系统的运动姿态数据，可使得反应关节级联系统的运动参数的运动数据得以获取，通过分析运动姿态传感器采集的运动数据则可检测出关节级联系统的运动参数；由于关节级联系统中由电机组成关节，运动姿态传感器设置于连杆上，分析关节级联系统的运动参数的源数据均可通过集合于关节级联系统的本体结构上的设备采集，进而由处理装置分析得出运动参数，因此，本发明实施例可在任意场地实现关节级联系统的运动参数检测，对于应用场地并无限制，由此解决了关节级联系统的运动参数检测过程中，对于使用要求的局限问题，使得关节级联系统的运动参数检测能够被普遍应用。

另外，运动姿态传感器体积小、重量轻便于携带，对于关节级联系统的负重影响极低。

可选的，所检测的关节级联系统的运动参数可以包括：关节级联系统的连杆的运动位置、或关节级联系统的重心、或关节级联系统的位移等；下面分别就各运动参数的检测进行介绍。

针对检测关节级联系统的连杆的运动位置的情况，基于图3所示的关节级联系统结构，图4示出了本发明实施例提供的关节级联系统的运动参数检测方法的另一流程图，该方法可应用于处理装置，参照图4，该方法可以包括：

步骤S200、对运动姿态传感器进行初始姿态校准，使得连杆处于初始姿态的初始连杆向量与运动姿态传感器的测量坐标系对齐；

对于运动姿态传感器输出姿态位置的设定，采用标准的N-POSE进行设定，设定的目的在于把连杆向量与姿态传感器测量坐标系对齐：N-POSE定义为连杆自由下垂，类似于人体在不运动时候的站立，负重加载于连杆上，关节电机不受力，如图5所示；对齐之后连杆与全局坐标系的XYZ三个坐标轴中的一个平行，之后，连杆的转动以全局坐标系为参考坐标系；

可选的，本发明实施例可在N-POSE校准时，将所有运动姿态传感器的输出数据通过减去偏置的方式置零；N-POSE校准后运动姿态传感器输出的姿态向量为一个三维的向量V(roll，pitch,yaw)，认为各关节处于图5情况，各个连杆的姿态向量输出为0；

具体的，本发明实施例在进行N-POSE校准时，连杆处于初始姿态的初始连杆向量可通过公式向量V＝向量V(输出)-向量V(补偿)实现，其中，向量V为连杆处于初始姿态的初始连杆向量，向量V(补偿)＝向量V(T-pose)，如果安装位置与连杆的坐标系统相同，并且进行坐标对齐则V(补偿)＝0。

步骤S210、获取连杆运动时，连杆运动的连杆姿态欧拉角，所述连杆姿态欧拉角包括：连杆运动的俯仰角θ，连杆运动的横滚角φ，和连杆运动的航向角ψ；

可选的，连杆运动的连杆姿态欧拉角可由运动姿态传感器采集。

步骤S220、根据所述连杆姿态欧拉角生成连杆运动所对应的三维旋转矩阵R，R为

[\begin{matrix} \cos θ \cos ψ & \sin φ \sin θ \cos ψ - \cos φ \sin ψ & \cos φ \sin θ \cos ψ + \sin φ \sin ψ \\ \cos θ \sin ψ & \sin φ \sin θ \sin ψ + \cos φ \cos ψ & \cos φ \sin θ \sin ψ - \sin φ \cos ψ \\ - \sin θ & \sin φ \cos θ & \cos φ \cos θ \end{matrix}];

步骤S230、将连杆处于初始姿态的初始连杆向量乘以R，得到连杆运动所对应的连杆向量，所述连杆向量表示连杆的运动位置。

在本发明实施例中，连杆的运动可以看作是连杆绕关节旋转，基于此，可以通过运动计算出肢端运动的轨迹，而连杆绕关节的旋转可以进一步抽象成向量绕定点的旋转过程；本发明实施例可先对传感器进行初始姿态校正(N-POSE校正)，使得连杆初始姿态下，连杆的连杆向量与姿态传感器测量坐标系对齐，此时，每一段连杆都可以看作是沿XYZ坐标系一坐标轴平行的向量；

以人体小腿连杆为例，进行N-POSE校准之后，腿部连杆被认为是沿Z轴负方向的长度为腿部长度的向量，两端分别为“确定点”和“推出点”，如图6所示；“确定点”为已知其位置的点，“推出点”为需要计算其位置的点；连杆可以被看作以确定点为原点(0，0，0)，通过由连杆的姿态数据组成的旋转矩阵进行旋转，此时连杆向量另一端的坐标即为推出点相对于所确定原点的位置，用此位置坐标加上确定点实际的位置即可以得到推出点实际的位置。

针对检测关节级联系统的重心的情况，图7示出了关节级联系统的另一结构，图7所示关节级联系统与图3所示相比，还包括：设置于关节级联系统的支撑点下方的压力分布传感器5；

在本发明实施例中，压力分布传感器能够获得关节级联系统的支撑点区域的压力分布结果，轻松地辨识出压力峰值的位置以及无压力点的位置，压力大小在整体平面上通过数值表示，用于每个感测点上的压力可由特别方块表示出其压力值；

其中，关节级联系统的重心为在重力场中，物体处于任何方位时所有各组成支点的重力的合力都通过的那一点；规则而密度均匀物体的重心就是它的几何中心；不规则物体的重心可以用悬挂法来确定，物体的重心不一定在物体上；

基于图7所示关节级联系统，图8示出了本发明实施例提供的关节级联系统的运动参数检测方法的再一流程图，该方法可应用于处理装置，参照图8，该方法可以包括：

步骤S300、获取关节级联系统重心沿x轴方向上的加速度和重心沿y轴方向上的加速度并确定关节级联系统的零力矩点(x_zmp，y_zmp，0)；

可设关节级联系统重心的表达式为(x_g，y_g，z_g)，假定关节级联系统的接触面(地面)是平坦的，则关节级联系统重心到地面的高度z_g是常量，因此可在计算关节级联系统重心时不考虑z_g；

关节级联系统重心沿x轴方向上的加速度和重心沿y轴方向上的加速度可以通过运动姿态传感器获取；

关节级联系统的零力矩点(ZMP)是关节级联系统动态稳定运动的重要指标，ZMP落在脚掌的范围里面，则关节级联系统(如机器人)可以稳定的行走；如图9所示，脚在接触地面时会有反作用力(N)也会产生力矩(M)，而反作用力、惯性力的净力矩的和为零的点则称为零点力矩(如图9的P点所示)。

步骤S310、根据公式计算x_g，根据公式计算y_g，其中，关节级联系统重心的表达式为(x_g，y_g，z_g)，z_g为常量。

需要说明的是，关节级联系统的ZMP的计算公式为：

[\begin{matrix} x_{z m p} \\ y_{z m p} \end{matrix}] = [\begin{matrix} z_{g} \\ y_{g} \end{matrix}] - \frac{z_{g}}{g} [\begin{matrix} \overset{\cdot \cdot}{x_{g}} \\ \overset{\cdot \cdot}{y_{g}} \end{matrix}];

通过ZMP的计算公式可改写出公式：和进而通过改写的公式可计算出x_g和y_g。

针对检测关节级联系统的位移的情况，基于图7所示关节级联系统，图10示出了本发明实施例提供的关节级联系统的运动参数检测方法的又一流程图，该方法可应用于处理装置，参照图10，该方法可以包括：

步骤S400、检测各接触点的总旋转角度速率，总旋转角度速率＝sqrt(X旋转角度速率*X旋转角度速率+Y旋转角度速率*Y旋转角度速率+Z旋转角度速率*Z旋转角度速率)；

接触点为关节级联系统与地面接触的点，如人体的脚。

步骤S410、根据接触点的总旋转角度速率确定落地点，其中，总旋转角度速率从大于阈值到小于阈值变化时，接触点的运动正处在落地点，总旋转角度速率从小于阈值到大于阈值变化时，接触点的运动则正处在离地点；

以接触点为脚为例，落地点可以为人体迈步时的落脚点，离地点可以为人体迈步时的起脚点；

图11示出了人体步行的过程示意图，参照图11，人在步行过程当中总是有一个脚与地接触，以此为支点，向前移动另一只脚的移动过程当中靠的是两条腿各个关节的转动产生肢端的位移来推动人体前行；而在此过程当中与地接触的点在步行的过程当中相对地面静止，其相对地面的位置没有变化，于是可以以接触点为标准，通过连杆旋转向量的原理推算人在迈步过程当中产生的相对于地面的位移；

运动方式解析过程中，需要知道系统在全局坐标系中移动的位移，本发明实施例进行运动追踪采用运动姿态传感器测量各个连杆的姿态数据，即通过安装在连杆部位的运动姿态传感器结合预测步态运动规律利用上述提到的向量旋转原理确定关节级联系统的运动轨迹；

由以上推算原理可以看出，为了推算人体步行的位移，正确判断落地点十分重要；本发明实施例采用的是“脚部角速度阈值法”进行落脚点判断；主要是连续测量计算人体脚端运动的总的转动角速度，不断计算判断所述脚部运动的角速度值并与预设阀值进行比较；

总旋转角度速率＝sqrt(X旋转角度速率*X旋转角度速率+Y旋转角度速率*旋转角度速率+Z旋转角度速率*旋转角度速率)；

具体的，判断每一时刻脚部的状态；当总的角速度值(总旋转角度速率)从大于阈值到小于阈值变化时，即脚的运动正处在落脚点(对应接触点的运动正处在落地点)；当角速度值从小于阈值到大于阈值变化时，脚的运动则正处在起脚点(对应接触点的运动则正处在离地点)；尤其需要关注脚的落脚点，即脚的状态从“晃动”到“静止”转换的位置，如图12所示。

可选的，通过压力分布传感器可以获知支撑点的压力值方向与趋势，可以作为关节级联系统的运动姿态的有益补充；

如图13所示，通过判断支撑点各个部分的压力分布，可以得出如图13所示的二维压力分布图形，通过判断压力点的运动走向，即：从足跟到足尖部分的压力走向，可以轻易确定关节级联系统的运动趋势。

步骤S420、根据连杆之间的层次关系，通过落地点的连杆位置推算，落地点与另一接触点之间关联的各连杆的位置；

步骤S430、根据落地点与另一接触点之间关联的各连杆的位置，计算接触点当前落地所对应的位移。

步骤S420主要以已知一段连杆一端的位置，由旋转矩阵推出另一端位置的方法为基本原理(即图4所示计算连杆的运动位置的方式)；如图14所示，以一个关节联系的下肢两条腿四段连杆A，B，C，D为例说明关节级联系统位移轨迹的推算过程，通过安装在脚上连杆的运动姿态传感器可判断p5正处在落地的位置，在落地时刻进行位移推算过程，计算人行走的位移：

具体的，判断连杆A的下端为接触点静止之后，接触点坐标P1在两段连杆转动的过程当中不会发生变化，确定其为坐标零点；把连杆看成竖直向上的向量，向量的长度为连杆的长度，通过连杆A的姿态构成旋转矩阵，推算连杆A上端的坐标位置P2；由于连杆A和连杆B通过关节互相连接，于是连杆A的上端和连杆B的下端具有相同的3维坐标P2对于连杆B；

用相同的方法，把连杆B看作是起点在P2，竖直向上的向量，并与通过连杆B的姿态构成旋转矩阵相乘，即使用旋转矩阵进行旋转，此时连杆B的上端三维坐标位置为P3；以此类推，分别通过A，B，C，D连杆的固连关系以及C和D的连杆姿态数据推算出P3，P4，P5点的坐标；

于是获得了落地点p1以及已知点p5之间位置的相互关系，也就获得了人两脚之间的位移关系，这样就可以通过上一步的位置p5反推得到新的落步点p1的位置，就可以得到了步行一步的位移。

值得注意的是，图10所示方法在步行过程中位移的连续推算过程为：

人的步行过程是以左脚右脚分别行走一步作为一个基本循环:即左脚向前迈出落地，右脚向前迈出落地作为一个循环交替前行，如图15所示；在步行的一个循环的过程当中，选取左脚落地时刻，右脚准备抬起的时刻开始计算，由于上一次右脚落地的位置p1是已知的，假设p5点为坐标原点开始沿着连杆层次关系寻找每段连杆的“父连杆”以及各个连杆的姿态按照上面提到向量旋转的方法算出胯部p3的位置；然后再通过寻找“子连杆”的顺序推得p1点的位置，此时p1点的位置为相对于与p5时的位置，而不是在全局坐标系中的全局位置；

即P5相对P1的位移＝-P1相对P5的位移；由于上一步的位置P1已知，于是根据此相对关系就可以计算左脚新迈出一步的位移:DeltaS＝P1+(P5相对P1)-(P1相对P5)；

相同的原理，在右脚落地时反推出P9和P5之间的位置关系；这样就可以由上一步已知的P5的位置推出P9的位置.这样在双脚交替进行步行的过程当中就可以推出位移。

本发明实施例提供的关节级联系统的运动参数检测方法，可实现关节级联系统的连杆运动位置、重心、位移等运动参数的检测，且无对于使用要求的局限，可被普遍应用。

下面对本发明实施例提供的关节级联系统进行介绍，下文描述的关节级联系统可与上文描述的关节级联系统的运动参数检测方法相互对应参照。

本发明实施例提供的关节级联系统的结构可如图3所示，包括：多个连杆1，设置于各连杆上的运动姿态传感器2，连接各关联连杆的电机3，和处理装置4；处理装置4与运动姿态传感器2通过有线或无线方式连接，以实现数据通信；

其中，处理装置4主要实现关节级联系统的运动参数检测，处理装置4可用于，获取运动姿态传感器所感应的运动姿态数据；根据所述运动姿态数据分析关节级联系统的运动参数。

可选的，运动参数可以包括：关节级联系统的连杆的运动位置；处理装置4可先对运动姿态传感器进行初始姿态校准，使得连杆处于初始姿态的初始连杆向量与运动姿态传感器的测量坐标系对齐；

具体的，在进行初始姿态校准时，可将连杆的向量V(输出)-向量V(补偿)，得到连杆处于初始姿态的初始连杆向量；

在检测关节级联系统的连杆的运动位置的方面，处理装置4具体可用于，获取连杆运动时，连杆运动的连杆姿态欧拉角，所述连杆姿态欧拉角包括：连杆运动的俯仰角θ，连杆运动的横滚角φ，和连杆运动的航向角ψ；根据所述连杆姿态欧拉角生成连杆运动所对应的三维旋转矩阵R，R为

[\begin{matrix} \cos θ \cos ψ & \sin φ \sin θ \cos ψ - \cos φ \sin ψ & \cos φ \sin θ \cos ψ + \sin φ \sin ψ \\ \cos θ \sin ψ & \sin φ \sin θ \sin ψ + \cos φ \cos ψ & \cos φ \sin θ \sin ψ - \sin φ \cos ψ \\ - \sin θ & \sin φ \cos θ & \cos φ \cos θ \end{matrix}];

可选的，本发明实施例提供的关节级联系统的结构还可如图7所示，还包括：设置于关节级联系统的支撑点下方的压力分布传感器5；

在图7所示关节级联系统的结构上，处理装置可实现关节级联系统的重心检测；对应的，运动参数可以包括：关节级联系统的重心；

在检测关节级联系统的重心方面，处理装置具体可用于，获取关节级联系统重心沿x轴方向上的加速度和重心沿y轴方向上的加速度确定关节级联系统的零力矩点(x_zmp，y_zmp，0)；根据公式计算x_g，根据公式计算y_g，其中，关节级联系统重心的表达式为(x_g，y_g，z_g)，z_g为常量。

在图7所示关节级联系统的结构上，处理装置可实现关节级联系统的位移检测；对应的，运动参数可以包括：关节级联系统的位移；

在检测关节级联系统的位移方面，处理装置具体可用于，获取各接触点的各轴旋转角度速率；检测各接触点的总旋转角度速率，总旋转角度速率＝sqrt(X旋转角度速率*X旋转角度速率+Y旋转角度速率*Y旋转角度速率+Z旋转角度速率*Z旋转角度速率)；

本发明实施例提供的关节级联系统，可实现关节级联系统的连杆运动位置、重心、位移等运动参数的检测，且无对于使用要求的局限，可被普遍应用。

本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims

1.一种关节级联系统的运动参数检测方法，其特征在于，基于一种关节级联系统，所述关节级联系统包括：多个连杆，设置于各连杆上的运动姿态传感器，连接各关联连杆的电机，和处理装置；

所述方法应用于处理装置，所述方法包括：

获取运动姿态传感器所感应的运动姿态数据；

根据所述运动姿态数据分析关节级联系统的运动参数。

2.根据权利要求1所述的关节级联系统的运动参数检测方法，所述运动参数包括：关节级联系统的连杆的运动位置；所述方法还包括：

所述获取运动姿态传感器所感应的运动姿态数据包括：

[\begin{matrix} \cos θ \cos ψ & \sin φ \sin θ \cos ψ - \cos φ \sin ψ & \cos φ \sin θ \cos ψ + \sin φ \sin ψ \\ \cos θ \sin ψ & \sin φ \sin θ \sin ψ + \cos φ \cos ψ & \cos φ \sin θ \sin ψ - \sin φ \cos ψ \\ - \sin θ & \sin φ \cos θ & \cos φ \cos θ \end{matrix}];

3.根据权利要求2所述的关节级联系统的运动参数检测方法，所述对运动姿态传感器进行初始姿态校准包括：

4.根据权利要求1所述的关节级联系统的运动参数检测方法，所述关节级联系统还包括：设置于关节级联系统的支撑点下方的压力分布传感器；所述运动参数包括：关节级联系统的重心；

所述获取运动姿态传感器所感应的运动姿态数据包括：

确定关节级联系统的零力矩点(x_zmp，y_zmp，0)；

5.根据权利要求1所述的关节级联系统的运动参数检测方法，所述关节级联系统还包括：设置于关节级联系统的支撑点下方的压力分布传感器；所述运动参数包括：关节级联系统的位移；

所述获取运动姿态传感器所感应的运动姿态数据包括：

获取各接触点的各轴旋转角度速率；

6.一种关节级联系统，其特征在于，包括：多个连杆，设置于各连杆上的运动姿态传感器，连接各关联连杆的电机，和处理装置；

7.根据权利要求6所述的关节级联系统，其特征在于，所述运动参数包括：关节级联系统的连杆的运动位置；

[\begin{matrix} \cos θ \cos ψ & \sin φ \sin θ \cos ψ - \cos φ \sin ψ & \cos φ \sin θ \cos ψ + \sin φ \sin ψ \\ \cos θ \sin ψ & \sin φ \sin θ \sin ψ + \cos φ \cos ψ & \cos φ \sin θ \sin ψ - \sin φ \cos ψ \\ - \sin θ & \sin φ \cos θ & \cos φ \cos θ \end{matrix}];

8.根据权利要求7所述的关节级联系统，其特征在于，所述处理装置在对运动姿态传感器进行初始姿态校准的方面具体用于，在进行初始姿态校准时，将连杆的向量V(输出)-向量V(补偿)，得到连杆处于初始姿态的初始连杆向量。

9.根据权利要求6所述的关节级联系统，其特征在于，还包括：设置于关节级联系统的支撑点下方的压力分布传感器；所述运动参数包括：关节级联系统的重心；

10.根据权利要求6所述的关节级联系统，其特征在于，还包括：设置于关节级联系统的支撑点下方的压力分布传感器；所述运动参数包括：关节级联系统的位移；