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

摘要

本发明公开了一种非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法建立测量系统，定义坐标系；视觉姿态测量、惯性角速度差分计算及坐标系归一化；建立非线性卡尔曼滤波系统模型；基于残差补偿的多速率CKF数据融合。本发明将单目视觉和双MEMS陀螺仪组合，并结合多传感器组合系统采样频率不一致的固有问题，使用基于残差补偿的多速率CKF进行多传感器数据融合，保证测量系统更新高频率输出的同时确保测量系统的高精度及稳定性。

主权项

一种非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法，其特征在于，包括以下步骤：步骤一，建立测量系统，定义坐标系：测量系统包括运动载体，所述运动载体上设置有被测运动物体和支架，所述被测运动物体上设置有主陀螺仪和立体靶标，所述支架上设置有辅助陀螺仪和摄像机，所述立体靶标面向摄像机形成由外向内的视觉测量结构；定义的坐标系包括空间惯性坐标系、摄像机坐标系、主陀螺仪坐标系、辅助陀螺仪坐标系、靶标坐标系和被测运动物体坐标系；步骤二，视觉姿态测量、惯性角速度差分计算及坐标系归一化：视觉姿态测量方法是使用摄像机拍摄被测运动物体上立体靶标的四个立体特征点，根据特征点在摄像机中成像坐标的变化，结合已知特征点在被测运动物体上的布局坐标，实现对被测运动物体位姿的测量，视觉姿态测量结果是靶标坐标系相对摄像机坐标系的空间姿态关系；将主陀螺仪和辅助陀螺仪输出角速度方向归一化之后，使用差分方法即将运动载体的有害运动量从主陀螺仪输出中除去，获取纯净的角速度，即为惯性测量结果；将视觉姿态测量结果和惯性测量结果均归一化到被测运动物体相对摄像机坐标系中；步骤三，建立非线性卡尔曼滤波系统模型：非线性卡尔曼滤波系统模型由非线性状态模型和线性观测模型组成；步骤四，基于残差补偿的多速率CKF数据融合：当存在视觉数据时，进行CKF滤波计算；在视觉数据的采样间隔，在进行时间更新过程的同时，增加估计残差补偿步骤。