一种巡检机器人自主导航运动控制方法

1.本发明属于机器人领域，尤其涉及一种巡检机器人自主导航运动控制方法。


背景技术：

2.为保证客货运车或列车、地铁列车、城轨列车等车辆的安全运行，需在运营时间外对其车底零部件全面检查，包括车底螺栓连接是否松动、轮对部件是否磨损、车底结构件有无裂纹等，进而降低其在运行中的安全隐患，保证车辆的正常运行。传统的车底巡检作业全部采用人工进入检修地沟巡检的方法，但由于其人员劳动强度大，检修效率低且容易导致误检、漏检的问题，目前已开始有车底巡检机器人代替人工巡检员完成车辆的车底巡检作业。
3.车底巡检机器人的工作流程与工作场景十分特殊，通常为：当接收到新的巡检任务，巡检机器人从地面充电站出发，通过升降电梯进入巡检地沟，按要求自动导航到固定点进行数据采集，采集完成后，机器人自主搭乘升降电梯回到地面，并自动回到充电站进行电量补充，重新进入待命状态。该工作场景的最大特点在于工作空间十分狭窄，尤其是在自主进出升降电梯与在巡检地沟内运行的过程中，巡检机器人左右两侧余量仅为3cm-5cm，这要求巡检机器人导航控制算法必须可以在厘米级的尺度上，对体积、质量较大的巡检机器人本体进行精密的控制，此外，为了保证采集到图像数据的质量，巡检机器人在沟道内必须保持直线行驶。然而，传统的巡检机器人一般都是在较为开阔的环境作业，无需做到对机器人本体如此精密的控制。
4.因此，有必要针对该场景研究巡检机器人在巡检地沟内、外的运动控制策略，以解决上述问题。


技术实现要素：

5.根据车底巡检机器人在巡检地沟内、外独特的工作场景，为保证对体积、质量较大的机器人本体在巡检地沟内、外均进行精密的控制，进而安全且高效的完成车辆车底的日常巡检作业。本发明提供一种巡检机器人自主导航运动控制方法。
6.本发明的一种巡检机器人自主导航运动控制方法包括巡检沟外导航运动控制方法和巡检地沟内导航运动控制方法。车底巡检机器人自主导航空间包括巡检沟外与巡检沟内；当接收到新的巡检任务，巡检机器人从地面充电站出发，自主导航到指定位置，为通过升降电梯进入地沟巡检做准备；巡检沟外导航运动控制方法具体如下：
7.步骤1：通过dijkstra算法获取全局导航路径并根据设置的航迹推算时长进行当前最远跟踪点提取，以此作为巡检机器人当前运动的目标点，并根据请求定位数据源确定机器人当前初始位姿，根据初始位姿朝向角与距离最终目标点路径的角度，判断机器人应该向前驱动或向后驱动。
8.步骤2：巡检机器人本体旋转至路径可推算段距离初始位姿角度的平均角度方向。
9.步骤3：利用改进的航迹推算算法进行采样轨迹预测，并通过评价函数选取评价最
高的路径作为巡检机器人的下一步运行状态，实时计算机器人当前的速度指令，如此往复进行航迹推算与速度指令计算，从而保证巡检机器人准确且高效地行驶至最终目标点平移容差范围内。
10.步骤4：巡检机器人本体旋转至最终目标点旋转容差范围内，导航结束。
11.巡检机器人通过升降电梯进入巡检地沟后，将按照指示自主导航到某一固定位置进行巡检作业；巡检地沟内导航运动控制方法具体如下：
12.步骤a：获取目标点距离，梯形加减速预计算线速度控制量。
13.步骤b：通过激光雷达对巡检地沟两侧墙面进行拟合，获取墙面几何特征，并据此构造表示左右墙面的两条直线，同时获取沟内运动距离定位数据。
14.步骤c：动态调整巡检机器人线速度v与角速度w，保持其在巡检地沟内呈直线行驶。
15.步骤d：重复步骤c，直到巡检机器人到达位移容差，导航结束。
16.进一步的，步骤2具体为：首先获取巡检机器人当前航向角，然后在全局路径中寻找航迹推算时长内可预计算的距离最远点，遍历并累加巡检机器人到每个点的直行角度，最后计算这一段距离内角度平均值，即为巡检机器人需要旋转的角度；角度平均值的计算公式如下：
[0017][0018]
式中，为角度平均值，p0为巡检机器人当前在全局路径中的点，p1为巡检机器人在全局路径中寻找航迹推算时长内可预计算的距离最远点，xi为p0点到i点的直行角度。
[0019]
进一步的，步骤3具体为：
[0020]
步骤3.1：确定速度采样空间范围，速度采样空间范围表示为：
[0021]vm
＝{v∈[v
min
,v
max
],w∈[w
min
,w
max
]}
[0022]
其中，v
min
、v
max
为巡检机器人最小、最大线速度，w
min
、w
max
为巡检机器人最小、最大角速度。
[0023]
步骤3.2：启用改进的航迹推算算法进行航迹推算与速度指令计算。
[0024]
巡检机器人在一次航迹推算中经历加速与匀速两个阶段，此时的运动学模型表达式为：
[0025][0026]
其中，x(k)、y(k)、x(k 1)、y(k 1)表示机器人在第k时刻与k 1时刻的位置坐标信息，x
acc
、y
acc
表示加速阶段所行驶距离，x
const
、y
const
表示匀速阶段所行驶距离，v
begin
表示机器人此次航迹推算的初始速度，v
end
表示此次航迹推算的目标搜索速度，t
sum
表示此航迹机器人行驶总的时间，t
acc
表示微分时间，x
acc
、ang
acc
表示加速度微分，ang
begin
表示巡检机器人此次航迹推算初始角度，ang
end
表示巡检机器人此次航迹推算目标搜索角度，v
offset
、ang
offset
表示初始速度与搜索速度之间差值，x
acc
、ang
acc
表示一次微分过程巡检机器人速度变化量，n表示实际速度加速到搜索速度所需要的加速次数。
[0027]
将上述推算出的轨迹代入轨迹评价函数中，根据评价选取一条最优路径，再发布该路径对应的线速度v与角速度w驱动巡检机器人运动；基于巡检机器人的工作场景，轨迹评价函数根据推演航迹与跟踪点的贴合程度以及与全局路径的贴合程度两个指标评分，数学表达式如下：
[0028][0029]
其中，score表示当前推演航迹的得分，α表示推演航迹终点到跟踪点距离在计算得分中所占权重，β表示推演航迹到全局路径的最短距离在计算得分中所占的权重，dist0表示推演航迹终点到跟踪点距离，dist1表示推演航迹到全局路径的最短距离。
[0030]
重复步骤3.1-3.2，直到巡检机器人到达位移容差后停车。
[0031]
进一步的，步骤c具体为：
[0032]
根据前后雷达，计算巡检机器人距离两侧墙体的距离差，以保持两条直线的几何关系呈对称状为目标，通过pid算法计算角速度控制量。
[0033]
以当前行驶里程为依据，采用梯形加减速算法预计算线速度控制量，发布当前预计算的线速度v与角速度w，巡检机器人本体按照发布的线速度v与角速度w运动，即可保证巡检机器人在巡检沟内呈直线行驶。
[0034]
梯形加减速预线速度计算方法如下：
[0035][0036]
约束如下：
[0037]
x1＝x
3-x2[0038]
其中，0≤x≤x1表示机器人处于加速阶段，x1≤x≤x2表示机器人处于匀速阶段，x2≤x≤x3表示机器人处于减速阶段，x3表示机器人距离目标点的里程，max(a,b)为a、b中的最大值，vel
min
为机器人的最小线速度，vel
max
为机器人的最大线速度，x为机器人当前行驶里程，acc
max
为常数。
[0039]
角速度控制量计算方法如下：
[0040][0041]
其中，w
output
(k)表示k时刻巡检机器人的角速度，k
p
、ki、kd表示比例、积分、微分环节的系数，dis
diff
(k)表示k时刻巡检机器人距离两侧墙体的距离差，v
cur
表示巡检机器人k时刻的实线际速度，v
max
表示巡检机器人可能达到的最大线速度。
[0042]
本发明与现有技术相比，其优点为：
[0043]
1)巡检机器人自主导航过程中，可根据初始位姿朝向角与距离最终目标点路径的角度，自主判断机器人应该向前驱动或向后驱动，提高自主导航效率。
[0044]
2)沟外导航在速度空间搜索阶段，将由实际速度经由运动学性能加速到搜索速度的过程考虑到航迹推算中，比传统的航迹推算结果更加精确。
[0045]
3)沟内导航提出了一种虚拟循迹方法，通过激光雷达对巡检地沟两侧墙面进行拟合，构造表示左右墙面的两条直线，以保持两条直线的几何关系呈对称状为目标，通过pid算法计算角速度控制量，保持两条直线的几何关系呈对称状，即可保证巡检机器人在巡检地沟内进行快速且精准的直线行驶，与现有agv循迹方法相比，不需要铺设轨道或电磁循迹线，降低了机器人部署成本。
附图说明
[0046]
图1为巡检地沟外导航运动控制方法流程图。
[0047]
图2为巡检地沟内导航运动控制方法流程图。
[0048]
图3为pid算法的原理框图。
具体实施方式
[0049]
下面结合附图和具体实施方法对本发明做进一步详细说明。
[0050]
本发明的一种巡检机器人自主导航运动控制方法包括巡检沟外导航运动控制方法和巡检地沟内导航运动控制方法。
[0051]
巡检机器人作为系统执行器，搭载了用于空间定位导航的三维激光雷达、惯性测量单元、电机轮速计，基于ros系统，实现巡检机器人的定位与导航。车底巡检机器人自主导航空间包括巡检沟外与巡检沟内；当接收到新的巡检任务，巡检机器人从地面充电站出发，
自主导航到指定位置，为通过升降电梯进入地沟巡检做准备。巡检沟外导航运动控制方法如图1所示，具体如下：
[0052]
步骤1：通过dijkstra算法获取全局导航路径并根据设置的航迹推算时长进行当前最远跟踪点提取，以此作为巡检机器人当前运动的目标点，并根据请求定位数据源确定机器人当前初始位姿，根据初始位姿朝向角与距离最终目标点路径的角度，判断机器人应该向前驱动或向后驱动。
[0053]
步骤2：巡检机器人本体旋转至路径可推算段距离初始位姿角度的平均角度方向。
[0054]
首先获取巡检机器人当前航向角，然后在全局路径中寻找航迹推算时长内可预计算的距离最远点，遍历并累加巡检机器人到每个点的直行角度，最后计算这一段距离内角度平均值，即为巡检机器人需要旋转的角度；角度平均值的计算公式如下：
[0055][0056]
式中，为角度平均值，p0为巡检机器人当前在全局路径中的点，p1为巡检机器人在全局路径中寻找航迹推算时长内可预计算的距离最远点，xi为p0点到i点的直行角度。
[0057]
本实施例中，巡检机器人硬件限制最高速度为1.0m/s，假定机器人按最高速度行驶航迹推算时长，行驶的距离与机器人到全局路径中各点的距离进行比较，即可求出可预计算的最远点；机器人到每个点的直行角度通过ros内置软件包angles中提供的方法求得。
[0058]
步骤3：利用改进的航迹推算算法进行采样轨迹预测，并通过评价函数选取评价最高的路径作为巡检机器人的下一步运行状态，实时计算机器人当前的速度指令，如此往复进行航迹推算与速度指令计算，从而保证巡检机器人准确且高效地行驶至最终目标点平移容差范围内。
[0059]
步骤3.1：根据巡检机器人当前的位姿、目标点的距离、动力学性能、机器人硬件性能、巡检机器人运行场景等因素，确定速度采样空间范围，速度采样空间范围表示为：
[0060]vm
＝{v∈[v
min
,v
max
],w∈[w
min
,w
max
]}
[0061]
其中，v
min
、v
max
为巡检机器人最小、最大线速度，w
min
、w
max
为巡检机器人最小、最大角速度。
[0062]
步骤3.2：启用改进的航迹推算算法进行航迹推算与速度指令计算。
[0063]
由步骤3.1已知巡检机器人的速度搜索空间范围，将此范围细分为多组速度，每一组线速度v与角速度w都对应着一条可行轨迹。本发明提出了一种改进的方法，在航迹推算时，将由实际速度经由运动学性能加速到搜索速度的过程考虑其中，从而实现高精度导航。具体过程为：根据动力学性能，引入微分时间与加速度微分的概念，将机器人由当前速度加速到搜索速度的过程按时间划分为多个微分阶段(微分时间与微分次数视巡检机器人工作需要而定)，巡检机器人在每个微分阶段按不同的速度运动，直到巡检机器人实际速度加速到搜索速度后保持匀速运动，因此最终所推算出的航迹考虑了加速与匀速两个阶段的距离，提高了导航精度。
[0064]
巡检机器人在一次航迹推算中经历加速与匀速两个阶段，此时的运动学模型表达式为：
[0065][0066]
其中，x(k)、y(k)、x(k 1)、y(k 1)表示机器人在第k时刻与k 1时刻的位置坐标信息，x
acc
、y
acc
表示加速阶段所行驶距离，x
const
、y
const
表示匀速阶段所行驶距离，v
begin
表示机器人此次航迹推算的初始速度，v
end
表示此次航迹推算的目标搜索速度，t
sum
表示此航迹机器人行驶总的时间，t
acc
表示微分时间，x
acc
、ang
acc
表示加速度微分，ang
begin
表示巡检机器人此次航迹推算初始角度，ang
end
表示巡检机器人此次航迹推算目标搜索角度，v
offset
、ang
offset
表示初始速度与搜索速度之间差值，x
acc
、ang
acc
表示一次微分过程巡检机器人速度变化量，n表示实际速度加速到搜索速度所需要的加速次数。
[0067]
基于本实施例中机器人的工作场景，测试结果显示，n＝10，t
sum
＝3，t
acc
＝0.1效果最佳。
[0068]
将上述推算出的轨迹代入轨迹评价函数中，根据评价选取一条最优路径，再发布该路径对应的线速度v与角速度w驱动巡检机器人运动；基于巡检机器人的工作场景，轨迹评价函数根据推演航迹与跟踪点的贴合程度以及与全局路径的贴合程度两个指标评分，数学表达式如下：
[0069][0070]
其中，score表示当前推演航迹的得分，α表示推演航迹终点到跟踪点距离在计算得分中所占权重，β表示推演航迹到全局路径的最短距离在计算得分中所占的权重，dist0表示推演航迹终点到跟踪点距离，dist1表示推演航迹到全局路径的最短距离。
[0071]
在本实施例中，α＝1，β＝1效果较佳。
[0072]
重复步骤3.1-3.2，直到巡检机器人到达位移容差后停车。
[0073]
步骤4：巡检机器人本体旋转至最终目标点旋转容差范围内，巡检沟外导航结束。
[0074]
巡检机器人通过升降电梯进入巡检地沟后，将按照指示自主导航到某一固定位置进行巡检作业；巡检地沟内导航运动控制方法如图2所示，具体如下：
[0075]
步骤a：获取目标点距离，梯形加减速预计算线速度控制量。
[0076]
步骤b：通过激光雷达对巡检地沟两侧墙面进行拟合，获取墙面几何特征，并据此构造表示左右墙面的两条直线，同时获取沟内运动距离定位数据。
[0077]
步骤c：动态调整巡检机器人线速度v与角速度w，保持其在巡检地沟内呈直线行驶。
[0078]
根据前后雷达，计算巡检机器人距离两侧墙体的距离差，以保持两条直线的几何关系呈对称状为目标，通过pid算法计算角速度控制量。
[0079]
为兼顾其行驶平稳度与效率，以当前行驶里程为依据，采用梯形加减速算法预计算线速度控制量，发布当前预计算的线速度v与角速度w，巡检机器人本体按照发布的线速度v与角速度w运动，即可保证巡检机器人在巡检沟内呈直线行驶。
[0080]
梯形加减速预线速度计算方法如下：
[0081][0082]
约束如下：
[0083]
x1＝x
3-x2[0084]
其中，0≤x≤x1表示机器人处于加速阶段，x1≤x≤x2表示机器人处于匀速阶段，x2≤x≤x3表示机器人处于减速阶段，x3表示机器人距离目标点的里程，max(a,b)为a、b中的最大值，vel
min
为机器人的最小线速度，vel
max
为机器人的最大线速度，x为机器人当前行驶里程，acc
max
为常数。
[0085]
本实施例测试了多组巡检机器人最小线速度、机器人最大线速度、机器人最大加速度，综合考虑到巡检机器人在该工作场景下的运行平稳度与导航效率，最佳组合为vel
min
＝0.02,vel
max
＝0.8,acc
max
＝0.6。
[0086]
pid算法的原理框图如图3所示，角速度控制量计算方法如下：
[0087][0088]
其中，w
output
(k)表示k时刻巡检机器人的角速度，k
p
、ki、kd表示比例、积分、微分环节的系数，dis
diff
(k)表示k时刻巡检机器人距离两侧墙体的距离差，v
cur
表示巡检机器人k时刻的实线际速度，v
max
表示巡检机器人可能达到的最大线速度。
[0089]
本实施例测试了多组pid系数，机器人在该工作场景下的运行最佳组合为k
p
＝0.2,ki＝0,kd＝0.05。
[0090]
步骤d：重复步骤c，直到巡检机器人到达位移容差，导航结束。
[0091]
为了验证车底巡检机器人运动控制策略的导航精度，我们选择在车体侧方安装激光笔，地面上粘贴胶带，以巡检机器人定位模块当前输出的位姿作为导航目标点，向巡检机器人重复下发导航指令，使得巡检机器人重复由其它点前往该目标点，并在胶带上使用圆珠笔记录停车点。当记录若干个点后，取误差绝对值后求平均值，值越小，则导航精度越高。记录巡检沟外导航和巡检地沟内导航结果分别如表1和表2所示。
[0092]
表1巡检沟外导航试验结果
[0093]
序号目标位姿(m)到达位姿(m)x误差(m)y误差(m)1(10.0,10.0)(9.993,9.992)-0.007-0.008
2(10.0,10.0)(10.008,10.007)0.0080.0073(10.0,10.0)(10.012,10.009)0.0120.0094(10.0,10.0)(10.00,10.00)0.000.005(10.0,10.0)(10.01,10.007)0.010.0076(10.0,10.0)(9.989,9.99)-0.011-0.017(10.0,10.0)(10.012,10.008)0.0120.0088(10.0,10.0)(9.99,9.992)-0.01-0.008
[0094]
表2巡检沟内导航试验结果
[0095]
序号目标位姿(m)到达位姿(m)x误差(m)y误差(m)1(100.0,0)(99.993,-0.003)-0.007-0.0032(100.0,0)(100.008,0.002)0.0080.0023(100.0,0)(100.001,0.002)0.010.0024(100.0,0)(99.994,0.00)-0.0060.005(100.0,0)(99.993,0.003)-0.0070.0036(100.0,0)(99.004,-0.001)0.004-0.0017(100.0,0)(99.998,0.003)-0.0080.0038(100.0,0)(100.01,-0.002)0.01-0.002
[0096]
表1展现了巡检地沟外定位导航x方向与y方向精度测试的结果，其八次打点测得的平均误差为x方向
±
0.01m，y方向
±
0.0085m，且八次导航中均未出现机器人无法到点的情况，验证了沟外定位导航算法的精度与可靠性。表2展现了巡检地沟内定位导航x方向与y方向精度测试的结果，其八次打点测得的平均误差为x方向
±
0.0075m，y方向
±
0.0025m，且八次导航中均未出现机器人无法到点的情况，验证了沟内定位导航算法的精度与可靠性。