[发明专利]一种激光雷达定位及地图创建方法

权利要求书

1.一种激光雷达定位及地图创建方法，其特征在于：基于里程计和激光雷达匹配的联合优化定位，获取全路径后验状态，剔除错误激光雷达匹配信息后进行再估计，从而适应颠簸的环境；具体步骤包括：

步骤1：建立激光雷达似然域模型；

步骤2：对步骤1所生成的似然域模型进行简化；

步骤3：对似然域模型中的似然域地图进行曲面拟合；

步骤4：通过负对数联合后验概率密度的方法，将最大后验概率转换为具有最小二乘形式的优化，从而生成联合优化的目标函数；

步骤5：选择优化算法用于解出目标函数的最优值；

所述步骤4中将联合后验概率最大似然估计转换为具有最小二乘形式的优化后得到：

L≤x≤U

F(x)为残差函数，包括基于激光雷达的测量残差函数F(x)_z和基于里程计的控制残差函数F(x)_u，x是待估计状态，L、U为优化是状态的边界；

基于激光雷达的测量残差函数可以通过拟合后的似然域地图得到：

为t时刻第i个激光雷达观测数据，x_t为t时刻的状态，m为占用栅格地图信息；

联合优化的目标函数为：

2.如权利要求1所述的一种激光雷达定位及地图创建方法，其特征在于：所述步骤2中采用高斯核函数对栅格地图进行局部卷积代替似然域模型。

3.如权利要求2所述的一种激光雷达定位及地图创建方法，其特征在于：在似然域模型进行简化的过程中忽略测量失效噪声以及无法解释的随机测量噪声。

4.如权利要求1所述的一种激光雷达定位及地图创建方法，其特征在于：似然域地图进行曲面拟合时选择双线性插值算法和双三次卷积插值算法其中的任一种。

5.如权利要求4所述的一种激光雷达定位及地图创建方法，其特征在于：选择采用双三次卷积插值算法对似然域地图进行曲面拟合，使其为优化算法提供准确的导数信息。

6.如权利要求1所述的一种激光雷达定位及地图创建方法，其特征在于：当无人机动平台控制方式为前轮转向后轮驱动时，控制残差函数表示为：||F(x)_u||²＝(x_t-f(x_t-1，u))^TΩ^-1(x_t-f(x_t-1，u))

其中观测为由后轮提供的速度信息v_i,k和由前轮提供的角度θ_i,k，统称为速度观测u。

7.如权利要求1所述的一种激光雷达定位及地图创建方法，其特征在于：所述步骤5中所选的优化算法为高斯-牛顿迭代法、莱温伯格马格纳特迭代法其中之一。