本发明公开了一种基于动态球形窗口的三维空间路径规划方法。该方法包括：在通过传感器感知环境信息的基础上滤波整合进而转化为空间栅格地图；建立动态球形窗口模型，对球体内部及表面进行动态采样以得到局部候选目标点；建立统一评价函数，根据评价函数对候选点进行最优化筛选；通过局部目标点逐渐向终点逼近直至最终达到目的地。本发明可应用于无人机\无人潜水器在三维空间中的快速路径规划问题，充分考虑到了三维空间中的高自由度，可快速得到兼顾安全性与效率的无碰撞三维路径。
1.一种基于动态球形窗口的三维空间路径规划方法，其特征在于，包含以下步骤：(1)感知环境得到环境三维点云，对此三维点云进行滤波整合；
(2)设置障碍物特征地图分辨率Res，将滤波整合后的三维点云插入地图，得到基于空间栅格的障碍物特征地图M(Res)；
(3)根据场景、目标物大小确定障碍物探测窗口W(a，b，h)的三维尺寸：长a，宽b，高h，以当前点O′为中心展开该窗口；
(4)建立空间球形窗口采样模型S(R)，初始化球体最大半径R，在球形窗口中，采样得到空间点记为P，在特征地图M中确定点P的可访问性，如果可访问则将其加入候选列表Candidate；
F(P)＝α·direction(θ，φ)+β·dist(P，W)+γ·step(O′，P)其中，direction(θ，φ)是俯仰角θ-方位角φ的方向评价函数，用于评价从当前点O′出发到候选点P的角度与当前点O′到最终目标点T的角度之间的差距；
dist(P，W)是安全性评价函数，计算候选点P到障碍物探测窗口W(a，b，h)内障碍物Obsi的最小距离，以此作为度量候选点安全性的指标；
step(O′，P)为效率评价函数，计算从当前点O′到候选点P之间的步长，以此来表征路径效率；
α，β，γ是子评价函数的三个权重参数，均取(0，1]；
遍历候选列表Candidate，找到最优候选点作为局部目标点，并到达该局部目标点；
(6)判断当前是否到达目标点T，计算当前点O′距离目标点T的直线距离d，设置距离阈值δ，当d≤δ时，认为已到达目标点，结束路径规划；否则，执行步骤(7)；
(7)根据障碍物情况动态调整障碍物探测窗口W(a，b，h)尺寸、球形窗口采样模型S(R)的最大半径R以及到达目标的距离阈值δ，然后执行步骤(3)。
2.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(1)中，利用激光雷达相机或双目相机或深度相机感知环境得到环境三维点云，并对三维点云使用体素滤波器和统计滤波器进行滤波整合。
3.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(2)中，使用射线插入方法将三维点云插入障碍物特征地图。
4.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(3)中，设置障碍物探测窗口后，使用障碍物特征地图边界迭代器快速遍历区域内部障碍物。
5.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(4)中使用球坐标建立空间球形窗口采样模型S(R)，按照相对球心距离ρ、俯仰角θ、方位角φ的顺序进行采样。
6.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(5)中，俯仰角θ-方位角φ的方向评价函数需和180°进行减法运算以保证结果的优劣与数值的大小成正比，俯仰角、方位角分别进行运算然后将最终结果进行求和，具体公式如下：direction(θ，φ)＝(180°-δθ)+(180°-δφ)分别指从第i个候选点Pi指向最终目标点T向量的俯仰角和方位角；
分别指从当前点O′指向第i个候选点Pi向量的俯仰角和方位角。
7.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(5)中，安全性评价函数需设置统一上限，防止在没有障碍物的情况下安全评价函数数值过大影响最终评价结果。
8.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(5)中，三项子评价函数的数值在分析对比前需进行统一的平滑化处理，将其限制在
9.根据权利要求1所述的基于动态球形窗口的三维空间路径规划方法，其特征在于，所述步骤(7)中，在前一步中障碍物探测窗口W(a，b，h)内无障碍时，下一次规划时放大障碍物探测窗口W(a，b，h)尺寸、球形窗口采样模型S(R)的最大半径R以及到达目标的距离阈值δ，然后执行步骤(3)；
如果前一步中障碍物探测窗口W(a，b，h)内存在障碍物，且障碍物探测窗口W(a，b，h)尺寸、球形窗口采样模型S(R)的最大半径R以及到达目标的距离阈值δ均大于初始设定值，则对上述三者进行适当缩小，然后执行步骤(3)，否则，不进行修改，直接执行步骤(3)。
基于动态球形窗口的三维空间路径规划方法
技术领域
[0001] 本发明涉及三维空间环境感知建模与路径规划技术，尤其涉及一种基于动态球形窗口在三维空间进行快速路径规划的方法。
背景技术
[0002] 路径规划技术是机器人、无人机等无人系统投入实际应用时所必需的重点技术。随着当前传感器技术的不断突破，对于环境感知的速度与精度也不断提高，在此基础上可以更为准确安全的进行无人系统的路径规划。
[0003] 路径规划的目的是在有障碍物的环境中按照某种评价指标寻找一条从起始点到目标点的最优或次优无碰撞路径。现有技术可大致分为全局路径规划与局部路径规划两大类。全局路径规划是指在有先验地图信息的基础上，利用全局地图信息找到符合要求的从起始点到达目标点的最优路径。全局路径规划的优势是其充分考虑到了整体地图信息，并且可以得到全局唯一的最优路径。但是由于其严重依赖于先验信息以及有着庞大的计算量，因此实时性较差。局部路径规划技术则侧重考虑无人系统当前位置周围的环境信息，这使得无人系统有着较好的避障能力。因此，与全局路径规划相比，局部规划技术有着更好的实时性以及实用性，对动态环境也有着较强的适应能力。
[0004] 动态窗口法是一种局部路径规划方法，相比于遗传算法、模糊算法等其他局部规划算法，动态窗口法有着计算量小、实时性好的优点。基于速度、角度空间(v，ω)采样的动态窗口法已经在二维地面机器人上进行了应用，取得了不错的避障效果。但是随着诸如无人机、无人潜水器等新兴智能的无人系统出现，无人系统的运动自由度已经从二维平面扩展到三维空间，为解决此类问题，一种常见的技术路线是：
[0005] 1)首先根据传感器数据对三维环境进行建模，重点勾勒障碍物模型。
[0006] 2)将有着障碍物模型的三维环境地图向下投影建立二维平面上的栅格化代价地图。
[0007] 3)在上述代价地图的基础上，运用现有路径规划技术进行处理，得到一条到达目标点的无碰撞路线。
[0008] 上述技术路线的核心思想是三维空间向下投影得到二维栅格地图，这种方法在一般情况下的确能够快速得到可行的无碰撞路径，但是也不可避免的存在着三维空间中存在路径但在二维空间中无法得到有效结果的情况。此外，这种方法的实质是忽略了三维空间垂直方向上的自由度，实际使用范围有限，此外还使得无人机、潜水器等无人系统灵活机动的优势荡然无存，无法得到令人满意的结果。
发明内容
[0009] 本发明的目的在于针对现有技术的不足，提出一种基于动态球形窗口的三维空间路径规划的方法，综合考虑了三维空间高自由度的特性，极大地提升了规划路径的质量与可靠性。
[0010] 本发明的目的是通过以下技术方案来实现的：一种基于动态球形窗口的三维空间路径规划方法，该方法包含以下步骤：
[0011] (1)感知环境得到环境三维点云，对此三维点云进行滤波整合；
[0012] (2)设置障碍物特征地图分辨率Res，将滤波整合后的三维点云插入地图，得到基于空间栅格的障碍物特征地图M(Res)；
[0013] (3)根据场景、目标物大小确定障碍物探测窗口W(a，b，h)的三维尺寸：长a，宽b，高h，以当前点O′为中心展开该窗口；
[0014] (4)建立空间球形窗口采样模型S(R)，初始化球体最大半径R，在球形窗口中，采样得到空间点记为P，在特征地图M中确定点P的可访问性，如果可访问则将其加入候选列表Candidate；
[0015] (5)建立统一评价函数模型F(P)：
[0016] F(P)＝α·direction(θ，φ)+β·dist(P，W)+γ·step(O′，P)[0017] 其中，direction(θ，φ)是俯仰角θ-方位角φ的方向评价函数，用于评价从当前点O′出发到候选点P的角度与当前点O′到最终目标点T的角度之间的差距；
[0018] dist(P，W)是安全性评价函数，计算候选点P到障碍物探测窗口W(a，b，h)内障碍物Obsi的最小距离，以此作为度量候选点安全性的指标；
[0019] step(O′，P)为效率评价函数，计算从当前点O′到候选点P之间的步长，以此来表征路径效率；
[0020] α，β，γ是子评价函数的三个权重参数，均取(0,1]；
[0021] 遍历候选列表Candidate，找到最优候选点作为局部目标点，并到达该局部目标点；
[0022] (6)判断当前是否到达目标点T，计算当前点O′距离目标点T的直线距离d，设置距离阈值δ，当d≤δ时，认为已到达目标点，结束路径规划；否则，执行步骤(7)；
[0023] (7)根据障碍物情况动态调整障碍物探测窗口W(a，b，h)尺寸、球形窗口采样模型S(R)的最大半径R以及到达目标的距离阈值δ，然后执行步骤(3)。
[0024] 进一步地，所述步骤(1)中，利用激光雷达/双目/深度相机感知环境得到环境三维点云，并对三维点云使用体素滤波器、统计滤波器进行滤波整合。
[0025] 进一步地，所述步骤(2)中，使用射线插入方法将三维点云插入障碍物特征地图。
[0026] 进一步地，所述步骤(3)中，设置障碍物探测窗口后，使用障碍物特征地图边界迭代器快速遍历区域内部障碍物。
[0027] 进一步地，所述步骤(4)中使用球坐标建立空间球形窗口采样模型S(R)，按照相对球心距离ρ、俯仰角θ、方位角φ的顺序进行采样。
[0028] 进一步地，所述步骤(5)中，俯仰角θ-方位角φ的方向评价函数需和180°进行减法运算以保证结果的优劣与数值的大小成正比，俯仰角、方位角分别进行运算然后将最终结果进行求和，具体公式如下：
[0029] direction(θ，φ)＝(180°-δθ)+(180°-δφ)
[0030]
[0031]
[0032] 分别指从第i个候选点Pi指向最终目标点T向量的俯仰角和方位角；
[0033] 分别指从当前点O′指向第i个候选点Pi向量的俯仰角和方位角。
[0034] 进一步地，所述步骤(5)中，安全性评价函数需设置统一上限，防止在没有障碍物的情况下安全评价函数数值过大影响最终评价结果。
[0035] 进一步地，所述步骤(5)中，三项子评价函数的数值在分析对比前需进行统一的平滑化处理，将其限制在0～1之间。
[0036] 进一步地，所述步骤(7)中，在前一步中障碍物探测窗口W(a，b，h)内无障碍时，下一次规划时放大障碍物探测窗口W(a，b，h)尺寸、球形窗口采样模型S(R)的最大半径R以及到达目标的距离阈值δ，然后执行步骤(3)；
[0037] 如果前一步中障碍物探测窗口W(a，b，h)内存在障碍物，且障碍物探测窗口W(a，b，h)尺寸、球形窗口采样模型S(R)的最大半径R以及到达目标的距离阈值δ均大于初始设定值，则对上述三者进行适当缩小，然后执行步骤(3)，否则，不进行修改，直接执行步骤(3)。
[0038] 本发明的有益效果是：传统的将三维地图投影到二维平面再进行路径规划的方法在一些情况下虽然能够得到最终路径，但是其质量较低，而且由于忽略了垂直方向自由度，使用范围受限，鲁棒性不高。本方法创新性地将采样窗口扩展到三维空间，建立动态球形采样窗口。通过建立统一可靠的评价函数直接对三维空间中的候选点进行评估，得到局部最优目标点。此外还根据障碍物情况动态调整窗口大小，使得规划结果更加安全灵活。本方法充分利用了三维空间中高自由的特性，大大提高了在三维空间下路径规划的鲁棒性，所得路径结果也更加自然灵活。
附图说明
[0039] 图1为基于动态球形窗口的三维空间路径规划方法的流程图；
[0040] 图2为球形动态窗口模型；
[0041] 图3为基于八叉树的三维空间特征地图；
[0042] 图4为对于起点终点处于同一水平面时本发明的实际效果图；
[0043] 图5为现有方法效果图；
[0044] 图6为对于起点终点处于不同水平面时本发明的实际效果图。
具体实施方式
[0045] 下面结合附图对本发明具体实施方式做进一步说明。
[0046] 如图1所示，本发明提供的一种基于动态球形窗口的三维空间路径规划方法，具体实施方式如下：
[0047] (1)决定所要进行路径规划的场景，针对不同场景选择不同的环境传感器作为前端感知器。例如，室外场景可选择激光雷达、双目成像系统；室内场景可选择单\双目相机或者RGB-D相机。通过对三维环境进行感知，得到含有深度信息的三维点云。本实施例使用体素滤波器、统计滤波器方法对含有杂点点云进行滤波处理。
[0048] (2)在得到三维空间点云的基础上，设置障碍物特征地图的分辨率Res，本实施例设置为0.01m，使用射线插入方法将三维点云插入特征地图，得到基于空间栅格的障碍物特征地图M(Res)。
[0049] (3)根据场景、目标物大小确定障碍物初始探测窗口W(a，b，h)的三维尺寸：本实施例设置初值长a＝2m，宽b＝1m，高h＝1m，根据该三维尺寸以及当前点O′坐标设置边界迭代器初始参数，为当前点O′中心展开该窗口，快速查询确定处于该窗口内的被占有率大于0.5的格点，将其视为障碍物。
[0050] (4)建立空间球形窗口采样模型S(R)，初始化球体最大半径R，本实施例中初始取R＝0.2m。使用球坐标建立球形采样空间S(R)，本发明按照相对球心距离ρ，方位角θ，俯仰角φ的顺序进行采样，采样的步长分别设置为0.1m，10°，20°。在球形窗口中，采样得到空间点记为P，在特征地图M中确定点P的可访问性，将其加入候选列表Candidate。
[0051] (5)建立统一评价函数模型F(P)：
[0052] F(P)＝α·direction(θ，φ)+β·dist(P，W)+γ·step(O′，P)[0053] 其中，对于俯仰角θ-方位角φ方向评价函数，根据以下公式进行计算：
[0054] direction(θ，φ)＝(180°-δθ)+(180°-δφ)
[0055]
[0056]
[0057] 是指从候选点P指向最终目标点T向量的俯仰角和方位角。同理是指从当前点O′到候选点P向量的俯仰角和方位角。当三点共线时，δθ，δφ均为0，达到最小，方向评价函数此时达到最大。
[0058] 对于dist(P，W)是安全性评价函数，计算候选点P到障碍物探测窗口W(a，b，c)内障碍物Obsi的最小距离，即：
[0059]
[0060] 以此作为度量候选点安全性的指标。
[0061] step(O′，P)为效率评价函数，计算从当前点到候选点之间的步长，以此来表征路径效率。
[0062] step(O′，P)＝||O′-P||
[0063] 然后，还需要对三项子评价函数进行平滑处理：
[0064]
[0065]
[0066]
[0067] 最后设置各子评价函数权重，本实施例设置权重为：α＝1，β＝0.7，γ＝0.1，均保证取值在(0，1]区间之内，求出最终评价函数值。
[0068] 遍历候选列表Candidate，根据统一评价函数找到最优候选点将其最为局部目标点，并到达该局部目标点。
[0069] (6)判断当前是否到达目标点T，计算当前点O′距离目标点的直线距离d：
[0070] d＝||T-O′||2
[0071] 本实施例设置距离阈值初值δ＝0.2m，当d≤δ时，认为已到达目标点；否则，执行步骤(7)。
[0072] (7)在前一步中障碍物探测窗口W(a，b，h)内无障碍时，下一次规划时放大障碍物探测窗口W(a，b，h)尺寸、球形窗口采样模型S(R)的最大半径R以及到达目标的距离阈值δ，然后执行步骤(3)；
[0073] 如果前一步中障碍物探测窗口W(a，b，h)内存在障碍物，且障碍物探测窗口W(a，b，h)尺寸、球形窗口采样模型S(R)的最大半径R以及到达目标的距离阈值δ均大于初始设定值，则对上述三者进行适当缩小，然后执行步骤(3)。否则，不进行修改，直接执行步骤(3)。
[0074] 实施例1
[0075] 本实施例对比使用本发明基于动态球形窗口的三维空间路径规划的方法与使用现有路径规划方法得到的路径模型。
[0076] 图3为本例的测试场景模型，模型刻画了办公室内一台放有电脑的桌子模型。其中，x坐标指向右侧，y坐标指向下方，z坐标指向桌子模型。
[0077] 情况1：设置起点Start为(0,0,-1)，终点End为(-1,0,5)。具体路径结果为图4(本发明提出方法)、图5(现有方法)。
[0078] 此时，终点与起点处于同一水平面，详细路径分析结果见表1。其中，规划用时指路径规划的总体耗时，中间节点个数是在最终路径结果中所包含的途径的节点数目，路径长度是指最终路径在三维空间中的总长度。由于本方法充分考虑了三维空间的自由度，路径质量的长度明显优于现有方法。虽然由于在三维空间采样增大了计算量，但依然能够满足实时性的要求。
[0079] 表1
[0080] 指标 本发明提出方法 现有规划方法规划用时 0.0146s 0.0127s
中间节点个数 51个 60个
路径长度 7.1m 7.2m
[0081] 情况2：设置起点Start为(0,0,-1)，终点End为(1,-2,3.5)。具体路径结果为图6。
[0082] 由于终点和起点处于不同水平面，现有方法无法直接规划到达终点。本发明提出的基于动态球形窗口的三维空间路径规划方法可以很好的完成规划任务，详细路径分析结果见表2。
[0083] 表2
[0084] 指标 本发明提出方法规划用时 0.0164s
中间节点个数 56个
路径长度 6.2m
[0085] 可以看出，基于动态球形窗口路径规划的方法在进行三维空间路径规划时有着更好的灵活性和鲁棒性，同时也兼顾了实时性，是一种较好的三维路径规划方法。
[0086] 以上仅为本发明具体实施方式，不能以此来限定本发明的范围，本技术领域内的一般技术人员根据本创作所作的均等变化，以及本领域内技术人员熟知的改变，都应仍属本发明涵盖的范围。
