CN114332647B - 一种用于无人船的河道边界检测与跟踪方法及系统 - Google Patents
一种用于无人船的河道边界检测与跟踪方法及系统 Download PDFInfo
- Publication number
- CN114332647B CN114332647B CN202111682613.4A CN202111682613A CN114332647B CN 114332647 B CN114332647 B CN 114332647B CN 202111682613 A CN202111682613 A CN 202111682613A CN 114332647 B CN114332647 B CN 114332647B
- Authority
- CN
- China
- Prior art keywords
- boundary
- points
- river
- point
- river channel
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Landscapes
- Image Analysis (AREA)
- Image Processing (AREA)
Abstract
本发明公开了一种用于无人船的河道边界检测与跟踪方法,包括以下步骤,步骤1、将无人船上的相机与激光雷达进行联合标定;步骤2、利用基于相机图像输入数据进行河道边界检测,分别得到左边界L1和右边界R1;步骤3、通过激光雷达点云数据检测河道边界,将边界点投影到xoy水平面坐标,分别得到左边界L2和右边界R2;步骤4、判断是否需要融合的参数设置,如果需要融合,将步骤2、步骤3中得到边界进行融合,输出融合后的边界线L3和R3,否则输出单步骤2或步骤3的结果,本发明通过利用多种方法检测河道边界,避免单一方法因环境变化导致失效或精度下降,为无人船在河道中的可行驶区域判断提供依据,保证了无人船在河道行驶安全。
Description
技术领域
本发明涉及无人船安全技术领域,具体是一种用于无人船的河道边界检测与跟踪方法及系统。
背景技术
无人船(USV)已被应用于各种河道水域的任务,例如环境测绘、巡逻任务、环境监测和水面垃圾打捞,这就要求无人船能自主巡航,具有一定程度的自主性有助于高效智能地完成任务,为了实现无人船在水面驾驶的自主性,检测河道的可通航区域边界是至关重要的一步,因为它不仅为避障、还为后续路径规划和自动化提供了必要的参考依据。
无人船在河道自主航行时,需要检测所行驶河道的可行驶区域,需要识别河道边界,以保证无人船在相对安全的范围内。计算机对河道信息实时采集和适应性要求高。是指系统必须与无人船同时进行实时数据采集和处理,适应性是指无人船采集图像处理算法必须在不同规则(如结构化河道、非结构化河道等)、不同环境(例如河道边缘的颜色、宽度、长度等因素)、不同的天气条件(例如雨、雪等环境)具有良好的适应性。此外,与路面不同,水面环境容易受到镜面反射干扰。目前,机器视觉系统在河道检测技术上存在瓶颈,河道检测系统受以上因素影响很大,检测效果会下降很多,边缘和河道可行驶区域检测容易误判。
河道边界检测比无人车车道检测复杂,车道主要是宽度固定,以直线为主,车道线两侧边界是平行的,受到干扰少,而无人船在河道受到水面干扰较多,水面镜面反射、水流、波浪等,且河道边界不规则有可能一侧河岸是直线,另一侧河岸是曲线。
道路边界线检测提供安全行驶范围信息,保障船体安全,是无人船感知系统的重要组成部分。河道检测在许多实际应用中是一项至关重要且具有挑战性的任务,例如无人船可行驶区域判断、河道偏离警告、河道巡逻等。河道检测是自动驾驶无人船面临的主要挑战之一。常用的两种传感器是摄像头和激光雷达,然而,它们中的每一个都有一些固有的缺点,为此使用两个传感器融合来综合二者的优点。
相机摄像头和激光雷达各有优缺点,由于水面船只的不稳定性,比如水面波浪和水流等都会影响其数据稳定性,特别是雨天天气,干扰较多。从抗干扰能力上来讲,由于激光雷达通过发射光束进行探测,受环境影响较大,光束受遮挡后就不能正常使用,因此无法在雨雪雾霾天,沙尘暴等恶劣天气中使用。摄像头等视觉测量方法具有效率高、系统结构简单、成本低等优点,其优势在于具有视觉特征,使其和成熟的视觉算法结合进行后处理很容易,但精度比激光雷达低。单一传感器有其局限性,为了充分利用各个传感器的优势,因此同时将激光雷达和摄像头结合起来,两者正好起到一种相辅相成,取长补短的作用。
发明内容
本发明的目的在于提供一种用于无人船的河道边界检测与跟踪方法及系统,为了解决上述问题,我们开发了更加实时和自适应的河道检测系统,对于提高无人船的安全性和自主航行能力具有重要意义。提出了一种用于无人船自主巡航的河道边界检测技术和跟踪方法。同时模糊周围环境,从而在加快提取速度的同时提高提取精度。
为实现上述目的,本发明提供如下技术方案:
一种用于无人船的河道边界检测与跟踪方法,包括以下步骤:
步骤1、将无人船上的相机与激光雷达进行联合标定,实现相机与激光雷达在空间、时间上的对准;
步骤2、利用基于相机图像输入数据进行河道边界检测,分别得到第一河道边界线左边界L1和右边界R1;
步骤3、通过激光雷达点云数据检测河道边界,将边界点投影到xoy水平面坐标,分别得到第二河道边界线左边界L2和右边界R2;
步骤4、判断是否需要融合的参数设置,如果需要融合,将步骤2、步骤3中得到的边界进行融合,输出融合后的第三河道边界线,左边界L3和右边界R3,否则输出单步骤2或步骤3的结果。
作为本发明进一步的方案:步骤2中基于相机基于图像的河道检测包括以下步骤:
步骤2.1、对相机获取的图像数据进行镜面反射快速去除,镜面反射区域部分从感兴趣区域ROI中删除;
步骤2.2、对河道图像噪声过滤,灰度化,Canny边缘检测和感兴趣区域ROI选择;
步骤2.3、用B样条曲线检测河道边界,同时分别计算代价评分,选取得分最高的前m条曲线,加入候选集;
步骤2.4、对直线和曲线拟合结果候选集进行筛选确认,利用最小二乘法选择误差最小的方案作为最终结果。
作为本发明进一步的方案:步骤2.1中的镜面反射去除方法包括以下步骤:
步骤2.1.1、检测包含镜面反射的区域,设置阈值d,去除亮度小于d的区域,其中d是镜面反射像素的最低强度,范围从180到255;
步骤2.1.2、对镜面反射区域进行聚类,选择亮度大于d的区域集合H1,H2,…Hn;
步骤2.1.3、对区域Hi(0<i<n)利用膨胀和腐蚀算法去除孤立或者小的点,分别得到区域Di和Ei;
步骤2.1.4、计算边界区域Bi=Di-Ei,分别计算Bi的三个通道RGB的平均强度;
步骤2.1.5、将检测到的镜面反射区域Hi替换为区域边界Bi中临近无人船当前位置的K近邻领域像素的平均强度。
作为本发明进一步的方案:所述曲线拟合通过迭代的方式实现,具体步骤如下:
步骤2.4.1:初始化最大得分记录max_score=0,最大迭代次数iter_num=1000;
步骤2.4.2:通过随机抽样一致RANSAC方法对感兴趣区域中的可用点进行采样,使用加权采样方法,权重与阈值图像的像素值成正比,有助于选择更多相关点,即具有更高机会属于该河道边界的点;
步骤2.4.3:在上一步采样结果中选择控制点,使用最小二乘法拟合三次B样条曲线,提供样条的控制点,使拟合采样点的平方误差总和最小;
步骤2.4.4:计算当前拟合得分cur_score,如果得分大于max_score,则更新max_score=cur_score,并记录当前拟合结果作为当前最优解;
步骤2.4.5:重新执行步骤2.4.2-2.4.4,直到迭代次数满足要求。
作为本发明进一步的方案:所述步骤3中基于激光雷达的河道边界拟合包括以下步骤:
步骤3.1、对激光雷达扫描的点云数据进行预处理,包括背景过滤、滤波,过滤掉散列点、孤立点;
步骤3.2、检测水面所在的平面,所述水面近似认为是一个平面,平面检测方法为,
z=zsurf
zsurf←argmax{NUM(cond{|z-zsurf|≤∈})}
z是垂直坐标,z=zsurf是使得满足条件|z-zsurf|≤∈的点p(x,y,z)最多的平面,
步骤3.3、过滤水面干扰,对于水平面上的点云进行过滤,得到高于水平面的点云,点的坐标pi(xi,yi,zi),0<i≤n,过滤后的点云集合G为:
G={pi(xi,yi,zi)|i∈N∧|zi-zsurf|>σ}
其中zsurf是水面在垂直方向的坐标,N是从1开始的自然数,σ是河道水平面过滤阈值;
步骤3.4、提取河道边界点,提取边界线上的点加入候选点,分别得到左右河道边界线的点的集合Cleft和Cright,采用点云分类、角度和距离过滤的方法提取候选点,将待提取的点云投影到xoy平面上,提取的候选点遵循方向角和距离满足以下条件,
其中d1、d2、θ1分别是距离和方向角度阈值,Pi(xi,yi),Pj(xj,yh)和Pusv(xusv,yusv)分别是两个相邻点和无人船在xoy平面投影坐标;
按照角度顺序选取第三个候选点Pk(xk,yk)时,相邻三点Pi、Pj、Pk需同时满足以下角度阈值
其中θ2是相邻三点的方向角度阈值;将满足上述阈值条件的点视为同一类边界;
最终得到一系列集合C1、C2、…Cn,选择点数最多的两个Cmax1,Cmax2,其点的个数远大于其余的点集Ci(i≠max1且i≠max2);
再根据Cmax1,Cmax2方向判断得到左右边界点集Cleft,Cright;
步骤3.5、分别对Cleft,Cright,做曲线拟合,得到河道两侧的边界线。
作为本发明进一步的方案:所述步骤3中基于激光雷达的河道边界拟合中通过对下一时刻的边界点进行预测和校准来消除噪声干扰,所述边界点预测和校准通过扩展卡尔曼滤波EKF进行,河道边界点更新时,判断当前检测结果Pcur(xcur,ycur)与前一帧的检测结果Ppre(xpre,ypre)距离满足阈值dm要求,则是有效的预测,并在更新步骤输入到扩展卡尔曼滤波器,否则丢弃当前的检测结果,将前一帧的检测结果作为当前检测结果,阈值要求为:
|xcur-xpre|+|ycur-ypre|≤dm
其中dm是两点之间的曼哈顿距离阈值。
作为本发明进一步的方案:所述步骤4中分别基于图像和激光雷达的方法得到两侧河道边界线融合包括以下步骤:
步骤4.1:将激光雷达河道边界点检测结果投影到相机坐标;
步骤4.2:初始化斜率k,斜率变化步长h;
步骤4.3:对经过无人船中心点Pusv做直线l:y=k(x-xusv)+yusv;
步骤4.4:计算l与L1和L2,R1和R2的交点,取距离最近的点Pi,Pj作为拟合边界候选点;
步骤4.5:令k=k+h,重复步骤2,最终得到P1,P2,…,Pn为候选点;
步骤4.6:对P1,P2,…,Pn边界点得到新的曲线拟合结果左边界L3,右边界R3。
一种用于无人船的河道跟踪方法,其特征在于,包括以下步骤:
步骤3:等间距采样依次重复步骤1和2,得到一系列中间点序列D1、D2…Dm,连接这些点,得到目标点和河道中央平行线为无人船跟踪河道的目标路径;
步骤4:通过PID控制算法调整无人船的跟踪航向,依次逼近目标点和目标路径。
一种无人船系统,包括设置于无人船上的工控机、激光雷达、摄像机、控制板、驱动部、定位系统,所述工控机内置有河道边界检测模块与河道跟踪模块,所述工控机与所述激光雷达、摄像机通过有线和/或无线网络连通,所述工控机通过控制板与所述驱动部连通,所述定位系统包括与所述工控机连通的电子罗盘、GPS/IMU。
与现有技术相比,本发明的有益效果是:
1、本发明通过利用多种方法检测河道边界,避免单一方法因环境变化导致失效或精度下降,为无人船在河道中的可行驶区域判断提供依据,保证了无人船在河道行驶安全;
2、本发明对相机获得的图像数据进行镜面反射去除后再进行河道检测,使得检测结果更准确,鲁棒性更高,本发明通过计算B样条曲线得分,并在得分最高的结果中选择最优方案作为最终边界;
3、本发明分别基于相机与激光雷达两种方法,利用三维点云检测对河道边界修正,避免了二维图像信息缺少深度信息,避免了单一传感器受到干扰导致检测结果不准确的问题;
4、本发明提出了一种基于扩展卡尔曼滤波器的河道边界点跟踪方法,可以提高检测结果的稳定性和准确性;
5、本发明能够使无人船沿着河道的方向自主巡逻,提供了沿河道中央自主航行的航向和距离偏差控制方法。
附图说明
图1为本发明河道边界检测及跟踪方法的简易流程图;
图2为本发明河道边界检测及跟踪方法的详细流程图;
图3为本发明基于激光雷达点云的河道检测流程图;
图4为本发明基于相机图像的河道检测流程图;
图5为本发明河道边界样条曲线评价计算符号说明;
图6为本发明无人船沿河道中央平行线行驶的航向和横向距离偏差计算符号说明;
图7为本发明河道边界线融合示意图;
图8为本发明河道中央行驶跟踪的目标点和目标轨迹示意图;
图9为本发明河道边界线拟合结果;
图10为本发明无人船系统示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
请参阅图1-10,本发明实施例中,一种用于无人船自主巡航的河道边界检测,包括以下步骤:
步骤1、将无人船上的相机与激光雷达进行联合标定,实现相机与激光雷达在空间、时间上的对准。
步骤2、利用基于相机图像输入数据进行河道边界检测,分别得到左边界L1和右边界R1;
步骤2中基于相机基于图像的河道检测包括以下步骤:
步骤2.1、对相机获取的图像数据进行镜面反射快速去除,镜面反射区域部分从感兴趣区域ROI中删除;
步骤2.1中的镜面反射去除方法包括以下步骤:
步骤2.1.1、检测包含镜面反射的区域,设置阈值d,去除亮度小于d的区域;
步骤2.1.2、对镜面反射区域进行聚类,选择亮度大于d的区域集合H1,H2,…Hn;
步骤2.1.3、对区域Hi(0<i<n)利用膨胀和腐蚀算法去除孤立或者小的点,分别得到区域Di和Ei;
步骤2.1.4、计算边界区域Bi=Di-Ei,分别计算Bi的三个通道RGB的平均强度;
步骤2.1.5、将检测到的镜面反射区域Hi替换为区域边界Bi中临近无人船当前位置的K近邻领域像素的平均强度。
具体详细步骤:首先,对每个像素的利用阈值操作来检测包含镜面反射的区域。原始图像的尺寸为m×n,对于区域中像素(x,y),定义:
其中,x∈[0,m],y∈[0,2n/3],Idark(x,y)是像素p(x,y)的最暗值,Es(x,y)是需要去除的镜面反射区域,d是镜面反射像素的最低强度,范围从180到255,在本实施例中,d=196。
其次,对镜面反射区域进行聚类,得到由高光像素点组成的连通区域,识别大的连通区域(小的点由形态学腐蚀操作处理)。为此,找到镜面反射率大的所有区域(A1,A2,…,An),使大小Ak>δ,其中δ是一个固定阈值,表示每个区域中的最小像素数。是实施例δ值固定为50像素,忽略非常小的镜面反射斑点,这些斑点可以通过形态学操作来处理。
再次,将检测到的大的镜面反射区域利用形态学处理得到边界区域。为了从非镜面反射的周围区域检测像素,用两种不同的卷积核(d_kernel和e_kernel)分别对图像进行膨胀和腐蚀,并取它们的差值作为边界。
ADk=Dilate(Ak,d_kernel)
AEk=Erode(Ak,e_kernel)
ADk和AEk分别是d_kernel和e_kernel获得的Ck的扩展区域。
Bordk=ADk-AEk
最后,将镜面反射区域替换为边界像素K近邻领域的平均强度。取位置(边界)k处存在的R、G和B通道的平均值,始终选择靠近无人船的边界区域作为平均区域,并用该平均值填充连接的分量Ak,对于Ak中的任一点p(x,y),通过下式更新其像素值,获得无镜面反射的图像。在Bordk中找到与pi最近的k个点,涵盖这k个点的领域记为Nk(x,y),近无人船K近邻算法差值:
N为Ak中的边界点临近无人船当前位置的K近邻像素数。
其最近邻的k个计算实例点,即K近邻像素pi(xi,yi)必须满足:
步骤2.2、对河道图像噪声过滤,灰度化,Canny边缘检测,感兴趣区域ROI选择;
步骤2.3、用B样条曲线检测河道边界,同时分别计算代价评分,选取得分最高的前m条曲线,加入候选集;
步骤2.4、对直线和曲线拟合结果候选集进行筛选确认,利用最小二乘法选择误差最小的方案作为最终结果;
步骤2.4中的曲线拟合使用三次B样条曲线,定义为
Q(t)=T(t)MP
其中t∈[0,1],Q(0)=pi,Q(1)=Pi+3,点Pi+1和Pi+2控制样条的形状,
拟合性能通过定义如下的样条分数进行评估,选择分数最高的曲线作为拟合结果
Score=s(1+k1l′+k2θ′)
l′=l/h-1
θ′=(θ-1)/2
θ=(cosθ1+cosθ2)/2
其中s是样条曲线的像素总数,l是样条的长度,h是图像的高度,如图5所示,θ1是在Pi+1处相交的两条线之间的夹角,θ2是在Pi+2处相交的两条线之间的夹角,k1和k2是两个正则化因子。一方面,该评价算法可以保证河道边界拟合的结果不会因注重局部拟合精度而造成的曲线过短;另一方面,它也会惩罚大曲率的样条,因为非常弯曲的河道在自然环境中并不常见。
曲线拟合通过迭代的方式实现,具体步骤如下:
步骤2.4.1:初始化最大得分记录max_score=0,最大迭代次数iter_num=1000;
步骤2.4.2:通过随机抽样一致RANSAC方法对感兴趣区域中的可用点进行采样,使用加权采样方法,权重与阈值图像的像素值成正比,有助于选择更多相关点,即具有更高机会属于该河道边界的点;
步骤2.4.3:在上一步采样结果中选择控制点,使用最小二乘法拟合三次B样条曲线,提供样条的控制点,使拟合采样点的平方误差总和最小;
步骤2.4.4:计算当前拟合得分cur_score,如果得分大于max_score,则更新max_score=cur_score,并记录当前拟合结果作为当前最优解;
步骤2.4.5:重新执行步骤2.4.2-2.4.4,直到迭代次数满足要求。
步骤3、通过激光雷达点云数据检测河道边界,将边界点投影到xoy水平面坐标,分别得到左边界L2和右边界R2;
基于激光雷达的河道边界拟合包括以下步骤:
步骤3.1、对激光雷达扫描的点云数据进行预处理,包括背景过滤、滤波,过滤掉散列点、孤立点;
步骤3.2、检测水面所在的平面,所述水面近似认为是一个平面,平面检测方法为,
z=zsurg
zsurf←argmax{NUM(cond{|z-zsurf|≤∈})}
z是垂直坐标,z=zsurf是使得满足条件|z-zsurf|≤∈的点p(x,y,z)最多的平面,
步骤3.3、过滤水面干扰,所述过滤水面干扰,是为了过滤蓝藻、落在水中的树叶等虚假噪声点,为了简化运算,对于水平面上的点云进行过滤,得到高于水平面的点云,点的坐标pi(xi,yi,zi),0<i≤n,过滤后的点云集合G为:
G={pi(xi,yi,zi)|i∈N∧|zi-zsurf|>σ}
其中zsurf是水面在垂直方向的坐标,N是从1开始的自然数,σ是河道水平面过滤阈值;
步骤3.4、提取河道边界点,提取边界线上的点加入候选点,分别得到左右河道边界线的点的集合Cleft和Cright。直接提取的河道边界候选点中有很多假点,河岸边上虚假河道边界候选点基本上是由河道两侧的杂草、树枝和石头造成的。我们采用点云分类、角度和距离过滤的方法提取候选点,具体过程为:首先,我们将待提取的点云投影到xoy平面上,提取的候选点遵循方向角和距离满足以下条件,
其中d1、d2、θ1分别是距离和方向角度阈值,Pi(xi,yi),Pj(xj,yj)和Pusv(xusv,yusv)分别是两个相邻候选点和无人船在xoy平面投影坐标;
按照角度顺序选取第三个候选点Pk(xk,yk)时,相邻三点Pi、Pj、Pk需同时满足以下角度阈值
其中θ2是相邻三点的方向角度阈值;将满足上述阈值条件的点视为同一类边界;
最终得到一系列集合C1、C2、…Cn,选择点数最多的两个Cmax1,Cmax2,其点的个数远大于其余的点集Ci(i≠max1且i≠max2);
再根据Cmax1,Cmax2方向判断得到左右边界点集Cleft,Cright;
步骤3.5、分别对Cleft,Cright,做曲线拟合,得到河道两侧的边界线。所述边界点拟合利用RANSAC(Random Sample Consensus)算法,根据多项式模型,通过多次迭代对边界点进行直线或曲线拟合。拟合结果示例如图9所示;
为了消除噪声干扰,得到平滑的河道边界线,在下一时刻进行边界点拟合前,对边界点进行预测和校准。所述边界点预测和校准通过扩展卡尔曼滤波EKF进行,河道边界点更新时,判断当前检测结果Pcur(xcur,ycur)与前一帧的检测结果Ppre(xpre,ypre)距离满足阈值dm要求,则是有效的预测,并在更新步骤输入到扩展卡尔曼滤波器,否则丢弃当前的检测结果,将前一帧的检测结果作为当前检测结果,阈值要求为:
|xcur-xpre|+|ycur-ypre|≤dm
其中dm是两点之间的曼哈顿距离阈值。
步骤4、判断是否需要融合的参数设置,如果需要融合,将步骤2、步骤3中得到边界进行融合,输出融合后的边界线L3和R3,否则输出单步骤2或步骤3的结果,在本实施例中,当处于浓雾恶劣天气或者其他激光雷达受干扰较大时,则输出步骤2的结果,当处于夜晚或其他摄像机成像较差时则输出步骤3的结果,如图2所示。
步骤4中对分别基于图像和激光雷达的方法得到的两侧河道边界线融合,选择距离无人船横向距离最近的边界点作为正确结果,保证安全,根据上述分别基于图像和激光雷达的方法得到两侧河道边界线,分别取两条线内侧作为拟合结果,通过计算经过Pusv(xusv,yusv)的所有直线与图像和点云得到的河道边界曲线的交点得到,如图7所示,具体融合步骤为:
步骤4.1:将激光雷达河道边界点检测结果投影到相机坐标;
步骤4.2:初始化斜率k,斜率变化步长h;
步骤4.3:对经过无人船中心点Pusv做直线l:y=k(x-xusv)+yusv;
步骤4.4:计算l与L1和L2,R1和R2的交点,取距离最近的点Pi,Pj作为拟合边界候选点;
步骤4.5:令k=k+h,重复步骤4.3-4.4,最终得到P1,P2,…,Pn为候选点;
步骤4.6:对P1,P2,…,Pn边界点得到新的曲线拟合结果左边界L3,右边界R3。
河道边界点跟踪是采用扩展卡尔曼滤波器来跟踪河道边界线,设传感器对河道边界点的测量向量表示为
zt=h(xt)+vt
其中,zt是边界点位置的测量值,观测函数h(·)定义河道边界点的观测模型,vt是测量噪声,其均值为零且噪声协方差为Rt的观测噪声。
(1)河道边界点预测:
xt=f(xt-1)+wt-1
其中,xt是河道边界点在t时刻的位置,函数f(·)表示预测模型的非线性状态转移函数,wt是均值为零的过程噪声,Pt是t时刻的位置协方差矩阵,JF是f的雅可比矩阵,Q是系统噪声协方差矩阵。
(2)河道边界点校准:
一种用于无人船的河道跟踪方法,河道两侧边界线可以不对称,根据河道边界计算拟合河道中央平行线,采样河道中心线的路径点为目标点,通过PID控制无人船沿着河道中心线行驶,如图8所示,具体包括以下步骤:
步骤3:等间距采样依次重复步骤1和2,得到一系列中间点序列D1、D2…Dm,连接这些点,得到目标点和河道中央平行线为无人船跟踪河道的目标路径;
步骤4:通过PID控制算法调整无人船的跟踪航向,依次逼近目标点和目标路径。
根据河道边界检测结果,虚拟出河道中心线,如图6所示,通过PID控制无人船沿着河道中心线行驶,与两侧河岸同时保持最大安全距离以保证安全,避免河道边缘搁浅或者进入浅滩。所述河道中心线与河道两侧边界平行,根据河道中心等距离线采样生成目标轨迹过渡点,通过PID控制算法调整无人船沿河道中心线的跟踪航向,具体描述如下:
无人船在行驶的过程中,由于受到水流和风力等影响,无人船航向及其与目标轨迹之间的距离会随着前进而增大。如图6所示,设Pn-1、Pn为河道中心线巡航路径采样目标点,小船航行的当前角度为θcur,目标方向的角度为θobj,则可得角度
α1=arctan((yn-yusv)/(xn-xusv))
α2=arctan((yn-yn-1)/(xn-xn-1))-α1
θobj=π/2-α1
当前航向角的偏差θe和横向距离偏差de分别为
θe=θobj-θcur
为了保持无人船能够始终沿着河道中央与河道保持平行行驶,始终保持与河道平行的航向,且与两侧岸边同时保持最大安全距离,通过基于PID控制器来控制无人船沿航路点的航向角,根据输入e(t)误差(包括目标航向偏差和目标轨迹横向距离偏差),设计无人船输出动力u(t)的PID控制器为
u(t)=[Fl,Fr]T=[Fv+KlξF,Fv+KrξF]T
式中,Fl、Fr分别为无人船左右推进器的动力输出(PWM波);Fv为推进器保持无人船匀速的控制输入参数;Kl、Kr分别为左右推进器转换为各推进器控制输入的转换系数;ξF为对应于偏航角和横向轨迹误差的控制输入,具体来说根据航向偏差和距离偏差的重要性调整偏航角和横向距离误差作为控制输入的权重,具体调整方法为
ξF(t)=ρξang(t)+(1-ρ)ξdis(t)
式中,和分别为偏航角的比例增益系数、积分增益系数和微分增益系数;和分别为横向距离的比例增益系数、积分增益系数和微分增益系数;θe、de分别为偏航角和运行轨迹与规划轨迹的横向距离偏差,该偏差主要由水流、风向和牵引力等引起;ρ为权重系数。根据误差设置参数ρ为航向优先或者距离优先,无人船航行过程中通过自动校准实现优化搜索适合指定路径和航向保持的参数。为了简化运算,实施例中航向优先ρ=0.4,当无人船与河道中心线距离偏差较大时调整为距离优先,本实施例中ρ=0.6。
一种无人船系统,包括设置于无人船上的工控机、激光雷达、摄像机、驱动部、定位系统;所述工控机内置有河道边界检测模块与河道跟踪模块,所述工控机与所述激光雷达、摄像机通过有线和/或无线网络连通,所述工控机通过控制板与所述驱动部连通;所述驱动部是外围设备,如驱动器、水面垃圾打捞装置、水质检测仪器和采样装置等;所述定位系统包括与所述工控机连通的电子罗盘、GPS/IMU。在本实施例中,数据采集装置包括安装在无人船前方的激光雷达和相机,均通过网口连接在局域网交换机上,其中激光雷达的激光扫描平面与无人船底座平面平行,4G/5G路由器和供电电池放置于船体内后排平台上的机架内。
中央处理单元为基于ROS系统的工控机,通过网口与4G/5G联网模块相连接,并与云端服务器建立通信。中央处理单元对激光雷达的数据进行解析获取点云位置信息,并与相机视觉信息进行融合,根据采集的环境信息以及无人船的实时姿态进行实时的环境三维建模,通过坐标系转换,运行河道边界检测和虚拟河道中心线程序,控制无人船沿着河道中央区域通行。
本发明在使用时,首先将图像和激光雷达点云对齐,实现激光波雷达和摄像头的空间、时间对准。
激光雷达和摄像头安装在船顶上,在本实施例中激光雷达有效视场角FOV范围为水平81.7平,垂直25.1°。需要将激光雷达捕获的点云和相机捕获的图像以及图像和点云的对齐,对相机和激光雷达进行融合校准,以便通过将激光雷达点投影到图像平面上来使点云与图像对齐。激光雷达坐标中的3D点表示为pl=[x y z 1]T,首先将其转换为相机坐标pc=[xc yc zc],其中是从激光雷达坐标到相机坐标的变换矩阵,Rrect校正矩阵。再用投影变换矩阵Prect投影到图像平面上,[u′ v′ w]T=Prect[xc yc zc 1]T,
激光雷达雷达检测与相机检测根据传感器精度和环境情况选择是否融合,二者可以并行任务执行融合输出,也可以互斥只执行其中的一种结果输出,具体输出方式通过上位机实现可配置化。
此外,如图10所示,无人船通过联网模块与云端服务器通信,联网模块4G/5G路由器与工控机、激光雷达和摄像头通过以太网网口相连接,驱动控制板用于驱动外围设备,驱动板通过RS232串口与工控机相连。工控机是ROS系统的工控机,是x86或ARM架构的高性能计算平台,在水面自动驾驶中发挥着“大脑”作用,提供高性能、高能效的计算和逻辑处理,在确保功能安全的前提下实现AI驱动的自动驾驶。工控机通过HDMI与显示屏相连接,用于显示无人船的实时状态信息;工控机还通过RS232串口与GPS/IMU、电子罗盘和控制板相连接,GPS/IMU为无人船提供定位和惯性导航信息与工控机通过RS232串口相连接;电子罗盘在桥洞等GPS信号丢失情况下为无人船提供航向信息,与工控机通过RS232串口相连接,控制板与左右驱动器通过RS485串口相连接,控制无人船的左右螺旋桨动力,控制板与水质检测仪器和采样装置相连接,用于无人船巡航过程中自动监测水质参数。
对于本领域技术人员而言,显然本发明不限于上述示范性实施例的细节,而且在不背离本发明的精神或基本特征的情况下,能够以其他的具体形式实现本发明。因此,无论从哪一点来看,均应将实施例看作是示范性的,而且是非限制性的,本发明的范围由所附权利要求而不是上述说明限定,因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。
此外,应当理解,虽然本说明书按照实施方式加以描述,但并非每个实施方式仅包含一个独立的技术方案,说明书的这种叙述方式仅仅是为清楚起见,本领域技术人员应当将说明书作为一个整体,各实施例中的技术方案也可以经适当组合,形成本领域技术人员可以理解的其他实施方式。
Claims (8)
1.一种用于无人船的河道边界检测方法,其特征在于,包括以下步骤:
步骤1、将无人船上的相机与激光雷达进行联合标定,实现相机与激光雷达在空间、时间上的对准;
步骤2、利用基于相机图像输入数据进行河道边界检测,分别得到第一河道边界线左边界L1和右边界R1;
步骤3、通过激光雷达点云数据检测河道边界,将边界点投影到xoy水平面坐标,分别得到第二河道边界线左边界L2和右边界R2;
步骤4、判断是否需要融合的参数设置,如果需要融合,将步骤2、步骤3中得到的边界进行融合,输出融合后的第三河道边界线,左边界L3和右边界R3,否则输出单步骤2或步骤3的结果;
所述步骤4中分别基于图像和激光雷达的方法得到两侧河道边界线融合包括以下步骤:
步骤4.1:将激光雷达河道边界点检测结果投影到相机坐标;
步骤4.2:初始化斜率k,斜率变化步长h;
步骤4.3:对经过无人船中心点Pusv做直线l:y=k(x-xusv)+yusv ;
步骤4.4:计算l与L1和L2,R1和R2的交点,取距离无人船横向距离最近的点Pi,Pj作为拟合边界候选点;
步骤4.5:令k=k+h,重复步骤4.3-4.4,最终得到P1,P2,…,Pn为候选点;
步骤4.6:对P1,P2,…,Pn边界点得到新的曲线拟合结果左边界L3,右边界R3。
2.根据权利要求1所述的一种用于无人船的河道边界检测方法,其特征在于,所述步骤2中基于相机基于图像的河道检测包括以下步骤:
步骤2.1、对相机获取的图像数据进行镜面反射快速去除,镜面反射区域部分从感兴趣区域ROI中删除;
步骤2.2、对河道图像噪声过滤,灰度化,Canny边缘检测和感兴趣区域ROI选择;
步骤2.3、用B样条曲线检测河道边界,同时分别计算代价评分,选取得分最高的前m条曲线,加入候选集;
步骤2.4、对直线和曲线拟合结果候选集进行筛选确认,利用最小二乘法选择误差最小的方案作为最终结果。
3.根据权利要求2所述的一种用于无人船的河道边界检测方法,其特征在于,所述步骤2.1中的镜面反射去除方法包括以下步骤:
步骤2.1.1、检测包含镜面反射的区域,设置阈值d,去除亮度小于d的区域,其中d是镜面反射像素的最低强度,范围从180到255;
步骤2.1.2、对镜面反射区域进行聚类,选择亮度大于d的区域集合H1,H2,…Hn;
步骤2.1.3、对区域Hi利用膨胀和腐蚀算法去除孤立或者小的点,分别得到区域Di和Ei,其中:0<i<n;
步骤2.1.4、计算边界区域Bi=Di-Ei,分别计算Bi的三个通道RGB的平均强度;
步骤2.1.5、将检测到的镜面反射区域Hi替换为区域边界Bi中临近无人船当前位置的K近邻领域像素的平均强度。
4.根据权利要求2所述的一种用于无人船的河道边界检测方法,其特征在于,所述曲线拟合通过迭代的方式实现,具体步骤如下:
步骤2.4.1:初始化最大得分记录max_score=0,最大迭代次数iter_num=1000;
步骤2.4.2:通过随机抽样一致RANSAC方法对感兴趣区域中的可用点进行采样,使用加权采样方法,权重与阈值图像的像素值成正比,有助于选择更多相关点,即具有更高机会属于该河道边界的点;
步骤2.4.3:在上一步采样结果中选择控制点,使用最小二乘法拟合三次B样条曲线,提供样条的控制点,使拟合采样点的平方误差总和最小;
步骤2.4.4:计算当前拟合得分cur_score,如果得分大于max_score,则更新max_score=cur_score,并记录当前拟合结果作为当前最优解;
步骤2.4.5:重新执行步骤2.4.2-2.4.4,直到迭代次数满足要求。
5.根据权利要求1所述的一种用于无人船的河道边界检测方法,其特征在于,所述步骤3中基于激光雷达的河道边界拟合包括以下步骤:
步骤3.1、对激光雷达扫描的点云数据进行预处理,包括背景过滤、滤波,过滤掉散列点、孤立点;
步骤3.2、检测水面所在的平面,所述水面近似认为是一个平面,平面检测方法为,
z=zsurf
zsurf←argmax{NUM(cond{|z-zsurf|≤∈})}
z是垂直坐标,z=zsurf是使得满足条件|z-zsurf|≤∈的点p(x,y,z)最多的平面,
步骤3.3、过滤水面干扰,对于水平面上的点云进行过滤,得到高于水平面的点云,点的坐标pi(xi,yi,zi),0<i≤n,过滤后的点云集合G为:
G={pi(xi,yi,zi)|i∈N∧|zi-zsurf|>σ}
其中zsurf是水面在垂直方向的坐标,N 是从1开始的自然数,σ是河道水平面过滤阈值;
步骤3.4、提取河道边界点,提取边界线上的点加入候选点,分别得到左右河道边界线的点的集合Cleft和Cright,采用点云分类、角度和距离过滤的方法提取候选点,将待提取的点云投影到xoy平面上,提取的候选点遵循方向角和距离满足以下条件,
其中d1、d2、θ1分别是距离和方向角度阈值,Pi(xi,yi),Pj(xj,yj)和Pusv(xusv,yusv)分别是两个相邻点和无人船在xoy平面投影坐标;
按照角度顺序选取第三个候选点Pk(xk,yk)时,相邻三点Pi、Pj、Pk需同时满足以下角度阈值
其中θ2是相邻三点的方向角度阈值;将满足上述阈值条件的点视为同一类边界;
最终得到一系列集合C1、C2、…Cn,选择点数最多的两个Cmax1,Cmax2,其点的个数远大于其余的点集Ci,其中i≠max1且i≠max2;
再根据Cmax1,Cmax2方向判断得到左右边界点集Cleft,Cright;
步骤3.5、分别对Cleft,Cright,做曲线拟合,得到河道两侧的边界线。
6.根据权利要求5所述的一种用于无人船的河道边界检测方法,其特征在于,所述步骤3中基于激光雷达的河道边界拟合中通过对下一时刻的边界点进行预测和校准来消除噪声干扰,所述边界点预测和校准通过扩展卡尔曼滤波EKF进行,河道边界点更新时,判断当前检测结果Pcur(xcur,ycur)与前一帧的检测结果Ppre(xpre,ypre)距离满足阈值dm要求,则是有效的预测,并在更新步骤输入到扩展卡尔曼滤波器,否则丢弃当前的检测结果,将前一帧的检测结果作为当前检测结果,阈值要求为:
|xcur-xpre|+| ycur-ypre|≤dm
其中dm是两点之间的曼哈顿距离阈值。
8.一种无人船系统,其特征在于,使用权利要求7所述的无人船河道跟踪方法,包括设置于无人船上的工控机、激光雷达、摄像机、控制板、驱动部、定位系统,所述工控机内置有河道边界检测模块与河道跟踪模块,所述河道边界检测模块使用所述河道边界检测方法,所述河道跟踪模块使用所述河道跟踪方法,所述工控机与所述激光雷达、摄像机通过有线和/或无线网络连通,所述工控机通过控制板与所述驱动部连通,所述定位系统包括与所述工控机连通的电子罗盘、GPS/IMU。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111682613.4A CN114332647B (zh) | 2021-12-31 | 2021-12-31 | 一种用于无人船的河道边界检测与跟踪方法及系统 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111682613.4A CN114332647B (zh) | 2021-12-31 | 2021-12-31 | 一种用于无人船的河道边界检测与跟踪方法及系统 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114332647A CN114332647A (zh) | 2022-04-12 |
CN114332647B true CN114332647B (zh) | 2022-10-21 |
Family
ID=81023510
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111682613.4A Active CN114332647B (zh) | 2021-12-31 | 2021-12-31 | 一种用于无人船的河道边界检测与跟踪方法及系统 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114332647B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114879685B (zh) * | 2022-05-25 | 2023-04-28 | 合肥工业大学 | 一种用于无人船的河岸线检测及自主巡航方法 |
CN115630185B (zh) * | 2022-09-23 | 2024-02-02 | 深圳市云洲创新科技有限公司 | 一种重定位方法、水面航行器及存储介质 |
Family Cites Families (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9090263B2 (en) * | 2010-07-20 | 2015-07-28 | GM Global Technology Operations LLC | Lane fusion system using forward-view and rear-view cameras |
DE102019112413A1 (de) * | 2019-05-13 | 2020-11-19 | Bayerische Motoren Werke Aktiengesellschaft | Verfahren und vorrichtung zur multi-sensor-datenfusion für automatisierte und autonome fahrzeuge |
CN110188696B (zh) * | 2019-05-31 | 2023-04-18 | 华南理工大学 | 一种水面无人装备多源感知方法及系统 |
CN111401208B (zh) * | 2020-03-11 | 2023-09-22 | 阿波罗智能技术(北京)有限公司 | 一种障碍物检测方法、装置、电子设备及存储介质 |
CN112464994B (zh) * | 2020-11-05 | 2024-03-26 | 航天时代(青岛)海洋装备科技发展有限公司 | 基于PointNet网络的船艇尾浪识别与去除方法 |
CN113778081B (zh) * | 2021-08-19 | 2022-07-22 | 中国农业科学院农业资源与农业区划研究所 | 一种基于激光雷达和视觉的果园路径识别方法和机器人 |
-
2021
- 2021-12-31 CN CN202111682613.4A patent/CN114332647B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN114332647A (zh) | 2022-04-12 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
KR102112935B1 (ko) | 장애물 지도를 이용한 경로 생성 방법 | |
CN109017780B (zh) | 一种车辆智能驾驶控制方法 | |
Bar Hillel et al. | Recent progress in road and lane detection: a survey | |
CN114332647B (zh) | 一种用于无人船的河道边界检测与跟踪方法及系统 | |
CN111399505B (zh) | 一种基于神经网络的移动机器人避障方法 | |
Kong et al. | Vanishing point detection for road detection | |
EP2574958B1 (en) | Road-terrain detection method and system for driver assistance systems | |
Wu et al. | Applying a functional neurofuzzy network to real-time lane detection and front-vehicle distance measurement | |
Wang et al. | Bionic vision inspired on-road obstacle detection and tracking using radar and visual information | |
CN114902070A (zh) | 传感器退化检测和修复 | |
CN102700548A (zh) | 具有前摄像机和后摄像机的稳健的车辆侧向控制 | |
KR102466804B1 (ko) | 이미지 세그멘테이션을 이용한 자율 운항 방법 | |
CN113985419A (zh) | 水面机器人协同障碍物检测及避障方法及系统 | |
CN112674646B (zh) | 基于多算法融合的自适应贴边作业方法及机器人 | |
CN114325634A (zh) | 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法 | |
CN110658826A (zh) | 一种基于视觉伺服的欠驱动无人水面艇自主靠泊方法 | |
CN114399748A (zh) | 一种基于视觉车道检测的农机实时路径校正方法 | |
Fries et al. | Autonomous convoy driving by night: The vehicle tracking system | |
Jun et al. | Autonomous driving system design for formula student driverless racecar | |
US11087158B2 (en) | Error correction of airborne vehicles using natural patterns | |
Li et al. | Real-time 3D-Lidar, MMW Radar and GPS/IMU fusion based vehicle detection and tracking in unstructured environment | |
Suto | Real-time lane line tracking algorithm to mini vehicles | |
Nitsch et al. | 3d ground point classification for automotive scenarios | |
Zhong et al. | Robust negative obstacle detection in off-road environments using multiple LiDARs | |
Rasmussen et al. | Trail following with omnidirectional vision |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |