中国指挥与控制学会会刊 
海洋事业对于我国至关重要,事关国家发展和人民利益。在海洋装备体系中,无人船(unmanned surface vehicle,USV)因其在执行巡逻、监视侦察、环境监测等任务中的出色能力而备受关注,特别是在复杂和不确定水域中的应用潜力巨大。为了实现无人船的安全运行,自主导航技术尤为关键,这包括路径规划、制导与控制等多个方面。其中,障碍物感知作为无人船路径规划的重要前提条件,对其安全性和高效性具有基础性意义。与此同时,路径规划作为自主导航的核心技术,直接影响无人船的运行效率和任务完成效果。
激光雷达(light detection and ranging,LiDAR)是除摄像头外无人船用来进行水面环境感知最常用的传感器,它向目标物体发射激光,然后接收这些物体反射的信号。通过比较接收信号与发送信号的相位差,可以得到目标物体的信息,如距离、反射率等。通过进一步的算法,还可以得到位置、高度、速度、姿态和形状等信息。
障碍物感知是无人船安全行驶的条件之一。PENG[2]通过随机采样一致性算法,对地面和环境的点云进行过滤后,在获取到周围环境的障碍物信息后,使用平滑度参数对欧几里得聚类算法进行重新优化,解决传统欧氏聚类算法对距离阈值敏感,易造成聚类对象过分割或欠分割的问题;RANSAC方法[2]根据不同的几何模型,估计相应的模型参数,在允许的误差范围内对点云数据进行分割,可以有效减少点云噪声对分割的影响,但是由于随机选点,不能较好利用先验信息。
在对环境进行感知后,算法对环境地图进行构建和路径规划便成了下一步的研究目标。SONG等人[3]引入了一种混合算法,该算法是由多项式*和粒子群算法组成的。新算法设法提高了路径的复杂性。然而,它具有很高的计算成本;ZHU等人[4]开发了一种反向标记Dijkstra算法(RLDA),该算法依赖于基本的Dijkstra算法,采用了反向标记,具有更快的收敛速度,但在特定节点时存在一定的计算成本问题;陈羽等人[5]将动态窗口和改进的A*算法融合,解决了无人船水面工作动态避障的实时性问题;赵贵祥等人[6]基于VO算法添加了避碰阈值,有效保障安全避碰的要求;GONG等人[7]在传统的粒子群算法中引入了遗传算法中的交叉和突变操作,能够有效避免进入局部最优解,全局路径更短,算法收敛更快。而RRT规划器易于实现,可以使用随机采样有效地找到可行的路径,并且可以进行调整以满足不同的规划目标和运动约束[8]。Tauhidul[9]提出了一种深度向量场-快速探索随机树算法,应对RRT现有方法仅考虑通过传感器测量的矢量场的空间变化,而忽略了矢量场的时间变化的问题。我国对无人船投入的研发成本提高,国内研发出的无人船硬件条件和软件算法逐步升级,而无人船的工作环境通常是不可预测的未知环境,保证无人船在未知环境下安全航行是非常关键的。为了满足这一要求,无人船需要极高的智能化水平作为支撑,即无人船可以通过船载传感器感知周围水面环境,对周围存在的岸基、礁石以及其他水面障碍物进行检测识别,从而规划出一条安全可行的航行路径,以顺利执行航行任务。为此,本文主要展开基于激光雷达对无人船所处水面环境进行目标检测和路径规划的研究。
1 三维激光点云处理
1.1 点云滤波
激光雷达在使用的过程中投射出的点云也会受环境因素的一些影响,比如温度的改变、光线强度的改变、水面涟漪,它们都会造成不同程度的噪声,但其影响远远不如环境因素改变对摄像头造成的影响。当夜晚光线很暗时,摄像头将无法正常进行检测任务,而激光雷达仍可正常执行任务。这些噪声增加了后续无人船路径规划算法所需要计算的数据量,因此对激光雷达点云数据进行分析和滤波是非常有必要的。本文采用双边滤波算法对点云进行去噪,它通过比对采样点附近的近邻点来对采样点进行校准和完成滤波。
在点云模型中,设点云集合为
$C=\left\{p_{i} \in R^{3} \mid i=1,2, \cdots, n\right\}$
式中,pi 是采样点,n是自然数。
算法将与采样点相邻的点定义为近邻域点,设置阈值为0.1,双边滤波可以定义为
$\hat{p}_{i}=p_{i}+\lambda n_{i}$
式中,$ \hat{p}_{i}$ 是pi 经过双边滤波后产生的新点,$ \boldsymbol{n}_{i}$是单位法向量。λ是双边滤波权重因子。此外,
$\lambda=\frac{\sum_{p_{j} \in N\left(p_{i}\right)} W_{C}\left(\left\|p_{j}-p_{i}\right\|\right) W_{S}\left(\left|\left\langle\boldsymbol{n}_{j}, \boldsymbol{n}_{i}\right\rangle-1\right|\right)\left\langle\boldsymbol{n}_{i}, p_{j}-p_{j}\right\rangle}{\sum_{p_{j} \in N\left(p_{i}\right)} W_{C}\left(\left\|p_{j}-p_{i}\right\|\right) W_{S}\left(\left|\left\langle\boldsymbol{n}_{j}, \boldsymbol{n}_{i}\right\rangle-1\right|\right)}$
式中,$ \boldsymbol{n}_{j}$是pi 的近邻域点pj 的单位法向矢量;WC 是空间域权重,WS是特征域权重。
1.2 水面分割
点云水面分割是激光雷达检测目标中非常关键的一步,在三维激光雷达进行障碍物检测的过程中,激光雷达的点云数据是一帧一帧地进行传输的,每一帧激光点云数据不仅包括障碍物信息,同时还存在大量的地面数据,比如湖岸线以及岸上的树木和行人等。此时需要进行水面分割,区分出原始点云中的水面点和非水面点。
激光点云的分割方法大致可分为3类:基于边缘、基于区域以及基于模型。
表1 GPF算法伪代码Tab.1 GPF Algorithm pseudocode |
| GPF算法伪代码: |
|---|
| 结果:Pg地面点云、Png非地面点云 |
| 1.初始化参数:Pin、Niter、NLPR、Thseeds、Thg |
| 2.Main loop: |
| 3.Pg=ExtraInitialseeds(Pin,NLPR,Thseeds); |
| 4.for i=1:Niter do |
| 5. model=EstimatePlane(Pg); |
| 6. Clear(Pg,Png); |
| 7. for k=1:Pin do |
| 8. if model(pk)﹤Thg then |
| 9. Pg←pk; |
| 10. else |
| 11. Png←pk; |
| 12. end |
| 13. end |
| 14.end |
| 15.ExtraInitialseeds: |
| 16.Psorted=SortOnHeight(Pin); |
| 17.LPR=Average(Psorted(1:NLPR)); |
| 18.for k=1:Pin do |
| 19. if pk.height﹤LPR.height+Thseeds then |
| 20. seeds←pk; |
| 21. end |
| 22.end |
| 23.return(seeds) |
图中分割为地面的点云用白色表示,非地面点云为彩色,可以看到,GPF算法对于环境的适应能力较强,可有效地完成分割任务。
1.3 障碍物聚类
在检测到水面目标后,下一步需要做的便是将目标分类,这一步骤即为障碍物聚类,聚类本质上就是通过目标的某些相似属性对目标实现归类,本文采用经典的欧氏聚类算法对水面目标进行聚类。算法的具体流程如图4所示。
本文算法中有3个关键参数:聚类半径阈值dth以及最大、最小聚类数目阈值。根据激光雷达的工作原理可知,距离激光雷达越近的地方点云密度越大,而远处的点云则较为稀疏,因而聚类半径阈值dth应根据距离有所调整。本文将点云根据半径分为3部分,根据经验调试分别设置如表2所示的阈值。
表2 聚类半径参数Tab.2 Clustering radius parameter |
| 半径范围/m | 0~30 | 30~45 | 45~60 |
|---|---|---|---|
| dth | 1.0 | 1.5 | 2.0 |
分割算法不能有效完全地分割地面的数据,会残留一些噪声点,如果聚类时将这些点视为障碍物,规划层则有可能错误判断当前的环境情况,不能得出最优规划结果,因而根据经验,设置最小聚类数目Nmin为20,如果小于该数值,则将该障碍物过滤掉。无人船航行的过程主要关心水面中障碍物,而岸边的建筑、树木等,本文将使用最大聚类阈值将其过滤,使用激光雷达扫描时,单个障碍物点云不会超过300个,因此本文设置该阈值Nmax为300。
2 无人船水面路径规划
上节研究了基于激光雷达的目标检测方法,通过获取无人船周围环境信息以及障碍物信息,为下一步的研究打下了基础。该节将对无人船的水面路径规划展开研究,对障碍物进行分析,最终使无人船在水面上避开障碍物并规划出一条路径,以确保可以安全、准确地到达指定的目标点。路径规划的原理框图如图7所示。
2.1 基于采样的随机安全区域KD-Tree生成方法
本文中的算法是利用KD-Tree数据结构执行的,该算法的原理是快速地搜索最近的邻近点。从路径规划的角度来看,最近邻居搜索也可以用来寻找三维空间中任意一个无人占据的位置到地图上最近的邻近点之间的距离。这个距离表示相对于这个位置的安全半径,根据这个距离可以产生一个球形的安全区域。基于这一思想,本文设计了一种基于点云采样的寻径方法,为无人船在水面环境中导航开辟空间。
如上文所述,可以通过在KD-Tree中快速搜索最近邻居点的方式找到地图中任意点的安全半径。与此同时,将定义一个球形的安全区域。基于此特性,本文设计了一种基于RRT*的方法,称为安全区域RRT*,用来生成将起点连接到目标点的安全路径。首先,本文遵循RRT*的核心思想,构造一棵随机采样的KD-Tree,其顶点为球形安全区域的中心,边缘为此类安全区域之间的连通桥梁。当生成新的样本时,将对KD-Tree进行扩展,然后,安全路径将由一系列连接的球形安全区域组成。实际上,由球形安全区域组成的随机树也可以看作是拓扑图,它表示环境中从起始位置到目标位置的自由空间分布。随机树扩展是本文设计的算法的基础,其伪代码如表3、表4所示。
表3 安全区域RRT*扩展算法伪代码Tab.3 Security area RRT* extended algorithm pseudocode |
| 安全区域RRT*扩展算法 |
|---|
| 1.获取P,M |
| 2.T←ns,t←0,Ω←M |
| 3.while t≤tmax do |
| 4. cr←Sample(Ω) |
| 5. nn←Nearest(cr,T) |
| 6. nr←Steer(cr,nn) |
| 7. if VolumeCheck(nr) then |
| 8. {nnear}←Near(nr,T) |
| 9. np←ParentChoose(nr,nn,{nnear}) |
| 10. T←nr∪T |
| 11. T←Rewire(nr,{nnear},T) |
| 12. if GoalContain(nr) AND nr·f﹤fbest then |
| 13. fbest←nr·f |
| 14. Ω←Update(fbest,Ω) |
| 15. end if |
| 16. end if |
| 17. t←Time( ) |
| 18.end while |
| 19.C←min(PathExtract(T)) |
| 20.return C |
表4 计算新节点nr算法伪代码Tab.4 Algorithmic pseudocode for computing new nodes |
| nr←Steer(nc,cr) |
|---|
| 1.获取P |
| 2.c←Intersect(cr,nn) |
| 3.r←RadiusSearch(nr·c,P) |
| 4.nr·c←c |
| 5.nr·r←r |
| 6.return nr |
假设有点云原始数据P和规划M的原始解空间,本文以无人船起始位置为根节点ns,初始化随机搜索树T,并将采样区域Ω被初始化为M。KD-Tree中的一个节点n表示自由空间中的一个安全区域,其中心的三维位置为n.c,其半径的长度为n.r,到根节点的总路径代价为n.f。在采样函数Sample(Ω)获取新的随机坐标cr后,函数Nearest(cr,T)在KD-Tree中即可找到最近的邻居点nn。算法通过Steer(cr,nn)函数来确保在生成树时,连接顶点的边缘即球形安全区域没有发生碰撞。如表4所示,Intersect(cr,nn)函数产生了一条由cr到nn.c的射线,函数的返回值为该射线与nn球体的交点,将此坐标设置为新节点nr.c的中心。然后,执行半径搜索函数RadiusSearch(nr.cr,P),在点云原始数据P中找出以nr.c为中心的最大球面的半径,将其设为nr.r。函数VolumeCheck(nr)用来检查新节点nr,以确认它有足够的空间供无人船安全通过。如果nr符合要求,则搜索树T中其所有的邻居节点{nnear}。然后,按照与RRT*算法[11]相同的方式,由函数ParentChoose(nr,nn,{nnear})筛选出nr的父节点并且由函数Rewire(nr,{nnear},T)重新构建KD-Tree。
本文的算法创建了一个仅以最近节点球体为中心的新节点,因此,每两个节点之间的连接自然不会发生冲突,且不需要冲突检查这一步骤。此外,由于安全空间决定了采样点的转向距离,KD-Tree的扩展速度会根据障碍物的密度而变化,从而使其特别适用于具有大量自由空间的环境。
2.2 路径优化
在获得了初始航行路径之后,无人船需要跟踪目标路径。由于安全区域RRT*算法的初始化是在非常有限的时间内完成的,生成的航行路径的质量可能较差。因此,需要在无人船航行的时候不断完善在线路径,从而最终缩短路径。当逐步添加样本并完善航行路径时,路径一致性也是一个至关重要的问题。本文遵循RRT*的原则来修复部分所生成路径,并将重点放在改善其余路径上。这样,在执行范围内的航行路径是一致的,如图8所示。
在图8中,障碍物用橙色的几何图形表示,安全的航行范围被形象化为蓝色球体,即航行通道,黑色路径是到全局计划目标的生成路径,而绿色路径是已规划好的局部路径。用tm表示轨迹的持续时间,选择tm的基本要求是所规划的路径必须在无人船的感知范围内。由于这一部分路径将在不做进一步修改的情况下执行,必须通过对传感器的最新观测来保证其可行性。在航行计划框架中,找到初始航行路径后,将生成的路径发送到无人船进行跟踪。这时,在[0,tm]的路径已经被固定,将不再发生变化。算法将设置KD-Tree新的根节点,其中包含已规划好的路径的结束坐标。此外,算法剪切了所有未连接到新的根节点的其他分支,采样区域Ω也随之更新。在无人船跟踪已规划好的路径期间,随机树会不断扩展和重新布线以提高航行路径的质量。当无人船航行到规划路径的末尾时,将根据树中当前维护的最佳航行路径生成新路径,并确定新的全局路径。该过程将反复执行,直到无人船到达全局导航目标为止。
2.3 安全动态可行的路径规划
2.3.1 分段伯恩斯坦基础轨迹数学表达
本文使用伯恩斯坦多项式,并将路径表示为分段贝塞尔曲线。首先比较这两个基础公式。变量i的第i阶多项式的基为ti 。每段分段路径的多项式函数可以表示为
$ p_{j}(t)=p_{j}^{0}+p_{j}^{1} t+p_{j}^{2} t^{2}+\cdots+p_{j}^{n} t^{n} $
式中,n是一阶多项式的次数,$ \left[p_{j}^{0}, p_{j}^{1}, \cdots, p_{j}^{n}\right]$是第j条路径的系数集。
一阶伯恩斯坦多项式的基$ b_{n}^{i}(t) $定义为
$ b_{n}^{i}(t)=\left[\begin{array}{c}n \\i\end{array}\right] \cdot t^{i} \cdot(1-t) n-i $
式中,n是伯恩斯坦多项式的次数,$ \left[\begin{array}{ll}n & i\end{array}\right]^{T} $是二项式系数。由此基构成的多项式函数称为贝塞尔曲线,其写作
$ B_{j}(t)=\sum_{i=0}^{n} c_{j}^{n} b_{n}^{i}(t) $
式中,$[c_{j}^{0},c_{j}^{1},\cdots,c_{j}^{n}]$表示为cj,是贝塞尔曲线的第j条控制点的集合。
实际上,对于贝塞尔曲线,算法可以将控制点视为伯恩斯坦的基的权重,并将伯恩斯坦的基也视为控制点的权重。贝塞尔曲线参数化定义变量t的范围是t∈[0,1]。算法需要将参数t缩放为每条曲线的任意分配时间。因此,可以将x,y,z中一维μ的m分段贝塞尔曲线表示为如下形式:
$f_{\mu}(t)=\left\{\begin{array}{c}s_{1} \cdot \sum_{i=0}^{n} c_{\mu_{1}}^{i} b_{n}^{i}\left(\frac{t-T_{0}}{s_{1}}\right), t \in\left[T_{0}, T_{1}\right] \\s_{2} \cdot \sum_{i=0}^{n} c_{\mu_{2}}^{i} b_{n}^{i}\left(\frac{t-T_{1}}{s_{2}}\right), t \in\left[T_{1}, T_{2}\right] \\\vdots \\s_{m} \cdot \sum_{i=0}^{n} c_{\mu_{m}}^{i} b_{n}^{i}\left(\frac{t-T_{m-1}}{s_{m}}\right), t \in\left[T_{m-1}, T_{m}\right]\end{array}\right.$
式中,cji 是第j段路径的第i个控制点,T1,T2,…,Tm 是每段路径的结束时间,贝塞尔曲线的总持续时间为T=Tm-T0,s1,s2,…,sm 是每条曲线的比例因子,其作用是将时间间隔从[0,1]扩展到每一段路径的分配时间[Ti-1,Ti]。
2.3.2 强制约束规则
对于分段路径生成问题,算法必须强制执行多个约束条件以确保路径的平滑性、安全性和动态可行性。对于分段贝塞尔曲线的每条曲线来说,其高阶导数可以用相应的低阶控制点的线性组合表示为
式中,l是导数的阶数,n是伯恩斯坦的基的阶数。
路径必须经过几个航路点,例如无人船的起始位置和目标位置以及它们的n阶导数。根据端点插值属性,贝塞尔曲线总会经过第一个和最后一个控制点。因此,本文通过在相应的控制点上设置相同的约束条件,就可以直接实现航路点约束。对于μ维起始处的第l(l≤n)阶导数(即航路点)dμj(i),即第j条路径,有
$a_{\mu_{j}}^{l, 0} \cdot s_{j}^{(1-l)}=d_{\mu_{j}}^{(l)}$
贝塞尔曲线B(t)由一组控制点c组成,这些控制点被完全限制在所定义的凸包内。由于贝塞尔曲线存在凸包属性,需要添加安全性约束,将一段路径内的所有控制点置于2.2节描述的相应球体内。因为球体是一个凸空间,并且所有控制点都在其中,所以这些控制点的凸包肯定在球体内,这就意味着路径的每一段都被限制在球体内部。所有控制点的安全性约束均采用二次约束。对于第j段路径的控制点,其约束为
$\sum_{\mu \in\{x, y, z\}}\left(c_{j_{i}}^{\mu}-p_{j}^{\mu}\right)^{2} \leqslant r_{j}^{2}$
式中,pj 和rj 是第j段路径对应的球体的中心和半径。
图9为该算法生成路径的仿真结果,其中无人船的起始位置用红色圆点表示,目标位置用绿色圆点表示,中心的矩形代表障碍物,黄色分支代表无人船在当前位置通过快速扩展随机树的方法规划出的局部路径,紫色路径即为规划出的全局路径,下文将针对此工况进行实验验证。
3 实验平台设计
4 算法实验验证
本节主要针对上文的算法和仿真进行实验验证,由于水深及回收难度大等原因,不方便在湖面布置真实的障碍物,故用另一艘无人船作为水面障碍物进行实验。
由于激光雷达是被固定在无人船顶端的,激光雷达的光束发射角度范围为-15°~15°,故搭载有激光雷达的无人船处于检测盲区,不在地图中。
路径规划过程中将视距拉近,图16为局部路径规划的过程,红色球体为无人船起始位置,绿色球体为无人船当前位置,围绕着当前位置的红色球体即为安全区域。当前位置与起始位置之间的连线为算法规划出的路径。安全区域RRT*路径规划算法会根据无人船当前位置周围的障碍物信息进行局部路径规划。
综上,无人船可较好地感知周围环境并根据检测到的水面障碍物信息进行路径规划,验证了第2节所介绍的通过快速扩展随机树实现的安全区域的RRT*路径规划算法是有效可行的。
5 结束语
无人驾驶是当下最热门的研究方向之一,其研究领域已经逐渐从地面上的无人车向天空中的无人机和海洋中的无人船发展。获取环境信息和路径规划是无人驾驶中最重要的两个概念,也是进行避障的前提。本文对通过三维激光雷达检测到无人船周围的环境信息进行获取并进行滤波、分割和聚类,然后利用这些数据进行无人船水面路径规划;最后通过搭建的无人船实验平台在人工湖中对算法进行了实验验证。所得结论概括如下:
(1)在目标检测部分,通过处理雷达点云数据完成目标检测,其过程主要包括点云滤波、水面分割和障碍物聚类。对于原始点云采用双边滤波算法滤除冗余点,然后介绍了GPF分割算法,并通过实验验证了算法的有效性;水面分割完成后,引入欧氏聚类算法对水面点云进行聚类;最后通过实验验证以上算法能够对障碍物进行有效的聚类分割。
(2)在路径规划部分,首先介绍了一种基于采样的随机安全区域KD-Tree生成方法,并设计了一种通过快速扩展随机树实现安全区域的RRT*路径规划算法;然后,对每一段局部路径施加强制约束规则从而实现局部路径优化,并通过仿真验证了上述算法的可行性。
(3)搭建了一套无人船实验平台,针对前文的水面目标检测和路径规划算法分别进行实验验证和分析,结果表明本文中的算法是有效且稳定的。
后续研究将考虑动态目标对环境感知的准确性和鲁棒性所产生的负面影响,对动态目标滤波方法进行研究。