[发明专利]一种基于激光雷达的AGV室内地图构建方法及系统

权利要求书

1.一种基于激光雷达的AGV室内地图构建方法，其特征在于，包括以下步骤：

S1、激光雷达探测周围环境数据形成360°二维点云数据，同时收集里程计信息；

S2、采集激光雷达探测的数据和里程计数据进行预处理，随后利用扩展信息滤波算法进行位姿估计和更新，并将数据上传到地图构建单元；

激光雷达数据预处理过程方法为：

从获取的360°二维点云数据中选取AGV正前方180°范围内的二维点云数据，所得环境坐标向量为：

z^k＝(d_k,θ_k) k＝1,2,3,…,N

其中：z^k为点云数据集，d_k为激光雷达在θ_k角度上获取的距离值；

里程计数据预处理得到机器人运行速度、转动角度和运动方程，具体方法为：

假设，AGV左驱动轮速度为v_l，右驱动轮速度为v_r，两驱动轮间距为d，则：

机器人速度表示为：

机器人转动角度表示为：

机器人运动方程为：其中：v_x为机器人在x轴上的速度，v_y为机器人在y轴上的速度，ω为机器人角速度；

利用扩展信息滤波算法进行位姿估计和更新的过程如下：

1)估计过程

扩展信息滤波的估计过程如下，首先得到预测的机器人位姿x_k+1并加入到环境暂态向量中，环境暂态向量X_k变为增量向量即然后求x_k+1的边缘概率，将x_k从增量向量中去除，得到向量

(1)预测得到x_k+1，并加入到环境暂态向量中

假设运动方程为：

其中，G为函数g雅可比矩阵；μ_k为x_k的估计值；w_k为高斯白噪声；

(2)求x_k+1的边缘概率

其中：

2)更新过程

假设观测方程为：l_k＝h(x_k,z_k)+v_k＝h(μ_k)+H(x_k-μ_k)+v_k；其中，l_k为增加的特征点，h为观测函数，H为雅克比矩阵；v_k为高斯白噪声；μ_k为x_k的估计值；

在k时刻，对应的更新为：

根据贝叶斯公式，有

P(x_k,M|z^k,u^k)∝P(z^k|x_k,M)P(x_k,M|z^k-1,u^k) (8)

因此更新后P(x_k,M|z^k+1,u^k)＝N^-1(η_k,Λ_k)，扩展信息滤波通过估计和更新过程不断更新信息矩阵Λ和信息向量η，从而更新机器人当前位姿；

S3、地图构建单元利用处理后的激光雷达和里程计数据以设置频率实时建立栅格地图并保存在本地，其中栅格地图用于后续机器人运行时的导航地图。

2.一种基于权利要求1所述方法的AGV室内地图构建系统，其特征在于，包括数据采集部分、控制部分和地图构建部分，其中数据采集部分包括激光雷达、里程计和数据采集单元，控制部分包括直流电机、电机驱动模块和控制单元，地图构建部分包括地图构建单元和数据处理单元；激光雷达、数据采集单元、数据处理单元、地图构建单元依次连接；里程计、电机驱动模块分别与控制单元连接，控制单元进一步与数据处理单元连接；里程计、直流电机分别与电机驱动模块连接。