一种激光导航AGV高精度定位及目标对准控制方法

摘要

一种激光导航AGV高精度定位及目标对准控制方法，涉及移动机器人导航技术领域。基于激光点云对目标环境特征进行点云分割与提取，实现AGV实时获取目标特征点云的能力；基于最近邻方法，解决点云匹配中寻找对应点、建立目标函数的问题；设计一种基于梯度点云密度、多分辨率搜索匹配方法，并通过分支定界优化搜索。实现AGV对目标特征点云进行高精度低延时的匹配，解决了激光导航AGV在特定任务点高精度定位的问题；基于AGV平滑运动控制器，实现AGV平滑、精准的运动控制到达目标点的能力。解决激光导航AGV高精度定位及位姿对准、充电对接等目标对准控制问题，弥补SLAM技术定位精度不足、传统对准控制方法成本过高等缺陷。

权利要求

1.一种激光导航AGV高精度定位及目标对准控制方法，其特征在于包括以下步骤：

1)一种“目标”点云分割提取的方法；

2)一种存储“目标”点云作为模板点云的方法，根据对准任务点附近不会发生变化的任意环境特征作为“目标”，并提取“目标”点云存储至本地，作为模板点云，用于AGV点云匹配与定位；

3)一种基于最近邻方法的点云匹配的方法，具体步骤为：(3.1)给定两组点云X、P，其中X＝{x1，x2，…，xm}为“目标”模板点云，P＝{p1，p2，…pn}为“目标”当前帧点云；

(3.2)计算两组点云的质心 并将两组点云坐标进行质心去除,去掉质心后的坐标为X′＝{xi‑μi}＝{x′i}，P′＝{pi‑μp}＝{p′i}；

(3.3)寻找当前帧点云P的匹配点，将两组点云质心对齐后，在模板点云X中选择与其欧氏距离最近的点作为匹配点，并计算其距离；

(3.4)采用kd‑tree进行加速搜索最近邻点；

4)一种基于快速优化搜索的匹配的方法，基于最近邻方法寻找对应点，通过一种梯度点云密度‑多分辨率优化搜索方法寻找出最优匹配，所述一种基于快速优化搜索的匹配的方法，具体步骤为：(4.1)一种建立优化搜索分支树的方法：分别基于不同分辨率建立搜索分支树和基于不同点云密度建立搜索分支树；

所述基于不同分辨率建立搜索分支树的方法为：多分辨率密度搜索树的每一层搜索范围与精度不同，最开始为第0层，其搜索范围为：定义一个固定常数num作为 搜索范围里其中一个维度的采样点数，则分支树Tn第n层的搜索范围为：

分支树Tn第n层的搜索步长为：

所述基于不同点云密度建立搜索分支树的方法为：每一层匹配的点云数量不同，其中最开始为第0层，给定一个起始匹配点云数量常数为m

seed0；分支树Tm第m层节点的待匹配点云的数量为：seedm＝seed0·2 ，当大seedm于当前点云数量N时，则分支树Tm的层数m不再增加；

(4.2)通过最近邻方法建立目标函数，并用于分支树寻找最优解；

(4.3)一种通过启发式的搜索方法寻找最优值方法，具体步骤为：(4.3.1)采用基于优先队列的优先搜索原则；

(4.3.2)优先队列的评价函数为启发函数，该方法的启发函数为对目标函数的预测值，其公式如下：

fe＝(em‑em‑1)·(M‑m)(4.3.3)目标函数计算方法为其中p′i为待匹配点，q′i为其最近邻点，N为当前点云待匹配点数量；

(4.3.4)计算当前节点的目标函数值em，em‑1为该节点上一层节点的目标函数值；

(4.3.5)对该节点通过步骤(4.3.2)的方法计算启发函数，并将启发函数置入优先队列进行排序；

(4.3.6)计算完当前节点后，分支树自动跳转至优先队列中最优节点继续进行计算；

(4.4)通过比较目标函数对分支树进行剪枝，具体方法为：(4.4.1)当获得第一个局部最优值时，将该值视为最优值eb；

(4.4.2)分支树返回没有搜索完全的节点继续搜索，在搜索过程中，若该节点目标函数值em＞eb，对该枝节点进行剪枝，即不再对其进行分支搜索；

(4.4.3)若该节点为叶节点且该节点目标函数值eM＜eb，则将该值视为最优值eb，继续返回搜索，在该最优值的w范围内进行更高精度搜索，直至所有的节点搜索完全或被剪掉；

5)一种目标对准的平滑运动控制方法。

2.如权利要求1所述一种激光导航AGV高精度定位及目标对准控制方法，其特征在于在步骤1)中，所述一种“目标”点云分割提取的方法，具体步骤为：(1.1)对激光点云数据进行体素滤波，用体素栅格内所有点的重心来表示该栅格的环境数据：对于一个含有N个点的二维体素，其重心坐标为：(1.2)基于设置的角度对“目标”点云进行双向搜索，将范围内搜索到的第一个符合距离要求的点云作为起始点；

(1.3)基于起始点对点云进行双向搜索，将所有邻近点云提取，得到当前帧的“目标”点云，其中邻近点云的判断规则为：当前点与上一点之间的距离dc小于阈值dm。

3.如权利要求1所述一种激光导航AGV高精度定位及目标对准控制方法，其特征在于在步骤2)中，所述一种存储“目标”点云作为模板点云的方法，具体步骤为：(2.1)设置不会发送变化的任意环境特征作为“目标”，并确定其所在方向；

(2.2)基于“目标”所在方向使用步骤1)所述进行“目标”点云提取；

(2.3)将“目标”点云的距离与角度信息通过文件的形式存储在本地；

(2.4)读取该文件，用于实时点云匹配，获得AGV定位信息。

4.如权利要求1所述一种激光导航AGV高精度定位及目标对准控制方法，其特征在于在步骤5)中，所述一种目标对准的平滑运动控制方法，具体步骤为：(5.1)通过激光SLAM技术导航到“目标”前的粗略点；

(5.2)获取“目标”与AGV之间的相对位姿，并建立如下运动学模型：(5.3)设计如下控制器使机器人能够平滑稳定地到达“目标点”位姿：(5.4)设计如下控制器使机器人能够快速跟踪虚拟控制变量δ；

(5.5)通过控制系统解算出来的角速度w与线速度v，使AGV平滑稳定的运动到对准任务的“目标点”。

5.如权利要求4所述一种激光导航AGV高精度定位及目标对准控制方法，其特征在于在步骤5)第(5.2)部分中，所述获取“目标”与AGV之间的相对位姿，并建立运动学模型的具体方法为：“目标”与AGV的相对位姿 其中R和t表示为：将相对位姿转换为步骤5)中“AGV平滑运动控制”的期望目标控制量，转换公式如下：

说明书

一种激光导航AGV高精度定位及目标对准控制方法

技术领域

[0001] 本发明涉及移动机器人导航技术领域，尤其是涉及到一种激光导航AGV高精度定位及目标对准控制方法。

背景技术

[0002] SLAM(Simultaneous Localization and Mapping)技术，即时定位与地图构建，在移动机器人的定位、跟踪和路径规划术中具有重要作用。目前，SLAM技术被大量应用于扫地
机器人、AGV(Automated Guided Vehicle)、无人驾驶汽车等移动机器人自主导航领域。特
别是无轨导航AGV由于不需要预先铺设磁条等路径轨道，具有更柔性、更智能、更灵活、自由
调度等优势，是AGV的重要发展方向。而激光导航AGV基于激光雷达传感器的SLAM方法进行
自主导航，具有布设简单、稳定性高、受光照环境影响小等特点，目前已经成为无轨导航AGV
的主流方式。但由于目前SLAM技术中存在的定位误差，难以满足AGV任务中高稳定性、高精
度的位姿对准、充电对接等目标对准控制要求。因此，一种能够适用于SLAM激光导航AGV的
高精度定位及目标对准控制成了目前AGV领域亟待解决的问题。

[0003] 针对该问题，特别是“自动充电”等对准控制问题，主要有以下代表性的解决方案。(1)自动充电桩充电：该类方法将自主充电系统集成在充电桩上，实现AGV自动充电，是目前
普遍采用的方式。如中国专利201711383235.3公开一种AGV自动充电装置、系统及方法同，
但该类方法的缺点在于需要搭建复杂的具备自主充电功能的充电桩，且一般价格较为昂
贵。而且该方法仅适用于AGV自主充电，无法完成AGV高精度自动对准控制的问题，不具备适
用性。(2)视觉识别自动充电：该方法通过AGV上的视觉传感器对任务点、充电桩进行识别与
对准。如中国专利201810398588.9公开一种高精度载车板对接方法。该类方法依赖于特定
的人工标识，且受光照变化影响较大，普遍精度不高。且目前大多无轨导航AGV采用激光传
感器进行导航，增加视觉传感器提高了AGV的成本。(3)有轨导航自动充电：该类方法通过预
先铺设轨道进行导航，如现有技术中国专利201810939998.X公开一种磁导航AGV驱动单元
及AGV车。该类导航方式不够灵活，对于没有铺设轨道的区域不能进行导航，且路径难以更
改和扩展，具有很大的局限性。

发明内容

[0004] 本发明针对现有技术下的AGV自动充电、自动对准系统成本过高、精度差、稳定性差等缺陷，提供成本较低、稳定型、灵活适用性高的一种激光导航AGV高精度定位及目标对
准控制方法。

[0005] 本发明包括以下步骤：

[0006] 1)一种“目标”点云分割提取的方法；

[0007] 2)一种存储“目标”点云作为模板点云的方法，根据对准任务点附近不会发生变化的任意环境特征作为“目标”，并提取“目标”点云存储至本地，作为模板点云，用于AGV点云
匹配与定位；

[0008] 3)一种基于最近邻方法的点云匹配的方法；

[0009] 4)一种基于快速优化搜索的匹配的方法，基于最近邻方法寻找对应点，通过一种梯度点云密度‑多分辨率优化搜索方法寻找出最优匹配；

[0010] 5)一种目标对准的平滑运动控制方法。

[0011] 在步骤1)中，所述一种“目标”点云分割提取的方法，具体步骤可为：

[0012] (1.1)对激光点云数据进行体素滤波，用体素栅格内所有点的重心来表示该栅格的环境数据：对于一个含有N个点的二维体素，其重心坐标为：

[0013] (1.2)基于设置的角度对“目标”点云进行双向搜索，将一定范围内搜索到的第一个符合距离要求的点云作为起始点；

[0014] (1.3)基于起始点对点云进行双向搜索，将所有邻近点云提取，得到当前帧的“目标”点云，其中邻近点云的判断规则为：当前点与上一点之间的距离dc小于阈值dm。

[0015] 在步骤2)中，所述一种存储“目标”点云作为模板点云的方法，具体步骤可为：

[0016] (2.1)设置不会发送变化的任意环境特征作为“目标”，并确定其所在方向；

[0017] (2.2)基于“目标”所在方向使用步骤1)所述进行“目标”点云提取；

[0018] (2.3)将“目标”点云的距离与角度信息通过文件的形式存储在本地；

[0019] (2.4)读取该文件，用于实时点云匹配，获得AGV定位信息。

[0020] 在步骤3)中，所述一种基于最近邻方法的点云匹配的方法，具体步骤可为：

[0021] (3.1)给定两组点云X、P，其中X＝{x1，x2，…，xm}为“目标”模板点云，P＝{p1，p2，…，pn}为“目标”当前帧点云；

[0022] (3.2)计算两组点云的质心 并将两组点云坐标进行质心去除,去掉质心后的坐标为X′＝{xi‑μi}＝{x′i}，P′＝{pi‑μp}＝{p′i}；

[0023] (3.3)寻找当前帧点云P的匹配点，将两组点云质心对齐后，在模板点云X中选择与其欧氏距离最近的点作为匹配点，并计算其距离；

[0024] (3.4)采用kd‑tree进行加速搜索最近邻点。

[0025] 在步骤4)中，所述一种基于快速优化搜索的匹配的方法，具体步骤可为：

[0026] (4.1)一种建立优化搜索分支树的方法：分别基于不同分辨率建立搜索分支树和基于不同点云密度建立搜索分支树；

[0027] (4.2)通过最近邻方法建立目标函数，并用于分支树寻找最优解；

[0028] (4.3)一种通过启发式的搜索方法寻找最优值方法；

[0029] (4.4)通过比较目标函数对分支树进行剪枝。

[0030] 在步骤4)第(4.1)部分中，所述基于不同分辨率建立搜索分支树的方法可为：

[0031] 多分辨率密度搜索树的每一层搜索范围与精度不同，最开始为第0层，其搜索范围为：

[0032]

[0033] 定义一个固定常数num作为 搜索范围里其中一个维度的采样点数，则分支树Tn第n层的搜索范围为：

[0034]

[0035] 分支树Tn第n层的搜索步长为：

[0036] 所述基于不同点云密度建立搜索分支树的方法可为：

[0037] 每一层匹配的点云数量不同，其中最开始为第0层，给定一个起始匹配点云数量常数为seed0；分支树Tm第m层节点的待匹配点云的数量为：seedm＝seed0·2m，当大seedm于当
前点云数量N时，则分支树Tm的层数m不再增加。

[0038] 在步骤4)第(4.3)部分中，所述一种通过启发式的搜索方法寻找最优值方法，具体步骤可为：

[0039] (4.3.1)采用基于优先队列的优先搜索原则；

[0040] (4.3.2)优先队列的评价函数为启发函数，该方法的启发函数为对目标函数的预测值，其公式如下：

[0041] fe＝(em‑em‑1)·(M‑m)

[0042] (4.3.3)目标函数计算方法为

[0043]

[0044] 其中p′i为待匹配点，q′i为其最近邻点，N为当前点云待匹配点数量；

[0045] (4.3.4)计算当前节点的目标函数值em，em‑1为该节点上一层节点的目标函数值；

[0046] (4.3.5)对该节点通过步骤(4.3.2)的方法计算启发函数，并将启发函数置入优先队列进行排序；

[0047] (4.3.6)计算完当前节点后，分支树自动跳转至优先队列中最优节点继续进行计算。

[0048] 在步骤4)第(4.4)部分中，所述通过比较目标函数对分支树进行剪枝，具体方法可为：

[0049] (4.4.1)当获得第一个局部最优值时，将该值视为最优值eb；

[0050] (4.4.2)分支树返回没有搜索完全的节点继续搜索，在搜索过程中，若该节点目标函数值em＞eb，对该枝节点进行剪枝，即不再对其进行分支搜索；

[0051] (4.4.3)若该节点为叶节点且该节点目标函数值eM＜eb，则将该值视为最优值eb，继续返回搜索，在该最优值的w范围内进行更高精度搜索，直至所有的节点搜索完全或被剪
掉。

[0052] 在步骤5)中，所述一种目标对准的平滑运动控制方法，具体步骤可为：

[0053] (5.1)通过激光SLAM技术导航到“目标”前的粗略点；

[0054] (5.2)获取“目标”与AGV之间的相对位姿，并建立如下运动学模型：

[0055]

[0056] (5.3)设计如下控制器使机器人能够平滑稳定地到达“目标点”位姿：

[0057]

[0058] (5.4)设计如下控制器使机器人能够快速跟踪虚拟控制变量δ；

[0059]

[0060] (5.5)通过控制系统解算出来的角速度w与线速度v，使AGV平滑稳定的运动到对准任务的“目标点”。

[0061] 在步骤5)第(5.2)部分中，所述获取“目标”与AGV之间的相对位姿，并建立运动学模型的具体方法为：“目标”与AGV的相对位姿 其中R和t可表示为：

[0062]

[0063] 将相对位姿转换为权力要求14中“AGV平滑运动控制”的期望目标控制量，转换公式如下：

[0064]

[0065] 本发明实现了AGV对目标特征点云进行高精度低延时的匹配，解决了激光导航AGV在特定任务点高精度定位的问题；基于AGV平滑运动控制器，实现了AGV平滑、精准的运动控
制到达目标点的能力。本发明解决了激光导航AGV高精度定位及位姿对准、充电对接等目标
对准控制问题，弥补了SLAM技术定位精度不足、传统对准控制方法成本过高等缺陷。

[0066] 本发明针对激光SLAM导航的AGV，利用激光雷达传感器进行末端高精度定位导航，完成自动充电对接或其它目标对准任务，其成本较低，且稳定型、灵活适用性高，具有较强
的市场竞争力。

附图说明

[0067] 图1为本发明采用的自动充电系统充电对准示意图。在图1中，①表示充电电刷，②表示识别目标“V型槽”，③对准控制的粗略点。

[0068] 图2为本发明采用目标点云分割与提取的特征点云结果。

[0069] 图3为本发明所述高精度定位流程图。

[0070] 图4为本发明所述梯度点云密度‑多分辨率优化搜索分支树示意图。

[0071] 图5为本发明采用的定位方法目标位姿定位结果。

[0072] 图6为本发明采用的定位方法目标位姿定位静态误差结果

[0073] 图7为本发明所述AGV平滑运动控制建立的运动学模型图

[0074] 图8为本发明所述AGV平滑运动控制方框图。

[0075] 图9为本发明所述AGV平滑运动控制轨迹图。

[0076] 图10为本发明采用自动充电与对准系统导航结果。

具体实施方式

[0077] 以下实施例将结合附图对本发明作进一步的说明。

[0078] 本发明针对的是AGV相对于“目标”的高精度定位与对准控制问题，根据具体应用的不同(如货架对准、充电对接等)，“目标”可以为不同的固定物体或局部环境。以下实施例
将结合附图对本发明作进一步说明。

[0079] 本发明“一种激光导航AGV高精度定位及目标对准控制方法”的内容包括以下四个部分。

[0080] (1)目标的点云分割与提取

[0081] 对采用激光雷达传感器获得的“目标”点云数据进行分割与提取，其主要步骤如下：

[0082] ①对激光点云数据进行体素滤波，减少点云的数量规模,同时保持“目标”点云的主要特征。基本方法是将点云数据划分在不同的体素栅格内，用体素栅格内所有点的重心
来表示该栅格的环境数据。对于一个含有N个点的二维体素,经过Voxel Grid滤波器处理后
得到表示环境数据的点的计算公式为：

[0083]

[0084] 其中x、y为该二维体素中N个点的重心坐标。

[0085] ②基于设置的角度对“目标”点云进行双向搜索，将一定范围内搜索到的第一个符合距离要求的点云作为起始点。其中设置的角度为AGV到达“目标”前的第一个粗略目标点
后，AGV机器人所在的方向角。

[0086] ③基于起始点对点云进行双向搜索，将所有邻近点云提取，得到当前帧的“目标”点云。其中邻近点的判断规则为当前点与上一点之间的距离dc小于阈值dm。

[0087] (2)基于最近邻方法的点云匹配

[0088] 将当前帧的“目标”点云与“目标”的先验模板点云进行高精度匹配，其中寻找对应点是点云匹配中的关键环节，本发明采用最近邻点作为对应点，采用kd‑tree进行快速寻找
点云的最近邻点。其主要内容如下：

[0089] ①给定两组点云X、P，其中X为“目标”模板点云，P为“目标”当前帧点云：

[0090] X＝{x1，x2，…，xm}

[0091] P＝{p1，p2，…，pn}

[0092] 计算两组点云的质心，并将两组点云坐标进行质心去除。

[0093] 两组点云的质心分布为：

[0094] 两组点云去掉质心后的坐标为：

[0095] X′＝{xi‑μx)＝{x′i}

[0096] P′＝{pi‑μp}＝{p′i}

[0097] ②寻找当前帧点云的匹配点，将两组点云质心对齐后，在模板点云X′中选择与其欧氏距离最近的点作为匹配点，并计算其距离。依次计算所有点的距离的均方根数值，该值
作为下面优化搜索的目标函数值。目标函数如下所示：

[0098]

[0099] 其中p′i为待匹配点，q′i为其最近邻点，N为当前点云待匹配点数量。

[0100] ③基于kd‑tree进行加速搜索最近邻点，给定待匹配点p′i与模板点云X′，将模板点云平面切割成两部分，选取包含有当前待匹配点p′i的平面部分，重复该操作直至包含p′i
的平面部分只剩下一个点，该点即为待匹配点p′i的最近邻点。

[0101] (3)梯度点云密度‑多分辨率优化搜索方法

[0102] 为了寻找到最优的匹配位姿，将两组“目标”点云质心重合之后的位姿作为初值，在该初始位姿附近进行搜索，搜索到使得目标函数e最优的位姿，基于该位姿的匹配即为最
优匹配，从而得到AGV相对于“目标”的高精度位姿定位。

[0103] 因此需要有足够的搜索范围，使得最优匹配位姿能够被搜索到，又需要高精度分辨率的搜索，使搜索到的位姿精度足够高。因此本发明设计并实现了一种梯度点云密度‑多
分辨率优化搜索方法，并通过分支定界进行加速。其主要内容如下：

[0104] ①优化搜索的目标函数，即为部分(2)中的最近邻点均方根数值e。当两组点云完全匹配时，该目标函数理论值为0。因此该数值存在最优值，此时匹配效果最好。

[0105] ②建立搜索分支树，分支树主要由两部分构成，第一部分为多分辨率分支树Tn。分支树每一层搜索分辨率不同，其中最开始为第0层，其搜索范围为：

[0106]

[0107] 定义一个固定常数num作为 搜索范围里其中一个维度的采样点数。则分支树Tn第n层的搜索范围为：

[0108]

[0109] 分支树Tn第n层的搜索步长为：

[0110]

[0111] 即说明在第n层的时候，一共需要搜索情况为num3种，每种情况间隔为stepn。

[0112] 第二部分为点云密度分支树Tm，每一层匹配的点云数量不同，其中最开始为第0层，给定一个起始匹配点云数量常数为seed0。分支树Tm第m层节点的待匹配点云的数量为：

[0113] seedm＝seed0·2m

[0114] 当seedm大于当前点云数量N时，则分支树Tm的层数m不再增加。

[0115] ③优先队列与启发函数，在搜索时，采用基于优先队列的优先搜索原则。优先队列的评价函数为启发函数，该方法种的启发函数为对目标函数的预测值，其公式如下：

[0116] fe＝(em‑em‑1)·(M‑m)

[0117] 其中，m为密度分支树Tm当前节点的层数，M为密度分支树Tm的最大层数，em为当前节点目标函数值，em‑1为该节点上一层节点的目标函数值。将每一层的节点计算启发函数，
并将启发函数置入优先队列进行排序。计算完当前节点后，分支树自动跳转至优先队列中
最优节点继续进行计算。该优先队列的目的为：当分支树第一次搜索到叶节点(该搜索情况
所有点云进行匹配)时，叶节点的目标函数值为一个局部最优值，这能大大增加剪枝规模，
提供搜索效率。

[0118] ④剪枝，当获得第一个局部最优值时，将该值视为最优值eb，此时搜索并没有结束。分支树将返回没有搜索完全的节点继续搜索，在搜索过程中，若该节点目标函数值em＞
eb，则视为该节点及其后面的节点的目标函数值em+i恒大于eb，事实也的确如此，对该枝节点
进行剪枝，即不再对其进行分支搜索；若该节点为叶节点且该节点目标函数值eM＜eb，则将
该值视为最优值eb，继续返回搜索。直至所有的节点搜索完全或被剪掉。

[0119] ⑤梯度点云密度‑多分辨率优化搜索步骤如下：

[0120] a)建立多分辨率分支树Tn，对于Tn中的每一个节点，建立点云密度分支树Tm。

[0121] b)获取优先队列中的最优节点，计算其目标函数值，若该节点目标函数值em＜eb，则减枝；若该节点为叶节点且该节点目标函数值eM＜eb，则更新最优值eb；否则继续分支，分
支后的节点计算启发函数，并置入优先队列。

[0122] c)重复步骤b直至Tn第n层中点云密度分支树Tm所有的节点搜索完全或被剪掉，则最优值eb为Tn第n层的最优值en_b，若该最优值符合要求，或目标函数值不再减小，则结束搜
索；否则将该最优情况返回步骤a，在该最优值的w范围内进行更高分辨率搜索。

[0123] (4)AGV平滑运动控制

[0124] 获得相对于“目标”的高精度位姿后，需要让AGV精准、平滑、稳定的运动到目标点，由于自动充电等对准任务的一些特性要求，还需要让AGV按照固定的姿态角到达目标点。因
此，为了实现对期望位置和姿态的精确对准，本发明通过运动控制器的设计来实现高精度
的平滑运动控制，其主要内容如下：

[0125] ①运动学模型建立

[0126] 在AGV的“目标”对准任务中，其运动的期望位置和姿态(即目标点)是已知的。对于两主动轮差速驱动的AGV机器人运动系统，可以对其建立运动学模型如下：

[0127]

[0128] 其中，r为期望位置与AGV位置的直线距离，θ为期望姿态与期望位置到AGV位置间的连线之间的角度差，δ为AGV姿态与期望位置到AGV位置间的连线之间的角度差。

[0129] 以“目标点”为原点建立目标坐标系，则可令期望控制目标量为(r，θ，δ)T＝(0，0，T
0) ，控制变量为(v，w)，其中，v为机器人当前线速度，w为机器人当前角速度。运动控制主要
流程为：通过虚拟控制变量δ(航向控制)，驱动控制系统(i)(位置控制)，使AGV控制到达坐
标系原点(期望位姿)，即满足期望控制目标量要求；w作为真正的控制通过控制系统(ii)使
δ快速稳定的到达所需要的虚拟值，从而使控制系统(i)成为一个平滑且稳定的系统。

[0130]

[0131]

[0132] ②控制器设计

[0133] a)对于控制系统(i)进行控制：

[0134] 对于控制系统，建立一个Lyapunov函数：

[0135]

[0136] 对于虚拟的控制变量δ设计如下控制器

[0137] δ＝arctan(‑k1θ)       (iii)

[0138] 其中，k1是一个大于0的常数。则控制系统可以通过控制器(iii)写成如下所示:

[0139]

[0140] 可以证得该式为负定，则该式在目标点处为渐进稳定，控制系统(i)可以由控制器(iii)控制。

[0141] b)对于控制系统(ii)进行控制：

[0142] 引入一个虚拟变量z，描述δ与其控制器(iii)之间的误差，其表达式如下：

[0143] z≡δ‑arctan(‑k1θ)

[0144] 可以直接描述出来：

[0145]

[0146] 因此将w设计为：

[0147]

[0148] 则 可以被描述为一个线性函数：

[0149]

[0150] 因此，整个控制系统的真实控制变量w可表述为：

[0151]

[0152] 控制量 为线性函数，因此可通过控制器(iv)对控制系统(ii)进行快速控制。

[0153] 以下给出具体实施例。

[0154] 在自动充电对接的应用中，激光导航AGV高精度定位及对准控制方法的具体步骤如下：

[0155] Step 1：设置“目标”。本实施例中自动充电系统采用“V型槽”作为“目标”(如图1中的②所示)，即为充电对接的环境特征。本发明采用但不仅局限于V型槽作为环境特征，可采
用不发生形态变化的任意物体作为“目标”，具有很强的适用性。

[0156] Step 2：确定目标点(目标对准控制的期望位姿)。如图1所示，本实施例中，可采用手动推动使AGV的车载触点与充电电刷(图1中的①所示)正好对接上，此时AGV相对于“V型
槽”的位姿即为充电对接的目标点(“V型槽”在AGV正前方，AGV搭载的2D激光雷达的扫描范
围为270°)。

[0157] Step3：提取“目标”的模板点云。采用本发明中“目标的点云分割与提取”方法，从激光雷达的点云中提取出“V型槽”的点云并保持至本地，作为AGV充电对准中点云匹配的
“目标”模板点云X。点云分割与提取的特征点云结果如图2所示，其中黄色V型部分是基于该
方法提取的“目标”点云。

[0158] Step 3：导航到“目标”前的粗略点(图1中的③所示)。AGV采用激光SLAM定位与导航方法，导航至目标点正后方的粗略点(距离目标点1m位置左右，方向与目标点的期望姿态
相同)，使“V型槽”在AGV激光传感器的探测范围内。

[0159] Step 4：高精度相对定位与目标对准控制。采用本发明中的高精度定位方法对AGV进行目标对准控制，该定位方法的流程图如图3所示。

[0160] 4.1)采用本发明中“目标的点云分割与提取”方法，提取“V型槽”的当前点云P，载入本地的模板点云X，计算两组点云的质心μx、μp，将两组点云坐标进行质心去除，使两组点
云初步对齐，得到去除质心后的两组点云X′，P′。

[0161] 4.2)采用本发明中“基于最近邻方法的点云匹配”寻找对应匹配点，计算最近邻距离的均方根数值，即为当前目标函数em，建立搜索问题模型。

[0162] 4.3)采用本发明中“梯度点云密度‑多分辨率优化搜索方法”的内容②建立分辨率搜索分支树Tn，如图4所示，在本实施例中，定义搜索分支树Tn初始的搜索范围为：角度搜索
范围Wθ＝{‑0.785rad，…0.785rad}，二维平面X轴搜索范围Wx＝{‑20mm，…，20mm}，二维平面
Y轴搜索范围Wy＝{‑20mm，…，20mm}，定义搜索分支树Tn第0层采样点数为：角度采样数numθ
为11，二维平面X轴、Y轴采样数numx、numy为5。

[0163] 4.4)采用本发明中“梯度点云密度‑多分辨率优化搜索方法”的内容②建立点云密度分支树Tm，在本实施例中，定义点云密度分支树Tm初始匹配的点云数量seed0为4。

[0164] 4.5)对于分支树Tn中的某一节点建立的点云密度分支树Tm中节点，当前点云P′中间隔取点，取点数目为seedm，并在模板点云中采用本发明中“基于最近邻方法的点云匹配”
计算目标函数em。若该节点非叶节点且该节点没有被剪枝，则计算目标函数的预测值，即启
发函数fe。

[0165] 4.6)采用本发明中“梯度点云密度‑多分辨率优化搜索方法”的内容④对分支树进行剪枝操作。Tn第n层中点云密度分支树Tm所有的节点搜索完全或被剪掉。将第n层的最优值
en_b进行判断是否已符合要求或者该值不再减小。若没有，则返回步骤4.3)以该最优值为基
础建立更高分辨率的分支树Tn+1，并继续搜索，直至搜索完成。该实施例中的分支树搜索规
模与优化效率如表1所示。

[0166] 表1

[0167]

[0168] 其中节点数与最优节点中的4为点云密度搜索树中的节点数，其值等于Tm中的最大层数M，11为角度采样数numθ，5为numx、numy。该表格数据的硬件测试环境为CPU：i5‑
7300HQ 2.50GHZ，搜索完成时耗时小于10ms，具备高精度的同时具有较好的实时性。

[0169] 4.7)分支树搜索完成，将搜索结果转换为期望位姿到AGV位姿的相对位姿从而获得AGV相对于“目标”的高精度定位。

[0170] ●AGV连续运动下的定位实验结果：在该实施例测试环境下AGV在不同位置对于“目标”的识别与检测效果。其中运动过程中相对于“目标”的位置定位结果和姿态定位结果
分别如图5中的(a)、(b)所示。红色线段是AGV位置定位和姿态定位的原生值，蓝色是经过滤
波后的位置和姿态。根据实际测量三个关键点的位置和姿态，该识别结果具备较高的精度。

[0171] ●AGV静态环境下的定位实验结果：其中静态环境下相对“目标”的位置定位静态抖动误差和姿态定位抖动误差分布如图6中的(a)、(b)、(c)所示。由图可以看出，AGV在静止
状态下相对于“目标”的定位结果的滤波前抖动误差为±0.0025m，滤波后抖动误差为±
0.0015m。姿态定位结果的滤波前抖动误差为±0.01rad，滤波后抖动误差为±0.005rad。具
有较高的精度，满足AGV对充电电刷的对准要求。

[0172] Step 5：采用本发明的AGV平滑运动控制对AGV进行运动控制，具体步骤如下：

[0173] 5.1)通过Step4获取“目标”与AGV的相对位姿 其中R和t可表示为：

[0174]

[0175] 将相对位姿T转换为本发明中“AGV平滑运动控制”中内容①的期望目标控制量(r，T
θ，δ) ，转换公式如下：

[0176]

[0177] 5.2)通过本发明中AGV平滑运动控制方法中的内容①建立运动学模型，其建立的运动学模型如图7所示。

[0178] 5.3)通过本发明中AGV平滑运动控制方法中的内容②中的控制器设计，采用如图8T
所示的控制结构，输入控制目标(r，θ，δ) ，获取AGV运动控制的控制量(v，w)，使AGV稳定平
滑的运动到目标点。

[0179] 5.4)可通过调节并设定参数k1，k2，使AGV能够以较大的曲率半径到达目标点位姿，从而能够达到充电对位，或某些特定任务对位对于AGV运动控制的角度姿态要求。其参数
k1，k2对与AGV运动控制的影响如图9中的(a)、(b)所示。其中图(a)描述了k1对于运动控制的
影响，由本发明中AGV平滑运动控制方法中的内容②中的控制系统(i)，可以得到如下所示：

[0180]

[0181]

[0182] k1反映角度θ变化速度与距离r变化速度之比。k1越小，AGV将以更大的曲率半径到达目标点。图(b)描述了k2对于运动控制的影响，由k2与运动控制器的关系：

[0183]

[0184] 可知k2越小，AGV对于虚拟变量δ的跟踪越缓慢，AGV将以更加平滑的轨迹到达目标点。通过设定参数k1，k2使AGV能够平滑稳定，且以较大曲率半径到达目标点，完成自动充电
等复杂高精度任务。

[0185] Step 6：基于AGV平台，对于本实施例进行AGV自动充电定位对准与运动控制测试。采用本发明的高精度定位与目标对准控制方法，AGV实现自动充电对接的测试结果如图10
所示：测试连续10次基于AGV到达粗略点位姿的不同情况的导航定位结果，图中的单位为毫
米。其中AGV定位位姿X轴最大重复定位误差为±3.4mm，Y轴最大重复定位误差为±5.2mm，
而本系统所使用的充电电刷容许最大对位误差为±10mm，所以完全满足自动充电的对接要
求，且具有较高稳定性。