智能清淤机器人及其行走控制方法与流程

1.本公开实施例涉及机器人技术领域，尤其涉及一种智能清淤机器人及其行走控制方法。


背景技术：

2.在煤矿厂区发展建设中，井下水仓是是防止矿井水灾的重要设施，也是每一个矿井必须配备的生产系统。井下水仓工作条件恶劣，淤积物中含水量大，搅动后往往变成半流态或流态，难以彻底清理。
3.目前煤炭行业采取的清淤工艺仍以人工清理淤泥为主，搭配罐车进行淤泥搬运。传统的清仓方式劳动强度大、工作效率低、清仓周期长，影响安全生产，并且具有一定的危险性，无法实现水仓淤泥的实时监测预警与快速清淤，不能满足安全生产的需要。
4.为了实现水仓淤泥的快速清理与实时监控，提高水仓利用率以及保证矿井水患安全的迫切需求。相关技术中提出智能清淤机器人来实现自动清理与实时监控，但是目前的智能清淤机器人的在煤矿厂区的行走控制精确度有待进一步提高。


技术实现要素：

5.为了解决上述技术问题或者至少部分地解决上述技术问题，本公开实施例提供了一种智能清淤机器人及其行走控制方法。
6.第一方面，本公开实施例提供了一种智能清淤机器人，包括：
7.角度传感器，设置于该清淤机器人的行走电机处，用于获取所述行走电机的转轴的旋转角度；
8.光电编码器，设置于该清淤机器人的行走电机处，用于输出与所述行走电机的转速对应的多个脉冲信号；
9.运动控制器，设置于该清淤机器人的底盘上，与所述角度传感器和所述光电编码器连接，用于基于所述旋转角度确定所述行走电机的第一转速，基于所述多个脉冲信号确定所述行走电机的第二转速，基于所述第一转速和所述第二转速确定第三转速，基于所述第三转速和设定转速生成pwm驱动信号，将所述pwm驱动信号发送至该清淤机器人的行走电机驱动器以使所述行走电机驱动器驱动所述行走电机。
10.在一个实施例中，还包括：
11.超声波传感器，设置于该清淤机器人的底盘上，用于检测该清淤机器人行进方向上是否有障碍物，若是则输出该清淤机器人与所述障碍物之间的距离；
12.所述运动控制器，与所述超声波传感器连接，用于在所述距离小于预设距离时，生成避障pwm驱动信号，将所述避障pwm驱动信号发送至所述行走电机驱动器，以使所述行走电机驱动器驱动所述行走电机的运转从而使该清淤机器人避开障碍物。
13.在一个实施例中，还包括：
14.激光雷达，与所述运动控制器连接；
15.主控制器，与所述运动控制器通信连接；
16.温度传感器，与所述主控制器连接，用于检测环境温度；
17.气体传感器，与所述主控制器连接，用于检测环境中的有害气体含量；其中，所述有害气体包括甲烷、瓦斯、硫化氢、一氧化碳中的一个或多个；
18.所述主控制器，用于在所述环境温度大于预设温度，和/或所述有害气体含量大于预设值时，发出报警信号；
19.视频采集装置，与所述主控制器连接，用于采集该清淤机器人现场的场景图像，所述视频采集装置包括云台摄像仪和网络摄像仪。
20.在一个实施例中，还包括以太网交换机，与所述云台摄像仪和网络摄像仪连接，用于将所述云台摄像仪和网络摄像仪采集的视频信息传输至外部的上位机；和/或，还包括设置于清淤机器人上的集料机构，所述集料机构包括螺旋集料器、驱动所述螺旋集料器旋转的液压马达、向所述液压马达输送液压油的液压管、与所述液管连接的液压泵，以及容纳所述液压泵的油箱。
21.在一个实施例中，所述运动控制器，还用于获取环境地图，基于所述环境地图确定包含清淤机器人行走路线的全局地图，基于所述全局地图进行路径规划，得到规划路径，基于所述规划路径控制所述电机驱动器驱动所述行走电机，以使所述清淤机器人基于所述规划路径行走。
22.在一个实施例中，所述全局地图包括多个节点，各节点之间的连接关系表示机器人可行的路线；所述运动控制器基于所述全局地图进行路径规划，得到规划路径，包括：
23.确定所述全局地图中的中间节点形成中间节点集合；
24.确定该清淤机器人的当前位置点，执行预设路径搜索算法确定当前位置点与所述中间节点集合中的各中间节点的最短路径，选择各中间节点的最短路径中路径距离最小的中间节点作为新的当前位置点；
25.执行预设路径搜索算法确定新的当前位置点与所述中间节点集合中的剩余各中间节点的最短路径，选择剩余各中间节点的最短路径中路径距离最小的中间节点作为新的当前位置点，重复执行预设搜索算法直至中间节点集合中的各中间节点被遍历；
26.确定所述中间节点集合中最后一个被遍历的中间节点与目标位置点之间的目标最短路径；
27.合并所述目标最短路径以及每次执行预设路径搜索算法时确定的各中间节点的最短路径中路径距离最小的最短路径得到规划路径。
28.在一个实施例中，所述预设路径搜索算法至少包括迪克斯特拉算法。
29.在一个实施例中，所述运动控制器包括pid控制器，所述pid运动控制器基于所述第三转速和设定转速的差值生成pwm驱动信号。
30.在一个实施例中，还包括压力传感器，所述压力传感器设置于所述油箱上，所述压力传感器与所述主控制器连接。
31.第二方面，本公开实施例提供一种智能清淤机器人行走控制方法，所述清淤机器人上设置有位于该清淤机器人的行走电机处的角度传感器和光电编码器，以及位于该清淤机器人的底盘上的运动控制器，该方法包括：
32.角度传感器获取所述行走电机的转轴的旋转角度；
33.光电编码器输出与所述行走电机的转速对应的多个脉冲信号；
34.运动控制器基于所述旋转角度确定所述行走电机的第一转速，基于所述多个脉冲信号确定所述行走电机的第二转速，基于所述第一转速和所述第二转速确定第三转速，基于所述第三转速和设定转速生成pwm驱动信号，将所述pwm驱动信号发送至该清淤机器人的行走电机驱动器以使所述行走电机驱动器驱动所述行走电机。
35.本公开实施例提供的技术方案与现有技术相比具有如下优点：
36.本公开实施例提供的智能清淤机器人及其行走控制方法，角度传感器获取所述行走电机的转轴的旋转角度，光电编码器输出与所述行走电机的转速对应的多个脉冲信号，运动控制器基于所述旋转角度确定所述行走电机的第一转速，基于所述多个脉冲信号确定所述行走电机的第二转速，基于所述第一转速和所述第二转速确定第三转速，基于所述第三转速和设定转速生成pwm驱动信号，将所述pwm驱动信号发送至该清淤机器人的行走电机驱动器以使所述行走电机驱动器驱动所述行走电机。如此可基于角度传感器和光电编码器各自的输出信号综合确定行走电机的转速即第三转速，再基于综合确定的行走电机的转速和设定转速生成相应的pwm驱动信号，然后将该pwm驱动信号发送至行走电机驱动器以使行走电机驱动器驱动行走电机运转，如此可以提高行走电机运转控制的精确度，使得清淤机器人的行走控制更精确。
附图说明
37.此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本公开的实施例，并与说明书一起用于解释本公开的原理。
38.为了更清楚地说明本公开实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，对于本领域普通技术人员而言，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
39.图1为本公开实施例智能清淤机器人的控制架构示意图；
40.图2为本公开另一实施例智能清淤机器人的控制架构示意图；
41.图3为本公开实施例的路径规划示意图；
42.图4为本公开实施例加减速曲线示意图；
43.图5为本公开实施例智能清淤机器人行走控制方法流程示意图。
具体实施方式
44.为了能够更清楚地理解本公开的上述目的、特征和优点，下面将对本公开的方案进行进一步描述。需要说明的是，在不冲突的情况下，本公开的实施例及实施例中的特征可以相互组合。
45.在下面的描述中阐述了很多具体细节以便于充分理解本公开，但本公开还可以采用其他不同于在此描述的方式来实施；显然，说明书中的实施例只是本公开的一部分实施例，而不是全部的实施例。
46.应当理解，在下文中，“至少一个(项)”是指一个或者多个，“多个”是指两个或两个以上。“和/或”用于描述关联对象的关联关系，表示可以存在三种关系，例如，“a和/或b”可以表示：只存在a，只存在b以及同时存在a和b三种情况，其中a，b可以是单数或者复数。字符“/”一般表示前后关联对象是一种“或”的关系。“以下至少一项(个)”或其类似表达，是指这些项中的任意组合，包括单项(个)或复数项(个)的任意组合。例如，a，b或c中的至少一项(个)，可以表示：a，b，c，“a和b”，“a和c”，“b和c”，或“a和b和c”，其中a，b，c可以是单个，也可以是多个。
47.图1为本公开实施例提供了一种智能清淤机器人的控制架构示意图，包括：角度传感器设置于该清淤机器人的行走电机处，例如前后轮的行走电机均设置角度传感器，用于获取所述行走电机的转轴的旋转角度；光电编码器设置于该清淤机器人的行走电机处，例如前后轮的行走电机均设置光电编码器，用于输出与所述行走电机的转速对应的多个脉冲信号。运动控制器设置于该清淤机器人的底盘上，与所述角度传感器和所述光电编码器连接，用于基于所述旋转角度确定所述行走电机的第一转速，基于所述多个脉冲信号确定所述行走电机的第二转速，基于所述第一转速和所述第二转速确定第三转速，基于所述第三转速和设定转速生成pwm驱动信号，将所述pwm驱动信号发送至该清淤机器人的行走电机驱动器以使所述行走电机驱动器驱动所述行走电机。
48.示例性的，运动控制器例如基于所述旋转角度确定旋转的圆周数，基于圆周数确定对应的距离，该距离再除以时间即可确定行走电机的第一转速，同时可基于例如单位时间如1秒内输出的脉冲信号的数量确定所述行走电机的第二转速，然后基于所述第一转速和所述第二转速确定第三转速，例如计算第一转速和第二转速的平均值得到第三转速。
49.本实施例中可基于角度传感器和光电编码器各自的输出信号综合确定行走电机的转速即第三转速，再基于综合确定的行走电机的转速和设定转速生成相应的pwm驱动信号，然后将该pwm驱动信号发送至行走电机驱动器以使行走电机驱动器驱动行走电机运转，如此可以提高行走电机运转控制的精确度，使得清淤机器人的行走控制更精确。
50.在一个实施例中，如图2所示，还包括超声波传感器，设置于该清淤机器人的底盘上，用于检测该清淤机器人行进方向上是否有障碍物，若是则输出该清淤机器人与所述障碍物之间的距离；所述运动控制器，与所述超声波传感器连接，用于在所述距离小于预设距离时，生成避障pwm驱动信号，将所述避障pwm驱动信号发送至所述行走电机驱动器，以使所述行走电机驱动器驱动所述行走电机的运转从而使该清淤机器人避开障碍物。
51.在一个实施例中，如图2所示，还包括：激光雷达，与所述运动控制器连接，基于激光雷达可以实现机器人的导航控制，提高导航控制的准确性。主控制器与所述运动控制器通信连接，主控制器可以向运动控制器发送控制指令如启动、停止等指令，以触发运动控制器控制行走电机的运转或停止运转等。温度传感器与所述主控制器连接，用于检测环境温度。气体传感器与所述主控制器连接，用于检测环境中的有害气体含量。其中，所述有害气体包括甲烷、瓦斯、硫化氢、一氧化碳中的一个或多个，相应的气体传感器可以是对应类型的气体传感器。所述主控制器，用于在所述环境温度大于预设温度，和/或所述有害气体含量大于预设值时，发出报警信号。
52.在一些场景下，清淤机器人完全的全自动控制方式存在不可靠性，在一个实施例中，激光雷达配置有两个，两个所述激光雷达对称设置，用于分别扫描得到煤矿厂区内的预设目标对象如配置的定位标识物的位置，所述预设目标对象相对于两个所述激光雷达在预设角度范围内移动，所述预设角度范围位于两个所述激光雷达的扫描范围之内，呈扇形且角度范围为65-110
°
。
53.所述主控制器，用于基于两个所述激光雷达各自扫描得到的预设目标对象的位置即两个位置数据生成煤矿厂区内的目标位置如二维空间的一个位置数据，基于所述目标位置生成pwm驱动信号并发送至所述行走电机驱动器，以使所述行走电机驱动器驱动所述行走电机的运转从而使该清淤机器人行走。如此可以基于预设目标对象的小范围的移动，借助扫描得到的预设目标对象的位置合成煤矿厂区内的目标位置，据此目标位置辅助引导驱动该清淤机器人行走，减少完全的全自动控制方式存在的不可靠性，也即提高该清淤机器人行走控制的可靠性。
54.视频采集装置与所述主控制器连接，用于采集该清淤机器人现场的场景图像，所述视频采集装置包括云台摄像仪和网络摄像仪。在一个实施例中，还包括以太网交换机，与所述云台摄像仪和网络摄像仪连接，用于将所述云台摄像仪和网络摄像仪采集的视频信息传输至外部的上位机，以便于观察周围环境。
55.在一个实施例中，还包括设置于清淤机器人上的集料机构，所述集料机构包括螺旋集料器、驱动所述螺旋集料器旋转的液压马达、向所述液压马达输送液压油的液压管、与所述液管连接的液压泵，以及容纳所述液压泵的油箱(图未示)，这些均可以设置于清淤机器人的机身上，机身位于底盘上。螺旋集料器可以旋转时吸附清理例如淤泥、灰尘等杂物，并实现杂物收集。液压泵可以与主控制器连接，由主控制器控制液压泵的工作如启停等。在一个实施例中，还可以包括压力传感器，所述压力传感器设置于所述油箱上，检测油箱的压力，所述压力传感器与所述主控制器连接，主控制器可在压力传感器检测油箱的压力大于预设压力时，发出报警信号，如声光电报警信号等。
56.在一个实施例中，所述运动控制器，还用于获取环境地图，基于所述环境地图确定包含清淤机器人行走路线的全局地图，基于所述全局地图进行路径规划，得到规划路径，基于所述规划路径控制所述电机驱动器驱动所述行走电机，以使所述清淤机器人基于所述规划路径行走。
57.为了降低路径规划的复杂度，同时又提高路径规划的准确性和效率。在一个实施例中，所述全局地图包括多个节点，各节点之间的连接关系表示机器人可行的路线。所述运动控制器基于所述全局地图进行路径规划，得到规划路径，包括：确定所述全局地图中的中间节点形成中间节点集合；确定该清淤机器人的当前位置点，执行预设路径搜索算法确定当前位置点与所述中间节点集合中的各中间节点的最短路径，选择各中间节点的最短路径中路径距离最小的中间节点作为新的当前位置点；执行预设路径搜索算法确定新的当前位置点与所述中间节点集合中的剩余各中间节点的最短路径，选择剩余各中间节点的最短路径中路径距离最小的中间节点作为新的当前位置点，重复执行预设搜索算法直至中间节点集合中的各中间节点被遍历；确定所述中间节点集合中最后一个被遍历的中间节点与目标位置点之间的目标最短路径；合并所述目标最短路径以及每次执行预设路径搜索算法时确定的各中间节点的最短路径中路径距离最小的最短路径得到规划路径。在一个实施例中，所述预设路径搜索算法至少包括迪克斯特拉算法即dijkstra算法。
58.具体的，清淤机器人在小区执行清淤工作的实际场景中，在从起始点如当前位置点到目标点如目标位置点可能需要经过某一个或者几个中间节点，所以在规划路径时需要找到一条经过所有中间节点集合中的节点的最短路径。传统的dijkstra算法只能解决经典的单源到其他节点的最短路径问题，无法解决包含中间节点的情况。针对这个问题，可以用
穷举法求出最终结果，通过一个个检验，最终找到包含所有中间节点的最短路径，但是这样算法的复杂程度会随着问题的复杂程度成指数的增长，即随着中间节点增多，中间节点集合不断变大，满足条件的路径会随着节点的增加而成指数增长，这样搜索最优的最短路径的空间复杂度和时间复杂度都太高，在实际的应用中显然是不可行的。在本实施例中，考虑到清淤巡检机器人实际的场景应用情况，要在有限的时间里规划出一条包含选定的所有中间节点的可行的路径，搜索一条近似最优解来代替最优解是可行的方案。因此，在dijkstra最短路径算法的基础上，提出一种包含中间节点集的改进路径搜索算法。
59.改进后的包含中间节点集合的路径搜索算法的基本过程如下：
60.将节点按是否是中间节点分类，中间节点集合c包含n 1个中间节点ki(其中i＝0,1,2,...,n)。中间节点即一条路径中起始点和终点之间的节点。
61.从当前起始点开始执行dijkstra算法，找到当前起始点到所有中间节点的最短路径。然后基于贪心策略的思想，将当前起始点到所有中间节点的最短路径的路径距离按长短大小进行排序，选择路径距离最小的最短路径作为分段路径，并将该路径距离最小的最短路径对应的中间节点ki作为新的当前起始点，重复执行dijkstra算法步骤，直到遍历访问完中间节点集合c中所有中间节点。
62.当所有中间节点都访问结束，再寻找最后一次访问的中间节点到目标点的最短路径作为分段路径，合并所有分段路径，最终得到一条从起始点到目标点的近似最优解的最短路径。
63.下面结合一个具体示例进一步说明。如图3所示，选择顶点1为当前起始点s(即当前位置点)，顶点5为终点g即目标位置点。采用传统的dijkstra算法进行路径规划，得到的结果是例如起始点s到终点g的最短路径的距离为20，最短路径为节点1-》3-》6-》5的连线，如图4所示。
64.采用本实施例的包含中间节点集的dijkstra改进路径搜索算法，定义一维数组dist来存储起始点到各个顶点的最短路径，数组s存储顶点即节点集合，数组path存储起始点到目标点的路径，数组pathlist存储所有的分段路径。例如将顶点2和顶点4放入中间节点集合c。按照算法，先以顶点1作为起点，计算到顶点2的路径距离dist[2]＝7，到顶点4的路径距离dist[4]＝20，中间节点已经遍历完，比较起点到各个中间节点的路径距离大小，dist[2]《dist[4]，因此将分段路径1-》2放入路径pathlist；接着将顶点2作为起点，按照算法计算到顶点4的路径距离dist[4]＝15，中间节点遍历完，此时因为只有一个中间节点，所以将路径2-》4放入pathlist；中间节点已经全部访问完，将最后访问的顶点4作为起点，寻找到终点即顶点5的最短路径即dist[5]＝6，将分段路径4-》5放入pathlist，合并各分段路径，即找到起始点s到终点g的路径，最终结果为路径1-》2-》4》5，如图3所示。
[0065]
通过分段找到最优解，当找到所有节点后，再合并分段路径，得到近似最短路径，从而可在有限的时间里规划出一条包含选定的所有中间节点的可行的路径，搜索一条近似最优解来代替最优解，从而降低路径规划的复杂度，同时又在一定程度上提高路径规划的准确性和效率。
[0066]
在一个实施例中，所述运动控制器包括pid控制器，所述pid运动控制器基于所述第三转速和设定转速的差值生成pwm驱动信号。关于pid控制的原理可以参考现有技术理解，本实施例中基于pid控制器实现清淤机器人行走控制，可提高在颠簸路面上行走的平稳
性。
[0067]
在一个实施例中，所述运动控制器还用于基于预设的加减速曲线控制所述清淤机器人行走。
[0068]
其中，所述预设的加减速曲线如图4所示，主要由7个速度阶段组成，分别依次为：加加速阶段、匀加速阶段、减加速阶段、匀速阶段、加减速阶段、匀减速阶段和减减速阶段(即t1～t7各自对应的曲线段)。可以看出，在各个阶段的曲线段之间，速度连接平滑，使得机器人运行更加平稳，速度变化更加平稳，整个系统的稳定性得到了提高。
[0069]
具体的，清淤机器人的运行速度，加速度，加加速度之间的数学关系式如下：
[0070][0071][0072]
其中，j(τ)为清淤机器人的加加速度函数，对其积分，可得到相应的加速度函数a(t)，对a(t)进行时间积分，可得到清淤机器人的速度函数v(t)。
[0073]
各个速度阶段，加加速度函数如下所示：
[0074][0075]
其中，j表示加加速度。
[0076]
根据上述公式，可推导出各个速度阶段如下的加速度公式：
[0077][0078]
再由上述加速度公式，可推导出清淤机器人在加减速曲线的各个速度阶段的运行速度计算公式：
[0079][0080]
本公开实施例提供一种智能清淤机器人行走控制方法，所述清淤机器人上设置有位于该清淤机器人的行走电机处的角度传感器和光电编码器，以及位于该清淤机器人的底盘上的运动控制器，如图5所示，该方法包括以下步骤：
[0081]
步骤s501：角度传感器获取所述行走电机的转轴的旋转角度；
[0082]
步骤s502：光电编码器输出与所述行走电机的转速对应的多个脉冲信号；
[0083]
步骤s503：运动控制器基于所述旋转角度确定所述行走电机的第一转速，基于所述多个脉冲信号确定所述行走电机的第二转速，基于所述第一转速和所述第二转速确定第三转速，基于所述第三转速和设定转速生成pwm驱动信号，将所述pwm驱动信号发送至该清淤机器人的行走电机驱动器以使所述行走电机驱动器驱动所述行走电机。
[0084]
在一个实施例中，该方法包括以下步骤：
[0085]
运动控制器获取环境地图，基于所述环境地图确定包含清淤机器人行走路线的全局地图；
[0086]
基于所述全局地图进行路径规划，得到规划路径；
[0087]
基于所述规划路径控制所述电机驱动器驱动所述行走电机，以使所述清淤机器人基于所述规划路径行走。
[0088]
在一个实施例中，所述全局地图包括多个节点，各节点之间的连接关系表示机器人可行的路线。所述运动控制器基于所述全局地图进行路径规划，得到规划路径，包括：确定所述全局地图中的中间节点形成中间节点集合；确定该清淤机器人的当前位置点，执行预设路径搜索算法确定当前位置点与所述中间节点集合中的各中间节点的最短路径，选择各中间节点的最短路径中路径距离最小的中间节点作为新的当前位置点；执行预设路径搜索算法确定新的当前位置点与所述中间节点集合中的剩余各中间节点的最短路径，选择剩余各中间节点的最短路径中路径距离最小的中间节点作为新的当前位置点，重复执行预设搜索算法直至中间节点集合中的各中间节点被遍历；确定所述中间节点集合中最后一个被遍历的中间节点与目标位置点之间的目标最短路径；合并所述目标最短路径以及每次执行预设路径搜索算法时确定的各中间节点的最短路径中路径距离最小的最短路径得到规划路径。
[0089]
在一个实施例中，所述预设路径搜索算法至少包括迪克斯特拉算法即dijkstra算法。
[0090]
需要说明的是，尽管在附图中以特定顺序描述了本公开中方法的各个步骤，但是，
这并非要求或者暗示必须按照该特定顺序来执行这些步骤，或是必须执行全部所示的步骤才能实现期望的结果。附加的或备选的，可以省略某些步骤，将多个步骤合并为一个步骤执行，以及/或者将一个步骤分解为多个步骤执行等。另外，也易于理解的是，这些步骤可以是例如在多个模块/进程/线程中同步或异步执行。
[0091]
需要说明的是，在本文中，诸如“第一”和“第二”等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个
……”
限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
[0092]
以上所述仅是本公开的具体实施方式，使本领域技术人员能够理解或实现本公开。对这些实施例的多种修改对本领域的技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本公开的精神或范围的情况下，在其它实施例中实现。因此，本公开将不会被限制于本文所述的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。