基于贪心覆盖与无ESDF差分巡检机器人自动巡检规划方法与流程
本发明涉及,具体涉及基于贪心覆盖与无esdf差分巡检机器人自动巡检规划方法。
背景技术:
1、工地、仓储、安防等作业场景广泛采用差分驱动巡检机器人开展自主巡检作业,该类机器人搭载视觉相机,需要先完成场地内相机观测点位与朝向的覆盖规划,再生成机器人可执行的无碰撞平滑轨迹,依次抵达各观测点位完成图像采集。实际作业环境内部存在大量多边形障碍物,自由空间形态复杂,巡检规划需要同时兼顾相机视觉覆盖效果、点位访问顺序、机器人运动学约束以及避障安全要求。现有巡检规划方案通常将相机覆盖规划与机器人轨迹优化作为两套独立模块处理,容易出现相机朝向评估计算开销大、轨迹优化内存占用高、非完整运动学约束处理效果差等现实问题,难以直接部署在算力有限的地面巡检机器人边缘计算单元上。
2、在学术研究层面,现有公开文献如tankasala等人发表的《beyond shortsightednavigation: merging best view trajectory planning with robot navigation》(arxiv:2408.12513)提出长时域视点规划框架,实现地面机器人巡检场景下观测视点与导航轨迹的联合求解,该方法可以在巡检过程中平衡环境覆盖面积与机器人运动代价,适用于安防、监测类地面机器人任务。但该文献采用传统光线投射逐次评估每一个候选相机朝向的可视覆盖范围,每更换一组朝向均需要重复执行光线投射与栅格统计运算,当候选朝向采样密度较高、场地栅格规模较大时,相机朝向评估环节计算量急剧上升;同时该文献轨迹优化模块没有针对差分驱动机器人的非完整运动特性做针对性设计,优化过程中将平面位置坐标作为独立优化变量,需要引入大权重软约束逼近机器人运动学方程,容易出现优化病态、约束不满足的现象,并且轨迹梯度求解采用显式构造稠密雅可比矩阵的方式,梯度计算复杂度随轨迹采样点数呈平方量级增长,当采样密度提升后计算耗时显著增加,难以满足复杂大场地巡检任务的实时运算需求。
3、国内专利公开号cn117075612a,名称为《一种基于改进可视图的快速路径规划与基于esdf地图的轨迹优化方法》,该专利先通过可视图算法生成初始路径,再构建全局esdf符号距离场地图,根据esdf提供的障碍物距离信息开展轨迹优化,依靠距离场梯度实现轨迹避障,能够输出平滑的机器人运动轨迹,在室内机器人导航领域具备一定实用价值。但是该专利方案存在明显局限:一方面该专利需要针对整张栅格地图预先完整构建全局esdf符号距离场,地图尺寸越大,构建距离场所消耗的内存资源与计算时间就越高,而实际轨迹优化过程中仅会用到少量局部区域的距离信息,大部分距离场计算资源属于无效开销;另一方面该专利并未针对巡检任务开展上层相机贪心覆盖规划,缺少相机点位、朝向的自动求解流程,同时在轨迹优化阶段没有针对差分驱动底盘的轮速、轮加速度硬件极限建立约束,也未采用线性复杂度的梯度求解手段,直接应用于带视觉观测任务的差分巡检机器人时,仍会出现相机部署依靠人工设置、优化计算效率不足、输出轨迹难以匹配底盘实际执行能力的缺陷。
技术实现思路
1、基于上述技术问题,本技术公开了基于贪心覆盖与无esdf差分巡检机器人自动巡检规划方法,包括:
2、将待巡检矩形场地离散化为均匀栅格地图,对每个候选相机部署点执行射线投射,构建该点的可视多边形,得到各候选位置在不同朝向下可覆盖的栅格集合;
3、构建当前残差掩码,将残差掩码按角度离散为角度直方图,将所述角度直方图与视场角对应的矩形窗函数进行循环卷积,通过快速傅里叶变换一次性计算所有候选朝向的覆盖增益值;
4、基于覆盖增益的次模性,以贪心策略逐轮选取边际覆盖增益最大的相机部署位置及对应朝向,直至满足预设覆盖率阈值;对已选定的相机部署点,通过旅行商问题求解确定巡检访问顺序;
5、对巡检路径中每对相邻相机部署点,在se(2)状态格空间中执行a*搜索生成无碰撞引导路径,并采用均匀三次b样条对所述引导路径进行参数化,以弧长控制点、航向控制点作为优化变量,得到初始轨迹;
6、通过梯形积分由轨迹的线速度、航向恢复车体位姿序列,使差分驱动非完整运动学约束在结构上被严格满足,不将平面位置坐标作为独立优化变量;采用伴随递推算法计算目标函数关于采样时刻线速度、航向的梯度,梯度计算复杂度为轨迹采样点数的线性阶;
7、在轨迹优化迭代中,采用多圆模型表示车体轮廓,对检测到碰撞的采样点,从预设无碰撞参考路径向当前碰撞圆圆心发射射线,仅提取局部障碍基点、外法向及安全距离信息;在安全距离范围内施加c¹连续的三次惩罚函数,沿外法向方向生成碰撞消除梯度;通过rebound外循环累积历史碰撞约束,并逐轮放大碰撞惩罚权重,迭代至轨迹全程无碰撞;
8、在优化目标中加入左右轮速度及加速度的二次越限软惩罚项;优化完成后,若轮速度或轮加速度仍超出物理限制,则通过增大轨迹总时间对速度曲线进行等比缩放,直至满足底盘物理约束;
9、机器人执行巡检过程中,对未来预设时段的规划轨迹进行碰撞预检,若检测到新增障碍导致碰撞,则触发局部重规划或急停状态机。
10、优选的,所述构建该点的可视多边形的步骤,包括射线-障碍物线段求交处理与可视多边形轮廓生成两个处理环节;
11、所述射线-障碍物线段求交处理:设定观测点,由观测点向外发射射线,射线方向单位向量为,射线参数方程记为,式中参数;取任意一条障碍物边界线段,线段两个端点为、,线段方向向量,线段参数方程记为,式中参数;定义二维叉积运算,记向量;联立射线与线段的参数方程,通过叉积消元求解交点参数:
12、
13、当时判定射线与障碍物边界线段相互平行;当,同时满足、时,判定射线与障碍物边界线段存在有效交点;单条射线遍历全部障碍物边界线段后,选取参数取值最小的有效交点作为该射线方向上的最近障碍交点;
14、所述可视多边形轮廓生成,通过在观测点处发射一组角度等分的探测射线,获取每一条探测射线对应的最近障碍交点,按照射线发射角度的先后顺序依次连接全部最近障碍交点,形成闭合多边形,得到该观测点对应的可视多边形。
15、优选的,所述构建当前残差掩码,将残差掩码按角度离散为角度直方图的步骤,包括残差掩码构建以及角度直方图统计;
16、所述残差掩码构建:为当前候选相机部署点对应的全向可视掩码,标记栅格是否处于该候选相机的全向可视多边形内部;为已经完成部署的全部相机所对应的并集覆盖掩码,标记栅格是否已经被已部署相机覆盖;为障碍物掩码,标记栅格是否属于障碍物区域;残差掩码表达式为:
17、
18、残差掩码取值为真代表对应栅格属于未被覆盖、非障碍物、且当前候选相机可以观测到的栅格;
19、所述角度直方图统计通过遍历残差掩码取值为真的全部栅格,取栅格中心坐标,计算该栅格中心相对于观测点的方位角;将方位角划分至对应的角度bin,统计每一个角度bin内部所有有效栅格的面积累加值,得到角度直方图。
20、优选的,所述将所述角度直方图与视场角对应的矩形窗函数进行循环卷积,通过快速傅里叶变换一次性计算所有候选朝向的覆盖增益值的步骤,包括配置循环矩形窗,执行基于fft的循环卷积运算求解各个候选朝向对应的覆盖增益;
21、配置循环矩形窗,根据相机视场角fov,计算视场角所占用的角度bin的总数量;设置周期等于角度离散总数量的循环矩形窗,循环矩形窗表达式:
22、
23、式中为循环窗的索引,循环矩形窗取1的索引区间对应相机扇形视场所覆盖的角度bin范围;
24、执行循环卷积运算,在角度直方图与循环矩形窗做循环卷积运算,卷积输出,为相机取第个候选朝向时对应的残余覆盖增益;根据循环卷积定理,通过傅里叶变换实现卷积计算:
25、
26、式中代表快速傅里叶变换,代表逆快速傅里叶变换,代表复数数组按元素逐点相乘;遍历卷积输出全部取值,取最大值对应的索引确定相机最优朝向。
27、优选的,所述对已选定的相机部署点,通过旅行商问题求解确定巡检访问顺序的步骤,包括:
28、以贪心最近邻规则生成开放tsp的初始访问序列,以指定的机器人起始点位作为序列的第一个访问节点;每一步从尚未访问的相机点位集合中,选取距离当前节点欧氏距离最小的点位作为下一个访问节点,直至全部相机点位纳入访问序列;
29、采用2-opt算法对初始访问序列迭代优化,迭代过程中对序列中两组边执行断开、重连接操作,当路径总长度不再下降时终止迭代,输出相机点位的开放巡检访问序列。
30、优选的,所述采用均匀三次b样条对所述引导路径进行参数化,以弧长控制点、航向控制点作为优化变量,得到初始轨迹的步骤,包括采用均匀三次b样条基函数构建轨迹表达式;
31、均匀三次b样条的4个紧支撑基函数:
32、
33、式中为b样条单段内部的局部参数;
34、以弧长控制点、航向控制点作为优化变量,轨迹的弧长随时间变化表达式、航向随时间变化表达式:
35、
36、对b样条基函数做解析求导得到一阶、二阶、三阶导数基,结合单段轨迹的时间间隔,由导数基计算得到轨迹采样时刻对应的线速度、角速度、线加速度、角加速度。
37、优选的,所述通过梯形积分由轨迹的线速度、航向恢复车体位姿序列的步骤,包括:
38、设置固定采样时间间隔,为采样时刻索引,为第采样时刻的线速度,为第采样时刻航向角;定义航向向量;代表第采样时刻机器人二维平面位置;梯形积分迭代式:
39、
40、由初始位置逐时刻迭代,累加位置增量得到全部采样时刻的机器人平面位置;差分驱动运动学约束、由该积分迭代式的结构约束实现。
41、优选的,所述采用伴随递推算法计算目标函数关于采样时刻线速度、航向的梯度的步骤,包括定义位置伴随量、设置边界条件、反向递推伴随量、再由伴随量求解采样点梯度;
42、定义代价函数为,为代价函数对第采样时刻机器人平面位置的梯度;定义位置伴随量,位置伴随量为从采样时刻开始向后所有位置梯度的矢量求和:;
43、设置递推边界条件,采样序列最后一个采样时刻索引记为,边界条件;
44、从序列末尾向序列起始方向反向执行递推计算,伴随量递推关系式:;基于已经求得的伴随量、,计算代价函数对第采样时刻线速度的梯度:,同理计算代价函数对第采样时刻航向的梯度;完成采样时刻梯度计算后,基于b样条的局部紧支撑特性,将采样时刻梯度散射映射到对应的弧长控制点、航向控制点。
45、优选的,所述在安全距离范围内施加c¹连续的三次惩罚函数,沿外法向方向生成碰撞消除梯度;通过rebound外循环累积历史碰撞约束,并逐轮放大碰撞惩罚权重的步骤,包括三次惩罚函数构建与rebound外循环迭代处理;
46、为全部激活的碰撞约束集合;代表轨迹采样索引,代表车体多圆模型内的圆索引;为第采样时刻第个车体圆对应的有向间距,,为车体圆圆心世界坐标,为障碍基点,为障碍外法向单位向量,为预设安全距离;为碰撞惩罚权重;三次惩罚函数:
47、
48、为预设权重增长系数且,为外循环迭代轮次,在rebound外循环每一轮迭代开始前,更新碰撞惩罚权重,将碰撞惩罚权重的取值设置为当前权重与的次幂两者的乘积;将上一轮迭代得到的碰撞约束集合累积至本轮约束集合,执行后端优化,输出本轮迭代轨迹;对轨迹做碰撞检测,更新碰撞约束集合,继续下一轮rebound迭代。
49、优选的,所述在优化目标中加入左右轮速度及加速度的二次越限软惩罚项;优化完成后,若轮速度或轮加速度仍超出物理限制,则通过增大轨迹总时间对速度曲线进行等比缩放的步骤,包括执行轮空间映射、二次越限软惩罚以及时间伸缩处理;
50、为差分机器人轮距,为质心线速度,为质心角速度,为质心线加速度,为质心角加速度;质心运动参数向左右轮运动参数映射公式:
51、
52、针对映射得到的左轮速度、右轮速度、左轮加速度、右轮加速度分别构建二次越限软惩罚项;
53、时间伸缩处理通过计算轮速超限比、轮加速度超限比;基于与计算伸缩因子;完成轨迹时间尺度缩放将轨迹原始总时间 与伸缩因子相乘,得到缩放后的轨迹总时间,完成轨迹时间尺度缩放。
54、相对于现有技术,本技术的技术方案具有以下技术效果:
55、本发明能够完成待巡检场地的相机部署规划,通过射线投射构建可视多边形,根据残差掩码以及fft循环卷积完成相机最优朝向求解,结合贪心策略得到相机部署点位与朝向;可以高效完成场地视觉覆盖求解,输出适配场景障碍物分布的相机配置结果,为后续巡检任务提供可靠的观测点位基础。
56、本发明完成相机点位的访问顺序规划之后,根据se(2)状态格a*搜索得到引导路径,借助均匀三次b样条完成轨迹参数化;通过梯形积分恢复机器人位姿,在结构层面满足差分驱动机器人的非完整运动学约束,无需额外设置约束逼近项,保障生成轨迹符合机器人自身运动特性。
57、本发明采用伴随递推的方式完成梯度计算,实现梯度计算复杂度与轨迹采样点数呈线性关系;同时不构建全局esdf距离场,仅提取碰撞位置局部障碍信息完成碰撞惩罚迭代优化,能够有效消除轨迹碰撞,减少规划过程的计算资源占用,提升轨迹优化环节的运算效率。
58、本发明在优化过程中引入轮空间约束,并通过时间伸缩对轨迹做尺度调整,结合运行阶段的碰撞预检实现在线重规划;使得输出轨迹适配差分机器人底盘硬件能力,机器人执行过程中可应对环境变化,保障整体巡检作业能够稳定可靠运行。
59、上述说明仅是本技术技术方案的概述,为了能够更清楚了解本技术的技术手段,从而可依照说明书的内容予以实施,并且为了让本技术的上述和其他目的、特征和优点能够更明显易懂,以下以本技术的较佳实施例并配合附图详细说明如后。
60、根据下文结合附图对本技术具体实施例的详细描述,本领域技术人员将会更加明了本技术的上述及其他目的、优点和特征。
版权声明:本站专利文献数据主要来源于国家知识产权局等公开渠道,由 技术新讯 进行整理汇编。页面设计及数据汇编的著作权归本站所有。全部内容仅供个人学习、科研评估与技术交流参考,严禁用于任何商业用途(包括但不限于直接生产制造、技术转让、倒卖数据等)。
免责条款:专利技术具有时效性,本站不对资料的绝对准确性、完整性和可实施性提供任何明示或暗示的担保。因参考本站内容造成的直接或间接损失,本站不承担相关法律责任。若您认为本站内容有侵权问题,请及时提供权属证明与我们联系,本站将在核实后依法处理。反馈意见:yyfuon@163.com