一种矿用救援跟随机器人跟随速度控制方法与流程

1.本发明涉及机器人控制技术领域，具体涉及了一种矿用救援跟随机器人跟随速度控制方法。


背景技术：

2.在煤矿救援领域，救援人员自身需要穿戴防护设备，同时还要搬运各种救援装备以及被困人员，在危险且复杂的救援过程中，传统的搬运方式劳动强度大，效率低，且容易损坏搬运设备，而自主跟随机器人能够帮助救援人员搬运货物，对救援人员进行近距离跟随，从而解放救援人员双手，降低救援人员的劳动强度，提高搬运效率。
3.在对跟随机器人的速度进行控制时，现有的控制技术大多采用无极调速，即根据跟随目标与跟随机器人之间的距离，对机器人进行线性调速，而在实际应用过程中，由于测距传感器的测量误差以及跟随目标行走的速度不稳定，导致跟随目标与机器人之间的距离在不断进行变化，从而导致了机器人不断地进行速度调节，如果测得的跟随目标与跟随机器人的间距突然增大或减小，会导致机器人行走速度变化幅度大，出现卡顿或猛冲的现象，使得机器人的行走效果不理想。


技术实现要素：

4.本发明所解决的技术问题在于提供一种矿用救援跟随机器人速度控制方法，使机器人的在跟随过程中，控制速度阶梯式地变化，而非线性变化，防止机器人在改变速度时出现卡顿或猛冲的现象。
5.本发明提供的基础方案：一种矿用救援跟随机器人跟随速度控制方法，包括以下步骤：
6.距离分段步骤：设置机器人与被跟随人员之间的最大跟随距离和最小跟随距离，并将最大跟随距离和最小跟随距离之间的区间等分为若干份子距离区间段；
7.速度设置步骤：设置机器人的移动速度，每个子距离区间段对应一个相同的机器人的移动速度，且子距离区间段越大，机器人的移动速度越大；
8.距离检测步骤：机器人上设有距离检测器，被跟随人员佩带移动标签，实时检测距离检测器与移动标签之间的距离，作为机器人与被跟随人员之间的跟随距离；
9.速度控制步骤：根据所检测到的跟随距离所处的子距离区间段，控制机器人保持该子距离区间段对应的移动速度行驶。
10.本发明的原理及优点在于：在机器人上设置距离检测器，被跟随人员身上佩带移动标签，从而实时检测机器人与被跟随人员之间的距离，设置最大跟随距离和最小跟随距离，将最大跟随距离和最小跟随距离之间的区间等分为若干个子距离区间段，每个子距离区间段对应一个相同的移动速度，且子距离区间段越大，所对应的机器人的移动速度越快，根据检测到的跟随距离所处的子距离区间段，控制机器人的移动速度。从而实现了机器人跟随时移动速度呈阶梯式地变化。相比于现有技术中，机器人根据与被跟随人员的距离大
小，线性控制速度进行调整，本发明在本跟随人员进行相对较匀速的运动时，在机器人与被跟随人员之间的距离变化处于子距离区间段，机器人的移动速度不会发生变化，保持一个速度进行匀速的行驶，而当被跟随人员突然加速后，机器人先是保持原有的速度行驶，但由于被跟随人员加速，因此被跟随人员与机器人之间的距离逐渐增大，而当被跟随人员与机器人之间的距离增大至下一子距离区间端时，机器人的移动速度便变化为该子距离区间端对应的移动速度，若此时机器人与被跟随人员之间的距离还在逐渐增大，又增大到下一子距离区间段时，机器人的移动速度再变化为该子距离区间段对应的移动速度，被跟随人员减速也是同理。从而防止了机器人在跟随过程中频繁地进行速度调节，以及在机器人与被跟随人员之间的距离突然增大后减小导致的出现卡顿或者猛冲的现象，使机器人跟随行动的效果更好。
11.进一步，所述距离检测步骤还包括以下步骤：
12.步骤一：所述移动标签包括第一标签和第二标签，分别佩带在被跟随人员的左手和右手的手腕上，距离检测器包括第一距离检测器和第二距离检测器，分别设置在机器人的左右两端；
13.步骤二：第一距离检测器检测与第一标签之间的距离，记为第一距离，第二距离检测器检测与第二标签之间的距离，记为第二距离；
14.步骤三：将同一时刻所检测到的第一距离和第二距离取平均值，作为机器人与被跟随人员之间的跟随距离。
15.当被跟随人员在移动，身体会产生摆动，从而会产生距离检测上的误差，因此通过在被跟随人员的左右手上分别设置一个移动标签，分别检测与两个移动标签之间的距离，再取两个距离的平均值作为机器人与被跟随人员之间的距离，因为人在移动时，左右手的前后摆动幅度相对较同步且均衡，通过这种方式能够减少测量机器人与被跟随人员之间距离的误差。
16.进一步，所述速度控制步骤还包括以下步骤：
17.步骤一：检测跟随距离是否小于最小跟随距离，若是，则控制机器人的移动速度为零。
18.当检测到的跟随距离小于最小跟随距离时，此时机器人与被跟随人员之间的距离已经比较接近了，若此时被跟随人员还在进行减速，则机器人便会和被跟随人员发生碰撞，因此在机器人与被跟随人员之间的距离小于最小跟随距离时，控制机器人的速度为零，停止移动，防止机器人与被跟随人员发生碰撞。
19.进一步，所述速度控制步骤还包括以下步骤：
20.步骤二：设置极限跟随距离，所述极限跟随距离大于最大跟随距离，检测跟随距离是否大于最大跟随距离且小于极限跟随距离，若是，则控制机器人的移动速度为机器人的最大移动速度；
21.步骤三：检测跟随距离是否大于极限跟随距离，若是，则控制机器人的移动速度为零。
22.当机器人与被跟随人员之间的距离处于最大跟随距离与极限跟随距离之间时，说明机器人与被跟随人员之间的距离已将较大，因此在此时控制机器人保持最大的移动速度进行移动，而当机器人保持最大移动速度后，若机器人和被跟随人员之间的距离还在变大，
直到跟随距离大于极限跟随距离，说明此时机器人已经跟不上被跟随人员了，若机器人继续保持移动，却丢失与被跟随人员之间的信号，没有行动的目标和方向，会导致与障碍物发生碰撞，造成机器人损坏，因此控制机器人的移动速度为零，停止移动。
23.进一步，还包括警告提醒步骤：当跟随距离大于极限跟随距离，机器人停止移动时记录暂停时间，根据预设的时间阈值，在停止时间大于时间阈值后，机器人发出提醒警告。
24.当机器人与被跟随人员之间的距离超出极限跟随距离，且超出的时间大于时间阈值时，说明被跟随人员没有发现机器人已经跟丢，因此在此时发出提醒警告，告知被跟随人员此时机器人已经跟丢，防止机器人丢失。
25.进一步，所述速度控制步骤还包括以下步骤：
26.步骤四：机器人由于跟随距离大于极限跟随距离而暂停移动后，此时，若跟随距离变化为小于极限跟随距离，机器人依旧保持移动速度为零，直到跟随距离处于最小跟随距离所在的子距离区间段时，机器人的移动速度才变化为该子距离区间段所对应的移动速度。
27.当跟随距离大于极限跟随距离后，机器人便会暂停移动，而此时若跟随距离变化为小于极限跟随距离的话，说明此时被跟随人员已返回寻找机器人，但是若是在被跟随人员返回过程中，跟随距离一小于极限跟随距离，机器人便启动，那么机器人的移动速度将直接达到最大移动速度，这样便会造成机器人发生猛冲。因此，为了避免这种情况的发生，只有当被跟随人员与机器人之间的距离达到最小跟随距离所在的子距离区间段时，机器人才开始以该子距离区间段所对应的速度继续行驶，因为该子距离区间段的机器人的移动速度最小，且该子距离区间段的所对应的机器人的移动速度，也是当机器人与被跟随人员之间的距离小于最小跟随距离暂停移动之后，所起步的速度，因此该移动速度不会造成机器人发生猛冲的情况。
28.进一步，还包括以下步骤：
29.变化记录步骤：每当机器人的移动速度随着跟随距离发生变化时，进行一次记录，并设置一个周期，统计一个周期内机器人移动速度变化的总次数；
30.重置区间步骤：根据预设的变化次数阈值，变化次数阈值数为子距离区间段数的两倍，当一个周期内机器人移动速度变化的总次数大于变化次数阈值时，重新划分子距离区间段，且重新划分的子距离区间段的个数小于重新划分前的子距离区间段的个数。
31.机器人的移动速度每进行一次变化便进行一次记录，在一个周期内，机器人的移动速度变化次数大于预设的变化次数阈值时，重新划分子距离区间段。因为变化次数阈值数为子距离区间段数的两倍，所以，当一个周期内机器人的移动速度的变化次数大于变化次数阈值时，此时机器人必定时在加速和减速之间来回摇摆，从而反映了此时被跟随人员的移动速度也是忽快忽慢，因此在此时，重新划分子距离区间段，使重新划分的子距离区间段的个数小于之前的子距离区间段的个数，在被跟随人员的移动速度忽快忽慢时，减少机器人的移动速度变化的频率。
32.进一步，还包括以下步骤：
33.变化检测步骤：检测跟随距离是否是在两个相邻的子距离区间段内来回变化；
34.速度重置步骤：当检测到跟随距离在两个相邻的子距离区间段内来回变化时，将这两个子距离区间段对应移动速度较小的子距离区间段记为略小子区间，将对应移动速度
较大的子距离区间段记为略大子区间，增加略小子区间所对应的移动速度，且增加后的移动速度小于略大子区间的移动速度；
35.重复变化检测步骤和速度重置步骤，直到跟随距离不再在这两个相邻的子距离区间段内来回变化。
36.检测机器人与被跟随人员之间的距离是否是在两个相邻子距离区间段内来回变化，若是，则说明此时被跟随人员的移动速度大于机器人在略小子区间所对应的移动速度，小于在略大子区间所对应的移动速度，当机器人与被跟随人员之间的距离处于略小子区间时，由于此时被跟随人员的移动速度大于机器人的移动速度，所以机器人与被跟随人员之间的距离便会增加，来到略大子区间，而跟随人员的移动速度却又小于机器人在略大子区间所对应的移动速度，导致机器人与被跟随人员之间的距离会缩短，来到略小子区间，如此反复横跳，因此在此时增加机器人在略小子区间所对应的移动速度，直到跟随距离不再在这两个子距离区间段内来回变化。
附图说明
37.图1为本发明实施例一中一种矿用救援跟随机器人跟随速度控制方法的流程示意图。
具体实施方式
38.下面通过具体实施方式进一步详细说明：
39.实施例一
40.实施例一基本如附图1所示：
41.本实例的一种矿用救援跟随机器人跟随速度控制方法包括有距离检测步骤：距离分段步骤和速度设置步骤、距离检测步骤和速度控制步骤。
42.在机器人上设置距离检测器，包括第一距离检测器和第二距离检测器，分别设置在机器人的左右两端，被跟随人员身上佩带有移动标签，包括第一标签和第二标签，分别佩带在被跟随人员的左右手的手腕上，在本实施例中，距离检测器为uwb基站，移动标签为手腕型微标签。
43.在跟随过程中，第一距离检测器实时检测与第一标签之间的距离未作第一距离，第二距离检测器实时检测与第二标签之间的距离作为第二距离，然后将同一时刻的第一距离和第二距离取平均值，从而得到机器人与被跟随人员之间的跟随距离。因为人在移动时左右手的前后摆幅比较同步且均衡，通过取连个速度的平均值作为机器人与被跟随人员之间的距离，相对来说也较为准确，减少因行走时身体摆动产生的误差。
44.机器人设有与被跟随人员之间的最大跟随距离和最小跟随距离，并将最大跟随距离和最小跟随距离之间的区间等分为若干个子距离区间段，每个子距离区间段对应一个相同的速度，且子距离区间段越大，机器人的移动速度越快，机器人根据所检测到的跟随距离所处于的子距离区间段，控制跟随时行驶的速度。
45.在本实施例中，最小跟随距离为0.6m，最大跟随距离为2.1m，所划分有五个子距离区间端，第一子距离区间段为0.6m至0.9m，第二子距离区间段为0.9m至1.2m，第三子距离区间段为1.2m至1.5m，第四子距离区间段为1.5m至1.8m，第五子距离区间段为1.8m至2.1m。当
跟随距离处于第一子距离区间段时，机器人的移动速度为0.3m/s，当跟随距离处于第二子距离区间段时，机器人的移动速度为0.6m/s，当跟随距离处于第三子距离区间段时，机器人的移动速度为0.9m/s，当跟随距离处于第四子距离区间段时，机器人的移动速度为1.2m/s，当跟随距离处于第五子距离区间段时，机器人的移动速度为1.5m/s。在本技术的其他实施例中，子距离区间段的段数和每个子距离区间段的长度以及每个子距离区间段所对应的机器人的移动速度可以根据需求自行设置。
46.具体的，例如在跟随过程中，机器人与被跟随人员之间的距离为1m，处于第二子距离区间段内，机器人的移动速度便为0.6m/s，此时只要机器人与被跟随人员之间的距离保持在第二子距离区间段的0.9m至1.2m内，机器人的移动速度都不会发生变化，保持在0.6m/s，所以只要被跟随人员与机器人保持较均匀的速度移动，机器人的移动速度便不会发生变化。若被跟随人员加速前进，在他和机器之间的距离达到1.2m之前，机器人都还是保持0.6m/s的速度行驶，而当距离大于1.2m后，机器人与被跟随人员之间的距离达到了第三子距离区间段，此时机器人的移动速度便改变为第三子距离区间段所对应的速，保持0.9m/s的速度进行行驶，若此时机器人的移动速度能够跟随被跟随人员的速度，那么机器人与被跟随人员之间的距离变能够保持在第三子距离区间段内，若此时机器人的移动速度还是跟不上被跟随人员的移动速度，那么当机器人与被跟随人员之间的距离达到1.5m前时，机器人的移动速度还是保持0.9m/s进行移动，而达到1.5m后，来到第四距离区间段，机器的移动速度便改变为第四子距离区间段对应的1.2m/s，被跟随人员在行动时进行减速也是同理，便不再进行赘述。通过对机器人的移动速度进行阶梯式的调速，当被跟随人员的移动速度发生轻微变化时，不会改变机器人的速度，只有当本跟随人员的移动速度发生较大变化，或者是轻微加速或减速的持续时间较长时，才会改变机器人的移动速度，从而防止了机器人在跟随过程中不断地进行速度调节。并且在被跟随人员的移动速度发生剧烈变化时，机器人的移动速度也是按照阶梯式地一级一级地进行变化，不会发生卡顿或者猛冲的现象。
47.而当检测到机器人与被跟随人员之间的距离在0.6m以内时，此时距离已经小于第一子距离区间段，若此时机器人还保持0.3m/s的速度进行行驶，而被跟随人员还在进行减速，那么机器人便会与被跟随人员发生碰撞，因此设置此时机器人的移动速度为零，使机器人暂停移动，直到机器人与被跟随人员之间的距离大于0.6m，达到第一子距离区间段后，机器人在继续行驶，防止机器人与被跟随人员发送碰撞。
48.此外，机器人设有极限跟随距离，在本实施例中，机器人的极限跟随距离为5m，当机器人与被跟随人员之间的距离处于最大跟随距离的2.1m至极限跟随距离的5m之间时，机器人保持其最大移动速度进行行驶，在本实施例中，机器人的最大移动速度为2m/s。因为在跟随距离处于最大跟随距离与极限跟随距离之间时，说明此时被跟随人员的移动速度也较快，所以机器人也保持最大移动速度进行行驶。而当机器人与被跟随人员之间的距离大于极限跟随距离时，便控制机器的速度为零，停止移动。因为此时即使机器人保持最大移动速度，还是无法跟上被跟随人员，说明被跟随人员的移动速度已经大于机器的最大移动速度，机器人与被跟随人员之间的距离只会变得越来越大，若机器人继续保持移动，却丢失与被跟随人员之间的信号，失去了移动的方向和目标，很容易与障碍物发送碰撞，并且此时机器人的移动速度为最大移动速度，发生碰撞的话产生的损坏也较大，因此使机器人暂停移动。而当机器人因为跟随距离超过极限跟随距离而暂停移动后，便开始记录暂停时间，并根据
预设的时间阈值，在本实施例中，时间阈值为5秒。若在5秒内，被跟随人员发现机器人已经跟丢，那么当他会返回寻找，返回过程中，机器人与被跟随人员之间的距离首先会恢复到极限跟随距离内，但是此时依旧控制机器人的速度为零，直到机器人与被跟随人员之间的距离达到最小跟随距离所在的第一子距离区间，也就是说，当机器人与被跟随人员之间的距离达到0.9m以内时，机器人才开始以0.3m/s的速度继续移动。因为若是机器人与被跟随人员之间的距离一小于极限跟随距离就继续移动的话，那么机器人的移动速度会立刻达到2m/s的最大移动速度，便会出现猛冲的现象，而这也正是本发明所尽量避免的。所以只有当机器人与本跟随人员之间的距离来到了第一子距离区间，机器人才恢复移动。并且第一子距离区间所对应的移动速度，也是机器人因跟随距离小于最小跟随距离所暂停后再次起步的速度，因此该速度很适合机器人进行起步。而若在5秒后，机器人与被跟随人员之间的距离还是大于极限跟随距离，说明被跟随人员并没有发现机器人跟丢并返回寻找，因此机器人发出提醒警告来提醒被跟随人员，在本实施例中，机器人通过喇叭发出提示音来进行提醒。
49.实施例二
50.本实施例和实施例一的区别在于：
51.在跟随过程中，每当机器人的移动速度随着跟随距离发送变化时，便进行一次记录，并设置一个周期，统计一个周期内机器人移动速度变化的总次数。在本实施例中，周期设置为30秒。并且预设有变化次数阈值，变化次数阈值数量为子距离区间段数的两倍，因此在本实施例中，变化次数阈值为10次。而当机器人的移动速度在30秒内变化了10次后，由于子距离区间段数为五段，说明此时机器人并非是一直在加速或是一直在减速，而是时而加速，时而减速，从而放映了被跟随人员此时的移动速度就是忽快忽慢，所以也导致了机器人的速度频繁发生变化。因此，在此时对子距离区间段进行重新设置，且重置后的子距离区间段数小于之前的子距离区间段数。在本实施例中，将子距离区间段数重新设置为三个，具体的，重置后的第一子距离区间段为0.6m至1.1m，第二子距离区间段为1.1m至1.6m，第三子距离区间段1.6m至2.1m。并且重置后的各个子距离区间段所对应的机器人的移动速度为与当前子距离区间段有交集的重置前的子距离区间段所对应机器人移动速度的平均速度，例如重置后的第一子距离区间段与重置前的第一子距离区间段和第二子距离区间段存在交集，则重置后的第一子距离区间段所对应的速度便为，重置前第一子距离区间段对应机器人的移动速度(0.3m/s)和重置前第二子距离区间段对应机器人的移动速度(0.6m/s)的平均数，因此重置后的第一子距离区间段对应机器人的移动速度为0.45m/s。而重置后的第二子距离区间段与重置前的第二子距离区间段、第三子距离区间段和第三子距离区间段均存在交集，因此去这三个子距离区间段所对应机器人移动速度的平均速，为0.9m/s，同理得到重置后的第三子距离区间段所对应的机器人的移动速度为1.35m/s。从而达到在被跟随人员速度频繁变化的情况下，防止机器人的速度也频繁发生变化。
52.实施例三
53.本实施例和实施例一的区别在于：
54.在机器人的跟随过程中检测机器人是否是在两个相邻的子距离区间段内来回变化，若是，则将这两个子距离区间段对应移动速度较小的子距离区间段记为略小子区间，将对应移动速度较大的子距离区间段记为略大子区间，增加略小子区间所对应的移动速度，
且增加后的移动速度小于略大子区间的移动速度；
55.重复上述步骤，直到跟随距离不再在这两个相邻的子距离区间段内来回变化。
56.具体的，例如在跟随过程中，被跟随人员的移动速度保持在0.8m/s左右，而当机器人与被跟随人员之间的距离在第二子距离区间段时，会保持0.6m/s的速度进行移动，由于第二子距离区间段对应的机器人移动速度小于被跟随人员的移动速度，所以机器人与被跟随人员之间的距离会逐渐增大，而到跟随距离达到第三子距离区间段后，机器人将保持0.9m/s的速度行驶，此时机器人的移动速度大于被跟随人员的移动速度，因此机器人与被跟随人员之间的距离会逐渐减小，从而跟随距离又会回到第二子距离区间段，如此反复。
57.因此当检测到机器人与本跟随人员之间的距离在两个子距离区间内来回变化时，增加略小子区间段所对应的移动速度。在本实施例中，达到所述来回变化的标准时跟随距离在10s内在两个子距离区间段来回变化达到3次以上，满足这个标准后，便将略小子距离区间对应的速度增加0.1m/s，若仍然满足来回变化标准，则再次增加，直到不再满足来回变化的标准为止。
58.以上的仅是本发明的实施例，方案中公知的具体结构及特性等常识在此未作过多描述，所属领域普通技术人员知晓申请日或者优先权日之前发明所属技术领域所有的普通技术知识，能够获知该领域中所有的现有技术，并且具有应用该日期之前常规实验手段的能力，所属领域普通技术人员可以在本技术给出的启示下，结合自身能力完善并实施本方案，一些典型的公知结构或者公知方法不应当成为所属领域普通技术人员实施本技术的障碍。应当指出，对于本领域的技术人员来说，在不脱离本发明结构的前提下，还可以作出若干变形和改进，这些也应该视为本发明的保护范围，这些都不会影响本发明实施的效果和专利的实用性。本技术要求的保护范围应当以其权利要求的内容为准，说明书中的具体实施方式等记载可以用于解释权利要求的内容。