基于混沌PSO的SCARA机器人参数辨识

丰 非，扈宏杰

（北京航空航天大学 自动化科学与电气工程学院，北京 100191）

随着我国工业的高速发展，平面关节型SCARA机器人在工业生产的各个领域得到了广泛的应用[1]。建立精确的机器人动力学模型，是提高机器人的运动精度的一个重要因素。而机器人的动力学参数辨识是机器人动力学建模的关键。

在此，基于D-H参数法建立了SCARA机器人的运动学模型，建立了机器人的动力学模型；针对模型中存在的不确定的参数，选取合适的激励轨迹，提出了一种混沌的粒子群算法，对机器人的惯性参数进行辨识，与传统的递推最小二乘法和基本粒子群算法进行对比，通过Matlab仿真实验，验证了改进的混沌粒子群算法能够有效避免粒子群算法的早熟收敛问题，跳出局部最优，极大地提高了计算精度和全局寻优能力。

1 运动学建模

基于D-H参数方法[2]，对机器人各个连杆坐标系进行描述，如图1所示。SCARA机器人的第1关节、第2关节以及第4关节为转动关节，第3关节为移动关节。其中机器人的D-H参数见表1[3]。

图1 SCARA机器人连杆坐标系Fig.1 Link frames of SCARA

表1 SCARA机器人的D-H参数Tab.1 D-H link parameters of SCARA

由连杆变换公式、矩阵公式和变换传递式，可以得到第4关节到基坐标系的齐次变换矩阵[4]为

式中：t1=cos（θ1+θ2+θ4）；t2=-sin（θ1+θ2+θ4）；t3=a2cos·（θ1+θ2）+a1cosθ1；t4=sin（θ1+θ2+θ4）；t5=cos（θ1+θ2+θ4）；t5=cos（θ1+θ2+θ4）；t6=a2sin（θ1+θ2）+a1sinθ1；t7=d1+d2+d3

通过调用Matlab中的 Robotic Toolbox[5]，可以直观地将建立的运动学模型表示出来。通过调节4个关节的各个变量，可以看到机器人形态的变化，根据输入的各个关节变量和第4关节末端的位置，验证机器人运动学建模的正确性。

2 动力学建模

选用Lagrange法对SCARA机器人进行动力学建模[6]。Lagrange方程从能量的角度上对系统进行分析，通过Lagrange方程可以将忽略摩擦的机器人的动力学模型写为

式中：M（θ）为惯性力项；C（θ，θ˙）为科氏力和离心力项；G（θ）为重力项。

由于机械臂的后2个关节重量远远小于前2个关节，因此在建立动力学模型的时候，可以简化模型计算，将机器人的末端2个关节固连在第2个关节上，视为负载。这样，机器人模型就简化为一个平面双臂机器人模型，同时由于机械臂只在水平面内运动，重力项的作用可以忽略为0，大大简化了计算复杂度[7]。简化后的动力学方程可以写成二阶形式：

式中：m1，m2分别为SCARA机器人第1和第2关节的质量；I1，I2分别为相对于杆件质心的转动惯量；a1，a2为机械臂的连杆长度；x1，x2为质心位置；θ1，θ2为关节角。

分离式（4）中的运动学参数和惯性参数，将式（3）改写成为Ax=B的线性形式：

式中：ξ的参数为机器人的惯性参数和运动学参数，对于一个给定的机器人，其惯性参数为一给定的常量；为观测矩阵，其中的参数全部由组成，不依赖于机器人的运动学参数和惯性参数。通过计算，可以得到观测矩阵的各个参数为

ξ阵中的各个参数向量为

需辨识的ξ阵中一共有6个参数。在激励轨迹确定的情况下，测量机器人各个关节的力矩输出，就可以通过参数辨识的方法建立确定的机器人动力学模型。

3 参数辨识及激励轨迹设计

设计激励轨迹，使得机器人充分运动，在满足系统实际约束的同时，最大化地激发所有需要辨识的参数，减少辨识的误差，是参数辨识的重要问题之一。

目前常用的激励轨迹一般为傅里叶级数[8]。机器人第 i 个关节的角位移 θi，角速度以及加速度的轨迹为

式中：ωf为傅里叶级数的基频。傅里叶函数的周期为 2π/ωf。 每一个傅里叶级数中包含有（2N+1）个参数，即为 an，bn，θ0。 这里为了减小计算量，选取 N=3，故优化激励轨迹中需要优化的参数共有7个。

在优化激励轨迹之前，应首先使机器人满足实际工作的约束条件。即根据机器人的实际情况，将系统的运动范围、角速度、角加速度限制在一定的范围内，故约束条件为

在满足上述约束后，采用退化矩阵的条件数来优化轨迹各个参数。条件数越接近于1，方程组的状态越好。为此，优化激励轨迹参数的问题就变成了确定系数 an，bn，θ0，使得观测矩阵 W的条件数最小。

为减少计算量，使用遗传优化的方法对参数进行优化，采用Matlab中的遗传算法工具箱进行求解，选择交叉概率pc=0.5，变异概率pm=0.2。经过遗传优化，得到第1关节和第2关节的傅里叶级数参数，见表2。

表2 优化的傅里叶级数参数Tab.2 Optimized fourier parameters

根据优化的参数，获得如图2所示的激励轨迹。由图可见，第1关节和第2关节的角位移、角速度和角加速度均符合机器人的物理约束。

图2 两个关节激励轨迹的角位移、角速度、角加速度Fig.2 Angle，velocity and acceleration of two links on the incentive trajectory

4 混沌PSO方法辨识参数

粒子群算法源于对鸟群捕食行为的研究，是一种基于群体协作的随机搜索算法[9]。PSO算法中，每个粒子都有一个由被优化的函数所所决定的适应度函数，还有一个决定着粒子飞行方向和距离的速度，粒子追随当前的最优粒子在解空间中搜索解[10]。

粒子速度及位置更新公式为

式中：i=1，2，…，m，其中 m 为种群规模；d=1，2，…，G其中G为搜索空间维数；w为惯性权重；c1，c2为学习因子；r1，r2为服从 U（0，1）分布的随机数；pid为局部最优值；pgd为全局最优值。由式（6）知，需要辨识的参数共有6个，因此令粒子的搜索空间维数G=6。

在此利用PSO算法的收敛速度快和混沌运动的遍历性的优点，针对传统的PSO方法中存在的收敛过慢和“早熟”的问题，提出了一种基于混沌优化思想的混沌粒子群（CPSO）算法，改善了PSO算法固有的容易陷入局部最优的缺点，提高了算法的收敛速度和精度。

利用混沌粒子群进行优化的基本思想，主要有以下两方面：①采用混沌序列对粒子的位置和速度进行初始化，通过混沌的方法提高种群的多样性和种群搜索的遍历性；②以目前搜索到的最优位置为基础产生一个混沌序列，用该序列中的最优位置粒子替代当前粒子群中的一个粒子的位置，以此避免陷入局部最优的情况出现[11]。

在粒子信息初始化的阶段，随机产生1个六维每个分量数值在0～1之间的向量，选用Logisitc方程对其进行混沌初始化，得到6个混沌向量，然后将这六个向量载波到对应变量的取值区间中去，得到初始解。

Logisitc方程为

式中：μ为控制参量，取 μ＝2；设 0≤sn≤1。在每一次迭代结束前，对得到的最优位置进行混沌优化。将pgd（i=1，2，…，6）映射到 Logisitc 方程的定义域［0，1］；si=（pgd-xmin）（xmax-xmin）；用 Logisitc 方程进行迭代，产生关于每一个粒子的pgd的混沌序列；将产生的混沌序列通过逆映射返回到原解空间，从而得到一组新的pgd；对其进行适应度计算，从这一组pgd中选取性能最好的作为全局最优值，并将该组数据pgd取代当前群体中的任意一个粒子的位置，继续参与迭代，直至满足误差要求或完成迭代为止。

5 仿真结果与分析

对机器人参数进行辨识，将适应度函数定义为

分别使用基本PSO算法、混沌PSO算法对系统进行辨识，选取：种群个数为200个；学习因子c1=1.3，c2=1.8；惯性权重w从0.9递减至0.2。设置最大迭代次数为200次。得到的适应度函数变化曲线如图3所示。

图3 CPSO和PSO的适应度函数变化曲线Fig.3 Fitness function changes curve of CPSO and PSO

由图可见，应用混沌序列优化粒子群算法中的粒子初始化过程，能够使系统的初始解更加优秀。同时，在更新速度和位置信息的时候，应用混沌PSO的优化方法可以使得适应度函数的收敛速度大大加快，能够提高对最优值的全局搜索能力。

采用传统的递推最小二乘、基本PSO算法和混沌PSO算法，最终得到的适应度函数值和各个辨识参数见表3。

表3 参数辨识结果Tab.3 Result of parameters ddentification

由表可知，递推最小二乘法辨识的系统模型误差较大，基本PSO和混沌PSO方法对系模型辨识效果较好。采用混沌PSO方法对机器人参数进行辨识，得到的适应度函数最小，得到的模型相比递推最小二乘和基本PSO更符合实际模型。

把采用混沌PSO方法辨识出的各个参数代入式（5），得到基于辨识模型的2个关节的力矩输出，将其与实际模型的力矩输出进行对比，如图4所示。由图可见，2个关节的力矩输出曲线与其实际模型力矩输出曲线均基本吻合，说明采用混沌粒子群算法对机器人模型进行辨识具有较高的准确度。

图4 采用CPSO优化的辨识模型和实际模型力矩输出Fig.4 Torque of actual model and identification model using CPSO

分别将递推最小二乘法、基本粒子群法、混沌粒子群法辨识得到的模型与实际SCARA机器人模型进行比较，得到的输出力矩误差如图5所示。由图可见，采用混沌粒子群算法优化的机器人模型与实际模型最为接近，误差最小。由此进一步验证了混沌PSO算法相比传统的最小二乘和粒子群方法的优越性。

6 结语

对SCARA机器人进行运动学建模和动力学建模，提出了简化的SCARA机器人模型，设计优化的激励轨迹，对该模型进行参数辨识。在传统的PSO算法的基础上，提出了混沌PSO优化算法，将混沌的遍历性和粒子群收敛快的特点相结合。将该算法与递推最小二乘、传统PSO算法进行对比，通过仿真实验，验证其对于机器人模型参数辨识的正确性。仿真结果表明，该改进方法能够有效地改善粒子群算法的早熟收敛问题，跳出局部最优，极大地提高了计算精度和全局寻优能力。

图5 两个关节力矩误差Fig.5 Torque error of two links

[1]刘振宇，赵彬，朱海波.六自由度机械臂分拣系统仿真平台研究[J].机械设计与制造，2013，34（2）：210-213.

[2]Craig J J.Introduction to Robotics：Mechanics and Control[M].3rd ed.Prentice Hall，2005：62-89.

[3] 崔敏其.SCARA机器人的拉格朗日动力学建模[J].机械设计与制造，2013，34（12）：76-78.

[4]贠超.机器人学导论[M].3版.北京：机械工业出版社，2006：59.

[5]Corke P.A robotics toolbox for Matlab[J].IEEE Robotics and Automation Magazine，1996，3（1）：24-32.

[6]Ishibashi N，Maeda Y.Learning of inverse-dynamics for SCARA robot[C]//SICE Annual Conference（SICE），2011 Proceedings of IEEE，2011：1300-1303.

[7]闫昊.SCARA机器人动力学分析及鲁棒性控制研究[D].哈尔滨：哈尔滨工业大学，2013.

[8]刘金琨，沈晓蓉，赵龙.系统辨识理论及Matlab仿真[M].北京：电子工业出版社，2013：203.

[9]Lu Jian’an，Chen Yonghua.Particle swarm optimization （PSO）based topology optimization of part design with fuzzy parameter tuning[J].Computer-Aided Design and Applications，2013，（9）：62-68.

[10]Fan H Y.A modification to particle swarm optimization algorithm[J].Engineering Computations，2002，19（7-8）：970-989.

[11]唐贤伦.混沌粒子群优化算法理论及应用研究[D].重庆：重庆大学，2007.