[发明专利]一种针对时变动力学的自主水下航行器轨迹跟踪控制方法

权利要求书

1.一种针对时变动力学的自主水下航行器轨迹跟踪控制方法，其特征在于，该方法包括以下步骤：

1)建立时变动力学环境下水下自主航行器AUV轨迹跟踪控制问题；具体步骤如下：

1-1)确定AUV系统输入向量；

令AUV系统第k时间步的输入向量为τ_k＝[τ_uk,τ_rk]^T，其中，下标k表示第k时间步，τ_uk、τ_rk分别为AUV系统第k时间步的螺旋桨推力和力矩；τ_uk、τ_rk的取值范围分别为[-u_M,u_M]和[-r_M,r_M]，u_M、r_M分别为最大的螺旋桨推力和最大力矩；

1-2)确定AUV系统输出向量和状态向量；

令AUV系统第k时间步的输出向量为η_k＝[x_k,y_k,ψ_k]^T，其中x_k、y_k分别为第k时间步AUV系统在惯性坐标系I-XYZ下沿X、Y轴的坐标，ψ_k为第k时间步AUV系统前进方向与X轴的夹角；该输出向量的一阶导数为ν_k＝[u_k,v_k,r_k]^T，该一阶导数为AUV系统第k时间步的状态向量；其中u_k,v_k分别代表平面上第k时间步AUV系统前进方向的速度分量与垂直前进方向的速度分量，r_k代表ψ_k方向的角速度；

1-3)确定时变动力学AUV系统方程，表达式如下：

η_k+1＝η_k+T_sJ(ν_k),

ν_k+1＝ν_k+T_sM(ω)^-1F(ν_k,ω),

F(ν_k,ω)＝G(ν_k,ω)τ_k-C(ν_k,ω)ν_k-D(ν_k,ω)ν_k,

该方程包括输入向量τ_k、输出向量η_k、状态向量ν_k、时变系数向量ω；其中时变系数向量ω包括AUV系统质量系数m，鳍升力系数Y_uuδ，鳍力矩系数N_uuδ；

M,C,D,G,J分别代表考虑时变向量ω的带附加质量的系统惯性矩阵、水动力学阻尼矩阵、科里奥利力矩阵、输入矩阵和转换矩阵；

1-4)确定轨迹跟踪控制误差；

AUV系统第k时间步的参考轨迹点记为其中分别代表第k时间步参考轨迹点的X轴坐标和Y轴坐标，则AUV系统的参考轨迹为τ_d＝[d₁,d₂…,d_K]，K代表最大时间步；

则第k时间步的AUV系统轨迹跟踪控制误差为：

1-5)建立AUV系统轨迹跟踪控制目标；

建立时变动力学环境下的AUV系统轨迹跟踪控制的目标为在当前时变动力学AUV系统下找到一个最优系统输入序列τ^*使得k＝0的初始时刻的目标函数P₀(τ)最小，计算表达式如下：

其中，

其中，γ是折扣因子，H为权重矩阵；

2)建立AUV轨迹跟踪问题的马尔科夫决策过程模型；具体步骤如下：

2-1)批量马尔科夫决策过程划分；

随机采样一组时变系数向量ω_i,i＝1,2,…,n，根据该时变系数向量划分对应的n组马尔科夫决策过程，将每组马尔科夫决策过程对应的任务记为T_i,i＝1,2,…,n，n代表采样的时变向量组数即任务数；

2-2)定义状态向量；

针对每组马尔科夫决策过程，令AUV在第k时间步与第k+1时间步的参考轨迹点分别为与则第k时间步的AUV对应参考轨迹点沿X、Y轴的坐标误差分别为第k时间步的AUV距离第k+1时间步的参考轨迹点的前瞻距离前视偏转角度为

建立每组马尔科夫决策过程中第k时间步的状态向量如下：

s_k＝[d_fk,cos(ψ_dk),sin(ψ_dk),δ_xk,δ_yk,cos(ψ_k),sin(ψ_k),u_k,v_k,r_k]

2-3)定义动作向量；

针对每组马尔科夫决策过程，定义第k时间步的动作向量为该时间步的AUV系统输入向量，即a_k＝τ_k；

2-4)定义回报函数；

针对每组马尔科夫决策过程，确定第k时间步AUV系统与下一时间步参考轨迹点的坐标误差：

则第k时间步的回报函数为：

其中，α₁,α₂分别为权重系数；

2-5)将步骤1-5)建立的AUV系统轨迹跟踪控制目标转换为强化学习框架下的AUV轨迹跟踪控制目标；

在任务T下，定义策略π为在任一状态下AUV选择各个可能动作的概率，则定义第k时间步的状态动作值函数如下：

其中，表示对奖励函数、状态和动作的期望值；

则在时变动力学场景下，AUV系统轨迹跟踪控制目标是得到最优目标策略使得在任务分布ρ(T)下的所有任务期望状态动作值最大，计算表达式如下：

其中，p(s₀)为初始状态s₀的分布；a₀为初始动作向量；

3)元强化学习框架下的AUV轨迹跟踪控制方法；具体步骤如下：

3-1)建立元强化学习的优化目标函数，表达式如下：

其中

其中τ～P_T(τ|θ)代表轨迹τ由策略π_θ在任务T的分布P_T(τ|θ)采样得到；R(τ,θ)为在参数θ下，采样轨迹为τ的累计回报的神经网络拟合函数；令R(τ,θ)为优势函数其中为在任务T下执行策略π，状态动作对(s₀,a₀)的状态动作值函数，为s₀的状态值函数；θ′代表在任务T下进行一步内部更新得到的策略参数；

3-2)建立策略离散程度度量指标：

其中，N_u代表每个任务内部最大更新步数，代表第k步外部更新时的初始策略，代表策略在任务T_i做j步内部更新得到的策略，D_KL代表KL散度度量；

则更新后的元强化学习的优化目标函数为：

3-3)构建基于注意力机制的策略网络；

该网络的输入信息为包括当前第k时间步状态向量s_k及之前M个时间步的状态向量s_k-M,s_k-M+1…,s_k-1，输出信息为当前第k时间步的动作向量a_k；该网络包括依次连接的输入处理层、中间隐层和输出层；其中输入处理层包括注意力机制层与状态输入层，所述状态输入层将输入的s_k转化为输入隐层信息I_k，注意力机制层将前M个时间步状态向量提取为第k时间步的历史信息H_k；中间隐层的输入为[s_k,I_k,H_k]，输出为动作向量a_k，即为输出层输出向量；

4)求解AUV轨迹跟踪控制的目标策略，具体步骤如下：

4-1)参数设置；

分别设置外部最大迭代次数L、内部最大更新步数N_u、AUV任务分布ρ(T)、任务数n、外部更新学习速率α，内部更新速率β、折扣因子γ、批量数据大小B、AUV运行跟踪最大时间步K、奖励函数系数α₁,α₂、权重矩阵H；

4-2)初始化策略网络参数θ，得到对应的初始策略网络为π_θ；

4-3)外部迭代开始，对策略网络进行训练，初始化迭代次数episode＝1；

4-4)生成参考轨迹τ_d；

4-5)从任务分布ρ(T)随机采样批量任务T_i,i＝1,2,…,n，随机生成一组时变系数向量ω_i,i＝1,2,…,n；

4-6)任务迭代开始，对每个任务T_i，初始化迭代任务序号i＝1；

4-7)选择任务T_i对应的时变系数向量ω_i，设定参数θ_i＝θ，初始化内部更新步数j＝0；

4-8)初始化马尔科夫决策过程初始状态s₀；

4-9)利用网络使AUV与外界环境交互，当前策略网络输入为历史状态向量s_k-M,s_k-M+1…,s_k-1与当前时间步状态向量s_k；交互获取任务T_i采样轨迹数据τ_i＝{s₀,a₀,r₀,…,s_T+1}，计算第j步内部更新目标函数为R(τ_i,θ_i)与策略离散程度度量指标对策略网络参数做一步更新：

4-10)令j＝j+1并对j进行判定：如jN_u，则重新返回步骤4-9)，继续更新策略网络参数；否则，将更新后的策略网络记为进入步骤4-11)；

4-11)利用更新后的策略网络继续交互获取任务T_i采样轨迹数据，直到AUV系统跟踪轨迹长度为K，获得最终的轨迹数据τ′_i；

4-12)令i＝i+1并对i进行判定：如in，则重新返回步骤4-7)，AUV开始对任务T_i进行内部更新；否则，进入步骤4-13)；

4-13)利用各轨迹数据τ′_i,i＝1,2,…,n计算元强化学习的目标函数：

基于目标函数进行策略参数的外部更新：

4-14)令episode＝episode+1并对episode进行判定：如episodeL，则重新返回步骤4-4；否则，进入步骤4-15)；

4-15)迭代结束，终止策略网络的训练过程，得到训练完毕的策略网络参数记为确定最终的策略网络将AUV系统当前时间步的状态向量与前M步的状态向量输入到策略网络中，该网络输出相应的控制向量控制AUV系统完成时变动力学环境下的轨迹跟踪任务。