CN112179336B - 基于双目视觉和惯导组合定位的自动行李运输方法 - Google Patents

Abstract

本发明公开了一种基于双目视觉和惯导组合定位的自动行李运输方法，同步采集双目摄像头图像及行李运输车和导航牌的实时惯导数据；提取左右摄像头图像特征并匹配得到深度图；利用惯导计算行李运输车和导航牌的姿态和位置；建立卡尔曼滤波器模型融合视觉和惯导姿态估计；根据组合定位策略选择定位方法；根据定位方法完成运输车和导航牌的累积航位推算。本发明实现了自适应的滤波估计，有效提高了定位精度。

Description

基于双目视觉和惯导组合定位的自动行李运输方法

技术领域

本发明属于自动行李运输技术，具体为一种基于双目视觉和惯导组合定位的自动行李运输方法。

背景技术

人们在乘坐交通工具时携带较多行李，路途中可放在后备箱，但到达目的地后只能通过人力来运送，费时费力，而且还需要进行取票、安检等必要流程，手推行李箱变得更加费劲，特别是当乘客为残疾人士或者老年人，大量行李会其行动更加不变。自动行李运输车可以在乘客进入机场后，帮助他们搬运行李至李托运处，在这个过程中行李运输车自动定位跟随乘客。然而目前中国的跟随性小车主要是有轨机械跟随，用于工厂生产机车部分，或是比赛场地的有轨跟随拍摄车，并没有实际针对车站、飞机场等大众场合的产品。

为了实现行李运输车自动跟随的功能，就需要利用定位传感器对行李运输车和旅客进行定位。而在某些场合，单一的定位传感器获取的信息具有局限性，存在精度不够和稳定性差的问题。

发明内容

本发明的目的在于提出了一种基于双目视觉和惯导组合定位的自动行李运输方法。

实现本发明的技术解决方案为：一种基于双目视觉和惯导组合定位的自动行李运输方法，具体步骤为：

步骤1、同步采集双目摄像头拍摄的持有导航牌的行人图像及行李运输车和导航牌的实时惯导数据；

步骤2、提取步骤1采集到的行人图像特征并匹配得到深度图，从而得到双目视觉定位数据；

步骤3、根据实时惯导数据计算行李运输车和导航牌的位置，得到惯导定位数据；

步骤4、根据组合定位策略，确定定位方法，具体为：

当正确匹配的特征点数量大于阈值并且深度图中深度误差小于设定阈值时，将双目视觉定位数据作为定位结果；

当正确匹配的特征点数量大于阈值和深度图中深度误差小于设定阈值两者中只有一个满足条件时，融合惯导数据及双目视觉定位数据进行定位；

当正确匹配的特征点数量小于阈值并且深度图中深度误差大于设定阈值时，将惯导数据作为定位结果；

步骤5、根据定位结果，确定运输车和导航牌的航迹。

优选地，步骤2中提取步骤1采集到的行人图像特征并匹配得到深度图，从而得到双目视觉定位数据的具体方法为：

提取采集到的行人图像的特征点，以特征点之间的距离阀值作为判定条件，剔除误匹配点；对经过剔除后的匹配点进行重新再匹配，匹配之后将二维点集映射到三维空间，由三维点集求得两帧图像之间的变换矩阵，通过坐标变换求得导航坐标系下的运动参数得到深度图，最终得到双目视觉定位数据。

优选地，步骤3根据实时惯导数据计算行李运输车和导航牌的位置，得到惯导定位数据的具体方法为：

根据传感器得到的实时惯导数据，使用四元数法将运输车和导航牌上的载体坐标系转换至导航坐标系中，根据下式计算k+1时刻运输车和导航牌的位置：

式中，v_g(k)和a_g(k)为载体在导航坐标系中的速度和加速度，Δt为前一时刻与后一时刻之间的时间差，S(k)为k时刻运输车和导航牌的位置，S(k+1)为k+1时刻运输车和导航牌的位置；

按时间顺序进行迭代分别得到运输车和导航牌整个运动过程中运输车和导航牌在同一空间中的运动轨迹，最终得到惯导定位数据。

优选地，步骤4中融合惯导数据及双目视觉定位数据进行定位的具体方法为：

建立扩展卡尔曼滤波器模型；

利用扩展卡尔曼滤波器模型对双目视觉深度信息和惯导信息进行融合，得到定位结果。

优选地，建立的扩展卡尔曼滤波器模型包括：

将惯导数据测量值作为过程模型的输入变量，建立扩展卡尔曼滤波的状态模型：

其中，Δt为采样时间间隔，a_g表示导航坐标系的加速度值；

建立扩展卡尔曼滤波观测方程：

其中Y_k为高斯白噪声，满足Y_k～N(0，R)，R为噪声协方差，状态向量X_k为惯导载体当前的姿态，具体为：

X_k＝(P_k，ψ_k，V_k，δ_k，ε_k)^T

其中，P_k＝(x_k，y_k，z_k)^T表示k时刻的位置，x_k，y_k，z_k分别表示三个坐标；表示k时刻的姿态角，/>分别为偏航角、俯仰角和翻滚角，V_k表示k时刻的速度向量，δ_k＝(δ_x，δ_y，δ_z)^T表示k时刻的加速度误差，ε_k＝(ε_x，ε_y，ε_z)^T表示k时刻角速度的误差。

优选地，步骤5根据定位结果确定运输车和导航牌的航迹的具体步骤为：

根据定位方法，获得运输车和导航牌运动过程中的实时位置，最终确定运输车和导航牌的航迹。

本发明与现有技术相比，其显著优点为：1)本发明通过将惯性导航测得的姿态信息和双目视觉测得的姿态信息相结合的方式，通过对双目视觉采集的图像采用改进的二维特征点误匹配剔除算法来剔除双目视觉定位中的二维特征误匹配点，保证对导航牌所在位置与状态确定的精度；2)本发明具有数据更新率高、数据全面以及短时定位精度高等优点；3)本发明将双目视觉系统与捷联惯性导航系统利用扩展卡尔曼滤波进行融合，可以进一步减小运输车和导航牌的定位误差，提高了其定位误差的修正效果。

下面结合附图对本发明做进一步详细的描述。

附图说明

图1为捷联惯性导航原理示意图；

图2为基于双目视觉和惯导组合定位的自动行李运输系统的原理示意图；

图3为本发明实施例中运输车定位的一组轨迹图。

具体实施方式

一种基于双目视觉和惯导组合定位的自动行李运输方法，步骤如下：

步骤1、同步采集双目摄像头拍摄的持有导航牌的行人图像及行李运输车和导航牌的实时惯导数据，具体为：

运输车为通用轮式托运车，导航牌为内置惯性导航仪的长方体塑料牌，初始时放置于运输车固定位置上，随后被人拿起，运输车几何中心位置再放置一个惯性导航仪，采用捷联式惯性导航仪，其输出为三轴加速度和角速度，采样频率60Hz，初始时运输车和导航牌上的惯性导航仪坐标系以垂直地面向下为Z轴方向，运输车行进正前方为X轴方向，运输车正右侧且同时垂直于X轴方向和Z轴方向为Y轴方向，双目立体相机置于运输车正前方中心且无遮挡，俯仰角为20-40度，相机采样频率为60Hz。将惯性导航仪分别与运输车和导航牌固连，测量其三轴加速度和角速度数据。

步骤2、提取步骤1采集到的行人图像特征并匹配得到深度图，从而得到双目视觉定位数据，具体为：

提取采集到的行人图像的特征点，以特征点之间的距离阀值作为判定条件，剔除误匹配点；对经过剔除后的匹配点进行重新再匹配，匹配之后将二维点集映射到三维空间，由三维点集求得两帧图像之间的变换矩阵，通过坐标变换求得导航坐标系下的运动参数得到深度图，最终得到双目视觉定位数据。

步骤3、根据实时惯导数据计算行李运输车和导航牌的位置，得到惯导定位数据，具体为：

根据传感器得到的实时惯导数据，使用四元数法将运输车和导航牌上的载体坐标系转换至导航坐标系中，根据下式计算k+1时刻运输车和导航牌的位置：

式中，v_g(k)和a_g(k)为载体在导航坐标系中的速度和加速度，Δt为前一时刻与后一时刻之间的时间差，S(k)为k时刻运输车和导航牌的位置，S(k+1)为k+1时刻运输车和导航牌的位置；

根据初始位置，按时间顺序进行迭代分别得到运输车和导航牌整个运动过程中运输车和导航牌在同一空间中的运动轨迹，最终得到惯导定位数据。

步骤4、根据组合定位策略，确定定位方法，具体为：

组合定位策略数为：

其中，用*表示二者的融合，f₁为双目视觉定位结果，f₂为惯导定位结果，根据实际情况对二者的可信度进行衡量，然后选择对两者进行交替定位或者融合定位：

1.当x₁＞ζ，x₂＜ξ，则认为双目视觉定位数据可信度满足条件，可作为定位结果。

2.当x₁＜ζ，x₂任意值，则认为正确匹配的特征点数量导致双目视觉定位数据可信度不高，需要融合惯导数据进行组合定位。

3.当x₂＞ξ，x₁任意值，则认为双目摄像头的测距误差导致双目视觉定位数据可信度不高，需要融合惯导数据进行组合定位。

4.x₁＜ζ，x₂＞ξ，则认为双目视觉定位数据可信度不满足条件，采用惯导定位数据作为定位结果。

可以看出，以上一个动态过程，根据实际情况去选择融合或者不融合，也可以利用视觉和惯性导航进行交替定位，正确匹配的特征点数量阈值ζ和深度误差阈值ξ的具体值可由多次实验获得。

以上策略，可以归纳为以下方法：

当正确匹配的特征点数量大于阈值并且深度图中深度误差小于设定阈值时，将双目视觉定位数据作为定位结果；

当正确匹配的特征点数量大于阈值和深度图中深度误差小于设定阈值两者中只有一个满足条件时，融合惯导数据及双目视觉定位数据进行定位；

当正确匹配的特征点数量小于阈值并且深度图中深度误差大于设定阈值时，将惯导数据作为定位结果；

其中，融合惯导数据及双目视觉定位数据进行定位的具体方法为：利用扩展卡尔曼滤波对双目视觉深度信息和惯导计算的运输车和导航牌姿态进行信息融合；

定义状态向量X为惯导载体当前的姿态：

X_k＝(P_k，ψ_k，V_k，δ_k，ε_k)^T

其中，P_k＝(x_k，y_k，z_k)^T表示k时刻的位置，x_k，y_k，z_k分别表示三个坐标；表示k时刻的姿态角，/>分别为偏航角、俯仰角和翻滚角，V_k表示k时刻的速度向量，δ_k＝(δ_x，δ_y，δ_z)^T表示k时刻的加速度误差，ε_k＝(ε_x，ε_y，ε_z)^T表示k时刻角速度的误差，以惯性导航的测量值构建过程模型，将加速度测量值和陀螺仪测量值作为过程模型的输入变量，建立扩展卡尔曼滤波的状态模型：

其中Δt为采样时间间隔，a_g表示导航坐标系的加速度值，可由陀螺仪的测量值进行姿态解算求得，扩展卡尔曼滤波观测模型根据基于双目视觉传感器的测量数据，通过两个相邻时刻的图像可计算求得导航牌相对位姿，建立扩展卡尔曼滤波观测方程：

通常认为Y_k为高斯白噪声，满足Y_k～N(0，R)，噪声协方差R的值取决于双目视觉定位的可信度，该可信度由正确匹配的特征点数以及深度误差决定，可信度越小，噪声的协方差值越大。

根据惯导数据计算运输车和导航牌在任意时刻的位置信息，经过扩展卡尔曼滤波将双目摄像头测得的深度数据与其融合更新可以得到运动载体在导航坐标系中的准确位置，得到导航牌相对于运输车的方向和距离，解决了航位推算中姿态累积误差问题，最终实现运输车自动跟随导航牌的功能。

步骤5、根据定位结果，确定运输车和导航牌的航迹。

根据定位方法，获得运输车和导航牌运动过程中的实时位置，最终确定运输车和导航牌的航迹。

本发明引用扩展卡尔曼滤波器姿态估计模型，利用双目摄像头深度图作为补充，对惯导定位姿态进行融合估计，修正惯导定位累积误差，有效提高了了定位精度。

实施例

步骤1、同步采集双目摄像头拍摄的持有导航牌的行人图像及行李运输车和导航牌的实时惯导数据

采用通用轮式托运车，维特智能品牌的JY901型捷联式惯性导航模块和OV9750双目同步摄像头，导航牌为内置惯性导航仪的长方体塑料牌，初始时放置于运输车固定位置上，随后被人拿起，运输车几何中心位置再放置一个惯性导航仪，其中惯导采样频率50Hz，初始时运输车和导航牌上的惯性导航仪坐标系以垂直地面向下为Z轴方向，运输车行进正前方为X轴方向，运输车正右侧且同时垂直于X轴方向和Z轴方向为Y轴方向；双目立体相机置于运输车正前方中心且无遮挡，俯仰角为30度，相机采样频率为50Hz。

步骤2、提取左右摄像头图像特征并匹配得到深度图

对双目相机采集的左、右图，采用改进的二维特征点误匹配剔除算法来剔除双目视觉定位中的二维特征误匹配点，通常会采用RANSAC算法(Random Sample Consensus)来剔除一些错误的匹配点，减少误匹配点对变换矩阵的求解影响，为了减小迭代次数，提高匹配精度，对其作以改进，采用改进的二维特征点误匹配剔除算法，首先以特征点之间的距离阀值作为判定条件，先剔除一部分误匹配点；然后对经过第一步剔除后的匹配点进行重新再匹配。匹配之后然后将二维点集映射到三维空间，由三维点集求得两帧图像之间的变换矩阵，进而通过坐标变换求得导航坐标系下的运动参数，最终得到深度图。

步骤3、根据实时惯导数据计算行李运输车和导航牌的位置，得到惯导定位数据；

将惯性导航仪分别与运输车和导航牌固连，测量其三轴加速度和角速度数据，使用四元数法将运输车和导航牌上的载体坐标系转换至导航坐标系中，可以得到运动载体在导航坐标系中的位置S(k+1)，即：

其中v_g(k)和a_g(k)为载体在导航坐标系中的速度和加速度，Δt为前一时刻与后一时刻之间的时间差，根据初始位置，依次按时间顺序进行迭代可得到整个运动过程中运输车和导航牌在同一空间中的运动轨迹。

步骤4、根据组合定位策略，确定定位方法，具体为：

组合定位策略数为：

其中，用*表示二者的融合，f₁为双目视觉定位结果，f₂为惯导定位结果，根据实际情况对二者的可信度进行衡量，然后选择对两者进行交替定位或者融合定位：

1.当x₁＞ζ，x₂＜ξ，则认为双目视觉定位数据可信度满足条件，可作为定位结果。

2.当x₁＜ζ，x₂任意值，则认为正确匹配的特征点数量导致双目视觉定位数据可信度不高，需要融合惯导数据进行组合定位。

3.当x₂＞ξ，x₁任意值，则认为双目摄像头的测距误差导致双目视觉定位数据可信度不高，需要融合惯导数据进行组合定位。

4.x₁＜ζ，x₂＞ξ，则认为双目视觉定位数据可信度不满足条件，采用惯导定位数

融合惯导数据进行组合定位具体为：利用扩展卡尔曼滤波对双目视觉深度信息和惯导计算的运输车和导航牌姿态进行信息融合，定义状态向量X为惯导载体当前的姿态：

X_k＝(P_k，ψ_k，V_k，δ_k，ε_k)^T

其中，P_k＝(x_k，y_k，z_k)^T表示k时刻的位置，x_k，y_k，z_k分别表示三个坐标；表示k时刻的姿态角，/>分别为偏航角、俯仰角和翻滚角，V_k表示k时刻的速度向量，δ_k＝(δ_x，δ_y，δ_z)^T表示k时刻的加速度误差，ε_k＝(ε_x，ε_y，ε_z)^T表示k时刻角速度的误差，以惯性导航的测量值构建过程模型，将加速度测量值和陀螺仪测量值作为过程模型的输入变量，建立扩展卡尔曼滤波的状态模型：

其中Δt为采样时间间隔，a_g表示导航坐标系的加速度值，可由陀螺仪的测量值进行姿态解算求得，扩展卡尔曼滤波观测模型根据基于双目视觉传感器的测量数据，通过两个相邻时刻的图像可计算求得导航牌相对位姿，建立扩展卡尔曼滤波观测方程：

通常认为Y_k为高斯白噪声，满足Y_k～N(0，R)，噪声协方差R的值取决于双目视觉定位的可信度，该可信度由正确匹配的特征点数以及深度误差决定，可信度越小，噪声的协方差值越大。

根据惯导数据计算运输车和导航牌在任意时刻的位置信息，经过扩展卡尔曼滤波将双目摄像头测得的深度数据与其融合更新可以得到运动载体在导航坐标系中的准确位置，得到导航牌相对于运输车的方向和距离，解决了航位推算中姿态累积误差问题，最终实现运输车自动跟随导航牌的功能。

步骤5、根据定位结果，确定运输车和导航牌的航迹。

如图3，运输车移动一个正方形区域，总共采集图像帧300帧，航位推算得到的轨迹图，包括仅利用惯导方法定位的结果以及本方法的定位轨迹图。并且实施例中用毫米级定位精度的全站仪(Total Station)作为定位真值参考验证本方法的精度。

Claims (4)

1.一种基于双目视觉和惯导组合定位的自动行李运输方法，其特征在于，具体步骤为：

步骤1、同步采集双目摄像头拍摄的持有导航牌的行人图像及行李运输车和导航牌的实时惯导数据；通过在导航牌中设置惯性导航仪，以及在行李运输车上获取惯性导航仪，采集实时惯导数据；

步骤2、提取步骤1采集到的行人图像特征并匹配得到深度图，从而得到双目视觉定位数据；

步骤3、根据实时惯导数据计算行李运输车和导航牌的位置，得到惯导定位数据；

步骤4、根据组合定位策略，确定定位方法，具体为：

当正确匹配的特征点数量大于阈值并且深度图中深度误差小于设定阈值时，将双目视觉定位数据作为定位结果；

当正确匹配的特征点数量大于阈值和深度图中深度误差小于设定阈值两者中只有一个满足条件时，融合惯导数据及双目视觉定位数据进行定位；

当正确匹配的特征点数量小于阈值并且深度图中深度误差大于设定阈值时，将惯导数据作为定位结果；

融合惯导数据及双目视觉定位数据进行定位的具体方法为：

建立扩展卡尔曼滤波器模型，包括：

将惯导数据测量值作为过程模型的输入变量，建立扩展卡尔曼滤波的状态模型：

其中，Δt为采样时间间隔，a_g表示导航坐标系的加速度值，P_k表示k时刻的位置，V_k-1表示k-1时刻的速度向量；

建立扩展卡尔曼滤波观测方程：

其中，Y_k为高斯白噪声，满足Y_k～N(0,R)，R为噪声协方差，状态向量X_k为惯导载体当前的姿态，具体为：

X_k＝(P_k,ψ_k,V_k,δ_k,ε_k)^T

其中，P_k＝(x_k,y_k,z_k)^T表示k时刻的位置，x_k,y_k,z_k分别表示三个方向坐标；表示k时刻的姿态角，/>分别为偏航角、俯仰角和翻滚角，V_k表示k时刻的速度向量，δ_k＝(δ_x,δ_y,δ_z)^T表示k时刻的加速度误差，ε_k＝(ε_x,ε_y,ε_z)^T表示k时刻角速度的误差；

利用扩展卡尔曼滤波器模型对双目视觉深度信息和惯导信息进行融合，得到定位结果；

步骤5、根据定位结果，确定运输车和导航牌的航迹。

2.根据权利要求1所述的基于双目视觉和惯导组合定位的自动行李运输方法，其特征在于，步骤2中提取步骤1采集到的行人图像特征并匹配得到深度图，从而得到双目视觉定位数据的具体方法为：

提取采集到的行人图像的特征点，以特征点之间的距离阀值作为判定条件，剔除误匹配点；对经过剔除后的匹配点进行重新再匹配，匹配之后将二维点集映射到三维空间，由三维点集求得两帧图像之间的变换矩阵，通过坐标变换求得导航坐标系下的运动参数得到深度图，最终得到双目视觉定位数据。

3.根据权利要求1所述的基于双目视觉和惯导组合定位的自动行李运输方法，其特征在于，步骤3根据实时惯导数据计算行李运输车和导航牌的位置，得到惯导定位数据的具体方法为：

根据传感器得到的实时惯导数据，使用四元数法将运输车和导航牌上的载体坐标系转换至导航坐标系中，根据下式计算k+1时刻运输车和导航牌的位置：

式中，v_g(k)和a_g(k)为载体在导航坐标系中的速度和加速度，△t为前一时刻与后一时刻之间的时间差，S(k)为k时刻运输车和导航牌的位置，S(k+1)为k+1时刻运输车和导航牌的位置；

按时间顺序进行迭代分别得到运输车和导航牌整个运动过程中运输车和导航牌在同一空间中的运动轨迹，最终得到惯导定位数据。

4.根据权利要求1所述的基于双目视觉和惯导组合定位的自动行李运输方法，其特征在于，步骤5根据定位结果确定运输车和导航牌的航迹的具体步骤为：

根据定位方法，获得运输车和导航牌运动过程中的实时位置，最终确定运输车和导航牌的航迹。