五自由度摇摆台位置正解方法研究及MATLAB实现

李辉，宋诗

(1.安徽建筑工业学院机电学院，安徽合肥230601;2.中国科技大学信息科学技术学院，安徽合肥230088)

摇摆台实际上是一种并联机构，与串联机构相比具有刚度大、结构稳定、承载能力强、精度高、运动惯性较小、运动学位置反解易求和便于实时控制等优点［1］，具有广阔的应用前景。因此，国内外许多学者开始研究并联机构，从最初的六自由度并联机构到后来的少自由度并联机构，在理论和应用方面都取得了丰富的成果。

位置分析是并联机构分析和研究的基础，一切后续的应用都是从位置分析开始的。所谓位置分析是指求解机构运动平台的位置和姿态与输入杆长度之间的关系。机构的位置分析同时也是机构的速度分析、加速度分析、受力分析以及误差分析等的基础，位置分析包括两个问题，位置正解和位置反解。机构位置正解的任务就是在给定各个移动副位移的情况下，求出上平台在空间的位置和姿态;反之，则为位置反解。位置反解的问题比较简单，利用预先规划好的位姿轨迹再结合机构的尺寸采用坐标变换的方法可以很快求出移动副的位移。而位置正解却比较麻烦，其核心是求解一组维数较多、耦合性强的非线性方程组。目前，位置正解主要有两种解法，封闭解法和数值解法。这两种方法各有优缺点:封闭解法的优点是能够得到方程的全部解，缺点是求解难度很大，并且一种机构一种解法，没有通用性［2］;数值解法的优点是能够方便迅速地对任何机型机构求解，缺点是不能求出所有的位置解，并且最终的结果与所选的初值有直接的关系。

在工程应用中，如何快速的找到合理的解远比找到全部的解有意义，并且文献 ［3-4］指出由杆长驱动的并联机构在其工作空间内仅存在单解。基于此，作者着重讨论数值解法，提出一种粒子群优化算法和Newton迭代法相结合的数值解法，并简单介绍了如何在MATLAB中实现该方法。

1 基于粒子群优化算法的五自由度并联机构位置正解

粒子群优化算法(PSO)是KENNEDY和EBERHART于1995年提出的一种基于群集智能的演化计算技术。该算法具有参数设置少、搜索能力强、并行性好、鲁棒性强等特点，且搜索前期的收敛速度快，计算效率比传统的随机方法高，非常适合工程应用。PSO算法中的每个粒子都是解空间的一个假想解，它根据自身的飞行经验和同伴的飞行经验来调整自己的飞行。其算法核心在于粒子的位置xi和速度vi更新公式:

式中:ω为惯性权重，c1、c2为学习因子，r1、r2为区间［0，1］上的随机数，pbi为第i个粒子的个体极值，gb为全局极值。此外，粒子的运动还受到最大速度的限制，当vi＞vmax时，取vi=vmax。

文中所讨论的并联机构上、下平台以5个分支相连，每个分支中间为移动副，与上平面用球铰相连，与下平面中心点以转动副相连，其余用球铰相连。机构示意图如图1所示。

图1 五自由度摇摆台机构示意图

由位置反解的分析可知，并联机构每支杆的长度li(i=0，1，…，4)由动坐标系相对于定坐标系的3个独立转角θx，θy，θz以及动坐标系原点A0在定坐标系中的坐标P(xA0，yA0，zA0)决定，写成函数表达式:

因为定坐标系的原点处为转动副，所以RPS分支只能在zA0y平面内运动，即xA0≡0，故不予考虑。将式(3)改写成以下形式:

于是，位置正解的问题就转化成求解非线性方程组(4)的问题了。

令:

则有:

向量表示:

式中:a=(θx，θy，θz，yA0，zA0)T，

求解方程 (7)的问题可以转化为等价的函数优化问题

也就是说求解方程 (7)的解的问题就是寻找一向量a0(θx，θy，θz，yA0，zA0)使得P(a0)=0成立的问题。式(8)同时也是采用粒子群优化 (PSO)算法搜索最优解时的适应度函数。

以方程 (7)为例，搜索位姿参数的具体步骤如下:

步骤一，设置种群规模N，在并联机构可达空间内随机初始化各个粒子的位置xi0和速度vi0，并确定最大进化代数Q和与全局极值相匹配的粒子占全部粒子的比例P。

步骤二，根据适应度函数计算每个粒子的适应度。

步骤三，由适应度的大小确定个体最优位置pbi及全局最优位置gb。

步骤四，判断全局最优是否满足精度要求，如果不满足精度要求转到步骤第三;反之，以当前的全局最优位置gb为要寻找的位姿输出。

步骤五，看是否到达最大进化代数Q，如果到达最大进化代数，则以当前的全局最优位置gb为要寻找的合适初始位姿输出。否则，继续步骤六。

步骤六，根据PSO迭代公式(1)、(2)分别对群体的位置xi和速度vi进行更新。更新完以后转到步骤二。

2 并联机构位置正解方法的改进

由于PSO算法在搜索后期的收敛速度低，且容易陷入局部最优，既耗时又不精确。所以在搜索后期作者用经典的具有较高精度的Newton迭代法取而代之，虽然Newton迭代法计算量大，但是由于之前的PSO算法已经将搜索结果限定在离目标值很近的范围内，所以Newton迭代法只需要很少的几步迭代就可以使结果在误差范围以内，对于实时性的影响并不大，可以同时满足精度和实时性的双重要求。

为了说明问题的方便，文中提出匹配的概念，解空间的任意两个粒子xi和xj之间的距离σ(xi，xj)为:

对于给定的常数ε＞0，如果有σ(xi，xj)＜ε，则称粒子xi和xj匹配。

通过大量的仿真试验，发现当种群中有一半以上的粒子与当前全局最优粒子匹配的时候，可以认为PSO算法进入到后期搜索，此时以当前全局最优解为Newton迭代法的初始值。

对方程(7)采用Newton迭代法可得:

即:

在利用粒子群优化算法搜索到合适的初始位姿a0后，将a0代入式 (11)，反复利用上式进行迭代计算可以很快得到符合精度要求的位置解。

上述这种方法的流程图如图2所示。

图2 算法流程

3 并联机构位置正解的MATLAB实现

3.1 MATLAB粒子群优化算法工具箱简介

PSOt为PSO的工具箱，该工具箱将PSO算法的核心部分封装起来，用户只需要定义好自己需要优化的函数，并设置好函数自变量的取值范围、每步迭代允许的最大变化量等，即可自行优化。

PSO算法工具箱中的核心函数是pso_Trelea_ vectorized()，该函数实现了整个粒子群的初始化、个体最优和全局最优的计算与更新、个体速度和位置的更新。在实际操作过程中，只需要用pso_Trelea_ vectorized()调用已经编写好的目标函数，该函数会自动实现粒子群优化算法的寻优。

3.2 位置正解的PSOt实现

通过上面的分析，以式 (8)为目标函数。根据指标给出约束条件:

3.3 试验结果比较

如图3是使用粒子群优化算法得到的误差曲线，可以看出，执行300次搜索以后误差在0.001 6左右(当然这个误差不是一定的，几乎每次搜索的误差都不一样，这个数据是笔者在做了很多次搜索以后得出的平均值)。对于精度要求不高的系统这个误差是允许的，但是由于每次搜索的误差都不一样，并不能保证每次都能搜索到这么高的精度，所以对于精度要求比较高的系统，这样做是不能满足精度要求的。而且由于搜索时间较长，对于实时性要求较高的系统而言，这个速度不一定能满足要求。此外还可以看出在搜索初期粒子收敛的速度相当快，通过大约50次搜索就可以使误差限定在1以内;然而，再往后的搜索速度就明显变慢了许多，在这种情况下如果还采用PSO算法，那么性价比就会比较低。

图3 PSO算法求解的误差曲线图

图4是采用粒子群优化算法和 Newton迭代法相结合的方式得到的误差曲线，可以看出这种算法具有很高的精度(0.001)，几乎可以满足所有的工程要求，并且速度较快。

图4 PSO结合Newton迭代法求解的误差曲线图

4 结束语

重点探讨了某实用五自由度摇摆台的位置正解，采用粒子群优化算法和Newton迭代法相结合的方法，该方法利用粒子群优化算法的全局搜索能力和前期快速收敛的特点，在整个解空间里快速搜索，并且能够很快搜索到目标值附近。然而，越靠近目标值，PSO算法的收敛速度就越低，且容易陷入局部最优。所以作者在搜索后期直接摒弃了PSO算法，取而代之的是经典的Newton迭代法，这种经典的迭代法求解精度高，可以使最终的解达到理想的精度。然而，Newton迭代法也有计算量大的缺点，不过没有关系，因为在搜索前期，PSO算法已经将搜索结果锁定在目标值附近很小的范围内，所以采用Newton迭代法只要再迭代很少的几次就可以满足精度要求了。试验表明:这种改进有很高的精度和较好的实时性，对多数基于杆长驱动的摇摆台的位置正解具有借鉴意义。

【1】冯志友，李永刚，张策，等.并联机器人机构运动与动力分析研究现状及展望［J］.中国机械工程，2006，17(9): 979-984.

【2】黄真，赵永生，赵铁石.高等空间机构学［M］.北京:高等教育出版社，2006.

【3】LEE T Y，SHIM J K.Improved Dialytic Elimination Algorithm for the Forward Kinematic of the General Stewart-Gough Platform［J］.Mech Theory，2003，3(8):562-577.

【4】CODOUREY Alain.Dynamic Modeling of Parallel Robots for Computed-Torque Control Implementation［J］.The International Journal of Robotics Research，1998，17(12): 1325-1336.

【5】芮钧，陈守伦.Matlab粒子群算法工具箱求解水电站优化调度问题［J］.中国农村水利水电，2009(1):114-116.