CN114047743A - 一种带预测的无人艇目标跟踪控制方法与系统 - Google Patents

Abstract

本发明公开了一种带预测的无人艇目标跟踪控制方法与系统，该方法包括以下步骤：1)感知目标船只的航速，航向，经纬度信息；2)计算相遇点的坐标；3)根据相遇点的坐标确定自身无人艇的设定航速和设定舵角，对无人艇的航速和舵角进行修正；4)根据修正后的无人艇的航速和舵角进行目标跟踪。本发明方法通过预测跟踪的相遇点和相遇时间，使得跟踪更加迅速，更加灵敏，且不需要进行实时的路径规划，只需要知道每一个时刻的目标与自身的相关信息就能实现目标跟踪。

Description

一种带预测的无人艇目标跟踪控制方法与系统

技术领域

本发明涉及智能路径规划技术，尤其涉及一种带预测的无人艇目标跟踪控制方法与系统。

背景技术

随着人工智能科技的飞速发展，众多无人系统研发成功，无人水面艇在民用和军事等领域的需求越来越明显。无人艇常常被用于设计完成感知、监测、跟踪等任务，而对运动目标物进行跟踪也是其中一项重要的任务，在能稳定跟踪目标物后，也能进一步完成监测目标状态等其他一系列任务。而面对多变的海况情景，以及可能存在的静态障碍和行驶中的船舶等动态障碍，完成目标跟踪任务会存在很多限制。并且，由于无人艇自身性能的限制、感知系统的测量范围限制、目标的下一时刻行驶状态的位置，再加上复杂的航行环境，即使人工掌舵也无法做到全程最优的控制行为，因此利用无人艇来进行智能跟踪十分具有研究意义。

利用无人艇进行目标实体跟踪不需要完美的跟踪目标行驶过的轨迹，只需要采用合适的跟踪控制策略令无人艇稳定行驶在目标附近一定范围内，确保目标不会丢失即可。而由于复杂的航行环境以及感知系统存在的测量误差会对控制无人艇稳定航行造成极大的干扰。

发明内容

本发明要解决的技术问题在于针对现有技术中的缺陷，提供一种带预测的无人艇目标跟踪控制方法与系统。

本发明解决其技术问题所采用的技术方案是：一种带预测的无人艇目标跟踪控制方法，包括以下步骤：

1)通过无人艇自身装载的雷达感知目标船只的航速，航向，经纬度信息；

2)计算相遇点的坐标；

2.1)根据跟踪需求设定稳定距离L_set和稳定角度(边界条件，即无人艇位于目标船只航向向后开角一定度数的范围内)，最理想的跟踪位置为一个点，称为理想点；

2.2)根据目标船只的坐标、设置的稳定距离L_set和目标船的航向角β，确定理想点的坐标；

其中，(x_t，y_t)为目标船只的坐标；(x_d，y_d)为理想点的坐标；

2.3)根据目标船只的航速、无人艇的航速以及理想点的坐标，确定无人艇与理想点相遇的预测时间；

其中，T为无人艇与理想点相遇的预测时间，v_t为目标船只的实时航速，L_sd为无人艇与理想点的距离，θ为无人艇指向理想点的方向和目标船只航向的夹角，v_s为无人艇的实时航速；

2.4)根据无人艇与理想点相遇的时间，计算相遇点的坐标；

其中，(x_dt，y_dt)为相遇点的坐标；

3)根据相遇点的坐标确定自身无人艇的设定航速和设定舵角，对无人艇的航速和舵角进行修正；

4)根据修正后的无人艇的航速和舵角进行目标跟踪。

按上述方案，所述步骤3)中根据相遇点的坐标确定自身无人艇的设定航速和设定舵角，具体如下：

3.1)通过航向PID算法计算设定舵角

无人艇航向的控制采用位置式PD控制，控制的偏差为自身航向角和自身指向相遇点方向的夹角Δα，通过消除偏差Δα来达到控制效果，相关的算法公式如下：

式中，K_p为比例项系数，K_d为微分项系数，Δα为角度偏差，Δα₁为上一控制周期的角度偏差，P_out为比例项输出，D_out为微分项输出，δ_c为设定舵角；

3.2)通过航速PID算法计算设定航速

无人艇的航速控制采用PI控制，根据目标跟踪的距离要求，控制的偏差为距离偏差，通过消除距离偏差，达到控制效果。相关的算法公式如下：

式中，K_p为比例项系数，K_i为积分项系数，Δd为距离偏差，它等于设置的跟踪距离L_set减去自身与目标船只的距离d，S_i为距离偏差Δd累加的结果，P_out为比例项输出，I_out为积分项输出，v_t为目标船只的航速，V_E为设定航速。

按上述方案，所述步骤4)中，若自身无人艇距离理想点低于设定距离，此时将跟踪目标从理想点切换为目标船只坐标。

本发明还提供一种带预测的无人艇目标跟踪控制系统，包括：

采集模块，用于通过无人艇自身装载的雷达感知目标船只的航速，航向，经纬度信息；

相遇点确定模块，用于计算相遇点的坐标；具体过程如下：

1)根据跟踪需求设定稳定距离L_set和稳定角度(边界条件，即无人艇位于目标船只航向向后开角一定度数的范围内)，最理想的跟踪位置为一个点，称为理想点；

2)根据目标船只的坐标、设置的稳定距离L_set和目标船的航向角β，确定理想点的坐标；

其中，(x_t，y_t)为目标船只的坐标；(x_d，y_d)为理想点的坐标；

3)根据目标船只的航速、无人艇的航速以及理想点的坐标，确定无人艇与理想点相遇的时间；

其中，T为无人艇与理想点相遇的时间，v_t为目标船只的实时航速， L_sd为无人艇与理想点的距离，θ为无人艇指向理想点的方向和目标船只航向的夹角，v_s为无人艇的实时航速；

4)根据无人艇与理想点相遇的时间，计算相遇点的坐标；

其中，(x_dt，y_dt)为相遇点的坐标；

修正模块，用于根据相遇点的坐标确定自身无人艇的设定航速和设定舵角，对无人艇的航速和舵角进行修正；

跟踪控制模块，根据修正后的无人艇的航速和舵角进行目标跟踪。

按上述方案，所述修正模块中根据相遇点的坐标确定自身无人艇的设定航速和设定舵角，具体如下：

1)通过航向PID算法计算设定舵角

无人艇航向的控制采用位置式PD控制，控制的偏差为自身航向角和自身指向相遇点方向的夹角Δα，通过消除偏差Δα来达到控制效果，相关的算法公式如下：

式中，K_p为比例项系数，K_d为微分项系数，Δα为角度偏差，Δα₁为上一控制周期的角度偏差，P_out为比例项输出，D_out为微分项输出，δ_c为设定舵角；

2)通过航速PID算法计算设定航速

无人艇的航速控制采用PI控制，根据目标跟踪的距离要求，控制的偏差为距离偏差，通过消除距离偏差，达到控制效果。相关的算法公式如下：

式中，K_p为比例项系数，K_i为积分项系数，Δd为距离偏差，它等于设置的跟踪距离L_set减去自身与目标船只的距离d，S_i为距离偏差Δd累加的结果，P_out为比例项输出，I_out为积分项输出，v_t为目标船只的航速，V_E为设定航速。

按上述方案，所述跟踪控制模块中，若自身无人艇距离理想点低于设定距离，此时将跟踪目标从理想点切换为目标船只坐标。

本发明产生的有益效果是：

1、本发明通过预测跟踪的相遇点和相遇时间，使得跟踪更加迅速，更加灵敏。

2、本发明的无人艇目标跟踪方案不需要进行实时的路径规划，只需要知道每一个时刻的目标与自身的相关信息就能实现目标跟踪，节约了计算量，并且跟踪更加准确。

3、本发明的目标跟踪切换方式解决了当自身无人艇距离所要求的目标区域较近时舵机产生异常的过大打角的问题，使得跟踪更为稳定。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1是本发明实施例的方法流程图；

图2是本发明实施例的目标跟踪的理想点示意图；

图3是本发明实施例的通过预测算法计算相遇点的原理图；

图4是本发明实施例的通过消除角度偏差控制航向的示意图；

图5是本发明实施例的无人艇航速航向控制的流程图；

图6是本发明实施例的仿真的最终结果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

如图1所示，一种带预测的无人艇目标跟踪控制方法，包括以下步骤：

1)根据无人艇自身装载的雷达感知目标船只的航速，航向，经纬度信息；

2)计算相遇点的坐标

本发明中的无人艇目标跟踪是稳定距离，稳定角度的跟踪，即标准为自身无人艇和目标船只的距离要保持一定范围，自身无人艇要位于目标船只航向向后开角一定度数的范围内，符合要求的区域是目标船只后方的一个扇形圆环区域，最理想的跟踪位置为一个点，称为理想点，如图2。理想点坐标的计算公式如下：

式中，x_d，y_d为理想点的坐标，x_t，y_t为目标船只的坐标，L_set为设置的跟踪距离，β为目标船的航向角。

如图3，通过预测算法可以得到一段时间后，自身无人艇与理想点的相遇位置，由于最好的控制效果是让自身无人艇到达理想点，与理想点重合。所以，该相遇位置就是自身无人艇需要跟踪的目标。采用预测算法进行目标跟踪比传统的直接跟踪目标船本身跟踪效果更快，更灵敏。该算法方程如下：

式中，v_t是目标船只的航速，v_s是自身无人艇的航速，L_sd为自身无人艇与理想点的距离，θ为自身无人艇指向理想点的方向和目标船只航向的夹角，T为自身无人艇与理想点相遇的时间。

整理得：

解方程得：

T的两个解设为T₁，T₂。

当v_s>v_t时，有以下结论：

①T必有实数解T₁，T₂

②T₁，T₂的值必有一个为正，一个为负

③当式中的±号取“-”号时，T取得正值

由于在进行目标跟踪时，自身无人艇的航速大于目标船只的航速，即满足v_s>v_t，所以在实际运用时可取T₁，T₂中正值作为真正的T值。

得到T值后，可利用如下公式计算相遇点的坐标：

式中x_dt，y_dt为相遇点的坐标，x_d，y_d为理想点的坐标，v_t为目标船只的航速，β为目标船只的航向角。由此，便得到了相遇点的坐标，即自身无人艇真正的跟踪目标。

3)通过航向和航速PID算法，计算自身无人艇的设定航速和设定舵角。

3.1)通过航向PID算法计算设定舵角

如图4，无人艇航向的控制采用位置式PD控制，控制的偏差为自身航向角和自身指向相遇点方向的夹角Δα，通过消除偏差Δα来达到控制效果。相关的算法公式如下：

式中，K_p为比例项系数，K_d为微分项系数，Δα为角度偏差，Δα₁为上一控制周期的角度偏差，P_out为比例项输出，D_out为微分项输出，δ_c为设定舵角。

3.2)通过航速PID算法计算设定航速

无人艇的航速控制采用PI控制，根据目标跟踪的距离要求，控制的偏差为距离偏差，通过消除距离偏差，达到控制效果。相关的算法公式如下：

式中，K_p为比例项系数，K_i为积分项系数，Δd为距离偏差，它等于设置的跟踪距离L_set减去自身与目标船只的距离d，S_i为距离偏差Δd累加的结果，P_out为比例项输出，I_out为积分项输出，v_t为目标船只的航速，V_E为设定航速。

4)根据修正后的无人艇的航速和舵角进行控制无人艇的舵机和喷泵完成目标跟踪，控制流程如图5；

我们根据无人艇自身的性质建立无人艇的舵机模型(改变方向) 和喷泵模型(提供动力)，用于无人艇的方向控制和航速控制；

4.1)无人艇舵机模型

无人艇的舵机一般为较复杂的伺服系统，它主要包括舵令的发送，功率放大，舵角反馈等部分。在本发明中，将舵机视为一个惯性环节，舵机的特性可表示为：

式(1)中，K_c为舵机的增益系数，T_c为舵机系统的时间常数，δ_c为舵角的设定值，δ为舵角的真实值，δ_max为舵角限幅，r_S为舵角变化的角速度，r_Smax为舵角角速度限幅。

此外，还有一种表示了无人艇艏向角与舵角关系的模型——响应模型，该模型适用于无人艇的方向控制。常用的Nomoto响应模型分为一阶模型，二阶模型，线性，非线性模型。其中无人艇的二阶线性模型如下：

式(2)中，T₁，T₂，T₃，K为无人艇的操纵性参数，δ为舵机的舵角，_r为航向变化的角速度。将上式简化可得到Nomoto一阶线性模型，如下：

本发明即采用式(3)进行建模，式(3)也叫做野本方程。

4.2)无人艇喷泵模型

无人艇的喷泵为船艇提供动力，产生航行的速度，从无人艇航速控制器发出航速控制指令到无人艇达到该速度需要一定的时间，即存在延迟，所以采用一个一阶惯性环节来表示无人艇的喷泵模型。如下：

式(4)中，T_E为无人艇喷泵的时间常数，K_E为喷泵的速度增益， V_E为航速设定值，V为航速真实值。

4.3)建立无人艇运动学模型

无人艇的运动学模型体现了无人艇位置与其航速，航向的关系，如下：

式(5)中x_k+1，y_k+1是无人艇下一时刻的坐标，x_k，y_k为当前时刻的坐标，α_k为当前时刻的航向角，v_k为当前时刻的航速，r为无人艇航向变化的角速度，T_c为控制周期。

当自身无人艇坐标距离理想点较近时需要做特殊处理，将此时的跟踪目标切换为目标船只本身，本实施例中，一般我们将自身无人艇距离理想点的阈值设置为目标船只船长L的2-4倍。

当自身无人艇距离理想点较近时，从物理位置上看自身与理想点距离较小，但此时角度偏差有可能很大，这样会造成两者距离明明很近但是自身无人艇的舵机打角却很大，所以此时应将跟踪目标切换为目标船只本身。

仿真计算如下：自身无人艇船长10米，自身船与目标船只初始距离为2000米，目标船只航行速度为9米/s,仿真的控制周期为2s，目标跟踪的要求为：自身无人艇距离目标船只距离400±50米；自身无人艇需在目标船只航向向后开角30°的范围内。

采用本发明所提出的无人艇目标跟踪方法进行仿真，得到如图6 所示的最终仿真结果图。

应当理解的是，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，而所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (6)

1.一种带预测的无人艇目标跟踪控制方法，其特征在于，包括以下步骤：

1)感知目标船只的航速，航向，经纬度信息；

2)计算相遇点的坐标；

2.1)根据跟踪需求设定跟踪稳定距离L_set和稳定角度，即无人艇跟踪时位于目标船只航向向后开角小于稳定角度度数的范围内，最理想的跟踪位置称为理想点；

2.2)根据目标船只的坐标、设置的稳定距离L_set和目标船的航向角β，确定理想点的坐标；

其中，(x_t，y_t)为目标船只的坐标；(x_d，y_d)为理想点的坐标；

2.3)根据目标船只的航速、无人艇的航速以及理想点的坐标，确定无人艇与理想点相遇的预测时间；

其中，T为无人艇与理想点相遇的预测时间，v_t为目标船只的实时航速，L_sd为无人艇与理想点的距离，θ为无人艇指向理想点的方向和目标船只航向的夹角，v_s为无人艇的实时航速；

2.4)根据无人艇与理想点相遇的时间，计算相遇点的坐标；

其中，(x_dt，y_dt)为相遇点的坐标；

3)根据相遇点的坐标确定自身无人艇的设定航速和设定舵角，对无人艇的航速和舵角进行修正；

4)根据修正后的无人艇的航速和舵角进行目标跟踪。

2.根据权利要求1所述的带预测的无人艇目标跟踪控制方法，其特征在于，所述步骤3)中根据相遇点的坐标确定自身无人艇的设定航速和设定舵角，具体如下：

3.1)通过航向PID算法计算设定舵角

无人艇航向的控制采用位置式PD控制，控制的偏差为自身航向角和自身指向相遇点方向的夹角Δα，通过消除偏差Δα来达到控制效果，公式如下：

式中，K_p为比例项系数，K_d为微分项系数，Δα为角度偏差，Δα₁为上一控制周期的角度偏差，P_out为比例项输出，D_out为微分项输出，δ_c为设定舵角；

3.2)通过航速PID算法计算设定航速

无人艇的航速控制采用PI控制，根据目标跟踪的距离要求，控制的偏差为距离偏差，公式如下：

式中，K_p为比例项系数，K_i为积分项系数，Δd为距离偏差，它等于设置的跟踪距离L_set减去自身与目标船只的距离d，S_i为距离偏差Δd累加的结果，P_out为比例项输出，I_out为积分项输出，v_t为目标船只的航速，V_E为设定航速。

3.根据权利要求1所述的带预测的无人艇目标跟踪控制方法，其特征在于，所述步骤4)中，若自身无人艇距离理想点低于设定距离，此时将跟踪目标从理想点切换为目标船只坐标。

4.一种带预测的无人艇目标跟踪控制系统，其特征在于，包括：

采集模块，用于感知目标船只的航速，航向，经纬度信息；

相遇点确定模块，用于计算相遇点的坐标；具体过程如下：

1)根据跟踪需求设定稳定距离L_set和稳定角度，最理想的跟踪位置为一个点，称为理想点；

2)根据目标船只的坐标、设置的稳定距离L_set和目标船的航向角β，确定理想点的坐标；

其中，(x_t，y_t)为目标船只的坐标；(x_d，y_d)为理想点的坐标；

3)根据目标船只的航速、无人艇的航速以及理想点的坐标，确定无人艇与理想点相遇的预测时间；

其中，T为无人艇与理想点相遇的预测时间，v_t为目标船只的实时航速，L_sd为无人艇与理想点的距离，θ为无人艇指向理想点的方向和目标船只航向的夹角，v_s为无人艇的实时航速；

4)根据无人艇与理想点相遇的预测时间，计算相遇点的坐标；

其中，(x_dt，y_dt)为相遇点的坐标；

修正模块，用于根据相遇点的坐标确定自身无人艇的设定航速和设定舵角，对无人艇的航速和舵角进行修正；

跟踪控制模块，根据修正后的无人艇的航速和舵角进行目标跟踪。

5.根据权利要求4所述的带预测的无人艇目标跟踪控制系统，其特征在于，所述修正模块中根据相遇点的坐标确定自身无人艇的设定航速和设定舵角，具体如下：

1)通过航向PID算法计算设定舵角

无人艇航向的控制采用位置式PD控制，控制的偏差为自身航向角和自身指向相遇点方向的夹角Δα，通过消除偏差Δα来达到控制效果，相关的算法公式如下：

式中，K_p为比例项系数，K_d为微分项系数，Δα为角度偏差，Δα₁为上一控制周期的角度偏差，P_out为比例项输出，D_out为微分项输出，δ_c为设定舵角；

2)通过航速PID算法计算设定航速

无人艇的航速控制采用PI控制，根据目标跟踪的距离要求，控制的偏差为距离偏差，通过消除距离偏差，达到控制效果。相关的算法公式如下：

式中，K_p为比例项系数，K_i为积分项系数，Δd为距离偏差，它等于设置的跟踪距离L_set减去自身与目标船只的距离d，S_i为距离偏差Δd累加的结果，P_out为比例项输出，I_out为积分项输出，v_t为目标船只的航速，V_E为设定航速。

6.根据权利要求4所述的带预测的无人艇目标跟踪控制系统，其特征在于，所述跟踪控制模块中，若自身无人艇距离理想点低于设定距离，此时将跟踪目标从理想点切换为目标船只坐标。

Images