[发明专利]一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法

权利要求书

1.一种面向港口环境的多源数据融合的无人驾驶汽车实时定位方法，其特征在于，包括以下步骤：

(1)预测无人车的当前时刻状态：通过惯性传感器观测无人车惯性信息，根据前一时刻的状态和当前时刻的惯性传感器的测量值，得出服从高斯分布的当前时刻的预测状态的均值μ′和协方差∑′；

(2)点云配准：将所述步骤(1)中得到的无人车的预测状态的均值μ′和协方差∑′作为预测值，对先验点云地图与当前场景点云进行点云匹配，得到当前场景点云和场景之间的坐标变换矩阵，利用该变换矩阵对匹配前的预测值进行转换，输出匹配后配准结果，所述配准结果包括点云和无人车的位置信息；

(3)状态校正：根据所述步骤(1)的预测状态数据和步骤(2)的配准结果，校正得到校准后的无人车状态。

2.根据权利要求1所述的无人驾驶汽车实时定位方法，其特征在于，所述步骤(1)中预测出服从高斯分布的当前时刻的状态信息的步骤如下：

(1-1)对惯性传感器的滤波参数初始化，确定在当前时刻t的噪声协方差R_t和惯性元件的偏置参数b_t，计算出当前时刻t的协方差值∑，由如下公式计算；

∑＝VDV^T；

其中，D为对角阵，V为特征向量，R_t为惯性传感器在测量过程中环境和系统不可测变量对测量结果造成的干扰所服从的高斯分布的协方差矩阵；

(1-2)对前一时刻状态进行采样，计算出预测状态的采样点χ^[i]的分布，由如下公式计算：

χ^[0]＝μ

λ＝α²(n+κ)-n

其中，设定采样数量为2n+1，n是状态的维数，设定状态维数为10，μ、α、κ为确定σ点分布在均值范围内的比例系数；

(1-3)对所述步骤(1-2)中的采样点χ^[i]根据预设的运动模型函数进行状态转移，得到新的采样点为y^[i]，由如下公式计算：

y^[i]＝f(χ^[i]，ctr_t)；

ctr＝[acc，gyr]^T；

x_t-1＝[p_t-1，q_t-1，v_t-1]^T

[p_t，q_t，v_t]^T＝f_t＝[p_t-1，q_t-1，v_t-1]^T·[1，Δq，1]·[v_t-1d_t，0，acc_t·Δt]^T；

其中，y^[i]＝f(χ^[i]，ctr_t)为运动模型函数，ctr_t为当前时刻的惯性元件传感器测量值，acc为无人车加速度值，gyr为无人车角速度值，x_t-1为上一时刻无人车状态，p_t-1为上一时刻无人车的位置，q_t-1为上一时刻无人车的姿态，v_t-1为上一时刻无人车的速度，p_t为当前时刻t无人车的位置，q_t当前时刻t无人车的姿态，v_t为当前时刻t无人车的速度，Δq_t为无人车在当前时刻与上一时刻的时间间隔Δt内产生的旋转，分别为无人车在当前时刻t的x、y、z方向上的角速度值；v_t-1d_t是速度在时间上的积分；Δq为上一时刻和当前时刻无人车的姿态变化；

(1-4)对所述步骤(1-3)中得到采样点y^[i]进行加权统计，计算得出预测状态的均值μ′和协方差∑′，由如下公式计算：

其中，每个采样点有2个权值，为用于计算均值的权值，为用于计算协方差的权值，β为确定σ点分布在均值范围内的比例系数；y′是y^[i]的导数。