CN108253963A - 一种基于多传感器融合的机器人自抗扰定位方法以及定位系统 - Google Patents

Abstract

本发明涉及一种基于多传感器融合的机器人自抗扰定位方法，所述多传感器包括双目视觉里程计、轮式里程计以及惯导，所述定位方法包括以下步骤：步骤S1、获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β；步骤S2、设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β)；步骤S3、若α≥Threshold(α)且β≥Threshold(β)时，采用视觉惯导融合定位，否则采用轮式惯导融合定位。本发明提供的机器人自抗扰定位方法融合多种传感器的优势，优化机器人定位的计算方法,消除定位过程中出现的误差。

Description

一种基于多传感器融合的机器人自抗扰定位方法以及定位 系统

技术领域

本发明涉及机器人定位技术领域，尤其涉及一种基于多传感器融合的机器人自抗扰定位方法以及定位系统。

背景技术

传统的机器人定位方法是主要是依靠惯导和轮式里程计等来实现。其中航位推测法是最基本的定位方法,主要工作原理是通过运动估计推算移动机器人的位置信息。具体来讲是基于轮式编码器原理,主要通过测量轮子的旋转速率和轮子转的圈数来确定车轮的运动速度,所以有着短期精度高的特点,但也存在问题,只有在水平面进行运动时能够实现运动估计,无法计算垂直方向的变化,不能克服车轮打滑时引起得误差。同时,惯性测量单元结构上主要由三轴加速度计和三轴陀螺仪构成,通过对加速度和角速率进行积分计算机器人的姿态。由于惯性导航可以独立工作,能够依据自身测量数据完成姿态解算和航位推算,但由于在航位计算过程中,先要对加速度积分得到速度再对速度积分得到位移即要进行两次积分所以存在累计误差。

如今，越来越多的研究将视觉定位方法作为传统定位方法的补充，视觉定位方法适用于室内外、陆地及星际等多种环境，且定位精度高，但对图像序列质量要求高，图像中的移动物体、阴影都会影响其运动估计的准确性，而且外界环境，如光线和剧烈抖动也会严重影响图像的质量，从而导致定位精度下降。

因此,单一传感器的定位效果都会有各种各样的局限。

发明内容

本发明所要解决的技术问题是针对上述现有技术的不足，提供一种基于多传感器融合的机器人自抗扰定位方法，通过多传感器融合的思想来进一步优化机器人定位的计算方法,将不同传感器的优势集中起来,以消除定位过程中出现的误差问题。

本发明解决上述技术问题的技术方案如下：一种基于多传感器融合的机器人自抗扰定位方法，所述多传感器包括双目视觉里程计、轮式里程计以及惯导，所述定位方法包括以下步骤：

步骤S1、获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β；

步骤S2、设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β)；

步骤S3、若α≥Threshold(α)且β≥Threshold(β)时，采用视觉惯导融合定位，否则采用轮式惯导融合定位。

本发明的有益效果是：在左右特征匹配点对数α和前后跟踪匹配点对数β多时，视觉惯导融合定位的精度高，此时采用视觉惯导融合定位，当左右特征匹配点对数α和前后跟踪匹配点对数β少时，视觉惯导融合定位的精度低，此时采用轮式惯导融合定位。不同情况下两种融合定位算法的协同定位，主要以视觉惯导融合定位为主，以轮式惯导融合定位为辅，实现机器人长期和稳定的定位。

在上述技术方案的基础上，本发明还可以做如下改进：

进一步，所述视觉惯导融合定位具体包括以下子步骤：

步骤S31、通过所述双目视觉里程计获取双目图像，并根据所述双目图像获取旋转矩阵以及平移矩阵；

步骤S32、通过所述惯导获取角速度以及三轴加速度，并根据所述角速度和三轴加速度计算姿态角信息；

步骤S33、建立扩展卡尔曼滤波模型，并通过所述扩展卡尔曼滤波模型对所述姿态角信息以及所述旋转矩阵进行信息融合，计算出融合旋转矩阵，并结合所述平移矩阵以及所述融合旋转矩阵得到视觉惯导融合定位信息。

上述进一步方案的有益效果是：双目视觉里程计的定位精度高，但容易收到外界干扰，惯导数据稳定，但累计误差较为严重，本发明利用扩展卡尔曼滤波模型对双目视觉里程计和惯导进行融合互补，提高了定位精度和定位的稳定性。

进一步，所述子步骤S31进一步包括以下子步骤：

步骤S311、设定所述双目视觉里程计的双目相机平行安装，通过所述双目相机获取双目图像，对所述双目图像的特征点进行匹配，利用视差计算出所述特征点的三维坐标；

步骤S312、以所述双目相机的左相机为基准，建立相机坐标系下前后时刻对应特征点的运动方程组:

P_k＝R P_k-1+T

式中:P_k表示k时刻所述特征点在相机坐标系中的三维坐标值，P_k-1表示k-1时刻所述特征点在相机坐标系中的三维坐标值，R表示旋转矩阵，T表示平移矩阵；

去除所述特征点中的离群值并用最小二乘法求解所述运动方程组得到旋转矩阵R以及平移矩阵T。

上述进一步方案的有益效果是：通过双目视觉里程计获取旋转矩阵R以及平移矩阵T，以便与惯导获取的姿态角信息进行融合。

进一步，所述姿态角信息包括俯仰角θ、侧倾角φ以及航向角ψ，所述子步骤S32进一步包括以下子步骤：

步骤S321、通过所述惯导获取三轴加速度a_x、a_y以及a_z，仅重力作用时所述三轴加速度a_x、a_y以及a_z与俯仰角θ以及侧倾角φ之间满足以下关系式：

其中，g为重力加速度,求解上式得到俯仰角θ和侧倾角φ；

步骤S322、通过所述惯导获取角速度ω_g，在k时刻至k+1时刻之间的时间段内对角速度ω_g积分，得到航向角变化量Δψ，再将所述航向角变化量Δψ累加得到航向角ψ。

上述进一步方案的有益效果是：获取惯导输出的三轴加速度a_x、a_y以及a_z,以重力加速度g作为垂直方向参考计算机器人实时的俯仰角θ和侧倾角φ,避免了利用角速度ω_g积分求解俯仰角θ和侧倾角φ带来的累积误差

进一步，所述子步骤S33进一步包括以下子步骤：

步骤S331、利用所述姿态角信息计算姿态四元素e₀、e₁、e₂以及e₃，定义所述扩展卡尔曼滤波模型的系统状态变量D＝[e₀ e₁ e₂ e₃]^T，并建立系统的状态方程：

D_k+1＝q*D_k+w(k)

其中，D_K表示k时刻的状态变量，D_K+1表示k+1时刻的状态变量，q表示k时刻到k+1时刻机器人的旋转向量，q由旋转矩阵R计算得到，*为四元素乘法，w(k)表示所述状态方程的噪声向量，求解所述状态方程得到k+1时刻的状态变量D_k+1；

步骤S332、建立观测方程：

其中，v(k)表示观测过程的高斯噪声矩阵，求解所述观测方程以及状态方程得到所述扩展卡尔曼滤波模型的融合姿态角，所述融合姿态角包括融合俯仰角θ_k、融合侧倾角φ_k以及融合航向角变化量Δψ_k，利用所述融合姿态角计算融合旋转矩阵R1；

步骤S333、结合所述平移矩阵T以及融合旋转矩阵R1得到导航坐标系下的融合定位信息推算方程：

其中，表示k+1时刻载体在导航坐标系的三维坐标，为k时刻载体在导航坐标系的三维坐标，是累积前k时刻融合旋转矩阵R1的结果，T_k,k+1表k时刻到k+1时刻的平移矩阵。

上述进一步方案的有益效果是：采用扩展卡尔曼滤波模型实现双目视觉里程计的旋转矩阵与惯导的姿态角信息的信息融合,并结合双目视觉里程计平移矩阵实现视觉惯导融合定位。

进一步，所述轮式惯导融合定位包括以下子步骤：

步骤S41、通过轮式里程计获取k时刻到k+1时刻机器人的左轮的运动路程S1_k和机器人的右轮的运动路程S2_k；

步骤S42、通过所述惯导获取角速度ω_g以及三轴加速度，并根据所述三轴加速度计算俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标z_k；

步骤S43、建立离散卡尔曼滤波模型，并通过所述离散卡尔曼滤波模型对所述运动路程S1_k、运动路程S2_k以及角速度ω_g进行信息融合，生成融合位置信息，结合所述融合位置信息、俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标z_k得到轮式惯导融合定位信息。

上述进一步方案的有益效果是：单纯依靠轮式里程计进行定位的精度比较差，采用离散卡尔曼滤波模型将惯导定位和轮式里程计定位进行融合，提高定位精度。

进一步，所述子步骤S43进一步包括以下子步骤：

步骤S431、定义所述离散卡尔曼滤波模型的系统状态变量X＝[S1_k S2_k ω_g]^T，并建立系统状态方程：

X_k+1＝A_k+1|kX_k+W_k

其中，X_k为k时刻的系统状态变量，X_k+1为k+1时刻的系统状态变量，A_k+1|k为k时刻到k+1时刻的状态转移矩阵，W_k为系统状态方程的噪声矩阵，求解所述系统状态方程得到k+1时刻的系统状态变量X_k+1；

步骤S432、建立系统测量方程：

N_k＝H_kX_k+v_k

其中，H_k为系统量测矩阵并可从所述惯导以及所述轮式里程计中直接读出，v_k为所述系统量测方程的噪声矩阵，求解所述系统状态方程以及系统测量方程得到左轮的融合运动路程S3_k、右轮的融合运动路程S4_k以及融合航向角变化量Δψ1_k，累加所述航向角变化量Δψ1_k得到k时刻融合航向角ψ1_k；

步骤S433、建立融合定位信息推算方程：

其中，l为所述左轮与右轮之间的距离，Δψ1_k+1为k到k+1时刻间航向角变化量，x_k为机器人在导航坐标系中的X轴坐标,y_k为机器人在导航坐标系中的Y轴坐标,r_k＝S3_kl/(S4_k-S3_k)，求解所述融合位置信息推算方程得到融合位置信息Q_k；

步骤S434、结合所述融合位置信息Q_k、俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标z_k得到轮式惯导融合定位信息。

上述进一步方案的有益效果是：通过惯导的陀螺仪获取角速度，通过轮式里程计获取左轮的运动路程和右轮的运动路程，并利用离散卡尔曼滤波模型对角速度、左轮的运动路程以及右轮的运动路程进行融合生成融合位置信息，以提高定位精度。

本发明还提供一种基于多传感器融合的机器人自抗扰定位系统，所述多传感器包括双目视觉里程计、轮式里程计以及惯导，所述定位系统包括：

获取模块，所述获取模块连接所述双目视觉里程计，用于获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β；

控制模块，所述控制模块连接所述获取模块，用于设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β)；

所述双目视觉里程计、轮式里程计以及惯导均与所述控制模块连接；

所述控制模块还用于比较α与Threshold(α)以及β与Threshold(β)，若α≥Threshold(α)且β≥Threshold(β)时，所述控制模块控制所述双目视觉历程计以及惯导进行融合定位，否则控制所述轮式里程计以及惯导进行融合定位。

本发明提供的定位系统的有益效果与定位方法的有益效果相同，在此不再赘述。

在上述技术方案的基础上，本发明还可以做如下改进：

进一步，所述定位系统还包括扩展卡尔曼滤波器，所述双目视觉里程计以及所述惯导均与所述扩展卡尔曼滤波器连接；

所述双目视觉里程计用于获取双目图像，并根据所述双目图像获取旋转矩阵以及平移矩阵，所述惯导用于获取角速度以及三轴加速度，并根据所述角速度和三轴加速度计算姿态角信息，所述扩展卡尔曼滤波器用于对所述姿态角信息以及所述旋转矩阵进行信息融合，计算出融合旋转矩阵，并结合所述平移矩阵以及所述融合旋转矩阵得到视觉惯导融合定位信息。

进一步，所述定位系统还包括离散卡尔曼滤波器，所述轮式里程计以及所述惯导均与所述离散卡尔曼滤波器连接；

所述轮式里程计用于获取机器人的运动路程，所述惯导用于获取角速度以及三轴加速度，并根据所述三轴加速度计算俯仰角、侧倾角以及机器人在导航坐标系内的Z轴坐标，所述离散卡尔曼滤波器用于对所述运动路程以及角速度进行信息融合，生成融合位置信息，结合所述融合位置信息、俯仰角、侧倾角以及机器人在导航坐标系内的Z轴坐标得到轮式惯导融合定位信息。

附图说明

图1为本发明提供的一种基于多传感器融合的机器人自抗扰定位方法的流程图；

图2为本发明提供的一种基于多传感器融合的机器人自抗扰定位系统的结构示意图；

图3为本发明提供的一种基于多传感器融合的机器人自抗扰定位系统的扩展卡尔曼滤波器以及离散卡尔曼滤波器的连接关系示意图。

附图中，各标号所代表的部件列表如下：

1、双目视觉里程计，2、轮式里程计，3、惯导，4、获取模块，5、控制模块，6、扩展卡尔曼滤波器，7、离散卡尔曼滤波器。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。

下面结合附图，对本发明进行说明。

实施例一

如图1所示，本发明实施例提供一种基于多传感器融合的机器人自抗扰定位方法(以下简称定位方法)，所述多传感器包括双目视觉里程计、轮式里程计以及惯导，所述定位方法包括以下步骤：

步骤S1、获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β；

步骤S2、设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β)；

步骤S3、若α≥Threshold(α)且β≥Threshold(β)时，采用视觉惯导融合定位，否则采用轮式惯导融合定位。

双目视觉里程计精确程度非常依赖于较高的图像序列质量和足够数目的特征点匹配组，拍摄环境光线分布不均、双目视觉里程计出现碰撞、拍摄环境存在大量的白墙等情况都会导致双目视觉里程计匹配的特征点减少，而且误匹配的机率增大，甚至会导致双目视觉里程计的失效，定位精度下降。针对上述情况，本发明在双目视觉里程计失效的情况下，不再使用视觉惯导融合定位，而转为轮式惯导融合定位做一个短时补充，当双目视觉里程计恢复正常后，再重新使用视觉惯导融合定位，这样就能保证机器人长期和稳定的定位精度。

本发明针对视觉惯导融合定位过程中双目视觉里程计在外界环境干扰下容易失效和失真以及在视角内特征点稀少时定位不精确的情况，利用轮式惯导融合定位短期精确度高的优点，将其作为视觉惯导融合定位的补充，以实现机器人长期和稳定的定位。

优化上一实施例，所述视觉惯导融合定位具体包括以下子步骤：

步骤S31、通过所述双目视觉里程计获取双目图像，并根据所述双目图像获取旋转矩阵以及平移矩阵；

步骤S32、通过所述惯导获取角速度以及三轴加速度，并根据所述角速度和三轴加速度计算姿态角信息；

步骤S33、建立扩展卡尔曼滤波模型，并通过所述扩展卡尔曼滤波模型对所述姿态角信息以及所述旋转矩阵进行信息融合，计算出融合旋转矩阵，并结合所述平移矩阵以及所述融合旋转矩阵得到视觉惯导融合定位信息。

双目视觉里程计的定位精度高，但容易收到外界干扰，惯导数据稳定，但累计误差较为严重，本发明实施例利用扩展卡尔曼滤波模型对双目视觉里程计和惯导进行融合互补，提高了定位精度和定位的稳定性。

优化上一实施例，所述子步骤S31进一步包括以下子步骤：

步骤S311、设定所述双目视觉里程计的双目相机平行安装，通过所述双目相机获取双目图像，对所述双目图像的特征点进行匹配，利用视差计算出所述特征点的三维坐标；

步骤S312、以所述双目相机的左相机为基准，建立相机坐标系下前后时刻对应特征点的运动方程组:

P_k＝R P_k-1+T

式中:P_k表示k时刻所述特征点在相机坐标系中的三维坐标值，P_k-1表示k-1时刻所述特征点在相机坐标系中的三维坐标值，R表示旋转矩阵，T表示平移矩阵；

去除所述特征点中的离群值并用最小二乘法求解所述运动方程组得到旋转矩阵R以及平移矩阵T。

所述特征点包括:点状特征、线状特征和区域特征等。一般情况下,特征点的尺度越大，所包含的图像信息就越丰富,匹配所需特征点的数目也就越少,匹配速度越快。但是特征点的尺度越大，特征点的提取和描述也越复杂,定位精度越低。相反的，特征尺度越小，特征点描述相对越简单,定位精度越高，但是匹配所需数目偏多,所包含信息也比较少。因此选择合适的特征点提取方法至关重要。

为了得到精确的特征点的位置,首先对双目图像进行滤波处理，滤波处理方法包括5*5blob和角点标记等。对滤波后的双目图像使用非极大抑制和非极小抑制算法,产生候选的特征点,候选的特征点主要包括blob最大、blob最小、角点最大和角点最小四类。为了减少计算量,仅对上述四类特征点进行匹配。匹配时将特征点与无序的图像序列集进行对比,假设相机轨迹平滑,则不需要像SURF算法一样计算旋转尺度不变性特征描述符。选定两个特征点,特征点的窗口分为多个子窗口，计算每个子窗口的Sobel滤波响应,并将灰度差绝对值之和作为误差度量。

由于16个字节的灰度差绝对值之和可通过一种简单的SSE指令有效的计算,只需要两次调用水平和垂直方向的Sobel响应即可，因此为了加速提取匹配过程,将Sobel滤波响应量化为8位并采用稀疏特征点的方式,只计算中16个子窗口处的灰度差之和,来取代计算整个窗口中所有子窗口处的灰度差绝对值之和。

优化上一实施例，所述姿态角信息包括俯仰角θ、侧倾角φ以及航向角ψ，所述子步骤S32进一步包括以下子步骤：

步骤S321、通过所述惯导获取三轴加速度a_x、a_y以及a_z，仅重力作用时所述三轴加速度a_x、a_y以及a_z与俯仰角θ以及侧倾角φ之间满足以下关系式：

其中，g为重力加速度,求解上式得到俯仰角θ和侧倾角φ；

步骤S322、通过所述惯导获取角速度ω_g，在k时刻至k+1时刻之间的时间段内对角速度ω_g积分，得到航向角变化量Δψ，再将所述航向角变化量Δψ累加得到航向角ψ。

惯导由陀螺仪和加速度计两部分构成，通过加速度计获取三轴加速度a_x、a_y以及a_z，以重力加速度g作为垂直方向参考,计算机器人实时的俯仰角θ和侧倾角φ,这种方法的优势在于避免了利用角速度ω_g积分计算俯仰角θ和侧倾角φ带来的累积误差。但这种方法不能计算航向角ψ,因此利用陀螺仪获取角速度ω_g，对角速度ω_g积分获取航向角ψ。用陀螺仪估计的ψ与加速度计估计的θ和φ修正双目视觉里程计的姿态估计，可以使得姿态角估计解耦,降低累积误差的影响。

优化上一实施例，所述子步骤S33进一步包括以下子步骤：

步骤S331、利用所述姿态角信息计算姿态四元素e₀、e₁、e₂以及e₃，定义所述扩展卡尔曼滤波模型的系统状态变量D＝[e₀e₁e₂e₃]^T，并建立系统的状态方程：

D_k+1＝q*D_k+w(k)

其中，D_K表示k时刻的状态变量，D_K+1表示k+1时刻的状态变量，q表示k时刻到k+1时刻机器人的旋转向量，q由旋转矩阵R计算得到，*为四元素乘法，w(k)表示所述状态方程的噪声向量，求解所述状态方程得到k+1时刻的状态变量D_k+1；

步骤S332、建立观测方程：

其中，v(k)表示观测过程的高斯噪声矩阵，求解所述观测方程以及状态方程得到所述扩展卡尔曼滤波模型的融合姿态角，所述融合姿态角包括融合俯仰角θ_k、融合侧倾角φ_k以及融合航向角变化量Δψ_k，利用所述融合姿态角计算融合旋转矩阵R1；

步骤S333、结合所述平移矩阵T以及融合旋转矩阵R1得到导航坐标系下的融合定位信息推算方程：

其中，表示k+1时刻载体在导航坐标系的三维坐标，为k时刻载体在导航坐标系的三维坐标，是累积前k时刻融合旋转矩阵R1的结果，T_k,k+1表k时刻到k+1时刻的平移矩阵。

本方法采用扩展卡尔曼滤波模型实现双目视觉里程计的旋转矩阵和惯导的姿态角信息的融合,并结合双目视觉里程计的平移矩阵实现定位。

优化上一实施例，所述轮式惯导融合定位包括以下子步骤：

步骤S41、通过轮式里程计获取k时刻到k+1时刻机器人的左轮的运动路程S1_k和机器人的右轮的运动路程S2_k；

步骤S42、通过所述惯导获取角速度ω_g以及三轴加速度，并根据所述三轴加速度计算俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标z_k；

步骤S43、建立离散卡尔曼滤波模型，并通过所述离散卡尔曼滤波模型对所述运动路程S1_k、运动路程S2_k以及角速度ω_g进行信息融合，生成融合位置信息，结合所述融合位置信息、俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标z_k得到轮式惯导融合定位信息。

由双轮差动机器人的运动模型可知，单纯依靠轮式里程计获得的数据推算移动机器人的定位的精度比较差，采用卡尔曼滤波器将惯导的陀螺仪和轮式里程计获得的数据进行融合能提高定位精度。该系统是线性，采用离散卡尔曼滤波算法对轮式里程计和陀螺仪采集的信息进行融合。

优化上一实施例，所述子步骤S43进一步包括以下子步骤：

步骤S431、定义所述离散卡尔曼滤波模型的系统状态变量X＝[S1_k S2_k ω_g]^T，并建立系统状态方程：

X_k+1＝A_k+1|kX_k+W_k

其中，X_k为k时刻的系统状态变量，X_k+1为k+1时刻的系统状态变量，A_k+1|k为k时刻到k+1时刻的状态转移矩阵，W_k为系统状态方程的噪声矩阵，求解所述系统状态方程得到k+1时刻的系统状态变量X_k+1；

步骤S432、建立系统测量方程：

N_k＝H_kX_k+v_k

其中，H_k为系统量测矩阵并可从所述惯导以及所述轮式里程计中直接读出，v_k为所述系统量测方程的噪声矩阵，求解所述系统状态方程以及系统测量方程得到左轮的融合运动路程S3_k、右轮的融合运动路程S4_k以及融合航向角变化量Δψ1_k，累加所述航向角变化量Δψ1_k得到k时刻融合航向角ψ1_k；

步骤S433、建立融合定位信息推算方程：

其中，l为所述左轮与右轮之间的距离，Δψ1_k+1为k到k+1时刻间航向角变化量，x_k为机器人在导航坐标系中的X轴坐标,y_k为机器人在导航坐标系中的Y轴坐标,r_k＝S3_kl/(S4_k-S3_k)，求解所述融合位置信息推算方程得到融合位置信息Q_k；

步骤S434、结合所述融合位置信息Q_k、俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标z_k得到轮式惯导融合定位信息。

本实施例通过建立离散卡尔曼滤波模型求解出融合位置信息Q_k，并结合融合位置信息Q_k、俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标z_k得到轮式惯导融合定位信息。俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标z_k均可通过惯导直接测量计算得到，其中俯仰角θ和侧倾角φ优选使用步骤S321中方法计算得到。

实施例二

如图2所示，本发明实施例提供一种基于多传感器融合的机器人自抗扰定位系统(以下简称定位系统)，所述多传感器包括双目视觉里程计1、轮式里程计2以及惯导3，所述定位系统包括：

获取模块4，所述获取模块4连接所述双目视觉里程计1，用于获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β；

控制模块5，所述控制模块5连接所述获取模块4，用于设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β)；

所述双目视觉里程计1、轮式里程计2以及惯导3均与所述控制模块5连接；

所述控制模块5还用于比较α与Threshold(α)以及β与Threshold(β)，当α≥Threshold(α)且β≥Threshold(β)时，所述控制模块5控制所述双目视觉历程计1以及惯导3进行定位，否则控制所述轮式里程计2以及惯导3进行定位。

本实施例提供的定位系统通过获取模块4获取左右特征匹配点对数α和前后跟踪匹配点对数β，通过控制模块5比较α与Threshold(α)以及β与Threshold(β)，并根据比较结果选择不同的定位方法，当α≥Threshold(α)且β≥Threshold(β)时，控制模块5驱动双目视觉历程计1以及惯导3进行视觉惯导融合定位，否则，控制模块5驱动轮式里程计2以及惯导3进行轮式惯导融合定位。

本发明实施例提供的定位系统针对视觉惯导融合定位过程中双目视觉里程计在外界环境干扰下容易失效和失真以及在视角内特征点稀少时定位不精确的情况，利用轮式惯导融合定位短期精确度高的优点，将轮式惯导融合定位作为视觉惯导融合定位的补充，以实现机器人长期和稳定的定位效果。

优化上一实施例，如图3所示，所述定位系统还包括扩展卡尔曼滤波器6，所述双目视觉里程计1以及所述惯导3均与所述扩展卡尔曼滤波器6连接；

所述双目视觉里程计1用于获取双目图像，并根据所述双目图像获取旋转矩阵以及平移矩阵，所述惯导3用于获取角速度以及三轴加速度，并根据所述角速度和三轴加速度计算姿态角信息，所述扩展卡尔曼滤波器6用于对所述姿态角信息以及所述旋转矩阵进行信息融合，计算出融合旋转矩阵，并结合所述平移矩阵以及所述融合旋转矩阵得到视觉惯导融合定位信息。

利用扩展卡尔曼滤波器6实现双目视觉里程计1和惯导3的融合定位。

优化上一实施例，如图3所示，所述定位系统还包括离散卡尔曼滤波器7，所述轮式里程计2以及所述惯导3均与所述离散卡尔曼滤波器7连接；

所述轮式里程计2用于获取机器人的运动路程，所述惯导3用于获取角速度以及三轴加速度，并根据所述三轴加速度计算俯仰角、侧倾角以及机器人在导航坐标系内的Z轴坐标，所述离散卡尔曼滤波器7用于对所述运动路程以及角速度进行信息融合，生成融合位置信息，结合所述融合位置信息、俯仰角、侧倾角以及机器人在导航坐标系内的Z轴坐标得到轮式惯导融合定位信息。

利用离散卡尔曼滤波器7实现轮式里程计2和惯导3的融合定位。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于多传感器融合的机器人自抗扰定位方法，其特征在于，所述多传感器包括双目视觉里程计、轮式里程计以及惯导，所述定位方法包括以下步骤：

步骤S1、获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β；

步骤S2、设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β)；

步骤S3、若α≥Threshold(α)且β≥Threshold(β)时，采用视觉惯导融合定位，否则采用轮式惯导融合定位。

2.根据权利要求1所述基于多传感器融合的机器人自抗扰定位方法，其特征在于，所述视觉惯导融合定位具体包括以下子步骤：

步骤S31、通过所述双目视觉里程计获取双目图像，并根据所述双目图像获取旋转矩阵以及平移矩阵；

步骤S32、通过所述惯导获取角速度以及三轴加速度，并根据所述角速度和三轴加速度计算姿态角信息；

步骤S33、建立扩展卡尔曼滤波模型，并通过所述扩展卡尔曼滤波模型对所述姿态角信息以及所述旋转矩阵进行信息融合，计算出融合旋转矩阵，并结合所述平移矩阵以及所述融合旋转矩阵得到视觉惯导融合定位信息。

3.根据权利要求2所述基于多传感器融合的机器人自抗扰定位方法，其特征在于，所述子步骤S31进一步包括以下子步骤：

步骤S311、设定所述双目视觉里程计的双目相机平行安装，通过所述双目相机获取双目图像，对所述双目图像的特征点进行匹配，利用视差计算出所述特征点的三维坐标；

步骤S312、以所述双目相机的左相机为基准，建立相机坐标系下前后时刻对应特征点的运动方程组:

P_k＝R P_k-1+T

式中:P_k表示k时刻所述特征点在相机坐标系中的三维坐标值，P_k-1表示k-1时刻所述特征点在相机坐标系中的三维坐标值，R表示旋转矩阵，T表示平移矩阵；

去除所述特征点中的离群值并用最小二乘法求解所述运动方程组得到旋转矩阵R以及平移矩阵T。

4.根据权利要求3所述基于多传感器融合的机器人自抗扰定位方法，其特征在于，所述姿态角信息包括俯仰角θ、侧倾角φ以及航向角ψ，所述子步骤S32进一步包括以下子步骤：

步骤S321、通过所述惯导获取三轴加速度a_x、a_y以及a_z，仅重力作用时所述三轴加速度a_x、a_y以及a_z与俯仰角θ以及侧倾角φ之间满足以下关系式：

其中，g为重力加速度,求解上式得到俯仰角θ和侧倾角φ；

步骤S322、通过所述惯导获取角速度ω_g，在k时刻至k+1时刻之间的时间段内对角速度ω_g积分，得到航向角变化量Δψ，再将所述航向角变化量Δψ累加得到航向角ψ。

5.根据权利要求4所述基于多传感器融合的机器人自抗扰定位方法，其特征在于，所述子步骤S33进一步包括以下子步骤：

步骤S331、利用所述姿态角信息计算姿态四元素e₀、e₁、e₂以及e₃，定义所述扩展卡尔曼滤波模型的系统状态变量D＝[e₀ e₁ e₂ e₃]^T，并建立系统的状态方程：

D_k+1＝q*D_k+w(k)

其中，D_K表示k时刻的状态变量，D_K+1表示k+1时刻的状态变量，q表示k时刻到k+1时刻机器人的旋转向量，q由旋转矩阵R计算得到，*为四元素乘法，w(k)表示所述状态方程的噪声向量，求解所述状态方程得到k+1时刻的状态变量D_k+1；

步骤S332、建立观测方程：

其中，v(k)表示观测过程的高斯噪声矩阵，求解所述观测方程以及状态方程得到所述扩展卡尔曼滤波模型的融合姿态角，所述融合姿态角包括融合俯仰角θ_k、融合侧倾角φ_k以及融合航向角变化量Δψ_k，利用所述融合姿态角计算融合旋转矩阵R1；

步骤S333、结合所述平移矩阵T以及融合旋转矩阵R1得到导航坐标系下的融合定位信息推算方程：

其中，表示k+1时刻载体在导航坐标系的三维坐标，为k时刻载体在导航坐标系的三维坐标，是累积前k时刻融合旋转矩阵R1的结果，T_k,k+1表k时刻到k+1时刻的平移矩阵。

6.根据权利要求1所述基于多传感器融合的机器人自抗扰定位方法，其特征在于，所述轮式惯导融合定位包括以下子步骤：

步骤S41、通过轮式里程计获取k时刻到k+1时刻机器人的左轮的运动路程S1_k和机器人的右轮的运动路程S2_k；

步骤S42、通过所述惯导获取角速度ω_g以及三轴加速度，并根据所述三轴加速度计算俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标z_k；

步骤S43、建立离散卡尔曼滤波模型，并通过所述离散卡尔曼滤波模型对所述运动路程S1_k、运动路程S2_k以及角速度ω_g进行信息融合，生成融合位置信息，结合所述融合位置信息、俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标z_k得到轮式惯导融合定位信息。

7.根据权利要求6所述基于多传感器融合的机器人自抗扰定位方法，其特征在于，所述子步骤S43进一步包括以下子步骤：

步骤S431、定义所述离散卡尔曼滤波模型的系统状态变量X＝[S1_k S2_k ω_g]^T，并建立系统状态方程：

X_k+1＝A_k+1|kX_k+W_k

其中，X_k为k时刻的系统状态变量，X_k+1为k+1时刻的系统状态变量，A_k+1|k为k时刻到k+1时刻的状态转移矩阵，W_k为系统状态方程的噪声矩阵，求解所述系统状态方程得到k+1时刻的系统状态变量X_k+1；

步骤S432、建立系统测量方程：

N_k＝H_kX_k+v_k

其中，H_k为系统量测矩阵并可从所述惯导以及所述轮式里程计中直接读出，v_k为所述系统量测方程的噪声矩阵，求解所述系统状态方程以及系统测量方程得到左轮的融合运动路程S3_k、右轮的融合运动路程S4_k以及融合航向角变化量Δψ1_k，累加所述航向角变化量Δψ1_k得到k时刻融合航向角ψ1_k；

步骤S433、建立融合定位信息推算方程：

其中，l为所述左轮与右轮之间的距离，Δψ1_k+1为k到k+1时刻间航向角变化量，x_k为机器人在导航坐标系中的X轴坐标,y_k为机器人在导航坐标系中的Y轴坐标,r_k＝S3_kl/(S4_k-S3_k)，求解所述融合位置信息推算方程得到融合位置信息Q_k；

步骤S434、结合所述融合位置信息Q_k、俯仰角θ、侧倾角φ以及机器人在导航坐标系内的Z轴坐标z_k得到轮式惯导融合定位信息。

8.一种基于多传感器融合的机器人自抗扰定位系统，其特征在于，所述多传感器包括双目视觉里程计、轮式里程计以及惯导，所述定位系统包括：

获取模块，所述获取模块连接所述双目视觉里程计，用于获取所述双目视觉里程计的左右特征匹配点对数α和前后跟踪匹配点对数β；

控制模块，所述控制模块连接所述获取模块，用于设置左右特征匹配点对数阈值Threshold(α)和前后跟踪匹配点对数阈值Threshold(β)；

所述双目视觉里程计、轮式里程计以及惯导均与所述控制模块连接；

所述控制模块还用于比较α与Threshold(α)以及β与Threshold(β)，若α≥Threshold(α)且β≥Threshold(β)时，所述控制模块控制所述双目视觉历程计以及惯导进行融合定位，否则控制所述轮式里程计以及惯导进行融合定位。

9.根据权利要求8所述基于多传感器融合的机器人自抗扰定位系统，其特征在于，还包括扩展卡尔曼滤波器，所述双目视觉里程计以及所述惯导均与所述扩展卡尔曼滤波器连接；

所述双目视觉里程计用于获取双目图像，并根据所述双目图像获取旋转矩阵以及平移矩阵，所述惯导用于获取角速度以及三轴加速度，并根据所述角速度和三轴加速度计算姿态角信息，所述扩展卡尔曼滤波器用于对所述姿态角信息以及所述旋转矩阵进行信息融合，计算出融合旋转矩阵，并结合所述平移矩阵以及所述融合旋转矩阵得到视觉惯导融合定位信息。

10.根据权利要求8所述基于多传感器融合的机器人自抗扰定位系统，其特征在于，还包括离散卡尔曼滤波器，所述轮式里程计以及所述惯导均与所述离散卡尔曼滤波器连接；

所述轮式里程计用于获取机器人的运动路程，所述惯导用于获取角速度以及三轴加速度，并根据所述三轴加速度计算俯仰角、侧倾角以及机器人在导航坐标系内的Z轴坐标，所述离散卡尔曼滤波器用于对所述运动路程以及角速度进行信息融合，生成融合位置信息，结合所述融合位置信息、俯仰角、侧倾角以及机器人在导航坐标系内的Z轴坐标得到轮式惯导融合定位信息。