[发明专利]一种基于强化学习的蝾螈机器人路径跟踪的分层控制方法

权利要求书

1.一种基于强化学习的蝾螈机器人路径跟踪的分层控制方法，其特征在于该方法采用分层控制框架，所述蝾螈机器人跟踪路径根据任务不同分为两层控制器，分别是基于强化学习的上层控制器和基于逆运动学的底层控制器；其中，基于强化学习的上层控制器包括状态空间的设计、动作空间的设计和奖励函数的设计，底层控制器包括脊柱控制器和腿部控制器，腿部控制器由轨迹生成模块和逆运动学求解模块构成；具体地，蝾螈机器人在t时刻的状态和动作分别表示为s_t,a_t，上一时刻得到的奖励为表示r(s_t-1,a_t-1)，在训练阶段，上层控制器在t时刻输入r(s_t-1,a_t-1)和s_t，然后输出动作表示为a_t，动作a_t作为底层控制器的输入，底层控制器根据输入的指令输出蝾螈机器人19个关节的位置，并在仿真环境中执行，根据执行指令后机器人的状态，生成下一时刻的奖励r(s_t,a_t)，再将r(s_t,a_t)和s_t+1反馈给上层控制器，依次循环；在实验阶段，上层控制器的输入和环境的反馈只有状态s_t，中间步骤相同；

所述基于强化学习的上层控制器的设计如下：

蝾螈机器人跟踪路径的问题建模为一个马尔可夫决策过程(Markov decisionprocess)，通过一个元组来定义其中是连续的状态空间，表示连续的动作空间，p表示状态转移概率，r是在每次状态转移过程中来自环境的奖励；π(a_t|s_t)表示策略，在状态为s_t时采取动作a_t，用ρ_π(s_t,a_t)表示状态-动作的轨迹分布边际，soft Actor-Critic强化学习算法的目标是学习一个最优策略π(a_t|s_t)使得奖励与交叉熵的累积加权和最大，即

其中，α是超参数，权衡奖励与交叉熵的重要性，最大熵目标可以有效地扩大探索范围，加快强化学习的收敛速度；上层控制器包括状态空间的设计、动作空间的设计和奖励函数的设计，这三部分的设计保证了最终训练的控制器收敛，三部分属于并列关系；

(1)、状态空间的设计

对于路径跟踪问题，将目标路径离散为一系列散点，用一个滑动窗口从路径起点滑向终点，将滑动窗口中的n个散点在机器人坐标系中的坐标作为状态，即

其中，^RP_i^t表示滑动窗口中第i个散点在机器人坐标系中的坐标；滑动窗口的移动取决于窗口中第一个点与机器人之间的距离，如果距离小于设定的阈值，则滑动窗口向前移动一步，滑动窗口中第一个点将被移除，在队列末尾将增加一个点；当扫描完目标路径上的所有点后，路径跟踪任务就完成了，变化较快的传感器数据不被选择作为状态的一部分，从而使训练好的网络更容易应用到真实的机器人上；

在路径跟踪任务中容易出现稳态误差，为了有效地消除跟踪的稳态误差，引入状态积分补偿项，即

其中，是积分补偿项，表示滑动窗口中第一个点的坐标的累积求和；

(2)、动作空间设计

在分层控制框架中，上层控制器输出的动作作为底层控制器的输入，动作是一个抽象的命令，包括向前、向左、向右运动一个很小的距离，而不是具体的关节位置或者速度，这样做的优点是能够对任何路径有效，即任何路径的跟踪可以转化为机器人向前、向左、向右运动一个很小的距离；对于蝾螈机器人跟踪路径，动作是左腿的步长、右腿的步长和脊柱的偏置，如果左腿的步长比右腿的步长大，机器人则向右运动，脊柱偏置对转弯有一定的影响，动作空间设计如下:

其中，l_left表示左腿步长，l_right表示右腿步长，表示脊柱偏置；

(3)、奖励函数的设计

强化学习利用奖励来引导机器人学习到一个最优的策略，在机器人跟踪路径的任务中，机器人与滑动窗口内所有散点的距离的加权和的相反数作为奖励函数，直观意义是距离越小，奖励越大，即

其中，k_i是权重系数，用来调整每个目标点的重要性，随着i的增加，相应的目标点距离机器人越远，所以权重系数越小，注意到滑动窗口中的n个目标点表示跟踪路径的前视距离，机器人可以据此决定前进的方向，将奖励定义为负值，以激励机器人以最少的控制步数到达目标位置；

所述基于逆运动学的底层控制器的设计如下：

(1)、腿部控制器设计

每条腿有四个关节，通过解逆运动学得到每个关节的角度，使末端执行器按照给定的轨迹运动；这四条腿的参数相同，所以下面的分析适用于每条腿，下面对一条腿建立运动学模型，从腿部末端点到基座的四个坐标变换矩阵如下：

其中sθ＝sin(θ),cθ＝cos(θ)腿部末端点在第四个坐标系下的位置为⁴P＝[0 -96 0 1]^T,则末端点在基座坐标系下的位置为

如此便找到了四个关节角与腿部末端点在基坐标系的对应关系，基坐标系固定在机器人上，通过改变四个关节角就可以改变末端点的位置；腿部的控制是通过在基坐标系下给定一条轨迹，让末端点跟踪这条轨迹；末端点相对地面静止，利用腿部末端与地面的反作用力推动身体前进，轨迹通过贝塞尔曲线生成，通过改变控制点可以改变轨迹的步幅长度和高度；在每条轨迹上等间隔取样100个点p_i＝[x_i y_i z_i]^T作为腿部末端点的目标位置，通过逆运动学求解出所对应的四个关节角，以固定频率向四个舵机发送求解出的目标角度便可以让腿部走出对应的轨迹；

机器人腿部末端在基坐标系下的笛卡尔空间坐标为：

机器人腿部末端点的速度与四个关节角速度满足:

其中，

雅可比矩阵J是3×4阶的矩阵不可以对其求逆，否则利用雅可比逆矩阵，通过给定腿部末端点的目标位置与当前位置的误差项，便可以求出控制项关节角速度；现在将求解关节角转换为一个优化问题：

其中Δq＝q_t-q表示目标角度与当前角度的误差；

Δp＝p_t-p表示腿部末端点在笛卡尔空间下目标位置与当前的误差；

J是雅可比矩阵，λ是常数阻尼项，目的是避免关节角进入极点，转速太快，损坏舵机；

令

再求导

得到目标角度和当前角度的误差的最优解如下

Δq＝(J^TJ+λI)^-1J^TΔp (21)

根据公式(21)得到当前角度与目标角度的差值，通过积分得到当前角度，再运用正运动学公式(13)得到当前腿部末端点在基坐标系下的位置，计算腿部末端点的目标位置与当前位置的误差项Δp，如果误差项大于给定的阈值，则将误差项Δp继续带入公式(21)求解Δq，如果误差项小于给定的阈值，则将当前角度作为当前目标位置的解，然后更新下一个目标位置和雅可比矩阵，重新计算位置误差项Δp作为公式(21)的输入项，得到目标角度和当前角度的误差Δq，如此迭代计算可以求解出所有样本目标位置所对应的四个舵机的目标角度；

与蝾螈爬行相似，考虑四足蝾螈机器人以静态稳定、规则对称的爬行步态行走，先放下空中的腿，再抬起地面上的腿，至少有三条腿始终接地；在走路的过程中，前腿从摆动阶段切换到站立阶段，然后后腿的相对一侧切换到摆动阶段，然后前腿进入摆动阶段；四只腿的相位如下所示：

ψ_RF＝0,ψ_LH＝0.25,ψ_LF＝0.5,ψ_RH＝0.75 (22)

其中RF、LH、LF、RH分别代表右前肢、左后肢、左前肢、右后肢；

(2)、脊柱控制器设计

脊柱的运动可以改变重心的位置，增加运动的平稳性，还可以增大腿迈出的步幅长度，从而加快运动的速度；脊柱按照正弦信号摆动控制器设置如下

其中b_i是脊柱第i个舵机按正弦信号摆动的振幅，f是摆动的频率，通过与腿部运动频率配合能够改变运动速度，φ_i是第i个舵机正弦信号的初相位，是第i个舵机正弦信号的偏置项；

由于脊柱关节数目较少，中间关节的运动幅度较大；脊柱的正弦信号周期的四分之一是一个相位，有四个相位，因此

φ₁＝0,φ₂＝π,φ₃＝0 (24)

在公式(25)中给出了偏置项的选取原则，当机器人直行的时候，偏置项为零，脊柱按照正弦信号摆动，当机器人需要转弯的时候，脊柱向转弯相反的方向弯曲，偏置项的大小与转弯的半径的大小成正比。