CN117170382A - 一种适用于同向实时点位跟踪的双无人船协同控制方法 - Google Patents

Abstract

本发明提供一种适用于同向点位跟踪的双无人船协同控制方法，包括：建立领航‑跟随无人船位置协同编队控制模型，设定期望纵向距离及方位角；基于编队协同控制模型及领航船的实时位置，对跟随船建立距离及方位角的协同动态方程；基于协同动态方程，并考虑推进器约束限制，引入障碍李亚普诺夫函数，推导跟随船的实时期望速度及偏航角；并利用非线性扰动观测器观测跟随船的集合扰动；为了使跟随船在环境扰动下能够领航船保持期望距距离及方位角，引入深度强化学习双Deep Q‑Network(DQN)网络，依据跟随船的实时状态，控制其纵向推力力矩及艏向角力矩；相比传统控制方法，本发明的技术方案能够根据跟随船实时状态快速，准确地控制其与领航船保持期望纵向距离及方位角。

Description

一种适用于同向实时点位跟踪的双无人船协同控制方法

技术领域

本发明涉及无人船点位跟踪控制技术领域，具体涉及一种适用于同向实时点位跟踪的双无人船协同控制方法。

背景技术

近年来，随着科技的快速发展和自动化技术的不断创新，无人船的使用规模不断扩大，其在海洋调查、监测、救援、航道疏浚、风电场运维以及水上娱乐和旅游等领域发挥着越来越重要的作用。在这些场景中，需要根据路径上事先设定好的虚拟目标点进行精确航行的领域越来越多。同时，随着通信技术、和多智能体控制技术的不断发展，需要多无人船保精确的位置关系协同控制的应用场景或者与无人潜航器配合协同航行的场景也应运而生，因此，无人船点位跟踪控制技术已经成为了新的研究热点。但是，目前对于双无人船水面航行协同跟踪控制的研究相对缺乏，在现有技术中，为实现多无人船协同控制，常使用编队控制相关控制算法进行实现，但相关技术多适用于多船航行过程中的队形保持，对于双船协同航行的场景并不适用。同时，此类方法多关注队形保持及队形破坏后的重组能力，对于相对位置精确性以及在复杂水面环境下，应对海面各种环境干扰，同时保证跟随船与领航船之间航行的精确位置关系，现有技术存在明显不足。

目前现有无人船协同航行控制方法的研究主要有以下不足：

(1)传统基于编队位置的模型策略，需要精确的模型参数，并且需要较为复杂的计算过程。比如论文《Swarm control of USVs based on adaptive backstepping combinedwith sliding mode》对无人船集群建立虚拟结构，并利用反步滑模控制方法消除编队协同控制中的误差，对于双船协同航行场景，无可避免出现计算复杂问题。

(2)对于现有的控制方法，当跟随船与领航船之间的协同位置关系由于水面环境等原因受到破坏后，无法依据现有实时环境及与期望位置的距离关系，选择最为合适的动作快速恢复协同位置关系。

针对上述问题，本方法在双无人船协同控制模型的基础上，引入障碍李亚普诺夫函数限制纵向速度及艏向角期望幅值设计虚拟控制律，在此基础上，利用虚拟控制律及位置误差设计奖励函数，考虑环境扰动将其作为Deep Q-Network(DQN)神经网络的输入，依据实时状态选择训练DQN神经网络。最后将收敛后的DQN决策网络部署到跟随船代替控制器进行跟踪控制。

发明内容

为解决现有技术中在双无人船协同运动控制存在的问题，本发明提出一种基于预先训练DQN决策网络的双无人船协同运动控制方法，可使系统达到快速、精确、稳定的性能要求，并提供相应的控制方法。

为实现上述目的，本发明通过如下技术方案实现：

一种适用于同向实时点位跟踪的双无人船协同控制方法，其特征在于，包括以下步骤：

S1、建立跟随船运动学与动力学模型。

S2、基于领航船的实时位置，及跟随船相对领航船位置的期望方向角及相对距离，确定点位跟踪控制协同系统模型。

S3、基于点位跟踪协同控制模型，及实时距离误差与方向角误差，引入障碍李雅普诺夫函数对跟随船设计纵向速度虚拟控制律及艏向角虚拟控制律。

S4、引入非线性扰动观测器观测环境扰动，并将其作为外部环境变量，和跟随船与领航船的实时位置误差，设计并训练主DQN决策网络，该决策网络可根据实时输入的状态选择最为合适的动作完成点位跟踪协同控制。

S5、仿真验证所提出方法的有效性。

所述的S1中，考虑双无人船协同点位跟踪控制系统，对于跟随船，其运动学模型可被描述为：

式中，u_f、v_f、r_f分别为跟随船的纵向速度、横向速度以及转艏角速度，(x,y,ψ)为跟随船的纵向位置、横向位置及艏向角。令η_f＝[x_f,y_f,ψ_f]^T，则式(1)可重写为：

考虑系统误差及外部扰动，其动力学模型可设计为：

式中，m_11f、m_22f、m_33f表示跟随船包含附加质量的惯性矩阵系数，τ_duf、τ_dvf、τ_drf为三个方向上的扰动力矩，τ_uf、τ_rf为控制力矩输入，d_11f、d_22f、d_33f为跟随船的附加质量惯性矩阵系数，u_f、v_f、r_f为跟随船的纵向速度、横向速度及艏向角速度。令则式(3)可重写为：

式中，为斜对角矩阵。

所述的S2中，点位跟踪控制协同系统模型领航船与跟随船之间的距离及角度关系为：

式中，(x_l,y_l)为领航船的实时位置，(x_f,y_f)为跟随船的实时位置，d_i为实时距离,为跟随船相对领航船的方向角。

所述的S3中，为建立点位跟踪协同控制模型，定义距离误差与方向角误差为：

式中，d_resi、分别为期望距离与期望方向角。

为了使实际距离与实际方向角能跟踪上期望距离与方向角，定义距离与方向角的动态微分方程为：

对式(7)求导，并结合式(8)、(9)得：

式中，α_uf与α_ψf为跟随船纵向速度与艏向角虚拟控制律。并且设标量

为了使实际距离与实际方向角能够跟踪上期望距离及期望方向角，定义

其中，e _df、分别是跟踪距离误差的下界与上界，/>跟踪方向角误差的下界与上界。

为了保证控制输出不违反上述约束，引入障碍李亚普诺夫函数：

式中，

为求解α_uf，对式(13)求导得：

设并将式(10)代入，

则式(16)可被重写为：

因此标量w_d可被镇定为：

同理，为求解α_ψf，对式(14)求导得：

设并将式(11)代入，

则式(19)可被重写为：

因此标量可被镇定为：

最终，式(10)与式(11)中的虚拟控制律α_uf与α_ψf可被设计为：

所述的S4中，所引入非线性扰动观测器为：

式中，为扰动观测器对各方向扰动的估计值，K₀∈R^3×3为正定设计矩阵。

为使得跟随船的实时位置与其期望与领航船的相对位置的误差保持在极小范围内，本方法引入双DQN网络根据跟随船当前的位置速度、与期望位置的相对距离、实时扰动进行学习评估，并选择最适宜的动作对跟随船的纵向推进控制力矩，艏向角控制力矩进行实时调整，使其满足：

lim||η(x,y)-η_d(x_d,y_d)||＝ε ε≥0 (25)

式中，ε是一个极小的正整数，η(x,y)表示跟随船的实时位置，η_d(x_d,y_d)表示跟随船相对领航船的期望位置。

本方法引入的深度强化学习DON技术设计点位跟踪控制决策系统。首先，根据实时期望位置及跟随船的实际位置得到纵向及横向实时位置误差，然后将实时位置误差与跟随船的位置状态、速度、及非线性观测器对三个方向上的扰动力矩观测值作为实时环境变量输入训练完成的DQN决策网络中。DQN决策网络根据输入的实时状态，得到对于各种不同动作的奖励，选取奖励值最大的动作作为最优动作，接下来根据所选取的最优动作对纵向推力控制力矩及艏向角控制力矩进行调整，并将调整后的力矩作用到跟随船的运动学与动力学模型上得到新的实时位置及速度。最后将新的实时位置及速度、扰动估计值、位置误差输入到DQN决策网络进行下一轮控制。

Q网络的建立过程，具体包括：

建立状态空间S＝{u,v,r,x,y,ψ,e_x,e_y,e_u,e_ψ}，动作空间A＝{a_i|i＝1...9}，奖励r。A(s)表示在状态s下可选择的动作集合，S(t)、A(t)、R(t)、G(t)分别表示t步时的状态、选择动作、瞬时奖励和累计奖励。π(s)表示在状态s下选择的动作策略，π*表示最优策略，π(a|s)表示在状态s下策略π选择动作a的概率。r(s,a)指代在状态s下选择动作a的奖励，r(s',a,s)表示在s状态下选择动作a使状态s变成s'的奖励，v_π(s)为策略价值函数，表示策略在状态s下的累计奖励，q_π(s,a)为行动价值函数，表示在状态s下选择动作a的累计奖励。v*(s)为最优状态值，q*(s,a)为状态s下动作a的最优动作值。

因此，在状态s情况下，选择动作a所得到的期望奖励r(s,a)为：

式中，p(s'|s,a)为在状态s情况下选择动作a从而达到状态s'的概率。

从状态s下选择动作a转化到状态s'时，其期望获得的奖励r(s,a,s')为：

式中，p(s',r|s,a)为状态s时选择动作a，将状态转化为s'的概率。

深度强化学习的本质是从初始状态到最终状态的每一步选择最优策略π*，使得其累计奖励G(t)最大化，并且针对其中的各个状态，根据策略π*选择每一步的最优动作a。其中，G(t)是每一步瞬时奖励与折扣因子乘积的总和：

式中，0≤γ≤1为折扣因子，k为动作编号。

对于从初始状态到最终状态的每一个动作，都可以计算出每个状态下执行不同动作所对应的不同奖励值。因此策略价值函数v_π(s)可以表示为：

行动价值函数q_π(s,a)可表示为：

式中，表示对•的期望。因此，策略价值函数与行动价值函数的关系可表示为：

v_π(s)＝maxq_π(s,a) a∈A (31)

最后，选择最优动作使π(s)达到最大值，即满足：

π(s)＝argmax_avπ(s'∣s,a)＝argmax_aqπ(s,a) (32)

双DQN网络训练的体步骤为：

S4.1：初始化主DQN决策网络、target-DQN评价网络，设定前期观察轮数N_obs、训练轮数N_train，总轮数N_total，单轮最大步数T_episode，主DQN决策网络向target-DQN评价网络的更新周期N_renew，参数调节周期N_gap，纵向位置容许偏离e_x，横向位置容许偏离e_y。

S4.2：设定备选动作及动作选择策略，其中动作分别为纵向推力增加，艏向角控制力矩增加、纵向推力增加，艏向角控制力矩保持、纵向推力增加，艏向角控制力矩减小、纵向推力保持，艏向角控制力矩增加、纵向推力保持，艏向角控制力矩保持、纵向推力保持，艏向角控制力矩减小、纵向推力减小，艏向角控制力矩增加、纵向推力减小，艏向角控制力矩保持、纵向推力减小，艏向角控制力矩减小共9个动作；动作选择策略为：

式中，ε为随机选取动作的概率，表示在每个步每一轮的动作选择中有ε的概率是随机选择的，1-ε的概率是选择主DQN决策网络输出奖励值最高的动作；Ns是训练的总轮数即N_total，T表示训练的当前轮数。

S4.3：根据式(22)、(23)中的虚拟控制律及跟随船的实时位置与期望位置的误差设定奖励函数；其中，奖励函数设置为：

式中，k_x、k_y∈(0,1)分别为纵向方向与横向方向的奖励权重系数，e_x＝x-x_d为当前纵向位置与期望纵向位置中心点的距离误差，e_y＝y-y_d为当前横向位置与期望横向位置中心点的距离误差，为了使期望位置中心点附近有较高的奖励值，取σ＝0.5。

式中，k_u、k_ψ∈(0,1)分别为纵向速度奖励权重系数与艏向角奖励权重系数。取综合奖励r为r_p与r_v的总合，即：

r＝r_p+r_v (36)

S4.4：开始训练，若当前轮数小于N_total，进入步骤S4.5，否则结束训练过程。

S4.5：设置智能体及环境初始状态(跟随船初始位置、速度，初始期望位置、初始环境扰动)。

S4.6：判断前步数是否大于T_episode，若是，返回步骤S4.5，否则进入步骤S4.7。

S4.7：按照所设定的动作选择策略选择当前步的动作。

S4.8：将动作代入跟随船运动学与动力学模型进行状态更新。

S4.9：将前一状态、新状态、当前步动作选择存放入经验池中。

S4.10：若当前轮数小于N_gap的倍数，进入步骤S4.6，否则进入步骤S4.11。

S4.11：从经验池中随机抽取若干组数据，利用target-DQN评价网络及奖励函数对经验池中的当前状态下的各个动作进行评价，并利用梯度下降法对主DQN决策网络进行参数修正。

S4.12：判断当前轮数是否达到更新周期N_renew，若达到，将主DQN决策网络中的权重参数赋值给target-DQN评价网络。

S4.13：若跟随船进入期望位置允许偏离范围内，将此后的纵向及横向偏离位置分别大于e_x、e_y作为单轮结束条件。

S4.14：返回步骤S4.4。

与现有技术相比，本发明具有的有益效果为：

(1)本发明利用预先设计的DQN决策网络进行动作选择，代替传统方法中使用的控制器进行控制，能够依据实时位置误差和环境快速做出最优决策，同时避免了传统方法控制过程中复杂的计算过程，在协同位置关系受到破坏后能够快速恢复，提高了控制的平稳性。

(2)将实时扰动作为环境变量的一部分参与到DQN决策网络的训练中，能够更为充分的考虑环境扰动的影响，保证对双船协同航行的进行精确控制。

附图说明

为了更清楚地说明现有技术中的技术方案和本发明的实施例，下面将对现有技术和实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它的附图。

图1是本发明的控制系统结构图；

图2是本发明双DQN网络训练过程框图；

图3是本发明的DQN网络结构及动作选择；

图4是本发明双DQN网络训练流程图；

图5是本发明训练过程中每10轮平均控制步数；

图6是本发明数值仿真时领航船与跟随船实时位置关系图；

图7是本发明数值仿真时领航船与跟随船实时距离图；

图8是本发明跟随船控制力矩输出曲线。

具体实施方式

一种适用于同向实时点位跟踪的双无人船协同控制方法，其特征在于，包括以下步骤：

S1、建立跟随船运动学与动力学模型。

S2、基于领航船的实时位置，及跟随船相对领航船位置的期望方向角及相对距离，确定点位跟踪控制协同系统模型。

S3、基于点位跟踪协同控制模型，及实时距离误差与方向角误差，引入障碍李雅普诺夫函数对跟随船设计纵向速度虚拟控制律及艏向角虚拟控制律。

S4、引入非线性扰动观测器观测环境扰动，并将其作为外部环境变量，和跟随船与领航船的实时位置误差，设计并训练主DQN决策网络，该决策网络可根据实时输入的状态选择最为合适的动作完成点位跟踪协同控制。

S5、仿真验证所提出方法的有效性。

所述的S1中，考虑双无人船协同点位跟踪控制系统，对于跟随船，其运动学模型可被描述为：

式中，u_f、v_f、r_f分别为跟随船的纵向速度、横向速度以及转艏角速度，(x,y,ψ)为跟随船的纵向位置、横向位置及艏向角。令η_f＝[x_f,y_f,ψ_f]^T，则式(1)可重写为：

考虑系统误差及外部扰动，其动力学模型可设计为：

式中，m_11f、m_22f、m_33f表示跟随船包含附加质量的惯性矩阵系数，τ_duf、τ_dvf、τ_drf为三个方向上的扰动力矩，τ_uf、τ_rf为控制力矩输入，d_11f、d_22f、d_33f为跟随船的附加质量惯性矩阵系数，u_f、v_f、r_f为跟随船的纵向速度、横向速度及艏向角速度。令则式(3)可重写为：

式中，为斜对角矩阵。

所述的S2中，点位跟踪控制协同系统模型领航船与跟随船之间的距离及角度关系为：

式中，(x_l,y_l)为领航船的实时位置，(x_f,y_f)为跟随船的实时位置，d_i为实时距离,为跟随船相对领航船的方向角。

所述的S3中，为建立点位跟踪协同控制模型，定义距离误差与方向角误差为：

式中，d_resi、分别为期望距离与期望方向角。

为了使实际距离与实际方向角能跟踪上期望距离与方向角，定义距离与方向角的动态微分方程为：

对式(7)求导，并结合式(8)、(9)得：

式中，α_uf与α_ψf为跟随船纵向速度与艏向角虚拟控制律。并且设标量

为了使实际距离与实际方向角能够跟踪上期望距离及期望方向角，定义

其中，e_df、分别是跟踪距离误差的下界与上界，/>跟踪方向角误差的下界与上界。

为了保证控制输出不违反上述约束，引入障碍李亚普诺夫函数：

式中，

为求解α_uf，对式(13)求导得：

设并将式(10)代入，

则式(16)可被重写为：

因此标量w_d可被镇定为：

同理，为求解α_ψf，对式(14)求导得：

设并将式(11)代入，

则式(19)可被重写为：

因此标量可被镇定为：

最终，式(10)与式(11)中的虚拟控制律α_uf与α_ψf可被设计为：

所述的S4中，所引入非线性扰动观测器为：

式中，为扰动观测器对各方向扰动的估计值，K₀∈R^3×3为正定设计矩阵。

为使得跟随船的实时位置与其期望与领航船的相对位置的误差保持在极小范围内，本方法引入双DQN网络根据跟随船当前的位置速度、与期望位置的相对距离、实时扰动进行学习评估，并选择最适宜的动作对跟随船的纵向推进控制力矩，艏向角控制力矩进行实时调整，使其满足：

lim||η(x,y)-η_d(x_d,y_d)||＝ε ε≥0 (25)

式中，ε是一个极小的正整数，η(x,y)表示跟随船的实时位置，η_d(x_d,y_d)表示跟随船相对领航船的期望位置。

本方法引入的深度强化学习DON技术设计点位跟踪控制决策系统。如图1所示，首先，根据实时期望位置及跟随船的实际位置得到纵向及横向实时位置误差，然后将实时位置误差与跟随船的位置状态、速度、及非线性观测器对三个方向上的扰动力矩观测值作为实时环境变量输入训练完成的DQN决策网络中。DQN决策网络根据输入的实时状态，得到对于各种不同动作的奖励，选取奖励值最大的动作作为最优动作，接下来根据所选取的最优动作对纵向推力控制力矩及艏向角控制力矩进行调整，并将调整后的力矩作用到跟随船的运动学与动力学模型上得到新的实时位置及速度。最后将新的实时位置及速度、扰动估计值、位置误差输入到DQN决策网络进行下一轮控制。

Q网络的建立过程，具体包括：

建立状态空间S＝{u,v,r,x,y,ψ,e_x,e_y,e_u,e_ψ}，动作空间A＝{a_i|i＝1...9}，奖励r。A(s)表示在状态s下可选择的动作集合，S(t)、A(t)、R(t)、G(t)分别表示t步时的状态、选择动作、瞬时奖励和累计奖励。π(s)表示在状态s下选择的动作策略，π*表示最优策略，π(a|s)表示在状态s下策略π选择动作a的概率。r(s,a)指代在状态s下选择动作a的奖励，r(s',a,s)表示在s状态下选择动作a使状态s变成s'的奖励，v_π(s)为策略价值函数，表示策略在状态s下的累计奖励，q_π(s,a)为行动价值函数，表示在状态s下选择动作a的累计奖励。v*(s)为最优状态值，q*(s,a)为状态s下动作a的最优动作值。

因此，在状态s情况下，选择动作a所得到的期望奖励r(s,a)为：

式中，p(s'|s,a)为在状态s情况下选择动作a从而达到状态s'的概率。

从状态s下选择动作a转化到状态s'时，其期望获得的奖励r(s,a,s')为：

式中，p(s',r|s,a)为状态s时选择动作a，将状态转化为s'的概率。

深度强化学习的本质是从初始状态到最终状态的每一步选择最优策略π*，使得其累计奖励G(t)最大化，并且针对其中的各个状态，根据策略π*选择每一步的最优动作a。其中，G(t)是每一步瞬时奖励与折扣因子乘积的总和：

式中，0≤γ≤1为折扣因子，k为动作编号。

对于从初始状态到最终状态的每一个动作，都可以计算出每个状态下执行不同动作所对应的不同奖励值。因此策略价值函数v_π(s)可以表示为：

行动价值函数q_π(s,a)可表示为：

式中，表示对·的期望。因此，策略价值函数与行动价值函数的关系可表示为：

v_π(s)＝maxq_π(s,a) a∈A (31)

最后，选择最优动作使π(s)达到最大值，即满足：

π(s)＝argmax_avπ(s'∣s,a)＝argmax_aq_π(s,a) (32)

双DQN网络训练过程如图2所示。首先，定义双DQN评价训练网络，根据初始状态下跟随船实时位置与其期望位置及扰动建立状态空间，其次根据跟随船动力结构，为推进器配合设置九个动作。将初始状态输入到定义好的主DQN决策网络，决策网络的输出为当前状态下不同动作各自对应的奖励值。选择奖励值最大的动作为最优动作并将其放置进经验池中，控制器根据根据所选择的最优动作对纵向推力控制力矩与艏向角控制力矩进行改变，使跟随船的位置达到下一状态，并将下一状态与当前状态、所选择的最优动作、与实时期望位置的横向纵向误差、纵向速度和艏向角与式(23)、式(24)所示的虚拟控制律的误差一同放入经验池中。

在完成若干轮上述过程后，从经验池中随机抽取一组数据，分别将这组数据中每一项的上一步状态送入target-DQN网络，将下一状态和位置及纵向速度与期望值误差及艏向角与期望值误差送入奖励函数得到对于每一项的奖励r，将target-DQN的输出与奖励r输入优化器中，同时将当前时刻状态输入到主DQN决策网络后对各个动作输出值送入优化器中，优化器利用主DQN决策网络的输出值及奖励r与target-DQN的输出构建损失函数利用梯度下降法对主DQN决策网络进行优化。

在进行多轮上述步骤后，将主DQN网络的权重值幅值到target-DQN中，然后继续用target-DQN的输出结合奖励函数的奖励值对主DQN决策网络进行优化，直到收敛为止。

网络结构及动作确定：

本方法提出的用于训练和控制的网络架构(主DQN决策网络与target-DQN评价网络)结构如图3所示。该网络共有6层，包括1个输入层、4个完全连接的隐藏层和一个输出层。输入层有10个神经元，对应状态空间的10个状态。输出层有9个神经元，对应9个预定义动作。每个隐藏的完全连接层有256个神经元。其中，状态包括跟随船的实时位置及速度，还包括与期望位置及虚拟控制律误差的10个状态变量，动作包括纵向推力力矩与艏向角控制力矩增加、保持、减小的九个动作。在训练的过程中，利用target-DQN网络及智能体与环境交互产生的状态变化，结合期望值和奖励函数获得奖励值，并将状态分别输入到target-DQN和主DQN决策网络中得到每一个状态下对于每一个动作的奖励值，结合target-DQN的输出与奖励值利用下降梯度法对主DQN决策网络进行参数实时调整，当主DQN网络训练收敛时，将其部署到智能体上代替控制器做运动决策。

双DQN网络训练步骤如图4所示，具体为：

S4.1：初始化主DQN决策网络、target-DQN评价网络，设定前期观察轮数N_obs、训练轮数N_train，总轮数N_total，单轮最大步数T_episode，主DQN决策网络向target-DQN评价网络的更新周期N_renew，参数调节周期N_gap，纵向位置容许偏离e_x，横向位置容许偏离e_y。

S4.2：设定备选动作及动作选择策略，其中动作分别为纵向推力增加，艏向角控制力矩增加、纵向推力增加，艏向角控制力矩保持、纵向推力增加，艏向角控制力矩减小、纵向推力保持，艏向角控制力矩增加、纵向推力保持，艏向角控制力矩保持、纵向推力保持，艏向角控制力矩减小、纵向推力减小，艏向角控制力矩增加、纵向推力减小，艏向角控制力矩保持、纵向推力减小，艏向角控制力矩减小共9个动作；动作选择策略为：

式中，ε为随机选取动作的概率，表示在每个步每一轮的动作选择中有ε的概率是随机选择的，1-ε的概率是选择主DQN决策网络输出奖励值最高的动作；Ns是训练的总轮数即N_total，T表示训练的当前轮数。

S4.3：根据式(22)、(23)中的虚拟控制律及跟随船的实时位置与期望位置的误差设定奖励函数。其中，奖励函数设置为：

式中，k_x、k_y∈(0,1)分别为纵向方向与横向方向的奖励权重系数，e_x＝x-x_d为当前纵向位置与期望纵向位置中心点的距离误差，e_y＝y-y_d为当前横向位置与期望横向位置中心点的距离误差，为了使期望位置中心点附近有较高的奖励值，取σ＝0.5。

式中，k_u、k_ψ∈(0,1)分别为纵向速度奖励权重系数与艏向角奖励权重系数。取综合奖励r为r_p与r_v的总合，即：

r＝r_p+r_v (36)

S4.4：开始训练，若当前轮数小于N_total，进入步骤S4.5，否则结束训练过程。

S4.5：设置智能体及环境初始状态(跟随船初始位置、速度，初始期望位置、初始环境扰动)。

S4.6：判断前步数是否大于T_episode，若是，返回步骤S4.5，否则进入步骤S4.7。

S4.7：按照所设定的动作选择策略选择当前步的动作。

S4.8：将动作代入跟随船运动学与动力学模型进行状态更新。

S4.9：将前一状态、新状态、当前步动作选择存放入经验池中。

S4.10：若当前轮数小于N_gap的倍数，进入步骤S4.6，否则进入步骤S4.11。

S4.11：从经验池中随机抽取若干组数据，利用target-DQN评价网络及奖励函数对经验池中的当前状态下的各个动作进行评价，并利用梯度下降法对主DQN决策网络进行参数修正。

S4.12：判断当前轮数是否达到更新周期N_renew，若达到，将主DQN决策网络中的权重参数赋值给target-DQN评价网络。

S4.13：若跟随船进入期望位置允许偏离范围内，将此后的纵向及横向偏离位置分别大于e_x、e_y作为单轮结束条件。

S4.14：返回步骤S4.4。

所述步骤S5中，对跟随船对领航船协同点位跟踪控制进行仿真验证，验证方法的有效性，具体包括：

定义领航船的预定正弦轨迹如下：

设定跟随船纵向及横向未建模部分及系统内外部扰动τ_du和τ_dr为：

设定无人潜航器动力学方程M_f,C,D_f矩阵为：

设定DQN网络训练相关参数：前期观察期轮数N_obs＝300、训练总轮数N_train＝3000、总轮数N_total＝3300、单轮最大步数T_episode＝210，target-DQN决策网络更新周期N_renew＝50、单词训练抽取样本数nBatch＝40、参数训练调节斗气N_gap，纵向位置最大容许偏离e_x＝1m，横向位置最大容许偏离e_y＝2m。

为了使跟随船在领航船后方进行跟踪，设定跟随船与领航船之间的期望纵向距离d_resi＝5m，期望方位角跟随船初始状态[x(0),y(0),ψ(0)]^T＝[10m,-7m,0.3rad]^T，初始状态[u(0),v(0),r(0)]^T＝[3m/s,0m/s,0rad/s]^T，仿真时间200s。

本方法所使用的DQN神经网络训练过程中每10轮的平均步数如图5所示。从图中可以看出，在前期300轮时，由于尚未对DQN网络进行训练，在动作选择过程中DQN网络并未做出合适的动作选择，导致在前期300轮的观察期中每轮的平均步数仅为40次左右，并且杂乱无序，证明前期依靠随机选择控制效果极差。从第500轮到第1000轮的过程中，由于主DQN决策网络开始受到训练，并且根据式(33)选择每步执行动作的时候，更加偏向于采用训练后的主DQN决策网络输出的结果，因此在这段周期内每轮的平均步数有所提升，达到了140次左右，证明在此过程中主DQN决策网络在被训练调整的过程中，效果相对于训练初期有了明显改善。当训练轮数大于1000次时，由于动作选择更加依赖于DQN决策网络的输出，因此效果较为优秀，能够达到每轮训练的最大步数210次，证明在此过程中DQN神经网络的收敛效果较好。

图6、图7为本方法在领航船在正弦路径下的点位跟踪效果图及实时误差图，从图中可直观看出，跟随船能够在存在环境干扰的情况下在领航船后方的期望位置对领航船进行跟踪，并且跟踪效果良好，这得益于如图8所示控制器输出根据实时环境及位置变化的及时调整。如图7所示，在t＝10s时，跟随船能够跟踪上领航船的期望路径，并且在正弦曲线曲率较低的直线部分拥有优秀的跟踪性能，最大偏离误差不超过0.1m，但是在曲率较大部分，由于与期望位置在纵向与横向的变化较快，跟踪效果下降，最大偏离误差在0.8m内。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (1)

1.一种适用于同向实时点位跟踪的双无人船协同控制方法，其特征在于，包括以下步骤：

S1、建立跟随船运动学与动力学模型；

S2、基于领航船的实时位置，及跟随船相对领航船位置的期望方向角及相对距离，确定点位跟踪控制协同系统模型；

S3、基于点位跟踪协同控制模型，及实时距离误差与方向角误差，引入障碍李雅普诺夫函数对跟随船设计纵向速度虚拟控制律及艏向角虚拟控制律；

S4、引入非线性扰动观测器观测环境扰动，并将其作为外部环境变量，和跟随船与领航船的实时位置误差，设计并训练主Deep Q-Network(DQN)决策网络，该决策网络可根据实时输入的状态选择最为合适的动作完成点位跟踪协同控制；

所述的S1中，考虑双无人船协同点位跟踪控制系统，对于跟随船，其运动学模型可被描述为：

式中，u_f、v_f、r_f分别为跟随船的纵向速度、横向速度以及转艏角速度，(x,y,ψ)为跟随船的纵向位置、横向位置及艏向角；令η_f＝[x_f,y_f,ψ_f]^T，则式(1)可重写为：

考虑系统误差及外部扰动，其动力学模型可设计为：

式中，m_11f、m_22f、m_33f表示跟随船包含附加质量的惯性矩阵系数，τ_duf、τ_dvf、τ_drf为三个方向上的扰动力矩，τ_uf、τ_rf为控制力矩输入，d_11f、d_22f、d_33f为跟随船的附加质量惯性矩阵系数，u_f、v_f、r_f为跟随船的纵向速度、横向速度及艏向角速度；令则式(3)可重写为：

式中，为斜对角矩阵；

所述的S2中，点位跟踪控制协同系统模型领航船与跟随船之间的距离及角度关系为：

式中，(x_l,y_l)为领航船的实时位置，(x_f,y_f)为跟随船的实时位置，d_i为实时距离,为跟随船相对领航船的方向角；

所述的S3中，为建立点位跟踪协同控制模型，定义距离误差与方位角误差为：

式中，d_resi、分别为期望距离与期望方向角；

为了使实际距离与实际方向角能跟踪上期望距离与方向角，定义距离与方向角的动态微分方程为：

对式(7)求导，并结合式(8)、(9)得：

式中，α_uf与α_ψf为跟随船纵向速度与艏向角虚拟控制律；并且设标量

为了使实际距离与实际方向角能够跟踪上期望距离及期望方向角，定义：

其中，e _df、分别是跟踪距离误差的下界与上界，/>跟踪方向角误差的下界与上界；

为了保证控制输出不违反上述约束，引入障碍李亚普诺夫函数：

式中，

为求解α_uf，对式(13)求导得：

设并将式(10)代入，则式(16)可被重写为：

因此标量w_d可被镇定为：

同理，为求解α_ψf，对式(14)求导得：

设并将式(11)代入，则式(19)可被重写为：

因此标量可被镇定为：

最终，式(10)与式(11)中的虚拟控制律α_uf与α_ψf可被设计为：

所述的S4中，所引入非线性扰动观测器为：

式中，为扰动观测器对各方向扰动的估计值，K₀∈R^3×3为正定设计矩阵；

双DQN网络训练的体步骤为：

S4.1：初始化主DQN决策网络、target-DQN评价网络，设定前期观察轮数N_obs、训练轮数N_train，总轮数N_total，单轮最大步数T_episode，主DQN决策网络向target-DQN评价网络的更新周期N_renew，参数调节周期N_gap，纵向位置容许偏离e_x，横向位置容许偏离e_y；

S4.2：设定备选动作及动作选择策略，其中动作分别为纵向推力增加，艏向角控制力矩增加、纵向推力增加，艏向角控制力矩保持、纵向推力增加，艏向角控制力矩减小、纵向推力保持，艏向角控制力矩增加、纵向推力保持，艏向角控制力矩保持、纵向推力保持，艏向角控制力矩减小、纵向推力减小，艏向角控制力矩增加、纵向推力减小，艏向角控制力矩保持、纵向推力减小，艏向角控制力矩减小共9个动作；动作选择策略为：

式中，ε为随机选取动作的概率，表示在每个步每一轮的动作选择中有ε的概率是随机选择的，1-ε的概率是选择主DQN决策网络输出奖励值最高的动作；Ns是训练的总轮数即N_total，T表示训练的当前轮数；

S4.3：根据式(22)、(23)中的虚拟控制律及跟随船的实时位置与期望位置的误差设定奖励函数；其中，奖励函数设置为：

式中，k_x、k_y∈(0,1)分别为纵向方向与横向方向的奖励权重系数，e_x＝x-x_d为当前纵向位置与期望纵向位置中心点的距离误差，e_y＝y-y_d为当前横向位置与期望横向位置中心点的距离误差，为了使期望位置中心点附近有较高的奖励值，取σ＝0.5；

式中，k_u、k_ψ∈(0,1)分别为纵向速度奖励权重系数与艏向角奖励权重系数；

取综合奖励r为r_p与r_v的总合，即：

r＝r_p+r_v (28)

S4.4：开始训练，若当前轮数小于N_total，进入步骤S4.5，否则结束训练过程；

S4.5：设置智能体及环境初始状态(跟随船初始位置、速度，初始期望位置、初始环境扰动)；

S4.6：判断前步数是否大于T_episode，若是，返回步骤S4.5，否则进入步骤S4.7；

S4.7：按照所设定的动作选择策略选择当前步的动作；

S4.8：将动作代入跟随船运动学与动力学模型进行状态更新；

S4.9：将前一状态、新状态、当前步动作选择存放入经验池中；

S4.10：若当前轮数小于N_gap的倍数，进入步骤S4.6，否则进入步骤S4.11；

S4.11：从经验池中随机抽取若干组数据，利用target-DQN评价网络及奖励函数对经验池中的当前状态下的各个动作进行评价，并利用梯度下降法对主DQN决策网络进行参数修正；

S4.12：判断当前轮数是否达到更新周期N_renew，若达到，将主DQN决策网络中的权重参数赋值给target-DQN评价网络；

S4.13：若跟随船进入期望位置允许偏离范围内，将此后的纵向及横向偏离位置分别大于e_x、e_y作为单轮结束条件；

S4.14：返回步骤S4.4。