CN102121828B

CN102121828B - 一种仿人机器人身体姿态角度实时估计方法

Info

Publication number: CN102121828B
Application number: CN 201010597601
Authority: CN
Inventors: 相远飞; 熊蓉
Original assignee: Zhejiang University ZJU
Current assignee: Zhejiang University ZJU
Priority date: 2010-12-21
Filing date: 2010-12-21
Publication date: 2012-12-19
Anticipated expiration: 2030-12-21
Also published as: CN102121828A

Abstract

本发明公开了一种仿人机器人身体姿态角度实时估计方法，该方法首先建立机器人姿态的运动学模型；然后根据机器人姿态的运动学模型，建立带噪声的线性离散随机系统模型，利用卡尔曼滤波的相应方法，基于三轴加速度计的加速度数据，三轴陀螺仪的角速度数据和六维力/力矩传感器三维力数据估计机器人的姿态状态，再根据估计的机器人姿态信息，得到机器人实际的姿态角度，从而矫正机器人当前的姿态；本方法克服了角速度积分的漂移以及传统线性化方法测量范围较小的缺陷，实现了仿人机器人姿态实时准确的估计。

Description

一种仿人机器人身体姿态角度实时估计方法

技术领域

本发明属于机器人领域，尤其涉及一种基于六维力/力矩传感器，加速度计和陀螺仪的仿人机器人姿态估计方法。

技术背景

仿人机器人是一种模仿人体骨骼结构的机器人，它与其他机器人的最大区别在于其仿人的双足运动方式。仿生分析表明，仿人机器人具有环境适应能力强、动作灵活等优点，因此，具有更加广阔的应用和发展前景。

与轮式机器人和工业机器人不同，仿人机器人的支撑域更小，更容易摔倒。因此需要知道机器人的姿态角度，包括俯仰和滚动角度，以便检测机器人的状态，预防机器人摔倒。

一种比较常用的姿态估计算法是先对角速度的积分经过一个高通滤波器得到角度的高频信号，再对经加速度计算得到的角度经过一个低通滤波器，得到姿态角度的低频信号，这两个角度信号的叠加可以得到完整的姿态角度(参见“K. Loffler, M.Gienger, F.Pfeiffer：Sensor and Control Design of a Dynamically Stable Biped Robot，International Conference on Robotics and Automation，September，2003”) ，但是该方法需要设定合适的低通滤波器和高通滤波器的参数，以达到准确的估计，而且对于不同的步行模式，需要选择不同的滤波器参数，因此该方法的适用范围较小。

由于姿态估计的非线性，另外一种常见的方法是，对姿态运动学模型在平衡点进行线性化处理，得到该运动学方程的线性模型，利用kalman滤波的方法估计该线性模型的状态，从而得到姿态角度（参见“An Attitude Control System for small unmanned helicopters based on gyrometer and angular rate gyroscope”）。但是该方法对于偏离平衡状态的估计存在较大的误差，并且测量的范围较小。

Henrik Rehbinder和Xiaoming Hu 提出了姿态估计的另一种方法（参见“Drift-free attitude estimation for accelerated rigid bodies，Automatica, 2004”）。在该方法中，利用重力加速度修正机器人的姿态角度。但是该方法需要机器人的横向加速度较小。对于仿人机器人步行中的加速度变化，由于横向加速度引入的模型误差并不能忽略，所以该方法并不适用于仿人机器人的步行。

中国发明专利申请200810224579.4提出了一个仿人机器人足部姿态实时检测系统及方法，该姿态检测方法利用三轴加速度信息计算得到静态倾角信息；对采集的角速度信息进行计算得到角度的变化值；根据所述的静态面倾角信息和所述的角度变化值，得到实时的倾角信息。但是由于机器人运动过程中与地面的碰撞，利用加速度计计算所得的静态倾角信息并不准确，而且该方案也没办法克服角速度的积分误差。

因此，有必要设计一种能够实时，快速并且能有效克服陀螺仪积分误差的仿人机器人姿态角度估计方案。

发明内容

本发明的目的是针对现有技术的不足，提供了一种仿人机器人身体姿态角度实时估计方法。

本发明的目的是通过以下技术方案来实现的：一种仿人机器人身体姿态角度实时估计方法，仿人机器人姿态估计系统包括三轴陀螺仪、三轴加速度计、kalman滤波器和两个六维力/力矩传感器，其中，三轴陀螺仪、三轴加速度计、和两个六维力/力矩传感器分别与kalman滤波器相连；该方法包括以下几个步骤：

（1）安装六维力/力矩传感器及三轴陀螺仪和三轴加速度计；其中，两个六维力/力矩传感器分别安装在机器人的两个踝关节上，三轴加速度计和三轴陀螺仪安装在机器人的髋部中心点。

（2）建立机器人姿态的运动学模型；

（3）根据机器人姿态的运动学模型，建立带噪声的线性离散随机系统模型，利用卡尔曼滤波的相应方法，基于三轴加速度计的加速度数据，三轴陀螺仪的角速度数据和六维力/力矩传感器三维力数据估计机器人的姿态状态；

（4）根据估计的机器人姿态信息，得到机器人实际的姿态角度；

（5）根据得到的机器人实际的姿态角度，矫正机器人当前的姿态。

本发明的有益效果是，本发明仿人机器人身体姿态角度实时估计方法通过软测量的技术实现，根据机器人姿态的运动学方程，利用kalman滤波的方法，得到机器人姿态状态的最优估计，从而计算出相应的俯仰和滚动角度。本方法克服了角速度积分的漂移以及传统线性化方法测量范围较小的缺陷，实现了仿人机器人姿态实时准确的估计。

附图说明

图1是仿人机器人姿态估计系统的整体框图；

图2是传感器的安装示意图；

图3是机器人稳定行走时身体姿态的滚动角度数据图；

图4是机器人稳定行走时身体姿态的俯仰角度数据图。

具体实施方式

Kalman滤波的原理如下：利用给定的系统输出观测序列

，作状态

的最优估计

，使得k+1时刻的估计指标（既估计误差的协方差矩阵）J达到最小：

如图1所示，仿人机器人姿态估计系统包括三轴陀螺仪、三轴加速度计、kalman滤波器和两个六维力/力矩传感器，其中，三轴陀螺仪、三轴加速度计、和两个六维力/力矩传感器分别与kalman滤波器相连。三轴加速度计提供机器人运动时的三轴线性加速度数据，三轴陀螺仪提供三轴的角速度数据，两个六维力/力矩传感器提供三维力数据。基于机器人姿态变化的运动学模型建立系统状态方程，根据传感器获得的力数据，加速度数据以及角速度数据，通过kalman滤波的方法得到机器人姿态的最优估计。根据最优估计得到机器人姿态，计算可得相应的姿态角度。

本发明仿人机器人身体姿态角度实时估计方法，包括以下几个步骤：首先，安装六维力/力矩传感器及三轴陀螺仪和加速度计传感器；其次，建立机器人姿态的运动学模型；再次，根据机器人姿态的运动学模型，建立带噪声的线性离散随机系统模型，利用卡尔曼滤波的相应方法，基于三轴加速度计的加速度数据，三轴陀螺仪的角速度数据和六维力/力矩传感器三维力数据估计机器人的姿态状态；根据估计后的机器人姿态信息，得到机器人实际的姿态角度。最后，根据得到的机器人姿态俯仰和滚动角度，矫正机器人当前的姿态。

具体如下，

第一步，安装六维力/力矩传感器及三轴陀螺仪和加速度计传感器

传感器的安装示意图如图2所示：两个六维力/力矩传感器分别安装在机器人的两个踝关节（脚板与小腿的连接部位）上，并且传感器局部坐标系的x轴正方向沿机器人前进方向，y轴正方向从右至左，z轴正方向竖直向上。三轴加速度计和三轴陀螺仪安装在机器人的髋部中心点，并且其局部坐标系方向和六维力/力矩传感器的坐标系方向一致。

第二步，建立机器人姿态的运动学模型。

姿态可以用3×3的旋转矩阵来描述，对应绕x轴的旋转角称为滚动角（

），相应的旋转矩阵：

对应绕y轴的旋转角度称为俯仰角（）相应的旋转矩阵为：

对应绕z轴的旋转角度称为偏转角(

)，相应的旋转矩阵为：

对于机器人的姿态，我们用欧拉角度按偏转（

），俯仰（

），滚动（

）的顺序来描述, 则对应的姿态矩阵为：

对该旋转矩阵求导：

把

写为两个矩阵的组合，

，其中，在时间间隔中，绕轴K的微量旋转为

，则上式可以改写为：

对上式进行角度微分变换得，

_所以

安装在机器人髋部中心点的三轴加速度计和三轴陀螺仪，能够实时测量机器人髋部中心点沿自身坐标系的加速度和相应的角速度。当机器人静止的时候，加速度计测量的加速度为重力加速度。当机器人运动的时候，地面的摩擦力提供机器人前进的动力，机器人受到的加速度由合外力提供（见图2）。

假设机器人的脚板与地面完全接触，并且机器人是刚性的。我们以地面为世界坐标系的原点，建立笛卡尔右手坐标系，x轴的正方向为机器人的前进方向，z轴的正方先垂直地面向上。并且机器人髋部的加速度计和姿态传感器的初始姿态与世界坐标系的姿态相同。机器人足部受到的合外力f，在机器人髋部的加速度计上引起的加速度为a，此时机器人的姿态为R，则：

a = R*(f/M)

考虑到姿态R的状态转移方程，则机器人姿态的运动学模型为：

其中，M为机器人的质量，

，是两个六维力/力矩传感器测量的合力，表示机器人足部收到的合外力在坐标系的各个分量，是3×1的向量，表示三轴陀螺仪测量的三轴角速度数据，分别用

,

,

表示。R是3×3的矩阵，表示机器人的姿态矩阵；

，表示三轴加速度计测量的加速度值。

_{第三步，根据机器人姿态的运动学模型，建立带噪声的线性离散随机系统模型，} 再利用卡尔曼滤波的相应方法，基于三轴加速度计的加速度数据，三轴陀螺仪的角速度数据和六维力/力矩传感器三维力数据估计机器人的姿态状态。

_{考虑到R为3×3的矩阵，测量的加速度a和合外力f为3×1向量，对姿态矩阵按行拉直，同时假设传感器测量的噪声白噪声，则系统模型可重构为：}

式中状态变量

，

为模型噪声，方差为，为观测噪声，方差为

，

为三轴加速度，

对该模型状态的估计采用离散化的Kalman滤波方程，则以Ts为离散时间间隔，该模型的离散化方程为：

其中，

是9×1的向量，表示系统的模型噪声，

是3×1的向量，表示系统的观测噪声。

是9×1的向量，表示系统的状态变量。

以状态的提前一步预报量

为递推的基本变量，可写出Kalman滤波递推公式为：

是k时刻的三轴加速度计的加速度值

Kalman预报增益矩阵

为：

为预报误差的协方差矩阵：

是9×9的矩阵，表示模型噪声的方差，

是3×3的矩阵，表示观测噪声的方差。

第四步，根据估计后的机器人姿态信息，得到机器人实际的姿态角度。

利用kalman滤波算法，在线估算

，根据估算的姿态矩阵，计算俯仰和滚动的姿态角度：

式中

分别表示状态向量

第7，8，9行的元素

第五步，根据得到的机器人姿态俯仰和滚动角度，矫正机器人当前的姿态。

为了保持机器人的稳定运行，机器人运行过程的设定的滚动和俯仰角度为0，通过控制机器人髋部关节的俯仰和滚动的关节角度，利用PID调整方式，实现机器人身体姿态的稳定控制。

实施例1

图3和图4示出了在仿人机器人上应用本发明的方法和现有积分方法的的不同之处，设定机器人的步行速度为0.8km/h，由图3可以看出，由于机器人髋部刚性的不足，机器人在行走过程中，滚动角度存在周期性变化，该数据反映了机器人运动过程中的滚动角度变化。由于机器人摆放的初始位置或者零位的偏差，导致滚动角度并不为零。从与积分方法的对比可以看出，该方法有效地克服了角速度积分的漂移问题。由图4可以看出，当机器人运动结束后，俯仰角度的数据始终保持稳定。综上所述，该方法能有效克服角速度积分的漂移问题，反映机器人运动过程中俯仰和滚动姿态角度的变化，实现机器人俯仰和滚动姿态角度的有效测量。