1.一种基于车辆非完整约束的多相机运动状态估计方法,其特征在于,包括以下步骤:步骤1、多相机统一坐标系:完成多相机系统的联合标定,确定多相机系统中相机的相对位置关系,获取相机间的刚性约束;;
步骤2、单目SLAM的构建:用于获得初步的相机帧间位姿变换并通过三角化恢复出初始地图点;
步骤3、车辆运动模型的建立:车辆在平面运动具有三个自由度,对车辆在直线运动和转弯状态下分别进行建模,根据Ackerman转向原理计算出车辆和相机的旋转角θ,平移角和绝对偏移量ρ和λ的数学关系,获得多相机的运动学约束;
步骤4、多相机系统和车辆的运动估计:根据相机间的刚性约束和运动学约束,采用帧间的对极约束求解尺度信息和位姿变换矩阵,并将各个方向点云利用相机间的刚体约束配准,完成初步的位姿估计及局部点云地图生成;
步骤5、全局非线性优化:相机的帧间位姿恢复和特征点匹配的重投影误差采用光束法平差进行优化,获取全局点云地图,完成车辆及车载多相机系统的位姿估计。
2.根据权利要求1所述的一种基于车辆非完整约束的多相机运动状态估计方法,其特征在于,所述步骤1的多相机统一坐标系是采用四台广角相机固定在车辆的前向、左向、后向、左向的四个方向上,取前向相机坐标系为车辆坐标系,利用张正友的平面模板标定法建立多相机系统中每两两相机间的R、T关系,并将其统一在同一车辆坐标系下。
3.根据权利要求2所述的一种基于车辆非完整约束的多相机运动状态估计方法,其特征在于,所述步骤1具体包括以下步骤:步骤1‑1.固定相机传感器到车辆上,其中每个相机均为广角相机。取前向相机坐标系为车辆坐标系,利用张正友的平面模板标定法建立多相机系统中每两两相机间的旋转矩阵R和平移矩阵T的关系,记其相互之间的约束关系为[R1|T1]、[R2|T2]、[R3|T3];
步骤1‑2.采用100mm*100mm的棋盘格对广角相机进行标定,拍摄多张棋盘格角点图片,获取相机内参fu、fv、u0、v0以及相机外参R、T矩阵,可表示为如下所示公式。其中fu、fv分别表示相机的横、纵焦距,u0、v0表示相机光心,s为坐标轴倾斜参数,理想情况下为0;
步骤1‑3.根据相机标定获得的外参[R|T]矩阵,将其统一到车辆坐标系下;
步骤1‑4.由步骤1‑2获取相机的畸变系数k1,k2,k3,p1,p2,广角相机的畸变主要是径向桶形畸变,采用Opencv中的initUndistortRectifyMap和remap函数建立畸变映射到图像像点的映射关系;
步骤1‑5.重复上述步骤1‑2至步骤1‑4,完成车左、车右、车后广角相机标定和畸变矫正工作。
4.根据权利要求1‑3任一项所述的一种基于车辆非完整约束的多相机运动状态估计方法,其特征在于,所述步骤2单目SLAM的构建具体包括:首先选定一个ORB特征大于100的图像作为参考帧,匹配当前帧与参考帧的ORB特征,采用多线程同步计算参考帧到当前帧的Homography模型和Fundamental模型,采用RANCAC框架进行迭代,选择迭代分数最高的记为SH、SF,则有 当RH>0.45时,表示二维平面和低视差的情况,选择单应矩阵求解帧间R、T矩阵;否则选择基础矩阵,最后,以第一帧为世界坐标系采用直接线性变换DLT三角化测量匹配点对应的三维信息,生成局部点云。
5.根据权利要求4所述的一种基于车辆非完整约束的多相机运动状态估计方法,其特征在于,所述步骤3中,若前向相机和后向相机的位姿变换矩阵Rf=Rb, 则其为直线运动(其中Rb、Rf、Tf、Tb分别表示前向相机和后向相机的旋转矩阵和平移矩阵,表示归一化数组的(欧几里德)L2‑范数);否则为转弯状态,车辆在行驶过程中转弯可以近似为Ackerman转向模型;根据微分几何关系,获取相邻帧的车辆偏航角θ、相机偏航角 和车辆绝对偏移量ρ和相机绝对偏移量λ的数学关系,为多相机系统提供运动学约束,将刚性约束和运动学约束联立求取车辆及多相机系统的尺度信息。
6.根据权利要求5所述的一种基于车辆非完整约束的多相机运动状态估计方法,其特征在于,所述步骤3车载多相机系统运动模型的建立具体包括:汽车在平面上运动,有三个自由度,即它在平面上的位置和方向,车辆通过Ackerman转向原理对左右前轮在转弯时施加不同的转向角度来确保了车辆的平稳运动,固定在车辆上的多相机系统和车辆都在局部进行着平面圆周运动。运动学约束推导步骤如下:步骤3‑1.车辆平面运动主要涉及到三个参数,分别为旋转角θ,偏航角 和车辆偏移量ρ,取前向相机相机坐标系和车辆坐标系一致,则后向相机在车辆坐标系下的坐标可以表示为C3=[0 ‑e 0],e表示后向相机相对前向相机的距离。则其位置1的位置矩阵为,记为P1:步骤3‑2.若汽车前向相机与后向相机的位姿Rf=Rb, 则其为直线运动,遵循直线运动模型;否则则为转弯状态,遵循Ackerman转向原理,其在t时刻和t+1时刻的相对运动R、T可以表示为:
其中 表示车辆偏航角,θ表示车辆运动圆周角。
步骤3‑3.由此可得,在车辆坐标系中后向相机在位置2的位置矩阵为式3.3,为了描述后向相机从位置1到位置2的相对运动,将P2在车辆左边系下的矩阵转换到后向相机坐标系C3。将其在坐标系C3下的位置矩阵定义为P2',则P2'可以表示为式3.4,因为车辆上的每一点都随着车辆进行圆周运动,所以旋转矩阵R2和R′2是等价的,因此T′2可以表示为式3.5,因此P1到P′2的本质矩阵E=[T′2]×R′2可以表示为式3.6;
步骤3‑4.根据后向相机从位置1运动到位置2在相机坐标系C3的T′2、E及可以表示为式
3.7;
步骤3‑5.由步骤3‑3和步骤3‑4可知,式3.5和式3.7是等价的,由此可得式3.8,进而求得ρ、λ和 θ角的数学关系。
其中ρ代表前向相机偏移量、λ代表后向相机偏移量, 表示车辆偏航角,θ表示车辆运动圆周角。
7.根据权利要求6所述的一种基于车辆非完整约束的多相机运动状态估计方法,其特征在于,所述步骤4车辆及车载多相机系统运动状态估计具体包括:步骤4‑1.根据车辆运动过程中车载多相机系统在相邻两帧的对极约束关系,设前一帧T
k地图点在其相机坐标系的位置为p=(x,y,z) ,后一帧同一地图点在其坐标系的坐标p'=T
(x',y',z) ,则有:
T
p'Ep=0(4.1)
代入式3.7的本质矩阵,可得:
给定n个地图点,可以直接利用系数矩阵[xz′ zx′ zy′ yz′]奇异值分解来间接计算旋转角θ和偏航角 从而获得车辆运动的偏移量;
步骤4‑2.由相机间相互关系可知,相机间的刚性约束分别[R1|T1]、[R2|T2]、[R3|T3],由步骤2可得多相机系统在四个方向的点云,首先以相同标准提取点云集中的关键点,计算其在当前点云集中3D特征描述子和坐标的位置,利用相机间的变换矩阵进行点云的配准生成周围环境的地图点信息,将相机间刚性约束其和运动学模型提供的尺度信息结合,从而完成车辆及多相机系统的初步位姿估计及局部点云地图生成。
8.根据权利要求7所述的一种基于车辆非完整约束的多相机运动状态估计方法,其特征在于,所述步骤5)具体包括:
步骤5‑1.利用光束法平差BA对空间地图点进行极大似然估计,通过迭代方式逐渐收敛到目标函数的最小值,求取最优的估计,消除特征点匹配中的错误估计;
其中qij代表观测像素坐标,P(Ci,Xj)代表投影值,即根据位姿计算得到的像素坐标。wij代表指标变量,若点j在相机i中可见则为1,否则为0。