CN115712298A - 基于单线激光雷达的机器人在通道中行驶的自主导航方法 - Google Patents

Abstract

本发明涉及一种基于单线激光雷达的机器人在通道中行驶的自主导航方法，属于机器人自主导航技术领域。包括：实时获取机器人在通道内行驶时其上配置的单线激光雷达扫描到通道壁面的点云数据；对点云数据进行领域聚类，将点云数据划分为多个领域；计算各领域的角点，根据角点对各领域进行分割得到分离的线段；根据各领域分离的线段和路况属性判断机器人当前所处路况；根据各领域分离的线段和点云数据判断机器人所在位置相对于通道壁面的位姿；根据机器人当前所处路况和预设路口执行动作控制机器人在通道中行驶，并在行驶过程中根据机器人所在位置相对于通道壁面的位姿对机器人进行实时纠偏。本发明可以实现机器人在通道中行驶的完全自主导航。

Description

基于单线激光雷达的机器人在通道中行驶的自主导航方法

技术领域

本发明涉及机器人自主导航技术领域，尤其涉及一种基于单线激光雷达的通道机器人自主导航方法。

背景技术

移动机器人的自主导航问题一直是自动控制和机器人应用领域的重要研究课题。在许多情况下，移动机器人在作业范围内行驶的通道非常狭窄，例如中央空调的通风管道中。空调的通风管道在使用中会集聚污物，这不仅降低了空调的工作效率，而且不清洁的空气也影响人体健康，因此需要定期清洗空调的通风管道。然而类似于空调的通风管道这类型的通道通常比较狭窄、黑暗、较为封闭，且考虑到人员安全，通常采用移动的机器人来清洗。

移动的机器人的机体上通常安装若干摄像头及光源，能够随时监测通道内的状况和移动机器人工作情况。但目前移动机器人还不具备完全自主导航能力，故采用拖线或遥控方式控制，由操作人员在通道外进行操作。

目前自主导航方式主要有GPS导航、电磁导航、超声波测距导航等。由于管道中GPS信号会被屏蔽，且电磁导航线不便于布置与维护，因此GPS导航和电磁导航并不适合在室内狭窄的通道中使用。超声波测距传感器成本低，方便搭载在机器人上，可以获取通道机器人与通道壁面相对位置，例如中国发明专利CN201310390231.3，名称为“一种应用于温室的自主移动车辆超声波导航装置”，该专利虽然可以解决在狭窄直道实施往返行走的问题，但通道机器人不能应对各种路口(十字路口、T型路口、拐角等)识别和通行的难题。

综上，目前机器人在通道中行走存在以下问题：(1)还不具备完全自主导航的能力。(2)GPS导航无法在室内通道中使用。(3)巡线导航方式在狭窄通道中实施困难。(4)超声波导航装置在无法估计机器人在各种路口中位姿。

发明内容

为解决上述技术问题，本发明提供一种基于单线激光雷达的机器人在通道中行驶的自主导航方法。所述的技术方案如下：

一种基于单线激光雷达的机器人在通道中行驶的自主导航方法，其特征在于，包括如下步骤：

S1，实时获取机器人在通道内行驶时其上配置的单线激光雷达扫描到通道壁面的点云数据；

S2，对点云数据进行领域聚类，将点云数据划分为多个领域；

S3，计算各领域的点云数据中的角点，根据各领域的角点分别对各领域进行分割，得到各领域分离的线段；

S4，根据各领域分离的线段和路况属性判断机器人当前所处路况；

S5，根据各领域分离的线段和点云数据判断机器人所在位置相对于通道壁面的位姿；

S6，根据机器人当前所处路况和预设路口执行动作控制机器人在通道中行驶，并在行驶过程中根据机器人所在位置相对于通道壁面的位姿对机器人进行实时纠偏。

可选地，所述S2在对点云数据进行领域聚类，将点云数据划分为多个领域时，包括如下步骤：

S21，从点云数据中筛选距离小于r_t的点云数据，并通过如下公式(1)将激光雷达极坐标系下的点云数据转换到平面直角坐标系下：

x_i＝-L_icosθ_i,y_i＝-L_isinθ_i,L_i＜r_t (1)

其中，平面直角坐标系x_ry_r固连于机器人的底盘中心，x_r轴为机器人的前进方向，全局直角坐标系xy位于通道入口，单线激光雷达的第一个距离数据为L_0°，L_0°位于x_r轴的负向，其它数据沿逆时针方向排列，相邻激光的间隔角为χ；公式(1)中，L_i表示点云数据中的第i个距离数据，其对应的极角为θ_i，点云数据的数目总计为n，r_t表示筛选半径且w＜r_t，w表示通道宽度；

S22，初始化领域分割参数：建立矩阵DS来存放点云序号，其中，同一领域的点云数据的序号位于该矩阵DS的同一行；设置点云数据中的1号点位于矩阵DS的第一行第一列；

S23，依次判断i号点是否为点云数据中的最后一个点n；如果是否，则计算i号点与i+1号点之间的距离ρ_i；若ρ_i小于r_d，则确定i+1号与i号点属于同一领域，将i+1号记录在矩阵DS的同一行，矩阵DS的列号自增1；若ρ_i大于等于r_d，则确定i+1号与i号点属于不同领域，矩阵DS的行号自增1，列号设置为1；若i号点是最后一个点，则计算n号点与1号点之间的距离，且若这两点之间的距离小于r_d，则1号与n号点属于同一领域，矩阵DS的列号自增1；若这两点之间的距离不小于r_d，则直接跳转到S24；其中，r_d表示聚类半径，0＜r_d＜w；

S24，若1号和n号点属于同一个领域，则将预设的标志位M置1，并将第一行和最后一行合并为一行，并去掉重复的点号，用这些点号覆盖矩阵DS的第一行，接着去掉DS的最后一行；若1号和n号点属于不同领域，则将预设的标志位M置0，然后跳转至S25；

S25，将矩阵DS中非零数据数小于预设数值的行删除，并分别用不同的形状标记不同领域的点云数据。

可选地，所述S3在计算各领域的点云数据中的角点，根据各领域的角点分别对各领域进行分割，得到各领域分离的线段时，对于任一领域，在计算其点云数据中的角点和分离的线段时，包括如下步骤：

S31，初始化领域分割参数：建立矩阵DL来存放点云序号，其中同一线段的点云数据的序号位于矩阵DL的同一行；设置分割时遍历点云序号的递增量I，并从矩阵DS的第k行的第二个元素开始遍历该领域的点云数据；

S32，依次求解I点两侧的向量

和

并计算这两个向量的夹角α_I，直到I等于矩阵DS中该行的倒数第二个列号，其中，A和B分别是I点左右两侧的相邻点；

S33，获取α_I的最大值，并得到其所对应的下标Λ；

S34，判断α_Λ是否小于ψ，如果是，则该领域内无角点，确定该领域内的所有t_k个点为一整条直线，直线数n_l自增1，记录该直线包含的点云序号为：DL(n_l,:)＝DS(k,1:t_k)；否则，确定该领域内存在角点Λ，角点数n_t自增1；在角点处分离出两条直线段，线段数n_l自增1，记录该直线包含的点云序号为：DL(n_l,:)＝DS(k,1:Λ-1)，n_l再自增1，记录另外一条直线包含的点云序号为：DL(n_l,:)＝DS(k,Λ+1:t_k)；其中，‘:’在‘DL(n_l,:)’中表示矩阵DL第n_l行的所有列元素；‘Λ+1:t_k’在‘DS(k,Λ+1:t_k)’中表示矩阵DS第k行的第Λ+1列到t_k列的元素；其他类似的表示同理；ψ大于0且小于通道壁面间的最小夹角。

可选地，所述S4在根据各领域分离的线段和路况属性判断机器人当前所处路况时，包括如下几种情况：

S41，当各领域的角点数之和与线段数之和为n_t＝4，n_l＝8时，确定机器人当前所处路况为十字路口，机器人的候选移动行为包括直行、左转或右转；

S42，当各领域的角点数之和与线段数之和为n_t＝2，n_l＝5，且L_0°＞a，L_90°＞a，L_180°＜a，L_270°＞a时，确定机器人当前所处路况为T型路口，机器人的候选移动行为包括左转或右转；

S43，当各领域的角点数之和与线段数之和为n_t＝2，n_l＝5，且L_0°＞a，L_90°＜a，L_180°＞a，L_270°＞a时，确定机器人当前所处路况为T型路口，机器人的候选移动行为包括直行或左转；

S44，当各领域的角点数之和与线段数之和为n_t＝2，n_l＝5，且L_0°＞a，L_90°＞a，L_180°＞a，L_270°＜a时，确定机器人当前所处路况为T型路口，机器人的候选移动行为包括直行或右转；

S45，当各领域的角点数之和与线段数之和为n_t＝2，n_l＝4，δ＞0且90°-ε≥|δ|，或者n_t＝2，n_l＝4，L_90°＜a且90°-ε＜|δ|时，确定机器人当前所处路况为拐角，机器人的候选移动行为为左转；

S46，当各领域的角点数之和与线段数之和为n_t＝2，n_l＝4，δ＜0且90°-ε≥|δ|，或者n_t＝2，n_l＝4，L_270°＜a且90°-ε＜|δ|时，确定机器人当前所处路况为拐角，机器人的候选移动行为为右转；

其中，拐角包括左拐角和右拐角，左拐角和右拐角根据机器人右侧相邻两线段的夹角δ的正负值来区分：

公式(2)中，k₁和k₂为两线段的斜率，k₁ k₂≠-1，ε为一个小角；

可选地，所述S5在根据各领域分离的线段和点云数据判断机器人所在位置相对于通道壁面的位姿时，包括：

若机器人处于直道或者从直道进入路口，使用机器人侧后方的两个相邻距离L_j和L_j-1，通过如下公式(3)和公式(4)估计机器人相对于壁面的位姿：

公式(3)和公式(4)中，偏航角

和横偏量

由L_j和L_j-1计算得到，L_j-1所对应射线与壁面的夹角γ₂由L_j和L_j-1通过余弦定理求得，通道宽度w、L_j和L_j-1所对应射线间的夹角γ₁、L_j-1所对应射线与x_r轴负向的夹角γ₃、L_j-1所对应射线与y_r轴负向的夹角γ₄均已知，L_j和L_j-1所对应射线根据通道宽度w来选取；

若机器人离开路口进入直道，使用机器人侧前方的两个相邻距离估计机器人相对于壁面的位姿；

若机器人遇到路口，利用分离的线段来估计机器人在路口中的位姿，具体以机器人进路口之前的前进方向为前方，取路口右后侧的两条相邻线段估计机器人相对于壁面的位姿。

可选地，若机器人遇到路口为十字路口，假设机器人离壁面的水平方向和竖直方向距离d_h和d_v，则通过直线①和②来估计机器人相对于壁面的位姿，①和②通过属于DL矩阵中第一行和第二行的点云来拟合；在估计直线①时，假设待拟合点数为t，那么：

公式(5)中，(A⁺y)₍₁₎表示列向量(A⁺y)的第一个参数，也即直线的截距，(A⁺y)₍₂₎为直线的斜率，符号“+”表示伪逆；同理可得d_v；

那么机器人相对于T型路口或十字中心的的偏航距离为：

公式(6)中，

为前进方向的偏差，

为偏离通道中心线的距离；对于拐角路口来说，偏航距离为两个角点连线的中点坐标值；

机器人在路口中的偏航角通过直线①和如下公式(7)来估计：

上述所有可选地技术方案均可任意组合，本发明不对一一组合后的结构进行详细说明。

借由上述方案，本发明的有益效果如下：

通过在机器人上配置单线激光雷达，并通过对扫描到通道壁面的点云数据进行领域聚类和分割得到分离的线段，进而根据各领域分离的线段和路况属性判断机器人当前所处路况，以及根据各领域分离的线段和点云数据判断机器人所在位置相对于通道壁面的位姿，使得机器人在通道中行驶时，可以根据机器人当前所处路况和预设路口执行动作实现自主导航，并在行驶过程中根据机器人所在位置相对于通道壁面的位姿对机器人进行实时纠偏，从而可以避免机器人在通道中偏航。由于本发明基于单线激光雷达来实现，因而既可以在室内导航，也可以实现室外的导航，应用范围广。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，并可依照说明书的内容予以实施，以下以本发明的较佳实施例并配合附图详细说明如后。

附图说明

图1是本发明中机器人及其作业环境示意图。

图2是本发明的流程图。

图3是本发明的聚类结果示意图。

图4是本发明中领域聚类的算法流程图。

图5是本发明中领域分割得到分离的线段的示意图。

图6是本发明中角点识别与领域分割算法的流程图。

图7是T型路口的第一种情况辨识图。

图8是T型路口的第二种情况辨识图。

图9是T型路口的第三种情况辨识图。

图10是第一种拐角类型的辨识图。

图11是第二种拐角类型的辨识图。

图12是本发明中机器人沿直道中心线行走的示意图。

图13是本发明中机器人在十字路口直行时的参考线段示意图。

图14是本发明中机器人在十字路口右转时参考线段变化的示意图。

图15是本发明中机器人在十字路口左转时参考线段变化的示意图。

图16是本发明仿真时在十字路口的聚类分割效果图。

图17是本发明仿真时在T型路口的聚类分割效果图。

图18是本发明仿真时在拐角路口的聚类分割效果图。

图19是本发明的仿真结果图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

如图1所示，其为本发明提供的基于单线激光雷达的机器人在通道中行驶的自主导航方法中机器人的作业环境示意图。本发明中所述的机器人可以为两轮差速驱动的机器人，其底盘中心配置有单线激光雷达，本发明基于单线激光雷达扫描到通道壁面上的点云数据进行自主导航。由于单线激光雷达成本低，且能够有效重构平面路况，因此本发明选择单线激光雷达来感知机器人与通道壁面的相对位姿。本发明中所述的通道可以为中央空调的通风管道等比较闭塞的通道，通道中可以包括直道、拐角、T型路口和十字路口等路况。在本发明提供的方法的基础上，只需要预先给出机器人在路口的执行动作(直行或右转或左转)，机器人即可按照其设定的路线(图1所示的虚线)行进。

结合上述内容，如图2所示，本发明提供的基于单线激光雷达的机器人在通道中行驶的自主导航方法，包括如下步骤：

S1，实时获取机器人在通道内行驶时其上配置的单线激光雷达扫描到通道壁面的点云数据。

S2，对点云数据进行领域聚类，将点云数据划分为多个领域。

具体地，所述S2在对点云数据进行领域聚类，将点云数据划分为多个领域时，如图3所示，以十字路口为例，基本思想为从点云数据中的1号点开始，聚类半径r_d以内的点云为同一领域，r_d以外为不同领域。如图4所示，具体步骤如下：

S21，从点云数据中筛选距离小于r_t的点云数据，并通过如下公式(1)将激光雷达极坐标系下的点云数据转换到平面直角坐标系下：

x_i＝-L_icosθ_i,y_i＝-L_isinθ_i,L_i＜r_t (1)

其中，平面直角坐标系x_ry_r固连于机器人的底盘中心，x_r轴为机器人的前进方向，全局直角坐标系xy位于通道入口，单线激光雷达的第一个距离数据为L_0°，L_0°位于x_r轴的负向，其它数据沿逆时针方向排列，相邻激光的间隔角为χ；公式(1)中，L_i表示点云数据中的第i个距离数据，其对应的极角为θ_i，点云数据的数目总计为n，r_t表示筛选半径且w＜r_t，w表示通道宽度。

S22，初始化领域分割参数：建立矩阵DS来存放点云序号，其中，同一领域的点云数据的序号位于该矩阵DS的同一行；设置点云数据中的1号点位于矩阵DS的第一行第一列。

S23，依次判断i号点是否为点云数据中的最后一个点n；如果是否，则计算i号点与i+1号点之间的距离ρ_i；若ρ_i小于r_d，则确定i+1号与i号点属于同一领域，将i+1号记录在矩阵DS的同一行，矩阵DS的列号自增1(col＝col+1)，DS(row,col)＝i+1；若ρ_i大于等于r_d，则确定i+1号与i号点属于不同领域，矩阵DS的行号自增1，列号设置为1；若i号点是最后一个点，则计算n号点与1号点之间的距离，且若这两点之间的距离小于r_d，则1号与n号点属于同一领域，矩阵DS的列号自增1，矩阵DS(row,col)＝1；若这两点之间的距离不小于r_d，则直接跳转到S24；其中，r_d表示聚类半径，0＜r_d＜w。

S24，若1号和n号点属于同一个领域，则将预设的标志位M置1，并将第一行和最后一行合并为一行，并去掉重复的点号，用这些点号覆盖矩阵DS的第一行，接着去掉DS的最后一行；若1号和n号点属于不同领域，则将预设的标志位M置0，然后跳转至S25。

S25，将矩阵DS中非零数据数小于预设数值的行删除，并分别用不同的形状标记不同领域的点云数据。预设数值可以根据需要设置为3，由于矩阵DS中的数据主要用于后续的线段拟合，而通常少于三个点的数据不能较好的拟合线段，因此，本发明设置预设数值优选为3。聚类后最终得到如图5所示的不同领域的点云数据，分别用圆圈、方块、三角和五角星标记。

S3，计算各领域的点云数据中的角点，根据各领域的角点分别对各领域进行分割，得到各领域分离的线段。

具体地，所述S3在计算各领域的点云数据中的角点，根据各领域的角点分别对各领域进行分割，得到各领域分离的线段时，对于任一领域，在计算其点云数据中的角点和分离的线段时，如图6所示，包括如下步骤：

S31，初始化领域分割参数：建立矩阵DL来存放点云序号，其中同一线段的点云数据的序号位于矩阵DL的同一行；设置分割时遍历点云序号的递增量I，并从矩阵DS的第k行的第二个元素开始遍历该领域的点云数据。

S32，依次求解I点两侧的向量

和

并计算这两个向量的夹角α_I，直到I等于矩阵DS中该行的倒数第二个列号，其中，A和B分别是I点左右两侧的相邻点。

S33，获取α_I的最大值，并得到其所对应的下标Λ。

S34，判断α_Λ是否小于ψ，如果是，则该领域内无角点，确定该领域内的所有t_k个点为一整条直线，直线数n_l自增1，记录该直线包含的点云序号为：DL(n_l,:)＝DS(k,1:t_k)；否则，确定该领域内存在角点Λ，角点数n_t自增1；在角点处分离出两条直线段，线段数n_l自增1，记录该直线包含的点云序号为：DL(n_l,:)＝DS(k,1:Λ-1)，n_l再自增1，记录另外一条直线包含的点云序号为：DL(n_l,:)＝DS(k,Λ+1:t_k)；其中，‘:’在‘DL(n_l,:)’中表示矩阵DL第n_l行的所有列元素；‘Λ+1:t_k’在‘DS(k,Λ+1:t_k)’中表示矩阵DS第k行的第Λ+1列到t_k列的元素；其他类似的表示同理；ψ大于0且小于通道壁面间的最小夹角。

S4，根据各领域分离的线段和路况属性判断机器人当前所处路况。

具体地，结合上述分离的线段和下表1来判别机器人当前所处路况。

在具体实现时，如图7至图9所示，根据L_0°，L_90°，L_180°，L_270°与阈值a的关系，机器人在T型路口有三种情况。如图10和图11所示，路口有左拐角和右拐角之分。在上述内容的基础上，所述S4在根据各领域分离的线段和路况属性判断机器人当前所处路况时，包括如下几种情况：

S41，当各领域的角点数之和与线段数之和为n_t＝4，n_l＝8时，确定机器人当前所处路况为十字路口，机器人的候选移动行为包括直行、左转或右转；

S42，当各领域的角点数之和与线段数之和为n_t＝2，n_l＝5，且L_0°＞a，L_90°＞a，L_180°＜a，L_270°＞a时，确定机器人当前所处路况为T型路口，机器人的候选移动行为包括左转或右转；

S43，当各领域的角点数之和与线段数之和为n_t＝2，n_l＝5，且L_0°＞a，L_90°＜a，L_180°＞a，L_270°＞a时，确定机器人当前所处路况为T型路口，机器人的候选移动行为包括直行或左转；

S44，当各领域的角点数之和与线段数之和为n_t＝2，n_l＝5，且L_0°＞a，L_90°＞a，L_180°＞a，L_270°＜a时，确定机器人当前所处路况为T型路口，机器人的候选移动行为包括直行或右转；

S45，当各领域的角点数之和与线段数之和为n_t＝2，n_l＝4，δ＞0且90°-ε≥|δ|，或者n_t＝2，n_l＝4，L_90°＜a且90°-ε＜|δ|时，确定机器人当前所处路况为拐角，机器人的候选移动行为为左转；

S46，当各领域的角点数之和与线段数之和为n_t＝2，n_l＝4，δ＜0且90°-ε≥|δ|，或者n_t＝2，n_l＝4，L_270°＜a且90°-ε＜|δ|时，确定机器人当前所处路况为拐角，机器人的候选移动行为为右转；

其中，拐角包括左拐角和右拐角，左拐角和右拐角根据机器人右侧相邻两线段的夹角δ的正负值来区分：

公式(2)中，k₁和k₂为两线段的斜率，k₁ k₂≠-1，ε为一个较小的角如1°；

S5，根据各领域分离的线段和点云数据判断机器人所在位置相对于通道壁面的位姿。

该步骤为机器人在通道中行走的全域闭环控制算法步骤，用于实时确定机器人在通道中行走时是否发生偏航以及偏航量是多少。

具体地，所述S5在根据各领域分离的线段和点云数据判断机器人所在位置相对于通道壁面的位姿时，有如下情况：

如图12所示，其为机器人沿直道中心线行走的示意图。若机器人处于直道或者从直道进入路口，使用机器人侧后方的两个相邻距离L_j和L_j-1，通过如下公式(3)和公式(4)估计机器人相对于壁面的位姿，这样可以使机器人平稳的通过过渡位置：

公式(3)和公式(4)中，偏航角

和横偏量

由L_j和L_j-1计算得到，L_j-1所对应射线与壁面的夹角γ₂由L_j和L_j-1通过余弦定理求得，通道宽度w、L_j和L_j-1所对应射线间的夹角γ₁、L_j-1所对应射线与x_r轴负向的夹角γ₃、L_j-1所对应射线与y_r轴负向的夹角γ₄均已知，L_j和L_j-1所对应射线根据通道宽度w来选取。

若机器人离开路口进入直道，使用机器人侧前方的两个相邻距离，类似的可求得

和

若机器人遇到路口，利用分离的线段来估计机器人在路口中的位姿，具体以机器人进路口之前的前进方向为前方，取路口右后侧的两条相邻线段估计机器人相对于壁面的位姿。

进一步地，若机器人遇到路口为十字路口，如图5所示，假设机器人离壁面的水平方向和竖直方向距离d_h和d_v，则通过图5中所示的直线①和②来估计机器人相对于壁面的位姿直线①和②通过属于DL矩阵中第一行和第二行的点云来拟合；以估计直线①为例，假设待拟合点数为t，那么：

公式(5)中，(A⁺y)₍₁₎表示列向量(A⁺y)的第一个参数，也即直线的截距，(A⁺y)₍₂₎为直线的斜率，符号“+”表示伪逆；同理可得d_v；

那么机器人相对于T型路口或十字中心的的偏航距离为：

公式(6)中，

为前进方向的偏差，

为偏离通道中心线的距离。对于拐角路口来说，偏航距离为两个角点连线的中点坐标值。

机器人在路口中的偏航角通过直线①和如下公式(7)来估计：

需要说明的是，机器人在路口通行时，获取其偏航状态的参考线段号是不同的，根本宗旨是以机器人进路口之前的前进方向为前方，取路口右后方的两条线段计算偏航角。以十字路口为例，如图13所示，机器人处于直行状态时，其参考线段号为①和②；当其处于右转状态时，并且首尾点云合并标志位M发生上升沿跳变时，如图14所示，其参考线段号需要从①和②变为③和④；当其处于左转状态时，并且首尾点云合并标志位M发生下降沿跳变时，如图15所示，其参考线段号需要从①和②变为⑦和⑧。其他路口参考线段号如表2所示。

表2

S6，根据机器人当前所处路况和预设路口执行动作控制机器人在通道中行驶，并在行驶过程中根据机器人所在位置相对于通道壁面的位姿对机器人进行实时纠偏。

具体地，由于机器人在通道各个位置都能获取其偏航状态，因此可以进行全域闭环控制。机器人在通道中直行是以恒定线速度

沿x_r方向，转弯均为路口中心进行原地旋转相应的角度。机器人在纠偏时，通过实时调整其角速度来矫正其偏离中心线的程度。

为了验证本发明提出的上述方法的可行性，本发明还提供如下仿真案例：

首先在gazebo物理仿真软件中，如图1所示，gazebo软件中建立通道，通道宽度w为0.8m，并将配备单线激光雷达的两轮差速驱动的机器人放置在其中，激光雷达的激光间隔角为5°。接下来便测试本发明提供的方法。机器人起点为机器人中心距离通道入口0.5m的位置，初始偏航量为0.05m，初始偏航角为10°；给定机器人的行进路线，顺序经过的通行方式为直道、十字路(右转)、直道、T型路口(左转)、直道、90°拐角(左转)、直道、45°拐角(左转)，将第二个45°拐角的中心设置为机器人终点。在仿真过程中，传感器采样率和控制器控制率都为10Hz，路况判断的阈值a为0.8米，机器人直行时的线速度为0.1m/s，机器人在路口原地转向时的角速度为0.2rad/s。

首先令机器人静止在路口，通过上述方法估计机器人与通道壁面的相对位姿。图16至图18为聚类及线段分割结果示意图，从图中可以看出该方法很好将激光雷达的点云数据进行聚类分割，准确地识别出了角点数据。提取相关的点云进行参考直线拟合，进而估计出了机器人的偏航量Δx、Δy，以及偏航角β。估计结果如表3所示。

表3

从表3可以看出，估计值与实际值的相对误差均在2％以内。如图19所示，其为仿真结果示意图，从图19可以看出，机器人经过第一个路口前，在沿直道中心线行走的闭环控制算法下的很快的纠正了机器人偏航状态，之后机器人的实际轨迹与轨迹基本一致，说明算法是有效的。

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

Claims (6)

1.一种基于单线激光雷达的机器人在通道中行驶的自主导航方法，其特征在于，包括如下步骤：

S1，实时获取机器人在通道内行驶时其上配置的单线激光雷达扫描到通道壁面的点云数据；

S2，对点云数据进行领域聚类，将点云数据划分为多个领域；

S3，计算各领域的点云数据中的角点，根据各领域的角点分别对各领域进行分割，得到各领域分离的线段；

S4，根据各领域分离的线段和路况属性判断机器人当前所处路况；

S5，根据各领域分离的线段和点云数据判断机器人所在位置相对于通道壁面的位姿；

S6，根据机器人当前所处路况和预设路口执行动作控制机器人在通道中行驶，并在行驶过程中根据机器人所在位置相对于通道壁面的位姿对机器人进行实时纠偏。

2.根据权利要求1所述的基于单线激光雷达的机器人在通道中行驶的自主导航方法，其特征在于，所述S2在对点云数据进行领域聚类，将点云数据划分为多个领域时，包括如下步骤：

S21，从点云数据中筛选距离小于r_t的点云数据，并通过如下公式(1)将激光雷达极坐标系下的点云数据转换到平面直角坐标系下：

x_i＝-L_icosθ_i,y_i＝-L_isinθ_i,L_i＜r_t (1)

其中，平面直角坐标系x_ry_r固连于机器人的底盘中心，x_r轴为机器人的前进方向，全局直角坐标系xy位于通道入口，单线激光雷达的第一个距离数据为L_0°，L_0°位于x_r轴的负向，其它数据沿逆时针方向排列，相邻激光的间隔角为χ；公式(1)中，L_i表示点云数据中的第i个距离数据，其对应的极角为θ_i，点云数据的数目总计为n，r_t表示筛选半径且w＜r_t，w表示通道宽度；

S22，初始化领域分割参数：建立矩阵DS来存放点云序号，其中，同一领域的点云数据的序号位于该矩阵DS的同一行；设置点云数据中的1号点位于矩阵DS的第一行第一列；

S23，依次判断i号点是否为点云数据中的最后一个点n；如果是否，则计算i号点与i+1号点之间的距离ρ_i；若ρ_i小于r_d，则确定i+1号与i号点属于同一领域，将i+1号记录在矩阵DS的同一行，矩阵DS的列号自增1；若ρ_i大于等于r_d，则确定i+1号与i号点属于不同领域，矩阵DS的行号自增1，列号设置为1；若i号点是最后一个点，则计算n号点与1号点之间的距离，且若这两点之间的距离小于r_d，则1号与n号点属于同一领域，矩阵DS的列号自增1；若这两点之间的距离不小于r_d，则直接跳转到S24；其中，r_d表示聚类半径，0＜r_d＜w；

S24，若1号和n号点属于同一个领域，则将预设的标志位M置1，并将第一行和最后一行合并为一行，并去掉重复的点号，用这些点号覆盖矩阵DS的第一行，接着去掉DS的最后一行；若1号和n号点属于不同领域，则将预设的标志位M置0，然后跳转至S25；

S25，将矩阵DS中非零数据数小于预设数值的行删除，并分别用不同的形状标记不同领域的点云数据。

3.根据权利要求2所述的基于单线激光雷达的机器人在通道中行驶的自主导航方法，其特征在于，所述S3在计算各领域的点云数据中的角点，根据各领域的角点分别对各领域进行分割，得到各领域分离的线段时，对于任一领域，在计算其点云数据中的角点和分离的线段时，包括如下步骤：

S31，初始化领域分割参数：建立矩阵DL来存放点云序号，其中同一线段的点云数据的序号位于矩阵DL的同一行；设置分割时遍历点云序号的递增量I，并从矩阵DS的第k行的第二个元素开始遍历该领域的点云数据；

S32，依次求解I点两侧的向量

和

并计算这两个向量的夹角α_I，直到I等于矩阵DS中该行的倒数第二个列号，其中，A和B分别是I点左右两侧的相邻点；

S33，获取α_I的最大值，并得到其所对应的下标Λ；

S34，判断α_Λ是否小于ψ，如果是，则该领域内无角点，确定该领域内的所有t_k个点为一整条直线，直线数n_l自增1，记录该直线包含的点云序号为：DL(n_l,:)＝DS(k,1:t_k)；否则，确定该领域内存在角点Λ，角点数n_t自增1；在角点处分离出两条直线段，线段数n_l自增1，记录该直线包含的点云序号为：DL(n_l,:)＝DS(k,1:Λ-1)，n_l再自增1，记录另外一条直线包含的点云序号为：DL(n_l,:)＝DS(k,Λ+1:t_k)；其中，‘:’在‘DL(n_l,:)’中表示矩阵DL第n_l行的所有列元素；‘Λ+1:t_k’在‘DS(k,Λ+1:t_k)’中表示矩阵DS第k行的第Λ+1列到t_k列的元素；其他类似的表示同理；ψ大于0且小于通道壁面间的最小夹角。

4.根据权利要求3所述的基于单线激光雷达的机器人在通道中行驶的自主导航方法，其特征在于，所述S4在根据各领域分离的线段和路况属性判断机器人当前所处路况时，包括如下几种情况：

S41，当各领域的角点数之和与线段数之和为n_t＝4，n_l＝8时，确定机器人当前所处路况为十字路口，机器人的候选移动行为包括直行、左转或右转；

S42，当各领域的角点数之和与线段数之和为n_t＝2，n_l＝5，且L_0°＞a，L_90°＞a，L_180°＜a，L_270°＞a时，确定机器人当前所处路况为T型路口，机器人的候选移动行为包括左转或右转；

S43，当各领域的角点数之和与线段数之和为n_t＝2，n_l＝5，且L_0°＞a，L_90°＜a，L_180°＞a，L_270°＞a时，确定机器人当前所处路况为T型路口，机器人的候选移动行为包括直行或左转；

S44，当各领域的角点数之和与线段数之和为n_t＝2，n_l＝5，且L_0°＞a，L_90°＞a，L_180°＞a，L_270°＜a时，确定机器人当前所处路况为T型路口，机器人的候选移动行为包括直行或右转；

S45，当各领域的角点数之和与线段数之和为n_t＝2，n_l＝4，δ＞0且90°-ε≥|δ|，或者n_t＝2，n_l＝4，L_90°＜a且90°-ε＜|δ|时，确定机器人当前所处路况为拐角，机器人的候选移动行为为左转；

S46，当各领域的角点数之和与线段数之和为n_t＝2，n_l＝4，δ＜0且90°-ε≥|δ|，或者n_t＝2，n_l＝4，L_270°＜a且90°-ε＜|δ|时，确定机器人当前所处路况为拐角，机器人的候选移动行为为右转；

其中，拐角包括左拐角和右拐角，左拐角和右拐角根据机器人右侧相邻两线段的夹角δ的正负值来区分：

公式(2)中，k₁和k₂为两线段的斜率，k₁ k₂≠-1，ε为一个小角；

5.根据权利要求3所述的基于单线激光雷达的机器人在通道中行驶的自主导航方法，其特征在于，所述S5在根据各领域分离的线段和点云数据判断机器人所在位置相对于通道壁面的位姿时，包括：

若机器人处于直道或者从直道进入路口，使用机器人侧后方的两个相邻距离L_j和L_j-1，通过如下公式(3)和公式(4)估计机器人相对于壁面的位姿：

公式(3)和公式(4)中，偏航角

和横偏量

由L_j和L_j-1计算得到，L_j-1所对应射线与壁面的夹角γ₂由L_j和L_j-1通过余弦定理求得，通道宽度w、L_j和L_j-1所对应射线间的夹角γ₁、L_j-1所对应射线与x_r轴负向的夹角γ₃、L_j-1所对应射线与y_r轴负向的夹角γ₄均已知，L_j和L_j-1所对应射线根据通道宽度w来选取；

若机器人离开路口进入直道，使用机器人侧前方的两个相邻距离估计机器人相对于壁面的位姿；

若机器人遇到路口，利用分离的线段来估计机器人在路口中的位姿，具体以机器人进路口之前的前进方向为前方，取路口右后侧的两条相邻线段估计机器人相对于壁面的位姿。

6.根据权利要求5所述的基于单线激光雷达的机器人在通道中行驶的自主导航方法，其特征在于，若机器人遇到路口为十字路口，假设机器人离壁面的水平方向和竖直方向距离d_h和d_v，则通过直线①和②来估计机器人相对于壁面的位姿，①和②通过属于DL矩阵中第一行和第二行的点云来拟合；在估计直线①时，假设待拟合点数为t，那么：

y＝[y₁ y₂ … y_t]^T

公式(5)中，(A⁺y)₍₁₎表示列向量(A⁺y)的第一个参数，也即直线的截距，(A⁺y)₍₂₎为直线的斜率，符号“+”表示伪逆；同理可得d_v；

那么机器人相对于T型路口或十字中心的的偏航距离为：

公式(6)中，

为前进方向的偏差，

为偏离通道中心线的距离；对于拐角路口来说，偏航距离为两个角点连线的中点坐标值；

机器人在路口中的偏航角通过直线①和如下公式(7)来估计：

Images