[发明专利]一种视觉惯性系统的误差修正方法和装置

权利要求书

1.一种视觉惯性系统的误差修正方法，其特征在于，包括：

根据统计数据判断当前视觉惯性系统是否处于静止状态，其中，所述统计数据包括下述一项或多项：所述当前视觉惯性系统中惯性测量单元输出的方差、预设时间内的标准速度或所述当前视觉惯性系统中的摄像头所采集图像中特征点的位移；

如果所述当前视觉惯性系统处于静止状态，则设定当前视觉惯性系统的三向速度为零，且位置和航向均不变，并获取所述当前视觉惯性系统进入所述静止状态时各状态属性的参数值作为观测值，并获取各观测值分别所对应的预测值，其中，所述状态属性至少包括速度、位置和航向；

对于任意一个状态属性，利用该状态属性的观测值对其预测值进行修正；

其中，对于任意一个状态属性，利用该状态属性的观测值对其预测值进行修正，包括：

对于任意一个状态属性，将该状态属性的观测值与其预测值作差，得到估计误差；利用所述估计误差对该状态属性的预测值进行修正；

其中，若VIO系统估计系统速度，则其量测方程为H_v(X_k)＝v_k，此时，速度估计误差若VIO系统估计系统位置，则其量测方程为H_p(X_k)＝p_k，此时，位置估计误差其中，p_static为系统进入静止状态时记录下的位置；若VIO系统估计系统速度，则其量测方程为H_y(X_k)＝y_k，此时，航向估计误差为其中，y_static为系统进入静止状态时记录下的航向；

其中，所述根据统计数据判断当前视觉惯性系统是否处于静止状态，包括：

判断当前VIO系统中惯性测量单元输出的方差是否低于设定方差阈值，如果低于设定方差阈值，则确定当前VIO系统处于静止状态；或者，

在设定时间内判断当前VIO系统的最大速度是否不超过预设速度，如果不超过预设速度，则确定当前VIO系统处于静止状态；或者，

判断当前VIO系统中的摄像头所采集图像中特征点的位移是否超过设定的像素阈值，如果不超过设定的像素阈值，则确定当前VIO系统处于静止状态。

2.根据权利要求1所述的方法，其特征在于，利用所述估计误差对该状态属性的预测值进行修正，包括：

基于卡尔曼滤波或非线性优化算法，利用所述估计误差对该状态属性的预测值进行修正。

3.根据权利要求1所述的方法，其特征在于，如果所述当前视觉惯性系统处于静止状态，则获取所述当前视觉惯性系统进入所述静止状态时各状态属性的参数值作为观测值，包括：

如果所述当前视觉惯性系统第一次进入静止状态，则分别将获取的所述当前视觉惯性系统进入所述静止状态时的速度、位置和航向的参数值作为观测值。