[发明专利]惯性传感器和多鱼眼相机联合的地下停车场车辆定位方法

权利要求书

1.一种惯性传感器和多鱼眼相机联合的地下停车场车辆定位方法，其特征是按如下步骤进行：

步骤1：建立停车场初始三维可视化地图；

步骤1.1：利用鱼眼相机获取一幅停车场图像，并利用式(1)对所述停车场图像中的每个像素点进行校正，得到校正后的停车场图像：

式(1)中，x、y分别表示所述停车场图像中一个像素点的横、纵坐标；r表示横纵坐标x和y平方的和；x_dis、y_dis表示校正后的停车场图像中一个像素点的横、纵坐标；k₁,k₂,k₃表示鱼眼相机的三个内参，p₁,p₂表示鱼眼相机的畸变参数；

步骤1.2：在停车场中建立大地坐标系n；

步骤1.3：以大地坐标系n为基准测量所有m个鱼眼相机中心的坐标，得到鱼眼相机位置的坐标数据集以及相对位置关系，其中，CC_i表示第i个鱼眼相机中心的坐标，所述相对位置关系包括：旋转矩阵和平移矩阵R_pq表示第p个鱼眼相机和第q个鱼眼相机之间的旋转矩阵；T_pq表示第p个鱼眼相机和第q个鱼眼相机之间的平移矩阵，分别表示第p个鱼眼相机和第q个鱼眼相机之间在大地坐标系n中x轴、y轴、z轴上的坐标位置；

步骤1.4：应用双目测距原理，计算两两一组鱼眼相机的深度信息，并生成点云地图M_pq：

步骤1.4.1：利用式(2)计算第p个鱼眼相机和第q个鱼眼相机之间的左、右旋转矩阵和

步骤1.4.2：利用式(3)计算第p个鱼眼相机和第q个鱼眼相机之间的标准校正矩阵

式(3)中，e₁、e₂、e₃表示标准校正矩阵中的三个向量，并有：

步骤1.4.3：利用式(5)得到第p个鱼眼相机和第q个鱼眼相机之间的左、右立体校正矩阵和

步骤1.4.4：根据左、右立体校正矩阵和利用SGBM算法对校正后的停车场图像进行处理，得到深度信息，并生成第p个鱼眼相机和第q个鱼眼相机之间的点云地图M_pq；

步骤1.5：以第o个鱼眼相机为基准，利用式(6)对点云地图M_pq进行坐标变换，得到点云地图数据集M_po：

M_po＝M_pqR_po+T_po (6)

式(6)中，R_po表示第p个鱼眼相机和第o个鱼眼相机之间的旋转矩阵；T_po表示第p个鱼眼相机和第o个鱼眼相机之间的平移矩阵；

以第o个鱼眼相机为基准对齐所有的点云地图数据集，从而得到数据集M_o，且

步骤1.6：将数据集M_o进行高斯滤波处理，生成去除噪点后的停车场初始三维可视化点云地图M_w0；

步骤2：通过惯性传感器确定停车场内车辆相对于起点的相对位置；

步骤2.1：车辆在大地坐标系n中的初始位置设为W_g0＝[x_g0,y_g0,z_g0]；

以移动车辆的质心作为坐标原点，以移动车辆水平运动方向为X轴，以垂直于X轴的方向为Y轴，建立车辆自身坐标系c；

步骤2.2：在t时刻利用惯性传感器获得在车辆自身坐标系c下的车辆加速度a_t,c、角速度ω_t,c，t+1时刻的车辆加速度a_t+1,c、角速度ω_t+1,c；

将惯性传感器固联在车身上并与车辆自身坐标系c的三轴重合，利用式(7)得到在大地坐标系n下t时刻的车辆加速度a_t,n、t+1时刻车辆加速度a_t+1,n：

式(7)中，Q_t、Q_t+1分别为t时刻和t+1时刻相应的姿态变换矩阵，B_a为测量偏差，g为重力加速度，并有：

Q_t+1＝Q_t(ω′_t,cΔt) (8)

式(8)中，Δt表示t时刻和t+1时刻之间的差值；ω′_t,c表示大地坐标系n下的t时刻到t+1时刻的平均角速度，并有：

步骤2.3：利用式(10)得到t+1时刻的速度v_t+1和位置W(x_t+1,y_t+1,z_t+1)：

式(10)中，a′_t,n为大地坐标系n下的t时刻到t+1时刻的平均加速度，θ为航向角，x_t,y_t,z_t为t时刻的车辆在大地坐标系n下的坐标，当t＝0时，有[x_t,y_t,z_t]＝[x_g0,y_g0,z_g0]，x_t+1,y_t+1,z_t+1为t+1时刻的车辆在大地坐标系下的坐标，并有：

步骤2.4：根据式(10)得到任意时刻车辆相对于出发点的位置信息，并记车辆在任意时刻通过惯性传感器得到的定位坐标为W_g＝[x_g,y_g,z_g]；

步骤3：收集停车场鱼眼相机的图像数据，并利用所述图像数据定位车辆的位置W_c＝[x_c,y_c,z_c]；

步骤3.1：对鱼眼相机所获取的实时图像进行高斯滤波处理，去除图像噪点，从而得到去燥后的实时图像；

步骤3.2：按照步骤1的过程进行处理，并输出实时三维可视化点云地图M_w1；

步骤3.3：比较实时三维可视化点云地图M_w1和停车场初始三维可视化点云地图M_w0，由式(12)计算得到实时运动车辆的点云数据集其中，c_j为点云数据集M_c中第j个三维坐标向量；h为组成云数据集M_c的三维坐标向量的个数；

M_c＝M_w1-M_w0 (12)

步骤3.4：利用式(13)得到所定位的车辆位置W_c的三维坐标[x_c,y_c,z_c]，并作为重心位置：

步骤4：数据融合：

利用式(14)计算最终车辆位置信息W_D：

W_D＝kW_g+(1-k)W_c (14)

式(14)中，k为权重，且0＜k＜1。