CN111791241B - 机器人轨迹误差校正方法及装置、消防机器人 - Google Patents

Abstract

本发明提供了一种机器人轨迹误差校正方法及装置、消防机器人，其中，所述方法包括在机器人自动行驶的过程中，获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与所述预设道路线的偏离角度；在所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值的情况下，对所述机器人的运行轨迹进行校正；或者，在所述机器人的第一行驶轨迹与预设道路线的偏离角度大于第二标定阈值的情况下，对所述机器人的运行角度进行校正。通过本发明，能够实现自动校正消防机器人行驶轨迹，避免了消防机器人与周围环境发生碰撞的效果。

Description

机器人轨迹误差校正方法及装置、消防机器人

技术领域

本发明涉及消防特种装备领域，具体而言，涉及一种机器人轨迹误差校正方法及装置、消防机器人。

背景技术

消防机器人是一种代替消防官兵进入或靠近火场等危险环境，执行灭火、救援、侦察等任务的特种机器人。消防机器人的自动行驶功能，无论是在手动非视距内盲操场景，还是在全自动操控下的巡航场景，都有着广泛的应用场景。其中，手动非视距内盲操场景是指，在操作员操控消防机器人进入消防官兵无法进入的危险区域时，需要操作员观察手持端遥控器返回的消防机器人现场画面进行判断及操控。其中，全自动操控下的巡航场景是指，消防机器人在自主设定程序的控制下，在一定范围内依靠环境信息自主性行驶，并有效避免与地形障碍物的碰撞。

在上述操作场景中，当操作员需要操控消防机器人远距离直线行驶时，往往会因为缺少对消防机器人周围环境的判断参照，而出现较大的行驶偏差，影响救援效率，增加操作员操作压力。

针对相关技术中，在消防机器人脱离操作员视线范围进行自动行驶或声控行驶时，仅通过遥控器显示画面判断，难以及时分辨消防机器人行驶轨迹直线度的问题，目前尚未存在有效的解决方案。

发明内容

本发明实施例提供了一种机器人轨迹误差校正方法及装置、消防机器人，以至少解决相关技术中在消防机器人脱离操作员视线范围进行自动行驶或声控行驶时，仅通过遥控器显示画面判断，难以及时分辨消防机器人行驶轨迹直线度的问题。

根据本发明的一个实施例，提供了一种机器人轨迹误差校正方法，包括：在机器人自动行驶的过程中，获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与所述预设道路线的偏离角度；在所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值的情况下，对所述机器人的运行轨迹进行校正；或者，在所述机器人的第一行驶轨迹与预设道路线的偏离角度大于第二标定阈值的情况下，对所述机器人的运行角度进行校正。

根据本发明的另一个实施例，提供了一种机器人轨迹误差校正装置，包括：获取模块，在机器人自动行驶的过程中，获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与所述预设道路线的偏离角度；第一校正模块，用于在所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值的情况下，对所述机器人的运行轨迹进行校正；第二校正模块，用于在所述机器人的第一行驶轨迹与预设道路线的偏离角度大于第二标定阈值的情况下，对所述机器人的运行角度进行校正。

根据本发明的再一个实施例，还提供了一种消防机器人，包括：轨迹误差校正***，所述轨迹误差校正***被设置为运行时执行所述的机器人轨迹误差校正方法。

根据本发明的又一个实施例，还提供了一种存储介质，所述存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行上述任一项方法实施例中的步骤。

根据本发明的又一个实施例，还提供了一种电子装置，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行上述任一项方法实施例中的步骤。

通过本发明，通过在机器人自动行驶的过程中，获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与所述预设道路线的偏离角度，从而根据所述偏离距离或者偏离角度分别对机器人进行轨迹校正或者角度校正。因此，可以解决在消防机器人脱离操作员视线范围进行自动行驶或声控行驶时，仅通过遥控器显示画面判断，难以及时分辨消防机器人行驶轨迹直线度的问题，达到能够实现自动校正消防机器人行驶轨迹，避免了消防机器人与周围环境发生碰撞的效果。

附图说明

此处所说明的附图用来提供对本发明的进一步理解，构成本申请的一部分，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1(a)、图1(b)所示是本申请实施例中的方法所适用的机器人的结构示意图；

图2是根据本发明实施例的机器人轨迹误差校正方法的结构框图；

图3是根据本发明实施例的机器人轨迹误差校正装置的结构框图；

图4是根据本发明实施例中的轨迹误差及角度误差示意图；

图5是根据本发明实施例中的轨迹校正所使用的机器人***结构示意图；

图6是根据本发明可选实施例的轨迹误差校正方法工作流程图；

图7是根据本发明可选实施例中轨迹误差及角度误差校正曲线图；

图8是根据本发明可选实施例中轨迹误差及角度误差校正曲线图；

图9是根据本发明可选实施例中轨迹误差及角度误差校正曲线图。

具体实施方式

下文中将参考附图并结合实施例来详细说明本发明。需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。

实施例1

如图1(a)、图1(b)所示是本申请实施例中的方法所适用的机器人的结构示意图。其中，包括：消防机器人车体1、双目摄像机2、主板3、钣金外壳4、履带5、驱动轮6、电机驱动器7、编码器8、电机9和其他电器元件所实现的。其中，所述双目摄像机2安装在所述消防机器人车体 1中部的固定底座上方，用于观测和采集消防机器人车身前画面。所述主板3安装在消防机器人车体1内部的电器仓内，用于处理和分析图像数据。所述钣金外壳4安装在消防机器人车体外侧，可用于保护车体以及起装饰作用，同时在本申请的实施例中还包括钣金外壳4上的参考标记点A(43)、 B(44)、C(42)、D(41)。所述履带5共有左右两条，安装在消防机器人车体的左右悬架组件上，用于与地面接触获得摩擦力。所述驱动轮6共有2个，安装在电机减速机输出轴上，并与履带连接，用于将电机的输出转矩及转速传递给履带。所述电机驱动器7分别有左右2个，安装在电器仓内的支架上，用于将主板的控制信号转化为脉冲信号输出给电机。所述编码器8分别有左右2个，安装在电机输出轴尾端，用于采集电机角速度。所述电机9有左右两个，安装在消防机器人电器仓内，用于输出转矩及转速。

在本实施例中提供了一种运行于上述消防机器人的机器人轨迹误差校正方法，图2是根据本发明实施例的机器人轨迹误差校正方法的流程图，如图2所示，该流程包括如下步骤：

步骤S202，在机器人自动行驶的过程中，获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与所述预设道路线的偏离角度；

步骤S204，在所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值的情况下，对所述机器人的运行轨迹进行校正；或者，在所述机器人的第一行驶轨迹与预设道路线的偏离角度大于第二标定阈值的情况下，对所述机器人的运行角度进行校正。

具体地，通过在所述机器人自动行驶的过程中，通过识别摄像头中获取的图像得到所述机器人的第一位置与预设道路线的偏离距离。其中，预设道路线是指道路划线或道路边界。所述第一位置是指一个采样周期内机器人所在的位置。同时，通过识别摄像头中获取的图像得到所述机器人的的第一行驶轨迹与所述预设道路线的偏离角度。其中，第一行驶轨迹是指在一个采样周期内机器人所行驶的轨迹。

如果判断在所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值时，则对所述机器人的运行轨迹进行校正，或者在判断所述机器人的第一行驶轨迹与预设道路线的偏离角度大于第二标定阈值，则对所述机器人的运行角度进行校正。其中，所述第一标定阈值、第二标定阈值根据不同场景进行设定，在本申请的实施例中并不进行具体限定。

通过上述步骤，通过在在机器人自动行驶的过程中，获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与所述预设道路线的偏离角度，从而根据所述偏离距离或者偏离角度分别对机器人进行轨迹校正或者角度校正。因此，可以解决在消防机器人脱离操作员视线范围进行自动行驶或声控行驶时，仅通过遥控器显示画面判断，难以及时分辨消防机器人行驶轨迹直线度的问题，达到能够实现自动校正消防机器人行驶轨迹，避免了消防机器人与周围环境发生碰撞的效果。

可选地，步骤S204中还可以包括在所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值的情况下，对所述机器人的运行轨迹进行校正同时在所述机器人的第一行驶轨迹与预设道路线的偏离角度大于第二标定阈值的情况下，对所述机器人的运行角度进行校正的情况。

可选地，在获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与所述预设道路线的偏离角度之后，所述方法还包括以下之一：在所述机器人的第一位置与预设道路线的偏离距离不小于第一标定阈值的情况下，控制所述机器人按照所述偏离距离继续自动行驶；在所述机器人的第一行驶轨迹与预设道路线的偏离角度不大于第二标定阈值的情况下，控制所述机器人按照与所述预设道路线的所述偏离角度继续自动行驶。即如果所述机器人的第一位置与预设道路线的偏离距离不小于第一标定阈值时，认为不用进行校正，则控制所述机器人按照所述偏离距离继续自动行驶。如果所述机器人的第一行驶轨迹与预设道路线的偏离角度不大于第二标定阈值时，同样认为不用进行校正，则控制所述机器人按照与所述预设道路线的所述偏离角度继续自动行驶。

优选地，在所述机器人的所述第一位置与所述预设道路线的所述偏离距离不小于所述第一标定阈值且所述机器人的所述第一行驶轨迹与所述预设道路线的所述偏离角度大于所述第二标定阈值的情况下，对所述机器人的运行角度进行校正；在所述机器人的第一行驶轨迹与预设道路线的偏离角度不大于第二标定阈值且所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值的情况下，对所述机器人的运行轨迹进行校正。即如果所述机器人的所述第一位置与所述预设道路线的所述偏离距离不小于所述第一标定阈值且所述机器人的所述第一行驶轨迹与所述预设道路线的所述偏离角度大于所述第二标定阈值时，需要对所述机器人的运行角度进行校正。如果所述机器人的第一行驶轨迹与预设道路线的偏离角度不大于第二标定阈值且所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值时，需要对所述机器人的运行轨迹进行校正。

优选地，在所述机器人的所述第一位置与所述预设道路线的所述偏离距离小于所述第一标定阈值的情况下，对所述机器人的运行轨迹进行校正之后，所述方法还包括：在所述机器人的第二位置与预设道路线的偏离距离小于所述第二标定阈值的情况下，继续对所述机器人的运行轨迹进行校正直至所述机器人的目标位置与预设道路线的偏离距离大于所述第二标定阈值；所述在所述机器人的第一行驶轨迹与预设道路线的偏离角度大于第二标定阈值的情况下，对所述机器人的运行角度进行校正之后包括：在所述机器人的第二行驶轨迹与预设道路线的偏离角度大于第二标定阈值的情况下，继续对所述机器人的运行角度进行校正直至所述机器人的目标行驶轨迹与预设道路线的偏离角度小于第二标定阈值。即当机器人在下一个位置时，继续判断第二位置与预设道路线的偏离距离小于所述第二标定阈值，如果是则继续对所述机器人的运行轨迹进行校正直至所述机器人的目标位置与预设道路线的偏离距离大于所述第二标定阈值。

或者，当采集到机器人在下一个采样周期内的轨迹时，继续判断所述第二行驶轨迹与预设道路线的偏离角度大于第二标定阈值，如果是则继续对所述机器人的运行角度进行校正直至所述机器人的目标行驶轨迹与预设道路线的偏离角度小于第二标定阈值。

可选地，所述在所述机器人的所述第一位置与预设道路线的偏离距离小于第一标定阈值的情况下，对所述机器人的运行轨迹进行校正包括：获取所述机器人的所述第一位置与所述预设道路线的偏离方向和所述机器人的第一位置与预设道路线的偏离距离；根据所述机器人的所述第一位置与所述预设道路线的偏离距离确定所述机器人与预设道路线的偏离方向为向右偏离或者向左偏离；在所述机器人与预设道路线的偏离方向是向右偏离的情况下，确定第一校正参数，其中，所述第一校正参数包括：所述机器人的电机输出转速；根据所述第一校正参数，对所述机器人的运行轨迹进行校正；或者，在所述机器人与预设道路线的偏离方向是向左偏离的情况下，确定第二校正参数，其中，所述第二校正参数包括：所述机器人的电机输出转速；根据所述第二校正参数，对所述机器人的运行轨迹进行校正。

具体实施时，首先，计算偏离方向、偏离距离Δd、偏离角度Δθ；若Δα大于0，判断为向右偏，当前偏离距离为Δd＝αi-0.5(αi+βi)；若当Δβ大于0，判断为向左偏，当前偏离距离为Δd＝βi-0.5(βi+αi)。然后，计算校正参数。若消防机器人为向右偏，则计算校正参数为左电机输出转速： n0×(1-Δd/αi+βi)，右电机输出转速：n0×(1+Δd/αi+βi)，其中n0表示自动行驶时左右电机基础转速，取值:0-2500；若消防机器人为向左偏，计算校正参数为左电机输出转速：n0×(1+Δd/αi+βi)右电机输出转速：n0× (1-Δd/αi+βi)，n0表示自动行驶时左右电机基础转速，取值:0-2500。最后将消防机器人轨迹校正到道路中线处开始直线行驶；输出校正参数，主板将左右电机的控制参数分别发给对应的电机驱动器；主板控制左右驱动器向电机发出脉冲指令；左右电机执行控制指令后，校正消防机器人行驶轨迹。进一步地，还需再次进行距离判断，左侧距离αi或右侧距离βi是否小于标定值L，L取值范围1-1.5m；若小于标定值则运行轨迹校正，否则保持当前参数状态继续行驶。

可选地，所述在所述机器人的第一行驶轨迹与预设道路线的偏离角度小于第二标定阈值的情况下，对所述机器人的运行角度进行校正包括：获取所述机器人与预设道路线的偏离方向和所述机器人与预设道路线的偏离角度；根据所述机器人的第一行驶轨迹与预设道路线的偏离角度确定所述机器人与预设道路线的偏离角度为向右偏离或者向左偏离；在所述机器人的第一行驶轨迹与预设道路线的偏离角度是向右偏离的情况下，确定第三校正参数，其中，所述第三校正参数包括：所述机器人的电机输出转速；根据所述第三校正参数，对所述机器人的运行角度进行校正；或者，在所述机器人的第一行驶轨迹与预设道路线的偏离角度是向左偏离的情况下，确定第四校正参数，其中，所述第四校正参数包括：所述机器人的电机输出转速；根据所述第四校正参数，对所述机器人的运行角度进行校正。

具体实施时，首先，计算偏离方向、偏离角度Δθ；若Δθ大于0，则依据定义判断为向右偏；若Δθ小于0，则依据定义判断为向左偏。然后，计算校正参数，若消防机器人为向右偏，则计算校正参数为左电机输出转速：n0×(1-k*Δθ/π)，右电机输出转速：n0×(1+k*Δθ/π)，其中n0表示自动行驶时左右电机基础转速，取值:0-2500，k表示角度校正系数，取值范围：5-10；若消防机器人为向左偏，则计算校正参数为左电机输出转速：n0×(1+k*Δθ/π)，右电机输出转速：n0×(1-k*Δθ/π)，其中n0表示自动行驶时左右电机基础转速，取值:0-2500，k表示角度校正系数，取值范围：5-10。最后，将消防机器人轨迹校正到直线行驶方向；输出校正参数，主板将左右电机的控制参数分别发给对应的电机驱动器；主板控制左右驱动器向电机发出脉冲指令；左右电机执行控制指令后，校正消防机器人行驶轨迹。进一步地，还需要判断行驶轨迹和道路划线中线夹角大于标定值进行偏离角度判断，若偏离角度Δθ小于标定值θb，θb取值范围0-2.5°，则继续保持当前参数状态行驶，否则运行角度校正。

可选地，所述在机器人自动行驶的过程中，获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与预设道路线的偏离角度包括：识别采样图像中的所述机器人的车体上的预设标记点和预设道路线，其中，所述预设标记点包括：第一标记点、第二标记点、第三标记点以及第四标记点，其中，所述第一标记点位于所述第二标记点所在的所述机器人的车体平行线后侧；所述第二标记点位于所述机器人的车体左侧端部，第三标记点位于所述机器人的车体右侧端部，所述第四标记点位于所述第三标记点所在的所述机器人的车体平行线后侧；确定所述第二标记点和所述第三标记点的同轴方向上与所述预设道路向的两侧的交点a，b；确定所述第一标记点和所述第四标记点的同轴方向上与所述预设道路向的两侧的交点c，d；根据所述第二标记点和所述第三标记点的实际距离以及所述第二标记点和所述第三标记点在所述采样图像中的距离，获取所述机器人的第一位置与预设道路线的偏离距离，其中，所述第一位置与预设标记点在预设采样周期内具有对应关系；根据第五标记点和所述机器人的车体预设中点坐标，获取所述机器人的第一行驶轨迹与预设道路线的偏离角度，其中，所述第五标记点位于所述机器人的车体外部并作为所述预设道路线的两侧的延长线的交点，所述预设中点坐标为所述交点c，d 之间连线的中点。

本申请中的消防机器人，包括：轨迹误差校正***，所述轨迹误差校正***被设置为运行时执行所述的机器人轨迹误差校正方法，其中所述消防机器人，包括：摄像机，用于采集消防机器人在自动行驶过程中的图像信息；主板，用于根据所述图像信息中所述消防机器人的偏离距离和偏离角度，生成对应的控制信号对所述消防机器人的轨迹和/或角度进行校正；驱动器，用于将所述主板的控制信号转化为脉冲信号并输出给电机。

通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到根据上述实施例的方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如 ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，或者网络设备等)执行本发明各个实施例所述的方法。

实施例2

在本实施例中还提供了一种机器人轨迹误差校正装置，该装置用于实现上述实施例及优选实施方式，已经进行过说明的不再赘述。如以下所使用的，术语“模块”可以实现预定功能的软件和/或硬件的组合。尽管以下实施例所描述的装置较佳地以软件来实现，但是硬件，或者软件和硬件的组合的实现也是可能并被构想的。

图3是根据本发明实施例的机器人轨迹误差校正装置的结构框图，如图3所示，该装置包括

获取模块30，在机器人自动行驶的过程中，获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与所述预设道路线的偏离角度；

第一校正模块32，用于在所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值的情况下，对所述机器人的运行轨迹进行校正；

第二校正模块34，用于在所述机器人的第一行驶轨迹与预设道路线的偏离角度大于第二标定阈值的情况下，对所述机器人的运行角度进行校正。

具体地，通过在所述机器人自动行驶的过程中，通过识别摄像头中获取的图像得到所述机器人的第一位置与预设道路线的偏离距离。其中，预设道路线是指道路划线或道路边界。所述第一位置是指一个采样周期内机器人所在的位置。同时，通过识别摄像头中获取的图像得到所述机器人的的第一行驶轨迹与所述预设道路线的偏离角度。其中，第一行驶轨迹是指在一个采样周期内机器人所行驶的轨迹。

如果判断在所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值时，则对所述机器人的运行轨迹进行校正，或者在判断所述机器人的第一行驶轨迹与预设道路线的偏离角度大于第二标定阈值，则对所述机器人的运行角度进行校正。其中，所述第一标定阈值、第二标定阈值根据不同场景进行设定，在本申请的实施例中并不进行具体限定。

通过上述模块，通过在在机器人自动行驶的过程中，获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与所述预设道路线的偏离角度，从而根据所述偏离距离或者偏离角度分别对机器人进行轨迹校正或者角度校正。因此，可以解决在消防机器人脱离操作员视线范围进行自动行驶或声控行驶时，仅通过遥控器显示画面判断，难以及时分辨消防机器人行驶轨迹直线度的问题，达到能够实现自动校正消防机器人行驶轨迹，避免了消防机器人与周围环境发生碰撞的效果。

需要说明的是，上述各个模块是可以通过软件或硬件来实现的，对于后者，可以通过以下方式实现，但不限于此：上述模块均位于同一处理器中；或者，上述各个模块以任意组合的形式分别位于不同的处理器中。

为了更好的理解上述机器人轨迹误差校正方法流程，以下结合优选实施例对上述技术方案进行解释说明，但不用于限定本发明实施例的技术方案。

本发明优选实施例的方法，依据视频图像中的偏离距离和偏离角度等信息进行轨迹校正和/或角度校正，能够实现自动校正消防机器人行驶轨迹，从而能够针对多种不同工况，应用不同的校正算法，在保证校正有效性的基础上，提高操作员的感官舒适度。此外，通过自动识别消防机器人与周边环境距离，依据预设安全值，与周边环境保持安全距离，防止机器人碰撞。

如图4所示是根据本申请实施例中的轨迹误差及角度误差示意图，如图5所示是轨迹校正所使用的机器人***结构示意图，如图6所示是轨迹误差校正方法工作流程图，以消防机器人为例，其步骤包括：

步骤S601，启动。

步骤S602，摄像头复位。

当消防机器人进入自动直线行驶时，首先将双目摄像机复位，摄像机位置调整到水平方向与地面平行，镜头方向与车体行进方向同轴。

步骤S603，检测消防机器人距道路划线两侧距离及夹角。

具体地，消防机器人主板控制驱动器，驱动电机运转，开始自动向前行驶，同时开始通过摄像头采集行进过程中的环境信息。其中采集消防机器人距道路划线两侧距离α、β和消防机器人行驶轨迹和道路的夹角θ的具体方法如下：

步骤1，如图4所示，从摄像头采集的图像中识别出消防机器人车体上预设好的标记点A、B、C、D，其中B、C点分别位于消防机器人车体钣金左右侧最前端位置，而A点位于B点的车体平行线后侧，D点位于C 点的车体平行线后侧，A、B点之间距离＝C、D点之间距离＝l，l取值范围：0.3-0.5m。

步骤2，识别道路划线或道路边界。

步骤3，确定车体参考点B、C同一横轴方向上与两侧道路划线的交点H、I，同时可以求解出lHB、lBC和lCI三个线段的在采集图像中的长度，其中lHB和lCI分别表示消防机器人车体左侧距离道路划线的距离和车体右侧距离道路划线的距离，lBC表示消防机器人车体钣金左右侧标记点距离，lBC长度是已知的，取值范围：0.8-1.2m。

步骤4，确定参考点A、D同一横轴方向上与两侧道路划线的交点F、 G；

步骤5，作FG中点J，J坐标固定为第一次采样时FG中点坐标，拟合连线HF及GI。

步骤6，拟合车体连线lHF和lGI，将用于求解道路划线延长线交点， lHF和lGI在实际中是两条预设的平行线，但在摄像头采集的图像中因为成像原理将相交于一点；

步骤7，作lHF和lGI的延长线交点，求解参考点E的坐标(xE,yE)；

步骤8，确定实际的当前采样周期的消防机器人车体前端两侧距道路划线距离αi*(LBC/lBC)和βi*(LBC/lBC)，其中LBC表示消防机器人钣金上参考标记点BC的实际距离，lBC表示消防机器人钣金上参考标记点BC 的在采样图像中的距离。

步骤9，计算偏离角度，则偏离角度Δθ等于lEJ与纵轴的夹角，Δθ＝arctankEJ。

步骤S604，左侧距离或者右侧距离小于标定值？如果是则进入步骤 S605，如果否则进入步骤S613。

首先，进行距离判断，左侧距离αi或右侧距离βi是否小于标定值L， L取值范围1-1.5m；若小于标定值则运行轨迹校正算法，否则进行偏离角度Δθ判断。

其次，进行偏离角度判断，若偏离角度Δθ大于标定值θb，θb取值范围0-2.5°，则运行角度校正算法，否则继续保持当前参数状态行驶。

步骤S605，运行轨迹校正算法。

步骤S606，计算偏离方向、偏离距离、偏离角度。

步骤S607，计算校正参数。

步骤S608，将消防机器人轨迹校正到道路中线出开始直线行驶。

步骤S609，输出校正参数。

步骤S610，主板控制左右驱动器。

步骤S611，执行校正轨迹。

步骤S612，左侧、右侧距离差距小于标定值？

步骤S613，偏离角度大于标定值？若是则进入步骤S614。若否则进入步骤S622。

首先，计算偏离方向、偏离距离Δd、偏离角度Δθ；若Δα大于0，判断为向右偏，当前偏离距离为Δd＝αi-0.5(αi+βi)；若当Δβ大于0，判断为向左偏，当前偏离距离为Δd＝βi-0.5(βi+αi)；然后，计算校正参数，若消防机器人为向右偏，则计算校正参数为左电机输出转速：n0× (1-Δd/αi+βi)，右电机输出转速：n0×(1+Δd/αi+βi)，其中n0表示自动行驶时左右电机基础转速，取值:0-2500；若消防机器人为向左偏，计算校正参数为左电机输出转速：n0×(1+Δd/αi+βi)右电机输出转速：n0× (1-Δd/αi+βi)，n0表示自动行驶时左右电机基础转速，取值:0-2500。最后，将消防机器人轨迹校正到道路中线处开始直线行驶；如图5所示，输出校正参数，主板将左右电机的控制参数分别发给对应的电机驱动器；主板控制左右驱动器向电机发出脉冲指令；左右电机执行控制指令后，校正消防机器人行驶轨迹。优选地，再次进行距离判断，左侧距离αi或右侧距离βi 是否小于标定值L，L取值范围1-1.5m；若小于标定值则运行轨迹校正算法，否则保持当前参数状态继续行驶。

步骤S614，运行角度校正算法。

步骤S615，计算偏离方向、偏离角度。

步骤S616，计算校正参数。

步骤S617，将消防机器人轨迹校正到直线行驶方向。

步骤S618，输出校正参数。

步骤S619，主控板控制左右驱动器。

步骤S620，执行校正轨迹。

步骤S621，行驶轨迹和道路划线中线夹角大于标定值？若是则进入步骤S622。

首先，计算偏离方向、偏离角度Δθ；若Δθ大于0，则依据定义判断为向右偏；若Δθ小于0，则依据定义判断为向左偏；然后，计算校正参数，若消防机器人为向右偏，则计算校正参数为左电机输出转速：n0× (1-k*Δθ/π)，右电机输出转速：n0×(1+k*Δθ/π)，其中n0表示自动行驶时左右电机基础转速，取值:0-2500，k表示角度校正系数，取值范围：5-10；若消防机器人为向左偏，则计算校正参数为左电机输出转速：n0× (1+k*Δθ/π)，右电机输出转速：n0×(1-k*Δθ/π)，其中n0表示自动行驶时左右电机基础转速，取值:0-2500，k表示角度校正系数，取值范围： 5-10。最后，将消防机器人轨迹校正到直线行驶方向；输出校正参数，主板将左右电机的控制参数分别发给对应的电机驱动器；主板控制左右驱动器向电机发出脉冲指令；左右电机执行控制指令后，校正消防机器人行驶轨迹。优选地，还需要判断行驶轨迹和道路划线中线夹角大于标定值？进行偏离角度判断，若偏离角度Δθ小于标定值θb，θb取值范围0-2.5°，则继续保持当前参数状态行驶，否则运行角度校正算法。

步骤S622，机器人继续行驶。

步骤S623，关机信号。

检测到关机信号或手动控制信号，则停止运行程序，否则重新采集图像信息并运行程序。

步骤S624，关机。

如图7所示，是本申请实施例中轨迹误差及角度误差校正曲线图，当产生累计性***误差，消防机器人开启自动直线行驶功能后，由于左右侧电机性能、减速性能、履带松紧程度、地面摩擦力等因素的影响，会让消防机器人有一个偏离角度Δθ0，而这个偏离角度Δθ0很微小，且一直小于标定值，不能够适用角度校正算法，从而导致轨迹逐渐偏离，直到消防机器人左右侧的距离误差Δdi触发极限值后，先运行轨迹校正算法；将消防机器人行驶方向调整为反向偏离，偏离角度为-Δθi，逐渐将消防机器人轨迹调整到道路中线附近；随后再将偏离角度为-Δθi调整为零，既行驶方向与道路方向平行，调整期间引起的轨迹偏离Δdj未触发轨迹调整，消防机器人可以继续行驶。

如图8所示，是本申请实施例中轨迹误差及角度误差校正曲线图，当产生操作失误引入的随机误差1。消防机器人开启自动直线行驶功能后，由于操作人员操作偏差引入的误差，包含偏离角度Δθ0和距离误差Δd0，经过分析判断，这个距离误差Δd0小于极限值值，而偏离角度Δθ0大于标定值，因此运行角度校正算法，将偏离角度为-Δθ0调整为零，既行驶方向与道路方向平行，调整期间引起的轨迹偏离Δdj未触发轨迹调整，消防机器人可以继续行驶。

如图9所示，是本申请实施例中轨迹误差及角度误差校正曲线图，操作失误引入的随机误差2。消防机器人开启自动直线行驶功能后，由于操作人员操作偏差引入的误差，包含偏离角度Δθ0和距离误差Δd0，经过分析判断，这个距离误差Δd0大于极限值值，先运行轨迹校正算法；将消防机器人行驶方向调整为反向偏离，偏离角度为-Δθi，逐渐将消防机器人轨迹调整到道路中线附近；随后再将偏离角度为-Δθi调整为零，既行驶方向与道路方向平行，调整期间引起的轨迹偏离Δdj未触发轨迹调整，消防机器人可以继续行驶。

本发明的实施例还提供了一种存储介质，该存储介质中存储有计算机程序，其中，该计算机程序被设置为运行时执行上述任一项方法实施例中的步骤。

可选地，在本实施例中，上述存储介质可以被设置为存储用于执行以下步骤的计算机程序：

S1，在机器人自动行驶的过程中，获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与所述预设道路线的偏离角度；

S2，在所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值的情况下，对所述机器人的运行轨迹进行校正；或者，在所述机器人的第一行驶轨迹与预设道路线的偏离角度大于第二标定阈值的情况下，对所述机器人的运行角度进行校。

可选地，存储介质还被设置为存储用于执行以下步骤的计算机程序：

S1，在所述机器人的第一位置与预设道路线的偏离距离不小于第一标定阈值的情况下，控制所述机器人按照所述偏离距离继续自动行驶；

S2，在所述机器人的第一行驶轨迹与预设道路线的偏离角度不大于第二标定阈值的情况下，控制所述机器人按照与所述预设道路线的所述偏离角度继续自动行驶。

可选地，在本实施例中，上述存储介质可以包括但不限于：U盘、只读存储器(Read-Only Memory，简称为ROM)、随机存取存储器(Random Access Memory，简称为RAM)、移动硬盘、磁碟或者光盘等各种可以存储计算机程序的介质。

本发明的实施例还提供了一种电子装置，包括存储器和处理器，该存储器中存储有计算机程序，该处理器被设置为运行计算机程序以执行上述任一项方法实施例中的步骤。

可选地，上述电子装置还可以包括传输设备以及输入输出设备，其中，该传输设备和上述处理器连接，该输入输出设备和上述处理器连接。

可选地，在本实施例中，上述处理器可以被设置为通过计算机程序执行以下步骤：

S1，在机器人自动行驶的过程中，获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与所述预设道路线的偏离角度；

S2，在所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值的情况下，对所述机器人的运行轨迹进行校正；或者，在所述机器人的第一行驶轨迹与预设道路线的偏离角度大于第二标定阈值的情况下，对所述机器人的运行角度进行校正。

可选地，本实施例中的具体示例可以参考上述实施例及可选实施方式中所描述的示例，本实施例在此不再赘述。

显然，本领域的技术人员应该明白，上述的本发明的各模块或各步骤可以用通用的计算装置来实现，它们可以集中在单个的计算装置上，或者分布在多个计算装置所组成的网络上，可选地，它们可以用计算装置可执行的程序代码来实现，从而，可以将它们存储在存储装置中由计算装置来执行，并且在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤，或者将它们分别制作成各个集成电路模块，或者将它们中的多个模块或步骤制作成单个集成电路模块来实现。这样，本发明不限制于任何特定的硬件和软件结合。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种机器人轨迹误差校正方法，其特征在于，包括：

在机器人自动行驶的过程中，获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与所述预设道路线的偏离角度；

在所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值的情况下，对所述机器人的运行轨迹进行校正；或者，在所述机器人的第一行驶轨迹与预设道路线的偏离角度大于第二标定阈值的情况下，对所述机器人的运行角度进行校正；

在所述机器人的所述第一位置与所述预设道路线的所述偏离距离不小于所述第一标定阈值且所述机器人的所述第一行驶轨迹与所述预设道路线的所述偏离角度大于所述第二标定阈值的情况下，对所述机器人的运行角度进行校正；

在所述机器人的第一行驶轨迹与预设道路线的偏离角度不大于第二标定阈值且所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值的情况下，对所述机器人的运行轨迹进行校正。

2.根据权利要求1所述的方法，其特征在于，在获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与所述预设道路线的偏离角度之后，所述方法还包括以下之一：

在所述机器人的第一位置与预设道路线的偏离距离不小于第一标定阈值的情况下，控制所述机器人按照所述偏离距离继续自动行驶；

在所述机器人的第一行驶轨迹与预设道路线的偏离角度不大于第二标定阈值的情况下，控制所述机器人按照与所述预设道路线的所述偏离角度继续自动行驶。

3.根据权利要求1所述的方法，其特征在于，在所述机器人的所述第一位置与所述预设道路线的所述偏离距离小于所述第一标定阈值的情况下，对所述机器人的运行轨迹进行校正之后，所述方法还包括：

在所述机器人的第二位置与预设道路线的偏离距离小于所述第二标定阈值的情况下，继续对所述机器人的运行轨迹进行校正直至所述机器人的目标位置与预设道路线的偏离距离大于所述第二标定阈值；

所述在所述机器人的第一行驶轨迹与预设道路线的偏离角度大于第二标定阈值的情况下，对所述机器人的运行角度进行校正之后包括：

在所述机器人的第二行驶轨迹与预设道路线的偏离角度大于第二标定阈值的情况下，继续对所述机器人的运行角度进行校正直至所述机器人的目标行驶轨迹与预设道路线的偏离角度小于第二标定阈值。

4.根据权利要求1所述的方法，其特征在于，所述在所述机器人的所述第一位置与预设道路线的偏离距离小于第一标定阈值的情况下，对所述机器人的运行轨迹进行校正包括：

获取所述机器人的所述第一位置与所述预设道路线的偏离方向和所述机器人的第一位置与预设道路线的偏离距离；

根据所述机器人的所述第一位置与所述预设道路线的偏离距离确定所述机器人与预设道路线的偏离方向为向右偏离或者向左偏离；

在所述机器人与预设道路线的偏离方向是向右偏离的情况下，确定第一校正参数，其中，所述第一校正参数包括：所述机器人的电机输出转速；

根据所述第一校正参数，对所述机器人的运行轨迹进行校正；

或者，在所述机器人与预设道路线的偏离方向是向左偏离的情况下，确定第二校正参数，其中，所述第二校正参数包括：所述机器人的电机输出转速；

根据所述第二校正参数，对所述机器人的运行轨迹进行校正。

5.根据权利要求1所述的方法，其特征在于，所述在所述机器人的第一行驶轨迹与预设道路线的偏离角度小于第二标定阈值的情况下，对所述机器人的运行角度进行校正包括：

获取所述机器人与预设道路线的偏离方向和所述机器人与预设道路线的偏离角度；

根据所述机器人的第一行驶轨迹与预设道路线的偏离角度确定所述机器人与预设道路线的偏离角度为向右偏离或者向左偏离；

在所述机器人的第一行驶轨迹与预设道路线的偏离角度是向右偏离的情况下，确定第三校正参数，其中，所述第三校正参数包括：所述机器人的电机输出转速；

根据所述第三校正参数，对所述机器人的运行角度进行校正；

或者，在所述机器人的第一行驶轨迹与预设道路线的偏离角度是向左偏离的情况下，确定第四校正参数，其中，所述第四校正参数包括：所述机器人的电机输出转速；

根据所述第四校正参数，对所述机器人的运行角度进行校正。

6.根据权利要求1所述的方法，其特征在于，所述在机器人自动行驶的过程中，获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与预设道路线的偏离角度包括：

识别采样图像中的所述机器人的车体上的预设标记点和预设道路线，其中，所述预设标记点包括：第一标记点、第二标记点、第三标记点以及第四标记点，其中，所述第一标记点位于所述第二标记点所在的所述机器人的车体平行线后侧；所述第二标记点位于所述机器人的车体左侧端部，第三标记点位于所述机器人的车体右侧端部，所述第四标记点位于所述第三标记点所在的所述机器人的车体平行线后侧；

确定所述第二标记点和所述第三标记点的同轴方向上与所述预设道路线 的两侧的交点a，b；

确定所述第一标记点和所述第四标记点的同轴方向上与所述预设道路线 的两侧的交点c，d；

根据所述第二标记点和所述第三标记点的实际距离以及所述第二标记点和所述第三标记点在所述采样图像中的距离，获取所述机器人的第一位置与预设道路线的偏离距离，其中，所述第一位置与预设标记点在预设采样周期内具有对应关系；

根据第五标记点和所述机器人的车体预设中点坐标，获取所述机器人的第一行驶轨迹与预设道路线的偏离角度，其中，所述第五标记点位于所述机器人的车体外部并作为所述预设道路线的两侧的延长线的交点，所述预设中点坐标为所述交点c，d之间连线的中点。

7.一种机器人轨迹误差校正装置，其特征在于，包括：

获取模块，在机器人自动行驶的过程中，获取所述机器人的第一位置与预设道路线的偏离距离和所述机器人的第一行驶轨迹与所述预设道路线的偏离角度；

第一校正模块，用于在所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值的情况下，对所述机器人的运行轨迹进行校正；

第二校正模块，用于在所述机器人的第一行驶轨迹与预设道路线的偏离角度大于第二标定阈值的情况下，对所述机器人的运行角度进行校正；

所述第二校正模块还用于，在所述机器人的所述第一位置与所述预设道路线的所述偏离距离不小于所述第一标定阈值且所述机器人的所述第一行驶轨迹与所述预设道路线的所述偏离角度大于所述第二标定阈值的情况下，对所述机器人的运行角度进行校正；在所述机器人的第一行驶轨迹与预设道路线的偏离角度不大于第二标定阈值且所述机器人的第一位置与预设道路线的偏离距离小于第一标定阈值的情况下，对所述机器人的运行轨迹进行校正。

8.一种消防机器人，其特征在于，包括：轨迹误差校正***，所述轨迹误差校正***被设置为运行时执行所述权利要求1至6任一项中所述的方法。

9.根据权利要求8所述的消防机器人，其特征在于，包括：

摄像机，用于采集消防机器人在自动行驶过程中的图像信息；

主板，用于根据所述图像信息中所述消防机器人的偏离距离和偏离角度，生成对应的控制信号对所述消防机器人的轨迹和/或角度进行校正；

驱动器，用于将所述主板的控制信号转化为脉冲信号并输出给电机。

Images