[发明专利]一种基于视觉和IMU的车辆融合定位方法及车载终端

权利要求书

1.一种基于视觉和IMU的车辆融合定位方法，其特征在于，包括：

获取相机设备在第一时刻采集的道路图像，并基于所述道路图像中的道路特征与预设地图中的道路特征的匹配结果，确定在所述第一时刻的第一视觉位姿；

获取惯性测量单元IMU在第一时刻采集的IMU数据，并基于所述IMU数据，推算车辆在所述第一时刻的第一IMU位姿；

获取在多个第二时刻确定的多个第二视觉位姿，以及在多个第二时刻确定的多个第二IMU位姿；其中，多个第二时刻为所述第一时刻之前的时刻；

根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中同一时刻的各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数，通过改变所述估计融合位姿的取值，对所述相似约束优化函数进行迭代求解，将所述相似约束优化函数取最优化解时的估计融合位姿，确定为所述车辆在所述第一时刻融合后的定位位姿；

其中，所述第一轨迹为所述第一视觉位姿和多个第二视觉位姿对应的轨迹，所述第二轨迹为所述第一IMU位姿和多个第二IMU位姿对应的轨迹；

所述根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建相似约束优化函数，通过改变所述估计融合位姿的取值，对所述相似约束优化函数进行迭代求解，将所述相似约束优化函数取最优化解时的估计融合位姿，确定为所述车辆在所述第一时刻融合后的定位位姿的步骤，包括：

根据第一轨迹与第二轨迹中各个位置点之间的融合变换系数，以及所述第一轨迹和所述第二轨迹中各个位置点与估计融合位姿之间的相似关系，构建以下相似约束优化函数E：

其中，所述S(t_i)和R_s(t_i)分别为所述第一轨迹与第二轨迹中t_i时刻位置点之间的融合变换系数中的缩放比例和旋转矩阵，所述P_f(t_i)和R_f(t_i)分别为t_i时刻的估计融合位姿中的位置和姿态，所述P_v(t_i)和R_v(t_i)分别为所述第一轨迹中t_i时刻位置点的位置和姿态，所述Po(t_i)和Ro(t_i)分别为所述第二轨迹中t_i时刻位置点的位置和姿态，所述t₁为所述第一时刻，所述t_n为多个第二时刻中的最早时刻，所述n为所述第一时刻和多个第二时刻的总数量；

确定所述估计融合位姿的初始值，确定所述R_f,P_f,S,R_s的初始值；

根据所述估计融合位姿的当前取值和所述R_f,P_f,S,R_s的当前取值，确定所述相似约束优化函数的当前函数值；

获取所述相似约束优化函数的上一函数值，判断所述上一函数值与所述当前函数值的差值绝对值是否大于预设差值阈值；

如果是，则调整所述估计融合位姿的取值以及所述R_f,P_f,S,R_s的取值，返回执行所述根据所述估计融合位姿的当前取值和所述R_f,P_f,S,R_s的当前取值，确定所述相似约束优化函数的当前函数值的步骤；

如果否，则将所述估计融合位姿的当前取值，确定为所述车辆在所述第一时刻融合后的定位位姿。

2.如权利要求1所述的方法，其特征在于，采用以下方式确定各个第二时刻：

将早于所述第一时刻的多个时刻作为待选择时刻，确定从各个待选择时刻到所述第一时刻所述车辆的行驶距离；

当所述车辆的行驶距离满足预设距离条件时，将所述车辆的行驶距离对应的待选择时刻确定为时刻最早的第二时刻t_n，将(t₁，t_n]区间中的各个时刻作为各个第二时刻。