[发明专利]一种基于立体相机与IMU融合的低速无人车定位方法及系统

权利要求书

1.一种基于立体相机与IMU融合的低速无人车定位方法，其特征在于，所述方法包括以下步骤：

步骤1：传感器数据获取及时空同步，通过车载传感器采集无人车当前的环境信息，包括立体相机获取的图像信息以及六轴IMU获取的三轴角速度和三轴加速度数据，定义立体相机、IMU、无人车车体和世界坐标系、并完成立体相机与IMU坐标系之间的时空同步；

步骤2：系统状态向量构建，在MSCKF框架下构建系统状态向量，并将立体相机与六轴IMU间的联合外参加入到系统状态向量中以实现在线空间标定；

步骤3：系统初始化判断，若未完成初始化，则执行初始化，若已经完成初始化，则执行步骤4；

步骤4：车辆运动信息传播，根据IMU的运动学模型和测量模型构建IMU误差状态的线性化连续时间模型及系统状态协方差矩阵，然后分别执行系统状态向量及协方差矩阵传播，从而完成车辆运动信息传播；

步骤5：图像特征提取及跟踪，对获取到的新一帧图像执行FAST(Features fromAccelerated Segment Test)角点提取，并在不同帧图像和立体相机间执行KLT光流跟踪及匹配；

步骤6：立体相机状态及协方差增广，利用通过步骤4得到的IMU最新位姿估计值以及立体相机与IMU间的联合外参在线估计值计算出新一帧图像对应的相机位姿，并将新一帧图像对应的相机位姿增广系统状态向量及协方差矩阵，完成立体相机状态及协方差增广；

步骤7：车辆运动信息校正，根据立体相机观测构建系统残差观测模型，并按照扩展卡尔曼滤波器(Extended Kalman Filter,EKF)更新方式执行系统状态向量及协方差更新，从而完成车辆运动信息校正；

步骤8：滑动窗口状态管理，对系统状态向量中含有固定相机位姿个数的滑动窗口内的图像帧进行边缘化以维持滑动窗口的大小不变，从而达到节约计算量的目的；

步骤9：IMU/无人车车体坐标系位姿转换，将IMU坐标系下表示的车辆位姿转换为无人车车体坐标系下的车辆位姿，从而完成位姿转换；

步骤10：无人车位姿估计值实时输出，实时输出无人车的六自由度位姿估计值，从而实现低速无人车定位。

2.如权利要求1所述的基于立体相机与IMU融合的低速无人车定位方法，其特征在于，所述步骤2包括以下步骤：

步骤2.1：构建系统真实状态向量X，包括含有立体相机与IMU间联合外参的当前IMU状态X_I和一个含有N个过去相机位姿的滑动窗口X_C两部分：

其中，表示描述从世界坐标系{W}到IMU坐标系{I}间旋转的单位四元数，Wv_I表示IMU相对于{W}的速度，^Wp_I表示IMU相对于{W}的位置，b_g表示影响陀螺仪测量的偏差，b_a表示影响加速度计测量的偏差，和分别表示描述IMU坐标系{I}与左目相机坐标系{C_L}间的相对旋转和平移、即联合外参，分别表示滑动窗口内的相机C_i相对于{W}的姿态和位置；

步骤2.2：根据步骤2.1中构建的系统真实状态向量X，得到系统误差状态向量

其中，对于速度、位置和偏差，误差值等于真实值减去估计值；对于四元数，误差值真实值和估计值之间满足：其中表示四元数乘法，且采用误差轴角δθ表示误差四元数两者之间满足如下关系：