一种自移动设备探地路径规划方法与流程

1.本发明涉及路径规划及探测领域，尤其涉及适用于坝体安全检测用的一种自移动设备探地路径规划方法。


背景技术：

2.大坝ct技术是计算机层析成像技术在大坝安全监测中的应用，利用一定频段的电磁波在坝体中传播，形成探测区内部切面探测数据并利用计算机进行分析处理，以定量地反映坝体磁疗性质分布和老化情况、病害及缺陷部位。目前，常见的大坝ct技术存在问题如下：
3.(一)通常采用人工配合探地雷达进行勘探，探测效率低且人工成本高，费时费力，尤其针对包含平面、迎水面和背水面两个斜面的坝体，施工、探测难度大，且存在安全隐患。
4.(二)如何将智能无人车探地与大坝ct技术结合是本行业研发人员的重要研究方向，即使采用智能无人车探地，由于存在恶劣环境或复杂探测场景，若不对探地路径进行规划，则会降低智能无人车自主探测的安全性和可靠性，因此如何保证智能无人车在探地过程中能规划探地路径并且在探地过程中实现对障碍物检测、车辆打滑等情况的处理是本领域研究的重点方向。


技术实现要素：

5.针对上述现有技术的缺点，本发明的目的是提供一种自移动设备探地路径规划方法，相对于现有技术来说，本发明将探地雷达与智能无人车结合进行设计，集合探地雷达的勘探功能和智能无人车的自主巡查功能，对在线标定地图边界范围内的区域进行自适应幅宽的路径规划，同时可以处理在探地过程中出现的动态障碍物、打滑情况，本发明提出的路径规划方法，能够提高全覆盖式巡查作业的覆盖率和工作效率，并能适应复杂应用工作场景。
6.为实现上述目的，本发明的技术方案如下：
7.一种自移动设备探地路径规划方法，包括以下步骤：
8.创建地图信息并生成地图数据；
9.自适应探测作业幅宽；
10.规划作业路径及构成回充路径；
11.根据作业路径执行探地任务；
12.探地任务执行完成后，根据所述回充路径移动至起始位置。
13.可选的，在所述的一种自移动设备探地路径规划方法中，所述创建地图信息包括步骤如下：
14.同一属性下的地图数据三维信息存入vector容器，并标记相应属性；
15.不同属性的地图数据信息存入结构体，标记为path_map。
16.可选的，在所述的一种自移动设备探地路径规划方法中，所述自适应设定探测作
业幅宽包括步骤如下：
17.遍历path_map，查找所有边界点三维坐标信息；
18.根据所述三维坐标信息计算地图坡度；
19.设定第一探测幅宽；
20.根据计算所得地图坡度调整第一探测幅宽，以获取适配该地图坡度的第二探测幅宽；
21.存储所述第二探测幅宽。
22.可选的，在所述的一种自移动设备探地路径规划方法中，所述规划作业路径及构成回充路径包括步骤如下：
23.建立栅格地图和代价地图；
24.以代价地图矩形包络某个顶点作为基础三维坐标值；
25.从基础三维坐标值开始，按照规划初始路径集合，并储存形成具有多个三维坐标值的数据对结构；
26.依次通过规划算法连接初始路径集合中每个数据对的第二个三维坐标值和下一个数据对中的第一个三维坐标值，构成具有多个路径段的规划路径；
27.以初始路径集合中最后一个三维坐标值为起点，设备当前所在三维坐标值为终点，通过规划算法连接并生成回充路径；
28.将所有规划的路径点按序存入vector容器，完成全局路径规划。
29.可选的，在所述的一种自移动设备探地路径规划方法中，所述初始路径规划包括步骤如下：
30.从基点开始，按照一列递增查找，一列递减查找的方式，通过栅格重复查找出可达区域内的所有可达点，基于此获取到初始路径集合。
31.可选的，在所述的一种自移动设备探地路径规划方法中，所述构成具有多个路径段的规划路径包括步骤如下：
32.判断每个路径段起止点是否会产生碰撞；
33.若无碰撞，生成当前路径段；
34.若有碰撞，重新规划当前路径段为第二路径段；
35.第二路径段的规划如下：
36.缩短当前路径段数据对中起点坐标与终点坐标之间的间距形成第二路径段；
37.将该第二路径段更新至路径集合；
38.判断该第二路径段中起点与终点是否有碰撞；
39.若无，生成当前路径段；
40.若有，重新规划第n路径段直至无碰撞出现。
41.可选的，在所述的一种自移动设备探地路径规划方法中，所述动态避障的处理包括步骤如下：
42.实时获取自移动设备当前控制点前方设定长度内的规划路径作为感知路径、自移动设备周围点云数据；
43.根据所述点云数据并依据聚类算法获取动态障碍物检测框信息；
44.更新代价地图；
45.判断感知路径中各路径点是否会与动态障碍物检测框产生碰撞；
46.若产生碰撞，获取安全点，根据所述安全点和控制点规划避障路径；
47.若未产生碰撞，实时更新感知路径并继续获取点云数据及障碍物检测框信息进行碰撞检测。
48.可选的，在所述的一种自移动设备探地路径规划方法中，所述感知路径获取包括步骤如下：
49.获取车辆当前控制点及其所在作业路径的起点及终点；
50.判断控制点距离所在作业路径中终点的距离与所设定的参数值之间的关系，并更新感知路径的长度；
51.重复判断至更新的感知路径长度超过设定阈值或达到作业路径终点。
52.可选的，在所述的一种自移动设备探地路径规划方法中，所述打滑的处理包括步骤如下：
53.设定阈值，判断车辆是否处于打滑状态；
54.若出现打滑，则进行脱困处理；
55.若未产生打滑，继续控制无人车沿全局规划路径正常运行。
56.可选的，在所述的一种自移动设备探地路径规划方法中，所述脱困处理包括步骤如下：
57.以打滑点为圆心，根据设定半径值作圆，并在圆周上选取至少一个位置为打滑后退的终点作为备选点；
58.依次判断所述备选点是否为安全点；
59.若判断没有安全点，更换人工遥控脱困；
60.若判断有安全点，以打滑点为起点，规划从该打滑点至安全点的反向路径，控制自移动设备至该安全点；
61.更新代价地图；
62.查找更新的代价地图后，选取原规划路径上距离打滑点最近的一个安全点，规划备选点至该安全点的额外路径，绕过打滑区域。
63.可选的，在所述的一种自移动设备探地路径规划方法中，所述备选点在圆周上的位置按优先级划分为在所述打滑点的正后方、左后方、右后方、正左侧和正右侧中的任意一个方向。
64.与现有技术相比，本发明的有益技术效果如下：
65.本发明提供的一种自移动设备探地路径规划方法，利用无人车与探地雷达结合，设计自走式探地雷达，可实现全自动化的大坝勘探过程；并提出一种全覆盖式路径规划方法，对在线标定地图边界范围内的区域进行自适应幅宽的路径规划，同时，处理在探地过程中出现的动态障碍物、打滑情况；由此可见，采用本发明可以有效提高全覆盖式巡查作业的覆盖率和工作效率，并提升勘探安全性；可以实时进行动态避障，以提高自主探测的安全性和可靠性；对于打滑情况可以自主脱困，以适应恶劣环境或复杂探测场景。
附图说明
66.图1示出了本发明实施例一种自移动设备探地路径规划方法的流程图。
67.图2示出了本发明实施例一种自移动设备探地路径规划方法中的规划路径示意图。
68.图3示出了本发明实施例一种自移动设备探地路径规划方法中的动态避障方法示意图。
69.图4示出了本发明实施例一种自移动设备探地路径规划方法中的打滑脱困方法示意图。
具体实施方式
70.为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图和具体实施方式对本发明提出的一种自移动设备探地路径规划方法作进一步详细说明。根据下面说明，本发明的优点和特征将更清楚。需要说明的是，附图采用非常简化的形式且均使用非精准的比例，仅用以方便、明晰地辅助说明本发明实施方式的目的。为了使本发明的目的、特征和优点能够更加明显易懂，请参阅附图。须知，本说明书所附图式所绘示的结构、比例、大小等，均仅用以配合说明书所揭示的内容，以供熟悉此技术的人士了解与阅读，并非用以限定本发明实施的限定条件，故不具技术上的实质意义，任何结构的修饰、比例关系的改变或大小的调整，在不影响本发明所能产生的功效及所能达成的目的下，均应仍落在本发明所揭示的技术内容能涵盖的范围内。
71.实施例：
72.一种自移动设备探地路径规划方法，如图1所示，包括以下步骤：
73.步骤s1，创建地图信息并生成地图数据。
74.具体是遥控无人车至作业场景内，以手持遥控器为例，操控遥感控制无人车运动至所需打点位置，通过遥控器搭载的安卓平台内的应用程序创建地图，选择相应地图属性(边界、障碍物、回充路径等)，记录当前位置，并以网络通讯方式传输至无人车搭载的控制器内，生成并存储相应地图数据。所述生成地图数据具体步骤如下：
75.步骤s11，同一属性下的地图数据三维信息存入vector容器，并标记相应属性。
76.步骤s12，不同属性的地图数据信息存入结构体，标记为path_map。
77.步骤s2，自适应探测作业幅宽。
78.具体是依据地图边界信息计算地图坡度，自适应设定探测作业幅宽。所述自适应设定探测作业幅宽具体步骤如下：
79.步骤s21，遍历path_map，查找出作业边界属性下相关边界点三维坐标信息{p1,p2,...,pn}。
80.步骤s22，依据作业边界点三维信息计算地图坡度。具体的，所述计算坡度依据以下公式：
[0081][0082]ai
，
1-3
＝[p
i，x-c
x
,p
i，y-cy,p
i，z-cz],i＝1～n
[0083]
b＝[svd(a)
1,2
,svd(a)
2,2
,svd(a)
3,2
]
[0084]
d＝[0,0,1],
[0085]
slant_angle＝arccoss(v)*180/π
[0086]
其中，p
i,x/y/z
为第i个边界点的x/y/z坐标值，n为边界点个数；a为n
×
3矩阵，第i行第1-3列元素分别为p
i,x-c
x
、p
i,y-cy、p
i,z-cz；通过利用3维空间点pi和中心点c的协方差a，结合svd分解，获取最小特征值所对应的特征向量，求得平面法向量b。其中，svd(a)
i,j
表示a矩阵进行svd分解后所得矩阵的第i行第j列元素；最后，利用平面法向量b与指向z方向的单位向量d之间计算夹角的方式获取地图角度余弦值v，通过反三角函数及弧度转角度公式得到地图坡度slant_angle，具体的，其中，||||表示计算向量模长，*表示向量点积。
[0087]
步骤s23，依据车型车轮间距d设定初始探测幅宽w＝d。
[0088]
步骤s24，根据计算所得地图坡度调整第一探测幅宽，以获取适配该地图坡度的第二探测幅宽。具体的，所述自适应调整幅宽的公式如下：
[0089][0090]
具体的，d为车轮间距，w为探测幅宽，slant_angle为地图坡度。
[0091]
步骤s25，存储所述第二探测幅宽，用于规划全覆盖迂回路径。
[0092]
步骤s3，规划作业路径及构成回充路径，具体如图2所示。具体是根据作业幅宽w，利用混合a*算法规划全覆盖迂回式路径，并连接覆盖路径和起点，构成回充路径。具体的，所述路径规划方法具体步骤如下：
[0093]
步骤s31，依据path_map中作业边界属性下相关边界点三维坐标信息，找到一个矩形包络，将边界点组成的作业地图囊括在内，形成世界地图。
[0094]
步骤s32，利用世界地图，按照设定的地图分辨率r建立栅格地图。
[0095]
步骤s33，进一步的，依据栅格地图建立代价地图，设置作业边界内可达区域代价及属性、作业边界外及边界内障碍物等不可达区域代价值及属性。
[0096]
步骤s34，以代价地图某个顶点作为基点base_point(0,0)，设置代价地图中所有栅格坐标(xi,yi)。
[0097]
步骤s35，从基点开始，按照y方向规划初始路径集合。具体的，所述初始路径规划方法步骤如下：
[0098]
步骤s351，首先，从基点开始，按y递增方向，查找到第一个可达区域的栅格g1，计算栅格点中心坐标p1，放入路径集合第一个数据对第一个三维坐标点位置。
[0099]
步骤s352，按照y递增方向，继续查找下一个栅格g2，若g2为可达点，计算g2中心坐标p2，放入路径集合第一个数据对第二个三维坐标点位置。
[0100]
步骤s353，重复步骤s352，更新第二个三维坐标点位置点坐标为最新的可达点，直到当前查找的y值达到y
max
或查找到不可达属性栅格点。
[0101]
具体的，若当前未查找到y
max
且查找到不可达栅格，则结束一组数据对的查找。继续当前列，查找下一个可达栅格g3，计算栅格点中心坐标p3，放入当前路径集合下一个数据对第一个三维坐标点位置。按照y递增方向，继续查找当前数据对第二个三维坐标点的坐
标。
[0102]
若当前查找到y
max
，表示当前列查找完毕，则当前一组路径集合查找完毕。
[0103]
步骤s354，一列查找完毕后，按照设置的作业幅宽，计算下一列路径的x坐标：x+＝w，并从y＝y
max
开始，按照y递减方向，查找当前列第一个可达栅格gn，计算gn中心坐标pn，放入第二个路径集合第一个数据对第一个三维坐标点位置。
[0104]
步骤s355，按照y递减方向，更新第二个三维坐标点坐标为最新的可达点，直到当前查找的y值达到0或查找到不可达属性栅格点。
[0105]
具体的，若当前未查找到y＝0且查找到不可达栅格点，则结束一组数据对的查找。继续当前列，查找下一个可达栅格gk，计算栅格中心坐标pk，放入当前路径集合下一个数据对第一个三维坐标点位置。按照y递减反向，继续查找当前数据对第二个三维坐标点的坐标。
[0106]
若当前查找到y＝0，表示当前列查找完毕。则当前一组路径查找完毕。
[0107]
步骤s356，继续按照一列递增查找，一列递减查找的方式重复s352-s355，直到x≥x
max
，结束递归，获取到初始路径集合。
[0108]
步骤s36，依据s35所述方式获取初始路径集合后，利用混合a*依次连接路径中每个数据对的第二个三维坐标点和下一个数据对中的第一个三维坐标点，构成规划路径。具体的，构建规划路径的方法步骤如下：
[0109]
步骤s361，依据车辆模型、aabb碰撞检测算法预算每个路径段起止点是否会产生碰撞。
[0110]
步骤s362，具体的，若无碰撞产生，以相应路径段终点、下一路径段起点为规划算法起点和终点，首先尝试直接产生dubins曲线连接起止点，并通过aabb算法检测规划出的连接路径中的路径点是否存在碰撞可能。
[0111]
若无碰撞可能，则产生相应路径，将使用混合a*连接完毕的路径重新按单个路径点三维坐标值的形式按序存入路径容器集合中。
[0112]
若存在碰撞可能，利用混合a*按步长进行一步规划路径尝试后，更新规划起点为混合a*一步规划终点，并递归尝试dubins曲线，直到规划出完整相应路径规划曲线，存入路径容器集合中。
[0113]
步骤s363，具体的，若产生碰撞事实，则按设定参数缩短相应数据对中起点和终点间距，并更新相应路径集合。重复s361、s362，至规划出连接所有路径集合中的路径点，并按序将路径点存入路径容器集合中。
[0114]
步骤s37，以路径中最后一个三维坐标点为起点，车辆当前所在为终点，使用混合a*产生回充路径，添加至原路径中。
[0115]
步骤s38，将所有规划的路径点存入路径容器集合中，完成全局路径规划。
[0116]
步骤s4，根据作业路径执行探地任务。
[0117]
具体的，在执行探地任务过程中，可能存在探测环境复杂的情况，因此需对任务过程中的障碍物进行动态避障，对可能产生的打滑状况进行脱困处理；
[0118]
具体的，对所述动态避障的示意图如图三所示，其具体步骤如下：
[0119]
步骤a1，实时获取无人车当前控制点p1前方l长度内的规划路径作为感知路径l1，并利用激光雷达实时获取无人车车体周围内点云数据。
[0120]
步骤a2，采用地面分割算法、栅格聚类算法，获取动态障碍物检框{o1,o2,...on}。包含障碍物尺寸及三维坐标信息。
[0121]
步骤a3，更新代价地图代价地图，放置动态障碍物检测框{o1,o2,...on}，并设置相应区域内栅格为不可达区域。
[0122]
具体的，为使得规划路径尽可能远离障碍物检测框，设置动态障碍物代价地图代价值时，采用阶梯式代价下降方法。具体为：障碍物检测框内部cost(代价值)设置为不可达cost1(一般设置为最大值255)，障碍物检测框外部，采用类似等高线方式，设置靠近障碍物检测框边界处栅格cost值与障碍物边界距离成反比，使得整体障碍物周围栅格cost值呈现逐层下降趋势，直至cost下降为0(可达区域cost值)。
[0123]
步骤a4，结合具体车辆模型信息，利用aabb检测算法预测判断，模拟无人车控制点达到感知路径l1中的每个感知点时，是否会与障碍物检测框产生碰撞。具体的，所述感知路径获取方法如下：
[0124]
步骤a41，获取车辆当前控制点p1及p1所在全局路径的起止点s1、s2。
[0125]
步骤a42，判断p1距离所在全局路径终点s2的距离d。
[0126]
若d《0.1m，则直接将s2放入感知路径，并更新感知路径长度为l+＝d。
[0127]
若d≥0.1m，则按固定步长s取p1前方s长度，更新感知路径长度l+＝s，并将映射在全局路径上且距离p1长度为s的点s3放入感知路径。
[0128]
步骤a43，更新感知路径最新点为感知路径中最后一个点，重复b2，至感知路径长度超过设定阈值或达到全局路径终点(回充点)。
[0129]
步骤a5，若产生碰撞，则进行碰撞处理。具体的，所述处理步骤如下：
[0130]
步骤a51，通过aabb检测算法查找原全局路径中，在当前控制点p1前方不会产生碰撞的第一个路径点作为安全点p2。
[0131]
步骤a52，利用混合a*及dubins曲线重新规划从当前控制点p1至安全点p2的避障路径l2，并将其作为额外规划路径添加到原全局规划路径中。
[0132]
进一步的，进行混合a*规划，计算每个一步预测规划点的cost值时，将上述计算所得的障碍物cost进行缩放，并加入混合a*判断规划路径点代价值的计算中，使得规划出的避障路径尽可能远离障碍物，避免产生擦碰。
[0133]
步骤a53，在避障过程中，继续实时进行障碍物检测框更新、感知路径获取和碰撞检测。
[0134]
步骤a6，若未产生碰撞，继续控制无人车沿全局规划路径运行，并实时更新感知路径继续进行碰撞检测。
[0135]
具体的，所述打滑处理的示意图如图四所示，其具体步骤如下：
[0136]
步骤b1，利用gps高精度定位与里程计判断无人车是否处于里程计计数不断增加但定位一定时间内未发生明显变化的情况，判断车辆是否处于打滑过程。具体的，所述判断打滑依据以下公式：
[0137]
[0138][0139][0140][0141]
其中：
[0142]
current_odom_pose
x/y/z
为当前里程计所得定位x/y/z坐标；
[0143]
last_odom_pose
x/y/z
为上一时刻里程计所得定位x/y/z坐标；
[0144]
current_gps_pose
x/y/z
为当前gps所得定位x/y/z坐标；
[0145]
last_gps_pose
x/y/z
为上一时刻gps所得定位x/y/z坐标；
[0146]
current_odom_orientation
w/x/y/z
为当前里程计所得四元数的数值；
[0147]
last_odom_orientation
w/x/y/z
为上一时刻里程计所得四元数的数值；
[0148]
current_gps_orientation
w/x/y/z
为当前gps所得四元数的数值；
[0149]
last_gps_orientation
w/x/y/z
为上一时刻gps所得四元数的数值。
[0150]
具体的，判断打滑的条件为：|δo
p-δt
p
|》t1或|δo
o-δto|》t2，其中，t1、t2为设定的判断阈值。判断方法具体为：里程计记录车辆车轮转动状态，若检测到固定时间间隔内车辆车轮持续转动，但车辆本身gps定位未发生改变，则判定为打滑。
[0151]
步骤b2，若出现打滑，则进行脱困处理。
[0152]
具体的，打滑脱困步骤如下：
[0153]
步骤b21，按打滑点p3为圆心，设定长度s为半径的圆周上优先级依次为正后方、左后方、右后方、正左侧、正右侧的点e
1-5
作为打滑后退的终点。具体为，其中优先级最高的后退终点为正后方，最低的为正左/右侧，为方便车辆后退。
[0154]
步骤b22，依次判断上述e
1-5
中是否为安全点。模拟车辆运动至后退终点处，利用aabb碰撞检测车辆模型是否会与地图边界及动态障碍物等产生碰撞。按优先级找到第一个安全点即可。若没有安全点，则相应手机应用提示打滑脱困失败，更换为人工遥控操作脱困。
[0155]
步骤b23，将判断为安全点的e
1/2/3/4/5
设置为路径规划终点，打滑点p3为规划路径起点，利用混合a*和dubins曲线规划从车辆当前打滑点p3到后退安全点的反向路径，并控制车辆运行至后退安全点。
[0156]
步骤b24，更新代价地图，放入打滑点p3为圆心，略小于s长度为半径的圆为障碍物。如图4中灰色区域所示。其中，设置略小于s长度为半径为障碍物，为避免在后退终点e
1/2/3/4/5
处检测出碰撞，导致混合a*算法无法规划出避障路径。
[0157]
步骤b25，查找代价地图更新后原全局规划路径上打滑点p3前方最近的一个安全点p4，将其设置为路径规划终点，利用混合a*和dubins曲线规划从后退终点e
1/2/3/4/5
至安全点p4的额外路径，绕过打滑区域。
[0158]
代价地图更新后，对于打滑区域虚拟出的障碍物，不进行清除，保留至此次任务完成，为避免再次进入此区域造成打滑。
[0159]
步骤b3，若未产生打滑，继续控制无人车沿全局规划路径正常运行。
[0160]
步骤s5，探地任务执行完成后，根据所述回充路径移动至起始位置。
[0161]
以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
[0162]
以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

技术特征：
1.一种自移动设备探地路径规划方法，其特征在于，包括以下步骤：创建地图信息并生成地图数据；自适应探测作业幅宽；规划作业路径及构成回充路径；根据作业路径执行探地任务；探地任务执行完成后，根据所述回充路径移动至起始位置。2.如权利要求1所述的一种自移动设备探地路径规划方法，其特征在于：所述创建地图信息包括步骤如下：同一属性下的地图数据三维信息存入vector容器，并标记相应属性；不同属性的地图数据信息存入结构体，标记为path_map。3.如权利要求2所述的一种自移动设备探地路径规划方法，其特征在于：所述自适应设定探测作业幅宽包括步骤如下：遍历path_map，查找所有边界点三维坐标信息；根据所述三维坐标信息计算地图坡度；设定第一探测幅宽；根据计算所得地图坡度调整第一探测幅宽，以获取适配该地图坡度的第二探测幅宽；存储所述第二探测幅宽。4.如权利要求3所述的一种自移动设备探地路径规划方法，其特征在于：所述规划作业路径及构成回充路径包括步骤如下：建立栅格地图和代价地图；以代价地图矩形包络某个顶点作为基础三维坐标值；从基础三维坐标值开始，规划初始路径集合，并储存形成具有多个三维坐标值的数据对结构；依次通过规划算法连接初始路径集合中每个数据对的第二个三维坐标值和下一个数据对中的第一个三维坐标值，构成具有多个路径段的规划路径；以初始路径集合中最后一个三维坐标值为起点，设备当前所在三维坐标值为终点，通过规划算法连接并生成回充路径；将所有规划的路径点按序存入vector容器，完成全局路径规划。5.如权利要求4所述的一种自移动设备探地路径规划方法，其特征在于：所述初始路径集合规划包括步骤如下：从基点开始，按照一列递增查找，一列递减查找的方式，通过栅格重复查找出可达区域内的所有可达点，基于此获取到初始路径集合。6.如权利要求4所述的一种自移动设备探地路径规划方法，其特征在于：所述构成具有多个路径段的规划路径包括步骤如下：判断每个路径段起止点是否会产生碰撞；若无碰撞，生成当前路径段；若有碰撞，重新规划当前路径段为第二路径段；第二路径段的规划如下：缩短当前路径段数据对中起点坐标与终点坐标之间的间距形成第二路径段；
将该第二路径段更新至路径集合；判断该第二路径段中起点与终点是否有碰撞；若无，生成当前路径段；若有，重新规划第n路径段直至无碰撞出现。7.如权利要求1所述的一种自移动设备探地路径规划方法，其特征在于：所述根据作业路径执行探地任务时还包括处理动态避障，所述动态避障的处理包括步骤如下：实时获取自移动设备当前控制点前方设定长度内的规划路径作为感知路径、自移动设备周围点云数据；根据所述点云数据并依据聚类算法获取动态障碍物检测框信息；更新代价地图；判断感知路径中各路径点是否会与动态障碍物检测框产生碰撞；若产生碰撞，获取安全点，根据所述安全点和控制点规划避障路径；若未产生碰撞，实时更新感知路径并继续获取点云数据及障碍物检测框信息进行碰撞检测。8.如权利要求7所述的一种自移动设备探地路径规划方法，其特征在于：所述感知路径获取包括步骤如下：获取车辆当前控制点及其所在作业路径的起点及终点；判断控制点距离所在作业路径中终点的距离与所设定的参数值之间的关系，并更新感知路径的长度；重复判断至更新的感知路径长度超过设定阈值或达到作业路径终点。9.如权利要求1所述的一种自移动设备探地路径规划方法，其特征在于：根据作业路径执行探地任务时还包括进行打滑处理，所述打滑处理包括步骤如下：设定阈值，判断车辆是否处于打滑状态；若出现打滑，则进行脱困处理；若未产生打滑，继续控制无人车沿全局规划路径正常运行。10.如权利要求9所述的一种自移动设备探地路径规划方法，其特征在于：所述脱困处理包括步骤如下：以打滑点为圆心，根据设定半径值作圆，并在圆周上选取至少一个位置为打滑后退的终点作为备选点；依次判断所述备选点是否为安全点；若判断没有安全点，更换人工遥控脱困；若判断有安全点，以打滑点为起点，规划从该打滑点至安全点的反向路径，控制自移动设备至该安全点；更新代价地图；查找更新的代价地图后，选取原规划路径上距离打滑点最近的一个安全点，规划备选点至该安全点的额外路径，绕过打滑区域。11.如权利要求10所述的一种自移动设备探地路径规划方法，其特征在于：所述备选点在圆周上的位置按优先级划分为在所述打滑点的正后方、左后方、右后方、正左侧和正右侧中的任意一个方向。

技术总结
本发明涉及自移动设备探地路径规划方法，包括创建地图信息并生成地图数据；自适应探测作业幅宽；规划作业路径及构成回充路径；根据作业路径执行探地任务；探地任务执行完成后，根据所述回充路径移动至目标位置；创建地图信息包括：同一属性下的地图数据三维信息存入vector容器，并标记相应属性；不同属性的地图数据信息存入结构体，标记为path_map；自适应设定探测作业幅宽包括步骤如下：遍历path_map，查找所有边界点三维坐标信息；根据所述三维坐标信息计算地图坡度；设定第一探测幅宽；根据计算所得地图坡度调整第一探测幅宽，以获取适配该地图坡度的第二探测幅宽；利用无人车与探地雷达结合，设计自走式探地雷达，可实现坝体内部病害勘探过程。坝体内部病害勘探过程。坝体内部病害勘探过程。


技术研发人员：王敏 尤梦晨 汤冯炜 王烁
受保护的技术使用者：无锡卡尔曼导航技术有限公司南京技术中心
技术研发日：2023.04.26
技术公布日：2023/8/1