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）。
4.3 实验结果
图5数值法画直线 图6数值法画正弦曲线
图5的数值法画直线，其初始位置不定，但是其姿态是固定的，所以设定末端的终点（-350,-200,400），姿态不变，最终效果如图5所示，为一条直线
图6为一条正弦曲线，初始位置为(-350,-200,400)，姿态R=[100;010;001]，这条正弦曲线是z=400，R不变的情况上画的，具体情况是：1.建立正弦曲线函数2.对函数数据点进行采样，采120个样本3.然后用数值法尽心画线。最终效果如图6所示。
5 有关机器人一些其他的工作
5.1 差分进化算法辨识逆运动学解
百度介绍：
差分进化算法(Differential Evolution Algorithm，DE)是一种高效的全局优化算法。它也是基于群体的启发式搜索算法，群中的每个个体对应一个解向量。差分进化算法的进化流程则与遗传算法非常类似，都包括变异、杂交和选择操作，但这些操作的具体定义与遗传算法有所不同。
差分进化算法(Differential Evolution，DE)由Storn和Price于1995年首次提出。主要用于求解
实数优化问题。该算法是一类基于群体的自适应全局优化算法，属于演化算法的一种，由于其具有结构简单、容易实现、收敛快速、鲁棒性强等特点，因而被广泛应用在数据挖掘、模式识别、数字滤波器设计、人工神经网络、电磁学等各个领域。1996年在日本名古屋举行的第一届国际演化计算(ICEO)竞赛中，差分进化算法被证明是速度最快的进化算法。 和遗传算法一样，差分进化算法也是一种基于现代智能理论的优化算法，通过群体内个体之间的相互合作与竞争产生的群体智能来指导优化搜索的方向。该算法的基本思想是：从一个随机产生的初始种群开始，通过把种群中任意两个个体的向量差与第三个个体求和来产生新个体，然后将新个体与当代种群中相应的个体相比较，如果新个体的适应度优于当前个体的适应度，则在下一代中就用新个体取代旧个体，否则仍保存旧个体。通过不断地进化，保留优良个体，淘汰劣质个体，引导搜索向最优解逼近。
图7差分进化算法运算流程 图8差分进化算法的三个操作
5.2 关于本次报告中使用差分算法的一些理解与想法
1.首先对图8差分进化算法的三个操作进行基本的讲解
2.对下面的图9、图10差分进化算法的每代最佳适应度的曲线的一些讲解以及理解 3.对差分进化算法应用在求逆运动学解上的一些想法，即对差分进化算法的一些改进的想法 一、首先对图8差分进化算法的三个操作进行基本的讲解
图8中第一个式子：差分进化算法的变异，其中F为变异因子，变异因子的大小的选取在后面（二）中会有解释。其中1/2/3为随机的三个粒子，根据S_N = S_1 + F*(S_2 - S_3)来产生新的粒子S_N。第二个式子：差分进化算法的操作，上下两行为两个粒子里面的数据，其中new为刚生成的粒子、now为当前size循环下的粒子，通过一些随机概率，将两粒子之间的数据进行交换，生成新的粒子v，。第三个式子:将当前粒子与新生成的粒子v进行适应度fi对比，适应度高的留下，低的被淘汰。
图9 F=0.1时的fi曲线 图10 F=0.3时的fi曲线
二、对下面的图9、图10差分进化算法的每代最佳适应度的曲线的一些讲解以及理解 F为变异参数，F设置的较小时，群体的差异度减小，收敛过早 从图9来看F=0.1时收敛过早，并且适应度低，效果不好
图10 F = 0.3时最后迭代完也还未收敛，但是适应度相较于图9的效果好。 从上述两图的fi适应度曲线来看，因为我将适应度取期望位姿的倒数，所以根据适应度的上升趋势来看，如果适应度曲线上升趋势比较平滑，证明若出现这种情况，末端的位姿变化不会过于突兀，如果在中间具有小横线然后再上升，末端的迭代运动可能会停止在某一位姿，然后再突然运动，这样的效果并不好，所以，我们最希望的在求取机械臂逆解的适应度曲线应该是一条快速上升的，并且平滑的曲线，这样末端在迭代时速度快且运动比较平滑，不抖。
三、对差分进化算法应用在求逆运动学解上的一些想法，即对差分进化算法的一些改进的想法
虽然上述图9、图10的曲线是还可以的，但这仅仅反映了末端位姿，在其其余关节上可能会发生非常大的突变，这对于实际的控制是非常不可取的，这主要是因为在上述三个操作中的交叉操作出现的问题，对粒子间的数据进行按一定概率的交换，这样可能会导致角度的突变。
针对这一问题，提出了以下几点改进的想法
1.将牛顿-拉夫逊法与差分进化算法进行结合，利用牛顿拉夫逊法在前面范围的快速运动，在靠近期望位姿时牛顿拉夫逊法却迭代缓慢，这时利用差分进化算法进行搜索，快速找到期望位姿
2.在差分进化算法的初始粒子的设置方面，将牛顿拉夫逊法的最后的迭代的各关节的角度数据输出，在此角度数据的基础下进行随机的增加或减少一点增量，因为靠近期望位姿，所以增加这一点的增量范围的角度应该在期望位姿附近，然后利用差分进化算法进行快速搜索，这样，就可以得到理想位姿，并且，因为角度的数值差异不大，所以机械手各关节的变动不会很大，在可以接受的范围内。这样我们不仅可以避免掉牛顿拉夫逊法在靠近期望位姿的末端时缓慢的迭代，我们还可以利用差分进化算法的快速性，快速搜索期望位姿，也避免了其各关节角度的突变。总的来说，就是两种算法之间的互补。
3.在参数设置方面，我们对F的参数尽量设的小，因为对粒子差异度的需求并不大，最优解在初始粒子附近，所以F参数的设置不大。在交叉因子CR上的设置，我们可以加大交叉因子的CR的值，使发生交叉的概率增大，提高粒子的多样性，这样一边更快的寻找到最优粒子。
5.3 基于强化学习的路径寻优及程序流程图
图11原始问题 图12简化后的问题 图13问题的无向图 本次学习的路径就是要从0/1/2/3/4房间走出去，即走到5，上述图11位原始问题，图12位简化后的问题，图13位这个问题的无向图，相连代表可以通行。 我们根据上述的问题，采用的是强化学习的方法去解决。
图14强化学习的一些概念
上述的agent为智能体，这个智能体可以看做一个小机器人，在上图智能体有两输入、一输出，跟上图一样。
environment为环境，在上图环境有一输入、两输出。
action at{at= a1,...,an}，action为行为也可称动作，即智能体采取的行为，智能体采取的行为可以有n种，为a1/a2/.../an。
reward为奖励，奖励可以是离散的，也可以是线性的。如果奖励是离散的，离散的奖励可以形成一个奖励矩阵，也可以用分段函数表示等等，线性的奖励可以是一个奖励函数，如果奖励是一个函数，这样是即为方便的。
还有一个是state，状态，也可以叫观测observation，有很多种情况。
现在我们就根据强化学习中的其中一种方法做路径寻优，叫Q-learning。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表如下：
图15奖励矩阵R 图16经迭代后的Q表
出来了Q表，选取最优的Q值，进行行为选择，就是看着这些值，哪个值大选哪个。 然后根据上面的图以及选取策略，我们可以有以下图
图17观测与行为
dot0代表观测0，而down（1）代表相应的行为，1表示相应行为的编号，方便后面编程 有图17和图16我们可以进行编程，有如下流程图
基于Matlab强化学习路径寻优机器人建模与仿真报告
