[发明专利]基于近端策略优化的水下蛇形机器人高速步态生成方法

权利要求书

1.一种基于近端策略优化的水下蛇形机器人高速步态生成方法，其特征在于，包括如下步骤：

1)搭建强化学习的训练环境，包括reset()函数、step()函数；其中所述的：

(1.1).reset()函数：初始化状态，采样产生数据时每次试验的起始状态，采用随机初始化的方式，增加在初始位置的探索率；

(1.2).step()函数：用于得到马尔科夫决策过程，首先是采用迭代牛顿-欧拉的方法建立水下蛇形机器人的动力学模型，然后设置状态空间、动作空间和回报函数；其中，

(1.2.1)所述的采用迭代牛顿-欧拉的方法建立水下蛇形机器人的动力学模型，包括：

水下蛇形机器人是由N+1个连杆和N个关节组成，ρ是水的流体密度，连杆的附加质量m_a＝C_aρπr²l，连杆的附加力矩惯量连杆的转动惯量其中，C_a为附加质量系数，l是单个连杆长度，r是模块横截面半径，m是单个连杆质量；

附加质量矩阵附加力矩惯量矩阵对应的形状阻力和粘滞阻力的系数矩阵C_f和C_d分别表示切向和法向阻力系数；旋量变换矩阵惯量矩阵其中，为一个3×3的旋转矩阵，表示关节坐标系∑i-1相对于关节坐标系∑i的表示；为一个的平移矩阵，表示关节坐标系∑i的原点相对于关节坐标系∑i-1的位置；为关节坐标系∑i的原点相对于关节坐标系∑i-1中坐标的叉乘矩阵；I₃为3×3的单位矩阵；M_i为第i个连杆的质量；S_i表示第i个关节到第i个连杆质心的向量；表示第i个关节在第i个关节坐标系∑i中坐标的叉乘矩阵；是第i个连杆相对于坐标系∑i的一阶惯量，转动惯量矩阵

水下蛇形机器人在水下所受到的水动力包括附加质量力和水阻力，在第i个连杆的关节坐标系∑i下，施加在第i个连杆上的水阻力由线性阻力和非线性阻力构成，为：

其中，为第i个连杆在第i个连杆的关节坐标系下的线速度向量；

在第i个连杆的坐标系∑i下，施加在第i个连杆上的附加质量力为：

其中，表示第i个连杆在第i个连杆的关节坐标系下的角速度向量；为角速度向量的斜对称矩阵，S_i表示第i个关节到第i个连杆质心的向量；

作用在第i个连杆上的线性阻力矩和非线性阻力矩为：

其中，

作用在第i个连杆上的附加质量力矩为：

相邻的第i-1个关节确定的坐标系∑i-1传递到第i个关节的确定的坐标系∑i速度表示为：

其中，为第i个连杆的关节坐标系∑i的旋转角速度；参数其中，0_3×1为一个3×1全为零的矩阵，水下蛇形机器人在二维平面运动时，

相邻的第i-1个关节确定的坐标系∑i-1传递到第i个关节的确定的坐标系∑i的加速度表示为：

其中，为第i个连杆的关节坐标系∑i的旋转角加速度；

根据牛顿欧拉方程，得到第i个关节总的外部力旋量为：

其中，为第i个关节坐标系下第i个连杆所受的总外力向量；为第i个关节坐标系下第i个连杆绕着第i个连杆质心旋转的总外力矩向量；

力的平衡方程为：

其中，表示在第i个关节坐标系下第i-1个连杆施加在第i个连杆的力旋量；表示在第i个关节坐标系下第i-1个连杆施加在第i个连杆的外部力旋量；

采用迭代牛顿-欧拉的方法建立水下蛇形机器人的动力学模型的实现过程需要依次进行下面三个迭代过程：

Step1，前向迭代，利用公式(5)～公式(8)得到每个关节的速度、加速度和合外力，同时引入参数

Steps2，后向迭代：得到头部加速度，

其中，得到头部加速度为：

Steps3：前向迭代，得到每个关节加速度和力矩为

(1.2.2)所述的设置状态空间包括头部连杆在世界坐标系下的角度θ₀、关节角头部连杆的角速度关节角速度头部沿x轴方向的速度v_x和头部沿y轴方向的速度v_y，考虑水下蛇形机器人自身约束，这里将关节角约束条件设定在-0.5π～0.5π区间内；

(1.2.3)所述的设置动作空间是关节角加速度

(1.2.4)所述的设置回报函数：若水下蛇形机器人的关节角在设定的约束范围内，则回报函数为reward＝-r_v+time_bonus，其中参数r_v＝100v_x,参数time_bonus＝0.01；若关节角超过设定的约束条件，则回报函数为-10；

2)在强化学习的训练环境下实现近端策略优化算法；

3)将策略网络得到的权值利用正弦函数为基函数去拟合，从而转换为水下蛇形机器人运动步态参数。

2.根据权利要求1所述的基于近端策略优化的水下蛇形机器人高速步态生成方法，其特征在于，步骤2)包括：

近端策略优化算法的网络结构包括一个值函数网络和一个策略网络，值函数网络的输入是状态，输出是状态值函数；策略网络的输入是状态，输出是动作；

近端策略优化算法的目标函数L(θ)为：

其中，θ表示策略网络的网络参数；s_t为第t时刻的状态；a_t为第t时刻的动作；π_θ(a_t|s_t)表示在当前策略网络参数θ下在状态s_t下动作a_t的评估；表示在当前参数更新前的策略网络参数θ_old下在状态s_t下动作a_t的评估；为第t步的优势函数；ε表示裁剪系数，设置为0.2；

依据下式对值函数网络进行更新：

其中，ψ为值函数网络的网络参数；T为终止时刻；R_t为第t时刻的立即回报；V_ψ为值函数网络在网络参数为ψ下的值函数；

近端策略优化算法每采样一条轨迹或者多条轨迹达到设定的数据量，值函数网络和策略网络的权值更新一次，权值每更新一次包括以下3步：

(2.1)采样：在步骤2)的基础上，当前的策略网络π_θ根据状态s_t得到用高斯策略表示的动作a_t～N(μ,σ²；θ)，其中，μ为高斯分布的均值；σ为高斯分布的方差；与环境进行交互得到下一个时刻的状态s_t+1，得到此时刻的立即回报R_t，然后根据下一个时刻的状态s_t+1，利用当前的策略网络π_θ得到下一时刻的动作a_t+1；重复该采样过程，得到序列数据，构成一条轨迹

(2.2)对值函数网络更新：采用蒙特卡罗的方法估计值函数，得到折扣累计回报：

其中t'为第t'时刻，γ为折扣因子；

再利用均方误差作为值函数网络的损失函数进行更新：

将所述的轨迹τ分为若干个批处理数，对损失函数求梯度：

其中α^ψ是值函数网络的学习率；n为采集数据的轨迹数；为第i条轨迹在第t时刻的状态；

(2.3)对策略网络更新：即对近端策略优化算法的目标函数L(θ)求梯度：

其中，α^θ是策略网络的学习率。