面向变速曲率受限机器人的无碰撞覆盖路径规划方法

1.本发明属于移动机器人技术领域，具体涉及已知环境下变速曲率受限机器人的覆盖路径规划方法。本发明可应用到环境监控、自动化农业和室内清洁等场景下的机器人离线覆盖路径规划当中。


背景技术：

2.作为自动化机器人完成环境监控、地图重建、搜索营救等应用的基础，覆盖路径规划(coverage path planning,cpp)旨在设计一条访问目标区域所有点的路径，该路径在避免障碍物的同时最小化覆盖时间。在真实的覆盖场景中，cpp通常涉及包含复杂障碍物的环境和曲率受限的机器人。因此，有必要设计一个面向曲率受限机器人的无碰撞覆盖路径规划方法。
3.机器人最典型的动力学约束是对运动曲率和转弯速度的限制。针对无障碍物环境下，具有固定运动曲率及速度的机器人的路径规划问题，dubins基于几何方法规划出两点间的最短路径，该路径称为dubins路径。dubins路径具有可解析表达及快速计算等优点，目前研究人员提出了一系列基于dubins路径的dubins覆盖方法。dubins覆盖方法是一类典型、面向曲率约束的机器人区域覆盖方法，广泛应用于农业自动化、巡逻搜救以及海底检测。例如文献yu,xin,thaddeus a.roppel,and john y.hung."an optimization approach for planning robotic field coverage."iecon 2015-41st annual conference of the ieee industrial electronics society.ieee,2015，译为：yu,xin,thaddeus a.roppel,and john y.hung.一个机器人区域覆盖优化方法，ieee工业电子学会年会)提出的minnwt算法，该算法将将覆盖任务抽象成一个连通图，通过求解访问图中顶点的代价最小的序列来获得最后的覆盖路径。又比如文献(lewis,jeremy s.,et al."semi-boustrophedon coverage with a dubins vehicle."2017 ieee/rsj international conference on intelligent robots and systems(iros).ieee,2017，译为：lewis,jeremy s.,等"semi-boustrophedon coverage with a dubins vehicle."2017ieee国际智能与机器人系统会议)提出的semi-bcd算法，该算法在minnwt算法的基础上，利用一个图剪枝策略来删除简化连通图，从而减少计算时间。
4.尽管dubins覆盖方法具有诸多优势，但是它们存在如下两个问题。第一个问题是dubins覆盖方法假定机器人只能以固定的转弯半径及速度运动，这限制了利用机器人加速来减少时间代价的能力。事实上，机器人可以以不同的速度运动，而速度的变化有时可以有效节省时间。然而，当前变速曲率机器人的时间最优路径规划大都聚焦于点到点的路径规划，而变速曲率机器人的时间最优cpp问题尚未得到解决。
5.第二个问题是障碍物环境下机器人的安全问题。目前，大多数dubins覆盖方法都面向无障碍物环境，或者是虽然有障碍物，但机器人却可在障碍物上移动的环境。然而，在区域监控、战场损失评估、农业自动化等诸多现实应用中，目标环境内或者边界上可能存在不可穿越的障碍物。因此，直接将dubins覆盖方法应用到此类障碍物环境当中，将可能产生
不可行的结果。另外，一些dubins覆盖方法，通过在障碍物与待覆盖区域之间，设定一个用于机器人转弯的缓冲区域，避免机器人与障碍物发生碰撞。通常，机器人转弯被认为是非工作区域，较宽的缓冲区域会降低覆盖率，而较窄的缓冲区域会增加机器人与障碍物之间的碰撞风险。
6.如何解决障碍物环境下变速曲率受限机器人的覆盖路径规划问题，是覆盖路径规划领域需要解决的关键问题。


技术实现要素：

7.本发明要解决的技术问题是提供一种面向变速曲率约束机器人的无碰撞覆盖路径规划方法，在确保机器人安全的同时，使得覆盖时间最少。
8.本发明的技术方案是：首先，构建由客户端、服务端和机器人终端构成的面向变速曲率约束机器人的覆盖路径规划系统；客户端采集目标环境信息，服务端对目标区域进行单元分解，得到覆盖单元端点集合p。然后服务端构建出一个表征碰撞风险的势能场，计算出p中端点到端点的耗时最小的变速无碰撞路径。最后，构建数学模型，得到访问集合p所有端点的代价序列最小tour，进而得到最终的无碰撞路径。
9.具体技术方案是：
10.第一步，构建面向变速曲率约束机器人的覆盖路径规划系统。覆盖路径规划系统由服务端、客户端和机器人终端组成。
11.客户端是一台移动终端、pc机或者机器人。客户端上有激光雷达、双目相机等探测传感器，客户端通过探测传感器获取目标区域环境信息(包括目标区域的面积、障碍物、边界)，将目标区域环境信息抽象成一个地图g。g中值为0的元素代表障碍物，值为1的元素代表待覆盖网格。客户端将地图g和用户指定的速度采样间隔inter发送给服务端。
12.服务端是一台服务器或pc机，其上安装有覆盖路径规划系统。覆盖路径规划系统与客户端和机器人终端相连。它由区域分解模块、风险场构建模块、点到点路径规划模块和序列生成模块组成。
13.区域分解模块与客户端的探测传感器、机器人终端、点到点路径规划模块和序列生成模块相连，接收客户端发送的目标区域地图信息g、用户指定的速度采样间隔inter和机器人终端发送的机器人参数，根据机器人参数中的任务传感器覆盖半径r1将目标区域进行分解，得到单元端点集合p，将单元端点集合p、目标区域地图信息g、用户指定的速度采样间隔inter发送给点到点路径规划模块和序列生成模块。
14.风险场构建模块与区域分解模块、点到点路径规划模块和序列生成模块相连，接收区域分解模块发送的目标区域地图信息g和机器人参数，根据机器人参数中的机器人最小安全距离参数d
safe
，构建出一个表征机器人碰撞风险的势能场pe，将pe发送给点到点路径规划模块和序列生成模块。
15.点到点路径规划模块与区域分解模块、风险场构建模块、序列生成模块相连，从区域分解模块接收单元端点信息p、网格地图g和用户指定的速度采样间隔inter，从风险场构建模块接收势能场pe，从序列生成模块接收前进加速度a前进速度s、最大角速度u
max
等机器人参数，根据机器人参数中的机器人的前进速度、加速度、最大角速度，计算出机器人前进速度集合ps和单元到单元的无碰撞路径，进而得到一个表征路径代价的二维代价矩阵cm和
一个存储候选路径的二维路径矩阵cpath，并将cm和cpath发送到序列生成模块。
16.序列生成模块与机器人终端、区域分解模块、风险场构建模块、点到点路径规划模块相连，从机器人终端接收机器人参数，从区域分解模块接收单元端点集合p、目标区域地图信息g、用户指定的速度采样间隔inter，从风险场构建模块接收pe，从点到点路径规划模块接收二维代价矩阵cm和二维路径矩阵cpath，将覆盖路径规划问题建模成一个旅行商问题，生成访问所有覆盖单元的无碰撞覆盖路径path，并将覆盖路径path发送给机器人终端。
17.机器人终端是一台真实的机器人，比如阿克曼机器人。机器人终端装载有：(1)一个雷达传感器，可检测宽度为r2的矩形区域内的障碍物，r2∈(0,200]米；雷达传感器检测到的障碍物信息发送到定位模块，常见的雷达传感器有思岚激光雷达，可探测半径为8米区域内的障碍物；(2)一个用于执行覆盖任务的任务传感器(如房屋清洁任务中扫地机器人的边刷、草坪修剪任务中自动割草机的刀片等等)，可覆盖宽度为r1,r1≤r2大小的矩形区域；(3)用于实时定位的定位模块(常用的定位模块有ros amcl定位包(g.grisetti,c.stachniss,and w.burgard,“improved techniques for grid mapping with rao-blackwellized particle filters,”ieee transactions on robotics,vol.23,no.1,pp.34
–
46,feb 2007.译为：g.grisetti,c.stachniss,and w.burgard，rao-blackwellized粒子滤波器网格映射的改进技术，2007年2月机器人杂志，第23卷，第34-46页，ubuntu 16及其之后版本皆可)；(4)机器人路径跟随模块，常用的路径跟随模块有pure_persuit算法(参考文献coulter,r.craig.implementation of the pure pursuit path tracking algorithm.carnegie-mellon univ pittsburgh pa robotics inst,1992，译为coulter,r.craig.实现纯追踪路径跟踪算法。卡内基-梅隆大学匹兹堡pa机器人研究所，1992年，ubuntu 16及其之后版本皆可)进行路径跟踪；(5)其他机器人运行必备的软硬件控制模块，包含机器人固有的操作系统、底层驱动系统、运动控制系统等，这些是商用机器人的固有配置。机器人终端将机器人的前进速度s(s∈[s
min
，s
max
])、加速度a(a∈[a
min
，a
max
])、最大角速度u
max
、最小安全距离d
safe
和r1、r2等参数发送给服务端。
[0018]
第二步，客户端的探测传感器采集目标区域环境信息(包括目标区域的面积、边界、障碍物的位置)，构建出目标环境的网格地图g，并将网格地图g发送给服务端的区域分解模块。令网格地图g中网格行数为a，网格列数为b。
[0019]
第三步，服务端的区域分解模块接收客户端发送的网格地图g、用户指定的采样间隔inter和机器人终端发送的任务传感器覆盖半径r1，将目标区域进行分解，得到单元端点集合p，然后将单元端点集合p、网格地图g和用户指定的采样间隔inter发送给点到点路径规划模块和序列生成模块。具体方法如下：
[0020]
3.1区域分解模块采用semi-bcd分解算法(lewis，jeremy s.，et al.
″
semi-boustrophedon coverage with a dubins vehicle.
″
2017 ieee/rsj international conference on intelligent robots and systems(iros).ieee，2017，即lewis，jeremys.，et al.杜宾斯机器人的半布兰切特覆盖.
″
2017 ieee/rsj iros会议，第4页第v节的第2段到第4段)，根据网格地图g将目标区域划分为多个矩形单元。每个矩形单元中，位于x轴的边的宽度等于r1，位于y轴的边与障碍物或者目标区域边界相交。单个矩形单元对应单个覆盖任务，所有矩形单元构成初始的覆盖任务集合c，c＝{c1，...，cn，...，cn}，n为覆盖任务总数，n为正整数，cn代表c中第n个覆盖任务。
[0021]
3.2设定cn中线与其上下两条边的两个交点，为cn的上端点p
n，1
和下端点p
n，2
。p
n，1
的坐标为(px，pu)，p
n，2
的坐标为(px，pd)，其中px表示cn中线的x轴坐标，pu表示cn中线上方端点的y轴坐标，pd表示中线下方端点的y轴坐标。c中的n个单元，对应生成2n个端点{p
1，1
，p
1，2
，...，p
n，1
，p
n，2
，...，p
n，1
，p
n，2
}，其中p
n，1
，p
n，2
代表c中第n个单元的上下两个端点。为了描述方便，将p
n，1
和下端点p
n，2
进行简化，得到单元端点集合p＝{p1，...，pi，...，p
2n
}，1≤i≤2n，pi是{p
1，1
，p
1，2
，...，p
n，1
，p
n，2
，...，p
n，1
，p
n，2
}中的第i个端点。区域分解模块将单元端点集合p、网格地图g和用户指定的采样间隔inter发送给点到点路径规划模块和序列生成模块。
[0022]
第四步，服务端的风险场构建模块接收区域分解模块发送的目标区域网格地图g和机器人最小安全距离参数d
safe
，构建出一个表征碰撞风险的风险势能场pe，将pe发送给点到点路径规划模块和序列生成模块。构建碰撞风险势能场pe方法是：
[0023]
4.1风险场构建模块根据网格地图g构建风险势能场pe，pe是一个包含a
×
b个元素的二维矩阵，矩阵内的每个元素的值在0到1之间。矩阵元素值为1代表机器人在该元素处一定会发生碰撞风险，值为0代表机器人在该元素处是安全的。pe中元素值越大，代表机器人在该元素处与障碍物发生碰撞的可能性越大。构建pe的方法是：
[0024]
4.1.1初始化风险势能场pe的所有元素的值为0，初始化行序号a为1，列序号b为1。
[0025]
4.1.2根据网格地图g，计算pe中第a行第b列单元的风险势能pe
a，b
，pe
a，b
的计算如公式(1)所示：
[0026][0027]
其中，d表示网格地图g中第a行第b列单元g(a，b)，与g中距离g(a，b)其最近的障碍物单元之间的欧式距离。公式(1)表明，如果g(a，b)与最近的障碍物单元之间的距离大于机器人最小安全距离d
safe
，那么机器人在g(a，b)是安全的，风险值pe
a，b
＝0；反之，机器人在g(a，b)存在与障碍物发生碰撞的风险，该风险值为pe
a，b
。
[0028]
4.1.3如果a＝a且b＝b，说明风险场pe构建完毕，转4.2；如果a＝a但b≠b，令a＝1，令b＝b+1，转4.1.2；如果a≠a，且b≤b，令a＝a+1，转4.1.2。
[0029]
4.2风险场构建模块将pe发送给点对点路径规划模块和序列生成模块。
[0030]
第五步，服务端的点到点路径规划模块接收区域分解模块发送的单元端点集合p，计算与p对应的代价矩阵cm和路径矩阵cpath，方法是：
[0031]
5.1初始化与p对应的代价矩阵cm和路径矩阵cpath。代价矩阵cm是一个维度为2n
×
2n的二维矩阵，该矩阵里任意元素cm
d，e
代表p中第d个端点pd和第e个端点pe之间的无碰撞路径的时间代价，d＝1，...，2n，e＝1，...，2n。路径矩阵cpath是一个维度为2n
×
2n的二维矩阵，该矩阵里任意元素cpath
d，e
代表pd和pe之间无碰撞路径的航迹点集合，满足cpath
d，e
＝{(xf，yf，θf)，f＝1，...，f}，f为cpath中航迹点的个数，为正整数，其中(xf，yf，θf)表示机器人在pd和pe之间无碰撞路径的第f个航迹点，xf，yf为机器人在第f个航迹点处的x/y轴坐标，θf为机器人在第f个航迹点处的朝向。初始化代价矩阵cm的所有元素为0，初始化cpath中所有元素为空集，初始化行序号i为1，初始化列序号j为1。
[0032]
5.2点到点路径规划模块接收区域分解模块发送的p、网格地图g和速度采样间隔
inter，接收序列生成模块发送的机器人的前进速度、加速度、最大角速度等机器人参数，计算出从pd出发到达pe的无碰撞路径best_path，及best_path的耗时t
min
，令cpath
d，e
＝best_path，令cm
d，e
＝t
min
。方法如下：
[0033]
5.2.1点到点路径规划模块从区域分解模块接收p，根据pd和pe相对位置，得到机器人在pd处的位姿信息(xd，yd，θd)和pe处的位姿信息(xe，ye，θe)，xd，yd为机器人在pd点处的x/y轴坐标，θd为机器人在pd点处的朝向，xe，ye为机器人在pe点处的x/y轴坐标，θe为机器人在pe点处的朝向。方法如下：
[0034]
5.2.1.1根据区域分解模块发送的p，获取机器人在pd的x/y轴坐标(xd，yd)，机器人在pe的x/y轴坐标(xe，ye)。
[0035]
5.2.1.2如果pd和pe属于同一个单元，转5.2.1.3计算机器人在pd的朝向θd，和计算机器人在pe的朝向θe；如果pd和pe不属于同一个单元，转5.2.1.4计算机器人在pd的朝向θd，并计算机器人在pe的朝向θe。
[0036]
5.2.1.3此时pd和pe属于同一个单元，如果pd为该单元的上方端点，pe为该单元的下方端点，表示机器人从pd进入该单元，然后自上往下覆盖该单元，最后从pe离开，因此设定离开，因此设定如果pd为该单元的下方端点，pe为该单元的上方端点，设定转5.2.1.5。
[0037]
5.2.1.4此时pd和pe属于不同的单元，若pd为单元的上部端点，表示机器人自下往上覆盖完pd所属单元后，从pd离开pd所属单元。自下往上覆盖矩形单元时，机器人的朝向为因此设定否则令若pe为单元的上部端点，表示机器人覆盖完pd所属单元后，从pe进入pe所属单元，然后自上而下覆盖pe所属单元。自上而下覆盖矩形单元时，机器人的朝向为因此设定否则令转5.2.1.5。
[0038]
5.2.1.5令机器人在pd的位姿为(xd，yd，θd)，令机器人在pe的位姿为(xe，ye，θe)。
[0039]
5.2.2点到点路径规划模块接收区域分解模块发送的机器人采样间隔inter，序列生成模块发送的机器人加速度a(a∈[a
min
，a
max
]，a
min
≥0，a
max
＞0，a
min
＜a
max
，其中a
min
和a
max
分别代表机器人最小和最大前进加速度)、前进速度s(s∈[s
min
，s
max
]，s
min
≥0，s
max
＞0，s
min
＜s
max
，其中s
min
和s
max
分别代表机器人最小和最大前进速度)和最大角速度u
max
等机器人参数，对机器人前进速度进行等间隔采样，得到前进速度集合s＝{s1，...，sk，...，sk}，k是速度采样点个数，为正整数，其中sk是s中的第k个速度采样点，s1＝s
min
，sk＝s
max
。s中任意两个速度构成一个速度对，s中的k个速度对应k2个速度对，这些速度对构成了前进速度对集合ps＝{(s
k1
，s
k2
)，s
k1
，s
k2
∈s，k1，k2＝1，..，k}。
[0040]
5.2.3点到点路径规划模块接收风险场构建模块发送的风险势能场pe，计算出pd和pe之间的无碰撞路径best_path及该路径的耗时t
min
。方法如下：
[0041]
5.2.3.1初始化最小路径耗时t
min
为一个非常大的正数ta(如ta＝1000000)，初始化最优路径best_path为空集，令序列号k1＝1，k2＝1。
[0042]
5.2.3.2计算与速度对(s
k1
，s
k2
)条件下对应的无碰撞变速dubins路径集合dp。方法如下，
[0043]
5.2.3.2.1初始化dp为空集。
[0044]
5.2.3.2.2设定dp中6条dubins路径的速度和转弯半径。方法如下：根据dubins路径的定义，dubins路径包含rsl、rsr、lsr、lsl、lrl、rlr六种路径，其中r代表右转弧线段，l代表左转弧线段，s代表直线段。根据是否包含直线段(s)，以上六种dubins路径可以分为有直线段dubins路径(包含rsl、rsr、lsr、lsl四种路径)和无直线段dubins路径(包含lrl、rlr两种路径)两个类型。有直线段dubins路径和无直线dubins路径都包含三个路径段，令第一个到第三个路径段的转弯半径为r
α
、r
β
和r
γ
，机器人速度为s
α
、s
β
和s
γ
。
[0045]
5.2.3.2.2.1计算有直线段dubins路径的速度和转弯半径。有直线段dubins路径包括三条路径段，这三条路径段分别为第一条弧线段、第二条直线段和第三条弧线段。有直线段dubins路径要求第一条弧线段和第三条弧线段必须保持固定的转弯半径，因此点对点路径规划模块设定dubins路径中的第一条弧线段的转弯半径r
α
为设定dubins路径中的第一条弧线段的机器人速度s
α
为s
k1
，设定dubins路径中的第三条弧线段的转弯半径r
γ
为设定dubins路径中的第三条弧线段的机器人速度s
γ
为s
k2
。有直线段dubins路径的第二个路径段为直线段，设定直线段转弯半径为r
β
为0，采用速度配置方法(and jan faigl.”on finding time-efficient trajectories for fixed-wing aircraft using dubins paths with multiple radii.”proceedings of the 35th annual acm symposium on applied computing.2020.译为petrand jan faigl.，“利用具有多半径的杜宾斯路径为固定翼飞机寻找时间效率高的航迹。”第35届acm应用计算年会论文集，2020，第2页第3.2节第一段到第二段及图3)，计算机器人在第二条直线段上的速度s
β
，方法如下：令dubins路径第一条弧线段、第二条直线段和第三条弧线段长度分别为α、β和γ。
[0046]
当s
α
＜s
γ
时，令机器人以最大加速度a
max
，从s
α
加速到s
γ
的路径长度为l1，令机器人以最小加速度a
min
，从s
max
减速到s
γ
时路径长度为l2。机器人在第二条直线段上的速度s
β
有四种可能：(1)如果直线路径段β长度小于l1，说明直线段长度不足以使得机器人从s
α
开始加速到s
γ
，因此该路径物理不可行的路径，设定s
β
为0；(2)如果β＝l1，说明直线段长度刚好使得机器人从s
α
开始加速到s
γ
，因此设定s
β
从s
α
开始，以最大加速度a
max
加速到s
γ
；(3)如果l1＜β＜l1+l2，说明直线段长度使得机器人可以从s
α
加速到一个比s
γ
大但比s
max
小的速度s
t1
，s
α
＜s
t
＜s
max
，然后再减速到s
γ
，因此设定s
β
从s
α
开始，以最大加速度a
max
加速到s
t
，然后再以最小加速度a
min
减速到s
γ
。令机器人从s
α
开始，以最大加速度a
max
加速到s
t1
时的路径长度为al1，其中令机器人从s
t1
开始，以最小加速度a
min
减速到s
γ
时的路径长度为dl1，其中al1和dl1满足等式al1+dl1＝β，该等式中只有一个变量s
t1
，求解该等式即可获得s
t1
的值。(4)如果β＞l1+l2，说明直线段长度使得机器人可以从s
α
加速到s
max
，然后再减速到s
γ
，因此设定s
β
从s
α
开始，以最大加速度a
max
加速到最大速度s
max
，再以最小加速度a
min
减速到s
γ
。
[0047]
当s
α
＝s
γ
时，令机器人从s
α
开始，以最大加速度a
max
加速到s
max
，再以最小加速度a
min
减速到s
γ
时，整条路径的长度为l3。机器人在第二条直线段上的速度s
β
有两种可能：(1)如果直线路径段β＜l3，设定s
β
从s
α
开始，以最大加速度a
max
加速到s
t2
，然后再以最小加速度amin
减速到s
γ
。令机器人从s
α
开始，以最大加速度a
max
加速到s
t2
时的路径长度为al2，其中令机器人从s
t
开始，以最小加速度a
min
减速到s
γ
时的路径长度为dl2，其中al2和dl2满足等式al2+dl2＝β，该等式中只有一个变量s
t2
，求解该等式即可获得s
t2
的值。(2)如果β≥l3，设定s
β
从s
α
开始，以最大加速度a
max
加速到最大速度s
max
，然后再以最小加速度a
min
减速到s
γ
。
[0048]
当s
α
＞s
γ
时，令机器人从速度s
α
出发，以最小加速度a
min
，减速到s
γ
时路径长度为l4。令机器人从s
α
开始，以最大加速度a
max
加速到s
max
，再以最小加速度a
min
减速到s
γ
时，整条路径的长度为l5。机器人在第二条直线段上的速度s
β
有四种可能：(1)如果β＜l4，设定s
β
为0；(2)如果β＝l4，设定s
β
从s
α
开始，以最小加速度a
min
，从s
α
减速到s
γ
。(3)如果l4＜β＜l5，设定s
β
从s
α
开始，以最大加速度a
max
加速到s
t3
，然后再以最小加速度a
min
减速到s
γ
。令机器人从s
α
开始，以最大加速度a
max
加速到s
t3
时的路径长度为al3，其中，其中令机器人从s
t3
开始，以最小加速度a
min
减速到s
γ
时的路径长度为dl3，其中al3和dl3满足等式al3+dl3＝β，该等式中只有一个变量s
t3
，求解该等式即可获得s
t3
的值。；(4)如果β＞l5，那么设定s
β
从s
α
开始，以最大加速度a
max
加速到最大速度s
max
，然后再以最小加速度a
min
减速到s
γ
。
[0049]
5.2.3.2.2.2计算不包含直线段dubins路径的速度和转弯半径。不包含直线段的dubins路径中的三条路径段全为弧线段，这三条弧线段的转弯和机器人速度必须是相等的，故设置不包含直线段的dubins路径中的第一条到第三条路径段的转弯半径r
α
、r
β
、r
γ
皆为设置第一条到第三个路径段的机器人速度s
α
、s
β
、s
γ
为s
k1
。
[0050]
5.2.3.2.3计算dp中6条dubins路径的路径点集合。方法如下：根据变速dubins路径模型(参考文献j.p.wilson，k.mittal，and s.gupta，“novel motion models for time-optimal risk-aware motion planning for variable-speed auvs，”inoceans 2019 mts/ieee seattle.ieee，2019，pp.1-5，译为j.p.wilson，k.mittal，and s.gupta，“变速无人艇的时间最优风险可控运动规划的新运动模型”，2019海mts/ieee会议，西雅图，第2页第3段到第4段)，按照公式(2)、公式(3)和公式(4)，分别计算变速dubins路径中三个路径段的位姿。
[0051][0052][0053]sσ
(x，y，θ)＝(x+σcosθ，y+σsinθ，θ)
ꢀꢀꢀꢀ
(4)
[0054]
其中，l
σ
(x，y，θ)、r
σ
(x，y，θ)和s
σ
(x，y，θ)分别代表机器人在左转弧线段、右转弧线段和直线段上的坐标位姿；公式(2)和公式(3)中的σ代表机器人从该弧线段起点开始的弧度偏移，而公式(4)路径段的σ表示机器人在直线段上的位移，r
σ
代表机器人在该弧线段的转弯半径。
[0055]
根据公式(2)-(4)，可以得到单条dubins路径的三个路径段的所有航迹点，这些航迹点构成航迹点集合dpr，满足dpr＝{(xf，yf，θf)，f＝1，...，f}，f为dpr中导航点个数，f为正整数，其中(xf，yf，θf)为路径dpr中的第f个导航点，xf，yf，θf分别表示机器人在该点处的x/y轴坐标和朝向；对于rsl、rsr、lsr、lsl、lrl、rlr六种路径，分别得到六条变速dubins路径的航迹点集合，这六条路径构成变速dubins路径集合dp＝{dpr，r＝1，...，6}，其中dpr为dp中的第r条路径。
[0056]
5.2.3.3计算无碰撞变速dubins路径集合dp中各条路径对应的耗时集合dt，dt＝{dtr，r＝1，...，6}，其中dtr代表dt中的第r个元素，其值为路径dpr的耗时。方法如下：
[0057]
5.2.3.3.1初始化dt中的6个元素为一个非常大的正数ta(如ta＝1000000)，令序号r＝1。
[0058]
5.2.3.3.2判断路径dpr中是否存在碰撞风险。如果路径dpr存在一个航迹点u，航迹点u处的风险势能pe(u)＞0，说明路径dpr存在与障碍物发生碰撞的可能，设定dtr为ta；否则，说明路径dpr是安全的，不存在碰撞风险。在路径dpr不存在碰撞风险的情况下，按照公式(5)，计算得到计算路径dpr对应的路径耗时dtr，
[0059][0060]
其中，s
α，r
，s
β，r
和s
γ，r
分别代表路径dpr中第一个路径段、第二个路径段、第三个路径段上的前进速度；αr、βr、γr分别表示路径dpr中第一个路径段、第二个路径段、第三个路径段的长度。
[0061]
5.2.3.3.3如果r＝6，说明耗时集合dt计算完毕，转5.2.3.4；否则，令r＝r+1，转5.2.3.3.2。
[0062]
5.2.3.4在dt中搜索值最小的元素dt
min
，dt
min
＝min(dt)。令dt
min
为dt中的第w个元素，1≤w≤6。如果dt
min
＜t
min
，令t
min
＝dt
min
，令最优路径best_path为dp中耗时最少的无碰撞路径dpw，转5.2.3.5；否则，t
min
保持原值，转5.2.3.5。
[0063]
5.2.3.5如果k2≠b，令k2＝k2+1，转5.2.3.2；如果k2＝b，且k1≠a一1，令k2＝1，k1＝k1+1，转5.2.3.2；否则，说明已经得到pd和pe之间耗时最小的无碰撞路径best_path，令cpath
d，e
＝best_path，cm
d，e
＝t
min
，转5.3。
[0064]
5.3如果d＝2n且e＝2n，说明已经计算得到与p对应的代价矩阵cm和cpath，转5.4；如果d＝2n，e≠2n，令d＝1，e＝e+1，转第5.2步；如果d≠2n，e≤2n，令d＝d+1，转第5.2步。
[0065]
5.4点到点路径规划模块将cm和cpath发送给序列生成模块，转第六步。
[0066]
第六步，服务端的序列生成模块接收区域分解模块发送的单元端点集合p、点到点路径规划模块发送的代价矩阵cm和cpath和机器人终端发送的机器人参数，构建覆盖路径规划数学模型，得到访问p中所有端点的代价最小的序列tour，进而得到覆盖路径path。方法如下：
[0067]
6.1初始化tour为空集。
[0068]
6.2序列生成模块接收点到点路径规划子模块发送的cm，构建覆盖路径规划模型，如公式(6)-(10)所示：
[0069]
min(∑cm
i，j
*z
i，j
)，i＝1，2，...，2n，j＝1，2，...，2n，i≠j
ꢀꢀꢀ
(6)
[0070]
∑z
i，j
＝2n
ꢀꢀꢀꢀ
(7)
[0071][0072][0073]
z2×
n1+1，2
×
n1+2
+z2×
n1+2，2
×
n1+1
＝1，n1＝1，...，n
ꢀꢀꢀ
(10)
[0074]
其中自变量z
i，j
表示机器人顺序访问端点pi和pj的布尔值。如果z
i，j
＝1，表示机器人在访问端点pi之后，再访问端点pj；如果z
i，j
＝0，，表示机器人在访问端点pi之后，不会访问端点pj。公式(6)表示求解访问p中所有端点的代价最小的回路，公式(7)表示tour中包含2n条边，公式(8)表示机器人只能进入同一个端点一次，公式(9)表示机器人只能离开同一个端点一次，公式(10)表示机器人一定会顺序访问同一个矩形单元的两个端点。
[0075]
6.2公式(6)-(10)建立的数学模型是一个旅行商问题，序列生成模块利用现有的旅行商问题求解器(常用的旅行商问题求解器有lkh求解器https://aithub.com/unr-arl/lkh tsp和matlabtsp求解器https://www.mathworks.com/matlabcentral/fileexchange/64654-travelling-salesman-problem)对公式(6)-(10)进行求解，得到访问p中所有端点的代价最小的序列tour＝{t1，...，tn，...，t
2n
}，其中tn是tour中的第n个端点。
[0076]
6.3根据端点序列tour和cpath生成覆盖路径path。方法如下：
[0077]
6.3.1初始化端点序列号n＝1，初始化覆盖路径path为空集。
[0078]
6.3.2令tour中第n个端点和第n+1个端点分别为u和v，从路径矩阵cpath中提取出的耗时最小无碰撞路径cpath
u，v
，作为tour中第n个端点到tour中第n+1个端点之间路径pathn。令path＝path∪pathn。
[0079]
6.3.3如果n≠2n-1，令n＝n+1，转6.3.2步；否则，转6.3.4计算tour中第2n个端点返回tour中第1个端点的路径。
[0080]
6.3.4令tour中第2n个端点和第一个端点分别为uu和vv，从路径矩阵cpath中提取出cpath
uu，vv
，作为tour中第2n个端点和第一个端点之间路径path
2n
。令path＝path∪path
2n
。转6.4。
[0081]
6.4序列生成模块将覆盖路径path发送到机器人终端，转第七步。
[0082]
第七步，机器人终端接收序列生子模块输出的覆盖路径path，基于机器人终端配置的路径跟随模块、定位模块和机器人运行软硬件控制模块，按照覆盖路径path运动(一般是一边运动一边执行任务)，这条运动轨迹可以实现对目标区域的完全覆盖。
[0083]
采用本发明可以达到以下技术效果：
[0084]
1.本发明可以为变速曲率受限机器人规划出一条耗时最小的无碰撞覆盖路径。
[0085]
2.本发明第三步到第五步通过构建一个碰撞风险势来保证覆盖路径的安全性，通过构建变速dubins路径来保证路径的效率，从而可以得到任意单元的两个端点之间耗时最小的无碰撞路径。
[0086]
3.本发明的第六步将覆盖路径规划问题建模成一个旅行商问题，得到访问所有端点的代价最小的序列，进而得到访问目标区域所有点的安全无碰撞覆盖路径。
[0087]
为验证本发明的可行性，将本发明与semi-bcd算法与minnwt算法进行仿真实验和实物试验的对比。实验结果表明本发明在保证覆盖率的同时，有效减少了覆盖路径的耗时和覆盖方法的计算耗时。同时本发明也适用于阿克曼机器人。
附图说明
[0088]
图1为本发明整体流程图；
[0089]
图2为本发明第一步构建的面向变速曲率受限机器人的覆盖路径规划系统逻辑结构图；
[0090]
图3为本发明与具有不同前进速度semi-bcd算法和minnwt算法在大小为20m
×
20m的场景下覆盖时间、碰撞风险、覆盖率及计算耗时等参数的对比结果。图3(a)是本发明与前进速度为{0.2,0.36,0.52,0.68,0.84,1.0}m/s的semi-bcd算法和minnwt算法，在缓冲区域宽度为缓冲区域宽度为[1,1.5,2.0,2.5,3,3.5,4.0]*r2条件下的覆盖时间对比结果。图3(b)是本发明与前进速度为{0.2,0.36,0.52,0.68,0.84,1.0}m/s的semi-bcd算法和minnwt算法，在缓冲区域宽度为缓冲区域宽度为[1,1.5,2.0,2.5,3,3.5,4.0]*r2条件下的最大碰撞风险对比结果。图3(c)是本发明与前进速度为{0.2,0.36,0.52,0.68,0.84,1.0}m/s的semi-bcd算法和minnwt算法，在缓冲区域宽度为缓冲区域宽度为[1,1.5,2.0,2.5,3,3.5,4.0]*r2条件下的覆盖率对比结果。图3(d)是本发明与前进速度为{0.2,0.36,0.52,0.68,0.84,1.0}m/s的semi-bcd算法和minnwt算法，在缓冲区域宽度为缓冲区域宽度为[1,1.5,2.0,2.5,3,3.5,4.0]*r2条件下的计算耗时对比结果。
[0091]
图4为本发明与具有不同前进速度semi-bcd算法和minnwt算法在大小为30m
×
30m的场景下覆盖时间、碰撞风险、覆盖率及计算耗时等参数的对比结果。图4(a)是本发明与前进速度为{0.2,0.36,0.52,0.68,0.84,1.0}m/s的semi-bcd算法和minnwt算法，在缓冲区域宽度为缓冲区域宽度为[1,1.5,2.0,2.5,3,3.5,4.0]*r2条件下的覆盖时间对比结果。图4(b)是本发明与前进速度为{0.2,0.36,0.52,0.68,0.84,1.0}m/s的semi-bcd算法和minnwt算法，在缓冲区域宽度为缓冲区域宽度为[1,1.5,2.0,2.5,3,3.5,4.0]*r2条件下的最大碰撞风险对比结果。图4(d)是本发明与前进速度为{0.2,0.36,0.52,0.68,0.84,1.0}m/s的semi-bcd算法和minnwt算法，在缓冲区域宽度为缓冲区域宽度为[1,1.5,2.0,2.5,3,3.5,4.0]*r2条件下的覆盖率对比结果。图4(d)是本发明与前进速度为{0.2,0.36,0.52,0.68,0.84,1.0}m/s的semi-bcd算法和minnwt算法，在缓冲区域宽度为缓冲区域宽度为[1,1.5,2.0,2.5,3,3.5,4.0]*r2条件下的计算耗时对比结果。
[0092]
图5是本发明在真实实验室环境下的截图及规划的路径。图5(a)实验室环境和测试用的机器人。图5(b)是规划的覆盖路径。
[0093]
图6是本发明在真实实验室环境下的本发明在4个时刻的过程截图。每个截图的左侧在展示了规划的覆盖路径及机器人在当前时刻的位姿，右侧展示了机器人在真实实验室环境下的位姿。
具体实施方式
[0094]
如图1所示，本发明包括以下步骤：
[0095]
第一步，构建面向变速曲率约束机器人的覆盖路径规划系统。覆盖路径规划系统如图2所示，由服务端、客户端和机器人终端组成。
[0096]
客户端是一台移动终端、pc机或者机器人。客户端上有激光雷达、双目相机等探测传感器，客户端通过探测传感器获取目标区域环境信息(包括目标区域的面积、障碍物、边界)，将目标区域环境信息抽象成一个地图g。g中值为0的元素代表障碍物，值为1的元素代
表待覆盖网格。客户端将地图g和用户指定的速度采样间隔inter发送给服务端。
[0097]
服务端是一台服务器或pc机，其上安装有覆盖路径规划系统。覆盖路径规划系统与客户端和机器人终端相连。它由区域分解模块、风险场构建模块、点到点路径规划模块和序列生成模块组成。
[0098]
区域分解模块与客户端的探测传感器、机器人终端、点到点路径规划模块和序列生成模块相连，接收客户端发送的目标区域地图信息g、用户指定的速度采样间隔inter和机器人终端发送的机器人参数，根据机器人参数中的任务传感器覆盖半径r1将目标区域分解成若干个单元，将单元端点集合p、目标区域地图信息g、用户指定的速度采样间隔inter发送给点到点路径规划模块和序列生成模块。
[0099]
风险场构建模块与区域分解模块、点到点路径规划模块和序列生成模块相连，接收区域分解模块发送的目标区域地图信息g和机器人参数，根据机器人参数中的机器人最小安全距离参数d
safe
，构建出一个表征机器人碰撞风险的势能场pe，将pe发送给点到点路径规划模块和序列生成模块。
[0100]
点到点路径规划模块与区域分解模块、风险场构建模块、序列生成模块相连，从区域分解模块接收单元端点p、网格地图g和用户指定的速度采样间隔inter，从风险场构建模块接收势能场pe，从序列生成模块接收前进加速度a前进速度s、最大角速度u
max
等机器人参数，根据机器人参数中的机器人的前进速度、加速度、最大角速度，计算出机器人前进速度集合ps和单元到单元的无碰撞路径，进而得到一个表征路径代价的二维代价矩阵cm和一个存储候选路径的二维路径矩阵cpath，并将cm和cpath发送到序列生成模块。
[0101]
序列生成模块与机器人终端、区域分解模块、风险场构建模块、点到点路径规划模块相连，从机器人终端接收机器人参数，从区域分解模块接收单元端点集合p、目标区域地图信息g、用户指定的速度采样间隔inter，从风险场构建模块接收pe，从点到点路径规划模块接收二维代价矩阵cm和二维路径矩阵cpath，将覆盖路径规划问题建模成一个旅行商问题，生成访问所有覆盖单元的无碰撞覆盖路径path，并将覆盖路径path发送给机器人终端。
[0102]
机器人终端是一台真实的机器人，本实施例中采用阿克曼机器人。机器人终端装载有：(1)一个雷达传感器，可检测宽度为r2的矩形区域内的障碍物，r2∈(0，200]米；雷达传感器检测到的障碍物信息发送到定位模块，常见的雷达传感器有思岚激光雷达，可探测半径为8米区域内的障碍物；(2)一个用于执行覆盖任务的任务传感器(如房屋清洁任务中扫地机器人的边刷、草坪修剪任务中自动割草机的刀片等等)，可覆盖宽度为r1，r1≤r2大小的矩形区域；(3)用于实时定位的定位模块(本实施例中采用ros amcl定位包)；(4)机器人路径跟随模块(本实施例中采用pure_persuit算法)进行路径跟踪；(5)其他机器人运行必备的软硬件控制模块，包含机器人固有的操作系统、底层驱动系统、运动控制系统等，这些是商用机器人的固有配置。机器人终端将机器人的前进速度s(s∈[s
min
，s
max
])、加速度a(a∈[a
min
，a
max
])、最大角速度u
max
、最小安全距离d
safe
和r1、r2等参数发送给服务端。
[0103]
第二步，客户端的探测传感器采集目标区域环境信息(包括目标区域的面积、边界、障碍物的位置)，构建出目标环境的网格地图g，并将网格地图g发送给服务端的区域分解模块。令网格地图g中网格行数为a，网格列数为b。
[0104]
第三步，服务端的区域分解模块接收客户端发送的网格地图g、用户指定的采样间隔inter和机器人终端发送的任务传感器覆盖半径r1，将目标区域分解进行分解，得到单元
端点集合p，然后将单元端点集合p、网格地图g和用户指定的采样间隔inter发送给点到点路径规划模块和序列生成模块。具体方法如下：
[0105]
3.1区域分解模块采用semi-bcd分解算法，根据网格地图g将目标区域划分为多个矩形单元。每个矩形单元中，位于x轴的边的宽度等于r1，位于y轴的边与障碍物或者目标区域边界相交。单个矩形单元对应单个覆盖任务，所有矩形单元构成初始的覆盖任务集合c，c＝{c1，...，cn，...，cn}，n为覆盖任务总数，n为正整数，cn代表c中第n个覆盖任务。
[0106]
3.2设定cn中线与其上下两条边的两个交点，为cn的上端点p
n，1
和下端点p
n，2
。p
n，1
的坐标为(px，pu)，p
n，2
的坐标为(px，pd)，其中px表示cn中线的x轴坐标，pu表示cn中线上方端点的y轴坐标，pd表示中线下方端点的y轴坐标。c中的n个单元，对应生成2n个端点{p
1，1
，p
1，2
，...，p
n，1
，p
n，2
，...，p
n，1
，p
n，2
}，其中p
n，1
，p
n2
代表c中第n个单元的上下两个端点。为了描述方便，将p
n，1
和下端点p
n，2
进行简化，得到单元端点集合p＝{p1，...，pi，...，p
2n
}，1≤i≤2n，pi是{p
1，1
，p
1，2
，...，p
n，1
，p
n，2
，...，p
n，1
，p
n，2
}中的第i个端点。区域分解模块将单元端点集合p、网格地图g和用户指定的采样间隔inter发送给点到点路径规划模块和序列生成模块。
[0107]
第四步，服务端的风险场构建模块接收区域分解模块发送的目标区域网格地图g和机器人最小安全距离参数d
safe
，构建出一个表征碰撞风险的风险势能场pe，将pe发送给点到点路径规划模块和序列生成模块。构建碰撞风险势能场pe方法是：
[0108]
4.1风险场构建模块根据网格地图g构建风险势能场pe，pe是一个包含a
×
b个元素的二维矩阵，矩阵内的每个元素的值在0到1之间。矩阵元素值为1代表机器人在该元素处一定会发生碰撞风险，值为0代表机器人在该元素处是安全的。pe中元素值越大，代表机器人在该元素处与障碍物发生碰撞的可能性越大。构建pe的方法是：
[0109]
4.1.1初始化风险势能场pe的所有元素的值为0，初始化行序号a为1，列序号b为1。
[0110]
4.1.2根据网格地图g，计算pe中第a行第b列单元的风险势能pe
a，b
，pe
a，b
的计算如公式(1)所示：
[0111][0112]
其中，d表示网格地图g中第a行第b列单元g(a，b)，与g中距离g(a，b)其最近的障碍物单元之间的欧式距离。公式(1)表明，如果g(a，b)与最近的障碍物单元之间的距离大于机器人最小安全距离d
safe
，那么机器人在g(a，b)是安全的，风险值pe
a，b
＝0；反之，机器人在g(a，b)存在与障碍物发生碰撞的风险，该风险值为pe
a，b
。
[0113]
4.1.3如果a＝a且b＝b，说明风险场pe构建完毕，转4.2；如果a＝a但b≠b，令a＝1，令b＝b+1，转4.1.2；如果a≠a，且b≤b，令a＝a+1，转4.1.2。
[0114]
4.2风险场构建模块将pe发送给点对点路径规划模块和序列生成模块。
[0115]
第五步，服务端的点到点路径规划模块接收区域分解模块发送的单元端点集合p，计算与p对应的代价矩阵cm和路径矩阵cpath，方法是：
[0116]
5.1初始化与p对应的代价矩阵cm和路径矩阵cpath。代价矩阵cm是一个维度为2n
×
2n的二维矩阵，该矩阵里任意元素cm
d，e
代表p中第d个端点pd和第e个端点pe之间的无碰撞路径的时间代价，d＝1，...，2n，e＝1，...，2n。路径矩阵cpath是一个维度为2n
×
2n的二维
矩阵，该矩阵里任意元素cpath
d，e
代表pd和pe之间无碰撞路径的航迹点集合，满足cpath
d，e
＝{(xf，yf，θf)，f＝1，...，f}，f为cpath中航迹点的个数，为正整数，其中(xf，yf，θf)表示机器人在pd和pe之间无碰撞路径的第f个航迹点，xf，yf为机器人在第f个航迹点处的x/y轴坐标，θf为机器人在第f个航迹点处的朝向。初始化代价矩阵cm的所有元素为0，初始化cpath中所有元素为空集，初始化行序号i为1，初始化列序号j为1。
[0117]
5.2点到点路径规划模块接收区域分解模块发送的p、网格地图g和速度采样间隔inter，接收序列生成模块发送的机器人的前进速度、加速度、最大角速度等机器人参数，计算出从pd出发到达pe的无碰撞路径best_path，及best_path的耗时t
min
，令cpath
d，e
＝best_path，令cm
d，e
＝t
min
。方法如下：
[0118]
5.2.1点到点路径规划模块从区域分解模块接收p，根据pd和pe相对位置，得到机器人在pd处的位姿信息(xd，yd，θd)和pe处的位姿信息(xe，ye，θe)，xd，yd为机器人在pd点处的x/y轴坐标，θd为机器人在pd点处的朝向，xe，ye为机器人在pe点处的x/y轴坐标，θe为机器人在pe点处的朝向。方法如下：
[0119]
5.2.1.1根据区域分解模块发送的p，获取机器人在pd的x/y轴坐标(xd，yd)，机器人在pe的x/y轴坐标(xe，ye)。
[0120]
5.2.1.2如果pd和pe属于同一个单元，转5.2.1.3计算机器人在pd的朝向θd，和计算机器人在pe的朝向θe；如果pd和pe不属于同一个单元，转5.2.1.4计算机器人在pd的朝向θd，并计算机器人在pe的朝向θe。
[0121]
5.2.1.3此时pd和pe属于同一个单元，如果pd为该单元的上方端点，pe为该单元的下方端点，表示机器人从pd进入该单元，然后自上往下覆盖该单元，最后从pe离开，因此设定离开，因此设定如果pd为该单元的下方端点，pe为该单元的上方端点，设定转5.2.1.5。
[0122]
5.2.1.4此时pd和pe属于不同的单元，若pd为单元的上部端点，表示机器人自下往上覆盖完pd所属单元后，从pd离开pd所属单元。自下往上覆盖矩形单元时，机器人的朝向为因此设定否则令若pe为单元的上部端点，表示机器人覆盖完pd所属单元后，从pe进入pe所属单元，然后自上而下覆盖pe所属单元。自上而下覆盖矩形单元时，机器人的朝向为因此设定否则令转5.2.1.5。
[0123]
5.2.1.5令机器人在pd的位姿为(xd，yd，θd)，令机器人在pe的位姿为(xe，ye，θe)。
[0124]
5.2.2点到点路径规划模块接收区域分解模块发送的机器人采样间隔inter，序列生成模块发送的机器人加速度a(a∈[a
min
，a
max
]，a
min
≥0，a
max
＞0，a
min
＜a
max
，其中a
min
和a
max
分别代表机器人最小和最大前进加速度)、前进速度s(s∈[s
min
，s
max
]，s
min
≥0，s
max
＞0，s
min
＜s
max
，其中s
min
和s
max
分别代表机器人最小和最大前进速度)和最大角速度u
max
等机器人参数，对机器人前进速度进行等间隔采样，得到前进速度集合s＝{s1，...，sk，...，sk}，k是速度采样点个数，为正整数，其中sk是s中的第k个速度采样点，s1＝s
min
，sk＝s
max
。s中任意两个速度构成一个速度对，s中的k个速度对应k2个速度对，这些速度对构成了前进速度对集合ps＝{(s
k1
，s
k2
)，s
k1
，s
k2
∈s，k1，k2＝1，..，k}。
[0125]
5.2.3点到点路径规划模块接收风险场构建模块发送的风险势能场pe，计算出pd和pe之间的无碰撞路径best_path及该路径的耗时t
min
。方法如下：
[0126]
5.2.3.1初始化最小路径耗时t
min
为一个非常大的正数ta，ta＝1000000，初始化最优路径best_path为空集，令序列号kl＝1，k2＝1。
[0127]
5.2.3.2计算与速度对(s
k1
，s
k2
)条件下对应的无碰撞变速dubins路径集合dp。方法如下，
[0128]
5.2.3.2.1初始化dp为空集。
[0129]
5.2.3.2.2设定dp中6条dubins路径的速度和转弯半径。方法如下：根据dubins路径的定义，dubins路径包含rsl、rsr、lsr、lsl、lrl、rlr六种路径，其中r代表右转弧线段，l代表左转弧线段，s代表直线段。根据是否包含直线段(s)，以上六种dubins路径可以分为有直线段dubins路径(包含rsl、rsr、lsr、lsl四种路径)和无直线段dubins路径(包含lrl、rlr两种路径)两个类型。有直线段dubins路径和无直线dubins路径都包含三个路径段，令第一个到第三个路径段的转弯半径为r
α
、r
β
和r
γ
，机器人速度为s
α
、s
β
和s
γ
。
[0130]
5.2.3.2.2.1计算有直线段dubins路径的速度和转弯半径。有直线段dubins路径包括三条路径段，这三条路径段分别为第一条弧线段、第二条直线段和第三条弧线段。有直线段dubins路径要求第一条弧线段和第三条弧线段必须保持固定的转弯半径，因此点对点路径规划模块设定dubins路径中的第一条弧线段的转弯半径r
α
为设定dubins路径中的第一条弧线段的机器人速度s
α
为s
k1
，设定dubins路径中的第三条弧线段的转弯半径r
γ
为设定dubins路径中的第三条弧线段的机器人速度s
γ
为s
k2
。有直线段dubins路径的第二个路径段为直线段，设定直线段转弯半径为r
β
为0，采用速度配置方法计算机器人在第二条直线段上的速度s
β
，方法如下：令dubins路径第一条弧线段、第二条直线段和第三条弧线段长度分别为α、β和γ。
[0131]
当s
α
＜s
γ
时，令机器人以最大加速度a
max
，从s
α
加速到s
γ
的路径长度为l1，令机器人以最小加速度a
min
，从s
max
减速到s
γ
时路径长度为l2。机器人在第二条直线段上的速度s
β
有四种可能：(1)如果直线路径段β长度小于l1，说明直线段长度不足以使得机器人从s
α
开始加速到s
γ
，因此该路径物理不可行的路径，设定s
β
为0；(2)如果β＝l1，说明直线段长度刚好使得机器人从s
α
开始加速到s
γ
，因此设定s
β
从s
α
开始，以最大加速度a
max
加速到s
γ
；(3)如果l1＜β＜l1+l2，说明直线段长度使得机器人可以从s
α
加速到一个比s
γ
大但比s
max
小的速度s
t1
，s
α
＜s
t
＜s
max
，然后再减速到s
γ
，因此设定s
β
从s
α
开始，以最大加速度a
max
加速到s
t
，然后再以最小加速度a
min
减速到s
γ
。令机器人从s
α
开始，以最大加速度a
max
加速到s
t1
时的路径长度为al1，其中令机器人从s
t1
开始，以最小加速度a
min
减速到s
γ
时的路径长度为dl1，其中al1和dl1满足等式al1+dl1＝β，该等式中只有一个变量s
t1
，求解该等式即可获得s
t1
的值。(4)如果β＞l1+l2，说明直线段长度使得机器人可以从s
α
加速到s
max
，然后再减速到s
γ
，因此设定s
β
从s
α
开始，以最大加速度a
max
加速到最大速度s
max
，再以最小加速度a
min
减速到s
γ
。
[0132]
当s
α
＝s
γ
时，令机器人从s
α
开始，以最大加速度a
max
加速到s
max
，再以最小加速度a
min
减速到s
γ
时，整条路径的长度为l3。机器人在第二条直线段上的速度s
β
有两种可能：(1)如
果直线路径段β＜l3，设定s
β
从s
α
开始，以最大加速度a
max
加速到s
t2
，然后再以最小加速度a
min
减速到s
γ
。令机器人从s
α
开始，以最大加速度a
max
加速到s
t2
时的路径长度为al2，其中令机器人从s
t
开始，以最小加速度a
min
减速到s
γ
时的路径长度为dl2，其中al2和dl2满足等式al2+dl2＝β，该等式中只有一个变量s
t2
，求解该等式即可获得s
t2
的值。(2)如果β≥l3，设定s
β
从s
α
开始，以最大加速度a
max
加速到最大速度s
max
，然后再以最小加速度a
min
减速到s
γ
。
[0133]
当s
α
＞s
γ
时，令机器人从速度s
α
出发，以最小加速度a
min
，减速到s
γ
时路径长度为l4。令机器人从s
α
开始，以最大加速度a
max
加速到s
max
，再以最小加速度a
min
减速到s
γ
时，整条路径的长度为l5。机器人在第二条直线段上的速度s
β
有四种可能：(1)如果β＜l4，设定s
β
为0；(2)如果β＝l4，设定s
β
从s
α
开始，以最小加速度a
min
，从s
α
减速到s
γ
。(3)如果l4＜β＜l5，设定s
β
从s
α
开始，以最大加速度a
max
加速到s
t3
，然后再以最小加速度a
min
减速到s
γ
。令机器人从s
α
开始，以最大加速度a
max
加速到s
t3
时的路径长度为al3，其中，其中令机器人从s
t3
开始，以最小加速度a
min
减速到s
γ
时的路径长度为dl3，其中al3和dl3满足等式al3+dl3＝β，该等式中只有一个变量s
t3
，求解该等式即可获得s
t3
的值。；(4)如果β＞l5，那么设定s
β
从s
α
开始，以最大加速度a
max
加速到最大速度s
max
，然后再以最小加速度a
min
减速到s
γ
。
[0134]
5.2.3.2.2.2计算不包含直线段dubins路径的速度和转弯半径。不包含直线段的dubins路径中的三条路径段全为弧线段，这三条弧线段的转弯和机器人速度必须是相等的，故设置不包含直线段的dubins路径中的第一条到第三条路径段的转弯半径r
α
、r
β
、r
γ
皆为设置第一条到第三个路径段的机器人速度s
α
、s
β
、s
γ
为s
k1
。
[0135]
5.2.3.2.3计算dp中6条dubins路径的路径点集合。方法如下：根据变速dubins路径模型，按照公式(2)、公式(3)和公式(4)，分别计算变速dubins路径中三个路径段的位姿。
[0136][0137][0138]sσ
(x，y，θ)＝(x+σcosθ，y+σsinθ，θ)
ꢀꢀꢀꢀ
(8)
[0139]
其中，l
σ
(x，y，θ)、r
σ
(x，y，θ)和s
σ
(x，y，θ)分别代表机器人在左转弧线段、右转弧线段和直线段上的坐标位姿；公式(2)和公式(3)中的σ代表机器人从该弧线段起点开始的弧度偏移，而公式(4)路径段的σ表示机器人在直线段上的位移，r
σ
代表机器人在该弧线段的转弯半径。
[0140]
根据公式(2)-(4)，可以得到单条dubins路径的三个路径段的所有航迹点，这些航迹点构成航迹点集合dpr，满足dpr＝{(xf，yf，θf)，f＝1，...，f}，f为dpr中导航点个数，f为正整数，其中(xf，yf，θf)为路径dpr中的第f个导航点，xf，yf，θf分别表示机器人在该点处的x/y轴坐标和朝向；对于rsl、rsr、lsr、lsl、lrl、rlr六种路径，分别得到六条变速dubins路径的
航迹点集合，这六条路径构成变速dubins路径集合dp＝{dpr，r＝1，...，6}，其中dpr为dp中的第r条路径。
[0141]
5.2.3.3计算无碰撞变速dubins路径集合dp中各条路径对应的耗时集合dt，dt＝{dtr，r＝1，...，6}，其中dtr代表dt中的第r个元素，其值为路径dpr的耗时。方法如下：
[0142]
5.2.3.3.1初始化dt中的6个元素为一个非常大的正数ta(如ta＝1000000)，令序号r＝1。
[0143]
5.2.3.3.2判断路径dpr中是否存在碰撞风险。如果路径dpr存在一个航迹点u，航迹点u处的风险势能pe(u)＞0，说明路径dpr存在与障碍物发生碰撞的可能，设定dtr为ta；否则，说明路径dpr是安全的，不存在碰撞风险。在路径dpr不存在碰撞风险的情况下，按照公式(5)，计算得到计算路径dpr对应的路径耗时dtr，
[0144][0145]
其中，s
α，r
，s
β，r
和s
γ，r
分别代表路径dpr中第一个路径段、第二个路径段、第三个路径段上的前进速度；αr、βr、γr分别表示路径dpr中第一个路径段、第二个路径段、第三个路径段的长度。
[0146]
5.2.3.3.3如果r＝6，说明耗时集合dt计算完毕，转5.2.3.4；否则，令r＝r+1，转5.2.3.3.2。
[0147]
5.2.3.4在dt中搜索值最小的元素dt
min，
dt
min
＝min(dt)。令dt
min
为dt中的第w个元素，1≤w≤6。如果dt
min
＜t
min
，令t
min
＝dt
min
，令最优路径best_path为dp中耗时最少的无碰撞路径dpw，转5.2.3.5；否则，t
min
保持原值，转5.2.3.5。
[0148]
5.2.3.5如果k2≠b，令k2＝k2+1，转5.2.3.2；如果k2＝b，且k1≠a-1，令k2＝1，k1＝k1+1，转5.2.3.2；否则，说明已经得到pd和pe之间耗时最小的无碰撞路径best_path，令cpath
d，e
＝best_path，cm
d，e
＝t
min
，转5.3。
[0149]
5.3如果d＝2n且e＝2n，说明已经计算得到与p对应的代价矩阵cm和cpath，转5.4；如果d＝2n，e≠2n，令d＝1，e＝e+1，转第5.2步；如果d≠2n，e≤2n，令d＝d+1，转第5.2步。
[0150]
5.4点到点路径规划模块将cm和cpath发送给序列生成模块，转第六步。
[0151]
第六步，服务端的序列生成模块接收区域分解模块发送的单元端点集合p、点到点路径规划模块发送的代价矩阵cm和cpath和机器人终端发送的机器人参数，构建覆盖路径规划数学模型，得到访问p中所有端点的代价最小的序列tour，进而得到覆盖路径path。方法如下：
[0152]
6.1初始化tour为空集。
[0153]
6.2序列生成模块接收点到点路径规划子模块发送的cm，构建覆盖路径规划模型，如公式(6)-(10)所示：
[0154]
min(∑cm
i，j
*z
i，j
)，i＝1，2，...，2n，j＝1，2，...，2n，i≠j
ꢀꢀ
(6)
[0155]
∑z
i，j
＝2n
ꢀꢀꢀꢀ
(7)
[0156][0157][0158]
z2×
n1+1，2
×
n1+2
+z2×
n1+2，2
×
n1+1
＝1，n1＝1，...，n
ꢀꢀꢀ
(10)
[0159]
其中自变量z
i，j
表示机器人顺序访问端点pi和pj的布尔值。如果z
i，j
＝1，表示机器人在访问端点pi之后，再访问端点pj；如果z
i，j
＝0，，表示机器人在访问端点pi之后，不会访问端点pj。公式(6)表示求解访问p中所有端点的代价最小的回路，公式(7)表示tour中包含2n条边，公式(8)表示机器人只能进入同一个端点一次，公式(9)表示机器人只能离开同一个端点一次，公式(10)表示机器人一定会顺序访问同一个矩形单元的两个端点。
[0160]
6.2公式(6)-(10)建立的数学模型是一个旅行商问题，序列生成模块利用lkh求解器对公式(6)-(10)进行求解，得到访问p中所有端点的代价最小的序列tour＝{t1，...，tn，...，t
2n
}，其中tn是tour中的第n个端点。
[0161]
6.3根据端点序列tour和cpath生成覆盖路径path。方法如下：
[0162]
6.3.1初始化端点序列号n＝1，初始化覆盖路径path为空集。
[0163]
6.3.2令tour中第n个端点和第n+1个端点分别为u和v，从路径矩阵cpath中提取出的耗时最小无碰撞路径cpath
u，v
，作为tour中第n个端点到tour中第n+1个端点之间路径pathn。令path＝path∪pathn。
[0164]
6.3.3如果n≠2n-1，令n＝n+1，转6.3.2步；否则，转6.3.4计算tour中第2n个端点返回tour中第1个端点的路径。
[0165]
6.3.4令tour中第2n个端点和第一个端点分别为uu和vv，从路径矩阵cpath中提取出cpath
uu，vv
，作为tour中第2n个端点和第一个端点之间路径path
2n
。令path＝path∪path
2n
。转6.4。
[0166]
6.4序列生成模块将覆盖路径path发送到机器人终端，转第七步。
[0167]
第七步，机器人终端接收序列生子模块输出的覆盖路径path，基于机器人终端配置的路径跟随模块、定位模块和机器人运行软硬件控制模块，按照覆盖路径path运动(一般是一边运动一边执行任务)，这条运动轨迹可以实现对目标区域的完全覆盖。
[0168]
图3和图4分别展示了20m
×
20m和30m
×
30m的场景下，本发明与前进速度为{0.2，0.36，0.52，0.68，0.84，1.0}m/s的semi-bcd算法及minnwt算法，在缓冲区域宽度为缓冲区域宽度为[1，1.5，2.0，2.5，3，3.5，4.0]
×
r2条件下的覆盖时间、最大碰撞风险、覆盖率及计算耗时的对比结果。两个实验场景内分布有若干个数量和边界已知的障碍物。仿真采用dubins机器人，该机器人受以下运动学约束：范围为[0.2，1]m/s的前进速度，范围为[0.5，1.5]m2/s的前进加速度，大小为1.0rad/s的转向速率，大小为0.2m的最小转弯半径和大小为0.25m的安全距离。semi-bcd算法及minnwt算法都假定机器人速度是固定的，因此实验测试了速度为0.2，0.36，0.52，0.68，0.84，1.0}m/s的semi-bcd算法及minnwt算法的结果。与具有固定速度的semi-bcd算法及minnwt算法不同，本发明假定机器人速度是可变的，其值可以是速度集合s中任意一个采样速度，本发明的性能结果用一条贯穿集合s所有采样速度的直线表示。为了获得可比较的结果，将本发明与具有.2，0.36，0.52，0.68，0.84，1.0}m/s的semi-bcd算法及minnwt算法全部进行比较。
[0169]
首先，图3(a)和图4(a)展示了机器人前进速度为前进速度为{0.2，0.36，0.52，0.68，0.84，1.0}m/s，缓冲区域宽度为[1，1.5，2.0，2.5，3，3.5，4.0]*r2时，本发明与semi-bcd算法及minnwt算法的覆盖时间，其中图3(a)和图4(a)的x轴为缓冲区域宽度，y轴为机器人前进速度，z轴为覆盖时间。观察图3(a)和图4(a)可知，在不同的缓冲区域宽度条件，相较于具有不同速度的semi-bcd算法及minnwt算法，本发明得到的覆盖时间都是最小或者接近
最小的。
[0170]
图3(b)和图4(b)展示了机器人前进速度为前进速度为{0.2,0.36,0.52,0.68,0.84,1.0}m/s，缓冲区域宽度为[1,1.5,2.0,2.5,3,3.5,4.0]*r2时，本发明与semi-bcd算法及minnwt算法的最大碰撞风险，其中图3(b)和图4(b)的x轴为缓冲区域宽度，y轴为机器人前进速度，z轴为最大碰撞风险。观察图3(b)和图4(b)可知，本发明在不同的缓冲区域宽度条件下，其路径的最大碰撞风险都为0，即本发明能获得安全的覆盖路径。相比之下，semi-bcd算法因为假定障碍物可穿越，其规划的路径会可能会横穿障碍物。minnwt算法的安全性与缓冲区域宽度和速度相关，速度小的要求较窄的缓冲区域宽度，速度高的要求较高的要求较宽的缓冲区域。
[0171]
图3(c)和图4(c)展示了机器人前进速度为前进速度为{0.2,0.36,0.52,0.68,0.84,1.0}m/s，缓冲区域宽度为[1,1.5,2.0,2.5,3,3.5,4.0]*r2时，本发明与semi-bcd算法及minnwt算法的覆盖率。其中图3(c)和图4(c)的x轴为缓冲区域宽度，y轴为机器人前进速度，z轴为覆盖率。观察图3(c)和图4(c)可知，本发明、semi-bcd算法、minnwt算法的覆盖率与缓冲区域宽度相关。缓冲区域越宽，对应的覆盖率越低。当缓冲区域宽度为r2时，本发明、semi-bcd算法、minnwt算法的覆盖率都达到了95％以上；但是，当缓冲区域宽度逐渐增大时，本发明、semi-bcd算法、minnwt算法的覆盖率逐渐减小。总的来说，当缓冲区域宽度小于等于2r2时，本发明能获取大于等于95％的覆盖率。
[0172]
图3(d)和图4(d)展示了机器人前进速度为前进速度为{0.2,0.36,0.52,0.68,0.84,1.0}m/s，缓冲区域宽度为[1,1.5,2.0,2.5,3,3.5,4.0]*r2时，本发明与semi-bcd算法及minnwt算法的计算时间，其中图3(d)和图4(d)的x轴为缓冲区域宽度，y轴为机器人前进速度，z轴为计算时间。观察图3(d)可知，大部分情况下，本发明的计算耗时都要小于semi-bcd算法和minnwt算法。观察图4(d)可知，本发明与semi-bcd算法的计算时间的计算耗时相当，而minnwt算法的时间远大于本发明与semi-bcd算法的计算耗时。
[0173]
图5(a)是本发明在真实实验室环境下的截图，图5(b)是本法明规划的覆盖路径。如图5(a)所示，实验环境是一个大小为9.5m
×
10.5m的实验室区域，其上分布有多个障碍物。客户端使用探测传感器，预先构建出目标环境的网格地图。机器人终端采用一个前向阿克曼机器人，该机器人的前进速度范围设定为[0.3,0.5]m/s，前进加速度范围为[0.5,1.0]m2/s，最小转弯半径大小为0.3m，最大角速度为1.0rad/s，安全距离为0.35m。一维速度集合s设定为{0.3,0.4,0.5}m/s，缓冲区域宽度设定为0.9m。机器人终端集成有一个ros amcl包作为定位模块，集成有pure_pursuit算法作为路径跟随模块。此外，机器人上搭载有一个探测范围为8米的思岚激光雷达传感器和一个探测范围为0.9米的覆盖任务传感器。基于环境信息和机器人参数，本发明规划出了图5(b)所示的覆盖路径。机器人在覆盖路径各个点处的前进速度，通过颜色进行编码，颜色浅的部分代表机器人速度快，颜色深的部分代表机器人在该处速度较慢。执行覆盖任务时，机器人从左下角出发，沿预先设定好的覆盖路径运动，最终返回起点。
[0174]
图6展示了是本发明在真实实验室环境下的过程截图。每个截图的左侧部分为rviz中记录的机器人位姿信息，右侧为阿克曼机器人在真实实验场景中对应的位姿。图6(a)展示了机器人从起点出发，开始执行覆盖的截图；图6(b)-(d)是覆盖过程中三个时刻的截图；图6(e)是覆盖结束时的截图。图6的5个截图表明，真实的机器人能够跟随本发明规划
出的覆盖路径，即本发明适用于真实的机器人。

技术特征：
1.一种面向变速曲率受限机器人的无碰撞覆盖路径规划方法，其特征在于包括以下步骤：第一步，构建面向变速曲率约束机器人的覆盖路径规划系统；覆盖路径规划系统由服务端、客户端和机器人终端组成；客户端是一台移动终端、pc机或者机器人；客户端上有激光雷达、双目相机等探测传感器，客户端通过探测传感器获取目标区域环境信息，将目标区域环境信息抽象成一个地图g；g中值为0的元素代表障碍物，值为1的元素代表待覆盖网格；客户端将地图g和用户指定的速度采样间隔inter发送给服务端；服务端是一台服务器或pc机，其上安装有覆盖路径规划系统；覆盖路径规划系统与客户端和机器人终端相连；它由区域分解模块、风险场构建模块、点到点路径规划模块和序列生成模块组成；区域分解模块与客户端的探测传感器、机器人终端、点到点路径规划模块和序列生成模块相连，接收客户端发送的目标区域地图信息g、用户指定的速度采样间隔inter和机器人终端发送的机器人参数，根据机器人参数中的任务传感器覆盖半径r1将目标区域进行分解，得到单元端点集合p，将单元端点集合p、目标区域地图信息g、用户指定的速度采样间隔inter发送给点到点路径规划模块和序列生成模块；风险场构建模块与区域分解模块、点到点路径规划模块和序列生成模块相连，接收区域分解模块发送的目标区域地图信息g和机器人参数，根据机器人参数中的机器人最小安全距离参数d
safe
，构建出一个表征机器人碰撞风险的势能场pe，将pe发送给点到点路径规划模块和序列生成模块；点到点路径规划模块与区域分解模块、风险场构建模块、序列生成模块相连，从区域分解模块接收单元端点集合p、网格地图g和用户指定的速度采样间隔inter，从风险场构建模块接收势能场pe，从序列生成模块接收机器人参数，根据机器人参数中的机器人的前进速度、加速度、最大角速度，计算出机器人前进速度集合ps和单元到单元的无碰撞路径，进而得到一个表征路径代价的二维代价矩阵cm和一个存储候选路径的二维路径矩阵cpath，并将cm和cpath发送到序列生成模块；序列生成模块与机器人终端、区域分解模块、风险场构建模块、点到点路径规划模块相连，从机器人终端接收机器人参数，从区域分解模块接收单元端点集合p、目标区域地图信息g、用户指定的速度采样间隔inter，从风险场构建模块接收pe，从点到点路径规划模块接收二维代价矩阵cm和二维路径矩阵cpath，将覆盖路径规划问题建模成一个旅行商问题，生成访问所有覆盖单元的无碰撞覆盖路径path，并将覆盖路径path发送给机器人终端；机器人终端是一台真实的机器人，机器人终端装载有：一个雷达传感器，雷达传感器检测宽度为r2的矩形区域内的障碍物，将检测到的障碍物信息发送到定位模块；一个用于执行覆盖任务的任务传感器，覆盖宽度为r1的矩形区域；用于实时定位的定位模块；机器人路径跟随模块；其他机器人运行必备的软硬件控制模块，包含机器人固有的操作系统、底层驱动系统、运动控制系统；机器人终端将机器人的前进速度s、加速度a、最大角速度u
max
、最小安全距离d
safe
和r1、r2发送给服务端；第二步，客户端的探测传感器采集目标区域环境信息，构建出目标环境的网格地图g，并将网格地图g发送给服务端的区域分解模块；令网格地图g中网格行数为a，网格列数为b；
标区域环境信息包括目标区域的面积、边界、障碍物的位置；第三步，服务端的区域分解模块接收客户端发送的网格地图g、用户指定的采样间隔inter和机器人终端发送的任务传感器覆盖半径r1，将目标区域进行分解，得到单元端点集合p，然后将单元端点集合p、网格地图g和用户指定的采样间隔inter发送给点到点路径规划模块和序列生成模块；方法如下：3.1区域分解模块采用semi-bcd分解算法，根据网格地图g将目标区域划分为多个矩形单元；每个矩形单元中，位于x轴的边的宽度等于r1，位于y轴的边与障碍物或者目标区域边界相交；单个矩形单元对应单个覆盖任务，所有矩形单元构成初始的覆盖任务集合c，c＝{c1，...，c
n
，...，c
n
}，n为覆盖任务总数，n为正整数，c
n
代表c中第n个覆盖任务；3.2设定c
n
中线与其上下两条边的两个交点，为c
n
的上端点p
n，1
和下端点p
n，2
；p
n，1
的坐标为(px，pu)，p
n，2
的坐标为(px，pd)，其中px表示c
n
中线的x轴坐标，pu表示c
n
中线上方端点的y轴坐标，pd表示中线下方端点的y轴坐标；c中的n个单元，对应生成2n个端点{p
1，1
，p
1，2
，...，p
n，1
，p
n，2
，...，p
n，1
，p
n，2
}，其中p
n，1
，p
n，2
代表c中第n个单元的上下两个端点；为了描述方便，将p
n，1
和下端点p
n，2
进行简化，得到单元端点集合p＝{p1，...，p
i
，...，p
2n
}，1≤i≤2n，p
i
是{p
1，1
，p
1，2
，...，p
n，1
，p
n，2
，...，p
n，1
，p
n，2
}中的第i个端点；区域分解模块将单元端点集合p、网格地图g和用户指定的采样间隔inter发送给点到点路径规划模块和序列生成模块；第四步，服务端的风险场构建模块接收区域分解模块发送的目标区域网格地图g和机器人最小安全距离参数d
safe
，构建出一个表征碰撞风险的风险势能场pe，将pe发送给点到点路径规划模块和序列生成模块；构建碰撞风险势能场pe方法是：4.1风险场构建模块根据网格地图g构建风险势能场pe，pe是一个包含a
×
b个元素的二维矩阵，矩阵内的每个元素的值在0到1之间；矩阵元素值为1代表机器人在该元素处一定会发生碰撞风险，值为0代表机器人在该元素处是安全的；pe中元素值越大，代表机器人在该元素处与障碍物发生碰撞的可能性越大；4.2风险场构建模块将pe发送给点对点路径规划模块和序列生成模块；第五步，服务端的点到点路径规划模块接收区域分解模块发送的单元端点集合p，计算与p对应的代价矩阵cm和路径矩阵cpath，方法是：5.1初始化与p对应的代价矩阵cm和路径矩阵cpath；代价矩阵cm是一个维度为2n
×
2n的二维矩阵，该矩阵里任意元素cm
d，e
代表p中第d个端点p
d
和第e个端点p
e
之间的无碰撞路径的时间代价，d＝1，...，2n，e＝1，...，2n；路径矩阵cpath是一个维度为2n
×
2n的二维矩阵，该矩阵里任意元素cpath
d，e
代表p
d
和p
e
之间无碰撞路径的航迹点集合，满足cpath
d，e
＝{(x
f
，y
f
，θ
f
)，f＝1，...，f}，f为cpath中航迹点的个数，为正整数，其中(x
f
，y
f
，θ
f
)表示机器人在p
d
和p
e
之间无碰撞路径的第f个航迹点，x
f
，y
f
为机器人在第f个航迹点处的x/y轴坐标，θ
f
为机器人在第f个航迹点处的朝向；初始化代价矩阵cm的所有元素为0，初始化cpath中所有元素为空集，初始化行序号i为1，初始化列序号j为1；5.2点到点路径规划模块接收区域分解模块发送的p、网格地图g和速度采样间隔inter，接收序列生成模块发送的机器人的前进速度、加速度、最大角速度等机器人参数，计算出从p
d
出发到达p
e
的无碰撞路径best_path，及best_path的耗时t
min
，令cpath
d，e
＝best_path，令cm
d，e
＝t
min
；方法如下：5.2.1点到点路径规划模块从区域分解模块接收p，根据p
d
和p
e
相对位置，得到机器人在
p
d
处的位姿信息(x
d
，y
d
，θ
d
)和p
e
处的位姿信息(x
e
，y
e
，θ
e
)，x
d
，y
d
为机器人在p
d
点处的x/y轴坐标，θ
d
为机器人在p
d
点处的朝向，x
e
，y
e
为机器人在p
e
点处的x/y轴坐标，θ
e
为机器人在p
e
点处的朝向；5.2.2点到点路径规划模块接收区域分解模块发送的机器人采样间隔inter，序列生成模块发送的机器人加速度a、前进速度s和最大角速度u
max
，a∈[a
min
，a
max
]，a
min
≥0，a
max
＞0，a
min
＜a
max
，其中a
min
和a
max
分别代表机器人最小和最大前进加速度；s∈[s
min
，s
max
]，s
min
≥0，s
max
＞0，s
min
＜s
max
，其中s
min
和s
max
分别代表机器人最小和最大前进速度；对机器人前进速度进行等间隔采样，得到前进速度集合s＝{s1，...，s
k
，...，s
k
}，k是速度采样点个数，为正整数，其中s
k
是s中的第k个速度采样点，s1＝s
min
，s
k
＝s
max
；s中任意两个速度构成一个速度对，s中的k个速度对应k2个速度对，这些速度对构成了前进速度对集合ps＝{(s
k1
，s
k2
)，s
k1
，s
k2
∈s，k1，k2＝1，..，k}；5.2.3点到点路径规划模块接收风险场构建模块发送的风险势能场pe，计算出p
d
和p
e
之间的无碰撞路径best_path及该路径的耗时t
min
；方法如下：5.2.3.1初始化最小路径耗时t
min
为一个正数ta，初始化最优路径best_path为空集，令序列号k1＝1，k2＝1；5.2.3.2计算与速度对(s
k1
，s
k2
)条件下对应的无碰撞变速dubins路径集合dp；dp中共有dubins路径中的包含rsl、rsr、lsr、lsl、lrl、rlr六条路径，其中r代表右转弧线段，l代表左转弧线段，s代表直线段；dp＝{dp
r
，r＝1，...，6}，dp
r
为dp中的第r条路径，dp
r
＝{(x
f
，y
f
，θ
f
)，f＝1，...，f}，f为dp
r
中导航点个数，f为正整数，(x
f
，y
f
，θ
f
)为dp
r
中的第f个导航点，x
f
，y
f
，θ
f
分别表示机器人在该点处的x/y轴坐标和朝向；5.2.3.3计算dp中各条路径对应的耗时集合dt，dt＝{dt
r
，r＝1，...，6}，其中dt
r
代表dt中的第r个元素，其值为路径dp
r
的耗时；5.2.3.4在dt中搜索值最小的元素dt
min
，dt
min
＝min(dt)；令dt
min
为dt中第w个元素，1≤w≤6；如果dt
min
＜t
min
，令t
min
＝dt
min
，令最优路径best_path为dp中耗时最少的无碰撞路径dp
w
，转5.2.3.5；否则，t
min
保持原值，转5.2.3.5；5.2.3.5如果k2≠b，令k2＝k2+1，转5.2.3.2；如果k2＝b，且k1≠a-1，令k2＝1，k1＝k1+1，转5.2.3.2；否则，说明已经得到p
d
和p
e
之间耗时最小的无碰撞路径best_path，令cpath
d，e
＝best_path，cm
d，e
＝t
min
，转5.3；5.3如果d＝2n且e＝2n，说明已经计算得到与p对应的代价矩阵cm和cpath，转5.4；如果d＝2n，e≠2n，令d＝1，e＝e+1，转第5.2步；如果d≠2n，e≤2n，令d＝d+1，转第5.2步；5.4点到点路径规划模块将cm和cpath发送给序列生成模块，转第六步；第六步，服务端的序列生成模块接收区域分解模块发送的单元端点集合p、点到点路径规划模块发送的代价矩阵cm和cpath和机器人终端发送的机器人参数，构建覆盖路径规划数学模型，得到访问p中所有端点的代价最小的序列tour，进而得到覆盖路径path；方法如下：6.1初始化tour为空集；6.2序列生成模块接收点到点路径规划子模块发送的cm，构建覆盖路径规划模型，如公式(6)-(10)所示：min(∑cm
i，j
*z
i，j
)，i＝1，2，...，2n，j＝1，2，...，2n，i≠j
ꢀꢀꢀꢀ
(6)
∑z
i，j
＝2n
ꢀꢀꢀꢀ
(7)(7)z2×
n1+1，2
×
n1+2
+z2×
n1+2，2
×
n1+1
＝1，n1＝1，...，n
ꢀꢀꢀꢀ
(10)其中自变量z
i，j
表示机器人顺序访问端点p
i
和p
j
的布尔值；如果z
i，j
＝1，表示机器人在访问端点p
i
之后，再访问端点p
j
；如果z
i，j
＝0，，表示机器人在访问端点p
i
之后，不会访问端点p
j
；公式(6)表示求解访问p中所有端点的代价最小的回路，公式(7)表示tour中包含2n条边，公式(8)表示机器人只能进入同一个端点一次，公式(9)表示机器人只能离开同一个端点一次，公式(10)表示机器人一定会顺序访问同一个矩形单元的两个端点；6.2对公式(6)-(10)进行求解，得到访问p中所有端点的代价最小的序列tour＝{t1，...，t
n
，...，t
2n
}，其中t
n
是tour中的第n个端点；6.3根据端点序列tour和cpath生成覆盖路径path；6.4序列生成模块将覆盖路径path发送到机器人终端，转第七步；第七步，机器人终端接收序列生子模块输出的覆盖路径path，基于机器人终端配置的路径跟随模块、定位模块和机器人运行软硬件控制模块，按照覆盖路径path一边运动一边执行任务，实现对目标区域的完全覆盖。2.如权利要求1所述的面向变速曲率受限机器人的无碰撞覆盖路径规划方法，其特征在于所述机器人终端采用阿克曼机器人；所述雷达传感器检测的矩形区域的宽度r2∈(0，200]米；所述雷达传感器采用思岚激光雷达；所述任务传感器要求覆盖的宽度r1≤r2；定位模块采用ros amcl定位包；路径跟随模块采用纯追踪路径跟踪算法即pure_persuit算法进行路径跟踪。3.如权利要求1所述的面向变速曲率受限机器人的无碰撞覆盖路径规划方法，其特征在于4.1步所述风险场构建模块根据网格地图g构建风险势能场pe的方法是：4.1.1初始化风险势能场pe的所有元素的值为0，初始化行序号a为1，列序号b为1；4.1.2根据网格地图g，计算pe中第a行第b列单元的风险势能pe
a，b
，pe
a，b
的计算如公式(1)所示：其中，d表示网格地图g中第a行第b列单元g(a，b)，与g中距离g(a，b)其最近的障碍物单元之间的欧式距离；公式(1)表明，如果g(a，b)与最近的障碍物单元之间的距离大于机器人最小安全距离d
sare
，那么机器人在g(a，b)是安全的，风险值pe
a，b
＝0；反之，机器人在g(a，b)存在与障碍物发生碰撞的风险，该风险值为pe
a，b
；4.1.3如果a＝a且b＝b，说明风险场pe构建完毕，结束；如果a＝a但b≠b，令a＝1，令b＝b+1，转4.1.2；如果a≠a，且b≤b，令a＝a+1，转4.1.2。4.如权利要求1所述的面向变速曲率受限机器人的无碰撞覆盖路径规划方法，其特征在于5.2.1步所述点到点路径规划模块根据p
d
和p
e
相对位置，得到机器人在p
d
处的位姿信息(x
d
，y
d
，θ
d
)和p
e
处的位姿信息(x
e
，y
e
，θ
e
)的方法是：5.2.1.1根据区域分解模块发送的p，获取机器人在p
d
的x/y轴坐标(x
d
，y
d
)，机器人在p
e
的x/y轴坐标(x
e
，y
e
)；5.2.1.2如果p
d
和p
e
属于同一个单元，转5.2.1.3计算机器人在p
d
的朝向θ
d
，和计算机器人在p
e
的朝向θ
e
；如果p
d
和p
e
不属于同一个单元，转5.2.1.4计算机器人在p
d
的朝向θ
d
，并计算机器人在p
e
的朝向θ
e
；5.2.1.3此时p
d
和p
e
属于同一个单元，如果p
d
为该单元的上方端点，p
e
为该单元的下方端点，表示机器人从p
d
进入该单元，然后自上往下覆盖该单元，最后从p
e
离开，设定离开，设定如果p
d
为该单元的下方端点，p
e
为该单元的上方端点，设定转5.2.1.5；5.2.1.4此时p
d
和p
e
属于不同的单元，若p
d
为单元的上部端点，表示机器人自下往上覆盖完p
d
所属单元后，从p
d
离开p
d
所属单元；自下往上覆盖矩形单元时，机器人的朝向为设定否则令若p
e
为单元的上部端点，表示机器人覆盖完p
d
所属单元后，从p
e
进入p
e
所属单元，然后自上而下覆盖p
e
所属单元；自上而下覆盖矩形单元时，机器人的朝向为设定否则令转5.2.1.5；5.2.1.5令机器人在p
d
的位姿为(x
d
，y
d
，θ
d
)，令机器人在p
e
的位姿为(x
e
，y
e
，θ
e
)。5.如权利要求1所述的面向变速曲率受限机器人的无碰撞覆盖路径规划方法，其特征在于5.2.3.1步所述ta＝1000000。6.如权利要求1所述的面向变速曲率受限机器人的无碰撞覆盖路径规划方法，其特征在于5.2.3.2步所述计算与速度对(s
k1
，s
k2
)条件下对应的无碰撞变速dubins路径集合dp的方法是：5.2.3.2.1初始化dp为空集；5.2.3.2.2设定dp中6条dubins路径的速度和转弯半径；方法如下：根据dubins路径的定义，dubins路径包含rsl、rsr、lsr、lsl、lrl、rlr六种路径，其中r代表右转弧线段，l代表左转弧线段，s代表直线段；根据是否包含直线段，六种dubins路径分为有直线段dubins路径即rsl、rsr、lsr、lsl四种路径和无直线段dubins路径即lrl、rlr两种路径；有直线段dubins路径和无直线dubins路径都包含三个路径段，令第一个到第三个路径段的转弯半径为r
α
、r
β
和r
γ
，机器人速度为s
α
、s
β
和s
γ
；5.2.3.2.2.1计算有直线段dubins路径的速度和转弯半径；有直线段dubins路径包括三条路径段，这三条路径段分别为第一条弧线段、第二条直线段和第三条弧线段；有直线段dubins路径要求第一条弧线段和第三条弧线段必须保持固定的转弯半径，点对点路径规划模块设定dubins路径中的第一条弧线段的转弯半径r
α
为设定dubins路径中的第一条弧线段的机器人速度s
α
为s
k1
，设定dubins路径中的第三条弧线段的转弯半径r
γ
为设定dubins路径中的第三条弧线段的机器人速度s
γ
为s
k2
；有直线段dubins路径的第二个路径段为直线段，设定直线段转弯半径为r
β
为0，计算机器人在第二条直线段上的速度s
β
；5.2.3.2.2.2计算不包含直线段dubins路径的速度和转弯半径；不包含直线段的dubins路径中的三条路径段全为弧线段，设置不包含直线段的dubins路径中的第一条到第三条路径段的转弯半径r
α
、r
β
、r
γ
皆为设置第一条到第三个路径段的机器人速度s
α
、s
β
、s
γ
为s
k1
；
5.2.3.2.3计算dp中6条dubins路径的路径点集合，方法是：根据变速dubins路径模型，按照公式(2)、公式(3)和公式(4)，分别计算变速dubins路径中三个路径段的位姿；l
σ
(x，y，θ)＝(x-r
σ
sinθ+r
σ
sin(θ+σ)，
ꢀꢀꢀꢀ
(2)y+r
σ
cosθ-r
σ
cos(θ+σ)，θ+σ)r
σ
(x，y，θ)＝(x+r
σ
sinθ-r
σ
sin(θ-σ)，
ꢀꢀꢀꢀ
(3)y-r
σ
cosθ+r
σ
cos(θ-σ)，θ-σ)s
σ
(x，y，θ)＝(x+σcosθ，y+σsinθ，θ)
ꢀꢀꢀꢀ
(4)其中，l
σ
(x，y，θ)、r
σ
(x，y，θ)和s
σ
(x，y，θ)分别代表机器人在左转弧线段、右转弧线段和直线段上的坐标位姿；公式(2)和公式(3)中的σ代表机器人从该弧线段起点开始的弧度偏移，而公式(4)路径段的σ表示机器人在直线段上的位移，r
σ
代表机器人在该弧线段的转弯半径；根据公式(2)-(4)，得到单条dubins路径的三个路径段的所有航迹点，这些航迹点构成航迹点集合dp
r
，满足dp
r
＝{(x
f
，y
f
，θ
f
)，f＝1，...，f}，对于rsl、rsr、lsr、lsl、lrl、rlr六种路径，分别得到六条变速dubins路径的航迹点集合，这六条路径构成变速dubins路径集合dp＝{dp
r
，r＝1，...，6}。7.如权利要求6所述的面向变速曲率受限机器人的无碰撞覆盖路径规划方法，其特征在于5.2.3.2.2.1步所述计算机器人在第二条直线段上的速度s
β
时采用速度配置方法：令dubins路径第一条弧线段、第二条直线段和第三条弧线段长度分别为α、β和γ；当s
α
＜s
γ
时，令机器人以最大加速度a
max
，从s
α
加速到s
γ
的路径长度为l1，令机器人以最小加速度a
min
，从s
max
减速到s
γ
时路径长度为l2，机器人在第二条直线段上的速度s
β
有四种：(1)如果直线路径段β长度小于l1，设定s
β
为0；(2)如果β＝l1，设定s
β
从s
α
开始，以最大加速度a
max
加速到s
γ
；(3)如果l1＜β＜l1+l2，设定s
β
从s
α
开始，以最大加速度a
max
加速到s
t1
，然后再以最小加速度a
min
减速到s
γ
；令机器人从s
α
开始，以最大加速度a
max
加速到s
t1
时的路径长度为al1，其中令机器人从s
t1
开始，以最小加速度a
min
减速到s
γ
时的路径长度为dl1，其中al1和dl1满足等式al1+dl1＝β，该等式中只有一个变量s
t1
，求解该等式获得s
t1
的值；(4)如果β＞l1+l2，设定s
β
从s
α
开始，以最大加速度a
max
加速到最大速度s
max
，再以最小加速度a
min
减速到s
γ
；当s
α
＝s
γ
时，令机器人从s
α
开始，以最大加速度a
max
加速到s
max
，再以最小加速度a
min
减速到s
γ
时，整条路径的长度为l3；机器人在第二条直线段上的速度s
β
有两种：(1)如果直线路径段β＜l3，设定s
β
从s
α
开始，以最大加速度a
max
加速到s
t2
，然后再以最小加速度a
min
减速到s
γ
；令机器人从s
α
开始，以最大加速度a
max
加速到s
t2
时的路径长度为al2，其中，其中令机器人从s
t
开始，以最小加速度a
min
减速到s
γ
时的路径长度为dl2，其中al2和dl2满足等式al2+dl2＝β，求解该等式获得s
t2
的值；(2)如果β≥l3，设定s
β
从s
α
开始，以最大加速度a
max
加速到最大速度s
max
，然后再以最小加速度a
min
减速到s
γ
；当s
α
＞s
γ
时，令机器人从速度s
α
出发，以最小加速度a
min
，减速到s
γ
时路径长度为l4。令机器人从s
α
开始，以最大加速度a
max
加速到s
max
，再以最小加速度a
min
减速到s
γ
时，整条路径
的长度为l5；机器人在第二条直线段上的速度s
β
有四种：(1)如果β＜l4，设定s
β
为0；(2)如果β＝l4，设定s
β
从s
α
开始，以最小加速度a
min
，从s
α
减速到s
γ
；(3)如果l4＜β＜l5，设定s
β
从s
α
开始，以最大加速度a
max
加速到s
t3
，然后再以最小加速度a
min
减速到s
γ
。令机器人从s
α
开始，以最大加速度a
max
加速到s
t3
时的路径长度为al3，其中，其中令机器人从s
t3
开始，以最小加速度a
min
减速到s
γ
时的路径长度为dl3，其中al3和dl3满足等式al3+dl3＝β，求解该等式获得s
t3
的值；(4)如果β＞l5，设定s
β
从s
α
开始，以最大加速度a
max
加速到最大速度s
max
，然后再以最小加速度a
min
减速到s
γ
。8.如权利要求1所述的面向变速曲率受限机器人的无碰撞覆盖路径规划方法，其特征在于5.2.3.3步所述计算dp中各条路径对应的耗时集合dt的方法是：5.2.3.3.1初始化dt中的6个元素为ta，令序号r＝1；5.2.3.3.2判断路径dp
r
中是否存在碰撞风险；如果路径dp
r
存在一个航迹点u，航迹点u处的风险势能pe(u)＞0，说明路径dp
r
存在与障碍物发生碰撞的可能，设定dt
r
为ta；否则，说明路径dp
r
是安全的，不存在碰撞风险，按照公式(5)计算得到计算路径dp
r
对应的路径耗时dt
r
，其中，s
α,r
，s
β，r
和s
γ，r
分别代表路径dp
r
中第一个路径段、第二个路径段、第三个路径段上的前进速度；α
r
、β
r
、γ
r
分别表示路径dp
r
中第一个路径段、第二个路径段、第三个路径段的长度；5.2.3.3.3如果r＝6，说明耗时集合dt计算完毕，结束；否则，令r＝r+1，转5.2.3.3.2。9.如权利要求1所述的面向变速曲率受限机器人的无碰撞覆盖路径规划方法，其特征在于6.3步所述根据端点序列tour生成覆盖路径path的方法是：6.3.1初始化端点序列号n＝1，初始化覆盖路径path为空集；6.3.2令tour中第n个端点和第n+1个端点分别为u和v，从路径矩阵cpath中提取出的耗时最小无碰撞路径cpath
u，v
，作为tour中第n个端点到tour中第n+1个端点之间路径path
n
；令path＝path∪path
n
；6.3.3如果n≠2n-1，令n＝n+1，转6.3.2步；否则，转6.3.4计算tour中第2n个端点返回tour中第1个端点的路径；6.3.4令tour中第2n个端点和第一个端点分别为uu和vv，从路径矩阵cpath中提取出cpath
uu，vv
，作为tour中第2n个端点和第一个端点之间路径path
2n
；令path＝path∪path
2n
。

技术总结
本发明公开了一种面向变速曲率受限机器人的无碰撞覆盖路径规划方法，目的是解决现有覆盖路径规划方法因为固定转弯半径和速度引入的安全和效率问题，技术方案是先构建由客户端、服务端和机器人终端构成的面向变速曲率约束机器人的覆盖路径规划系统；客户端采集目标环境信息，服务端对目标区域进行单元分解，得到覆盖单元端点集合P。然后服务端构建出一个表征碰撞风险的势能场，计算出P中端点到端点的耗时最小的变速无碰撞路径。最后，构建数学模型，得到访问集合P所有端点的代价序列最小Tour，进而得到最终的无碰撞路径。本发明通过构建风险势能场和构建变速路径的方式对覆盖路径进行优化，在保证路径安全性的同时，有效提高覆盖效率。提高覆盖效率。提高覆盖效率。


技术研发人员：李林 史殿习 刘衡竹 杨文婧 杨绍武 杨思宁 刘哲 史燕燕 杨焕焕 安浩嘉 连尧宁
受保护的技术使用者：中国人民解放军国防科技大学
技术研发日：2023.03.22
技术公布日：2023/7/17