一种基于循迹航行的自主驾驶船舶避碰时机确定方法
技术领域
本发明涉及无人船舶航行技术领域，尤其涉及一种基于循迹航行的 自主驾驶船舶避碰时机确定方法。
背景技术
随着航运业的发展，船舶数量迅速增长，海上交通事故时有发生， 造成了人员伤亡、财产损失和海洋环境污染等巨大损失，因此，船舶避 碰决策系统的研究受到了航海界的高度重视。在船舶避碰决策中，如何 确定船舶避让时机是一个备受关注的问题。驾驶员若过早采取避让行动， 会造成本船偏离航线较多，浪费航程，从经济的角度考虑，是没有必要 的。若采取避让行动较晚，就可能会造成紧迫局面、紧迫危险甚至碰撞 事故。《1972年国际海上避碰规则》中规定让路船要“及早的”采取大 幅度的行动，但是这一规定是比较模糊的。不少专家学者致力于在遵守 规则的前提下将此问题量化，建立船舶避让时机决策模型，取得了一定 进展，但是尚无令航海人员普遍满意的成果。
为了船舶领域不被“侵犯”，驾驶员应提前采取行动，因此，有必 要确定动界。动界模型的逻辑是：当未来情况是本船领域受到来船“侵 犯”时，来船到达船舶动界边界时本船采取避碰行动。日本的长泽明用 类似的方法建立了避让开始时机模型，其方法是：如果来船的相对运动 线穿过本船的避让领域，则本船在来船到达避让开始领域边界时采取避 让行动。
但是现有技术中的动界仅仅是一个距离概念，避让规划中未考虑 TCPA、相对运动速度等时间因素对避让时机的影响，而实际场景下自主 驾驶船舶根据规划路径循迹航行时可能存在多航迹段变化，传统技术无 法直接应用于自主驾驶船舶。
上述缺陷是本领域技术人员期望克服的。
发明内容
(一)要解决的技术问题
为了解决现有技术的上述问题，本发明提供一种基于循迹航行的自 主驾驶船舶避碰时机确定方法，解决现有技术中无法充分利用自主驾驶 船舶优势导致决策时考虑因素不够全面，避碰时机不符合航海实际情况 的问题。
(二)技术方案
为了达到上述目的，本发明采用的主要技术方案包括：
本发明提供一种基于循迹航行的自主驾驶船舶避碰时机确定方法， 包括：
S10、基于电子海图利用路径规划算法确定多航段构成的初始航线， 根据所述多航段确定包含有多个航迹点的航迹点集合；
S20、在沿初始航线进行循迹航行的过程中，获取动态目标信息集合， 所述动态目标信息集合中包含至少一个动态目标的信息；
S30、通过遍历动态目标信息集合中的动态目标的信息，分别计算本 船避碰至少一个动态目标的最晚时间，确定本船避碰动态目标集合的最 晚时间；
S40、如果所述本船避碰动态目标集合的最晚时间大于0，且动态目 标集合中至少有一个动态目标与本船之间的距离小于安全会遇距离，则 基于所述本船避碰动态目标集合的最晚时间确定避碰时机。
在本发明的一种示例性实施例中，所述多个航迹段均已避开静态障 碍物。
在本发明的一种示例性实施例中，所述航迹点集合中包含n个航迹 点，起始点、终止点及其之间的n-2个航迹点形成的多个航迹段编号用 index表示，且index＝1,2,3,...n。
在本发明的一种示例性实施例中，所述动态目标的信息包括动态目 标的位置、航速和航向。
在本发明的一种示例性实施例中，所述动态目标集合中包括m个动 态目标。
在本发明的一种示例性实施例中，步骤S30中包括：
S31、循迹航行中获取本船的当前位置所在的航迹段编号index；
S32、基于本船的当前位置所在的航迹段编号index遍历航迹点集合 中的各个航迹点，计算本船开始避碰每一动态目标的最晚时间，得到m 个最晚时间计算值perTime；
S33、如果最晚时间计算值perTime小于最晚时间设定值，则更新最 晚时间设定值后转至步骤S34；如果最晚时间计算值perTime大于或等于 最晚时间设定值，则直接转至步骤S34；
S34、判断是否满足动态目标遍历结束的预设条件，如果满足则转至 步骤S40，如果不满足则重复步骤S31～S33。
在本发明的一种示例性实施例中，步骤S32包括：
S321、获取指定航迹段的起始航迹点坐标s(sx,sy)和终止航迹点坐标 e(ex,ey)；
s＝trackPoints(i)
e＝trackPoints(i+1)
其中，i为航迹点遍历迭代器，i＝index,index+1,…,n；
S322、计算航迹段方向cog；
S323、根据动态目标的信息计算最近会遇时间tcpa、相对航速Vr、相对 速度的方向
其中，Vxr为动态目标相对于本船在x轴的速度分量，Vyr为动态目标 相对于本船在y轴的速度分量，Rt为动态目标与本船之间的距离，
S324、根据本船的航速计算本船在tcpa的时间内将会前进的距离 forwardDis；
forwardDis＝tcpa*sog
其中sog为本船的航速；
S325、根据本船在tcpa的时间内将会前进的距离计算最近会遇点 naPt(naPtx,naPty)；
naPtx＝ptx+forwardDis*sin(cog)
naPty＝pty+forwardDis*cos(cog)
其中，pt(ptx,pty)为本船的当前位置；
S326、计算指定航迹段上离最近会遇点最近的点np；
S327、基于本船位置和离最近会遇点最近的点np计算本船到达np所 需时间nearT；
S328、计算本船到达np时，动态目标obsVessels(j)与本船之间的距离 nearD；
S329、更新本船开始避碰动态目标obsVessels(j)的最晚时间perTime；
其中，j为动态目标遍历迭代器，j＝0,1,2,…,m，SDA为安全会遇距离；
S3210、如果满足nearD＜SDA或满足循迹点遍历结束的预设条件，则 跳出航迹点遍历循环，转至步骤S33；否则继续航迹点遍历，返回S321。
在本发明的一种示例性实施例中，步骤S326包括：
情况一、如果最近会遇点位于指定航迹段的延长线上，且靠近指定航迹 段的起始航迹点本船的当前位置，则将指定航迹段的起始航迹点本船的当前 位置作为最近会遇点最近的点np；
情况二、如果最近会遇点位于指定航迹段的延长线上，且靠近指定航迹 段的终止航迹点，则将指定航迹段的终止航迹点作为最近会遇点最近的点 np；
其他情况下，将最近会遇点到指定航迹段所在直线的垂足作为最近 会遇点最近的点np。
在本发明的一种示例性实施例中，最晚时间计算值perTime的初始值 为0，最晚时间设定值的初始值为无穷大。
在本发明的一种示例性实施例中，步骤S20中基于本船上的感知系 统获取动态目标的信息。
(三)有益效果
本发明的有益效果是：本发明实施例提供的基于循迹航行的自主驾 驶船舶避碰时机确定方法，对于航线分为多个航迹段考虑而非单一的一 条直线，基于循迹航行来确定避碰时机，更加符合航海的实际情况，能 够准确确定本船的未来航行轨迹，避碰准确性更高。
附图说明
图1为本发明一实施例提供的一种基于循迹航行的自主驾驶船舶避 碰时机确定方法的流程图；
图2为本发明一实施例中实现基于循迹航行的自主驾驶船舶避碰时 机确定方法的整体流程图。
具体实施方式
为了更好的解释本发明，以便于理解，下面结合附图，通过具体实 施方式，对本发明作详细描述。
本文所使用的所有的技术和科学术语与属于本发明的技术领域的技 术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语 只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使 用的术语“和/或”包括一个或多个相关的所列项目的任意的和所有的组 合。
本发明提出的一种基于循迹航行的自主驾驶船舶避碰时机确定方 法，具有稳健性和准确度高等优点，更加符合航海的实际情况，为实现 自主驾驶船舶安全航行决策提供依据。该方法可以解决以下技术问题： 第一，动界不再只考虑距离，而是将距离和最近会遇时间、相对运动速 度等时间因素共同作为确定避碰时机的影响因素；第二，获取指定航迹段上距离最近会遇点最近的点；第三，确定本船开始避碰的临界条件， 即最晚避碰时间。
图1为本发明一实施例提供的一种基于循迹航行的自主驾驶船舶避 碰时机确定方法的流程图，如图1所示，具体包括以下步骤：
步骤S10、基于电子海图利用路径规划算法确定多航段构成的初始航 线，根据所述多航段确定包含有多个航迹点的航迹点集合；
步骤S20、在沿初始航线进行循迹航行的过程中，获取动态目标信息 集合，所述动态目标信息集合中包含至少一个动态目标的信息；
步骤S30、通过遍历动态目标信息集合中的动态目标的信息，分别计 算本船避碰至少一个动态目标的最晚时间，确定本船避碰动态目标集合 的最晚时间；
步骤S40、如果所述本船避碰动态目标集合的最晚时间大于0，且动 态目标集合中至少有一个动态目标与本船之间的距离小于安全会遇距 离，则基于所述本船避碰动态目标集合的最晚时间确定避碰时机。
在本发明的一种示例性实施例中，步骤S10中多个航迹段均已避开 静态障碍物，对于航迹段避开静态障碍物可以按照传统方法实现，在本 文不做具体限定。
在本发明的一种示例性实施例中，步骤S20中动态目标的信息包括 动态目标的位置、航速和航向。该步骤中基于本船上的感知系统获取动 态目标的信息，船端的感知系统包括但不限于风速风向仪、雷达、声呐、 AIS、GPS、测深仪等，可以直接获得或计算得到动态目标的位置、航速 和航向。
另外，本发明中动态目标信息集合如果为空集，则结束处理，不进 行后续步骤，本文是以动态目标信息集合为非空集的情况下如何基于动 态目标的信息进行避碰决策为主进行介绍。
在本发明的一种示例性实施例中，步骤S40中将动态目标集合中的 每个动态目标与本船的距离都与安全会遇距离进行比较，集合中只要有 一个或多个动态目标到本船的距离小于安全会遇距离就需要进行避碰时 机的规划，确定最晚避碰时间，且最晚避碰时间不等于避碰时间最晚， 本发明中所涉及的均是最晚避碰时间。
以下对图1所示方法进行详细介绍，图2为本发明一实施例中实现 基于循迹航行的自主驾驶船舶避碰时机确定方法的整体流程图，如图2 所示，具体包括如下步骤：
步骤S1、根据电子海图数据，使用路径规划算法获得一条避开静态障 碍物的多航迹段航线，得航迹点集合trackPoints，获取航迹点个数n；
步骤S2、根据所述航迹点集合trackPoints循迹航行开始后，通过本船的 感知系统获取动态目标信息集合obsVessels，获取动态目标个数m；
步骤S3、根据所述动态目标信息集合obsVessels，遍历动态目标的信息， 计算本船开始避碰每一个动态目标的最晚时间，得到本船开始避碰动态目标 集合的最晚时间Time(Time初始值为无穷大)。
步骤S3中具体包括以下步骤：
步骤S31、获取本船的当前位置所在航迹段编号index，index＝1,2,3,...n， 其中，n为航迹点个数，航迹点集合中包含n个航迹点，起始点、终止点及 其之间的n-2个航迹点；
步骤S32、根据所述航迹点集合trackPoints和本船的当前位置所在航迹段 编号index，遍历航迹点，计算本船开始避碰每一个动态目标的最晚时间 perTime(perTime初始值为0)，步骤S32具体包括以下步骤：
步骤S321、获取指定航迹段起始航迹点坐标s(sx,sy)和终止航迹点坐标 e(ex,ey)；
s＝trackPoints(i)
e＝trackPoints(i+1)
其中，i为航迹点遍历迭代器，i＝index,index+1,…,n。
步骤S322、计算航迹段方向cog；
步骤S323、根据动态目标的信息计算最近会遇时间tcpa、相对航速Vr、 相对速度的方向
其中，Vxr为动态目标相对于本船在x轴的速度分量，Vyr为动态目标相 对于本船在y轴的速度分量，Rt为动态目标与本船之间的距离，
步骤S324、根据本船的航速计算本船在tcpa的时间内，将会前进的距离forwardDis；
forwardDis＝tcpa*sog
其中，sog为本船的航速；
步骤S325、根据本船在tcpa的时间内将会前进的距离计算最近会遇点 naPt(naPtx,naPty)；
naPtx＝ptx+forwardDis*sin(cog)
naPty＝pty+forwardDis*cos(cog)
其中，pt(ptx,pty)为本船的当前位置。
步骤S326、计算指定航迹段上离最近会遇点最近的点np；
情况一、如果最近会遇点位于所述线段的延长线上，且靠近指定航迹段 的起始航迹点本船的当前位置，则将指定航迹段的起始航迹点作为最近会遇 点最近的点np，即np＝pt；
情况二、如果最近会遇点位于所述线段的延长线上，且靠近所述指定航 迹段终止航迹点，则将本船的当前位置作为最近会遇点最近的点np，即 np＝e；
其他情况下，将最近会遇点到指定航迹段所在直线的垂足作为将本船的 当前位置作为最近会遇点最近的点np。
步骤S327、基于本船位置和离最近会遇点最近的点np计算本船到达所 述np所需时间nearT；
步骤S328、计算本船到达np时，动态目标obsVessels(j)与本船之间的距 离nearD；
步骤S329、更新本船开始避碰动态目标obsVessels(j)的最晚时间perTime；
其中，j为动态目标遍历迭代器，j＝0,1,2,…,m；SDA为安全会遇距离。
步骤S3210、如果nearD＜SDA或满足循迹点遍历结束条件(即不满足i ＜n的条件)，则跳出航迹点遍历循环，执行步骤S33；否则继续航迹点遍历， 返回步骤S321。
步骤S33、如果最晚时间计算值perTime小于最晚时间设定值，即 perTime＜Time，则更新最晚时间设定值，即本船开始避碰动态目标集合的最 晚时间Time＝perTime，转到步骤S34；否则(即perTime≥Time)，直接转到步 骤S34。
步骤S34、如果满足动态目标遍历结束条件，则转到步骤S4；否则，返 回步骤S31开始重复步骤S31～S33。
步骤S4、如果本船开始避碰动态目标集合的最晚时间大于1，即Time>0， 并且nearD<SDA，则需要采取避碰措施，基于所述本船避碰动态目标集合的 最晚时间确定避碰时机且开始避碰的最晚时间为Time；否则，不需要采取避 碰措施，继续循迹航行即可。
综上所述，采用本发明实施例提供的基于循迹航行的自主驾驶船舶避碰 时机确定方法，具有如下效果：
(1)由于基于循迹航行来确认避碰时机，更加符合航海的实际情况，能 够更准确的获取船舶的未来轨迹；
(2)通过将航迹段上与最近会遇点的最短距离和到达航迹段上与动态目 标距离为安全会遇距离的位置所需要的时间作为确认避碰时机的影响因素， 这样在循迹航行中综合考虑时间因素和相对运动速度等时间因素，考虑因素 更全面，能够更加准确的确定避碰时机
(3)对于航线分为多个航迹段考虑而非单一的一条直线，考虑多航迹 段变化，可以避免非必要的避碰判定，减少不必要的计算量。
应当注意，尽管在上文详细描述中提及了用于动作执行的设备的若 干模块或者单元，但是这种划分并非强制性的。实际上，根据本发明的 实施方式，上文描述的两个或更多模块或者单元的特征和功能可以在一 个模块或者单元中具体化。反之，上文描述的一个模块或者单元的特征 和功能可以进一步划分为由多个模块或者单元来具体化。
本领域技术人员在考虑说明书及实践这里公开的发明后，将容易想 到本发明的其它实施方案。本申请旨在涵盖本发明的任何变型、用途或 者适应性变化，这些变型、用途或者适应性变化遵循本发明的一般性原 理并包括本发明未公开的本技术领域中的公知常识或惯用技术手段。说 明书和实施例仅被视为示例性的，本发明的真正范围和精神由下面的权 利要求指出。
应当理解的是，本发明并不局限于上面已经描述并在附图中示出的 精确结构，并且可以在不脱离其范围进行各种修改和改变。本发明的范 围仅由所附的权利要求来限制。
