一种移动机器人导航跟踪方法与流程

1.本发明涉及移动作业机器人室内外导航运动控制领域，更具体涉及一种移动机器人导航跟踪方法。


背景技术：

2.随着社会的快速发展和劳动力成本的逐步提升，移动机器人成为了目前机器人的热门研究方向之一，并且广泛应用在多个领域，比如室外的割草机、电力巡检、智能快递，室内的自主扫地机器人、机房巡检等。移动机器人可以多机全天候交替工作，极大弥补了人员不足的情况，除此之外，也可以极大避免由于人员失误或者疏忽大意而导致的一系列经济损失和安全事故，间接的保障了人员安全。
3.在实际场景应用中，移动机器人需要完成预先设定的工作任务，具体的每一项工作任务布置在指定的路径点上，因此控制移动机器人精准运动执行到上述的路径点位置是机器人导航任务的重中之重。传统的路径导航跟踪算法首先进行路径的拟合规划，根据车体的性能参数，路径点间的位置关系规划出一条预设轨迹，随后控制移动机器人进行轨迹跟踪。但是这种跟踪控制过程中，如果预设的轨迹存在偏差，那么就会导致机器人轨迹跟踪误差，从而最终导致机器人不能到达目标位置进行指定任务操作。
4.中国专利授权公告号cn104635730b，公开了一种机器人自主充电方法，计算所有激光点在机器人坐标系中的坐标；取每三个激光点为一组，分别计算激光点之间的距离结合此三点的激光测量值；判断是否对激光点进行点集分割；对可以分割的激光点进行分割，得到分组后的点集；将分组后的每组点集分割为两组，对每组点集进行最小二乘法的拟合，获得直线地图集合；根据点集与直线地图集合所包含的参数，判断是否与充电站的特征匹配；匹配的结果为只留有与充电站特征匹配的两条直线，根据两条直线方程以及充电站在全局坐标中的位置，计算出机器人在全局坐标中的位姿；机器人根据实时位姿与充电站前方的目标点进行距离偏差，角度偏差的计算；判断机器人角度偏差是否小于阈值9；当角度偏差大于阈值9时，机器人原地旋转180
°
并向后行走一段距离与充电站对接；当角度偏差小于阈值9时，由增量式pid算法给出机器人的角速度，而线速度由角速度的反比来进行计算；将机器人的线速度、角速度转化为电机转速发送给电机；进行与充电站的对接；判断是否充电成功；如果是，则充电完成；如果否，则重复此步骤。提高了机器人充电的准确性，但是其没有给出具体如何修正预设轨迹存在的误差，并不能保证对接的准确性，导致可能存在对接失败也即不能到达目标位置进行指定任务操作。


技术实现要素：

5.本发明所要解决的技术问题在于现有技术移动机器人导航跟踪方法难以修正预设的轨迹偏差，导致机器人轨迹跟踪误差，从而最终导致机器人不能到达目标位置进行指定任务操作。
6.本发明通过以下技术手段实现解决上述技术问题的：一种移动机器人导航跟踪方
法，所述方法包括：
7.步骤一：将移动机器人当前位置、当前方向以及移动机器人初始控制指令角速度和初始线速度代入移动机器人运动模型，获取特定时间下移动机器人预估的目标位置a和预估的目标方向；
8.步骤二：获取移动机器人预估的目标位置和预估的目标方向相对实际位置以及实际方向的角度偏差以及距离偏差，根据角度偏差获取第一角速度控制量，根据距离偏差获取第二角速度控制量；
9.步骤三：根据移动机器人预估的目标位置和预估的目标方向获取移动机器人的初始线速度下的第三角速度控制量；
10.步骤四：利用第一角速度控制量、第二角速度控制量以及第三角速度控制量计算拟合角速度控制量，根据拟合角速度控制量对移动机器人的初始控制指令角速度以及初始线速度进行更新，修正预估的目标位置a和预估的目标方向与实际位置以及实际方向之间的偏差；
11.步骤五：利用修正偏差后得到的移动机器人运动模型对特定时间下的目标位置以及目标方向进行预估并按照预估的结果进行导航跟踪控制。
12.本发明避免了预先的轨迹规划步骤，根据移动机器人当前位置、当前方向以及移动机器人初始控制指令角速度和初始线速度得到特定时间下移动机器人预估的目标位置a和预估的目标方向，然后修正移动机器人预估的目标位置和预估的目标方向相对实际位置以及实际方向的偏差，利用修正偏差后得到的移动机器人运动模型对特定时间下的目标位置以及目标方向进行预估并按照预估的结果进行导航跟踪控制，避免产生机器人轨迹跟踪误差，使得机器人能够到达目标位置进行指定任务操作。
13.进一步地，所述步骤二包括：根据移动机器人预估的目标位置a和预估的目标方向，在预估的目标方向上搜寻距离预估的目标位置a最近的路径点b以及路径点b前向最近路径点c，获取移动机器人预估的目标方向和bc朝向的角度偏差ang_err，获取预估的目标位置a到bc所在直线的距离偏差dis_err。
14.更进一步地，所述步骤二中距离偏差dis_err的计算公式为
15.dis_err＝((a
x
‑
b
x
)*(a
y
‑
c
y
)
‑
(a
x
‑
c
x
)*(a
y
‑
b
y
))/sqrt((c
x
‑
b
x
)2 (c
y
‑
b
y
)2)，其中，a点坐标为(a
x
,a
y
)，b点坐标为(b
x
,b
y
)，c点坐标为(c
x
,c
y
)，sqrt为平方根函数；
16.角度偏差ang_err的计算公式为ang_err＝arctan(c
y
‑
b
y
,c
x
‑
b
x
)
‑
ang_car，其中，arctan为反正切函数。
17.更进一步地，根据角度偏差通过pid控制方法对角度偏差乘以比例系数获取第一角速度控制量w1，根据距离偏差通过pid控制方法对距离偏差乘以比例系数获取第二角速度控制量w2。
18.更进一步地，所述步骤三包括：根据在预估的目标方向上搜寻得到的距离预估的位置最近的路径点b，前向搜寻路径点b的远程跟踪点d，通过路径点b和其前后两个路径点计算路径点b的方向角α1，通过路径点d和其前后两个路径点计算路径点d的方向角α2；
19.通过公式r＝l/2*sin((α2‑
α1)/2)获取移动机器人整体运动轨迹的圆弧半径，其中，l为路径点b与其远程跟踪点d连成的线段bd的长度；
20.通过公式w3＝v/r获取移动机器人的初始线速度下的第三角速度控制量，其中v表
示移动机器人的初始线速度。
21.更进一步地，所述步骤三中路径点b的远程跟踪点d的选取方法为：在路径点b的前向搜寻满足搜索点到路径点b的长度小于3倍的移动机器人长度的所有的点中距离路径点b最远的点作为远程跟踪点d。
22.更进一步地，所述步骤四包括：计算第一角速度控制量、第二角速度控制量以及第三角速度控制量的总和作为拟合角速度控制量w，当拟合角速度控制量w小于指定阈值w
th
时，更新移动机器人的初始控制指令角速度为w，初始线速度保持不变，判断是否到达目标路径点，若否，返回执行步骤一至步骤四，若是，停止更新，当拟合角速度控制量w大于等于指定阈值w
th
时，更新移动机器人的初始控制指令角速度为sign(w)*w
th
，更新移动机器人的初始线速度为v*w
th
/abs(w)，判断是否到达目标路径点，若否，返回执行步骤一至步骤四，若是，停止更新。
23.更进一步地，所述步骤五包括：将更新后的移动机器人初始控制指令角速度以及更新后的移动机器人初始线速度代入移动机器人运动模型得到修正偏差后的移动机器人运动模型，利用修正偏差后的移动机器人运动模型对特定时间下的目标位置以及目标方向进行预估并按照预估的结果进行导航跟踪控制。
24.本发明的优点在于：本发明避免了预先的轨迹规划步骤，根据移动机器人当前位置、当前方向以及移动机器人初始控制指令角速度和初始线速度得到特定时间下移动机器人预估的目标位置a和预估的目标方向，然后修正移动机器人预估的目标位置和预估的目标方向相对实际位置以及实际方向的偏差，利用修正偏差后得到的移动机器人运动模型对特定时间下的目标位置以及目标方向进行预估并按照预估的结果进行导航跟踪控制，避免产生机器人轨迹跟踪误差，使得机器人能够到达目标位置进行指定任务操作。
附图说明
25.图1为本发明实施例1所提供的一种移动机器人导航跟踪方法的流程图；
26.图2为本发明实施例1所提供的一种移动机器人导航跟踪方法的导航跟踪示意图。
具体实施方式
27.为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
28.如图1和图2所示，一种移动机器人导航跟踪方法，所述方法包括：
29.step1：设定特定的时间间隔，移动机器人初始线速度，角速度指定阈值w
th
，将移动机器人当前位置、当前方向以及移动机器人初始控制指令角速度和初始线速度代入移动机器人运动模型，获取特定时间下移动机器人预估的目标位置a和预估的目标方向；
30.step2：获取移动机器人预估的目标位置和预估的目标方向相对实际位置以及实际方向的角度偏差以及距离偏差，根据角度偏差获取第一角速度控制量，根据距离偏差获取第二角速度控制量；具体过程为：
31.如图2所示，current_pose表示移动机器人的当前位置，根据移动机器人预估的目
标位置a和预估的目标方向，在预估的目标方向上搜寻距离预估的目标位置a最近的路径点b以及路径点b前向最近路径点c，那么a、b、c三点构成一个三角形，获取移动机器人预估的目标方向和bc朝向的角度偏差ang_err，获取预估的目标位置a到bc所在直线的距离偏差dis_err。
32.距离偏差dis_err的计算公式为
33.dis_err＝((a
x
‑
b
x
)*(a
y
‑
c
y
)
‑
(a
x
‑
c
x
)*(a
y
‑
b
y
))/sqrt((c
x
‑
b
x
)2 (c
y
‑
b
y
)2)，其中，a点坐标为(a
x
,a
y
)，b点坐标为(b
x
,b
y
)，c点坐标为(c
x
,c
y
)，sqrt为平方根函数；距离偏差的计算结果包含正负号，表明移动机器人向左或者向右偏离bc所在直线。
34.角度偏差ang_err的计算公式为ang_err＝arctan(c
y
‑
b
y
,c
x
‑
b
x
)
‑
ang_car，其中，arctan为反正切函数。
35.根据角度偏差通过pid控制方法对角度偏差乘以比例系数获取第一角速度控制量w1，根据距离偏差通过pid控制方法对距离偏差乘以比例系数获取第二角速度控制量w2。实际应用中根据需要可以通过pid对角度偏差以及距离偏差进行比例、积分以及微分控制，本实施例中只是进行比例控制。
36.step3：根据移动机器人预估的目标位置和预估的目标方向获取移动机器人的初始线速度下的第三角速度控制量；具体过程为：
37.继续参阅图2，根据在预估的目标方向上搜寻得到的距离预估的位置最近的路径点b，前向搜寻路径点b的远程跟踪点d，通过路径点b和其前后两个路径点计算路径点b的方向角α1，通过路径点d和其前后两个路径点计算路径点d的方向角α2；
38.通过公式r＝l/2*sin((α2‑
α1)/2)获取移动机器人整体运动轨迹的圆弧半径，其中，l为路径点b与其远程跟踪点d连成的线段bd的长度；
39.通过公式w3＝v/r获取移动机器人的初始线速度下的第三角速度控制量，其中v表示移动机器人的初始线速度。
40.其中路径点b的远程跟踪点d的选取方法为：在路径点b的前向搜寻满足搜索点到路径点b的长度小于3倍的移动机器人长度的所有的点中距离路径点b最远的点作为远程跟踪点d。
41.需要说明的是，在已知路径点的情况下计算方向角属于现有技术，在此不做赘述。
42.step4：利用第一角速度控制量、第二角速度控制量以及第三角速度控制量计算拟合角速度控制量，根据拟合角速度控制量对移动机器人的初始控制指令角速度以及初始线速度进行更新，修正预估的目标位置a和预估的目标方向与实际位置以及实际方向之间的偏差；具体过程为：
43.计算第一角速度控制量、第二角速度控制量以及第三角速度控制量的总和作为拟合角速度控制量w，当拟合角速度控制量w小于指定阈值w
th
时，更新移动机器人的初始控制指令角速度为w，初始线速度保持不变，判断是否到达目标路径点，若否，返回执行step1至step4，若是，停止更新，当拟合角速度控制量w大于等于指定阈值w
th
时，更新移动机器人的初始控制指令角速度为sign(w)*w
th
，更新移动机器人的初始线速度为v*w
th
/abs(w)，判断是否到达目标路径点，若否，返回执行step1至step4，若是，停止更新。
44.step5：将更新后的移动机器人初始控制指令角速度以及更新后的移动机器人初始线速度代入移动机器人运动模型得到修正偏差后的移动机器人运动模型，利用修正偏差
后的移动机器人运动模型对特定时间下的目标位置以及目标方向进行预估并按照预估的结果进行导航跟踪控制。
45.通过以上技术方案，本发明避免了预先的轨迹规划步骤，根据移动机器人当前位置、当前方向以及移动机器人初始控制指令角速度和初始线速度得到特定时间下移动机器人预估的目标位置a和预估的目标方向，然后修正移动机器人预估的目标位置和预估的目标方向相对实际位置以及实际方向的偏差，利用修正偏差后得到的移动机器人运动模型对特定时间下的目标位置以及目标方向进行预估并按照预估的结果进行导航跟踪控制，避免产生机器人轨迹跟踪误差，使得机器人能够到达目标位置进行指定任务操作。
46.以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。