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

Method for Position Forward Solution for 5-DOF Swing Platformand Its Implementation Based on M ATLABLI Hui .S0NG Shi(1.Colege of Mechanical and Electrical Engineering，Anhui University of Architecture，Hefei Anhui 23060 1，China；2.School of Information Science and Technology，University of Science and Technology of China，Hefei Anhui 230088，China)Abstract：A practical algorithm for position forward solution of an asymmetric 5-DOF swing platform was studied.The solutionproblem for a set of nonlinear equations which described the 5-DOF swing platform was translated into function optimization.The parti-cle swarnl optimization(PSO)and Newton iteration algorithm were combined to solve the problem.And MATLAB PSO toolbox wasused to implement the position forw ard solution of the 5-DOF swing platform .The comparison of experiment results shows that using thealgorithm combining PSO algorithm with Newton iteration algorithm in is better than just using the PSO algorithm only，both in realtime aspects and precision。

Keywords：5-DOF swing platform ；Position forward solution；Particle swaHn optimization algorithm； Newton iteration algo·rithm：MATLAB implementation亿台实际上是-种并联机构，与串联机构相比具有刚度大、结构稳定、承载能力强、精度高、运动惯性较孝运动学位置反解易求和便于实时控制等优点 j，具有广阔的应用前景。因此，国内外许多学者开始研究并联机构 ，从最初的六 自由度并联机构到后来的少自由度并联机构，在理论和应用方面都取得了丰富的成果。

位置分析是并联机构分析和研究的基础，-切后续的应用都是从位置分析开始的。所谓位置分析是指求解机构运动平台的位置和姿态与输入杆长度之间的关系。机构的位置分析同时也是机构的速度分析、加速度分析、受力分析以及误差分析等的基础，位置分析包括两个问题，位置正解和位置反解。机构位置正解的任务就是在给定各个移动副位移的情况下，求出上平台在空间的位置和姿态；反之 ，则为位置反解。

位置反解的问题比较简单，利用预先规划好的位姿轨迹再结合机构的尺寸采用坐标变换的方法可以很快求出移动副的位移。而位置正解却比较麻烦 ，其核心是求解-组维数较多、耦合性强的非线性方程组。目前，位置正解主要有两种解法，封闭解法和数值解法。这两种方法各有优缺点：封闭解法的优点是能够得到方程的全部解，缺点是求解难度很大，并且-种机构-种解法，没有通用性 ；数值解法的优点是能够方便迅速地对任何机型机构求解，缺点是不能求出所有的位置解 ，并且最终的结果与所选的初值有直接的关系。

在工程应用中，如何快速的找到合理的解远 比找到全部的解有意义，并且文献 [3-4]指出由杆长驱动的并联机构在其工作空间内仅存在单解。基于此，作者着重讨论数值解法，提出-种粒子群优化算收稿日期：2012-03-06基金项目：安徽侍育厅 自然科学重点项 目 (KJ2011A062)作者简介：李辉 (1980-)，女，硕士 ，主要研究方向为机电-体化控制。E-mail：winhh1980###126.eom第 7期 李辉 等：五自由度亿台位置正解方法研究及 MATLAB实现 。37·法和 Newton迭代法相结合 的数值解法 ，了如何在 MATLAB中实现该方法。

并简单介绍 于是，位置正解的问题就转化成求解非线性方程1 基于粒子群优化算法的五自由度并联机构位置正解粒子群优化算法 (PSO)是 KENNEDY和 EBER-HART于 1995年提出的-种基于群集智能的演化计算技术。该算法具有参数设置少、搜索能力强、并行性好、鲁棒性强等特点，且搜索前期的收敛速度快，计算效率比传统的随机方法高，非常适合工程应用。

PSO算法中的每个粒子都是解空间的-个假想解，它根据自身的飞行经验和同伴的飞行经验来调整自己的飞行。其算法核心在于粒子的位置 和速度 V 更新公式：l6 fc1 r1(Pb - )c2r2(gb- ) (1)1 1 (2)式 中： 为惯性权重 ，C。、c：为学习因子 ，r 、r：为区间 [0，1]上的随机数，P 为第 i个粒子的个体极值，g 为全局极值。此外，粒子的运动还受到最大速度的限制，当 ；> -时，取 V 。

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

图1 五自由度亿台机构示意图由位置反解的分析可知，并联机构每支杆的长度(i0，1，，4)由动坐标系相对于定坐标系的 3个独立转角 0 ，0y，0z以及动坐标系原点 A。在定坐标系中的坐标 P( "Y z )决定，写成函数表达式 ：Z (0 ，0 ，0z，Ya ， ) i0，1，，4 (3)因为定坐标系的原点处为转动副 ，所 以 RPS分支只能在 zA。Y平面内运动 ，即 ；0，故不予考虑。

将式 (3)改写成以下形式：(Ox，0 ，0；，YAn， An)-Zf0 i0，1，，4(4)组 (4)的问题了。

令 ：Fi(Ox， ，Oz，YA。，ZA。) ( ，Oy，Oz，Ya。，ZA。)-z(5)则有：F (0 ，0 ，Oz，Ya ， )0 i0，1，，4 (6)向量表示：F(a)0 (7)式中：a (0 ，0 ，0 ，YA ， 。) ，F(a)[Fo(a)，F (a)，F2(a)，F3(a)，F4(a)]T。

求解方程 (7)的问题可以转化为等价的函数优化问题minP(口) l F (口)l (8)也就是说求解方程 (7)的解的问题就是寻找-向量a。(0 ，0，，0 ，YAn' )使得 P(a。)0成立 的问题。式(8)同时也是采用粒子群优化 (PSO)算法搜索最优解时的适应度函数。

以方程 (7)为例，搜索位姿参 数的具体 步骤如下 ：步骤- ，设置种群规模 Ⅳ，在并联机构可达空间内随机初始化各个粒子的位置 和速度 V ，并确定最大进化代数 Q和与全局极值相匹配的粒子占全部粒子的比例 P。

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

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

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

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

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

为了说明问题的方便，文中提出匹配的概念，解空间的任意两个粒子 和 之间的距离 ( ， )为 ：厂 ---------- ( )/∑( - ) (9) k 0对于给定的常数占>0，如果有 or( ，xj)< ，则称粒子 和 ，匹配。

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

对方程 (7)采用Newton迭代法可得：。 - [ ]～ (10)即：。

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

图2 算法流程3 并联机构位置正解的MATLAB实现3.1 MATLAB粒子群优化算法工具箱简介PSOt为PSO的工具箱，该工具箱将 PSO算法的核心部分封装起来 ，用户只需要定义好自己需要优化的函数，并设置好函数自变量的取值范围、每步迭代允许的最大变化量等，即可自行优化。

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

3.2 位置正解的 PSOt实现通过上面的分析 ，以式 (8)为目标函数。根据指标给出约束条件 ：- 0.1 < <0.1 -0.1

而且由于搜索时间较长，对于实时性要求较高的系统而言，这个速度不-定能满足要求。此外还可以看出在搜索初期粒子收敛 图3 PSO算法求解的误差曲线图- - - - - a-a f。d . .a-- - - - - a a . .a-a- - ～ - - f6d -a . . -d第7期 李辉 等：五自由度亿台位置正解方法研究及MATLAB实现 39·的速度相当快，通过大约 50次搜索就可以使误差限定在 1以内；然而，再往后的搜索速度就明显变慢了许多，在这种情况下如果还采用 PSO算法，那么性价比就会 比较低。

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

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