CN115686011A - 机器人沿栅栏沿边行走的算法、系统及电子设备 - Google Patents

Abstract

本发明公开了一种机器人沿栅栏沿边行走的算法、系统及电子设备，算法包括以下步骤：控制机器人沿栅栏的一侧边沿行走；采集机器人的激光雷达的点云数据；过滤点云数据中，机器人与栅栏之间超过预设距离的激光点；根据过滤后的点云数据，拟合直线，构建直线方程；根据直线方程，修正机器人的行走误差，以维持机器人定距离且平行于栅栏行走。本发明的机器人沿栅栏沿边行走的算法，通过采集激光雷达的点云数据，并且能够过滤掉过远的不符合要求的激光点，并构建直线方程，得到机器人行走的距离偏差和角度偏差，以此修正机器人的行走误差，控制机器人保持定距离、平行于栅栏行走。

Description

机器人沿栅栏沿边行走的算法、系统及电子设备

技术领域

本发明涉及移动机器人技术领域，更具体地，涉及一种机器人沿栅栏沿边行走的算法、系统及电子设备。

背景技术

现有技术中的机器人沿栅栏沿边行走时，容易出现行走偏差，无法保持定距离沿着栅栏沿边行走。

发明内容

本发明的一个目的是提供一种器人沿栅栏沿边行走的算法的新技术方案，至少能够解决现有技术中机器人沿栅栏沿边行走时，容易出现行走偏差等问题。

本发明的第一方面，提供了一种机器人沿栅栏沿边行走的算法，包括以下步骤：

控制机器人沿栅栏的一侧边沿行走；

采集所述机器人的激光雷达的点云数据；

过滤所述点云数据中，所述机器人与所述栅栏之间超过预设距离的激光点；

根据过滤后的所述点云数据，拟合直线，构建直线方程；

根据所述直线方程，修正所述机器人的行走误差，以维持所述机器人定距离且平行于所述栅栏行走。

可选地，采集所述机器人的激光雷达的点云数据包括：截取所述机器人的点云数据，针对每个所述激光点，构建二维坐标Pi(x,y)。

可选地，所述机器人与所述栅栏之间的所述预设距离为0.3-0.8m。

可选地，所述的机器人沿栅栏沿边行走的算法还包括：过滤所述点云数据中不满足预设反射强度的所述激光点。

可选地，根据过滤后的所述点云数据，拟合直线，构建直线方程的步骤包括：

根据每两个所述激光点拟合第一直线方程；所述第一直线方程为y＝k*x+b；其中，x为横坐标，y为纵坐标，k、b均为所述第一直线方程的系数，且k＝(y_w-y_z)/(x_w-x_z)，b＝y_w-k*x_w；y_w、x_w为Pl点的二维坐标，y_z、x_z为Pm的二维坐标，Pl和Pm为不同的所述激光点坐标；

Pl点从1开始到n/2，则Pm点为第(m-l-1)个点，算出来多个k和b后取均值k_agv、b_agv；

拟合出第二直线方程，所述第二直线方程为y＝k_agv*x+b_agv。

可选地，根据所述第二直线方程计算出所述机器人的中心。

可选地，在所述第二直线方程中，k_agv与x轴的夹角关系为theta＝arctan(k_agv)，机器人与所述栅栏保持定距离时的距离误差为d_error＝D-d_to_line，角度误差为theta，机器人修正误差的角速度为：

Angle_speed＝p1*d_error+p2*theta

其中，D为所述预设距离，d_to_line为坐标原点到所述第二直线的距离，p1为距离误差修正的控制系数，p2角度误差修正的控制系数。

根据本发明的第二方面，提供一种机器人沿栅栏沿边行走的算法系统，应用于上述实施例中所述的机器人沿栅栏沿边行走的算法，包括：

控制模块，所述控制模块用于控制机器人沿栅栏的一侧边沿行走；

采集模块，所述采集模块用于采集所述机器人的激光雷达的点云数据；

过滤模块，所述过滤模块用于过滤所述点云数据中，所述机器人与所述栅栏之间超过预设距离的激光点；

拟合模块，所述拟合模块用于根据过滤后的所述点云数据，拟合直线，并构建直线方程；

修正模块，所述修正模块根据所述直线方程，修正所述机器人的行走误差，以维持所述机器人定距离且平行于所述栅栏行走。

根据本发明的第三方面，提供一种电子设备，包括：处理器和存储器，在所述存储器中存储有计算机程序指令，其中，在所述计算机程序指令被所述处理器运行时，使得所述处理器执行上述实施例中所述的机器人沿栅栏沿边行走的算法的步骤。

根据本发明的第四方面，提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器运行时，使得所述处理器执行上述实施例中所述的机器人沿栅栏沿边行走的算法的步骤。

本发明的机器人沿栅栏沿边行走的算法，通过采集激光雷达的点云数据，并且能够过滤掉过远的不符合要求的激光点，并构建直线方程，得到机器人行走的距离偏差和角度偏差，以此修正机器人的行走误差，控制机器人保持定距离、平行于栅栏行走。

通过以下参照附图对本发明的示例性实施例的详细描述，本发明的其它特征及其优点将会变得清楚。

附图说明

被结合在说明书中并构成说明书的一部分的附图示出了本发明的实施例，并且连同其说明一起用于解释本发明的原理。

图1是根据本发明实施例的机器人沿栅栏沿边行走的算法的流程图

图2是根据本发明实施例的电子设备的原理图。

附图标记：

处理器201；

存储器202；操作系统2021；应用程序2022；

网络接口203；

输入设备204；

硬盘205；

显示设备206。

具体实施方式

现在将参照附图来详细描述本发明的各种示例性实施例。应注意到：除非另外具体说明，否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本发明的范围。

以下对至少一个示例性实施例的描述实际上仅仅是说明性的，决不作为对本发明及其应用或使用的任何限制。

对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论，但在适当情况下，所述技术、方法和设备应当被视为说明书的一部分。

在这里示出和讨论的所有例子中，任何具体值应被解释为仅仅是示例性的，而不是作为限制。因此，示例性实施例的其它例子可以具有不同的值。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步讨论。

下面结合附图具体描述根据本发明实施例的机器人沿栅栏沿边行走的算法。

如图1所示，根据本发明实施例的机器人沿栅栏沿边行走的算法，包括以下步骤：

S1、控制机器人沿栅栏的一侧边沿行走；

S2、采集机器人的激光雷达的点云数据；

S3、过滤点云数据中，机器人与栅栏之间超过预设距离的激光点；

S4、根据过滤后的点云数据，拟合直线，构建直线方程；

S5、根据直线方程，修正机器人的行走误差，以维持机器人定距离且平行于栅栏行走。

换言之，在本发明实施例的机器人沿栅栏沿边行走的算法中，机器人具有激光雷达和具备差速且可控制的底盘。激光雷达可以是2D激光雷达，激光雷达的每个点都有反射强度信息。在发明的算法中，参见图1，首先，可以控制机器人沿栅栏的一侧边沿行走，例如控制机器人沿栅栏的右侧定距离行走。然后，可以采集机器人的激光雷达的点云数据，并且可以过滤点云数据中，机器人与栅栏之间超过预设距离的激光点，将过远的激光点进行过滤，减少干扰，提升行走算法的准确性。接着，可以根据过滤后的点云数据，拟合直线，构建直线方程。最后，可以根据直线方程，修正机器人的行走误差，以维持机器人定距离且平行于栅栏行走。本发明先剔除除栅栏外的激光点，避免其他杂乱数据的影响，同时根据激光点拟合出来的直线方程，计算出来与栅栏边的距离和夹角，控制机器人与栅栏保持定距离、平行栅栏往前走。

当然，对于本领域技术人员来说，机器人的其他结构及其具体工作原理是可以理解并且能够实现的，在本发明中不再详细赘述。

由此，根据本发明实施例的机器人沿栅栏沿边行走的算法，通过采集激光雷达的点云数据，并且能够过滤掉过远的不符合要求的激光点，并构建直线方程，得到机器人行走的距离偏差和角度偏差，以此修正机器人的行走误差，控制机器人保持定距离、平行于栅栏行走。

根据本发明的一个实施例，采集机器人的激光雷达的点云数据包括：截取机器人的点云数据，针对每个激光点，构建二维坐标Pi(x,y)。

也就是说，本发明可以采集一帧2D激光雷达的点云数据,可以保持定距离沿边走的栅栏在右侧，截取在机器人右侧的点云，对于每一个激光点Pi，都用在机器人坐标系下的二维坐标，即Pi(x,y)。机器人与栅栏之间的预设距离为0.3-0.8m，例如，0.5m。假设机器人要与栅栏保持的定距离为D，则超过距离d_outside的点剔除，其中，d_outside＝D+thrd_d，thrd_d为设定的阈值，这里先设定thrd_d为0.5m。

根据本发明的一个实施例，机器人沿栅栏沿边行走的算法还包括：过滤点云数据中不满足预设反射强度的激光点。

换句话说，本发明需要过滤掉不满足栅栏反射强度的点。激光雷达数据中，每个点都有反射强度，可提前人为选定栅栏点后，选定栅栏激光点的反射强度范围。然后，在该反射强度外的点都去除，这样就能尽可能保留栅栏的激光点。如栅栏的反射强度范围是100-120，则该反射强度范围外的都剔除。本发明用距离和反射强度剔除了多余的、除开栅栏外的噪点。直线拟合后可直接得出距离偏差和角度偏差，用于控制机器人保持定距离、平行于栅栏往前走。

在本发明的一些具体实施方式中，根据过滤后的点云数据，拟合直线，构建直线方程的步骤包括：

根据每两个激光点拟合第一直线方程；第一直线方程为y＝k*x+b；其中，x为横坐标，y为纵坐标，k、b均为第一直线方程的系数，且k＝(y_w-y_z)/(x_w-x_z)，b＝y_w-k*x_w；y_w、x_w为Pl点的二维坐标，y_z、x_z为Pm的二维坐标，Pl和Pm为不同的所述激光点坐标；

Pl点从1开始到n/2，则Pm点为第(m-l-1)个点，算出来多个k和b后取均值k_agv、b_agv；

拟合出第二直线方程，第二直线方程为y＝k_agv*x+b_agv。

根据第二直线方程计算出机器人的中心。

在第二直线方程中，k_agv与x轴的夹角关系为theta＝arctan(k_agv)，机器人与栅栏保持定距离时的距离误差为d_error＝D-d_to_line，角度误差为theta，机器人修正误差的角速度为：

Angle_speed＝p1*d_error+p2*theta

其中，D为预设距离，d_to_line为坐标原点到第二直线的距离，p1为距离误差修正的控制系数，p2角度误差修正的控制系数。

也就是说，本发明用栅栏激光点拟合直线，对于栅栏激光点，表示为Pj，j＝1,2,...,n，逆时针排序，每两个激光点拟合出一条直线，当两个激光点距离小于20cm时，不进行拟合。对于每两个拟合的直线方程为：

y＝k*x+b

其中，k＝(y_w-y_z)/(x_w-x_z)，b＝y_w-k*x_w；y_w、x_w为Pl点的二维坐标，y_z、x_z为Pm的二维坐标。Pl点从1开始到n/2，则Pm点为第(m-l-1)个点，算出来多个k和b后取均值k_agv、b_agv，则栅栏激光点拟合出来的直线方程为y＝k_agv*x+b_agv。

栅栏激光点的直线方程中k_agv与x轴的夹角关系为theta＝arctan(k_agv)。根据直线方程可算出来机器人中心，即坐标原点到直线的距离为d_to_line，那么机器人与栅栏保持定距离时的距离误差为d_error＝D-d_to_line，角度误差为theta，则按照此误差，控制差速机器人修正误差的角速度为：

Angle_speed＝p1*d_error+p2*theta

其中，p1为距离误差修正的控制系数，p2角度误差修正的控制系数，机器人线速度固定；这样机器人在往前走的过程中会慢慢修正距离误差和角度误差，到最后保持定距离、平行于栅栏往前走。

本发明先剔除除栅栏外的激光点，避免其他杂乱数据的影响，同时根据激光点拟合出来的直线方程，计算出来与栅栏边的距离和夹角，控制机器人与栅栏保持定距离、平行栅栏往前走。

总而言之，根据本发明实施例的机器人沿栅栏沿边行走的算法，通过采集激光雷达的点云数据，并且能够过滤掉过远的不符合要求的激光点，并构建直线方程，得到机器人行走的距离偏差和角度偏差，以此修正机器人的行走误差，控制机器人保持定距离、平行于栅栏行走。

根据本发明的第二方面，提供一种机器人沿栅栏沿边行走的算法系统，应用于上述实施例中的机器人沿栅栏沿边行走的算法，包括控制模块、采集模块、过滤模块、拟合模块和修正模块。其中，控制模块用于控制机器人沿栅栏的一侧边沿行走。采集模块用于采集机器人的激光雷达的点云数据。过滤模块用于过滤点云数据中，机器人与栅栏之间超过预设距离的激光点。拟合模块用于根据过滤后的点云数据，拟合直线，并构建直线方程。修正模块根据直线方程，修正机器人的行走误差，以维持机器人定距离且平行于栅栏行走。

总而言之，根据本发明实施例的机器人沿栅栏沿边行走的算法系统，通过采集激光雷达的点云数据，并且能够过滤掉过远的不符合要求的激光点，并构建直线方程，得到机器人行走的距离偏差和角度偏差，以此修正机器人的行走误差，控制机器人保持定距离、平行于栅栏行走。

根据本发明的第三方面，还提供一种电子设备，包括：处理器201和存储器202，在存储器202中存储有计算机程序指令，其中，在计算机程序指令被处理器201运行时，使得处理器201执行上述实施例中的机器人沿栅栏沿边行走的算法的步骤。

进一步地，如图2所示，电子设备还包括网络接口203、输入设备204、硬盘205、和显示设备206。

上述各个接口和设备之间可以通过总线架构互连。总线架构可以包括任意数量的互联的总线和桥。具体由处理器201代表的一个或者多个中央处理器201(CPU)，以及由存储器202代表的一个或者多个存储器202的各种电路连接在一起。总线架构还可以将诸如外围设备、稳压器和功率管理电路等之类的各种其它电路连接在一起。可以理解，总线架构用于实现这些组件之间的连接通信。总线架构除包括数据总线之外，还包括电源总线、控制总线和状态信号总线，这些都是本领域所公知的，因此本文不再对其进行详细描述。

网络接口203，可以连接至网络(如因特网、局域网等)，从网络中获取相关数据，并可以保存在硬盘205中。

输入设备204，可以接收操作人员输入的各种指令，并发送给处理器201以供执行。输入设备204可以包括键盘或者点击设备(例如，鼠标，轨迹球(trackball)、触感板或者触摸屏等。

显示设备206，可以将处理器201执行指令获得的结果进行显示。

存储器202，用于存储操作系统2021运行所必须的程序和数据，以及处理器201计算过程中的中间结果等数据。

可以理解，本发明实施例中的存储器202可以是易失性存储器或非易失性存储器，或可包括易失性和非易失性存储器两者。其中，非易失性存储器可以是只读存储器(ROM)、可编程只读存储器(PROM)、可擦除可编程只读存储器(EPROM)、电可擦除可编程只读存储器(EEPROM)或闪存。易失性存储器可以是随机存取存储器(RAM)，其用作外部高速缓存。本文描述的装置和方法的存储器202旨在包括但不限于这些和任意其它适合类型的存储器202。

在一些实施方式中，存储器202存储了如下的元素，可执行模块或者数据结构，或者他们的子集，或者他们的扩展集：操作系统2021和应用程序2022。

其中，操作系统2021，包含各种系统程序，例如框架层、核心库层、驱动层等，用于实现各种基础业务以及处理基于硬件的任务。应用程序2022，包含各种应用程序2022，例如浏览器(Browser)等，用于实现各种应用业务。实现本发明实施例方法的程序可以包含在应用程序2022中。

上述处理器201，当调用并执行存储器202中所存储的应用程序2022和数据，具体的，可以是应用程序2022中存储的程序或指令时，执行根据上述实施例的机器人沿栅栏沿边行走的算法的步骤。

本发明上述实施例揭示的方法可以应用于处理器201中，或者由处理器201实现。处理器201可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器201中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器201可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件，可以实现或者执行本发明实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器201也可以是任何常规的处理器201等。结合本发明实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器202，处理器201读取存储器202中的信息，结合其硬件完成上述方法的步骤。

可以理解的是，本文描述的这些实施例可以用硬件、软件、固件、中间件、微码或其组合来实现。对于硬件实现，处理单元可以实现在一个或多个专用集成电路(ASIC)、数字信号处理器DSP)、数字信号处理设备(DSPD)、可编程逻辑设备(PLD)、现场可编程门阵列(FPGA)、通用处理器、控制器、微控制器、微处理器、用于执行本申请功能的其它电子单元或其组合中。

对于软件实现，可通过执行本文功能的模块(例如过程、函数等)来实现本文的技术。软件代码可存储在存储器202中并通过处理器201执行。存储器202可以在处理器201中或在处理器201外部实现。

具体地，处理器201还用于读取计算机程序，执行如下步骤:对桩充电方法预测并输出用户提问的问题答案。

本发明第四方面实施例，还提供一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器201运行时，使得处理器201执行上述实施例的机器人沿栅栏沿边行走的算法的步骤。

在本申请所提供的几个实施例中，应该理解到，所揭露方法和装置，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理包括，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。

上述以软件功能单元的形式实现的集成的单元，可以存储在一个计算机可读取存储介质中。上述软件功能单元存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例收发方法的部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，简称ROM)、随机存取存储器(Random Access Memory，简称RAM)、磁碟或者光盘等各种可以存储程序代码的介质。

虽然已经通过例子对本发明的一些特定实施例进行了详细说明，但是本领域的技术人员应该理解，以上例子仅是为了进行说明，而不是为了限制本发明的范围。本领域的技术人员应该理解，可在不脱离本发明的范围和精神的情况下，对以上实施例进行修改。本发明的范围由所附权利要求来限定。

Claims (10)

1.一种机器人沿栅栏沿边行走的算法，其特征在于，包括以下步骤：

控制机器人沿栅栏的一侧边沿行走；

采集所述机器人的激光雷达的点云数据；

过滤所述点云数据中，所述机器人与所述栅栏之间超过预设距离的激光点；

根据过滤后的所述点云数据，拟合直线，构建直线方程；

根据所述直线方程，修正所述机器人的行走误差，以维持所述机器人定距离且平行于所述栅栏行走。

2.根据权利要求1所述的机器人沿栅栏沿边行走的算法，其特征在于，采集所述机器人的激光雷达的点云数据包括：截取所述机器人的点云数据，针对每个所述激光点，构建二维坐标Pi(x,y)。

3.根据权利要求1所述的机器人沿栅栏沿边行走的算法，其特征在于，所述机器人与所述栅栏之间的所述预设距离为0.3m-0.8m。

4.根据权利要求1所述的机器人沿栅栏沿边行走的算法，其特征在于，还包括：过滤所述点云数据中不满足预设反射强度的所述激光点。

5.根据权利要求1所述的机器人沿栅栏沿边行走的算法，其特征在于，根据过滤后的所述点云数据，拟合直线，构建直线方程的步骤包括：

根据每两个所述激光点拟合第一直线方程；其中，所述第一直线方程为y＝k*x+b；其中，x为横坐标，y为纵坐标，k、b均为所述第一直线方程的系数，且k＝(y_w-y_z)/(x_w-x_z)，b＝y_w-k*x_w；y_w、x_w为Pl点的二维坐标，y_z、x_z为Pm的二维坐标，Pl和Pm为不同的所述激光点坐标；

Pl点为从1开始到n/2，则Pm点为第(m-l-1)个点，计算多个k和b后取均值k_agv和b_agv；

拟合出第二直线方程，所述第二直线方程为y＝k_agv*x+b_agv。

6.根据权利要求5所述的机器人沿栅栏沿边行走的算法，其特征在于，根据所述第二直线方程计算出所述机器人的中心。

7.根据权利要求5所述的机器人沿栅栏沿边行走的算法，其特征在于，在所述第二直线方程中，k_agv与x轴的夹角关系为theta＝arctan(k_agv)，机器人与所述栅栏保持定距离时的距离误差为d_error＝D-d_to_line，角度误差为theta，机器人修正误差的角速度为：

Angle_speed＝p1*d_error+p2*theta

其中，D为所述预设距离，d_to_line为坐标原点到所述第二直线的距离，p1为距离误差修正的控制系数，p2为角度误差修正的控制系数。

8.一种机器人沿栅栏沿边行走的算法系统，应用于权利要求1-7中任一项所述的机器人沿栅栏沿边行走的算法，其特征在于，包括：

控制模块，所述控制模块用于控制机器人沿栅栏的一侧边沿行走；

采集模块，所述采集模块用于采集所述机器人的激光雷达的点云数据；

过滤模块，所述过滤模块用于过滤所述点云数据中所述机器人与所述栅栏之间超过预设距离的激光点；

拟合模块，所述拟合模块用于根据过滤后的所述点云数据，拟合直线，并构建直线方程；

修正模块，所述修正模块根据所述直线方程，修正所述机器人的行走误差，以维持所述机器人定距离且平行于所述栅栏行走。

9.一种电子设备，其特征在于，包括：处理器和存储器，在所述存储器中存储有计算机程序指令，其中，在所述计算机程序指令被所述处理器运行时，使得所述处理器执行权利要求1-7中任一项所述的机器人沿栅栏沿边行走的算法的步骤。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器运行时，使得所述处理器执行权利要求1-7中任一项所述的机器人沿栅栏沿边行走的算法的步骤。

Images