前言
提示：这里可以添加本文要记录的大概内容：
记录以前上课时学习的一些知识
本文需要掌握的一些前置知识：
1.机器人的D-H建模
2.机器人的正运动学
3.机器人的逆运动学
4.强化学习
5.差分进化算法
下面，我们围绕上述五点，进行详细的讲解。
提示：以下是本篇文章正文内容，下面案例可供参考
一、需要构建一个怎样的机器人？
这款机器人为一个六自由度机器人，其第三个关节为滑动关节，其余关节均为转动关节，关于将第三个关节设置为滑动关节，想法主要是第一关节为转动关节，主要是控制机器人在xy面上的大范围运动，第二关节以及第三关节主要是控制机器人在空间中的xyz上大幅度的运动，后三个关节主要是对末端姿态的调整为主。具体如下图所示
如上图所示，各个符号下标即表示对应的关节，
使用D-H建模方法对其进行建模，可以得到如下D-H参数表
由D-H表变换顺序θ、d、y、a、α可得相邻两个坐标系间的齐次变换矩阵i^T_j为如下：
使用D-H表中的数据依次代入则能够得到每个相邻坐标系间的变换矩阵，若要求某一个坐标系与基坐标系的关系，则将其两者之间的所有坐标变换矩阵连乘即可。
根据上述内容，即是机器人正运动学相关的内容。就是给转动关节一个θ值，根据是否滑动，机械臂的长短，计算机器人的姿态，这个就是正运动学。
下面简要讲一下逆运动学，就是，期望机器人到达某一种姿态，然后求取其要转动关节的θ值及滑动的位置的大小（带滑动机构的机器人）。
有关逆运动学，这里分别使用两种方法进行计算，具体如下：
第一种方法如下：
以下给出微分运动的计算流程：
1）给定末端关节参考位姿P_ref, R_ref。
2）给定6个关节变量的数组params，计算正运动学，得出每个关节相对世界坐标系的位姿。
3）（P_err, R_err）= (P_ref– P,R’R_ref)
4）通过R_err计算出关节误差W_err，当P_err的二范数和W_err的二范数之和小于0.1时，结束流程。否则：
5）计算Jocobi矩阵，计算关节微量delta，delta=inv(J) *[P_err, W_err]’，将其叠加到原关节变量数组params上。
6）循环至（2）。
根据上述这些内容，使用MATLAB进行仿真，得到如下结果：
以上这些都是书本上的知识，下面计算逆运动学，根据智能控制里的差分进化算法去辨识逆运动学的解。
二、使用差分进化算法辨识逆运动学的解
差分进化算法(Differential Evolution Algorithm，DE)是一种高效的全局优化算法。它也是基于群体的启发式搜索算法，群中的每个个体对应一个解向量。差分进化算法的进化流程则与遗传算法非常类似，都包括变异、杂交和选择操作，但这些操作的具体定义与遗传算法有所不同。
算法思想：
1. 从一个随机产生的初始种群开始
2. 通过把种群中任意两个个体的向量差与第三个个体求和来产生新个体
3. 将新个体与当代种群中相应的个体相比较，如果新个体的适应度优于当前个体的适应度，则在下一代中就用新个体取代旧个体，否则仍保存旧个体
4. 通过不断地进化，保留优良个体，淘汰劣质个体，引导搜索向最优解逼近。
具体流程及思想如图所示：
首先对图8差分进化算法的三个操作进行基本的讲解：
图8中第一个式子：差分进化算法的变异，其中F为变异因子，变异因子的大小的选取在后面中会有解释。其中1/2/3为随机的三个粒子，根据S_N = S_1 + F*(S_2 - S_3)来产生新的粒子S_N。第二个式子：差分进化算法的操作，上下两行为两个粒子里面的数据，其中new为刚生成的粒子、now为当前size循环下的粒子，通过一些随机概率，将两粒子之间的数据进行交换，生成新的粒子v。第三个式子:将当前粒子与新生成的粒子v进行适应度fi对比，适应度高的留下，低的被淘汰。
下面看一下fi适应度曲线的图表
对上面的图9、图10差分进化算法的每代最佳适应度的曲线的一些讲解以及理解。
F为变异参数，F设置的较小时，群体的差异度减小，收敛过早
从图9来看F=0.1时收敛过早，并且适应度低，效果不好
图10 F = 0.3时最后迭代完也还未收敛，但是适应度相较于图9的效果好。
从上述两图的fi适应度曲线来看，因为我将适应度取期望位姿的倒数，所以根据适应度的上升趋势来看，如果适应度曲线上升趋势比较平滑，证明若出现这种情况，末端的位姿变化不会过于突兀，如果在中间具有小横线然后再上升，末端的迭代运动可能会停止在某一位姿，然后再突然运动，因为，我们最希望的在求取机械臂逆解的适应度曲线应该是一条快速上升的，并且平滑的曲线，这样末端在迭代时速度快且运动比较平滑，不抖。
虽然上述图9、图10的曲线是还可以的，但这仅仅反映了末端位姿，在其其余关节上可能会发生非常大的突变，这对于实际的控制是非常不可取的，这主要是因为在上述三个操作中的交叉操作出现的问题，对粒子间的数据进行按一定概率的交换，这样可能会导致角度的突变。
针对这一问题，提出了以下几点改进的想法：
1.将牛顿-拉夫逊法与差分进化算法进行结合，利用牛顿拉夫逊法在前面范围的快速运动，在靠近期望位姿时牛顿拉夫逊法却迭代缓慢，这时利用差分进化算法进行搜索，快速找到期望位姿
2.在差分进化算法的初始粒子的设置方面，将牛顿拉夫逊法的最后的迭代的各关节的角度数据输出，在此角度数据的基础下进行随机的增加或减少一点增量，因为靠近期望位姿，所以增加这一点的增量范围的角度应该在期望位姿附近，然后利用差分进化算法进行快速搜索，这样，就可以得到理想位姿，并且，因为角度的数值差异不大，所以机械手各关节的变动不会很大，在可以接受的范围内。这样我们不仅可以避免掉牛顿拉夫逊法在靠近期望位姿的末端时缓慢的迭代，我们还可以利用差分进化算法的快速性，快速搜索期望位姿，也避免了其各关节角度的突变。总的来说，就是两种算法之间的互补。
3.在参数设置方面，我们对F的参数尽量设的小，因为对粒子差异度的需求并不大，最优解在初始粒子附近，所以F参数的设置不大。在交叉因子CR上的设置，我们可以加大交叉因子的CR的值，使发生交叉的概率增大，提高粒子的多样性，这样一边更快的寻找到最优粒子。
三、基于强化学习的机械臂末端运动
有关这部分内容比较简陋，主要是看了一个强化学习在迷宫中的最优路径应用，即，丢一个机器人在一个迷宫中，这个机器人在经过训练后，无论出于迷宫的哪个位置，都可以按最优路径走出迷宫。这里使用的是Q-learning的算法，如果我没记错的话。。。，太久远了，这东西。
先列出如下图表
本次学习的路径就是要从0/1/2/3/4房间走出去，即走到5，上述图11位原始问题，图12位简化后的问题，图13位这个问题的无向图，相连代表可以通行。
先介绍一些术语：
agent为智能体，这个智能体可以看做一个小机器人，agent是输出action的执行者。
environment为环境，这个环境可以视为上图中的这些房间。
action at{at= a1,…,an}，action为行为也可称动作，即智能体采取的行为，智能体采取的行为可以有n种，为a1/a2/…/an。
reward为奖励，奖励可以是离散的，也可以是线性的。如果奖励是离散的，离散的奖励可以形成一个奖励矩阵，也可以用分段函数表示等等，线性的奖励可以是一个奖励函数，如果奖励是一个函数，这样是即为方便的。说简单点，经训练后可以得到一张奖励表，小机器人走出最优路径，就是从奖励表中获取最大的奖励值（这个跟贪心算法有点不同，因为这个奖励值是看全局的）
还有一个是state，状态，也可以叫观测observation，有很多种情况，这个就是知道小机器人当前处于哪一个房间，执行了一个动作后，小机器人会从state1（房间a）转入到state2（房建2）。
步入正题：
现在我们就根据强化学习中的其中一种方法做路径寻优。Qlearning的核心是根据上述的那些概念生成一个Q表（奖励表），而机器人的action根据Q表选择
在算法实现前，我们先做一些准备工作：
1.根据图11或图13生成奖励矩阵
2.生成初始的Q表，以备更新
3.设定一些基本的参数数值
下面讲一下Q-learning的算法步骤：
1.随机生成一个观测（状态）
2.将当前观测中可通行的行为，全部存起来，并一一对应的Q表中的位置。
3.然后我们随机选择一个可行的动作，注意是可行的动作
4.然后我们根据公式：Q(state, action) = R(state, action) + Gamma * Max[Q(next state, all actions)]进行计算
5.根据3.中的动作，可预见下一个观测
6.如果不满足观测=5，回到2
在上述算法中是一个回合的迭代，这种情况下不足以算出所有的情况，所以在上述的算法步骤的基础上，在外面增加一个大循环，每次都可以随机生成观测，将大循环次数增加至一定数量即可迭代出完整的Q表。
最后根据奖励矩阵R和算法，迭代出的Q表如下：
出来了Q表，选取最优的Q值，进行行为选择，就是看着这些值，哪个值大选哪个。
然后根据上面的图以及选取策略，我们可以有以下图
dot0代表观测0，而down（1）代表相应的行为，1表示相应行为的编号，方便后面编程
有图17和图16我们可以进行编程，有如下流程图
最终效果可以在视频中观看。相应的主程序test3也在程序里，一些程序功能也编写在相应的说明文件中。
在本文中为什么生成Q表使用的是Python程序，而不是MATLAB程序，以及为什么不在途中训练机器人，主要原因有如下：
1.Python的运行效率较MATLAB高（其实，就是把环境抽象出来，得到结果，要是真把机器人放进去训练，带上逆运动学的算法迭代，这个不知道要迭代到什么时候才训练处一张Q表。。。）
2.加入机器人后的训练，这样虽然训练过程非常直观，但是同时因为大量的程序的嵌入，导致运行效率将会非常低。
考虑到上述两点，所以将问题抽象出来在Python中训练得到Q表，最后再对策略进行编程，加大时间效用。
如果想让机器人训练过程比较直观，只需在Q-learning算法步骤中添加机器人的程序再作部分修改即可。
四、代码
下面贴出关键的代码：
差分进化算法：
-------------------------------------------差分进化算法主程序chap8_16-------------------------------------------
clear all;
close all;
tic
global BestS
Size=50;
CodeL=6;
MinX=-90;
MaxX=90;
G=100;
F=0.3; %变异因子[0,2]
cr=0.9; %交叉因子[0.6,0.9]
%初始化种群
for i=1:1:CodeL
P(:,i)=MinX+(MaxX-MinX)*rand(Size,1);
if CodeL == 3
P(:,i) = 0 + (350 - 0) * rand(Size,1)
end
end
BestS=P(1,:); %全局最优个体
for i=2:Size
if(chap8_16obj( P(i,1),P(i,2),P(i,3),P(i,4),P(i,5),P(i,6))>chap8_16obj( BestS(1),BestS(2),BestS(3),BestS(4),BestS(5),BestS(6)))
BestS=P(i,:);
end
end
fi=chap8_16obj( BestS(1),BestS(2),BestS(3),BestS(4),BestS(5),BestS(6));
%进入主要循环，此处无精度要求，循环走完代数G即可
for kg=1:1:G
time(kg)=kg;
%变异
for i=1:Size
r1 = 1;r2=1;r3=1;
while(r1 == r2|| r1 == r3 || r2 == r3 || r1 == i || r2 ==i || r3 == i )
r1 = ceil(Size * rand(1));
r2 = ceil(Size * rand(1));
r3 = ceil(Size * rand(1));
end
h(i,:) = P(r1,:)+F*(P(r2,:)-P(r3,:));
for j=1:CodeL%检查位置是否越界
if h(i,j)<MinX
h(i,j)=MinX;
elseif h(i,j)>MaxX
h(i,j)=MaxX;
end
end
%交叉
for j = 1:1:CodeL
tempr = rand(1);
if(tempr<cr)
v(i,j) = h(i,j);
else
v(i,j) = P(i,j);
end
end
%选择
if(chap8_16obj( v(i,1),v(i,2),v(i,3),v(i,4),v(i,5),v(i,6))>chap8_16obj( P(i,1),P(i,2),P(i,3),P(i,4),P(i,5),P(i,6)))
P(i,:)=v(i,:);
end
%判断和更新
if(chap8_16obj( P(i,1),P(i,2),P(i,3),P(i,4),P(i,5),P(i,6))>fi)
%判断当此时的位置是否为最优的情况
fi=chap8_16obj(P(i,1),P(i,2),P(i,3),P(i,4),P(i,5),P(i,6));
BestS=P(i,:);
end
end
Best_f(kg)=chap8_16obj( BestS(1),BestS(2),BestS(3),BestS(4),BestS(5),BestS(6));
end
toc
disp(['运行时间: ',num2str(toc)]);
BestS %最佳个体
Best_f(kg)%最大函数值
figure(1);
plot(time,Best_f(time),'k','linewidth',2);
xlabel('Times');ylabel('Best f');
-------------------------------------------------------------结束-------------------------------------------------------------
----------------------------------------------适应度程序chap8_16obj-------------------------------------------------
function J=evaluate_objective(th1,th2,dz,th4,th5,th6) %计算函数值
global Link
init_dh_list;
Link(2).th=th1*pi/180; %link(2)='th', 90*ToRad 0.01745/度 1-2
Link(3).th=th2*pi/180; %link(3)='th', 0*ToRad, 0.01745/度 2-3
Link(4).dz=dz+250; %link(4)='th', 90*ToRad 0.01745/度 3-4
Link(5).th=th4*pi/180; %link(5)='th', 0*ToRad 0.01745/度 4-5
Link(6).th=th5*pi/180; %link(6)='th', 0*ToRad 0.01745/度 5-6
Link(7).th=th6*pi/180; %for initial position 'th', 0*ToRad 6-E
for i=1:7
Matrix_DH_Ln(i); %这个是构建Ｄ－Ｈ变换矩阵和无位置信息的旋转矩阵，各关节变换矩阵
end
for i=2:7
Link(i).A=Link(i-1).A*Link(i).A; %右乘变换矩阵
Link(i).p= Link(i).A(:,4);
Link(i).n= Link(i).A(:,1);
Link(i).o= Link(i).A(:,2);
Link(i).a= Link(i).A(:,3);
Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];
end
x_now=Link(7).p(1);
y_now=Link(7).p(2);
z_now=Link(7).p(3);
R = Link(7).R;
R_taget = [1 0 0;0 1 0;0 0 1];
p_err = [-350 - x_now;
-200 - y_now;
400 - z_now];
R_err = R' * R_taget;
Err_th = acos((R_err(1,1) + R_err(2,2) + R_err(3,3) - 1)/2);
r_err = (Err_th / (2 * sin(Err_th)+10^(-10)))^2 * ... %数值下溢
[R_err(3,2)-R_err(2,3); R_err(1,3)-R_err(3,1); R_err(2,1)-R_err(1,2)];
err = norm(r_err) + norm(p_err);
J = 1/err ;
end
-------------------------------------------------------------结束-------------------------------------------------------------
Q-learning
import numpy as np
# init Q matrix is zeros
q = np.matrix(np.zeros([6, 6]))
#The Reward matrix is defined in advance.Similar to the generating matrix of HMM.Minus one means no connected edges
r = np.matrix([[-1, -1, -1, -1, 0, -1],
[-1, -1, -1, 0, -1, 100],
[-1, -1, -1, 0, -1, -1],
[-1, 0, 0, -1, 0, -1],
[ 0, -1, -1, 0, -1, 100],
[-1, 0, -1, -1, 0, 100]])
# hyperparameter
#discount factor
gamma = 0.8
#wheather the probability of choosing the final strategy
epsilon = 0.4
# the main training loop
for episode in range(101):
# random initial state
state = np.random.randint(0, 6)
# If it's not the final state
while (state != 5):
# Select possible actions
# Even in random case, we cannot choose actions whose r[state, action] = -1.
possible_actions = []
possible_q = []
for action in range(6):
if r[state, action] >= 0:
possible_actions.append(action)
#Gets the aciton append in possible_actions
possible_q.append(q[state, action])
#Gets the state and action of the Q table append in possible_q
# Step next state, here we use epsilon-greedy algorithm.
action = -1
if np.random.random() < epsilon:
#if the random value less than epsilon
# choose random action
action = possible_actions[np.random.randint(0, len(possible_actions))] #choose the random action in list of possible_actions
else:
# greedy
action = possible_actions[np.argmax(possible_q)]
#return the index(the value of action) of the largest number in possible_q
# Update Q value
q[state, action] = r[state, action] + gamma * q[action].max()
#updata list Q
# Go to the next state
state = action
# Display training progress
if episode % 10 == 0:
print("------------------------------------------------")
print("Training episode: %d" % episode)
print(q)
五、总结
里面一些有关机器人
1.机器人的D-H建模
2.机器人的正运动学
3.机器人的逆运动学
4.差分进化算法
5.强化学习的一些推导
这些基础的内容，有空再写吧。
扫一扫
1092
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
