[发明专利]一种车辆的位姿确定方法、装置、车辆和存储介质

权利要求书

1.一种车辆的位姿确定方法，其特征在于，所述车辆包括轮速计、相机和惯性传感器IMU，所述方法包括：

根据相机采集的相邻两帧图像和所述轮速计检测得到的预估速度，确定预估尺度；

获取基于所述轮速计确定的第一残差项，所述第一残差项包括：所述轮速计的预估速度与车辆真实速度的差，和所述轮速计的估计角速度与车辆真实角速度的差；

获取基于所述IMU确定的第二残差项，所述第二残差项包括所述IMU的预积分旋转姿态与真实旋转姿态的差、所述IMU的预积分速度与真实速度的差，和所述IMU的预积分位置和真实位置的差；

获取基于所述相机采集的多帧图像的空间点和所述预估尺度确定的第三残差项，所述第三残差项包括：相机估计位置与相机真实位置的差，和相机估计姿态与相机真实姿态的差；

对所述第一残差项、所述第二残差项和所述第三残差项进行非线性优化处理，以得到车辆位置和车辆姿态。

2.根据权利要求1所述的车辆的位姿确定方法，其特征在于，所述根据相机采集的相邻两帧图像和所述轮速计检测得到的预估速度，确定预估尺度，包括：

根据所述轮速计检测得到的预估速度v和相邻两帧图像的时间差Δt及公式Δx＝vΔt，确定预估位移Δx；

针对相邻两帧图像，基于对极约束确定相对尺度下的相对平移t；

根据所述预估位移Δx和所述相对平移t及公式Δx≈st，确定预估尺度s。

3.根据权利要求1所述的车辆的位姿确定方法，其特征在于，所述对所述第一残差项、所述第二残差项和所述第三残差项进行非线性优化处理，以得到车辆位置和车辆姿态，包括：

采用非线性最小二乘，求解所述第一残差项、第二残差项、第三残差项，以得到车辆位置和车辆姿态。

4.根据权利要求1所述的车辆的位姿确定方法，其特征在于，

所述第一残差项为：

R_oi＝ω_ov(|v_i-v_oi|)+ω_ow(|w_i-w_oi|)；其中，ω_ov和ω_ow为轮速计测量的速度和角速度的权重，v_oi为轮速计的预估速度，v_i为车辆真实速度，w_oi为轮速计的估计角速度，w_i为车辆真实角速度。

5.根据权利要求1所述的车辆的位姿确定方法，其特征在于，

所述第二残差项为：

R_ik＝[ΔX,ΔV,ΔR,ΔB_a,ΔB_w]^T、

其中，ΔX为位置差，ΔV为速度差，ΔR为旋转姿态差，ΔB_a为加速度偏置差，ΔB_w为角速度偏置差，为车辆真实位置，为车辆真实姿态，Δt为两帧图像时间间隔[t_k,t_k+1]，为IMU的预积分位置，为IMU的预积分速度；为IMU的预积分旋转姿态；b_a为图像的加速度偏置噪声、b_w为图像的角速度偏置噪声。

6.根据权利要求1所述的车辆的位姿确定方法，其特征在于，

所述第三残差项为：

其中为相机的旋转，为相机的平移，s_i为空间点对应的预估尺度，为车辆真实位置，为车辆真实姿态，为相机相对于IMU的安装姿态，为相机相对于IMU的安装位置，K为相机内参矩阵。