基于双BP网络增强学习框架的水下机器人轨迹跟踪方法

摘要

一种基于双BP网络增强学习框架的水下机器人轨迹跟踪方法，它属于水下机器人轨迹跟踪技术领域。本发明解决了现有技术在进行控制器参数的在线优化时，需要依赖大量的专家先验知识建立模糊规则，导致控制器参数的在线优化耗时耗力的问题。本发明利用强化学习方法可以通过与环境的不断交互，在得到环境给出的强化值后便能通过循环迭代寻找到最优策略的特点，将强化学习方法与双BP网络结合起来，通过在线调节水下机器人的速度和艏向控制系统控制律的相关参数，使得所设计的速度和艏向控制系统能在不同的环境中选择与该环境相对应的最优控制参数，克服了现有技术中控制器参数在线优化的耗时耗力的问题。本发明可以应用于水下机器人的轨迹跟踪。

权利要求

1.一种基于双BP网络增强学习框架的水下机器人轨迹跟踪方法，其特征在于，该方法包括以下步骤：步骤一、根据水下机器人的速度和艏向控制系统的控制律确定待设计的控制参数k1、k2、k3和k4；过程为：水下机器人的速度和艏向控制系统的控制律如公式(1)至(3)所示：其中，τu为推进器纵向推力，m为水下机器人质量， Xu|u|、Nr|r|、Nr均为无量纲水动力参数，v为水下机器人横向速度，r为水下机器人偏航角速度，|r|为水下机器人偏航角速度的绝对值，u为水下机器人纵向速度，|u|为水下机器人纵向速度的绝对值，ud为水下机器人纵向期望速度， 代表水下机器人纵向期望加速度，τr为转艏力矩，Iz为水下机器人绕运动坐标系z轴的转动惯量， 为水下机器人的艏向角，β为侧漂角， 为侧漂角速度，为参考型水下机器人在惯性参考系下的艏向角， 为参考型水下机器人艏向角速度，δΤ为导航角，为导航角的角速度，ur为参考型水下机器人纵向速度，vt＝(u,v) 为水下机器人合速度， 为艏向角误差，s为沿路径的广义弧长，为s的一阶导数，xe为待控制的水下机器人与参考型水下机器人的纵向位置误差， 为期望角角速度，k1、k2、k3和k4均为待设计的控制参数，且k1、k2、k3和k4均为正常数；

导航角δ的表达式：

其中，ye为待控制的水下机器人与参考型水下机器人的横向位置误差，k0为导航角控制增益，Δ为超前长度；

所述运动坐标系为：以水下机器人的重心为原点O，以指向水下机器人的艏向方向为x轴正向，以指向水下机器人的右舷方向为为y轴正向，以指向水下机器人的底部方向为z轴正向，且x轴、y轴、z轴符合右手螺旋定则的坐标系；

所述惯性参考系为：以海平面上的任一固定点为原点E，以地理北极方向为ξ轴正向，以地理正东方向为η轴正向，以指向地心方向为ζ轴正向，且ξ轴、η轴、ζ轴符合右手螺旋定则的坐标系；

步骤二、构建双BP神经网络结构，所述双BP神经网络的结构包括当前BP神经网络和目标BP神经网络，且目标BP神经网络的结构与当前BP神经网络的结构相同；

所述当前BP神经网络的输入为当前状态st，目标BP神经网络的输入为当前状态st执行最优动作at后所得到的下一时刻的状态st+1；当前BP神经网络和目标BP神经网络均根据输入的状态确定输出的动作；

步骤三、根据当前BP神经网络输出的实际Q值Qm(st,at)，目标BP神经网络输出的目标Q值 以及当前BP神经网络在当前状态st下选择最优动作at并执行后得到的下一时刻状态st+1和奖赏值rt+1(st+1,a)，来计算TD误差，再利用计算出的TD误差对当前BP神经网络的参数进行更新；

所述计算TD误差的具体过程为：

TD误差的计算公式如公式(7)所示：

其中，rt+1(st+1,a)为t+1时刻的奖励值，Qm(st,at)为当前BP神经网络实际输出Q值，为目标BP神经网络的目标输出Q值，γ为折扣率，at为t时刻状态st下选择的最优动作；

计算得到TD误差后，对当前BP神经网络的参数w进行更新；

每个时刻对应的状态动作样本对均存入经验样本池，在当前BP神经网络训练的空闲时间，从经验样本池中随机抽取状态动作样本对，利用抽取的状态动作样本对对当前BP神经网络继续进行训练；

且每经过N个时刻后，目标BP神经网络对当前BP神经网络的参数进行复制，实现对当前BP神经网络和目标BP神经网络参数的更新；

步骤四、当每个时刻的状态依次输入当前BP神经网络后，当前BP神经网络分别根据每个时刻的状态输出对应时刻的动作a，再利用ε贪婪策略选择出最优动作；

将每个时刻对应的最优动作代入待设计的控制参数的表达式，获得待设计的控制参数的参数值，将获得的参数值输入水下机器人的速度和艏向控制系统的控制律，实现对水下机器人轨迹跟踪的控制。

2.根据权利要求1所述的一种基于双BP网络增强学习框架的水下机器人轨迹跟踪方法，其特征在于，所述步骤二中，当前BP神经网络的输入为当前状态st，当前状态st表示为其中：xe、ye以及 分别表示当前状态下，待控制的水下机器人与参考型水下机器人的纵向位置误差、横向位置误差以及艏向角误差；

所述当前BP神经网络和目标BP神经网络均根据输入的状态确定输出动作，所述输出动作的表达式为a＝{k1′,k2′,k3′,k4′}，其中，a为输出动作；

其中，k1′、k2′、k3′和k4′分别为对动作值k1′0、k2′0、k3′0和k4′0的划分结果，选取k1′0∈[‑

1,1]，k2′0∈[‑1,1]，将k1′0和k2′0每隔0.5平均划分成5个动作值，选取k3′0∈[‑0.4,0.4]，k4′0∈[‑0.4,0.4]，将k3′0和k4′0每隔0.2划分成5个动作值；

则待设计的控制参数k1、k2、k3和k4的表达式为：

ki＝ki0+ki′,′i＝1,2,3,4                           (5)其中，k10、k20、k30和k40分别为四个待控制参数的初始值，ki″为从动作ki′中选择出的最优动作。

3.根据权利要求2所述的一种基于双BP网络增强学习框架的水下机器人轨迹跟踪方法，其特征在于，所述利用ε贪婪策略选择出最优动作，随机概率ε随控制回合数的增加不断衰减，随机概率ε的表达式为：(μ‑step)ξ

ε＝ε0·e  (6)

式中，ε0为初始值，μ为衰减因子，step为控制回合数，ξ为控制因子。

4.根据权利要求3所述的一种基于双BP网络增强学习框架的水下机器人轨迹跟踪方法，其特征在于，所述奖励值rt+1(st+1,a)的计算方法为：Τ

rt+1(st+1,a)＝exp[‑(σΛσ)]                        (8)其中，σ表示t+1时刻的误差向量，Λ为四阶对角矩阵，上脚标T代表转置。

5.根据权利要求4所述的一种基于双BP网络增强学习框架的水下机器人轨迹跟踪方法，其特征在于，所述步骤三中，每经过N个时刻后，目标BP神经网络对当前BP神经网络的参数进行复制，N的取值为10。

说明书

基于双BP网络增强学习框架的水下机器人轨迹跟踪方法

技术领域

[0001] 本发明属于水下机器人轨迹跟踪技术领域，具体涉及一种基于双BP网络增强学习框架的水下机器人轨迹跟踪方法。

背景技术

[0002] 水下机器人在海洋环境检测、海底测绘、海底管道检测等任务中扮演着重要角色，其轨迹跟踪控制性能的好坏，在很大程度上影响了水下机器人在海底作业时的安全性、稳定性和经济性。通常，水下机器人在进行特定作业时，要求水下机器人负载可变，如海底管道铺设，并且海风、海浪、海流等会随时间和位置的不同而发生改变，使得水下机器人作业环境具有高度非线性和不确定性，这些因素导致设计能够在不同情况下都具有良好控制效果的控制器较为困难。因此，水下机器人的控制系统应该具备对控制器参数进行自动优化的能力。目前，对于控制器参数的自动优化问题，学者们进行了大量研究，通常结合智能控制技术，如神经网络、模糊以及进化算法等技术对相关控制器的控制参数进行调整。然而神经网络的学习效果通常与环境影响息息相关，多变的外界干扰易引起自学习不稳定，会对机器人控制系统可靠性造成不利影响；进化算法虽然有很好的寻优能力，但仅仅是离线学习，在仿真环境下的寻优结果只能为实际环境提供一定的参考，并不能应用到实际环境中去；模糊技术虽然能够进行控制器参数的在线优化，但需要大量的专家先验知识以建立模糊规则，完善的模糊规则的建立耗时耗力，因此建立完善的模糊规则具有一定困难。

发明内容

[0003] 本发明的目的是为解决现有技术在进行控制器参数的在线优化时，需要依赖大量的专家先验知识建立模糊规则，导致控制器参数的在线优化耗时耗力的问题，而提出了一种基于双BP网络增强学习框架的水下机器人轨迹跟踪方法。

[0004] 本发明为解决上述技术问题采取的技术方案是：一种基于双BP网络增强学习框架的水下机器人轨迹跟踪方法，该方法包括以下步骤：

[0005] 步骤一、根据水下机器人的速度和艏向控制系统的控制律确定待设计的控制参数k1、k2、k3和k4；

[0006] 步骤二、构建双BP神经网络结构，所述双BP神经网络的结构包括当前BP神经网络和目标BP神经网络，且目标BP神经网络的结构与当前BP神经网络的结构相同；

[0007] 所述当前BP神经网络的输入为当前状态st，目标BP神经网络的输入为当前状态st执行最优动作at后所得到的下一时刻的状态st+1；当前BP神经网络和目标BP神经网络均根据输入的状态确定输出的动作；

[0008] 步骤三、根据当前BP神经网络输出的实际Q值Qm(st,at)，目标BP神经网络输出的目标Q值 以及当前BP神经网络在当前状态st下选择最优动作at并执行后得到的下一时刻状态st+1和奖赏值rt+1(st+1,a)，来计算TD误差，再利用计算出的TD误差对当前BP神经网络的参数进行更新；

[0009] 每个时刻对应的状态动作样本对均存入经验样本池，在当前BP神经网络训练的空闲时间，从经验样本池中随机抽取状态动作样本对，利用抽取的状态动作样本对对当前BP神经网络继续进行训练；

[0010] 且每经过N个时刻后，目标BP神经网络对当前BP神经网络的参数进行复制，实现对当前BP神经网络和目标BP神经网络参数的更新；

[0011] 步骤四、当每个时刻的状态依次输入当前BP神经网络后，当前BP神经网络分别根据每个时刻的状态输出对应时刻的动作a，再利用ε贪婪策略选择出最优动作；

[0012] 将每个时刻对应的最优动作代入待设计的控制参数的表达式，获得待设计的控制参数的参数值，将获得的参数值输入水下机器人的速度和艏向控制系统的控制律，实现对水下机器人轨迹跟踪的控制。

[0013] 本发明的有益效果是：本发明提出了一种基于双BP网络增强学习框架的水下机器人轨迹跟踪方法，本发明利用强化学习方法可以通过与环境的不断交互，在得到环境给出的强化值后便能通过循环迭代寻找到最优策略的特点，将强化学习方法与双BP网络结合起来，通过在线调节水下机器人的速度和艏向控制系统控制律的相关参数，使得所设计的速度和艏向控制系统能在不同的环境中选择与该环境相对应的最优控制参数。仅仅通过与环境的交互就能获得最优控制参数，克服了现有技术中控制器参数在线优化的耗时耗力的问题。

附图说明

[0014] 图1是本发明一种基于双BP网络增强学习框架的水下机器人轨迹跟踪方法的框图；

[0015] 图中，(xd,yd)为期望目标点位置坐标， 为期望艏向角，et为误差向量，(x,y)为水下机器人当前位置坐标，为水下机器人当前艏向角，u为水下机器人当前速度；

[0016] 图2是双BP神经网络结构及输入输出示意图；

[0017] 其中：S为BP神经网络的输入，Q(s,a1)、Q(s,a2)、…、Q(s,ak)为BP神经网络输出的Q值；

[0018] 图3是水下机器人S‑F坐标系的示意图；

[0019] 图中， 为待控制水下机器人在惯性参考系下的艏向角， 为加上侧漂角后的水下机器人艏向角；

[0020] 图4是折线段轨迹跟踪效果对比图；

[0021] 图5是x方向轨迹跟踪误差对比图；

[0022] 图6是y方向轨迹跟踪误差对比图；

[0023] 图7是折线段轨迹跟踪纵向推力图；

[0024] 图8是折线段轨迹跟踪偏航力矩图；

[0025] 图9是艏向控制参数k1和k2的变化曲线图。

具体实施方式

[0026] 具体实施方式一：本实施方式所述的一种基于双BP网络增强学习框架的水下机器人轨迹跟踪方法，该方法包括以下步骤：

[0027] 步骤一、根据水下机器人的速度和艏向控制系统的控制律确定待设计的控制参数k1、k2、k3和k4；

[0028] 步骤二、构建双BP神经网络结构，所述双BP神经网络的结构包括当前BP神经网络和目标BP神经网络，且目标BP神经网络的结构与当前BP神经网络的结构相同；

[0029] 所述当前BP神经网络的输入为当前状态st，目标BP神经网络的输入为当前状态st执行最优动作at后所得到的下一时刻的状态st+1；当前BP神经网络和目标BP神经网络均根据输入的状态确定输出的动作；

[0030] 步骤三、根据当前BP神经网络输出的实际Q值Qm(st,at)，目标BP神经网络输出的目标Q值 以及当前BP神经网络在当前状态st下选择最优动作at并执行后得到的下一时刻状态st+1和奖赏值rt+1(st+1,a)，来计算TD误差，再利用计算出的TD误差对当前BP神经网络的参数进行更新；

[0031] 每个时刻对应的状态动作样本对均存入经验样本池，在当前BP神经网络训练的空闲时间，从经验样本池中随机抽取状态动作样本对，利用抽取的状态动作样本对对当前BP神经网络继续进行训练；

[0032] 且每经过N个时刻后，目标BP神经网络对当前BP神经网络的参数进行复制，实现对当前BP神经网络和目标BP神经网络参数的更新；

[0033] 步骤四、当每个时刻的状态依次输入当前BP神经网络后，当前BP神经网络分别根据每个时刻的状态输出对应时刻的动作a，再利用ε贪婪策略选择出最优动作；

[0034] 将每个时刻对应的最优动作代入待设计的控制参数的表达式，获得待设计的控制参数的参数值，将获得的参数值输入水下机器人的速度和艏向控制系统的控制律，实现对水下机器人轨迹跟踪的控制。

[0035] 本实施方式基于马尔可夫决策过程(Markov Decision Process，MDP)：马尔可夫决策过程是强化学习建模的基础，简称MDP。一个MDP由{S,A,P,r,γ}组成，其中S为状态集合；A为动作集合；P(s,a,s′)∈[0,1]为状态转移概率，表示在状态s下选择动作a后使环境状态转移到状态s′的概率； 为长期累计回报值；γ为折扣因子，用来衡量未来可能状态对此刻情况的影响程度。

[0036] 强化学习方法(Reinforcement Learning，RL)：强化学习是一种通过不断迭代学习寻找到最大回报函数以实现最优决策的过程。当给定智能体某一任务时，智能体会根据初始策略选择动作a与环境产生交互，并达到下一状态，与此同时，环境会反馈给智能体该动作a所产生的即时回报值，智能体通过该回报值调整初始策略，不断循环往复，智能体最终会完成相应的任务，并得到完成该任务的最优策略。

[0037] 为了提升水下机器人轨迹跟踪控制的自主性与智能性，保证其控制器的参数能够实时在线进行调整，从而提高在不同作业环境下水下机器人的运动控制性能，同时为了避免基于单一BP神经网络而导致的目标Q值与当前实际输出Q值具有相关性的缺陷，本发明专利设计了基于双BP神经网络Q学习框架的水下机器人轨迹跟踪控制器，将当前位置偏差和下一时刻的位置偏差分别作为当前BP神经网络和目标神经网络的输入，输出分别为不同动作所对应的实际Q值和经过奖励函数作用的目标Q值，对轨迹跟踪控制器的各个参数进行在线实时整定。

[0038] 双BP神经网络Q学习框架的输入为各项偏差，输出为轨迹跟踪控制律所需要调整的相关参数。水下机器人基于双BP神经网络Q学习轨迹跟踪控制器的结构设计如图1所示。将BP神经网络引入强化学习控制器中意味着，在某一时刻t，控制策略不再是由查询Q值表得到，而是通过BP神经网络中的w进行表示，当输入平缓变化的状态值时，通过BP神经网络的作用，所输出的最大Q值是是连续的，但由于所执行的动作与Q值是一一对应的关系，所以动作值是离散的。

[0039] 当前BP神经网络和目标BP神经网络的结构均如图2所示。

[0040] 具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤一的具体过程为：

[0041] 水下机器人的速度和艏向控制系统的控制律如公式(1)至(3)所示：

[0042]

[0043]

[0044]

[0045] 其中，τu为推进器纵向推力，m为水下机器人质量， Xuu、Nrr、Nr均为无量纲水动力参数，v为水下机器人横向速度，r为水下机器人偏航角速度，|r|为水下机器人偏航角速度的绝对值，u为水下机器人纵向速度，|u|为水下机器人纵向速度的绝对值，ud为水下机器人纵向期望速度， 代表水下机器人纵向期望加速度，τr为转艏力矩，Iz为水下机器人绕运动坐标系z轴的转动惯量， 为水下机器人的艏向角，β为侧漂角， 为侧漂角速度， 为参考型水下机器人在惯性参考系下的艏向角， 为参考型水下机器人艏向角速
Τ
度，δ为导航角，为导航角的角速度，ur为参考型水下机器人纵向速度，vt＝(u,v) 为水下机器人合速度， 为艏向角误差，s为沿路径的广义弧长，为s的一阶导数，xe为待控制的水下机器人与参考型水下机器人的纵向位置误差， 为期望角角速度，k1、k2、k3和k4均为待设计的控制参数，且k1、k2、k3和k4均为正常数；

[0046] ur为额外增加的一个自由度的控制量，可以视为参考型水下机器人的运动学控制律；

[0047] 导航角δ的表达式：

[0048]

[0049] 其中，ye为待控制的水下机器人与参考型水下机器人的横向位置误差，k0为导航角控制增益， ，Δ为超前长度；k0通常可以设置为某一确定的常值；

[0050] 所述运动坐标系为：以水下机器人的重心为原点O，以指向水下机器人的艏向方向为x轴正向，以指向水下机器人的右舷方向为为y轴正向，以指向水下机器人的底部方向为z轴正向，且x轴、y轴、z轴符合右手螺旋定则的坐标系；

[0051] 所述惯性参考系为：以海平面上的任一固定点为原点E，以地理北极方向为ξ轴正向，以地理正东方向为η轴正向，以指向地心方向为ζ轴正向，且ξ轴、η轴、ζ轴符合右手螺旋定则的坐标系。

[0052] 惯性参考系用来描述水下机器人的位置和姿态，固联于大地上。运动坐标系用来描述水下机器人的运动信息，固联于水下机器人上。

[0053] 本实施方式中，惯性参考系和运动坐标系间的转换关系：惯性参考系通过三次旋转即可与运动坐标系重合，两个坐标系的转换关系如下所示：

[0054]

[0055]

[0056] 式中，ψ、θ、分别表示水下机器人的艏向角、纵倾角和横倾角；R和T分别表示线速度转换矩阵和角速度转换矩阵，且均为正交阵。

[0057] 水下机器人六自由度运动方程：根据刚体动力学理论，水下机器人六自由度运动的一般方程如下所示：

[0058]

[0059] 式中，(xG,yG,zG)表示水下机器人重心坐标，Ix,Iy,Iz分别表示水下机器人绕x,y,z轴的转动惯量，u,v,w,p,q,r分别表示六自由度(角)速度， 分别表示六自由度(角)加速度，X,Y,Z,K,M,N分别表示六自由度力(矩)。

[0060] 具体实施方式三：本实施方式与具体实施方式二不同的是：所述步骤二中，当前BP神经网络的输入为当前状态st，当前状态st表示为 其中：xe、ye以及 分别表示当前状态下，待控制的水下机器人与参考型水下机器人的纵向位置误差、横向位置误差以及艏向角误差；

[0061] 所述当前BP神经网络和目标BP神经网络均根据输入的状态确定输出动作，所述输出动作的表达式为a＝{k′1,k′2,k′3,k′4}，其中，a为输出动作；

[0062] 其中，k′1、k′2、k′3和k′4分别为对动作值k′10、k′20、k′30和k′40的划分结果，选取k′10∈[‑1,1]，k′20∈[‑1,1]，将k′10和k′20每隔0.5平均划分成5个动作值，选取k′30∈[‑0.4,0.4]，k′40∈[‑0.4,0.4]，将k′30和k′40每隔0.2划分成5个动作值；

[0063] 则待设计的控制参数k1、k2、k3和k4的表达式为：

[0064] ki＝ki0+k′i,i＝1,2,3,4                           (5)

[0065] 其中，k10、k20、k30和k40分别为四个待控制参数的初始值，k″i为从动作k′i中选择出的最优动作。

[0066] 具体实施方式四：本实施方式与具体实施方式三不同的是：所述利用ε贪婪策略选择出最优动作，随机概率ε随控制回合数的增加不断衰减，随机概率ε的表达式为：

[0067] ε＝ε0·e(μ‑step)/ξ    (6)

[0068] 式中，ε0为初始值，μ为衰减因子，step为控制回合数，ξ为控制因子，用于控制ε处于(0,1)区间内。

[0069] 基于双BP神经网络的强化学习框架动作选择方式采用ε贪婪策略，传统的强化学习中使用的贪婪策略ε的值通常为一固定的常值，且ε∈(0,1)。本发明将模拟退火思想融入贪婪算法中，采用一种随机概率ε随步数增加不断衰减的ε贪婪策略，当ε的值越来越趋近于0，表示训练到最后阶段，强化学习系统更加偏向于利用所学到的经验。

[0070] 具体实施方式五：本实施方式与具体实施方式四不同的是：所述计算TD误差的具体过程为：

[0071] TD误差的计算公式如公式(7)所示：

[0072]

[0073] 其中，rt+1(st+1,a)为t+1时刻的奖励值，Qm(st,at)为当前BP神经网络实际输出Q值，为目标BP神经网络的目标输出Q值，γ为折扣率，at为t时刻状态st下选择的最优动作；

[0074] 计算得到TD误差后，对当前BP神经网络的参数w进行更新。

[0075] 得到TD误差后，根据式(27)至(29)通过梯度下降和微分链式规则对BP神经网络的参数w进行更新。

[0076] 具体实施方式六：本实施方式与具体实施方式五不同的是：所述奖励值rt+1(st+1,a)的计算方法为：

[0077] rt+1(st+1,a)＝exp[‑(σΤΛσ)]                        (8)

[0078] 其中，σ表示t+1时刻的误差向量，Λ为四阶对角矩阵，上脚标T代表转置。

[0079] 奖惩值函数的设置是为了让基于双BP神经网络Q学习框架能有一个较好的学习性能。轨迹跟踪的目标是为了使AUV跟踪上目标轨迹，奖励值函数由受控变量与其期望值之间的平方误差决定，由于强化学习的目标是寻找最大Q值所对应的一系列动作值，因此，利用指数函数对误差向量进行变换； 表示下一时刻的误差向量，根据强化学习的学习机制，其奖惩函数描述的是选择某一动作at达到状态st+1后，对状态st+1所作出的评价，由于xe、ye、 、ue均为当前时刻的误差变化，不能直接使用，因此将其变为σ向量所示形式；Λ为四阶对角矩阵，对角线上的各元素代表的是σ的各个分量对奖惩函数的影响因子。

[0080] 具体实施方式七：本实施方式与具体实施方式六不同的是：所述步骤三中，每经过N个时刻后，目标BP神经网络对当前BP神经网络的参数进行复制，N的取值为10。

[0081] 为了加快双BP神经网络Q学习参数自适应框架的收敛速度，本发明考虑加入经验样本池，将每一个控制回合中收集到的所有状态动作样本对(st,at,st+1,rt+1)存入经验样本池，进行经验回放。在每次训练的空闲时间，从经验样本池中抽取固定数目(本发明中设置为2000个)的样本对BP神经网络的参数进行更新，从而在更大程度上缩短学习时间。样本池的容量固定，随着训练采样的样本数量的增加，当样本池的容量达到上限时，将最先进入样本池的样本剔除，并保留新样本，始终维持样本池容量固定。同时保证双BP神经网络能够在全状态空间中进行训练，可以避免神经网络对过去经验的遗忘，使BP神经网络能够建立起全局控制思路。

[0082] 理论基础

[0083] 1、运动学和动力学模型简化

[0084] 水下机器人的数学模型比较复杂，其空间运动为六自由度方向的运动。本发明主要研究水下机器人在水平面运动时，基于双BP神经网络Q学习框架对给定轨迹跟踪控制器的优化作用。因此作出以下假设：

[0085] 水下机器人六自由度运动可分为分解为水平面运动和垂直面运动。在水平面运动时，水下机器人主要对航向和纵向航速进行控制，此时重心在水平面内运动；在垂直面运动时，水下机器人主要对纵倾角和深度进行控制，此时重心在垂直面内运动。另外由于水下机器人具有自稳特性，因此不需对横滚运动进行控制。

[0086] 水下机器人水平面运动方程和动力学方程如下所示：

[0087]

[0088]

[0089] 2、Q学习算法

[0090] Q学习算法是由Watkins提出的一种与模型无关的值迭代型强化学习算法。Q学习算法在学习过程中，采用状态‑动作值函数作为评估函数，而不是状态值函数，这保证了智能体对每一个动作都进行了考量，使得Q学习算法更容易收敛。在Q学习算法中，智能体通过表格形式对每一步的Q值函数进行记录，并不断迭代，最终形成最优Q值表，该最优Q值便反映了智能体最终学到的最优策略。智能体采取的动作通过查表得方式获得。具体Q值更新如下式所示：

[0091]

[0092] 式中，α为学习率；rt+1为即时回报；Qm‑1(st,at)为(st,at)所对应的Q值大小；Qm‑1(st+1,a′)为状态st+1所对应的Q值大小；γ为折扣率。

[0093] Q学习算法具体更新流程如下表1所示：

[0094] 表1单步Q学习算法流程

[0095]

[0096] 3、ε贪婪策略

[0097] ε贪婪策略是解决智能体在学习过程中的探索与利用问题的一种方法，它表示在概率为ε时智能体采取所有动作中的某一随机动作进行探索，在概率为1‑ε时，智能体采取最大Q值所对应的动作进行利用，以此保证智能体在每一回合训练完毕后，累积奖赏值最大。具体公式如下所示：

[0098]

[0099] 式中，π为智能体选取的动作，即策略。

[0100] 4、BP神经网络

[0101] BP神经网络是一种包含输入层、隐层和输出层的前向神经网络，通过每一神经元激活函数以及网络权值的选取可以对任意非线性函数进行逼近。三层BP神经网络结构如图2所示：

[0102] 输入层、隐层和输出层分别有j、i、k个神经元，输入层的输入向量为[x1,x2,...,Τ Τxj] ，输出层的输出向量为[y1,y2,...,yk] 。输入层至隐层的连接权记为wij，隐层至输出层的连接权记为wki。

[0103] BP神经网络的实现流程如下所示：

[0104] (1)初始化，对所有权值赋以随机任意小值，同时对阈值设定初值；

[0105] (2)给定训练样本数据；

[0106] (3)计算实际输出y

[0107] yj＝f(ΣwkiOi)                            (16)

[0108] 其中，Oi为隐层输出，f为sigmoid函数，

[0109] (4)调整权值，按误差反向传播，从输出节点开始返回到隐层修正权值为：

[0110] wki(t+1)＝wki(t)+ηδkyk                        (17)

[0111] 其中，η为学习率，δk为节点k的实际活性与期望活性间的差值，根据节点k的形式不同，δk可计算为：

[0112]

[0113] 当使用冲量时，权值调整公式变为：

[0114] wki(t+1)＝wki(t)+ηδkyk+α[wki(t)‑wki(t‑1)]               (19)

[0115] (5)返回步骤(2)重复，直至误差满足要求。

[0116] 5、轨迹跟踪控制器设计

[0117] 如图3所示，建立水下机器人S‑F坐标系，其中s是任意给定的轨迹跟踪参考路径，p是参考型水下机器人的运动点，以p为原点，以参考路径的切线为横轴xSF，按右手螺旋定则建立参考型水下机器人随体坐标系F，同时以水下机器人重心为原点，建立随体坐标系B，设和 分别为水下机器人和参考型水下机器人在惯性坐标系下的位Τ
置坐标， 为S‑F坐标系下位置跟踪误差坐标。vt＝(u,v) 是水下机器人合速
度，β为侧漂角， 为导航角。则S‑F坐标系下的跟踪误差可以描述为如式(20)所示形式：

[0118]

[0119] 其中，

[0120] 对式(20)进行差分，得到如式(21)所示的误差动力学方程：

[0121]

[0122] 其中，wF为参考型水下机器人角速度， 为参考型水下机器人纵向速度，s表示沿路径的广义弧长。

[0123] 且根据S‑F坐标系下曲率与角速度对应关系，可以得到参考型水下机器人角速度其中 表示二维曲线的曲率，f(x)为某二维曲线。则可以将式(21)写成如式(22)形式：

[0124]

[0125] 根据误差动力学方程，基于视线法，便可以对水下机器人轨迹跟踪控制律进行设Τ计。轨迹跟踪的最终目标是使得跟踪偏差pe＝(xe,ye) 逐渐收敛于0，而艏向偏差逐渐收敛于导航角δ，导航角的表达式如式(23)所示：

[0126]

[0127] 其中，Δ为超前长度， 为导航角控制增益。

[0128] 根据本发明的水下机器人动力学模型，我们可以得到水下机器人艏向控制系统如下：

[0129]

[0130] 由于艏向偏差 需要收敛至导航角δ，则可以得到期望艏向角为定义艏向偏差 z2＝r‑rd，其中rd为中间虚拟控制量，并对z1求导，可得：
同时考虑如下Lyapunov函数：

[0131]

[0132] 对式(25)求导可得：

[0133]

[0134] 为了保证V1渐近收敛，需要保证 负定，因此设计中间虚拟控制量 ，即 此时将rd代入式(26)可得：

[0135]

[0136] 从式(27)可以发现，只要设计参数k1为正数，即可满足Lyapunov稳定性条件，从而使系统z1被镇定。对于系统z2，我们定义Lyapunov函数如下：

[0137]

[0138] 对式(28)求导可得：

[0139]

[0140] 将式(27)以及z1、 、z2、 代入式(29)可得：

[0141]

[0142] 要令 满足负定，则需要对控制力矩τr进行设计，具体公式如下：

[0143]

[0144] 最后，将式(31)代入式(30)可以得到：

[0145]

[0146] 从式(32)可以看出，只要设计参数k1和参数k2为正数即可保证艏向控制器的稳定性，将rd和 代入式(31)，可以得到最终的控制力矩τr如式(33)所示：

[0147]

[0148] 对于跟踪偏差pe＝(xe,ye)Τ，我们希望其逐渐收敛于0，所以选定如下所示的正定二次型Lyapunov函数：

[0149]

[0150] 将式(34)进行微分，可得：

[0151]

[0152] 设计控制律如式(36)所示；

[0153]

[0154] 式中k3为正常数，ur为额外增加的一个自由度的控制量，可以视为参考型水下机器人的运动学控制律，当水下机器人位置落后于参考型水下机器人时，即xe＜0，此时参考型水下机器人便会降低速度；当水下机器人位置超前于参考型水下机器人时，即xe＞0，此时参考型水下机器人便会加快速度。因此，参考型水下机器人能够能够根据位置偏差随时调整自身速度，从而配合待控制的水下机器人。

[0155] 将式(36)代入式(35)，可得：

[0156]

[0157] 因此，由式(37)可以看出，只要设计参数k0和参数k3为正数即可保证其稳定性。

[0158] 对于待控制的水下机器人，设置其期望速度为ud，同时根据本发明中水下机器人动力学模型，我们可以得到水下机器人速度控制系统如下：

[0159]

[0160] 定义速度偏差z3＝u‑ud，并对z求导，可得： 同时考虑如下Lyapunov函数：

[0161]

[0162] 对式(39)求导可得：

[0163]

[0164] 为了保证其能渐进收敛至零，只需满足Lyapunov稳定性理论，即保证V3正定的同时 负定，所以根据式(40)，对τu进行设计：

[0165]

[0166] 将式(41)代入式(40)，可得： 由此可以看出，只要设计参数k4为正数，即可满足Lyapunov稳定性理论，从而保证 是负定的，最终保证速度控制器渐近稳定。

[0167] 最后，由分布设计中的Lyapunov函数及通过LaSalle不变集原理综合得到结论，可将如式(42)所示的二次型函数作为待控制水下机器人轨迹跟踪系统的Lyapunov函数。

[0168] V＝V2+V3+V4                            (42)

[0169] 将式(42)进行微分，可得：

[0170]

[0171] 即 其中 因此，系统全局指数稳定，各项误差最终收敛至0。

[0172] 本发明利用基于双BP神经网络Q学习算法与水下机器人轨迹跟踪控制器相结合实现了对轨迹跟踪控制器参数的在线调节。同时由于引入了经验回放技术，使得基于双BP神经网络Q学习能够在训练较少次数的情况下达到更好的控制效果，同时保证了BP神经网络能够在全状态空间下进行训练，避免了其对过去经验的遗忘，从而建立起全局控制思路。

[0173] 与基于模糊技术的参数优化方法相比：

[0174] Londhe P.S等人针对AUV定深和艏向控制系统，分别设计了基于模糊逻辑的PD控制器，并给出了在任意有界洋流和波浪干扰情况下AUV定深和艏向控制仿真研究结果，结果表明，所设计的控制器具有较强的抗干扰能力。KashifIshaque等人提出了一种用于AUV上的模糊逻辑控制器，将传统的双输入模糊控制器简化为单输入单输出的控制器，大大减少规则推论时间，同时使控制参数调优变得简单易行，并通过仿真验证了该控制算法的有效性。基于模糊技术的控制器参数优化方法需要大量的先验知识以建立模糊规则，完善的模糊规则的建立耗时耗力，而本发明提出的基于双BP神经网络Q学习参数优化方法并不需要人为参与，只需设定好奖励函数，控制系统便可自行对轨迹跟踪控制器的参数进行优化。

[0175] 与基于自适应技术的参数优化方法相比：

[0176] Barbalata等人提出了AUV四自由度双闭环比例控制器的自适应在线调整方法，该方法根据位置和速度误差在线确定控制器的增益，解决了系统参数的不确定性，使系统能够在水动力存在干扰的情况下正常运行，大大降低了AUV固定增益整定过程中的复杂度。Rout和Subudhi等人提出了一种PID控制器的自适应调整方法，用于AUV路径跟踪控制，仿真实验证明，即使存在海流或水动力参数引起的不确定性，AUV同样具备良好的路径跟踪控制性能。上述研究均需要对未知参数进行自适应控制律的设计，导致控制律复杂，不利于在实际环境下运行。而本发明提出的基于双BP神经网络Q学习框架仅通过与环境的不断交互，便可以对控制器参数进行优化，并不需要复杂的自适应控制律。

[0177] 算法仿真

[0178] 为了检验基于双BP神经网络Q学习轨迹跟踪控制器的控制效果，设计了一条折线段作为目标运动轨迹，从而验证所研究水下机器人的直线跟踪能力以及转艏性能，进而观察基于双BP神经网络Q学习方法在轨迹跟踪控制中的在线调参及自主决策能力。本次仿真实验轨迹跟踪相关参数设置如下：制导参数Δ＝15，导航角控制增益k0＝3，ud＝1m/s，目标轨迹为(0,0)→(40,57)→(120,‑5)三个点组成的折线段；选取单步控制步长Ts＝0.5s，仿真时间M＝150s，强化学习折扣率γ＝0.9，强化学习学习率α＝1；BP神经网络的输入神经元设置为4个，输出神经元为20个，BP神经网络的参数w初始化为[‑1,1]的随机数，同时设置神经网络参数学习率β＝0.05；初始化水下机器人速度、角速度及艏向角：u0＝0m/s，v0＝0m/s，r0＝0rad/s， 水下机器人初始位置为(x0,y0)＝(0,5)；初始控制增益设置为：k1＝2，k2＝2，k3＝1，k4＝1。

[0179] (2)仿真实验结果及分析

[0180] 为了验证基于双BP神经网络Q学习框架在水下机器人轨迹跟踪这一更加复杂的控制任务中的适用性和有效性，对其进行了目标轨迹为折线段的轨迹跟踪仿真实验。其中，图4为折线段轨迹跟踪控制效果对比图，图5和图6分别为x方向和y方向上的轨迹跟踪误差对比图，图7和图8分别为折线段轨迹跟踪纵向推力图和偏航力矩图，图9为在同一个控制周期中，艏向控制参数k1和k2的变化曲线图。

[0181] 在使用基于双BP神经网络Q学习框架的轨迹跟踪控制过程中，水下机器人在航行了一段时间后抵达目标运动轨迹，并沿着直线的目标轨迹运动至转折点，此时，对艏向进行了较大幅度的调整，然后沿着另一条直线轨迹直至运动到终点，整个跟踪控制过程中跟踪效果良好。从图4中可以看出，在使用基于双BP神经网络Q学习框架后，轨迹跟踪效果明显改善，且从图5和图6中可以看到，参数优化后的轨迹跟踪控制器能够更加快速地对跟踪误差作出相应调整。图9为同一个控制周期中艏向参数值的变化情况，可以看到，在一个控制周期中，参数值k1并不是一层不变的，而是由1变成了2.5，这说明基于双BP神经网络Q学习框架在水下机器人轨迹跟踪控制任务中具备在线调参和自主学习能力。

[0182] 本发明的上述算例仅为详细地说明本发明的计算模型和计算流程，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动，这里无法对所有的实施方式予以穷举，凡是属于本发明的技术方案所引伸出的显而易见的变化或变动仍处于本发明的保护范围之列。