[发明专利]非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法

说明书

技术领域

本发明涉及视觉姿态测量技术、惯性姿态测量技术、多传感器组合技术、多速率非线性滤波技术，更具体的说，是涉及一种非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法。

背景技术

从经典物理学的角度，除非物体保持静止或匀速直线运动否则其进行的是非惯性运动。对于车辆、飞机、轮船、运动模拟器等存在多种运动形式的运动载体，其进行的运动通常为非惯性运动，此时与这些载体固连的坐标系为非惯性系。

在载体保持静止的惯性坐标系中，基于单目视觉和单惯性传感器组合的测量系统可完成静止载体上物体相对姿态的测量任务。此类组合测量系统可发挥视觉测量精度高、重复性好的优点以及惯性传感器更新频率高、自主性强的特点，抑制惯性角度随时间漂移并且可避免单纯视觉测量易受环境光干扰、快速运动导致图像模糊造成无法测量等缺点，形成更新频率高、鲁棒性强、输出稳定的姿态测量系统。

当载体运动形式多样时，单目视觉和单惯性器件组合的测量系统将无法实现运动载体上物体相对姿态的测量。这是因为载体非惯性运动过程中的惯性力会作用在与其固连的物体上。惯性传感器的输出是相对空间惯性系的，在应用中一般需要将其安装在被测被测运动物体上。而微机械(Micro-Electro-Mechanical Systems，MEMS)惯性器件，因其体积小、重量轻、价格低获得越来越广泛的关注和应用，尤其当被测被测运动物体对承重有严格要求时。使用MEMS陀螺仪测量运动载体上被测运动物体相对参考系的相对运动姿态时，载体本身的运动成为有害运动并同样被安装在目标物上的陀螺仪感测。要获取纯净的被测运动物体的角速度需要将有害的载体运动部分消去。

发明内容

本发明的目的是为了克服现有技术中的不足，为实现非惯性系下运动载体上被测被测运动物体相对运动参考系的姿态测量，提供了一种非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法，将单目视觉和双MEMS陀螺仪组合，并结合多传感器组合系统采样频率不一致的固有问题，使用基于残差补偿的多速率CKF进行多传感器数据融合，保证测量系统更新高频率输出的同时确保测量系统的高精度及稳定性。

本发明的目的是通过以下技术方案实现的。

本发明的非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法，包括以下步骤：

步骤一，建立测量系统，定义坐标系：

测量系统包括运动载体，所述运动载体上设置有被测运动物体和支架，所述被测运动物体上设置有主陀螺仪和立体靶标，所述支架上设置有辅助陀螺仪和摄像机，所述立体靶标面向摄像机形成由外向内的视觉测量结构；定义的坐标系包括空间惯性坐标系、摄像机坐标系、主陀螺仪坐标系、辅助陀螺仪坐标系、靶标坐标系和被测运动物体坐标系；

步骤二，视觉姿态测量、惯性角速度差分计算及坐标系归一化：

视觉姿态测量方法是使用摄像机拍摄被测运动物体上立体靶标的四个立体特征点，根据特征点在摄像机中成像坐标的变化，结合已知特征点在被测运动物体上的布局坐标，实现对被测运动物体位姿的测量，视觉姿态测量结果是靶标坐标系相对摄像机坐标系的空间姿态关系；

将主陀螺仪和辅助陀螺仪输出角速度方向归一化之后，使用差分方法即将运动载体的有害运动量从主陀螺仪输出中除去，获取纯净的角速度，即为惯性测量结果；将视觉姿态测量结果和惯性测量结果均归一化到被测运动物体相对摄像机坐标系中；

步骤三，建立非线性卡尔曼滤波系统模型：非线性卡尔曼滤波系统模型由非线性状态模型和线性观测模型组成；

步骤四，基于残差补偿的多速率CKF数据融合：

当存在视觉数据时，进行CKF滤波计算；在视觉数据的采样间隔，在进行时间更新过程的同时，增加估计残差补偿步骤。

步骤一中所述空间惯性坐标系是指牛顿定律适用的非加速坐标系；所述摄像机坐标系是指原点在摄像机光学中心，z轴与镜头光轴方向一致；所述主陀螺仪坐标系是指与主陀螺仪本身固连的坐标系，用于输出主陀螺仪测量的角速度；所述辅助陀螺仪坐标系是指与辅助陀螺仪本身固连的坐标系，用于输出辅助陀螺仪测量的角速度；所述靶标坐标系是指三个特征点组成的主平面，中心点投射到主平面上的位置为坐标系的原点，z轴垂直于主平面由原点指向中心点；所述被测运动物体坐标系是指被测被测运动物体自身运动过程中形成进而建立的坐标系。

步骤二中所述惯性测量结果归一化到被测运动物体相对摄像机坐标系的空间位置变换关系式表示为：

所述视觉姿态测量结果归一化到被测运动物体相对摄像机坐标系的空间位置变换关系式表示为：