CN117234217B - 基于三维时空域的水面无人艇轨迹跟踪制导方法及系统 - Google Patents
基于三维时空域的水面无人艇轨迹跟踪制导方法及系统 Download PDFInfo
- Publication number
- CN117234217B CN117234217B CN202311499564.XA CN202311499564A CN117234217B CN 117234217 B CN117234217 B CN 117234217B CN 202311499564 A CN202311499564 A CN 202311499564A CN 117234217 B CN117234217 B CN 117234217B
- Authority
- CN
- China
- Prior art keywords
- unmanned ship
- time
- target
- track
- view
- 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
- 238000000034 method Methods 0.000 title claims abstract description 55
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 title claims abstract description 27
- 239000013598 vector Substances 0.000 claims abstract description 20
- 230000033001 locomotion Effects 0.000 claims description 20
- 230000008859 change Effects 0.000 claims description 10
- 238000012545 processing Methods 0.000 claims description 10
- 230000001133 acceleration Effects 0.000 claims description 9
- 230000007613 environmental effect Effects 0.000 claims description 7
- 238000004590 computer program Methods 0.000 claims description 5
- 206010034701 Peroneal nerve palsy Diseases 0.000 claims description 4
- 230000003321 amplification Effects 0.000 claims description 3
- 238000003199 nucleic acid amplification method Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 6
- 238000013459 approach Methods 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 4
- 238000003491 array Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 238000013480 data collection Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 230000008569 process Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 230000002123 temporal effect Effects 0.000 description 2
- 238000004458 analytical method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000009191 jumping Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000008092 positive effect Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明属于无人艇轨迹跟踪控制技术领域，公开了一种基于三维时空域的水面无人艇3D‑LOS轨迹跟踪制导方法及系统，引入时间维度T，将无人艇目标航迹视为三维时空曲线；选定前视球半径R T，以无人艇当前时空点为球心、R T为半径作球体；基于解析几何原理计算前视球面与目标航迹的交点，即为前视点；根据当前时间与前视点时间大小关系，将前视点分类为历史前视点和未来前视点两类；跟踪历史前视点的策略为以无人艇最高航速驶向历史前视点，跟踪未来前视点的策略为在实际航速限制下通过当前点与前视点所定空间矢量解算目标航向角与目标航速，引导无人艇作航迹跟踪。本发明可以快速解算最佳目标航向和目标航速，提高航迹跟踪精度和纠偏能力。
Description
技术领域
本发明属于无人艇轨迹跟踪控制技术领域，尤其涉及一种基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法、系统、设备及终端。
背景技术
水面无人艇是一种无人驾驶的水面船舶，通过搭载不同的功能模块和载荷设备，可自主进行各种复杂的水上无人作业。当前无人艇被广泛应用于水文勘测、海洋资源勘探、港口与海岸监视、反潜作战等多个领域。航迹跟踪的目的是通过合理的跟踪制导与控制律输出控制指令，操控桨、舵等操纵机构，使无人艇按照路径规划模块制定的目标航迹航行，是实现无人艇智能驾驶和自主避障的关键，也是无人艇开展无人化作业所必须具备的核心功能。无人艇制导律是通过在航行过程中不断改变无人艇航向角以达到轨迹跟踪的目的。但当前无人艇制导技术主要关注目标航向角的确定，而对目标航速处理过于简单，仅以恒定航速或规划航速作为目标航速，忽视了无人艇航向角趋向目标航向角的收敛速率，导致在航迹偏差较大时存在无人艇动态纠偏能力弱，跟踪精度低等问题，难以满足无人艇局部动态避障和高精度作业的需求。
通过上述分析，现有技术存在的问题及缺陷为：当前无人艇制导律仅以恒定航速或规划航速为目标航速，对目标航速的处理过于简单；且在航迹偏差较大时无人艇动态纠偏能力弱，跟踪精度低，难以满足无人艇局部动态避障和高精度作业的需求。
发明内容
针对现有技术存在的问题，本发明提供了一种无人艇轨迹跟踪制导方法、系统、设备及终端，尤其涉及一种基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法、系统、介质、设备及终端。
本发明是这样实现的，一种基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法，无人艇3D-LOS轨迹跟踪制导方法包括：在现有无人艇二维航行轨迹平面基础上，引入时间维度T，在（X，Y，T）三维时空域中将无人艇目标航迹视为三维时空曲线，即一条带有时间约束的三维轨迹线；选定前视球半径RT，以无人艇当前时空点为球心、RT为半径作球体；基于空间解析几何原理计算前视球面与目标航迹的交点，即为前视点；以当前时间与前视点时间大小关系为依据，将前视点分类为历史前视点和未来前视点两类；前视点时间小于当前时间为历史前视点，前视点时间大于或等于当前时间为未来前视点；跟踪历史前视点的策略为以无人艇最高航速驶向历史前视点，跟踪未来前视点的策略为在实际航速限制下通过当前点与前视点所定空间矢量解算目标航向角与目标航速，引导无人艇作航迹跟踪。通过上述无人艇航速的设定，可使无人艇当前航向角向目标航向角快速收敛，以实现航迹跟踪的精确控制。
进一步，最高效率接近历史前视点的策略为：
以无人艇最高航速为目标航速，以当前点和前视点在（X，Y）二维平面投影所定矢量方向为目标航向角，则，/>。式中，/>表示3D-LOS轨迹跟踪制导律所得目标航速；/>表示无人艇最高航速；/>表示3D-LOS轨迹跟踪制导律所得目标航向角；/>表示前视点的二维坐标；/>表示无人艇当前位置的二维坐标。
最高效率接近未来前视点的策略为：
以当前点和前视点所定空间矢量解算目标航速和目标航向角，则，且/>，/>。式中，/>表示前视点所处的时间；/>表示无人艇当前的时间。
进一步，基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法包括以下步骤：
步骤一，初始化前视半径，并给定目标航迹；
步骤二，初始化无人艇当前位置与当前时间；
步骤三，分别解算前视点、目标航向与目标航速；
步骤四，更新无人艇当前位置与当前时间。
进一步，步骤一中的前视半径为在包含时间维度的（X，Y，T）三维时空域下的前视球体半径，前视球体半径值由环境干扰力大小、无人艇跟踪控制系统精度或结合实际航行经验确定。
目标航迹由若干段匀加速直线运动拼接组成，通过平均速度处理将目标航迹简化为匀速直线运动，则作匀速直线运动的无人艇目标航迹S方程为：
；
式中，x、y表示目标航迹直线上各点在二维平面的坐标；X0、Y0分别表示航迹段起点坐标；Vx、Vy分别表示航迹段在X、Y方向上平均速度；t为时间坐标。
进一步，步骤二中的当前位置与当前时间为无人艇在（X，Y，T）时空域下的实际三维坐标。
进一步，步骤三中的解算前视点的方法为：联立目标航迹方程和前视球面方程，消元并采用一元二次方程公式法求解；其中，前视球面方程为：
；
式中，XUSV、YUSV、TUSV为无人艇当前实际时空坐标；RT为前视半径；K为时间放大系数，其作用是调节时空比例。
通过联立作匀速直线运动的无人艇目标航迹S方程和前视球面方程，并消去x、y得到关于时间t的一元二次方程，采用求根公式求得前视点时间坐标，再代入目标航迹方程得到前视点X、Y坐标；根据前视点时间坐标与当前时间相对大小将前视点分为历史前视点和未来前视点。
进一步，步骤四中，根据前视球与目标航迹交点情况，将前视点分为6种情形进行目标航迹和目标航向角的解算；将解算得到的目标航向与目标航速交予无人艇底层控制器，引导无人艇驶向前视点；通过在无人艇控制周期内重复制导、控制步骤，实现轨迹跟踪。其中，6种情形下的目标航向与目标航速的解算，具体如下：
（1）无前视点：将当前点与目标航迹的垂足作为引导点，问题转化为下述情形2或3；
（2）一个历史前视点：以当前点与前视点在（X，Y）二维平面投影所定矢量方向为目标航向角，以无人艇最高航速为目标航速；
（3）一个未来前视点：以当前点与前视点所定空间矢量确定目标航向角和目标航速；
（4）两个历史前视点：以航向角变化较小的前视点作引导，转化为情形2；
（5）两个未来前视点：以航向角变化较小的前视点作引导，转化为情形3；
（6）一个历史前视点和一个未来前视点：以未来前视点作引导，转化为情形3。
本发明的另一目的在于提供一种应用所述的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法的水面无人艇3D-LOS轨迹跟踪制导系统，基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导系统包括：
前视半径初始化模块，用于根据环境干扰力大小、无人艇跟踪控制系统精度或结合实际航行经验确定三维时空域下的前视球体半径；
目标航迹确定模块，用于确定由若干段匀加速直线运动拼接组成的目标航迹，并通过平均速度处理将目标航迹简化为匀速直线运动；
位置与时间初始化模块，用于通过确定无人艇在（X，Y，T）时空域下的实际三维坐标，初始化无人艇当前位置与当前时间；
前视点解算模块，用于联立目标航迹方程与前视球面方程，利用数值求解得到前视点坐标，并确定目标航向与目标航速；
轨迹跟踪制导模块，用于将解算得到的目标航向与目标航速交予无人艇底层控制器，引导无人艇驶向前视点；通过在无人艇控制周期内重复制导、控制步骤，实现轨迹跟踪，同时更新无人艇当前位置与当前时间。
本发明的另一目的在于提供一种计算机设备，计算机设备包括存储器和处理器，存储器存储有计算机程序，计算机程序被处理器执行时，使得处理器执行所述的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法的步骤。
本发明的另一目的在于提供一种计算机可读存储介质，存储有计算机程序，计算机程序被处理器执行时，使得处理器执行所述的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法的步骤。
本发明的另一目的在于提供一种信息数据处理终端，信息数据处理终端用于实现所述的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导系统。
结合上述的技术方案和解决的技术问题，本发明所要保护的技术方案所具备的优点及积极效果为：
第一，本发明提供了一种基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法，基于经典LOS前视距离的思想提出了一种基于三维时空域的3D-LOS轨迹跟踪制导算法：在现有无人艇二维航行轨迹平面基础上，引入时间维度T，在（X，Y，T）三维时空域中将无人艇目标航迹视为三维时空曲线，即一条带有时间约束的三维轨迹线；以无人艇当前时空点为球心、前视半径RT为半径作球体，该球面与目标航迹曲线交点为前视点，由前视点与当前点所定矢量解算目标航速与航向，引导无人艇作航迹跟踪。
同时，本发明按照前视点与当前点时刻的大小关系，将前视点分为历史前视点和未来前视点两类；前视点时刻小于当前点时刻的为历史前视点，前视点时间大于或等于当前点时间的为未来前视点。本发明对解算目标航速作了详尽分析，通过3D-LOS制导律解算得到轨迹跟踪过程的目标航速和目标航向，再结合PID、滑模等底层控制律，实现了无人艇的精确轨迹跟踪。
第二，本发明的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法，可以根据无人艇当前位置和目标航迹快速解算出最佳目标航向和目标航速，结合无人艇底层航向航速控制器实现带时间约束的高精度航迹跟踪，提高航迹跟踪精度和航迹纠偏能力。
第三，基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法带来了以下显著的技术进步：
1．三维时空域跟踪：传统的航行轨迹跟踪，仅限于二维平面曲线，而本方法引入了时间维度，形成三维时空曲线，更全面地考虑了无人艇的航行轨迹和速度变化，使得航迹跟踪更加准确和流畅。
2．前视策略的应用：通过定义前视球并计算其与目标航迹的交点，该方法可以预测并选择最佳的航迹跟踪点。这种预测机制大大提高了航迹跟踪的精确性和效率。
3．历史与未来前视点的分类：根据前视点的时间属性进行分类，并为两类前视点分别定义了不同的跟踪策略，这使得无人艇可以更加灵活地应对实际航行中的各种情况，提高了航迹跟踪的稳定性。
4．实时航速和航向调整：通过空间矢量解算目标航向角与目标航速，无人艇可以实时调整航速和航向，确保高效且精确地跟踪目标航迹，降低了偏航和错过目标的可能性。
5．提高自主导航能力：该方法增强了无人艇的自主导航和决策能力，使其能够在复杂的海域环境中自主、精确地跟踪预定航迹。
6．提高安全性和可靠性：对于出现的预测误差或实际航行中的障碍，无人艇可以根据前视策略快速做出响应和调整，从而提高航行的安全性和可靠性。
综上所述，基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法在航迹跟踪技术上带来了显著的创新和进步，提高了无人艇导航的高效性、精确性和安全性。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例中所需要使用的附图做简单的介绍。显而易见地，下面所描述的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法流程图；
图2是本发明实施例提供的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法原理图；
图3是本发明实施例提供的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导系统交互图；
图4是本发明实施例提供的基于三维时空域的水面无人艇3D-LOS制导律与无人艇航向航速控制器协同工作实现轨迹跟踪原理图；
图5是本发明实施例提供的基于三维时空域的水面无人艇3D-LOS制导律与经典LOS制导律下无人艇轨迹跟踪三维航迹仿真对比图；
图6是本发明实施例提供的基于三维时空域的水面无人艇3D-LOS制导律与经典LOS制导律下无人艇轨迹跟踪二维投影航迹仿真对比图；
图7是本发明实施例提供的基于三维时空域的水面无人艇3D-LOS制导律与经典LOS制导律下无人艇轨迹跟踪航迹偏差对比图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
针对现有技术存在的问题，本发明提供了一种基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法、系统、设备及终端，下面结合附图对本发明作详细的描述。
3D-LOS是本发明提出的新型路径跟踪方法。其中：3D是指三维时空域，LOS是一种轨迹跟踪制导算法。其区别于LOS经典算法及其现有的其他变种算法的特征在于，3D-LOS不局限于二维运动平面，综合考虑时间维度与空间维度，结合车辆或船舶的运动学模型及动力学约束，根据具体对象与具体应用场景设置动力学参数的阈值约束，对追踪目标进行基于方向角、二维坐标与速度的精确跟踪。该方法引入时间维度的目的有两个，首先是作为动力学参数的数值解算基础；其次是辅助跟踪任务的时空限制与运动平稳性。3D-LOS作为新提出的算法首次应用于本发明着眼于解决水面无人艇轨迹跟踪问题。本发明结合水面无人艇的航向角趋近、航路点跟踪、跟踪速度等具体信息，设计3D-LOS运用于轨迹跟踪算法的实现。
如图1所示，本发明实施例提供的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法包括以下步骤：
S101，在现有无人艇二维航行轨迹平面基础上，引入时间维度T，将无人艇目标航迹视为三维时空曲线，即一条带有时间约束的三维航行轨迹线，选定前视球半径RT，以无人艇当前时空点为球心、RT为半径作球体；
S102，基于空间解析几何原理计算前视球面与目标航迹交点，以当前时间与前视点时间大小为依据，将前视点分类为历史前视点和未来前视点；
S103，跟踪历史前视点的策略为以无人艇最高航速驶向历史前视点，跟踪未来前视点的策略为在实际航速限制下通过当前点与前视点所定空间矢量解算目标航向角与目标航速，引导无人艇作航迹跟踪。
作为优选实施例，如图2所示，本发明实施例提供的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法具体包括以下步骤：
步骤1：初始化前视半径；
前视半径为在包含时间维度的（X，Y，T）三维时空域下前视球体半径，该值由环境干扰力大小、无人艇跟踪控制系统精度或结合实际航行经验确定。
步骤2：给定目标航迹；
目标航迹由若干段匀加速直线运动拼接组成，通过平均速度处理将其简化为匀速直线运动，简化后航迹段在时空域下方程为：
式中，x、y表示目标航迹直线上各点在二维平面的坐标；X0、Y0分别表示航迹段起点坐标；Vx、Vy分别表示航迹段在X、Y方向上平均速度；t为时间坐标。
步骤3：初始化无人艇当前位置与当前时间；
当前位置与当前时间为无人艇在（X，Y，T）时空域下实际三维坐标。
步骤4：解算前视点；
解算前视点的方法是联立目标航迹方程和前视球面方程，消元并采用一元二次方程公式法求解；其中，前视球面方程为：
式中，XUSV、YUSV、TUSV为无人艇当前实际时空坐标；RT为前视半径；K为时间放大系数，其作用是调节时空比例。
通过联立两式并消去x、y得到关于时间t的一元二次方程，采用求根公式求得前视点时间坐标，再代入目标航迹方程得到前视点X、Y坐标。根据前视点时间坐标与当前时间相对大小将前视点分为历史前视点和未来前视点。根据解的情况，前视点共有以下6种情形：（1）无前视点；（2）有且仅有一个前视点，且为历史前视点；（3）有且仅有一个前视点，且为未来前视点；（4）有两个前视点，且均为历史前视点；（5）有两个前视点，且均为未来前视点；（6）有两个前视点，其中一个是历史前视点而另一个是未来前视点。
最高效率接近历史前视点的方法是：
以无人艇最高航速为目标航速，以当前点和前视点在（X，Y）二维平面投影所定矢量方向为目标航向角，即，/>。
最高效率接近未来前视点的方法是：
以当前点和前视点所定空间矢量解算目标航速和目标航向角，即，且/>，/>。
步骤5：解算目标航向与目标航速；
目标航迹和目标航向角具体解算方法如下：
（1）无前视点：将当前点与目标航迹的垂足作为引导点，问题转化为如下情形2或3；
（2）有且仅有一个前视点，且为历史前视点：以无人艇最高航速为目标航速，以当前点和前视点在（X，Y）二维平面投影所定矢量方向为目标航向角；
（3）有且仅有一个前视点，且为未来前视点：以当前点和前视点所定空间矢量解算目标航速和目标航向角；
（4）有两个前视点，且均为历史前视点：取二者中航向角变化较小的前视点作引导，转化为情形2；
（5）有两个前视点，且均为未来前视点：取二者中航向角变化较小的前视点作引导，转化为情形3；
（6）有两个前视点，其中一个是历史前视点而另一个是未来前视点：选取未来前视点为引导点，转化为情形3。
将上述解算所得目标航向与目标航速交予无人艇底层控制器，引导无人艇驶向前视点。通过在无人艇控制周期内重复上述制导、控制步骤，最终实现完整的轨迹跟踪。
步骤6：更新无人艇当前位置与当前时间。
如图3所示，本发明实施例提供的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导系统包括：
前视半径初始化模块，用于根据环境干扰力大小、无人艇跟踪控制系统精度或结合实际航行经验确定三维时空域下的前视球体半径；
目标航迹确定模块，用于确定由若干段匀加速直线运动拼接组成的目标航迹，并通过平均速度处理将目标航迹简化为匀速直线运动；
位置与时间初始化模块，用于确定无人艇在（X，Y，T）时空域下的实际三维坐标，初始化无人艇当前位置与当前时间；
前视点解算模块，用于联立目标航迹方程与前视球面方程，通过数值求解得到前视点坐标，并确定目标航向与目标航速；
轨迹跟踪制导模块，用于将解算得到的目标航向与目标航速交予无人艇底层控制器，引导无人艇驶向前视点；通过在无人艇控制周期内重复制导、控制步骤，实现轨迹跟踪，同时更新无人艇当前位置与当前时间。
如图4所示，本发明实施例提供的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法应用于无人艇航迹跟踪。其中，3D-LOS航迹跟踪器输入量为目标航迹、无人艇当前位置，输出量为目标航速、目标航向。该输出量作为底层控制器输入，结合无人艇当前状态量，采用一定的控制律输出控制指令，引导无人艇逼近目标航迹。通过在无人艇控制周期内重复上述制导、控制步骤，最终实现完整的轨迹跟踪。
本发明实施例提供的3D-LOS航迹跟踪器的具体控制步骤如下：
步骤一：初始化前视半径
根据环境干扰力大小、无人艇跟踪控制系统精度，或结合实际航行经验确定前视半径RT。
步骤二：给定目标航迹
目标航迹S（P1-P2-P3-P4）由匀加速、匀速、匀减速三段直线运动组成，三个直线段的四个点在三维时空（X，Y，T）下坐标依次是：P1（0m，0m，0s）、P2（0m，20m，8s）、P3（30m，20m，14s）、P4（30m，0m，22s）。三段直线在（X，Y）二维平面上呈倒‘U’字形，转角为直角。
步骤三：初始化无人艇当前位置与当前时间
将无人艇当前实际时空坐标作为系统输入，在本例中为（2m，0m，0s）。
步骤四：解算前视点
联立目标航迹方程与前视球面方程，通过数值求解得到前视点坐标。简便起见，将匀加速的目标航迹作平均速度下的匀速处理，则目标航迹方程为：
。
前视球面方程为：。
联立两式消去x、y，得到关于t的一元二次方程，通过求根公式计算前视点时间坐标，再代入目标航迹方程进一步求得前视点x、y坐标。根据解的情况，前视点共有以下6种情形：（1）无前视点；（2）有且仅有一个前视点，且为历史前视点；（3）有且仅有一个前视点，且为未来前视点；（4）有两个前视点，且均为历史前视点；（5）有两个前视点，且均为未来前视点；（6）有两个前视点，其中一个是历史前视点而另一个是未来前视点。
步骤五：解算目标航向与目标航速
针对步骤四的6种前视点情形，对目标航向与目标航速解算分析如下：
（1）无前视点：将当前点与目标航迹的垂足作为引导点，问题转化为如下情形2或3；
（2）有且仅有一个前视点，且为历史前视点：以无人艇最高航速为目标航速，即，以当前点和前视点在（X，Y）二维平面投影所定矢量方向为目标航向角，即；
（3）有且仅有一个前视点，且为未来前视点：以当前点和前视点所定空间矢量解算目标航速和目标航向角，，且/>，；
（4）有两个前视点，且均为历史前视点：取二者中航向角变化较小的前视点作引导，转化为情形2；
（5）有两个前视点，且均为未来前视点：取二者中航向角变化较小的前视点作引导，转化为情形3；
（6）有两个前视点，其中一个是历史前视点而另一个是未来前视点：选取未来前视点为引导点，转化为情形3。
步骤六：更新无人艇当前位置与当前时间
将解算所得目标航向与目标航速交付予无人艇控制层，控制层根据一定控制律输出相应的控制指令，使得无人艇驶向引导点；在一个控制周期内实现无人艇当前位置与时间更新，再跳转至步骤四，直至完成整个航迹跟踪。
本发明实施例提供的3D-LOS与经典LOS下无人艇轨迹跟踪结果对比如图5~图7所示。图5给出了轨迹跟踪三维图，由图5可知，无人艇在3D-LOS制导律下，在起点附近相对更快接近目标航迹曲线，且在跟踪最后时刻位置相对更接近目标航迹终点，验证了3D-LOS在航迹纠偏能力和跟踪精确度上优于经典LOS。图6给出了轨迹跟踪二维图，由图6可知，3D-LOS制导律与经典LOS二维航迹跟踪差别不大，均能很好地完成平滑航迹跟踪。图7是两种制导律下实时航迹偏差对比图，由图7可知，3D-LOS制导律下航迹偏差不超过2.5m，且在终止时刻航迹偏差低于0.5m，整体上跟踪精度优于经典LOS制导律，验证了3D-LOS制导律的可行性和优越性。
实施例1：无人艇导航
1．数据收集：首先在湖泊中部署浮标或使用GPS获取无人艇的实时位置，并收集其预期的航行轨迹数据。
2．三维时空域构建：在二维湖面航行轨迹基础上，引入时间维度T，构建（X，Y，T）三维时空域。
3．目标航迹曲线绘制：在三维时空域中绘制预期的无人艇航迹，该航迹视为带时间约束的三维轨迹线。
4．前视球面选择：根据无人艇的当前速度和湖泊的特性，选择一个合适的前视球半径RT。
5．前视点确定：基于空间解析几何原理，计算前视球面与目标航迹的交点，确定前视点。
6．前视点分类：基于前视点的时间属性，将其分类为历史前视点或未来前视点。
7．航迹跟踪：根据前视点的分类，执行相应的跟踪策略，引导无人艇完成湖面上的航迹跟踪。
实施例2：沿海港口区域的无人艇巡逻
1．数据收集：使用GPS和其他传感器收集无人艇在港口区域的实时位置，同时收集预定的巡逻航线数据。
2．三维时空域构建：在二维航行轨迹基础上，考虑巡逻任务的时间要求，引入时间维度T，构建（X，Y，T）三维时空域。
3．目标航迹曲线绘制：在三维时空域中，将无人艇的预期巡逻路线视为带时间约束的三维轨迹线。
4．前视球面选择：考虑无人艇的当前速度和港口的海流、风向等特性，选择合适的前视球半径RT。
5．前视点确定：基于空间解析几何原理，找到前视球面与目标航迹的交点。
6．前视点分类：根据前视点的时间特性，进行分类。
7．航迹跟踪：执行相应的跟踪策略，确保无人艇能够准确地完成在港口区域的巡逻任务。
应当注意，本发明的实施方式可以通过硬件、软件或者软件和硬件的结合来实现。硬件部分可以利用专用逻辑来实现；软件部分可以存储在存储器中，由适当的指令执行系统，例如微处理器或者专用设计硬件来执行。本领域的普通技术人员可以理解上述的设备和方法可以使用计算机可执行指令和/或包含在处理器控制代码中来实现，例如在诸如磁盘、CD或DVD-ROM的载体介质、诸如只读存储器（固件）的可编程存储器或者诸如光学或电子信号载体的数据载体上提供了这样的代码。本发明的设备及其模块可以由诸如超大规模集成电路或门阵列、诸如逻辑芯片、晶体管等的半导体、或者诸如现场可编程门阵列、可编程逻辑设备等的可编程硬件设备的硬件电路实现，也可以用由各种类型的处理器执行的软件实现，也可以由上述硬件电路和软件的结合例如固件来实现。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。
Claims (10)
1.一种基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法，其特征在于，在现有无人艇二维航行轨迹平面基础上，引入时间维度T，在三维时空域中将无人艇目标航迹视为三维时空曲线；选定前视球体半径RT，以无人艇当前时空点为球心、RT为半径作球体；基于空间解析几何原理计算前视球面与目标航迹的交点，即为前视点；以当前时间与前视点时间大小关系为依据，将前视点分类为历史前视点和未来前视点两类；前视点时间小于当前时间为历史前视点，前视点时间大于或等于当前时间为未来前视点；跟踪历史前视点的策略为以无人艇最高航速驶向历史前视点，跟踪未来前视点的策略为在实际航速限制下通过当前点与前视点所定空间矢量解算目标航向角与目标航速，引导无人艇实现航迹跟踪。
2.如权利要求1所述的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法，其特征在于，最高效率接近历史前视点的方法为：
以无人艇最高航速为目标航速，以当前点和前视点在（X，Y）二维平面投影所定矢量方向为目标航向角，则，/>；式中，/>表示3D-LOS轨迹跟踪制导律所得目标航速；/>表示无人艇最高航速；/>表示3D-LOS轨迹跟踪制导律所得目标航向角；/>表示前视点的二维坐标；/>表示无人艇当前位置的二维坐标；最高效率接近未来前视点的方法为：
以当前点和前视点所定空间矢量解算目标航速和目标航向角，则，且/>，/>；式中，/>表示前视点时间；/>表示无人艇当前的时间。
3.如权利要求1所述的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法，其特征在于，所述基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法包括以下步骤：
步骤一，初始化前视半径，并给定目标航迹；
步骤二，初始化无人艇当前位置与当前时间；
步骤三，解算三维时空域中的前视点，确定前视点种类，进一步解算无人艇目标航向与目标航速；
步骤四，更新无人艇当前位置与当前时间。
4.如权利要求3所述的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法，其特征在于，步骤一中的前视半径为在包含时间维度的（X，Y，T）三维时空域下的前视球体半径，前视球体半径值由环境干扰力大小、无人艇跟踪控制系统精度或结合实际航行经验确定；
目标航迹由若干段匀加速直线运动拼接组成，通过平均速度处理将目标航迹简化为匀速直线运动，作匀速直线运动的无人艇目标航迹S方程为：
；
式中，x、y表示目标航迹直线上各点在二维平面的坐标；X0、Y0表示航迹段起点坐标；Vx、Vy分别表示航迹段在X、Y方向上平均速度；t为时间坐标；
步骤二中的当前位置与当前时间为无人艇在（X，Y，T）时空域下的实际三维坐标。
5.如权利要求3所述的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法，其特征在于，步骤三中的解算前视点的方法为：联立目标航迹方程和前视球面方程，消元并采用一元二次方程公式法求解；其中，由前视半径确定的前视球面方程为：
；
式中，XUSV、YUSV、TUSV为无人艇当前实际时空坐标；RT为前视半径；K为时间放大系数，其作用是调节时空比例；
通过联立作匀速直线运动的无人艇目标航迹S方程和前视球面方程，并消去x、y得到关于时间t的一元二次方程，采用求根公式求得前视点时间坐标，再代入目标航迹方程得到前视点X、Y坐标；根据前视点时间坐标与当前时间相对大小将前视点分为历史前视点和未来前视点。
6.如权利要求3所述的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法，其特征在于，步骤三中，根据前视球与目标航迹交点情况，将前视点分为6种情形进行目标航迹和目标航向角的解算；将解算得到的目标航向角与目标航速交予无人艇底层控制器，引导无人艇驶向前视点；通过在无人艇控制周期内重复制导、控制步骤，实现轨迹跟踪；其中，6种情形下的目标航向与目标航速的解算，具体如下：
（1）无前视点：将当前点与目标航迹的垂足作为引导点，问题转化为下述情形2或3；
（2）一个历史前视点：以当前点与前视点在（X，Y）二维平面投影所定矢量方向为目标航向角，以无人艇最高航速为目标航速；
（3）一个未来前视点：以当前点与前视点所定空间矢量确定目标航向角和目标航速；
（4）两个历史前视点：以航向角变化较小的前视点作引导，转化为情形2；
（5）两个未来前视点：以航向角变化较小的前视点作引导，转化为情形3；
（6）一个历史前视点和一个未来前视点：以未来前视点作引导，转化为情形3。
7.一种应用如权利要求1~6任意一项所述的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法的无人艇3D-LOS轨迹跟踪制导系统，其特征在于，无人艇3D-LOS轨迹跟踪制导系统包括：
前视半径初始化模块，用于根据环境干扰力大小、无人艇跟踪控制系统精度或结合实际航行经验确定三维时空域下的前视球体半径；
目标航迹确定模块，用于确定由若干段匀加速直线运动拼接组成的目标航迹，并通过平均速度处理将目标航迹简化为匀速直线运动；
位置与时间初始化模块，用于通过确定无人艇在（X，Y，T）时空域下的实际三维坐标，初始化无人艇当前位置与当前时间；
前视点解算模块，用于通过联立目标航迹方程与前视球面方程，利用数值求解得到前视点坐标，并确定目标航向与目标航速；
轨迹跟踪制导模块，用于将解算得到的目标航向与目标航速交予无人艇底层控制器，引导无人艇驶向前视点；通过在无人艇控制周期内重复制导、控制步骤，实现轨迹跟踪，同时更新无人艇当前位置与当前时间。
8.一种计算机设备，其特征在于，计算机设备包括存储器和处理器，存储器存储有计算机程序，计算机程序被处理器执行时，使得处理器执行如权利要求1~6任意一项所述的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法的步骤。
9.一种计算机可读存储介质，存储有计算机程序，计算机程序被处理器执行时，使得处理器执行如权利要求1~6任意一项所述的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导方法的步骤。
10.一种信息数据处理终端，其特征在于，信息数据处理终端用于实现如权利要求7所述的基于三维时空域的水面无人艇3D-LOS轨迹跟踪制导系统。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311499564.XA CN117234217B (zh) | 2023-11-13 | 2023-11-13 | 基于三维时空域的水面无人艇轨迹跟踪制导方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311499564.XA CN117234217B (zh) | 2023-11-13 | 2023-11-13 | 基于三维时空域的水面无人艇轨迹跟踪制导方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117234217A CN117234217A (zh) | 2023-12-15 |
| CN117234217B true CN117234217B (zh) | 2024-02-02 |
Family
ID=89093249
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311499564.XA Active CN117234217B (zh) | 2023-11-13 | 2023-11-13 | 基于三维时空域的水面无人艇轨迹跟踪制导方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117234217B (zh) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101726298A (zh) * | 2009-12-18 | 2010-06-09 | 华中科技大学 | 一种用于前视导航制导的立体地标选择和参考图制备方法 |
| CN101900564A (zh) * | 2010-07-21 | 2010-12-01 | 宇龙计算机通信科技(深圳)有限公司 | 一种动态视角的导航方法、终端、服务器及系统 |
| CN104166134A (zh) * | 2014-08-25 | 2014-11-26 | 电子科技大学 | 一种实波束前视扫描雷达目标二维定位方法 |
| CN111487966A (zh) * | 2020-04-13 | 2020-08-04 | 哈尔滨工程大学 | 一种基于航路点的水面无人艇自适应路径跟踪控制方法 |
| EP3739295A1 (en) * | 2019-05-13 | 2020-11-18 | S.A.T.E. - Systems and Advanced Technologies Engineering S.R.L. | Method for determining an optimal naval navigation routes from historical gnss data of naval trajectories |
| CN114005018A (zh) * | 2021-10-14 | 2022-02-01 | 哈尔滨工程大学 | 一种小算力驱动的水面无人艇多目标跟踪方法 |
| CN114967702A (zh) * | 2022-06-14 | 2022-08-30 | 集美大学 | 一种无人艇控制系统及路径跟踪方法 |
| CN116430856A (zh) * | 2023-03-21 | 2023-07-14 | 武汉理工大学 | 一种基于变前视距离los算法的船舶航迹控制方法 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7409290B2 (en) * | 2004-04-17 | 2008-08-05 | American Gnc Corporation | Positioning and navigation method and system thereof |
| WO2012034119A2 (en) * | 2010-09-11 | 2012-03-15 | Wayne State University | Guidance and control system for under-actuated marine surface ships and other automomous platforms |
| JP7507753B2 (ja) * | 2018-10-10 | 2024-06-28 | ファーサウンダー， インコーポレイテッド | 機械学習を用いた３次元前方探知ソナー標的認識 |
-
2023
- 2023-11-13 CN CN202311499564.XA patent/CN117234217B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101726298A (zh) * | 2009-12-18 | 2010-06-09 | 华中科技大学 | 一种用于前视导航制导的立体地标选择和参考图制备方法 |
| CN101900564A (zh) * | 2010-07-21 | 2010-12-01 | 宇龙计算机通信科技(深圳)有限公司 | 一种动态视角的导航方法、终端、服务器及系统 |
| CN104166134A (zh) * | 2014-08-25 | 2014-11-26 | 电子科技大学 | 一种实波束前视扫描雷达目标二维定位方法 |
| EP3739295A1 (en) * | 2019-05-13 | 2020-11-18 | S.A.T.E. - Systems and Advanced Technologies Engineering S.R.L. | Method for determining an optimal naval navigation routes from historical gnss data of naval trajectories |
| CN111487966A (zh) * | 2020-04-13 | 2020-08-04 | 哈尔滨工程大学 | 一种基于航路点的水面无人艇自适应路径跟踪控制方法 |
| CN114005018A (zh) * | 2021-10-14 | 2022-02-01 | 哈尔滨工程大学 | 一种小算力驱动的水面无人艇多目标跟踪方法 |
| CN114967702A (zh) * | 2022-06-14 | 2022-08-30 | 集美大学 | 一种无人艇控制系统及路径跟踪方法 |
| CN116430856A (zh) * | 2023-03-21 | 2023-07-14 | 武汉理工大学 | 一种基于变前视距离los算法的船舶航迹控制方法 |
Non-Patent Citations (3)
| Title |
|---|
| Finite-Time LOS Path Following of Unmanned Surface Vessels With Time-Varying Sideslip Angles and Input Saturation;Yalei Yu 等;IEEE/ASME TRANSACTIONS ON MECHATRONICS;第27卷(第1期);第463-472页 * |
| 无人艇自适应航迹跟踪控制策略研究;刘志强 等;江汉大学学报（自然科学版）;第51卷(第2期);第78-88页 * |
| 海洋环境下欠驱动无人艇航迹跟踪控制算法;陈霄 等;哈尔滨工业大学学报;第50卷(第10期);第110-117页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117234217A (zh) | 2023-12-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Chen et al. | A hybrid path planning algorithm for unmanned surface vehicles in complex environment with dynamic obstacles | |
| Wang et al. | A COLREGs-based obstacle avoidance approach for unmanned surface vehicles | |
| JP4495703B2 (ja) | 水中ロボットの動作制御方法、装置及びプログラム | |
| CN113093804B (zh) | 基于反演滑模控制的无人艇编队控制方法和控制系统 | |
| Liu et al. | Adaptive barrier Lyapunov function-based obstacle avoidance control for an autonomous underwater vehicle with multiple static and moving obstacles | |
| Li et al. | Toward time-optimal trajectory planning for autonomous ship maneuvering in close-range encounters | |
| CN111026135B (zh) | 一种无人艇高性能航行前馈控制系统及其控制方法 | |
| CN112947431A (zh) | 一种基于强化学习的无人船路径跟踪方法 | |
| CN111798702B (zh) | 无人艇路径跟踪控制方法、系统、存储介质及终端 | |
| CN114088094A (zh) | 一种无人艇的智能航路规划方法及系统 | |
| Wang et al. | Vector field-based integral LOS path following and target tracking for underactuated unmanned surface vehicle | |
| Sans-Muntadas et al. | Spiral path planning for docking of underactuated vehicles with limited FOV | |
| Wang et al. | A modified ALOS method of path tracking for AUVs with reinforcement learning accelerated by dynamic data-driven AUV model | |
| Du et al. | Multi-objective cooperative control for a ship-towing system in congested water traffic environments | |
| CN111798701B (zh) | 无人艇路径跟踪控制方法、系统、存储介质及终端 | |
| Zhang et al. | Time-optimal path planning and tracking based on nonlinear model predictive control and its application on automatic berthing | |
| Wen et al. | Online heuristically planning for relative optimal paths using a stochastic algorithm for USVs | |
| Du et al. | A target tracking guidance for unmanned surface vehicles in the presence of obstacles | |
| CN117234217B (zh) | 基于三维时空域的水面无人艇轨迹跟踪制导方法及系统 | |
| CN117369441A (zh) | 一种考虑船舶运动学和CORLEGs的自适应智能船舶路径规划方法 | |
| Zhu et al. | A novel route-plan-guided artificial potential field method for ship collision avoidance: Modeling, integration and test | |
| CN114035574A (zh) | 水面无人艇自主避障方法 | |
| Gan et al. | Reward shaping-based double deep q-networks for unmanned surface vessel navigation and obstacle avoidance | |
| Miotto et al. | UUV on-board path planning in a dynamic environment for the Manta test vehicle | |
| CN115268459B (zh) | 一种基于双喷水推进器无人艇自主靠泊控制方法 |
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 |