CN107782309A - 非惯性系视觉和双陀螺仪多速率ckf融合姿态测量方法 - Google Patents
非惯性系视觉和双陀螺仪多速率ckf融合姿态测量方法 Download PDFInfo
- Publication number
- CN107782309A CN107782309A CN201710861449.0A CN201710861449A CN107782309A CN 107782309 A CN107782309 A CN 107782309A CN 201710861449 A CN201710861449 A CN 201710861449A CN 107782309 A CN107782309 A CN 107782309A
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- coordinate system
- gyroscope
- msubsup
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Length Measuring Devices By Optical Means (AREA)
Abstract
本发明公开了一种非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法：建立测量***，定义坐标系；视觉姿态测量、惯性角速度差分计算及坐标系归一化；建立非线性卡尔曼滤波***模型；基于残差补偿的多速率CKF数据融合。本发明将单目视觉和双MEMS陀螺仪组合，并结合多传感器组合***采样频率不一致的固有问题，使用基于残差补偿的多速率CKF进行多传感器数据融合，保证测量***更新高频率输出的同时确保测量***的高精度及稳定性。
Description
技术领域
本发明涉及视觉姿态测量技术、惯性姿态测量技术、多传感器组合技术、多速率非线性滤波技术，更具体的说，是涉及一种非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法。
背景技术
从经典物理学的角度，除非物体保持静止或匀速直线运动否则其进行的是非惯性运动。对于车辆、飞机、轮船、运动模拟器等存在多种运动形式的运动载体，其进行的运动通常为非惯性运动，此时与这些载体固连的坐标系为非惯性系。
在载体保持静止的惯性坐标系中，基于单目视觉和单惯性传感器组合的测量***可完成静止载体上物体相对姿态的测量任务。此类组合测量***可发挥视觉测量精度高、重复性好的优点以及惯性传感器更新频率高、自主性强的特点，抑制惯性角度随时间漂移并且可避免单纯视觉测量易受环境光干扰、快速运动导致图像模糊造成无法测量等缺点，形成更新频率高、鲁棒性强、输出稳定的姿态测量***。
当载体运动形式多样时，单目视觉和单惯性器件组合的测量***将无法实现运动载体上物体相对姿态的测量。这是因为载体非惯性运动过程中的惯性力会作用在与其固连的物体上。惯性传感器的输出是相对空间惯性系的，在应用中一般需要将其安装在被测被测运动物体上。而微机械(Micro-Electro-Mechanical Systems，MEMS)惯性器件，因其体积小、重量轻、价格低获得越来越广泛的关注和应用，尤其当被测被测运动物体对承重有严格要求时。使用MEMS陀螺仪测量运动载体上被测运动物体相对参考系的相对运动姿态时，载体本身的运动成为有害运动并同样被安装在目标物上的陀螺仪感测。要获取纯净的被测运动物体的角速度需要将有害的载体运动部分消去。
发明内容
本发明的目的是为了克服现有技术中的不足，为实现非惯性系下运动载体上被测被测运动物体相对运动参考系的姿态测量，提供了一种非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法，将单目视觉和双MEMS陀螺仪组合，并结合多传感器组合***采样频率不一致的固有问题，使用基于残差补偿的多速率CKF进行多传感器数据融合，保证测量***更新高频率输出的同时确保测量***的高精度及稳定性。
本发明的目的是通过以下技术方案实现的。
本发明的非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法，包括以下步骤：
步骤一，建立测量***，定义坐标系：
测量***包括运动载体，所述运动载体上设置有被测运动物体和支架，所述被测运动物体上设置有主陀螺仪和立体靶标，所述支架上设置有辅助陀螺仪和摄像机，所述立体靶标面向摄像机形成由外向内的视觉测量结构；定义的坐标系包括空间惯性坐标系、摄像机坐标系、主陀螺仪坐标系、辅助陀螺仪坐标系、靶标坐标系和被测运动物体坐标系；
步骤二，视觉姿态测量、惯性角速度差分计算及坐标系归一化：
视觉姿态测量方法是使用摄像机拍摄被测运动物体上立体靶标的四个立体特征点，根据特征点在摄像机中成像坐标的变化，结合已知特征点在被测运动物体上的布局坐标，实现对被测运动物***姿的测量，视觉姿态测量结果是靶标坐标系相对摄像机坐标系的空间姿态关系；
将主陀螺仪和辅助陀螺仪输出角速度方向归一化之后，使用差分方法即将运动载体的有害运动量从主陀螺仪输出中除去，获取纯净的角速度，即为惯性测量结果；将视觉姿态测量结果和惯性测量结果均归一化到被测运动物体相对摄像机坐标系中；
步骤三，建立非线性卡尔曼滤波***模型：非线性卡尔曼滤波***模型由非线性状态模型和线性观测模型组成；
步骤四，基于残差补偿的多速率CKF数据融合：
当存在视觉数据时，进行CKF滤波计算；在视觉数据的采样间隔，在进行时间更新过程的同时，增加估计残差补偿步骤。
步骤一中所述空间惯性坐标系是指牛顿定律适用的非加速坐标系；所述摄像机坐标系是指原点在摄像机光学中心，z轴与镜头光轴方向一致；所述主陀螺仪坐标系是指与主陀螺仪本身固连的坐标系，用于输出主陀螺仪测量的角速度；所述辅助陀螺仪坐标系是指与辅助陀螺仪本身固连的坐标系，用于输出辅助陀螺仪测量的角速度；所述靶标坐标系是指三个特征点组成的主平面，中心点投射到主平面上的位置为坐标系的原点，z轴垂直于主平面由原点指向中心点；所述被测运动物体坐标系是指被测被测运动物体自身运动过程中形成进而建立的坐标系。
步骤二中所述惯性测量结果归一化到被测运动物体相对摄像机坐标系的空间位置变换关系式表示为：
所述视觉姿态测量结果归一化到被测运动物体相对摄像机坐标系的空间位置变换关系式表示为：
上述中，表示主陀螺仪和辅助陀螺仪之间随时间变化的旋转关系，表示立体靶标和摄像机之间随时间变化的旋转关系，表示主陀螺仪和被测运动物体之间的固定旋转关系，表示摄像机和辅助陀螺仪之间的固定旋转关系，表示立体靶标和被测运动物体之间的固定旋转矩阵。
步骤三中所述非线性状态模型的离散形式表示为：
xk＝Φk-1xk-1+Bk-1uk-1+wk-1
所述线性观测模型的离散形式表示为：
zk＝Hkxk+vk
上述中，xk为状态量，uk-1为角速度，zk为观测量；Φk-1为状态转移矩阵，Bk-1为输入控制矩阵，Hk为观测矩阵；wk-1和vk为不相关的零均值高斯过程噪声和零均值高斯观测噪声，方差分别为Qk-1和Rk。
步骤四中所述估计残差补偿指的是：
上述中，εk+i为估计的残差，αk+i为乘性调节因子，在标准CKF滤波过程中相当于滤波增益，是对状态量的估计。
步骤四中所述残差是时间变化的函数，需要随时间不断更新，当存在视觉数据时，对残差εk-1和状态误差ek-1分别进行更新：
当在视觉数据采样间隔，状态误差通过自更新过程进行传递，用公式表示为：
ek+i＝Φk+i|k+i-1ek+i-1
根据残差和状态误差之间的关系，残差用自更新后的状态误差表示为：
εk+i＝Hk+iΦk+i|k+i-1ek+i-1
上述中，是对状态量的估计，zk-1为观测量，Φk为状态转移矩阵，Ηk为观测矩阵，Kk为滤波增益。
与现有技术相比，本发明的技术方案所带来的有益效果是：
本发明提出了一种将单目视觉和双MEMS陀螺仪组合进行非惯性系下被测目标物体相对运动参考系的空间相对姿态测量的方法。结合多传感器***中存在的多频率问题，提出了基于多速率非线性卡尔曼滤波的数据融合方案。双MEMS陀螺仪组合惯性***可有效消除运动载体的运动信息获取表示被测运动物体运动的纯净角速度信息。根据测量模型的特点，定义了多个坐标系并进行了坐标系的旋转变换，将视觉测量和惯性测量归一化到同一个被测运动物体相对于摄像机坐标系坐标***中。使用多速率CKF当视觉数据可用时进行正常的滤波步骤。当视觉数据不可用时，进行时间更新结合估计残差补偿的滤波步骤。充分使用高频惯性数据同时保证滤波稳定输出。
在车辆、轮船、飞机、运动模拟器等运动载体上需要确定被测运动物体相对参考的运动载体的空间相对姿态时，可使用本发明中提出的测量模型和方法。测量***的基本组成为两个MEMS陀螺仪、摄像机、立体靶标。结构简单，可快速完成***搭建和初始标定，并用于非惯性系下相对姿态的测量。同时惯性器件固有的高频输出特点能够满足追踪快速运动目标的需求，多传感器融合增加***可靠性如视线遮挡造成某些时刻视觉数据不可用的问题。
附图说明
图1是本发明测量***的结构示意图；
图2是本发明测量***各坐标系定义以及归一化示意图；
图3是数据融合流程图示意图；
图4是多速率CKF数据融合示意图；
图5是滤波过程中残差更新原理图；
图6是测量***数据多速率对比图；
图7是测量中主陀螺仪和辅助陀螺仪输出角速度示意图；
图8是非惯性系下视觉和双陀螺仪***相对姿态测量结果示意图；
图9是非惯性系下视觉和双陀螺仪***相对姿态测量误差示意图。
附图标记:1运动载体；2被测运动物体；3主陀螺仪；
4立体靶标；5辅助陀螺仪；6摄像机；7支架。
具体实施方式
下面结合附图对本发明作进一步的描述。
本发明采用的方法是增加一个安装在载体上的辅助惯性器件。辅助陀螺仪感测载体运动信息，然后通过差分方法获取纯净的角速度信息并用于进一步的相对姿态计算。通常使用非线性卡尔曼滤波方法将视觉和惯性数据进行融合。在非线性条件下，容积卡尔曼滤波(Cubature Kalman Filter，CKF)在高斯滤波域中效果最好。因此本方案使用CKF将视觉和惯性数据进行融合。在视觉和惯性组合测量***中存在器件采样频率不一致的问题，并且通常视觉传感器采样频率低，而惯性器件采样频率高。为了解决采样数据频率不一致的问题本发明提出了基于残差补偿的多速率CKF数据融合方法，保证姿态高频率稳定输出。
本发明的非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法，具体过程如下：
(一)建立测量***，定义坐标系
本发明搭建的单目视觉和双MEMS陀螺仪非惯性系下运动载体上多速率数据融合姿态测量***模型示意图如图1所示。测量***包括运动载体1，运动载体1上设置有被测运动物体2和支架7，可用转台模拟被测运动物体2。所述被测运动物体2上固定设置有主陀螺仪3和立体靶标4，所述支架7上固定设置有辅助陀螺仪5和摄像机6，所述立体靶标4面向摄像机6形成由外向内的视觉测量结构。主陀螺仪3和辅助陀螺仪5均采用MEMS陀螺仪。摄像机6视场要足够大保证测量范围内可以拍摄到立体靶标4上完整的四个立体特征点。主陀螺仪3和立体靶标4分别和被测运动物体2刚性连接。辅助陀螺仪5和摄像机6分别通过支架7与运动载体1刚性连接。被测运动物体2可***以便进行测试。
测量***涉及多个传感器坐标系和计算坐标系，对各坐标系以及相互之间的转换关系进行了定义如图2所示，其中定义的六个右手坐标系分别为：空间惯性坐标系、摄像机坐标系、主陀螺仪坐标系、辅助陀螺仪坐标系、靶标坐标系和被测运动物体坐标系。
A)空间惯性坐标系，牛顿定律适用的非加速坐标系。在空间中固定，不随地球的自转而旋转，惯性器件输出的参考系。
B)摄像机坐标系，原点在摄像机光学中心，z轴与镜头光轴方向一致。
C)主陀螺仪坐标系，与主陀螺仪本身固连的坐标系，用于输出主陀螺仪测量的角速度。
D)辅助陀螺仪坐标系，与辅助陀螺仪本身固连的坐标系，用于输出辅助陀螺仪测量的角速度。
E)靶标坐标系，三个特征点组成的主平面，中心点投射到主平面上的位置为坐标系的原点，z轴垂直于主平面由原点指向中心点。
F)被测运动物体坐标系，被测被测运动物体自身运动过程中形成进而建立的坐标系。
(二)视觉姿态测量、惯性角速度差分计算及坐标系归一化
视觉姿态测量方法是使用摄像机拍摄被测运动物体上立体靶标的四个立体特征点，根据特征点在摄像机中成像坐标的变化，结合已知特征点在被测运动物体上的布局坐标，实现对被测运动物***姿的测量。视觉姿态测量结果是靶标坐标系相对摄像机坐标系的空间姿态关系。双MEMS陀螺仪组合形成惯性***。在运动载体上，主陀螺仪测量被测运动物体相对空间惯性坐标系的运动信息即角速度；辅助陀螺仪测量运动载体相对空间惯性坐标系的运动信息。其中，主陀螺仪的角速度不仅包含被测运动物体的运动，还包含运动载体的运动，而运动载体的运动部分对于被测运动物体姿态确定算法来说是有害的运动分量。要获取纯净的被测运动物体运动量，用于进一步的空间定位计算，需要从主陀螺仪的总角速度中除去有害的运动载体的运动量。而辅助陀螺仪测量的角速度即是运动载体的运动信息。将主陀螺仪和辅助陀螺仪输出角速度方向归一化之后，使用差分方法即将运动载体的有害运动量从主陀螺仪输出中除去，获取纯净的角速度，即为惯性测量结果。
从以上分析过程可知，测量***进行空间定位计算时，获取的姿态是主陀螺仪坐标系相对辅助陀螺仪坐标系的，与视觉空间定位计算分别表示在不同的坐标系中。在将两个测量值进行进一步融合之前需要将两个不同的测量归一化到同一个***中。从进行被测运动物体空间定位计算的最终目的考虑，本发明将两个测量值均归一化到被测运动物体相对运动载体坐标系中，考虑到摄像机或辅助陀螺仪和运动载体均是刚性连接，而且运动载体坐标系通常难以确定使得在需要进行坐标系之间的转换时难以进行，因此本发明将视觉姿态测量结果和惯性测量结果均归一化到被测运动物体相对摄像机坐标系中，如图2所示。
结合图1和图2，可知摄像机和辅助陀螺仪之间的空间位置关系是固定的，两者之间的旋转关系可表示为同理主陀螺仪和被测运动物体之间的固定旋转关系可表示为立体靶标和被测运动物体之间的固定旋转矩阵可表示为而立体靶标和摄像机之间的空间位置关系是随时间变化的，两者之间的旋转关系可表示为同理主陀螺仪和辅助陀螺仪之间随时间变化的旋转关系可表示为则将惯性测量结果归一化到被测运动物体相对摄像机坐标系的空间位置变换关系式可表示为：
即在惯性测量进行归一化过程中需要使用固定旋转矩阵和进行转换。
而将视觉姿态测量结果归一化到被测运动物体相对摄像机坐标系的空间位置变换关系式可表示为：
即在视觉姿态测量进行归一化过程中需要使用固定旋转矩阵进行转换。其中式子(1)和(2)中涉及的旋转矩阵均为3×3的矩阵，在本发明中除非特别指出，否则大写字母R均表示3×3的方向旋转矩阵。
(三)建立非线性卡尔曼滤波***模型
经过坐标系归一化之后的视觉和惯性量可进行数据融合计算最终姿态如图3所示。卡尔曼滤波随时间更新可不断循环迭代计算，使用卡尔曼滤波方法进行数据融合。在本发明中，视觉姿态在滤波循环之外，而惯性角速度包含在滤波循环内并作为滤波估计的输入。因此，建立非线性状态模型和线性的观测模型，共同构成非线性卡尔曼滤波***模型，***模型的离散形式可表示为：
xk＝Φk-1xk-1+Bk-1uk-1+wk-1 (3)
zk＝Hkxk+vk (4)
上述中，xk为状态量，uk-1为角速度，zk为观测量；Φk-1为状态转移矩阵，Bk-1为输入控制矩阵，Hk为观测矩阵；wk-1和vk为不相关的零均值高斯过程噪声和零均值高斯观测噪声，方差分别为Qk-1和Rk。
(四)基于残差补偿的多速率CKF数据融合
测量***中视觉数据更新频率低一般为几赫兹到几十赫兹，而惯性数据更新频率高一般为几十到几百赫兹，待融合数据存在更新频率不一致的问题。为了充分利用高频惯性视觉同时保证滤波结果的稳定性，使用基于残差补偿的多速率滤波方法进行多频率数据融合示意图如图4所示。在非线性高斯滤波域中，CKF具有最好的滤波效果因此本方案中使用CKF。从图4中可以看出当存在视觉数据时，进行正常的CKF滤波计算。不同的是在低频视觉数据的采样间隔，此时没有可用的视觉数据无法进行标准的滤波观测更新过程，但时间更新过程不受影响。在进行时间更新过程的同时，增加估计残差补偿步骤。估计残差补偿指的是：
上述中，εk+i为估计的残差，是对状态量的估计，αk+i为较小的乘性调节因子，在标准CKF滤波过程中相当于滤波增益。在视觉数据缺失时刻，滤波增益无法获得因此该乘性调节因子也是由估计得到。在实际实验过程中，增大或减小该乘性调节因子滤波会表现出发散或者重合于没有经过残差补偿只进行时间更新的滤波过程。因此乘性调节因子的最佳值通过几步简单测试即可得到。
残差是时间变化的函数，需要随时间不断更新。当视觉数据可用时即存在视觉数据，对残差εk-1和状态误差ek-1分别进行更新：
当视觉数据不可用时即在视觉数据采样间隔，状态误差通过自更新过程进行传递，用公式可表示为：
ek+i＝Φk+i|k+i-1ek+i-1 (8)
根据残差和状态误差之间的关系，残差可用自更新后的状态误差表示为：
εk+i＝Hk+iΦk+i|k+i-1ek+i-1 (9)
上述中，是对状态量的估计，zk-1为观测量，Φk为状态转移矩阵，Ηk为观测矩阵，Kk为滤波增益。
在视觉采样间隔时刻，残差可用于状态估计补偿，如公式(5)所示。具体更新过程如图5所示。
最终搭建的测量***的测量结果如图6、图7、图8、图9所示，其中图6为***测量中视觉、惯性以及多速率CKF更新频率的对比。图7为载体运动情况下即测量***处于非惯性系下，双陀螺仪各自的原始角速度测量输出。图8为使用纯净角速度和视觉相融合得到的最终的相对姿态测量结果。图9为使用纯净角速度和视觉相融合得到的最终的相对姿态测量误差。
尽管上面结合附图对本发明的功能及工作过程进行了描述，但本发明并不局限于上述的具体功能和工作过程，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可以做出很多形式，均属于本发明的保护之内。
Claims (6)
1.一种非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法，其特征在于，包括以下步骤：
步骤一，建立测量***，定义坐标系：
测量***包括运动载体，所述运动载体上设置有被测运动物体和支架，所述被测运动物体上设置有主陀螺仪和立体靶标，所述支架上设置有辅助陀螺仪和摄像机，所述立体靶标面向摄像机形成由外向内的视觉测量结构；定义的坐标系包括空间惯性坐标系、摄像机坐标系、主陀螺仪坐标系、辅助陀螺仪坐标系、靶标坐标系和被测运动物体坐标系；
步骤二，视觉姿态测量、惯性角速度差分计算及坐标系归一化：
视觉姿态测量方法是使用摄像机拍摄被测运动物体上立体靶标的四个立体特征点，根据特征点在摄像机中成像坐标的变化，结合已知特征点在被测运动物体上的布局坐标，实现对被测运动物***姿的测量，视觉姿态测量结果是靶标坐标系相对摄像机坐标系的空间姿态关系；
将主陀螺仪和辅助陀螺仪输出角速度方向归一化之后，使用差分方法即将运动载体的有害运动量从主陀螺仪输出中除去，获取纯净的角速度，即为惯性测量结果；将视觉姿态测量结果和惯性测量结果均归一化到被测运动物体相对摄像机坐标系中；
步骤三，建立非线性卡尔曼滤波***模型：非线性卡尔曼滤波***模型由非线性状态模型和线性观测模型组成；
步骤四，基于残差补偿的多速率CKF数据融合：
当存在视觉数据时，进行CKF滤波计算；在视觉数据的采样间隔，在进行时间更新过程的同时，增加估计残差补偿步骤。
2.根据权利要求1所述的非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法，其特征在于，步骤一中所述空间惯性坐标系是指牛顿定律适用的非加速坐标系；所述摄像机坐标系是指原点在摄像机光学中心，z轴与镜头光轴方向一致；所述主陀螺仪坐标系是指与主陀螺仪本身固连的坐标系，用于输出主陀螺仪测量的角速度；所述辅助陀螺仪坐标系是指与辅助陀螺仪本身固连的坐标系，用于输出辅助陀螺仪测量的角速度；所述靶标坐标系是指三个特征点组成的主平面，中心点投射到主平面上的位置为坐标系的原点，z轴垂直于主平面由原点指向中心点；所述被测运动物体坐标系是指被测被测运动物体自身运动过程中形成进而建立的坐标系。
3.根据权利要求1所述的非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法，其特征在于，步骤二中所述惯性测量结果归一化到被测运动物体相对摄像机坐标系的空间位置随时间的变换关系式表示为：
<mrow>
<msubsup>
<mi>R</mi>
<mi>c</mi>
<mi>b</mi>
</msubsup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>k</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msubsup>
<mi>R</mi>
<mi>m</mi>
<mi>b</mi>
</msubsup>
<msubsup>
<mi>R</mi>
<mi>s</mi>
<mi>m</mi>
</msubsup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>k</mi>
</msub>
<mo>)</mo>
</mrow>
<msubsup>
<mi>R</mi>
<mi>c</mi>
<mi>s</mi>
</msubsup>
</mrow>
所述视觉姿态测量结果归一化到被测运动物体相对摄像机坐标系的空间位置变换关系式表示为：
<mrow>
<msubsup>
<mi>R</mi>
<mi>c</mi>
<mi>b</mi>
</msubsup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>k</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msubsup>
<mi>R</mi>
<mi>t</mi>
<mi>b</mi>
</msubsup>
<msubsup>
<mi>R</mi>
<mi>c</mi>
<mi>t</mi>
</msubsup>
<mrow>
<mo>(</mo>
<msub>
<mi>t</mi>
<mi>k</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
上述中，表示主陀螺仪和辅助陀螺仪之间随时间变化的旋转关系，表示立体靶标和摄像机之间随时间变化的旋转关系，表示主陀螺仪和被测运动物体之间的固定旋转关系，表示摄像机和辅助陀螺仪之间的固定旋转关系，表示立体靶标和被测运动物体之间的固定旋转矩阵。
4.根据权利要求1所述的非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法，其特征在于，步骤三中所述非线性状态模型的离散形式表示为：
xk＝Φk-1xk-1+Bk-1uk-1+wk-1
所述线性观测模型的离散形式表示为：
zk＝Hkxk+vk
上述中，xk为状态量，uk-1为角速度，zk为观测量；Φk-1为状态转移矩阵，Bk-1为输入控制矩阵，Hk为观测矩阵；wk-1和vk为不相关的零均值高斯过程噪声和零均值高斯观测噪声，方差分别为Qk-1和Rk。
5.根据权利要求1所述的非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法，其特征在于，步骤四中所述估计残差补偿指的是：
<mrow>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
<mo>|</mo>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>&alpha;</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
</msub>
<mo>&CenterDot;</mo>
<msub>
<mi>&epsiv;</mi>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
</msub>
</mrow>
上述中，εk+i为估计的残差，αk+i为乘性调节因子，在标准CKF滤波过程中相当于滤波增益，是对状态量的估计。
6.根据权利要求1所述的非惯性系视觉和双陀螺仪多速率CKF融合姿态测量方法，其特征在于，步骤四中所述残差是时间变化的函数，需要随时间不断更新，当存在视觉数据时，对残差εk-1和状态误差ek-1分别进行更新：
<mrow>
<msub>
<mi>&epsiv;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<msub>
<mi>z</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>H</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<msub>
<mover>
<mi>x</mi>
<mo>^</mo>
</mover>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
<mo>|</mo>
<mi>k</mi>
<mo>-</mo>
<mn>2</mn>
</mrow>
</msub>
</mrow>
<mrow>
<msub>
<mi>e</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>=</mo>
<mrow>
<mo>(</mo>
<msup>
<mrow>
<mo>(</mo>
<mrow>
<msubsup>
<mi>H</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
<msub>
<mi>H</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
<mo>)</mo>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msup>
<msubsup>
<mi>H</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
<mi>T</mi>
</msubsup>
<mo>-</mo>
<msub>
<mi>K</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
<mo>)</mo>
</mrow>
<msub>
<mi>&epsiv;</mi>
<mrow>
<mi>k</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msub>
</mrow>
当在视觉数据采样间隔，状态误差通过自更新过程进行传递，用公式表示为：
ek+i＝Φk+i|k+i-1ek+i-1
根据残差和状态误差之间的关系，残差用自更新后的状态误差表示为：
εk+i＝Hk+iΦk+i|k+i-1ek+i-1
上述中，是对状态量的估计，zk-1为观测量，Φk为状态转移矩阵，Ηk为观测矩阵，Kk为滤波增益。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710861449.0A CN107782309A (zh) | 2017-09-21 | 2017-09-21 | 非惯性系视觉和双陀螺仪多速率ckf融合姿态测量方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710861449.0A CN107782309A (zh) | 2017-09-21 | 2017-09-21 | 非惯性系视觉和双陀螺仪多速率ckf融合姿态测量方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN107782309A true CN107782309A (zh) | 2018-03-09 |
Family
ID=61433557
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710861449.0A Pending CN107782309A (zh) | 2017-09-21 | 2017-09-21 | 非惯性系视觉和双陀螺仪多速率ckf融合姿态测量方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107782309A (zh) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109186592A (zh) * | 2018-08-31 | 2019-01-11 | 腾讯科技（深圳）有限公司 | 用于视觉惯导信息融合的方法和装置以及存储介质 |
| CN110640737A (zh) * | 2018-11-07 | 2020-01-03 | 宁波赛朗科技有限公司 | 一种数据融合姿态测量的工业机器人 |
| CN111220145A (zh) * | 2019-11-28 | 2020-06-02 | 湖南率为控制科技有限公司 | 用于自动驾驶汽车的多采样率下导航数据的融合方法 |
| CN112697074A (zh) * | 2020-12-10 | 2021-04-23 | 易思维（天津）科技有限公司 | 动态待测物角度测量仪及测量方法 |
| CN113008271A (zh) * | 2019-08-15 | 2021-06-22 | 深圳市瑞立视多媒体科技有限公司 | 校准3d旋转差异的数学模型构建方法、校准方法及其装置 |
| CN113776556A (zh) * | 2021-05-30 | 2021-12-10 | 南京理工大学 | 基于数据融合的陀螺仪与相机相对位置矩阵标定方法 |
| CN114383612A (zh) * | 2022-01-05 | 2022-04-22 | 中国兵器工业计算机应用技术研究所 | 一种视觉辅助惯性差分位姿测量*** |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102980579A (zh) * | 2012-11-15 | 2013-03-20 | 哈尔滨工程大学 | 一种自主水下航行器自主导航定位方法 |
| CN104655131A (zh) * | 2015-02-06 | 2015-05-27 | 东南大学 | 基于istssrckf的惯性导航初始对准方法 |
| CN105698765A (zh) * | 2016-02-22 | 2016-06-22 | 天津大学 | 双imu单目视觉组合测量非惯性系下目标物位姿方法 |
| CN106595640A (zh) * | 2016-12-27 | 2017-04-26 | 天津大学 | 基于双imu和视觉融合的动基座上物体相对姿态测量方法及*** |
| CN106767791A (zh) * | 2017-01-13 | 2017-05-31 | 东南大学 | 一种采用基于粒子群优化的ckf的惯性/视觉组合导航方法 |
-
2017
- 2017-09-21 CN CN201710861449.0A patent/CN107782309A/zh active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102980579A (zh) * | 2012-11-15 | 2013-03-20 | 哈尔滨工程大学 | 一种自主水下航行器自主导航定位方法 |
| CN104655131A (zh) * | 2015-02-06 | 2015-05-27 | 东南大学 | 基于istssrckf的惯性导航初始对准方法 |
| CN105698765A (zh) * | 2016-02-22 | 2016-06-22 | 天津大学 | 双imu单目视觉组合测量非惯性系下目标物位姿方法 |
| CN106595640A (zh) * | 2016-12-27 | 2017-04-26 | 天津大学 | 基于双imu和视觉融合的动基座上物体相对姿态测量方法及*** |
| CN106767791A (zh) * | 2017-01-13 | 2017-05-31 | 东南大学 | 一种采用基于粒子群优化的ckf的惯性/视觉组合导航方法 |
Non-Patent Citations (1)
| Title |
|---|
| GUO, XIAOTING, SUN, CHANGKU, WANG, PENG: "Multi-rate cubature Kalman filter based data fusion method with residual compensation to adapt to sampling rate discrepancy in attitude measurement system", 《REVIEW OF SCIENTIFIC INSTRUMENTS》 * |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109186592A (zh) * | 2018-08-31 | 2019-01-11 | 腾讯科技（深圳）有限公司 | 用于视觉惯导信息融合的方法和装置以及存储介质 |
| CN110640737A (zh) * | 2018-11-07 | 2020-01-03 | 宁波赛朗科技有限公司 | 一种数据融合姿态测量的工业机器人 |
| CN113008271A (zh) * | 2019-08-15 | 2021-06-22 | 深圳市瑞立视多媒体科技有限公司 | 校准3d旋转差异的数学模型构建方法、校准方法及其装置 |
| CN111220145A (zh) * | 2019-11-28 | 2020-06-02 | 湖南率为控制科技有限公司 | 用于自动驾驶汽车的多采样率下导航数据的融合方法 |
| CN112697074A (zh) * | 2020-12-10 | 2021-04-23 | 易思维（天津）科技有限公司 | 动态待测物角度测量仪及测量方法 |
| CN112697074B (zh) * | 2020-12-10 | 2022-07-15 | 易思维（天津）科技有限公司 | 动态待测物角度测量仪及测量方法 |
| CN113776556A (zh) * | 2021-05-30 | 2021-12-10 | 南京理工大学 | 基于数据融合的陀螺仪与相机相对位置矩阵标定方法 |
| CN113776556B (zh) * | 2021-05-30 | 2024-05-07 | 南京理工大学 | 基于数据融合的陀螺仪与相机相对位置矩阵标定方法 |
| CN114383612A (zh) * | 2022-01-05 | 2022-04-22 | 中国兵器工业计算机应用技术研究所 | 一种视觉辅助惯性差分位姿测量*** |
| CN114383612B (zh) * | 2022-01-05 | 2024-04-12 | 中国兵器工业计算机应用技术研究所 | 一种视觉辅助惯性差分位姿测量*** |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107782309A (zh) | 非惯性系视觉和双陀螺仪多速率ckf融合姿态测量方法 | |
| CN104658012B (zh) | 一种基于惯性与光学测量融合的运动捕捉方法 | |
| CN105698765B (zh) | 双imu单目视觉组合测量非惯性系下目标物位姿方法 | |
| Yun et al. | Design, implementation, and experimental results of a quaternion-based Kalman filter for human body motion tracking | |
| CN109141433A (zh) | 一种机器人室内定位***及定位方法 | |
| CN106052584B (zh) | 一种基于视觉及惯性信息融合的轨道空间线形测量方法 | |
| CN106052685B (zh) | 一种两级分离融合的姿态和航向估计方法 | |
| CN110095116A (zh) | 一种基于lift的视觉定位和惯性导航组合的定位方法 | |
| CN109540126A (zh) | 一种基于光流法的惯性视觉组合导航方法 | |
| CN108036785A (zh) | 一种基于直接法与惯导融合的飞行器位姿估计方法 | |
| CN105953796A (zh) | 智能手机单目和imu融合的稳定运动跟踪方法和装置 | |
| CN108846857A (zh) | 视觉里程计的测量方法及视觉里程计 | |
| CN106056664A (zh) | 一种基于惯性和深度视觉的实时三维场景重构***及方法 | |
| CN110398257A (zh) | Gps辅助的sins***快速动基座初始对准方法 | |
| CN108170297B (zh) | 实时六自由度vr/ar/mr设备定位方法 | |
| CN106482746B (zh) | 一种用于混合式惯导***的加速度计内杆臂标定与补偿方法 | |
| CN105606127A (zh) | 一种双目立体相机与惯性测量单元相对姿态标定方法 | |
| Sun et al. | Adaptive sensor data fusion in motion capture | |
| CN110455301A (zh) | 一种基于惯性测量单元的动态场景slam方法 | |
| CN105953795B (zh) | 一种用于航天器表面巡视的导航装置及方法 | |
| CN107621266B (zh) | 基于特征点跟踪的空间非合作目标相对导航方法 | |
| CN103027657A (zh) | 基于多传感器的内窥镜跟踪定位与数字人动态同步显示方法 | |
| CN103940442A (zh) | 一种采用加速收敛算法的定位方法及装置 | |
| CN107167131A (zh) | 一种微惯性测量信息的深度融合与实时补偿的方法及*** | |
| CN107728182A (zh) | 基于相机辅助的柔性多基线测量方法和装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20180309 |
|
| WD01 | Invention patent application deemed withdrawn after publication |