强化学习——双臂攀爬机构运动控制
使用强化学习控制双臂攀爬机构的运动学模型
- 源码地址
- 演示效果
- 测试环境
- 概述
- 核心内容
- 机器人运动学
- DDPG
- DQN
- 实验过程及效果
- 代码结构
- 运动学环境搭建
- 移动上臂攀爬
- Actor
- Critic
- 移动下臂攀爬
- 宏观决策
- 不足与改进
- 参考文献
源码地址
链接: https://github.com/aaronworry/two-arms-climb
学习使用时备注出处即可
演示效果
链接: https://www.bilibili.com/video/av41743426/?p=1
测试环境
CPU：Intel i7-4710MQ
RAM：16G
显卡：GTX 850M
概述
搭建了一个双臂攀爬的运动学模型，分别用DDPG训练了固定下臂，移动上臂和固定上臂，移动下臂的运动控制。然后使用DQN建立了宏观运动决策模型。
核心内容
机器人运动学
通常将表示相邻的两连杆相对关系的矩阵称为A矩阵。
- 一般需要两个参数来描述一个连杆
- 公法线距离 a i a_i ai
- 垂直于 a i a_i ai所在平面内两轴的夹角 α i \alpha_i αi。
- 同样相邻两连杆的关系也需要两个参数
- 两连杆的相对位置 d i d_i di
- 两连杆的法线夹角 θ i \theta_i θi。
一般用两个旋转和两个平移来表示相邻连杆的相对位置关系。
在此环境下，连杆只有3个自由度，且 a i = l i , α i = 0 , d i = l i , θ i = θ 相 邻 连 杆 的 夹 角 a_i=l_i, \alpha_i=0,d_i=l_i,\theta_i=\theta_{相邻连杆的夹角} ai=li,αi=0,di=li,θi=θ相邻连杆的夹角
因此可以只用一个旋转和一个平移表示。设Z轴为运动平面的法向量，X轴为连杆的方向。因此连杆的变换矩阵为
A i = R o t ( z , θ z ) ⋅ T r a n s ( l i , 0 , 0 ) A_i=Rot(z,\theta_z)\cdot Trans(l_i,0,0) Ai=Rot(z,θz)⋅Trans(li,0,0)表示新系先相对于旧系旋转，然后沿旋转之后的坐标系的X轴平移。
左乘广义矩阵时，若变换是相对于新系的，则是右乘。即 T 2 = A 1 ⋅ A 2 T_2=A_1\cdot A_2 T2=A1⋅A2(相对于基系作了 A 1 A_1 A1变换之后，相对于变换之后的新系作了 A 2 A_2 A2变换)。若变换是相对于旧系的，则是左乘。即 T 2 = A 2 ⋅ A 1 T_2=A_2\cdot A_1 T2=A2⋅A1(相对于基系作了 A 1 A_1 A1变换之后，相对于变换之前的基系作了 A 2 A_2 A2变换)。
A i = [ cos θ i sin θ i 0 l i cos θ i sin θ i − cos θ i 0 l i sin θ i 0 0 1 0 0 0 0 1 ] A_i = \left[ \begin{array} {cccc}\cos\theta_i&\sin\theta_i&0&l_i\cos\theta_i\\\sin\theta_i&-\cos\theta_i&0&l_i\sin\theta_i\\0&0&1&0\\0&0&0&1\end{array} \right] Ai=⎣⎢⎢⎡cosθisinθi00sinθi−cosθi000010licosθilisinθi01⎦⎥⎥⎤
DDPG
参考博客：https://blog.csdn.net/kenneth_yu/article/details/78478356
该算法结合了DQN以及Actor Critic，吸收了二者的优点。
- 先建立一个记忆库R
- 初始化了Actor和Critic网络，对其进行了复制得到两个target网络
- 在每次的任务中：
- 状态环境初始化，随机过程初始化
- 每次进行决策时：
- 根据策略 μ \mu μ和噪声生成 a t a_t at
- 执行动作，获取奖励及更新环境，并将 ( s t , a t , r t , s t + 1 ) (s_t, a_t, r_t, s_{t+1}) (st,at,rt,st+1)存入记忆库
- 从记忆库中抽取一组 ( s i , a i , r i , s i + 1 ) (s_i, a_i, r_i, s_{i+1}) (si,ai,ri,si+1)
- 计算 y i y_i yi
- 计算并根据损失函数更新critic的参数
- 计算策略梯度更新actor的参数
- 更新对应的target网络
- 在每次的任务中：
DQN
参考:https://morvanzhou.github.io/tutorials/machine-learning/reinforcement-learning/4-1-DQN1/
- 先建立一个记忆库D
- 用神经网络作为 Q ( s , a ) Q(s,a) Q(s,a)，进行权重初始化。建立一个 Q t a r g e t Q_{target} Qtarget，模型和Q一样，进行初始化
- 在M次的任务中：
- 状态环境初始化
- 每次进行决策时：
- 使用 ϵ − g r e e d y \epsilon-greedy ϵ−greedy进行决策，选取随机的 a t a_t at或 a t = a r g m a x a Q ( s t , a t ) a_t=argmax_aQ(s_t,a_t) at=argmaxaQ(st,at)
- 执行动作，获取奖励及更新环境，并将 ( s t , a t , r t , s t + 1 ) (s_t, a_t, r_t, s_{t+1}) (st,at,rt,st+1)存入记忆库
- 从记忆库中抽取一组 ( s i , a i , r i , s i + 1 ) (s_i, a_i, r_i, s_{i+1}) (si,ai,ri,si+1)
- 计算 y i y_i yi，根据下一步是否为最后的一步，设置 y i = r i + γ m a x Q t a r g e t ( s t + 1 , a ) y_i = r_i+\gamma maxQ_{target}(s_{t+1},a) yi=ri+γmaxQtarget(st+1,a)
- 计算损失 ( y i − Q ( s t , a i ) ) 2 {(y_i-Q(s_t,a_i))}^2 (yi−Q(st,ai))2并对 θ \theta θ进行优化
- 根据固定的step更新 Q t a r g e t Q_{target} Qtarget，将权重用 Q Q Q替换
- 在M次的任务中：
实验过程及效果
代码结构
kinematicEnv是整个环境模型。bottomDDPG和upDDPG分别是控制如何动下臂、动上臂攀爬的agent，QL是进行宏观决策的agent。三个以train开头的文件用来训练(评估)三个agent。Evaluation是展示整体效果的文件。
运动学环境搭建
双臂攀爬机构由两短臂及两长臂组成。state设置为3个关节及2个端点的X,Y坐标，加上1个描述此时状态(上臂离开杆，下臂离开杆，双臂都在杆上)的参数，还有一个判断此动作是否完成的FLAG。action设置为3个关节的角度(相对于靠近机座的连杆的角度)以及2个端点的角度(相对于世界坐标系的X轴角度)
| item | dim |
|---|---|
| state | 12 |
| action | 5 |
在向上或向下攀爬的过程中，中间三个关节的角度是相对于下边连杆的夹角。因此机构移动上臂时，关节的角度不需要变换。而在移动下臂时，关节需要做一个变换，将角度改为相对于上边连杆的夹角，以便于运动学演算。
def thetaTrans(self, theta):
return 2 * np.pi - theta #关节的相对角度改变，从相对于下端改变到相对于上端
- 整个环境包含：
- state以及机构等环境的初始化 init
- 每个episode的初始化
- 单臂在杆上的正运动学初始化
- 上臂在杆上 initialDown
- 下臂在杆上 initialUp
- 双臂在杆上的逆运动学初始化 initialOn
- 单臂在杆上的正运动学初始化
- 运动上臂的部分 stepUp
- 运动下臂的部分 stepDown
- 模型的可视化 Viewer
机构攀爬时，可认为是固定下臂，用上臂抓取物体或固定上臂，用下臂抓取物体。
移动上臂时，可认为是抓远点的物体，移动下臂可认为是抓取近点的物体。物体的中心和竖杆都在Y轴上。
下面重点讲解stepDown
def stepDown(self, action):
done = False
#执行ddpg生成的action，然后将执行完动作之后的角度控制在360°以内
action = np.clip(action, *self.action_bound)
self.jointState += action * self.dt
self.jointState %= 2*np.pi
#读取关节及端点的夹角
self.downPointAngle, self.downJointAngle, self.centerAngle, self.upJointAngle, self.upPointAngle = self.jointState
#由于移动下臂时，连杆的位姿仅根据上面四个角度和上端点的位置就可以确定，
#因此下端点与X轴的夹角需要根据连杆的位姿重新计算
self.jointState[0] = (7 * np.pi + self.upPointAngle - self.downJointAngle - self.centerAngle - self.upJointAngle) % (2*np.pi)
#正运动学求解关节位置
#thetaTrans是将相对于下端连杆的角度转化为相对于上边连杆的角度
#A1~A4分别是4个描述连杆相对位置的A矩阵，文章以上部分有提到
A1 = self.A(self.thetaTrans(self.upPointAngle), self.pole)
A2 = self.A(self.thetaTrans(self.upJointAngle), self.crank)
A3 = self.A(self.thetaTrans(self.centerAngle), self.crank)
A4 = self.A(self.thetaTrans(self.downJointAngle), self.pole)
#在参考坐标系变换后，各关节的位置都是相对于新系的，都为[0,0,0]
#每次变换都是相对于新系，因此都是右乘 A1.dot(A2).dot(A3)...
#关节的坐标都是通过最上面的端点进行计算的
self.upJointLocation = self.upPointLocation + (A1.dot(
np.concatenate((np.array([0., 0.]), np.array([0., 1.]))).reshape(4, 1))).reshape(1,4)[0][0:2]
self.centerLocation = self.upPointLocation + (A1.dot(A2).dot(
np.concatenate((np.array([0., 0.]), np.array([0., 1.]))).reshape(4, 1))).reshape(1,4)[0][0:2]
self.downJointLocation = self.upPointLocation + (A1.dot(A2.dot(A3)).dot(
np.concatenate((np.array([0., 0.]), np.array([0., 1.]))).reshape(4, 1))).reshape(1,4)[0][0:2]
self.downPointLocation = self.upPointLocation + (A1.dot(A2.dot(A3.dot(A4))).dot(
np.concatenate((np.array([0., 0.]), np.array([0., 1.]))).reshape(4, 1))).reshape(1,4)[0][0:2]
#更新state，即记忆库里面的s_ 倒数第二个表示机构在移动下臂，最后一个表示此动作还未完成
s = np.concatenate((self.downPointLocation, self.downJointLocation, self.centerLocation, self.upJointLocation,
self.upPointLocation, [2.], [1. if self.on_goal else 0.]))
#计算reward：根据下端离上端是否有100来定义reward，为100时reward最大
r = - 2 * np.sqrt((self.downPointLocation[0]/300.) ** 2 + ((self.upPointLocation[1] - self.downPointLocation[1] - 100)/300)**2)
#根据在最大奖励附近的停留时间，给予额外奖励，提高ddpg的稳定性
if - self.width / 2 < self.downPointLocation[0] < self.width / 2:
if - self.width / 2 < self.upPointLocation[1] - self.downPointLocation[1] - 100 < self.width / 2:
r += 1.
self.on_goal += 1
if self.on_goal > 50:
done = True
else:
self.on_goal = 0
#更新arm的state，以便于可视化
self.armState = np.concatenate(
(self.downPointLocation, self.downJointLocation, self.centerLocation, self.upJointLocation,
self.upPointLocation))
return s, r, done
其他的部分和这个类似，不做过多叙述。
环境的评估视频地址： https://www.bilibili.com/video/av41743426/?p=2
移动上臂攀爬
参考莫烦的DDPG代码： https://github.com/MorvanZhou/train-robot-arm-from-scratch/blob/master/part5/rl.py
神经网络结构如下:
Actor
Critic
输出层用来决策关节沿逆时针运动的角度。Actor和Critic学习率都设置为0.001。
训练时的环境初始化是随机选择initialOn()和initialUp()，episode设置为9000，step设置为300
训练之后的效果： https://www.bilibili.com/video/av41743426/?p=3
移动下臂攀爬
网络结构和移动上臂攀爬一样，环境初始化随机选择initialOn()和initailDown()。episode设置为3000，step设置为200
训练之后的效果： https://www.bilibili.com/video/av41743426/?p=4
宏观决策
参考莫烦的DQN代码：https://github.com/MorvanZhou/Reinforcement-learning-with-tensorflow/blob/master/contents/5_Deep_Q_Network/RL_brain.py
网络结构如下：
输出层表示接下来是移动上臂还是移动下臂。学习率为0.001， ϵ \epsilon ϵ为0.9。episode设置为6000，step设置为300。
机器人进行决策后，根据当前的state判断此决策是否合理，奖励设置如下:
- 生成了了正确的策略 r = 1 r=1 r=1
- 机构生成了错误的策略，从竖杆掉下(两端不在竖杆上) r = − 2 r=-2 r=−2
- 决策错误，机构仍在竖杆上 r = − 1 r=-1 r=−1
决策准确度计算如下：
在每个episode中，统计 r = 1 r=1 r=1 的数量，计算 机构 掉下之前或完成1个episode的决策次数。精确度为二者的比值。下图为6000次episode的精确度。
使用hardcode决策的效果 evalWithHardCode()： https://www.bilibili.com/video/av41743426/?p=5
使用DQN决策的效果 evalWithoutLimit()： https://www.bilibili.com/video/av41743426/?p=6
使用 DQN决策，并作出了一点限制 的效果 evalAll()： https://www.bilibili.com/video/av41743426/?p=7
不足与改进
- 当机构执行“抓取”时，运动学逆问题不止一组解。机构在运动时会有些“突兀”。
- 可以对中间的三个关节角度进行限制
- 在进行宏观决策时，虽然精度达到95%左右，但是一些失误的决策仍会让双臂同时离开竖杆。
- 在测试时， ϵ = 0.9 \epsilon=0.9 ϵ=0.9，仍有一部分随机决策。可以增加episode；修改网络结构；在训练好模型之后，评估时去掉 ϵ − g r e e d y \epsilon-greedy ϵ−greedy。
- 用DDPG训练的模型收敛较慢，稳定性较低
- 尝试增加episode；修改神经网络结构
1.可以将可视化的环境图像作为网络的输入，搭建卷积神经网络。
2.使用PPO算法。
3.在移动上臂或下臂时，DDPG输出的5个action只有4个对运动控制有影响。可以使用神经进化的方式修改神经网络的形态和参数。
参考文献
1.《机器人学基础》蔡自兴著
2. 论文：Playing Atari with Deep Reinforcement Learning
3. 论文：Continuous control with deep reinforcement learning