CN110286672A - 机器人及其导航控制方法、导航控制装置以及存储介质 - Google Patents

Abstract

本申请公开了机器人及其导航控制方法、导航控制装置以及存储介质，其中，方法包括以下步骤：获取机器人的从起始坐标到目标坐标的目标路径轨迹以及获取机器人的实时坐标和实时航向角；根据实时坐标和实时航向角计算机器人相对于目标路径轨迹的位置偏差和角度偏差；根据位置偏差和角度偏差确定机器人的目标线速度和目标角速度；根据目标线速度和目标角速度控制机器人运动。通过该方法，本申请能够提高机器人的导航控制精度。

Description

机器人及其导航控制方法、导航控制装置以及存储介质

技术领域

本申请涉及机器人技术领域，特别是涉及一种机器人及其导航控制方法、导航控制装置以及存储介质。

背景技术

导航与定位是移动机器人研究的两个重要问题。根据环境信息的完整程度、导航指示信号类型、导航地域等因素的不同，移动机器人的导航方式可分为:基于环境信息的地图模型匹配导航；基于路标导航；基于视觉导航；基于传感器导航等。环境地图模型匹配导航是在机器人内部存储关于环境的完整信息，并在预先规划出的一条全局路线的基础上，采用路径跟踪和避障技术，实现机器人导航。

其中，路径跟踪，是在平面坐标系下设定一条理想的几何路径，然后要求机器人从某一处出发，按照某种控制规律到达该路径上，并实现其跟踪运动。

要实现机器人的自主导航，使得机器人按照规划好的路径进行跟踪运动，则需要不断提高机器人的导航控制精度。

发明内容

本申请主要解决的技术问题是如何提高机器人的导航控制精度。为解决上述技术问题，本申请采用的一个技术方案是：提供一种机器人的导航控制方法，包括以下步骤：获取机器人的从起始坐标到目标坐标的目标路径轨迹以及获取机器人的实时坐标和实时航向角；根据实时坐标和实时航向角计算机器人相对于目标路径轨迹的位置偏差和角度偏差；根据位置偏差和角度偏差确定机器人的目标线速度和目标角速度；根据目标线速度和目标角速度控制机器人运动。

为解决上述技术问题，本申请采用的另一个技术方案是：提供一种存储介质，包括：存储介质存储有软件程序，软件程序能够被执行以实现上述方法的步骤。

为解决上述技术问题，本申请采用的另一个技术方案是：提供一种机器人的导航控制装置，包括：存储器和处理器，处理器耦接存储器，存储器中存储有软件程序，处理器执行软件程序时实现上述方法的步骤。

为解决上述技术问题，本申请采用的另一个技术方案是：提供一种机器人，包括上述的导航控制装置。

本申请的有益效果是：区别于现有技术的情况，本申请通过获取机器人的从起始坐标到目标坐标的目标路径轨迹以及获取机器人的实时坐标和实时航向角，根据实时坐标和实时航向角计算机器人相对于目标路径轨迹的位置偏差和角度偏差，根据位置偏差和角度偏差确定机器人的目标线速度和目标角速度，根据目标线速度和目标角速度控制机器人运动，可以提高机器人的导航控制精度。

附图说明

为了更清楚地说明本申请实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本申请的导航控制方法一实施例的流程示意图；

图2是本申请的导航控制方法一实施例的导航原理示意图；

图3是图1中本申请的导航控制方法一实施例中的S140的具体流程示意图；

图4是图1中本申请的导航控制方法一实施例中的S160的一具体流程示意图；

图5是图1中本申请的导航控制方法一实施例中的S160的另一具体流程示意图；

图6是本申请的导航控制方法另一实施例的流程示意图；

图7是本申请存储介质实施例的模块示意图；

图8是本申请导航控制装置实施例的模块示意图；

图9是本申请机器人实施例的模块示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请的一部分实施例，而不是全部的实施例。本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

请参阅图1，图1是本申请的导航控制方法一实施例的流程示意图。

在本实施例中，机器人的导航控制方法包括以下步骤：

S120：获取机器人的从起始坐标到目标坐标的目标路径轨迹以及获取机器人的实时坐标和实时航向角。

在导航开始时，首先要获取机器人的目标路径轨迹，以供后续机器人对该目标路径轨迹进行跟踪。

规划好的全局目标路径可以是直线、折线、或曲线。为了方便根据路径对机器人进行导航，通常会对全局目标路径进行数据分段，以得到若干直线段，当全局目标路径为曲线时，还将同时进行数据分段和直线拟合以得到若干直线段。因此，本申请中，所提及的目标路径轨迹指的是全局目标路径进行数据分段后所分段路径，该分段路径为直线段。

机器人的实时坐标和实时航向角可通过机器人自身所搭载的自定位装置进行实时检测，例如通过GPS定位或者RTK定位。

S140：根据实时坐标和实时航向角计算机器人相对于目标路径轨迹的位置偏差和角度偏差。

S160：根据位置偏差和角度偏差确定机器人的目标线速度和目标角速度。

机器人对位置偏差和角度偏差进行偏差纠正计算后，得到目标线速度和目标角速度。

S180：根据目标线速度和目标角速度控制机器人运动。

得到目标线速度和目标角速度后，可根据该结果控制机器人的运动机构以该目标线速度和目标角速度进行运动，进而纠正机器人的位置偏差和角度偏差，使机器人回到目标路径轨迹上，从而可达到机器人自主导航的效果。

本实施例中，导航控制方法的执行主体可以是机器人自身所设置的处理器，也可以是远程操控服务器。

本实施例所提供的机器人的导航控制方法，通过获取机器人的从起始坐标到目标坐标的目标路径轨迹以及获取机器人的实时坐标和实时航向角，根据实时坐标和实时航向角计算机器人相对于目标路径轨迹的位置偏差和角度偏差，根据位置偏差和角度偏差确定机器人的目标线速度和目标角速度，根据目标线速度和目标角速度控制机器人运动，可以提高机器人的导航控制精度。

请参阅图2，图2是本申请的导航控制方法一实施例的导航原理示意图。

在本实施例中，定义：起始点为S，起始坐标为(x1，y1)，目标点为T，目标坐标为(x2，y2)，实时点(机器人的实时位置)为R，实时坐标为(x0，y0)，实时航向角为θ，位置偏差为d，角度偏差记为δ，目标线速度为v，目标角速度为ω。

同时，以图2中所建立的直角坐标系为例，定义机器人沿目标路径轨迹相对起始坐标(x1，y1)向目标坐标(x2，y2)运动的方向为向前运动，此时v>0，定义机器人进行逆时针转向时，ω>0。航向角是以x轴正方向为0°，逆时针方向为正。则：

当V＝0，ω≠0时，机器人的运动状态为以R点为中心原地打转。具体的，当ω>0时，机器人以R点为中心逆时针打转，当ω<0时，机器人以R点为中心顺时针打转。

当V≠0，ω＝0时，机器人的运动状态为沿V的直线方向运动。具体的，当V>0时，机器人沿V的正方向运动，当V<0时，机器人沿V的负方向运动。

当V≠0，ω≠0时，机器人的运动状态为既有沿V的直线方向运动，也有按ω方向的转向运动。具体的：

当V>0，ω>0时，机器人沿V的正方向运动，同时有逆时针方向的转向；

当V>0，ω<0时，机器人沿V的正方向运动，同时有顺时针方向的转向；

当V<0，ω<0时，机器人沿V的负方向运动，同时有顺时针方向的转向；

当V<0，ω>0时，机器人沿V的负方向运动，同时有逆时针方向的转向。

当V＝0，ω＝0时，机器人的运动状态为静止。

综上，当V与ω分别取不同的值时，机器人的运动状态不同。

请结合参阅图2和图3，图3是图1中本申请的导航控制方法一实施例中的S140的具体流程示意图。

可选的，步骤S140包括：

S142：根据实时坐标、起始坐标以及目标坐标，计算机器人与目标路径轨迹之间的距离并作为位置偏差。

可选的，S142中位置偏差d的计算方法包括：

确定以实时坐标(x0，y0)、起始坐标(x1，y1)以及目标坐标(x2，y2)为顶点的三角形的三条边长的长度值a、b、c。其中，实时点R到起始点S之间的边长为a，实时点R到目标点T之间的边长为b，起始点S到目标点T之间的边长为c，即c为目标路径轨迹ST的长度值。

通过以下联立方程计算得到实时坐标(x0，y0)与目标路径轨迹ST之间的垂直距离d并作为位置偏差：

其中，p为三角形的周长，S为三角形的面积。

定义实时点R到目标路径轨迹ST的垂线为RM，垂足为M，实时点R到目标路径轨迹ST的垂直距离d，即三角形的高RM。

S144：根据实时坐标和目标坐标计算机器人相对于目标坐标的目标航向角，将实时航向角与目标航向角的差值作为角度偏差。

请继续参阅图2，以x轴正方向为0度，连接实时点R和目标点T，线段RT与x轴正方向的夹角为机器人在实时点R的目标航向角β，实时航向角为θ，角度偏差记为δ，即：δ＝θ-β。

请参阅图4，图4是图1中本申请的导航控制方法一实施例中的S160的第一具体流程示意图。

可选的，步骤S160中，根据位置偏差和角度偏差确定机器人的目标角速度，具体包括：

S162：判断角度偏差的绝对值是否大于第一阈值。

可选的，第一阈值为8～15°。第一阈值可以是8°、10°、12°或者15°。

若判断结果为是，即当角度偏差δ的绝对值是否大于第一阈值时，进行步骤S164，若判断结果为否，即当角度偏差δ的绝对值是否等于或小于第一阈值时，进行步骤S166。

S164：根据角度偏差确定机器人的目标角速度。

当角度偏差δ的绝对值大于第一阈值时，判断此时机器人的实时航向角θ偏离目标航向角β较大，此时，使用角度偏差δ确定机器人的目标角速度ω以对机器人进行导航控制，可以提高导航控制的精确度。

S166：根据位置偏差确定机器人的目标角速度。

当角度偏差δ的绝对值是否等于或小于第一阈值时，判断此时机器人的实时航向角θ偏离目标航向角β较小，此时，使用位置偏差d确定机器人的目标角速度ω以对机器人进行导航控制，可以进一步提高导航控制的精确度。

可选的，将预设的第一目标值与角度偏差的差值作为第一误差值，以第一误差值为输入值执行角度PID算法得到目标角速度。

将预设的第二目标值与位置偏差的差值作为第二误差值，以第二误差值为输入值执行位置PID算法得到目标角速度。

本实施例中，导航控制方法中可包括两套PID算法，即角度PID算法和位置PID算法。

具体的，可使机器人包括PID控制器，PID控制器中设置有两套PID控制电路：角度PID控制电路用于执行角度PID算法，位置PID控制电路用于执行位置PID算法。

本实施例中，可以根据不同的条件(角度偏差δ是否大于阈值)，选择不同的PID算法，即使用位置偏差d或角度偏差δ分别得到不同的误差值作为输入值，并输入对应的PID算法中，分别计算得到目标角速度ω。使得只能进行单输入单输出的PID算法可以实现多输入多输出的计算，从而使计算模型简化，便于方法实现，提高计算效率，最终提高了本方法所计算得到的目标角速度ω的可靠度。

可选的，第一目标值和第二目标值均为0。

当第一目标值和第二目标值均为0时，第一误差值e(t1)＝0-d，第二误差值e(t2)＝0-δ。此时，导航精度最高。

可以理解的是，第一目标值和第二目标值也可以是大于0的数值，通过设定不同大小的第一目标值和第二目标值可以调节机器人的导航控制精度。

其中，角度PID算法和位置PID算法可以用下述方程式表示：

角度PID算法：

位置PID算法：

在上式中，Kp为比例系数，Ti为积分时间常数，Td为微分时间常数，在不同的算法中，Kp、Ti、Td可能不同。u(t1)和u(t2)为输出的目标角度ω。

请参阅图5，图5是图1中本申请的导航控制方法一实施例中的S160的另一具体流程示意图。

S162：判断角度偏差的绝对值是否大于第一阈值。

可选的，第一阈值为8～15°。第一阈值可以是8°、10°、12°或者15°。

若判断结果为是，即当角度偏差δ的绝对值大于第一阈值时，进行步骤S165；判断结果为否，即当角度偏差δ的绝对值小于或等于第一阈值时，进行步骤S167。

S165：控制机器人的目标线速度为第一预设值。

S167：控制机器人的目标线速度为第二预设值。

其中，第一预设值大于第二预设值。

通过判断角度偏差δ的绝对值是否大于第一阈值，控制机器人的目标线速度v为第一预设值或第二预设值，且第一预设值大于第二预设值，使得可以在当角度偏差δ较大时，控制机器人在以较低的目标线速度v进行运动的情况下，调整机器人的角速度至目标值ω，从而提高调整的精度，进而提高导航的精度。而当角度偏差δ较小时，控制机器人在以较高的目标线速度v进行运动的情况下，调整机器人的角速度至目标值ω，从而提高对机器人的运动的调整效率，进而提高导航的效率。

可选的，第一预设值为0，第二预设值大于0。

通过将第一预设值设置为0，使得当角度偏差较大时，可以在线速度为0的条件下，调整机器人的角速度至目标值ω，从而进一步提高调整的精度。

其中，第二预设值可为设定的机器人的正常运行线速度，即机器人一般以第二预设值进行运动，当角度偏差δ大于第一阈值时，则使机器人的目标线速度v调整为第一预设值。从而当角度偏差δ较小时，可以在机器人不改变正常运行线速度情况下，调整机器人的角速度至目标值ω，一方面可提高对机器人的运动的调整效率，另一方面还可以降低机器人的线速度的调整频率，以减少运算量，降低能耗。

可选的，S180中，可根据目标线速度和目标角速度控制机器人运动，可具体包括以下两种情况：

第一种：根据目标线速度和目标角速度，同时控制机器人调整至目标线速度和目标角速度。

在这种情况下，通过同时控制机器人调整至目标线速度v和目标角速度ω，可以提高对机器人的运动控制的效率。

或者第二种：先根据目标线速度控制机器人调整至目标线速度，再根据目标角速度控制机器人调整至目标角速度。

在这种情况下，先控制机器人调整至目标线速度v，例如，当角度偏差δ较大时，可以在将机器人调整至线速度为0的条件下，使机器人先停下来，再控制机器人调整至目标角速度ω，也就是说，使机器人以R点为中心进行旋转，通过这种方式，可以进一步提高角速度调整的精度。而当角度偏差较小时，机器人不改变正常运行线速度情况下，可直接将调整机器人的角速度至目标值ω，此时，也不会影响角速度调整的效率。

请参阅图6，图6是本申请的导航控制方法另一实施例的流程示意图。

S120：获取机器人的从起始坐标到目标坐标的目标路径轨迹以及获取机器人的实时坐标和实时航向角。

S140：根据实时坐标和实时航向角计算机器人相对于目标路径轨迹的位置偏差和角度偏差。

S160：根据位置偏差和角度偏差确定机器人的目标线速度和目标角速度。

S170：判断位置偏差的绝对值是否小于第二阈值且角度偏差的绝对值是否小于第三阈值。

可选的，第二阈值为0～0.2m，第三阈值为0～3°。

若判断结果为是，则返回执行步骤S120，若判断结果为否，即位置偏差d的绝对值小于第二阈值或者角度偏差δ的绝对值小于第三阈值，进行步骤S180。

S180：根据目标线速度和目标角速度控制机器人运动。

本实施例，通过判断位置偏差d的绝对值是否小于第二阈值且角度偏差δ的绝对值是否小于第三阈值，可以在位置偏差和角度偏差均较小的情况下，不对机器人的当前线速度v和当前角速度ω进行调整，从而在保持较高的导航控制精度的同时，适当较少对机器人的运动的调整频率，提高对机器人的运动控制效率，降低系统损耗。

本实施例可以与本申请导航控制方法的上述实施例相结合。

需要说明的是，在不冲突的情况下，本申请中的各个实施例，以及各实施例中的步骤、特征可以相互结合。

请参阅图7，图7是本申请存储介质实施例的模块示意图。

本实施例中，具有存储功能的存储介质100，存储有软件程序110，该软件程序110能够被执行以实现上述任一实施例步骤。具体请参照本申请新导航控制方法的第一实施例至第二实施例，在此不再赘述。

具有存储功能的存储介质100可以是：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储软件程序的介质中的一种。

请参阅图8，图8是本申请导航控制装置实施例的模块示意图。

本实施例中，导航控制装置200包括：存储器210和处理器220。

存储器210存储有软件程序。处理器220耦接到存储器210，执行软件程序以执行上述任一实施例所提供的导航控制方法。

在本实施例中，处理器220还可以称为CPU(Central Processing Unit，中央处理单元)。处理器220可能是一种集成电路芯片，具有信号的处理能力。处理器220还可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑、分立硬件组件。通用处理器可以是微处理器或者该处理器220也可以是任何常规的处理器等。

本实施例中关于电子设备处理器220的更多执行过程等可以参照上述本申请导航控制方法的第一实施例至第二实施例，在此不再赘述。

请参阅图9，图9是本申请机器人实施例的模块示意图。

本实施例中，机器人300包括上述实施例中的导航控制装置310。

本实施例中，机器人300是指能够实现包括上述实施例中的导航控制装置310，能够执行上述任一实施例中的导航控制方法的机器装置。

具体的，机器人300还可以包括运动机构(图未示)。

一般来说，机器人300可以是工业机器人或特种机器人。其中，特种机器人可以分为：服务机器人、水下机器人、娱乐机器人、军用机器人、农业机器人、机器人化机器等。

在一具体应用场景中，本实施例所提供的机器人300，可以是服务机器人，具体可以是一种清洁机器人，其可应用于光伏电站中，用于对太阳能光伏组件表面进行清洁、检测等。

当机器人300为清洁机器人时，机器人300还可包括清扫机构(图未示)。

本实施例所提供的机器人300能够通过导航控制装置310实现上述的导航控制方法，完成自主导航，自动行走，无需人工操控。

以上仅为本申请的实施方式，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。

Claims (15)

1.一种机器人的导航控制方法，其特征在于，包括以下步骤：

获取所述机器人的从起始坐标到目标坐标的目标路径轨迹以及获取所述机器人的实时坐标和实时航向角；

根据所述实时坐标和所述实时航向角计算所述机器人相对于所述目标路径轨迹的位置偏差和角度偏差；

根据所述位置偏差和所述角度偏差确定所述机器人的目标线速度和目标角速度；

根据所述目标线速度和所述目标角速度控制所述机器人运动。

2.根据权利要求1所述的导航控制方法，其特征在于，所述根据所述实时坐标和所述实时航向角确定所述机器人相对于所述目标路径轨迹的位置偏差和角度偏差包括：

根据所述实时坐标、所述起始坐标以及所述目标坐标，计算所述机器人与所述目标路径轨迹之间的距离并作为位置偏差；

根据所述实时坐标和所述目标坐标计算所述机器人相对于所述目标坐标的目标航向角，将所述实时航向角与所述目标航向角的差值作为角度偏差。

3.根据权利要求2所述的导航控制方法，其特征在于，所述根据所述实时坐标、所述起始坐标以及所述目标坐标，计算所述机器人与所述目标路径轨迹之间的距离并作为位置偏差包括：

确定以所述实时坐标、所述起始坐标以及所述目标坐标为顶点的三角形的三条边长的长度值a、b、c；

通过以下联立方程计算得到所述实时坐标与所述目标路径轨迹之间的垂直距离d并作为位置偏差：

其中，p为所述三角形的周长，c为所述目标路径轨迹的长度值。

4.根据权利要求1所述导航控制方法，其特征在于，所述根据所述位置偏差和所述角度偏差确定所述机器人的目标线速度和目标角速度包括：

当所述角度偏差的绝对值大于第一阈值时，根据所述角度偏差确定所述机器人的目标角速度；

当所述角度偏差的绝对值小于或等于所述第一阈值时，根据所述位置偏差确定所述机器人的目标角速度。

5.根据权利要求4所述的导航控制方法，其特征在于，所述根据所述角度偏差确定所述机器人的目标角速度包括：

将预设的第一目标值与所述角度偏差的差值作为第一误差值，以所述第一误差值为输入值执行角度PID算法得到目标角速度；

所述根据所述位置偏差确定所述机器人的目标角速度包括：

将预设的第二目标值与所述位置偏差的差值作为第二误差值，以所述第二误差值为输入值执行位置PID算法得到目标角速度。

6.根据权利要求5所述的导航控制方法，其特征在于，所述第一目标值和所述第二目标值均为0。

7.根据权利要求4所述的导航控制方法，其特征在于，所述第一阈值为8～15°。

8.根据权利要求1所述导航控制方法，其特征在于，所述根据所述位置偏差和所述角度偏差确定所述机器人的目标线速度和目标角速度包括：

当所述角度偏差的绝对值大于所述第一阈值时，控制所述机器人的目标线速度为第一预设值；

当所述角度偏差的绝对值小于或等于所述第一阈值时，控制所述机器人的目标线速度为第二预设值；

其中，第一预设值大于第二预设值。

9.根据权利要求8所述的导航控制方法，其特征在于，所述第一预设值为0，所述第二预设值大于0。

10.根据权利要求1所述的导航控制方法，其特征在于，所述根据所述目标线速度和所述目标角速度控制所述机器人运动包括：

根据所述目标线速度和所述目标角速度，同时控制所述机器人调整至所述目标线速度和所述目标角速度；或者

先根据所述目标线速度控制所述机器人调整至所述目标线速度，再根据所述目标角速度控制所述机器人调整至所述目标角速度。

11.根据权利要求1所述的导航控制方法，其特征在于，在所述根据所述位置偏差和所述角度偏差确定所述机器人的目标线速度和目标角速度的步骤之前，所述方法还包括：

判断所述位置偏差的绝对值是否小于第二阈值且所述角度偏差的绝对值是否小于第三阈值；

若是，则返回继续获取所述机器人的实时坐标和实时航向角。

12.根据权利要求11所述的导航控制方法，其特征在于，所述第二阈值为0～0.2m，所述第三阈值为0～3°。

13.一种存储介质，其特征在于，所述存储介质存储有软件程序，所述软件程序能够被执行以实现权利要求1-12中任一项所述方法的步骤。

14.一种机器人的导航控制装置，其特征在于，包括：存储器和处理器，所述处理器耦接所述存储器，所述存储器中存储有软件程序，所述处理器执行所述软件程序时实现权利要求1-13中任一项所述方法的步骤。

15.一种机器人，其特征在于，包括如权利要求14所述的导航控制装置。