CN110389590A - 一种融合2d环境地图和稀疏人工地标的agv定位系统及方法 - Google Patents

Abstract

本发明公开了一种融合2D环境地图和稀疏人工地标的AGV定位系统及方法，该系统包括车载设备、人工地标以及2D环境地图；2D环境地图为车载设备根据当前环境状态所建；人工地标包括位置信息以及编号信息；每个人工地标在环境地图中具有唯一编号；车载设备包括激光雷达、里程计、陀螺仪和定位单元；定位单元包括激光数据处理模块、传感器数据存储模块、点云匹配建图模块、环境定位模块、人工地标定位模块和定位结果融合模块；由点云匹配建图模块进行建图，得到2D环境地图后由环境定位模块以及人工地标定位模块得到定位结果，最后再由定位结果融合模块融合定位结果并给出最终定位信息，实现AGV小车在环境中的高精度定位。

Description

一种融合2D环境地图和稀疏人工地标的AGV定位系统及方法

技术领域

本发明属于自动控制领域，尤其涉及一种融合2D环境地图和稀疏人工地标的AGV定位系统及方法。

背景技术

目前自动导引车辆已经成为企业内部物流、智能工厂、先进物流的重要设备，在提升企业内部物流的自动化、信息化和智能化方面起着非常重要的作用。而自动导引和物料自动搬运的前提是车辆能够准确定位。目前常见的激光SLAM定位因为计算机算力以及概率地图最小分辨率的限制导致定位精度无法满足某些需要高精度定位的工业场景的要求，并且在某些环境较为开阔，超出激光雷达扫描范围的场景下激光SLAM无法完成满足运动控制要求的定位,而人工地标定位需要在环境中添加大量的人工地标，增加导航成本，并且当AGV车辆的激光雷达安装在较低的高度时人工地标很容易被遮挡进而造成定位精度下降。因此需要探索相对比较灵活、可靠而且精度高的AGV定位方式。

发明内容

本发明的目的在于克服现有激光雷达定位技术的不足，提供一种融合2D环境地图和稀疏人工地标的AGV高精度定位系统及方法，该系统采用2D环境地图，在AGV工作的环境中需要高精度定位的场景以及较为开阔的场景中放置人工地标，控制AGV在工作环境内移动使得激光雷达扫描周围的环境点完成2D环境地图的建立。环境地图建立完成后控制AGV在其工作区域内移动，通过识别激光雷达数据中的光强数据来标定当前环境中的地标并且编号记录识别出的人工地标位置，标定完成后使用人工地标定位和环境定位两种定位模式进行定位并对定位结果融合，从而实现相对比较灵活、可靠而且定位精度高的AGV定位方式、定位精度可达±10mm。

本发明的目的是通过以下技术方案来实现的：一种融合2D环境地图和稀疏人工地标的AGV定位系统，该系统包括车载设备、根据当前环境建立的2D环境地图和布置于环境内的人工地标；

所述2D环境地图为占据栅格地图，占据栅格地图将环境划分为一系列的栅格，其中每一栅格给定一个概率值，表示环境中对应该栅格具有障碍物的概率，利用灰度值表征2D环境地图中栅格被占据的概率值(当灰度值为0时(纯黑色)，表示当前栅格有障碍物且被占据概率为1；灰度值为127时(灰色)，表示当前栅格是否被占据未知且被占据概率为0.5；灰度值为255时(白色)，表示当前栅格未被占据且被占据概率为0)；

每个人工地标在2D环境地图中具有唯一编号和对应编号的位置信息，AGV通过识别人工地标进行定位；

所述车载设备包括激光雷达、里程计、陀螺仪和定位单元；

所述定位单元包括激光数据处理模块、传感器数据存储模块、点云匹配建图模块、环境定位模块、人工地标定位模块和定位结果融合模块；

所述激光数据处理模块将激光雷达测得的当前环境信息转换成点云数据，并交由传感器数据存储模块进行存储；

所述传感器数据存储模块对点云数据以及里程计和陀螺仪采集到的数据进行存储，并交由点云匹配建图模块进行建图；所述点云匹配建图模块将当前采集到的点云数据使用体素滤波器进行稀疏化，将稀疏化后的点云数据与当前2D环境地图进行匹配；将匹配后得到的位置作为当前点云数据插入进2D环境地图的位置，从而完成建图；

所述环境定位模块利用陀螺仪以及里程计预测AGV的当前位姿，然后利用预测出的位姿作为当前点云数据与2D环境地图匹配的初始位姿进行最小二乘匹配，匹配的结果即为当前AGV在2D环境地图中的位姿，从而完成利用2D环境地图进行定位；

所述人工地标定位模块采集环境定位模块的定位结果作为初始位姿，并结合陀螺仪与里程计预测出AGV的当前位姿，利用此位姿以及在此位姿上扫描到的人工地标与已有的人工地标列表进行人工地标匹配，得到匹配的人工地标列表，之后将匹配的人工地标列表作为人工地标定位模块中的卡尔曼滤波器的测量值对预测的AGV位姿进行迭代，从而完成位姿更新，得到人工地标定位模块的定位信息；

所述定位结果融合模块采集人工地标定位模块与环境定位模块的定位信息，并将定位信息作为定位结果融合模块中的卡尔曼滤波器的测量值对AGV位姿进行更新，从而得到最终的定位结果，实现高精度定位。

进一步地，所述AGV为具有差速驱动轮的AGV时，所述激光雷达安装于车体的对角各一个；所述AGV为四轮驱动的AGV时，所述激光雷达安装于车体的对角各一个；所述AGV为舵轮驱动的AGV时，所述激光雷达安装于垂直于舵轮中心点的车体正上方。

进一步地，所述人工地标为钻石级反光材质的反光膜制成的反光柱，且在环境中布置反光柱时需要注意不能使得地标在环境中的位置形成等边三角形。

进一步地，所述激光数据处理模块的处理过程包括：

对激光雷达采集的激光点数据进行处理，提取出距离、光强、角度信息，记为H＝{h_k}_{k＝1，...,K},h_k∈R²，其中H代表采集到的所有激光点形成的点集，而h_k为点集中的第k个激光点在激光雷达坐标系下的坐标，K为激光点总个数，R²代表激光点的维数为2维，并且根据如下公式对激光数据进行坐标转换：

其中T_ξ为激光雷达坐标系转换至地图坐标系的坐标转换矩阵，ξ_θ为激光雷达坐标系转换至地图坐标系的旋转角度分量，ξ_x和ξ_y为激光雷达坐标系转换至地图坐标系的平移分量；转换过后即可得到点云匹配建图模块所需的点云数据格式。

进一步地，所述点云匹配建图模块中，点云数据的稀疏化过程具体为：通过将点云所在空间以给定初始分辨率划分为一系列栅格，每个栅格内仅保留一个点云数据，最后计算经过上述操作留下的点云集的大小，如果大小满足个数要求，则稀疏化完成，如果大小不满足个数要求则调整分辨率的大小来使得最终留下的点云集满足个数要求。

进一步地，所述点云匹配建图模块包括如下步骤：

S1：以第一帧点云数据对应的AGV位姿作为建图的初始位姿。

S2：根据里程计以及陀螺仪还有上一时刻AGV的位姿信息；推算出当前AGV位姿的预测值，推算公式如下：

O_p＝O_imu

P_ξ＝P_k+V_{liner_odom}*(t_n-t_k)

其中O_p为AGV当前位姿的朝向角，O_imu为陀螺仪返回的当前角度信息，V_{liner_odom}为根据里程计数据计算出来的当前AGV线速度，t_k和t_k-1为第k以及第k-1时刻的时间戳，Δodom为t_k与t_k-1时刻里程计的差值，P_ξ为AGV位姿的预测值，P_k为AGV第k时刻的位姿，t_n为当前时刻的时间戳，利用上述公式可以得出位姿的预测值。

S3：得到位姿的预测值之后以位姿的预测值作为计算点云匹配最小二乘法迭代的初始值，点云匹配的公式如下：

其中ξ为激光雷达的位姿，T_ξ为激光雷达坐标系转换至地图坐标系的坐标转换矩阵，h_k为第k个激光点在激光雷达坐标系下的坐标，M_smooth为双三次线性插值法的插值函数，K为激光点总个数，此时的匹配是将当前帧的点云数据与正在生成的2D环境地图上的栅格进行匹配；

S4：使用牛顿法求上述点云匹配公式的最小值，牛顿法公式如下:

其中ξ_n+1为第n+1次迭代所得位姿，ξ_n为第n次迭代所得位姿，为对f(ξ_n)求一阶导数，[H(f(ξ_n))]为对f(ξ_n)求二阶导数。使用此公式可以通过牛顿法对S3中点云匹配公式进行线性化求解，得到匹配完的精确位姿。

S5：匹配完成后以匹配完成的精确位姿将该帧点云数据插入进地图中，然后对栅格中已记录的概率p进行更新，概率更新公式如下：

M_new(x)＝clamp(odds^-1(odds(M_old(x))·odds(p_hit)))

其中odds(p)为概率更新函数，p为需要更新的概率，P_hit为当前栅格未在当前环境地图中被观测到时赋给栅格的概率初始值，clamp为钳位函数，保证括号内的值在0到1之间，而M_new(x)为更新完后该栅格被占据的概率，M_old(x)为更新前该栅格被占据的概率；

S6：在执行上述操作S1-S5的同时采集激光数据处理模块中激光点的光强信息并加入点云数据中，通过识别光强满足强度要求的激光点来识别环境中的人工地标，并对识别出的人工地标位置进行编号记录。

S7：操作AGV在需要AGV工作的空间内移动并循环执行上述步骤S2至S6直到建图完毕，建图完毕后可以得到2D环境地图以及地图中包含的人工地标列表。

进一步地，所述环境定位模块包括如下步骤：

S1：将AGV放置在工作环境中的初始点，并且手动给定对应初始点在2D环境地图的坐标。

S2：根据里程计以及陀螺仪还有上一时刻AGV的位姿信息；推算出当前AGV位姿的预测值，推算公式如下：

O_p＝O_imu

P_ξ＝P_k+V_{liner_odom}*(t_n-t_k)

其中O_p为AGV当前位姿的朝向角，O_imu为陀螺仪返回的当前角度信息，V_{liner_odom}为根据里程计数据计算出来的当前AGV线速度，t_k和t_k-1为第k以及第k-1时刻的时间戳，Δodom为t_k与t_k-1时刻里程计的差值，P_ξ为AGV位姿的预测值，P_k为AGV第k时刻的位姿，t_n为当前时刻的时间戳，利用上述公式可以得出位姿的预测值。

S3：得到位姿的预测值之后以位姿的预测值作为点云匹配建图模块计算点云匹配最小二乘法迭代的初始值，点云匹配的公式如下：

其中ξ为激光雷达的位姿，T_ξ为激光雷达坐标系转换至地图坐标系的坐标转换矩阵，h_k为第k个激光点在激光雷达坐标系下的坐标，M_smooth为双三次线性插值法的插值函数，K为激光点总个数，此时的匹配是将当前帧的点云数据与已有的2D环境地图上的栅格进行匹配。

S4：使用牛顿法求上述点云匹配公式的最小值，牛顿法公式如下:

其中ξ_n+1为第n+1次迭代所得位姿，ξ_n为第n次迭代所得位姿，为对f(ξ_n)求一阶导数，[H(f(ξ_n))]为对f(ξ_n)求二阶导数。使用此公式可以通过牛顿法对S3中点云匹配公式进行线性化求解，得到匹配完的精确位姿，并将此位姿输出。

进一步地，所述人工地标定位模块包括如下步骤：

S1：接收到环境定位模块给出的位姿作为人工地标定位的初始位姿。

S2：实时采集激光数据处理模块的数据，根据其中的光强信息识别出环境中的人工地标，并根据AGV当前位置将识别出的人工地标与建图时得到的人工地标列表进行匹配，因为此时AGV的位置已知，而识别出的人工地标相对于激光雷达的相对位姿可以通过激光雷达返回的距离和角度信息得出，故可以完成匹配。

S3：完成人工地标匹配后可根据匹配的人工地标列表计算AGV的精确位姿，计算的公式展开式如下：

其中，x'_m，y'_m为激光识别到的第m个人工地标在激光雷达坐标系下的坐标，x_m，y_m为对应第m个人工地标在2D环境地图中的坐标。X,Y为激光雷达坐标系转换到2D环境地图坐标系下的坐标转换平移分量，φ为坐标转换关系的旋转分量，通过坐标转换关系即可得到AGV在2D环境地图下的精确坐标，为方便表示，可有以下公式计算上述匹配结果：

H＝[cosφ sinφ X Y]^T＝A⁺b＝(A^T·A)^-1·A^Tb

其中T为矩阵转置求解符号，A⁺为求A矩阵的伪逆矩阵，A和b两个矩阵在展开式中已标注。H矩阵中的X,Y,φ即为当前AGV位姿的坐标和偏航角。

进一步地，所述定位结果融合模块的融合方法如下：

当AGV运行时，定位结果融合模块实时采集环境定位模块定位结果作为人工地标定位的初始位置；当未识别到环境中的人工地标时，仅使用环境定位模块定位结果进行输出，不启用人工地标定位。

当识别到环境中的人工地标时，启用人工地标定位模块进行定位，并将定位结果加入定位结果融合模块中的卡尔曼滤波器作为测量值进行滤波，公式如下：

z_k＝Cx_k-1+v_k

其中z_k为第k时刻AGV车载传感器(包括激光雷达、里程计和陀螺仪)的测量值矩阵，C是状态变量到测量值的转换矩阵，x_k-1为第k-1时刻AGV位姿，v_k为测量噪声。将人工地标定位结果加入测量值矩阵中，并且对应修改转移矩阵C。

将人工地标定位结果加入测量值矩阵后，卡尔曼滤波器其他步骤均与正常卡尔曼滤波器一致。

通过上述方法将人工地标定位结果作为测量值融合进卡尔曼滤波器中，保证两种定位模式平滑切换，且定位结果融合时定位结果不发生跳变，避免AGV运行时抖动。

一种融合2D环境地图和稀疏人工地标的AGV定位方法，该方法包括以下步骤：

(1)在AGV工作区域中需要高精度定位的场景以及过于开阔以至于激光雷达扫不到环境中障碍物的地方布置人工地标；

(2)人工地标布置完成后操作AGV在需要AGV工作的环境中移动来建立2D环境地图，直到建图完成；

(3)建图完成后操作AGV在环境中移动并识别人工地标，将识别出的人工地标位置记录并且编号，然后加入进2D环境地图中；

(4)人工地标标定完成后即可进行环境定位，将环境定位的结果作为初始值给入人工地标定位模块进行动态定位，得到两者定位结果后将定位结果交给定位结果融合模块，定位结果融合模块中的卡尔曼滤波器将两者结果滤波融合后输出最终定位结果，实现高精度定位。

本发明的有益效果是：本发明提供了一种融合2D环境地图和稀疏人工地标的AGV高精度定位系统及方法，该系统融合2D环境地图、稀疏人工地标进行定位，可实现AGV在工作环境中任意点的高精度定位，且具备较高的可靠性主要数据检测仅依赖于激光雷达，里程计以及陀螺仪，并且大幅减少了需要在环境中布置人工地标的数量，减少了后期维护成本，定位系统定位精度可达±10mm。

附图说明

图1为AGV小车示意图；

图2为2D环境地图示意图；

图3为定位单元结构框图。

具体实施方式

下面结合附图进一步详细描述本发明的技术方案，但本发明的保护范围不局限于以下所述。

本发明提供的一种融合2D环境地图和稀疏人工地标的AGV高精度定位系统，该系统包括车载设备、布置于工作场景中的人工地标；所述车载设备包括两个激光雷达、陀螺仪、里程计和定位单元，以差速轮驱动的AGV为例，如图1所示，激光雷达必须安装于车体的两个对角上保证激光雷达扫描点能够覆盖车体周围。而陀螺仪需要安装在AGV两个驱动轮连线的中点。

如图2所示为2D环境地图部分示意图，将地图分边长为一定长度的正方形栅格，当激光雷达扫描环境中的点时更新激光扫描范围内栅格的概率，使得地图与现实环境相匹配，当AGV在其工作范围内移动一圈后，即可得到匹配当前环境的地图，完成2D环境地图的建立后，AGV可以根据当前扫描到的环境点实时与2D环境地图进行匹配，然后经过从激光坐标系到AGV控制中心坐标系的坐标转换，得到AGV当前的位置。

如图3所示，所述定位单元包括激光数据处理模块、传感器数据存储模块、点云匹配建图模块、环境定位模块、人工地标定位模块和定位结果融合模块；

所述激光数据处理模块将激光雷达测得的当前布置人工地标的工作环境的信息转换成点云数据，并交由传感器数据存储模块进行存储；

所述传感器数据存储模块对点云数据以及里程计和陀螺仪采集到的数据进行存储，并交由点云匹配建图模块进行建图；所述点云匹配建图模块将当前采集到的点云数据使用体素滤波器进行稀疏化，将稀疏化后的点云数据与当前2D环境地图进行匹配；将匹配后得到的位置作为当前点云数据插入进2D环境地图的位置，从而完成建图；

所述环境定位模块利用陀螺仪以及里程计预测AGV的当前位姿，然后利用预测出的位姿作为当前点云数据与2D环境地图匹配的初始位姿进行最小二乘匹配，匹配的结果即为当前AGV在2D环境地图中的位姿，从而完成利用2D环境地图进行定位；

所述人工地标定位模块采集环境定位模块的定位结果作为初始位姿，并结合陀螺仪与里程计预测出AGV的当前位姿，利用此位姿以及在此位姿上扫描到的人工地标与已有的人工地标列表进行人工地标匹配，得到匹配的人工地标列表，之后将匹配的人工地标列表作为人工地标定位模块中的卡尔曼滤波器的测量值对预测的AGV位姿进行迭代，从而完成位姿更新，得到人工地标定位模块的定位信息；

所述定位结果融合模块采集人工地标定位模块与环境定位模块的定位信息，并将定位信息作为定位结果融合模块中的卡尔曼滤波器的测量值对AGV位姿进行更新，从而得到最终的定位结果，实现高精度定位。

所述激光数据处理模块的处理过程包括以下步骤：

对激光雷达采集的激光点数据进行处理，提取出距离、光强、角度信息，记为H＝{h_k}_{k＝1，...,K},h_k∈R²，其中H代表采集到的所有激光点形成的点集，而h_k为点集中的第k个激光点在激光雷达坐标系下的坐标，K为激光点总个数，R²代表激光点的维数为2维，并且根据如下公式对激光数据进行坐标转换：

其中T_ξ为激光雷达坐标系转换至地图坐标系的坐标转换矩阵，ξ_θ为激光雷达坐标系转换至地图坐标系的旋转角度分量，ξ_x和ξ_y为激光雷达坐标系转换至地图坐标系的平移分量；转换过后即可得到点云匹配建图模块所需的点云数据格式。

所述点云匹配建图模块中，点云数据的稀疏化过程具体为：通过将点云所在空间以给定初始分辨率划分为一系列栅格，每个栅格内仅保留一个点云数据，最后计算经过上述操作留下的点云集的大小，如果大小满足个数要求，则稀疏化完成，如果大小不满足个数要求则调整分辨率的大小来使得最终留下的点云集满足个数要求。

所述点云匹配建图模块包括如下步骤：

S1：以第一帧点云数据对应的AGV位姿作为建图的初始位姿。

S2：根据里程计以及陀螺仪还有上一时刻AGV的位姿信息；推算出当前AGV位姿的预测值，推算公式如下：

O_p＝O_imu

P_ξ＝P_k+V_{liner_odom}*(t_n-t_k)

其中O_p为AGV当前位姿的朝向角，O_imu为陀螺仪返回的当前角度信息，V_{liner_odom}为根据里程计数据计算出来的当前AGV线速度，t_k和t_k-1为第k以及第k-1时刻的时间戳，Δodom为t_k与t_k-1时刻里程计的差值，P_ξ为AGV位姿的预测值，P_k为AGV第k时刻的位姿，t_n为当前时刻的时间戳，利用上述公式可以得出位姿的预测值。

S3：得到位姿的预测值之后以位姿的预测值作为计算点云匹配最小二乘法迭代的初始值，点云匹配的公式如下：

其中ξ为激光雷达的位姿，T_ξ为激光雷达坐标系转换至地图坐标系的坐标转换矩阵，h_k为第k个激光点在激光雷达坐标系下的坐标，M_smooth为双三次线性插值法的插值函数，K为激光点总个数，此时的匹配是将当前帧的点云数据与正在生成的2D环境地图上的栅格进行匹配；

S4：使用牛顿法求上述点云匹配公式的最小值，牛顿法公式如下:

其中ξ_n+1为第n+1次迭代所得位姿，ξ_n为第n次迭代所得位姿，为对f(ξ_n)求一阶导数，[H(f(ξ_n))]为对f(ξ_n)求二阶导数。使用此公式可以通过牛顿法对S3中点云匹配公式进行线性化求解，得到匹配完的精确位姿。

S5：匹配完成后以匹配完成的精确位姿将该帧点云数据插入进地图中，然后对栅格中已记录的概率p进行更新，概率更新公式如下：

M_new(x)＝clamp(odds^-1(odds(M_old(x))·odds(p_hit)))

其中odds(p)为概率更新函数，p为需要更新的概率，P_hit为当前栅格未在当前环境地图中被观测到时赋给栅格的概率初始值，clamp为钳位函数，保证括号内的值在0到1之间，而M_new(x)为更新完后该栅格被占据的概率，M_old(x)为更新前该栅格被占据的概率；

S6：在执行上述操作S1-S5的同时采集激光数据处理模块中激光点的光强信息并加入点云数据中，通过识别光强满足强度要求的激光点来识别环境中的人工地标，并对识别出的人工地标位置进行编号记录。

S7：操作AGV在需要AGV工作的空间内移动并循环执行上述步骤S2至S6直到建图完毕，建图完毕后可以得到2D环境地图以及地图中包含的人工地标列表。

所述环境定位模块包括如下步骤：

S1：将AGV放置在工作环境中的初始点，并且手动给定对应初始点在2D环境地图的坐标。

S2：根据里程计以及陀螺仪还有上一时刻AGV的位姿信息；推算出当前AGV位姿的预测值，推算公式如下：

O_p＝O_imu

P_ξ＝P_k+V_{liner_odom}*(t_n-t_k)

其中O_p为AGV当前位姿的朝向角，O_imu为陀螺仪返回的当前角度信息，V_{liner_odom}为根据里程计数据计算出来的当前AGV线速度，t_k和t_k-1为第k以及第k-1时刻的时间戳，Δodom为t_k与t_k-1时刻里程计的差值，P_ξ为AGV位姿的预测值，P_k为AGV第k时刻的位姿，t_n为当前时刻的时间戳，利用上述公式可以得出位姿的预测值。

S3：得到位姿的预测值之后以位姿的预测值作为点云匹配建图模块计算点云匹配最小二乘法迭代的初始值，点云匹配的公式如下：

其中ξ为激光雷达的位姿，T_ξ为激光雷达坐标系转换至地图坐标系的坐标转换矩阵，h_k为第k个激光点在激光雷达坐标系下的坐标，M_smooth为双三次线性插值法的插值函数，K为激光点总个数，此时的匹配是将当前帧的点云数据与已有的2D环境地图上的栅格进行匹配。

S4：使用牛顿法求上述点云匹配公式的最小值，牛顿法公式如下:

其中ξ_n+1为第n+1次迭代所得位姿，ξ_n为第n次迭代所得位姿，为对f(ξ_n)求一阶导数，[H(f(ξ_n))]为对f(ξ_n)求二阶导数。使用此公式可以通过牛顿法对S3中点云匹配公式进行线性化求解，得到匹配完的精确位姿，并将此位姿输出。

所述人工地标定位模块包括如下步骤：

S1：接收到环境定位模块给出的位姿作为人工地标定位的初始位姿。

S2：实时采集激光数据处理模块的数据，根据其中的光强信息识别出环境中的人工地标，并根据AGV当前位置将识别出的人工地标与建图时得到的人工地标列表进行匹配，因为此时AGV的位置已知，而识别出的人工地标相对于激光雷达的相对位姿可以通过激光雷达返回的距离和角度信息得出，故可以完成匹配。

S3：完成人工地标匹配后可根据匹配的人工地标列表计算AGV的精确位姿，计算的公式展开式如下：

其中，x'_m，y'_m为激光识别到的第m个人工地标在激光雷达坐标系下的坐标，x_m，y_m为对应第m个人工地标在2D环境地图中的坐标。X,Y为激光雷达坐标系转换到2D环境地图坐标系下的坐标转换平移分量，φ为坐标转换关系的旋转分量，通过坐标转换关系即可得到AGV在2D环境地图下的精确坐标，为方便表示，可有以下公式计算上述匹配结果：

H＝[cosφ sinφ X Y]^T＝A⁺b＝(A^T·A)^-1·A^Tb

其中T为矩阵转置求解符号，A⁺为求A矩阵的伪逆矩阵，A和b两个矩阵在展开式中已标注。H矩阵中的X,Y,φ即为当前AGV位姿的坐标和偏航角。

所述定位结果融合模块的融合方法如下：

当AGV运行时，定位结果融合模块实时采集环境定位模块定位结果作为人工地标定位的初始位置；当未识别到环境中的人工地标时，仅使用环境定位模块定位结果进行输出，不启用人工地标定位。

当识别到环境中的人工地标时，启用人工地标定位模块进行定位，并将定位结果加入定位结果融合模块中的卡尔曼滤波器作为测量值进行滤波，公式如下：

z_k＝Cx_k-1+v_k

其中z_k为第k时刻AGV车载传感器(包括激光雷达、里程计和陀螺仪)的测量值矩阵，C是状态变量到测量值的转换矩阵，x_k-1为第k-1时刻AGV位姿，v_k为测量噪声。将人工地标定位结果加入测量值矩阵中，并且对应修改转移矩阵C。

将人工地标定位结果加入测量值矩阵后，卡尔曼滤波器其他步骤均与正常卡尔曼滤波器一致。

通过上述方法将人工地标定位结果作为测量值融合进卡尔曼滤波器中，保证两种定位模式平滑切换，且定位结果融合时定位结果不发生跳变，避免AGV运行时抖动。

本发明融合2D环境地图和人工地标，使用激光雷达扫描周围环境点建图并定位，相较于传统的人工地标定位对环境改造较小，大大降低了维护成本，并且在传统激光SLAM上大大提高了定位精度，可以实现工作环境内任意点的高精度定位。定位系统定位精度可达±10mm。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变型，这些改进和变型也应视为本发明的保护范围。

Claims (10)

1.一种融合2D环境地图和稀疏人工地标的AGV定位系统，其特征在于，该系统包括车载设备、根据当前环境建立的2D环境地图和布置于环境内的人工地标；

所述2D环境地图为占据栅格地图，占据栅格地图将环境划分为一系列的栅格，其中每一栅格给定一个概率值，表示环境中对应该栅格具有障碍物的概率，利用灰度值表征2D环境地图中栅格被占据的概率值；

每个人工地标在2D环境地图中具有唯一编号和对应编号的位置信息，AGV通过识别人工地标进行定位；

所述车载设备包括激光雷达、里程计、陀螺仪和定位单元；

所述定位单元包括激光数据处理模块、传感器数据存储模块、点云匹配建图模块、环境定位模块、人工地标定位模块和定位结果融合模块；

所述激光数据处理模块将激光雷达测得的当前环境信息转换成点云数据，并交由传感器数据存储模块进行存储；

所述传感器数据存储模块对点云数据以及里程计和陀螺仪采集到的数据进行存储，并交由点云匹配建图模块进行建图；所述点云匹配建图模块将当前采集到的点云数据使用体素滤波器进行稀疏化，将稀疏化后的点云数据与当前2D环境地图进行匹配；将匹配后得到的位置作为当前点云数据插入进2D环境地图的位置，从而完成建图；

所述环境定位模块利用陀螺仪以及里程计预测AGV的当前位姿，然后利用预测出的位姿作为当前点云数据与2D环境地图匹配的初始位姿进行最小二乘匹配，匹配的结果即为当前AGV在2D环境地图中的位姿，从而完成利用2D环境地图进行定位；

所述人工地标定位模块采集环境定位模块的定位结果作为初始位姿，并结合陀螺仪与里程计预测出AGV的当前位姿，利用此位姿以及在此位姿上扫描到的人工地标与已有的人工地标列表进行人工地标匹配，得到匹配的人工地标列表，之后将匹配的人工地标列表作为人工地标定位模块中的卡尔曼滤波器的测量值对预测的AGV位姿进行迭代，从而完成位姿更新，得到人工地标定位模块的定位信息；

所述定位结果融合模块采集人工地标定位模块与环境定位模块的定位信息，并将定位信息作为定位结果融合模块中的卡尔曼滤波器的测量值对AGV位姿进行更新，从而得到最终的定位结果，实现高精度定位。

2.根据权利要求1所述的一种融合2D环境地图和稀疏人工地标的AGV定位系统，其特征在于：所述AGV为具有差速驱动轮的AGV时，所述激光雷达安装于车体的对角各一个；所述AGV为四轮驱动的AGV时，所述激光雷达安装于车体的对角各一个；所述AGV为舵轮驱动的AGV时，所述激光雷达安装于垂直于舵轮中心点的车体正上方。

3.根据权利要求1所述的一种融合2D环境地图和稀疏人工地标的AGV定位系统，其特征在于：所述人工地标为钻石级反光材质的反光膜制成的反光柱，且在环境中布置反光柱时需要注意不能使得地标在环境中的位置形成等边三角形。

4.根据权利要求1所述的一种融合2D环境地图和稀疏人工地标的AGV定位系统，其特征在于：所述激光数据处理模块的处理过程包括：

对激光雷达采集的激光点数据进行处理，提取出距离、光强、角度信息，记为H＝{h_k}_{k＝1，...,K},h_k∈R²，其中H代表采集到的所有激光点形成的点集，而h_k为点集中的第k个激光点在激光雷达坐标系下的坐标，K为激光点总个数，R²代表激光点的维数为2维，并且根据如下公式对激光数据进行坐标转换：

其中T_ξ为激光雷达坐标系转换至地图坐标系的坐标转换矩阵，ξ_θ为激光雷达坐标系转换至地图坐标系的旋转角度分量，ξ_x和ξ_y为激光雷达坐标系转换至地图坐标系的平移分量；转换过后即可得到点云匹配建图模块所需的点云数据格式。

5.根据权利要求1所述的一种融合2D环境地图和稀疏人工地标的AGV定位系统，其特征在于：所述点云匹配建图模块中，点云数据的稀疏化过程具体为：通过将点云所在空间以给定初始分辨率划分为一系列栅格，每个栅格内仅保留一个点云数据，最后计算经过上述操作留下的点云集的大小，如果大小满足个数要求，则稀疏化完成，如果大小不满足个数要求则调整分辨率的大小来使得最终留下的点云集满足个数要求。

6.根据权利要求1所述的一种融合2D环境地图和稀疏人工地标的AGV定位系统，其特征在于：所述点云匹配建图模块包括如下步骤：

S1：以第一帧点云数据对应的AGV位姿作为建图的初始位姿。

S2：根据里程计以及陀螺仪还有上一时刻AGV的位姿信息；推算出当前AGV位姿的预测值，推算公式如下：

O_p＝O_imu

P_ξ＝P_k+V_{liner_odom}*(t_n-t_k)

其中O_p为AGV当前位姿的朝向角，O_imu为陀螺仪返回的当前角度信息，V_{liner_odom}为根据里程计数据计算出来的当前AGV线速度，t_k和t_k-1为第k以及第k-1时刻的时间戳，Δodom为t_k与t_k-1时刻里程计的差值，P_ξ为AGV位姿的预测值，P_k为AGV第k时刻的位姿，t_n为当前时刻的时间戳，利用上述公式可以得出位姿的预测值。

S3：得到位姿的预测值之后以位姿的预测值作为计算点云匹配最小二乘法迭代的初始值，点云匹配的公式如下：

其中ξ为激光雷达的位姿，T_ξ为激光雷达坐标系转换至地图坐标系的坐标转换矩阵，h_k为第k个激光点在激光雷达坐标系下的坐标，M_smooth为双三次线性插值法的插值函数，K为激光点总个数，此时的匹配是将当前帧的点云数据与正在生成的2D环境地图上的栅格进行匹配；

S4：使用牛顿法求上述点云匹配公式的最小值，牛顿法公式如下:

其中ξ_n+1为第n+1次迭代所得位姿，ξ_n为第n次迭代所得位姿，为对f(ξ_n)求一阶导数，[H(f(ξ_n))]为对f(ξ_n)求二阶导数。使用此公式可以通过牛顿法对S3中点云匹配公式进行线性化求解，得到匹配完的精确位姿。

S5：匹配完成后以匹配完成的精确位姿将该帧点云数据插入进地图中，然后对栅格中已记录的概率p进行更新，概率更新公式如下：

M_new(x)＝clamp(odds^-1(odds(M_old(x))·odds(p_hit)))

其中odds(p)为概率更新函数，p为需要更新的概率，P_hit为当前栅格未在当前环境地图中被观测到时赋给栅格的概率初始值，clamp为钳位函数，保证括号内的值在0到1之间，而M_new(x)为更新完后该栅格被占据的概率，M_old(x)为更新前该栅格被占据的概率；

S6：在执行上述操作S1-S5的同时采集激光数据处理模块中激光点的光强信息并加入点云数据中，通过识别光强满足强度要求的激光点来识别环境中的人工地标，并对识别出的人工地标位置进行编号记录。

S7：操作AGV在需要AGV工作的空间内移动并循环执行上述步骤S2至S6直到建图完毕，建图完毕后可以得到2D环境地图以及地图中包含的人工地标列表。

7.根据权利要求1所述的一种融合2D环境地图和稀疏人工地标的AGV定位系统，其特征在于：所述环境定位模块包括如下步骤：

S1：将AGV放置在工作环境中的初始点，并且手动给定对应初始点在2D环境地图的坐标。

S2：根据里程计以及陀螺仪还有上一时刻AGV的位姿信息；推算出当前AGV位姿的预测值，推算公式如下：

O_p＝O_imu

P_ξ＝P_k+V_{liner_odom}*(t_n-t_k)

其中O_p为AGV当前位姿的朝向角，O_imu为陀螺仪返回的当前角度信息，V_{liner_odom}为根据里程计数据计算出来的当前AGV线速度，t_k和t_k-1为第k以及第k-1时刻的时间戳，Δodom为t_k与t_k-1时刻里程计的差值，P_ξ为AGV位姿的预测值，P_k为AGV第k时刻的位姿，t_n为当前时刻的时间戳，利用上述公式可以得出位姿的预测值。

S3：得到位姿的预测值之后以位姿的预测值作为点云匹配建图模块计算点云匹配最小二乘法迭代的初始值，点云匹配的公式如下：

其中ξ为激光雷达的位姿，T_ξ为激光雷达坐标系转换至地图坐标系的坐标转换矩阵，h_k为第k个激光点在激光雷达坐标系下的坐标，M_smooth为双三次线性插值法的插值函数，K为激光点总个数，此时的匹配是将当前帧的点云数据与已有的2D环境地图上的栅格进行匹配。

S4：使用牛顿法求上述点云匹配公式的最小值，牛顿法公式如下:

其中ξ_n+1为第n+1次迭代所得位姿，ξ_n为第n次迭代所得位姿，为对f(ξ_n)求一阶导数，[H(f(ξ_n))]为对f(ξ_n)求二阶导数。使用此公式可以通过牛顿法对S3中点云匹配公式进行线性化求解，得到匹配完的精确位姿，并将此位姿输出。

8.根据权利要求1所述的一种融合2D环境地图和稀疏人工地标的AGV定位系统，其特征在于：所述人工地标定位模块包括如下步骤：

S1：接收到环境定位模块给出的位姿作为人工地标定位的初始位姿。

S2：实时采集激光数据处理模块的数据，根据其中的光强信息识别出环境中的人工地标，并根据AGV当前位置将识别出的人工地标与建图时得到的人工地标列表进行匹配，因为此时AGV的位置已知，而识别出的人工地标相对于激光雷达的相对位姿可以通过激光雷达返回的距离和角度信息得出，故可以完成匹配。

S3：完成人工地标匹配后可根据匹配的人工地标列表计算AGV的精确位姿，计算的公式展开式如下：

其中，x'_m，y'_m为激光识别到的第m个人工地标在激光雷达坐标系下的坐标，x_m，y_m为对应第m个人工地标在2D环境地图中的坐标。X,Y为激光雷达坐标系转换到2D环境地图坐标系下的坐标转换平移分量，φ为坐标转换关系的旋转分量，通过坐标转换关系即可得到AGV在2D环境地图下的精确坐标，为方便表示，可有以下公式计算上述匹配结果：

H＝[cosφ sinφ X Y]^T＝A⁺b＝(A^T·A)^-1·A^Tb

其中T为矩阵转置求解符号，A⁺为求A矩阵的伪逆矩阵，A和b两个矩阵在展开式中已标注。H矩阵中的X,Y,φ即为当前AGV位姿的坐标和偏航角。

9.根据权利要求1所述的一种融合2D环境地图和稀疏人工地标的AGV定位系统，其特征在于：所述定位结果融合模块的融合方法如下：

当AGV运行时，定位结果融合模块实时采集环境定位模块定位结果作为人工地标定位的初始位置；当未识别到环境中的人工地标时，仅使用环境定位模块定位结果进行输出，不启用人工地标定位。

当识别到环境中的人工地标时，启用人工地标定位模块进行定位，并将定位结果加入定位结果融合模块中的卡尔曼滤波器作为测量值进行滤波，公式如下：

z_k＝Cx_k-1+v_k

其中z_k为第k时刻AGV车载传感器的测量值矩阵，C是状态变量到测量值的转换矩阵，x_k-1为第k-1时刻AGV位姿，v_k为测量噪声。将人工地标定位结果加入测量值矩阵中，并且对应修改转移矩阵C。

10.一种融合2D环境地图和稀疏人工地标的AGV定位方法，其特征在于：包括以下步骤：

(1)在AGV工作区域中需要高精度定位的场景以及过于开阔以至于激光雷达扫不到环境中障碍物的地方布置人工地标；

(2)人工地标布置完成后操作AGV在需要AGV工作的环境中移动来建立2D环境地图，直到建图完成；

(3)建图完成后操作AGV在环境中移动并识别人工地标，将识别出的人工地标位置记录并且编号，然后加入进2D环境地图中；

(4)人工地标标定完成后即可进行环境定位，将环境定位的结果作为初始值给入人工地标定位模块进行动态定位，得到两者定位结果后将定位结果交给定位结果融合模块，定位结果融合模块中的卡尔曼滤波器将两者结果滤波融合后输出最终定位结果，实现高精度定位。