专利名称：：一种仿人机器人身体姿态角度实时估计方法
技术领域：
：本发明属于机器人领域，尤其涉及一种基于六维力/力矩传感器，加速度计和陀螺仪的仿人机器人姿态估计方法。
背景技术：
：仿人机器人是一种模仿人体骨骼结构的机器人，它与其他机器人的最大区别在于其仿人的双足运动方式。仿生分析表明，仿人机器人具有环境适应能力强、动作灵活等优点，因此，具有更加广阔的应用和发展前景。与轮式机器人和工业机器人不同，仿人机器人的支撑域更小，更容易摔倒。因此需要知道机器人的姿态角度，包括俯仰和滚动角度，以便检测机器人的状态，预防机器人摔倒。一种比较常用的姿态估计算法是先对角速度的积分经过一个高通滤波器得到角度的高频信号，再对经加速度计算得到的角度经过一个低通滤波器，得到姿态角度的低频信号，这两个角度信号的叠加可以得到完整的姿态角度(参见“K.Loffler,M.Gienger,F.Pfeiffer:SensorandControlDesignofaDynamicallyStableBipedRobot，InternationalConferenceonRoboticsandAutomation,September，2003，，)，但是该方法需要设定合适的低通滤波器和高通滤波器的参数，以达到准确的估计，而且对于不同的步行模式，需要选择不同的滤波器参数，因此该方法的适用范围较小。由于姿态估计的非线性，另外一种常见的方法是，对姿态运动学模型在平衡点进行线性化处理，得到该运动学方程的线性模型，利用kalman滤波的方法估计该线性模型的状态，从而得至IJ姿态角度(参见"AnAttitudeControlSystemforsmallunmannedhelicoptersbasedongyrometerandangularrategyroscope，，)。{Β亥方^去又寸于ill离平衡状态的估计存在较大的误差，并且测量的范围较小。HenrikRehbinder和XiaomingHu提出了姿态估计的另一种方法(参见"Drift-freeattitudeestimationforacceleratedrigidbodies,Automatica,2004”)。在该方法中，利用重力加速度修正机器人的姿态角度。但是该方法需要机器人的横向加速度较小。对于仿人机器人步行中的加速度变化，由于横向加速度引入的模型误差并不能忽略，所以该方法并不适用于仿人机器人的步行。中国发明专利申请200810224579.4提出了一个仿人机器人足部姿态实时检测系统及方法，该姿态检测方法利用三轴加速度信息计算得到静态倾角信息；对采集的角速度信息进行计算得到角度的变化值；根据所述的静态面倾角信息和所述的角度变化值，得到实时的倾角信息。但是由于机器人运动过程中与地面的碰撞，利用加速度计计算所得的静态倾角信息并不准确，而且该方案也没办法克服角速度的积分误差。因此，有必要设计一种能够实时，快速并且能有效克服陀螺仪积分误差的仿人机器人姿态角度估计方案。
发明内容本发明的目的是针对现有技术的不足，提供了一种仿人机器人身体姿态角度实时估计方法。本发明的目的是通过以下技术方案来实现的一种仿人机器人身体姿态角度实时估计方法，仿人机器人姿态估计系统包括三轴陀螺仪、三轴加速度计、kalman滤波器和两个六维力/力矩传感器，其中，三轴陀螺仪、三轴加速度计、和两个六维力/力矩传感器分别与kalman滤波器相连；该方法包括以下几个步骤(1)安装六维力/力矩传感器及三轴陀螺仪和三轴加速度计；其中，两个六维力/力矩传感器分别安装在机器人的两个踝关节上，三轴加速度计和三轴陀螺仪安装在机器人的髋部中心点。(2)建立机器人姿态的运动学模型；(3)根据机器人姿态的运动学模型，建立带噪声的线性离散随机系统模型，利用卡尔曼滤波的相应方法，基于三轴加速度计的加速度数据，三轴陀螺仪的角速度数据和六维力/力矩传感器三维力数据估计机器人的姿态状态；(4)根据估计的机器人姿态信息，得到机器人实际的姿态角度；(5)根据得到的机器人实际的姿态角度，矫正机器人当前的姿态。本发明的有益效果是，本发明仿人机器人身体姿态角度实时估计方法通过软测量的技术实现，根据机器人姿态的运动学方程，利用kalman滤波的方法，得到机器人姿态状态的最优估计，从而计算出相应的俯仰和滚动角度。本方法克服了角速度积分的漂移以及传统线性化方法测量范围较小的缺陷，实现了仿人机器人姿态实时准确的估计。图1是仿人机器人姿态估计系统的整体框图；图2是传感器的安装示意图3是机器人稳定行走时身体姿态的滚动角度数据图；图4是机器人稳定行走时身体姿态的俯仰角度数据图。具体实施例方式Kalman滤波的原理如下禾Ij用给定的系统输出观测序列权利要求1.一种仿人机器人身体姿态角度实时估计方法，仿人机器人姿态估计系统包括三轴陀螺仪、三轴加速度计、kalman滤波器和两个六维力/力矩传感器，其中，三轴陀螺仪、三轴加速度计、和两个六维力/力矩传感器分别与kalman滤波器相连；其特征在于，该方法包括以下几个步骤(1)安装六维力/力矩传感器及三轴陀螺仪和三轴加速度计；其中，两个六维力/力矩传感器分别安装在机器人的两个踝关节上，三轴加速度计和三轴陀螺仪安装在机器人的髋部中心点；(2)建立机器人姿态的运动学模型；(3)根据机器人姿态的运动学模型，建立带噪声的线性离散随机系统模型，利用卡尔曼滤波的相应方法，基于三轴加速度计的加速度数据，三轴陀螺仪的角速度数据和六维力/力矩传感器三维力数据估计机器人的姿态状态；(4)根据估计的机器人姿态信息，得到机器人实际的姿态角度；(5)根据得到的机器人实际的姿态角度，矫正机器人当前的姿态。2.根据权利要求1所述仿人机器人身体姿态角度实时估计方法，其特征在于，所述步骤(2)建立的运动学模型为3.根据权利要求1所述仿人机器人身体姿态角度实时估计方法，其特征在于，所述步骤(2)建立的线性离散随机系统模型为4.根据权利要求1所述仿人机器人身体姿态角度实时估计方法，其特征在于，所述步骤(4)得到的机器人实际的姿态角度为)，X(S)5X{9)分别表示状态向量μ-D第7，8，9行的元素。(Ζ-tan-1β——sin其中，it；全文摘要本发明公开了一种仿人机器人身体姿态角度实时估计方法，该方法首先建立机器人姿态的运动学模型；然后根据机器人姿态的运动学模型，建立带噪声的线性离散随机系统模型，利用卡尔曼滤波的相应方法，基于三轴加速度计的加速度数据，三轴陀螺仪的角速度数据和六维力/力矩传感器三维力数据估计机器人的姿态状态，再根据估计的机器人姿态信息，得到机器人实际的姿态角度，从而矫正机器人当前的姿态；本方法克服了角速度积分的漂移以及传统线性化方法测量范围较小的缺陷，实现了仿人机器人姿态实时准确的估计。文档编号G01C21/16GK102121828SQ20101059760公开日2011年7月13日申请日期2010年12月21日优先权日2010年12月21日发明者熊蓉,相远飞申请人:浙江大学