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

权利要求书

1.一种非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法，其特征在于，包括以下步骤：

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

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

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

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

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

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

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

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

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

3.根据权利要求1所述的非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法，其特征在于，步骤二中所述惯性测量结果归一化到被测运动物体相对摄像机坐标系的空间位置随时间的变换关系式表示为：

Rcb(tk)=RmbRsm(tk)Rcs]]>

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

Rcb(tk)=RtbRct(tk)]]>

上述中，表示主陀螺仪和辅助陀螺仪之间随时间变化的旋转关系，表示立体靶标和摄像机之间随时间变化的旋转关系，表示主陀螺仪和被测运动物体之间的固定旋转关系，表示摄像机和辅助陀螺仪之间的固定旋转关系，表示立体靶标和被测运动物体之间的固定旋转矩阵。