CN110864685A - 一种基于松耦合的车辆单目视觉轮式里程计定位方法 - Google Patents

Abstract

本发明公开了一种基于松耦合的车辆单目视觉轮式里程计定位方法，采用单目相机以及车辆的速度传感器和转角传感器进行数据采集，实现较成本，通过对车辆CAN报文解析模块和车载单目相机进行同步采样匹配，采用松耦合的传感器融合方法，来构建车辆数据，计算量少，实时性好，最终基于构建的车辆数据通过辆运动学航迹推测算法和扩展卡尔曼滤波算法实现车辆定位；优点是在具有低成本的基础上，通过对单目视觉和车辆轮式里程计进行融合定位，提高了车辆定位的精度。

Description

一种基于松耦合的车辆单目视觉轮式里程计定位方法

技术领域

本方法涉及一种定位方法，尤其是涉及一种基于松耦合的车辆单目视觉轮式里程计 定位方法。

背景技术

实现车辆厘米级定位要求是无人驾驶车辆自主导航的关键，基于单目视觉的定位方 法采用价格低廉的单目相机实现，成本比较低，是目前应用比较广泛的一种定位方法，但是该定位方法存在无法得到绝对尺度的问题，定位精度不是很高。

目前，采用单目视觉与IMU进行融合的定位方法(即视觉惯性里程计VIO)在定位领域得到了广泛研究。但是现有的单目视觉与IMU进行融合的定位方法主要应用于无 人机定位中，当应用到无人驾驶车辆中时，由于车辆启动较为平缓，IMU的有效激励不 足，因此VIO在车辆启动时候无法有效初始化导致失效，无法完成无人驾驶车辆的定位 要求。

发明内容

本方法所要解决的技术问题是提供一种成本较低，且定为精度较高的基于松耦合的 车辆单目视觉轮式里程计定位方法。该定位方法采用单目相机以及车辆的速度传感器和 转角传感器进行数据采集，实现较成本，并且通过采用松耦合的传感器融合方法，计算量少，实时性好，对单目视觉和车辆轮式里程计进行融合定位，提高了车辆定位的精度。

本方法解决上述技术问题所采用的技术方案为：一种基于松耦合的车辆单目视觉轮 式里程计定位方法，包括以下步骤：

(1)将车辆CAN报文解析模块的相邻两次采样之间的时间间隔记为△t₁，△t₁＝0.01s， 将车载单目相机相邻两次采样之间的时间间隔记为△t₂，△t₂＝0.033s，创建一个用于存 放关键帧的数组，将数组中存放的关键帧的数量记为变量n，初始状态时，所述的数组 中不存在关键帧，此时所述的数组中存储的关键帧数量为0，n的取值为0；设计一个用于存储所述的车辆CAN报文解析模块每次采样获取的采样数据的时间戳的缓存，该缓存 的容量为100，当超出其容量时，该缓存中已存时间戳将按照存储时间从早到晚的顺序 被覆盖掉；所述的车辆CAN报文解析模块每次采样获取的采样数据包括车速数据和方向 盘转角数据，所述的车辆CAN报文解析模块每次采样时的时刻采用UTC时间表示，将所 述的车辆CAN报文解析模块每次采样时的时刻作为该次采样时获取的采样数据的时间 戳保存到所述的缓存中，所述的车载单目相机每次采样时获取单目图像，所述的车载单 目相机每次采样的时刻采用UTC时间表示；

(2)设定车辆CAN报文解析模块和车载单目相机的同步采样成功匹配次数变量，将其记为t，对t进行初始化赋值，令t＝0；

(3)同时开启所述的车辆CAN报文解析模块和所述的车载单目相机，所述的车辆CAN报文解析模块和所述的车载单目相机同时开始进行第1次采样，所述的车辆CAN报 文解析模块和所述的车载单目相机后续每采样一次其采样次数增加1；

(4)假设所述的车载单目相机的当前采样次数为第h次，此时对所述的车辆CAN报文解析模块和所述的车载单目相机进行第h次同步采样匹配，具体匹配过程为：

4.1将所述的车载单目相机第h次采样得到的单目图像记为I_h，将所述的车载单目相机第h次采样的时刻记为t_h；

4.2对数组进行第h次更新，具体过程为：

如果h＝1，设定车载单目相机第h次采样的关键帧，将其记为^keyI_h，对^keyI_h进行赋值， 令^keyI_h＝I_h，然后将^keyI_h存入所述的数组中作为所述的数组中的第1个数据，并将关键帧 ^keyI_h存入所述的数组中的存入时刻记为key_h，对key_h进行赋值，令key_h＝t_h；

如果h大于等于2，则判断n的当前值是否小于等于3以及

是否大于等于 5*△t₂，其中，*为乘运算符号，

为车载单目相机进行第h次采样之前最后一次向 所述的数组中存入关键帧的存入时刻，如果n的当前值小于等于3以及

大于等于5*△t₂这两个条件中的任意一个成立，则设定车载单目相机第h个采样时刻的关键帧， 将其记为^keyI_h，对^keyI_h进行赋值，令^keyI_h＝I_h，然后将^keyI_h存入所述的数组中作为所述的 数组中的最后一个数据，并将关键帧^keyI_h存入所述的数组中的存入时刻记为key_h，对key_h进行赋值，令key_h＝t_h；如果n的当前值小于等于3以及

大于等于5*△t₂这两个条 件均不成立，则判断n的当前值是否大于10，如果n的当前值大于10，则将数组中的关键 帧^keyI_h-n删除，否则，保持当前数组不变；

由此实现数组的更新，得到第h次更新后的数组；

4.3统计当前数组中关键帧的数量，采用统计的关键帧的数量更新n的值，将当前数 组表示为(^keyI’₁,^keyI’₂,…,^keyI’_n-1,^keyI’_n)，其中^keyI’_l为当前数组中的第l个数据，l＝1,2，…，n；

4.4通过FAST特征点匹配算法对当前数组中每相邻两个关键帧进行特征点匹配，其 中FAST特征点匹配算法中的特征点检测数量参数设定为200，具体过程为：

4.4.1、设定匹配变量，将该匹配变量记为l’；

4.4.2、对l’进行初始化，令l’＝2；

4.4.3、通过FAST特征点匹配算法对当前数组中第l’个关键帧^keyI’_l’和第l’-1个关键帧 ^keyI’_l’-1进行特征匹配，匹配得到第l’个关键帧^keyI’_l’的200个特征点数据，每个特征点数 据包括该特征点在关键帧^keyI’_l’所在图像平面上的像素坐标以及在关键帧^keyI’_l’所在世 界坐标系下的三维坐标，将第l’个关键帧^keyI’_l’的第i个特征点在关键帧^keyI’_l’所在图像平 面上的像素坐标记为

将第l’个关键帧^keyI’_l’的第i个特征点在关键帧 ^keyI’_l’所在世界坐标系下的三维坐标记为

i＝1,2,……,200；

4.4.4、采用第l’个关键帧^keyI’_l’的200个特征点数据分别构建像素坐标向量和三维坐 标向量，将第l’个关键帧^keyI’_l’的第i个特征点数据构建的像素坐标向量记为

将第 l’个关键帧^keyI’_l’的第i个特征点数据构建的三维坐标向量记为

和

分别 采用式(1)和式(2)表示为：

4.4.5、构建当前数组中的第l’个关键帧^keyI’_l’的相对位姿估计模型，将该模型采用式 (3)、式(4)和式(5)表示为：

其中，△R’_l’表示当前数组中的第l’个关键帧^keyI’_l’相对于第l’-1个关键帧^keyI’_l’-1的位姿旋转 矩阵，△t’_l’表示当前数组中的第l’个关键帧^keyI’_l’相对于第l’-1个关键帧^keyI’_l’-1的位姿平移 向量；π(·)为相机的标准投影方程，ρ(·)为Huber鲁棒核函数，δ＝1，|| ||²为求二范数 运算符号，| |为取绝对值符号，·为乘运算符号。

表示括号中算式取值最小时， 变量△R’_l’和△t’_l’的值；

4.4.6、采用Levenberg-Marquardt(LM优化算法)对式(3)进行求解，得到△R’_l’和△t’_l’；

4.4.7、采用△R’_l’和△t’_l’构建当前数组中的第l’个关键帧^keyI’_l’相对于第l’-1个关键帧 ^keyI’_l’-1的位姿变换矩阵，将其记为△T’_l’，采用式(6)表示为：

其中，0_1x3为行向量[0 0 0]；

4.4.8、判断l’的当前值是否等于n，如果等于n则进入步骤4.4.9，如果不等于n，则采用l’的当前值加1的和更新l’的值，返回步骤4.4.3进行下一次特征匹配，直至l’的 当前值等于n；

4.4.9设定车载单目相机第h次采样的位姿变换矩阵，将其记为T’_h，采用式(7)计算得到T’_h：

其中，

∏为累乘符号；

4.4.10将车载单目相机第h次采样的位姿变换矩阵T’_h，经过标准齐次坐标变换得到车载单目相机第h次采样的车辆坐标系下车辆位置坐标

和航向角数据θ_h ^mono，其中

为横坐标，

为纵坐标；

4.4.11判定第h次同步采样匹配是否成功，具体过程为：

将当前数组中最后存入的关键帧的存入时刻记为

将

与缓存中存储的所有时间 戳进行匹配，查找与

相差小于5毫秒且相差最小的时间戳，如果找到，则第h次同步 采样匹配成功，获取查找到的时间戳对应的车速数据和方向盘转角数据，进入步骤(5)， 如果没有找到，则采用h的当前值加1的和更新h的值，重复步骤(4)，直到满足同步 采样匹配成功条件；

5.先采用t的当前值加1的和更新t的值，然后构建第t次采样同步匹配成功的车辆数据，具体过程为：

5.1将当前采样同步匹配成功的车辆坐标系下车辆位置坐标记为(^monox_t、^monoy_t)，航 向角数据记为^monoθ_t，车速数据记为v_t，方向盘转角数据记为α_t；

5.2将

的值赋值给^monox_t，将

的值赋值给^monoy_t，将θ_h ^mono的值赋值给^monoθ_t；将当前采样同步匹配成功后查找到的时间戳对应的车速数据赋值给v_t，方向盘转角数据赋值给α_t，第t次采样同步匹配成功的车辆数据构建完成；

6.对车辆进行第t次定位，具体过程为：

6.1通过车辆运动学航迹推测算法得到车辆运动学生成的运动轨迹，即第t次定位时 车辆位置的估计坐标

和航向角估计值

表示第t次定位时车辆坐标系下车辆位置的横坐标估计值，

表示第t次定位时车辆坐标系下车辆位置的纵坐标估计值；其中车辆运动学航迹推测算法公式如下所示：

ds_t＝v_t-1·dt (9)

δ_ft＝α_t·η (11)

其中，

表示第t-1次定位时车辆坐标系下车辆位置的横坐标估计值，

表示第t-1次定位时车辆坐标系下车辆位置的纵坐标估计值，

表示第t-1次定位时车辆位置的航向角估计值，ds_t表示从第t次定位时相对于第t-1次定位时车辆行驶的距离，dθ_t表示 第t次定位时相对于第t-1次定位时车辆位置的航向角变化量，v_t-1为第t-1次定位时车辆的 车速，dt为车辆CAN报文解析模块的采样周期，dt＝△t₁，L为车辆轴距，δ_ft为第t次定 位时车辆前轮转角，η为车辆的角传动比，η预先根据车辆型号通过现有成熟的实验方 法标定得到，“·”为乘运算符号，当t＝1时，

v_t-1＝0，sin表示 正弦函数，cos表示余弦函数，tan表示正切函数；

6.2通过扩展卡尔曼滤波算法对第t次采样同步匹配成功的车辆数据：车速v_t、方向盘 转角α_t、^monox_t、^monoy_t和^monoθ_t进行数据融合，得到车辆第t次定位的位置数据，具体过程如下：

a.采用

和

构建第t次定位的状态向量，将该状态向量记为A_t，采用以下 公式对A_t进行初始化赋值：

其中，在t＝1时的初始时刻，

b.采用v_t-1和δ_ft构建第t次定位的控制输入向量，将该控制输入向量记为B_t：

其中，v_t-1＝0；

c、建立第t次定位时带噪声的车辆运动学模型，将其向量表达式记为f(A_t,B_t)：

其中，N(·)为高斯白噪声生成函数，N(0,Q)表示采用高斯白噪声生成函数生成的维度为3×1的高斯白噪声向量，0为高斯白噪声生成函数的均值，Q为高斯白噪声生成 函数的状态传播过程协方差矩阵，状态传播过程协方差矩阵Q为采用随机函数生成的 维度为为3×3的矩阵，生成后为固定值。

d、将第t次定位时f(A_t,B_t)关于状态向量A_t的雅克比矩阵记为F_t，F_t采用式(15) 表示为：

e、将状态传播后的协方差矩阵记为

采用公式(16)对状态传播后的协方差矩阵

进行更新：

其中P表示第t次定位之前状态协方差矩阵的最新值，上标T表示矩阵的转置；当 t＝1时，P初始化为维度为3×3的单位矩阵，即：

f、建立第t次定位时的观测模型：

其中，Z_t为第t次定位时的观测模型，

为第t次定位时的观测函数；N(·)为高斯白噪声生成函数，N(0,R)表示采用高斯白噪声生成函数生成的高斯白噪声向量， N(0,R)的维度为3×1，0为高斯白噪声生成函数的均值，R为观测协方差矩阵，观测 协方差矩阵R的维度为3×3，观测协方差矩阵R为：

g、将第t次定位时观测函数

关于状态向量A_t的雅克比矩阵记为H_t，H_t采用式(21)表示为：

h、将第t次定位时的观测残差记为y_t，计算第t次定位时的观测残差y_t：

i、将第t次定位时的卡尔曼增益记为K_t，计算第t次定位时的卡尔曼增益K_t：

上式中，

的取值为其当前最新值；上标“-1”表示矩阵的逆运算；

j、对状态向量A_t和状态协方差矩阵P进行更新：

其中，I为维度为3×3的单位矩阵，

的取值均为其当前最新值；

k、将(x’_t，y’_t)作为第t次定位时车辆位置坐标，将θ’_t作为第t次定位时车辆航向角， 采用第t次定位时车辆位置坐标(x’_t，y’_t)和第t次定位时车辆航向角θ’_t构成第t次定位时 车辆的位置数据，完成车辆第t次定位；

(7)采用h的当前值加1的和更新h，返回步骤(4)进行下一次定位，周而复始。

与现有技术相比，本方法的优点在于该定位方法采用单目相机以及车辆的速度传感 器和转角传感器进行数据采集，实现较成本，通过对车辆CAN报文解析模块和车载单 目相机进行同步采样匹配，采用松耦合的传感器融合方法，来构建车辆数据，计算量少， 实时性好，最终基于构建的车辆数据通过辆运动学航迹推测算法和扩展卡尔曼滤波算法 实现车辆定位，由此本发明在具有低成本的基础上，通过对单目视觉和车辆轮式里程计 进行融合定位，提高了车辆定位的精度。

具体实施方式

以下结合实施例对本方法作进一步详细描述。

实施例：一种基于松耦合的车辆单目视觉轮式里程计定位方法，包括以下步骤：

(1)将车辆CAN报文解析模块的相邻两次采样之间的时间间隔记为Δt₁，Δt₁＝0.01s， 将车载单目相机相邻两次采样之间的时间间隔记为Δt₂，Δt₂＝0.033s，创建一个用于存 放关键帧的数组，将数组中存放的关键帧的数量记为变量n，初始状态时，数组中不存 在关键帧，此时数组中存储的关键帧数量为0，n的取值为0；设计一个用于存储车辆CAN报文解析模块每次采样获取的采样数据的时间戳的缓存，该缓存的容量为100，当超出 其容量时，该缓存中已存时间戳将按照存储时间从早到晚的顺序被覆盖掉；车辆CAN报 文解析模块每次采样获取的采样数据包括车速数据和方向盘转角数据，车辆CAN报文解 析模块每次采样时的时刻采用UTC时间表示，将车辆CAN报文解析模块每次采样时的时 刻作为该次采样时获取的采样数据的时间戳保存到缓存中，车载单目相机每次采样时获 取单目图像，车载单目相机每次采样的时刻采用UTC时间表示；

(2)设定车辆CAN报文解析模块和车载单目相机的同步采样成功匹配次数变量，将其记为t，对t进行初始化赋值，令t＝0；

(3)同时开启车辆CAN报文解析模块和车载单目相机，车辆CAN报文解析模块和车载单目相机同时开始进行第1次采样，车辆CAN报文解析模块和车载单目相机后续每采 样一次其采样次数增加1；

(4)假设车载单目相机的当前采样次数为第h次，此时对车辆CAN报文解析模块和车载单目相机进行第h次同步采样匹配，具体匹配过程为：

4.1将车载单目相机第h次采样得到的单目图像记为I_h，将车载单目相机第h次采样 的时刻记为t_h；

4.2对数组进行第h次更新，具体过程为：

如果h＝1，设定车载单目相机第h次采样的关键帧，将其记为^keyI_h，对^keyI_h进行赋值， 令^keyI_h＝I_h，然后将^keyI_h存入数组中作为数组中的第1个数据，并将关键帧^keyI_h存入数组 中的存入时刻记为key_h，对key_h进行赋值，令key_h＝t_h；

如果h大于等于2，则判断n的当前值是否小于等于3以及

是否大于等于 5*Δt₂，其中，*为乘运算符号，

为车载单目相机进行第h次采样之前最后一次向 数组中存入关键帧的存入时刻，如果n的当前值小于等于3以及

大于等于5*Δt₂这两个条件中的任意一个成立，则设定车载单目相机第h个采样时刻的关键帧，将其记 为^keyI_h，对^keyI_h进行赋值，令^keyI_h＝I_h，然后将^keyI_h存入数组中作为数组中的最后一个数 据，并将关键帧^keyI_h存入数组中的存入时刻记为key_h，对key_h进行赋值，令key_h＝t_h；如 果n的当前值小于等于3以及

大于等于5*Δt₂这两个条件均不成立，则判断n的当 前值是否大于10，如果n的当前值大于10，则将数组中的关键帧^keyI_h-n删除，否则，保持 当前数组不变；

由此实现数组的更新，得到第h次更新后的数组；

4.3统计当前数组中关键帧的数量，采用统计的关键帧的数量更新n的值，将当前数 组表示为(^keyI’₁,^keyI’₂,…,^keyI’_n-1,^keyI’_n)，其中^keyI’_l为当前数组中的第l个数据，l＝1,2，…，n；

4.4通过FAST特征点匹配算法对当前数组中每相邻两个关键帧进行特征点匹配，其 中FAST特征点匹配算法中的特征点检测数量参数设定为200，具体过程为：

4.4.1、设定匹配变量，将该匹配变量记为l’；

4.4.2、对l’进行初始化，令l’＝2；

4.4.3、通过FAST特征点匹配算法对当前数组中第l’个关键帧^keyI’_l’和第l’-1个关键帧 ^keyI’_l’-1进行特征匹配，匹配得到第l’个关键帧^keyI’_l’的200个特征点数据，每个特征点数 据包括该特征点在关键帧^keyI’_l’所在图像平面上的像素坐标以及在关键帧^keyI’_l’所在世 界坐标系下的三维坐标，将第l’个关键帧^keyI’_l’的第i个特征点在关键帧^keyI’_l’所在图像平 面上的像素坐标记为

将第l’个关键帧^keyI’_l’的第i个特征点在关键帧 ^keyI’_l’所在世界坐标系下的三维坐标记为

i＝1,2,……,200；

4.4.4、采用第l’个关键帧^keyI’_l’的200个特征点数据分别构建像素坐标向量和三维坐 标向量，将第l’个关键帧^keyI’_l’的第i个特征点数据构建的像素坐标向量记为

将第 l’个关键帧^keyI’_l’的第i个特征点数据构建的三维坐标向量记为

和

分别 采用式(1)和式(2)表示为：

4.4.5、构建当前数组中的第l’个关键帧^keyI’_l’的相对位姿估计模型，将该模型采用式 (3)、式(4)和式(5)表示为：

其中，△R’_l’表示当前数组中的第l’个关键帧^keyI’_l’相对于第l’-1个关键帧^keyI’_l’-1的位姿旋转 矩阵，△t’_l’表示当前数组中的第l’个关键帧^keyI’_l’相对于第l’-1个关键帧^keyI’_l’-1的位姿平移 向量；π(·)为相机的标准投影方程，ρ(·)为Huber鲁棒核函数，δ＝1，|| ||²为求二范数 运算符号，| |为取绝对值符号，·为乘运算符号。

表示括号中算式取值最小时， 变量△R’_l’和△t’_l’的值；

4.4.6、采用Levenberg-Marquardt(LM优化算法)对式(3)进行求解，得到△R’_l’和△t’_l’；

4.4.7、采用△R’_l’和△t’_l’构建当前数组中的第l’个关键帧^keyI’_l’相对于第l’-1个关键帧 ^keyI’_l’-1的位姿变换矩阵，将其记为△T’_l’，采用式(6)表示为：

其中，0_1x3为行向量[0 0 0]；

4.4.8、判断l’的当前值是否等于n，如果等于n则进入步骤4.4.9，如果不等于n，则采用l’的当前值加1的和更新l’的值，返回步骤4.4.3进行下一次特征匹配，直至l’的 当前值等于n；

4.4.9设定车载单目相机第h次采样的位姿变换矩阵，将其记为T’_h，采用式(7)计算得到T’_h：

其中，

∏为累乘符号；

4.4.10将车载单目相机第h次采样的位姿变换矩阵T’_h，经过标准齐次坐标变换得到车载单目相机第h次采样的车辆坐标系下车辆位置坐标

和航向角数据θ_h ^mono，其中

为横坐标，

为纵坐标；

4.4.11判定第h次同步采样匹配是否成功，具体过程为：

将当前数组中最后存入的关键帧的存入时刻记为

将

与缓存中存储的所有时间 戳进行匹配，查找与

相差小于5毫秒且相差最小的时间戳，如果找到，则第h次同步 采样匹配成功，获取查找到的时间戳对应的车速数据和方向盘转角数据，进入步骤(5)， 如果没有找到，则采用h的当前值加1的和更新h的值，重复步骤(4)，直到满足同步 采样匹配成功条件；

5.先采用t的当前值加1的和更新t的值，然后构建第t次采样同步匹配成功的车辆数据，具体过程为：

5.1将当前采样同步匹配成功的车辆坐标系下车辆位置坐标记为(^monox_t、^monoy_t)，航 向角数据记为^monoθ_t，车速数据记为v_t，方向盘转角数据记为α_t；

5.2将

的值赋值给^monox_t，将

的值赋值给^monoy_t，将θ_h ^mono的值赋值给^monoθ_t；将当前采样同步匹配成功后查找到的时间戳对应的车速数据赋值给v_t，方向盘转角数据赋值给α_t，第t次采样同步匹配成功的车辆数据构建完成；

6.对车辆进行第t次定位，具体过程为：

6.1通过车辆运动学航迹推测算法得到车辆运动学生成的运动轨迹，即第t次定位时 车辆位置的估计坐标

和航向角估计值

表示第t次定位时车辆坐标系下车辆位置的横坐标估计值，

表示第t次定位时车辆坐标系下车辆位置的纵坐标估计值；其中车辆运动学航迹推测算法公式如下所示：

ds_t＝v_t-1·dt (9)

δ_ft＝α_t·η (11)

其中，

表示第t-1次定位时车辆坐标系下车辆位置的横坐标估计值，

表示第t-1次定位时车辆坐标系下车辆位置的纵坐标估计值，

表示第t-1次定位时车辆位置的航向角估计值，ds_t表示从第t次定位时相对于第t-1次定位时车辆行驶的距离，dθ_t表示 第t次定位时相对于第t-1次定位时车辆位置的航向角变化量，v_t-1为第t-1次定位时车辆的 车速，dt为车辆CAN报文解析模块的采样周期，dt＝△t₁，L为车辆轴距，δ_ft为第t次定 位时车辆前轮转角，η为车辆的角传动比，η预先根据车辆型号通过现有成熟的实验方 法标定得到，“·”为乘运算符号，当t＝1时，

v_t-1＝0，sin表示 正弦函数，cos表示余弦函数，tan表示正切函数；

6.2通过扩展卡尔曼滤波算法对第t次采样同步匹配成功的车辆数据：车速v_t、方向盘 转角α_t、^monox_t、^monoy_t和^monoθ_t进行数据融合，得到车辆第t次定位的位置数据，具体过程如下：

a.采用

和

构建第t次定位的状态向量，将该状态向量记为A_t，采用以下 公式对A_t进行初始化赋值：

其中，在t＝1时的初始时刻，

b.采用v_t-1和δ_ft构建第t次定位的控制输入向量，将该控制输入向量记为B_t：

其中，v_t-1＝0；

c、建立第t次定位时带噪声的车辆运动学模型，将其向量表达式记为f(A_t,B_t)：

其中，N(·)为高斯白噪声生成函数，N(0,Q)表示采用高斯白噪声生成函数生成的维度为3×1的高斯白噪声向量，0为高斯白噪声生成函数的均值，Q为高斯白噪声生成 函数的状态传播过程协方差矩阵，状态传播过程协方差矩阵Q为采用随机函数生成的 维度为为3×3的矩阵，生成后为固定值。

d、将第t次定位时f(A_t,B_t)关于状态向量A_t的雅克比矩阵记为F_t，F_t采用式(15) 表示为：

e、将状态传播后的协方差矩阵记为

采用公式(16)对状态传播后的协方差矩阵

进行更新：

其中P表示第t次定位之前状态协方差矩阵的最新值，上标T表示矩阵的转置；当 t＝1时，P初始化为维度为3×3的单位矩阵，即：

f、建立第t次定位时的观测模型：

其中，Z_t为第t次定位时的观测模型，

为第t次定位时的观测函数；N(·)为高斯白噪声生成函数，N(0,R)表示采用高斯白噪声生成函数生成的高斯白噪声向量， N(0,R)的维度为3×1，0为高斯白噪声生成函数的均值，R为观测协方差矩阵，观测 协方差矩阵R的维度为3×3，观测协方差矩阵R为：

g、将第t次定位时观测函数

关于状态向量A_t的雅克比矩阵记为H_t，H_t采用式(21)表示为：

h、将第t次定位时的观测残差记为y_t，计算第t次定位时的观测残差y_t：

i、将第t次定位时的卡尔曼增益记为K_t，计算第t次定位时的卡尔曼增益K_t：

上式中，

的取值为其当前最新值；上标“-1”表示矩阵的逆运算；

j、对状态向量A_t和状态协方差矩阵P进行更新：

其中，I为维度为3×3的单位矩阵，

的取值均为其当前最新值；

k、将(x’_t，y’_t)作为第t次定位时车辆位置坐标，将θ’_t作为第t次定位时车辆航向角， 采用第t次定位时车辆位置坐标(x’_t，y’_t)和第t次定位时车辆航向角θ’_t构成第t次定位时 车辆的位置数据，完成车辆第t次定位；

(7)采用h的当前值加1的和更新h，返回步骤(4)进行下一次定位，周而复始。

Claims (1)

1.一种基于松耦合的车辆单目视觉轮式里程计定位方法，其特征在于包括以下步骤：

(1)将车辆CAN报文解析模块的相邻两次采样之间的时间间隔记为△t₁，△t₁＝0.01s，将车载单目相机相邻两次采样之间的时间间隔记为△t₂，△t₂＝0.033s，创建一个用于存放关键帧的数组，将数组中存放的关键帧的数量记为变量n，初始状态时，所述的数组中不存在关键帧，此时所述的数组中存储的关键帧数量为0，n的取值为0；设计一个用于存储所述的车辆CAN报文解析模块每次采样获取的采样数据的时间戳的缓存，该缓存的容量为100，当超出其容量时，该缓存中已存时间戳将按照存储时间从早到晚的顺序被覆盖掉；所述的车辆CAN报文解析模块每次采样获取的采样数据包括车速数据和方向盘转角数据，所述的车辆CAN报文解析模块每次采样时的时刻采用UTC时间表示，将所述的车辆CAN报文解析模块每次采样时的时刻作为该次采样时获取的采样数据的时间戳保存到所述的缓存中，所述的车载单目相机每次采样时获取单目图像，所述的车载单目相机每次采样的时刻采用UTC时间表示；

(2)设定车辆CAN报文解析模块和车载单目相机的同步采样成功匹配次数变量，将其记为t，对t进行初始化赋值，令t＝0；

(3)同时开启所述的车辆CAN报文解析模块和所述的车载单目相机，所述的车辆CAN报文解析模块和所述的车载单目相机同时开始进行第1次采样，所述的车辆CAN报文解析模块和所述的车载单目相机后续每采样一次其采样次数增加1；

(4)假设所述的车载单目相机的当前采样次数为第h次，此时对所述的车辆CAN报文解析模块和所述的车载单目相机进行第h次同步采样匹配，具体匹配过程为：

4.1将所述的车载单目相机第h次采样得到的单目图像记为I_h，将所述的车载单目相机第h次采样的时刻记为t_h；

4.2对数组进行第h次更新，具体过程为：

如果h＝1，设定车载单目相机第h次采样的关键帧，将其记为^keyI_h，对^keyI_h进行赋值，令^keyI_h＝I_h，然后将^keyI_h存入所述的数组中作为所述的数组中的第1个数据，并将关键帧^keyI_h存入所述的数组中的存入时刻记为key_h，对key_h进行赋值，令key_h＝t_h；

如果h大于等于2，则判断n的当前值是否小于等于3以及

是否大于等于5*△t₂，其中，*为乘运算符号，

为车载单目相机进行第h次采样之前最后一次向所述的数组中存入关键帧的存入时刻，如果n的当前值小于等于3以及

大于等于5*△t₂这两个条件中的任意一个成立，则设定车载单目相机第h个采样时刻的关键帧，将其记为^keyI_h，对^keyI_h进行赋值，令^keyI_h＝I_h，然后将^keyI_h存入所述的数组中作为所述的数组中的最后一个数据，并将关键帧^keyI_h存入所述的数组中的存入时刻记为key_h，对key_h进行赋值，令key_h＝t_h；如果n的当前值小于等于3以及

大于等于5*△t₂这两个条件均不成立，则判断n的当前值是否大于10，如果n的当前值大于10，则将数组中的关键帧^keyI_h-n删除，否则，保持当前数组不变；

由此实现数组的更新，得到第h次更新后的数组；

4.3统计当前数组中关键帧的数量，采用统计的关键帧的数量更新n的值，将当前数组表示为(^keyI′₁,^keyI′₂,…,^keyI′_n-1,^keyI′_n)，其中^keyI′_l为当前数组中的第l个数据，l＝1,2，…，n；

4.4通过FAST特征点匹配算法对当前数组中每相邻两个关键帧进行特征点匹配，其中FAST特征点匹配算法中的特征点检测数量参数设定为200，具体过程为：

4.4.1、设定匹配变量，将该匹配变量记为l'；

4.4.2、对l'进行初始化，令l'＝2；

4.4.3、通过FAST特征点匹配算法对当前数组中第l'个关键帧^keyI′_l′和第l'-1个关键帧^keyI′_l′-1进行特征匹配，匹配得到第l'个关键帧^keyI′_l′的200个特征点数据，每个特征点数据包括该特征点在关键帧^keyI′_l′所在图像平面上的像素坐标以及在关键帧^keyI′_l′所在世界坐标系下的三维坐标，将第l'个关键帧^keyI′_l′的第i个特征点在关键帧^keyI′_l′所在图像平面上的像素坐标记为

将第l'个关键帧^keyI′_l′的第i个特征点在关键帧^keyI′_l′所在世界坐标系下的三维坐标记为

4.4.4、采用第l'个关键帧^keyI′_l′的200个特征点数据分别构建像素坐标向量和三维坐标向量，将第l'个关键帧^keyI′_l′的第i个特征点数据构建的像素坐标向量记为

将第l'个关键帧^keyI′_l′的第i个特征点数据构建的三维坐标向量记为

和

分别采用式(1)和式(2)表示为：

4.4.5、构建当前数组中的第l'个关键帧^keyI′_l′的相对位姿估计模型，将该模型采用式(3)、式(4)和式(5)表示为：

其中，△R'_l'表示当前数组中的第l'个关键帧^keyI′_l′相对于第l'-1个关键帧^keyI′_l′-1的位姿旋转矩阵，△t'_l'表示当前数组中的第l'个关键帧^keyI′_l′相对于第l'-1个关键帧^keyI′_l′-1的位姿平移向量；π(·)为相机的标准投影方程，ρ(·)为Huber鲁棒核函数，δ＝1，|| ||²为求二范数运算符号，| |为取绝对值符号，·为乘运算符号。

表示括号中算式取值最小时，变量△R'_l'和△t'_l'的值；

4.4.6、采用Levenberg-Marquardt(LM优化算法)对式(3)进行求解，得到△R'_l'和△t'_l'；

4.4.7、采用△R'_l'和△t'_l'构建当前数组中的第l'个关键帧^keyI′_l′相对于第l'-1个关键帧^keyI′_l′-1的位姿变换矩阵，将其记为△T′_l′，采用式(6)表示为：

其中，0_1x3为行向量[0 0 0]；

4.4.8、判断l'的当前值是否等于n，如果等于n则进入步骤4.4.9，如果不等于n，则采用l'的当前值加1的和更新l'的值，返回步骤4.4.3进行下一次特征匹配，直至l'的当前值等于n；

4.4.9设定车载单目相机第h次采样的位姿变换矩阵，将其记为T′_h，采用式(7)计算得到T′_h：

其中，

∏为累乘符号；

4.4.10将车载单目相机第h次采样的位姿变换矩阵T′_h，经过标准齐次坐标变换得到车载单目相机第h次采样的车辆坐标系下车辆位置坐标

和航向角数据θ_h ^mono，其中

为横坐标，

为纵坐标；

4.4.11判定第h次同步采样匹配是否成功，具体过程为：

将当前数组中最后存入的关键帧的存入时刻记为

将

与缓存中存储的所有时间戳进行匹配，查找与

相差小于5毫秒且相差最小的时间戳，如果找到，则第h次同步采样匹配成功，获取查找到的时间戳对应的车速数据和方向盘转角数据，进入步骤(5)，如果没有找到，则采用h的当前值加1的和更新h的值，重复步骤(4)，直到满足同步采样匹配成功条件；

5.先采用t的当前值加1的和更新t的值，然后构建第t次采样同步匹配成功的车辆数据，具体过程为：

5.1将当前采样同步匹配成功的车辆坐标系下车辆位置坐标记为(^monox_t、^monoy_t)，航向角数据记为^monoθ_t，车速数据记为v_t，方向盘转角数据记为α_t；

5.2将

的值赋值给^monox_t，将

的值赋值给^monoy_t，将θ_h ^mono的值赋值给^monoθ_t；将当前采样同步匹配成功后查找到的时间戳对应的车速数据赋值给v_t，方向盘转角数据赋值给α_t，第t次采样同步匹配成功的车辆数据构建完成；

6.对车辆进行第t次定位，具体过程为：

6.1通过车辆运动学航迹推测算法得到车辆运动学生成的运动轨迹，即第t次定位时车辆位置的估计坐标

和航向角估计值

表示第t次定位时车辆坐标系下车辆位置的横坐标估计值，

表示第t次定位时车辆坐标系下车辆位置的纵坐标估计值；其中车辆运动学航迹推测算法公式如下所示：

ds_t＝v_t-1·dt (9)

δ_ft＝α_t·η (11)

其中，

表示第t-1次定位时车辆坐标系下车辆位置的横坐标估计值，

表示第t-1次定位时车辆坐标系下车辆位置的纵坐标估计值，

表示第t-1次定位时车辆位置的航向角估计值，ds_t表示从第t次定位时相对于第t-1次定位时车辆行驶的距离，dθ_t表示第t次定位时相对于第t-1次定位时车辆位置的航向角变化量，v_t-1为第t-1次定位时车辆的车速，dt为车辆CAN报文解析模块的采样周期，dt＝△t₁，L为车辆轴距，δ_ft为第t次定位时车辆前轮转角，η为车辆的角传动比，η预先根据车辆型号通过现有成熟的实验方法标定得到，“·”为乘运算符号，当t＝1时，

v_t-1＝0，sin表示正弦函数，cos表示余弦函数，tan表示正切函数；

6.2通过扩展卡尔曼滤波算法对第t次采样同步匹配成功的车辆数据：车速v_t、方向盘转角α_t、^monox_t、^monoy_t和^monoθ_t进行数据融合，得到车辆第t次定位的位置数据，具体过程如下：

a.采用

和

构建第t次定位的状态向量，将该状态向量记为A_t，采用以下公式对A_t进行初始化赋值：

其中，在t＝1时的初始时刻，

b.采用v_t-1和δ_ft构建第t次定位的控制输入向量，将该控制输入向量记为B_t：

其中，v_t-1＝0；

c、建立第t次定位时带噪声的车辆运动学模型，将其向量表达式记为f(A_t,B_t)：

其中，N(·)为高斯白噪声生成函数，N(0,Q)表示采用高斯白噪声生成函数生成的维度为3×1的高斯白噪声向量，0为高斯白噪声生成函数的均值，Q为高斯白噪声生成函数的状态传播过程协方差矩阵，状态传播过程协方差矩阵Q为采用随机函数生成的维度为为3×3的矩阵，生成后为固定值。

d、将第t次定位时f(A_t,B_t)关于状态向量A_t的雅克比矩阵记为F_t，F_t采用式(15)表示为：

e、将状态传播后的协方差矩阵记为

采用公式(16)对状态传播后的协方差矩阵

进行更新：

其中P表示第t次定位之前状态协方差矩阵的最新值，上标T表示矩阵的转置；当t＝1时，P初始化为维度为3×3的单位矩阵，即：

f、建立第t次定位时的观测模型：

其中，Z_t为第t次定位时的观测模型，

为第t次定位时的观测函数；N(·)为高斯白噪声生成函数，N(0,R)表示采用高斯白噪声生成函数生成的高斯白噪声向量，N(0,R)的维度为3×1，0为高斯白噪声生成函数的均值，R为观测协方差矩阵，观测协方差矩阵R的维度为3×3，观测协方差矩阵R为：

g、将第t次定位时观测函数

关于状态向量A_t的雅克比矩阵记为H_t，H_t采用式(21)表示为：

h、将第t次定位时的观测残差记为y_t，计算第t次定位时的观测残差y_t：

i、将第t次定位时的卡尔曼增益记为K_t，计算第t次定位时的卡尔曼增益K_t：

上式中，

的取值为其当前最新值；上标“-1”表示矩阵的逆运算；

j、对状态向量A_t和状态协方差矩阵P进行更新：

其中，I为维度为3×3的单位矩阵，

的取值均为其当前最新值；

k、将(x′_t，y′_t)作为第t次定位时车辆位置坐标，将θ′_t作为第t次定位时车辆航向角，采用第t次定位时车辆位置坐标(x′_t，y′_t)和第t次定位时车辆航向角θ′_t构成第t次定位时车辆的位置数据，完成车辆第t次定位；

(7)采用h的当前值加1的和更新h，返回步骤(4)进行下一次定位，周而复始。