冗余空间机械臂最小基座姿态扰动的轨迹规划方法
【专利摘要】本发明涉及一种基于改进混合粒子群算法的冗余空间机械臂最小基座姿态扰动的轨迹规划方法，其属于航空航天技术领域。本发明的步骤如下：(1)设计冗余空间机械臂的三维模型；(2)建立冗余空间机械臂的运动学模型；(3)利用广义雅克比矩阵建立冗余空间机械臂的运动学方程；(4)采用5阶正弦多项式函数进行关节的参数化；(5)在基座姿态扰动最小的约束条件下建立代价函数方程式；(6)提出改进混合粒子群算法；(7)利用混合粒子群算法对代价函数进行优化求解，得出在约束条件下的规划轨迹。本发明解决了在基座姿态扰动最小约束条件下冗余空间机械臂的轨迹规划问题，利用改进的混合粒子群算法求解的规划轨迹精度高且平稳，效果较好。
【专利说明】
冗余空间机械臂最小基座姿态扰动的轨迹规划方法
技术领域
[0001] 本发明涉及一种冗余空间机械臂最小基座姿态扰动的轨迹规划方法，属于航空航 天技术领域。
【背景技术】
[0002] 随着各个国家在轨服务技术的不断发展，空间机器人的应用越来越广泛，也成为 了学者们研究的热点和难点。当空间机器人执行任务时，为了节约燃料的消耗，姿控系统会 被主动关闭。此时，一般的空间机器人就会处于自由漂浮状态。自由漂浮空间机器人的基座 具有不固定性，机械臂与基座之间存在动力学耦合。当机械臂执行任务时会引起基座姿态 的扰动，反过来，基座姿态的扰动会影响机械臂正常的工作状态。为了保证空间在轨服务的 顺利完成，需要基座姿态保持不变或被控制在一定的范围之内。因此，许多学者对于空间机 械臂的轨迹规划算法进行了研究，以减小基座姿态扰动的影响。
[0003] 对于机械臂的轨迹规划问题，国内外学者提出了多种不同的解决方案。主要包括 有基于Lyaponov双向搜索法、关节周期运动法、增强扰动图法、零反作用法、关节轨迹参数 法和各种智能优化算法。基于Lyaponov函数的方法是由Nakamura和Mukher jee在1989年提 出的。此方法可以同时调整关节角和基座位姿的状态，但是优化后的关节轨迹不够平滑，而 且系统的稳定性在理论上没有被证明。1991年，Dubowsky和Torres提出了一种增强扰动图 法进行空间机械臂的运动规划。此方法可以优化空间机器人的运动轨迹进而减小对基座的 扰动，但是仅对两自由度空间机械臂进行了论证，不能广泛应用于多自由度空间机械臂。在 1993年，Vafa和Dubowsky利用虚拟机械臂的思想提出了一种路径规划方法，即关节周期运 动法。此方法可以使末端执行器到达指定的位姿，但是机械臂的模型与数学模型不太相符 合。基于广义雅克比矩阵，在2001年Yoshida等提出了一种零反作用的思想。在基座姿态无 扰动的约束下进行空间机械臂轨迹规划，但不适用于冗余空间机械臂。在2005年， Papadopoulos提出了一种利用多项式函数对关节角进行轨迹参数化的方法。此方法不受奇 异性的影响，优化后的路径平滑，但是收敛时间过长。Huang等在2006年提出了一种基于遗 传算法的最小基座扰动路径规划。但遗传算法的实现比较复杂且收敛时间较长。2011年，王 明等人提出了基于混沌粒子群算法的基座干扰力矩最小的轨迹规划，但并未考虑基座的位 姿扰动最小。Liu Xiaodong等人在2013年提出了利用一种新型的混合全局优化算法解决冗 余空间机械臂的轨迹规划问题，但此方法的缺陷是在某些情况下计算时间比较长。2014年， 夏红伟等提出基于混沌粒子群算法的基座姿态扰动最小轨迹规划，其应用对象为6自由度 空间机械臂，而未考虑冗余空间机械臂的情况。Wang Mingming等人在2015年阐述了利用改 进的粒子群算法求解7自由度空间机械臂的轨迹参数，而求解的精度不高。
【发明内容】
[0004] 鉴于现有技术中存在的问题，本发明提供一种冗余空间机械臂最小基座姿态扰动 的轨迹规划方法，从冗余空间机械臂的设计开始，并成功地将实际应用问题转化为数学求 解问题，直到利用改进的混合粒子群算法，采用自然选择机理与粒子群算法相结合，快速有 效的解决了最优参数的求解，得到了良好的规划轨迹，提高了精度。
[0005] 为达到上述目的，本发明研究了冗余空间机械臂最小基座姿态扰动的轨迹规划问 题，在基座姿态扰动最小的约束条件下，提出一种解决此问题的方法，并且采用改进的混合 粒子群算法进行参数优化。本发明的技术方案如下：
[0006] (1)基于SolidWorks 2013设计了7自由度冗余空间机械臂的三维模型，7个自由度 均采用旋转关节，采用S-R-S结构，大臂与小臂的比例为1:0.618;
[0007] (2)采用经典的D-H参数法建立冗余空间机械臂的运动学模型；
[0008] (3)为了避免计算过程中奇异性的出现，利用正运动学方程与广义雅克比矩阵推 导出冗余空间机械臂的运动学方程；
[0009] (4)采用5阶正弦多项式函数法对关节角进行参数化；
[0010] (5)在基座姿态扰动最小的约束条件下建立代价函数方程；
[0011] (6)根据自然界中"适者生存，不适者被淘汰"的原则，并且结合多群体协同进化， 提出一种改进混合粒子群算法；
[0012] (7)利用改进的混合粒子群算法求解最优参数，得到最优规划轨迹。
[0013] 所述步骤（1)中，冗余空间机械臂的7个自由度均采用旋转关节，能够满足复杂空 间环境下的在轨服务。其常见的结构有四种，本文采用的S-R-S结构被证明在运动学上是最 优的。根据冗余空间机械臂的设计模型，基于SolidWorks 2013进行三维建模。在建模过程 中采用模块化和自顶向下的设计原则，即从草图设计-工作特征设计-零件设计-装配体。
[0014] 所述步骤(5)中，在基座姿态扰动最小的约束下建立代价函数方程为：
[0015] 0i(t)= A (t)
[0017]其中，范数| |5qb| | =5qbT5qb，为姿态误差的权重系数。
[0018] 所述步骤(7)中，改进的混合粒子群算法策略受由自然界中的生物原则启发，包括 自然选择机理与多群体协同进化。算法的具体实验方案包括：（1)设置参数；（2)随机初始化 三个种群中粒子的速度和位置；（3)每个种群计算每个粒子的代价函数值，将各个粒子自身 最优的位置和代价函数值存储在PU中，将种群的最优位置和代价函数值存储在p gj中；（4) 更新粒子的速度和位置；（5)对每个粒子自身当前的位置与之前最优位置进行比较，如果比 较好，则将其更新为最好的位置，反之，则保留之前最优位置；（6)比较当前所有的 Plj和pgJ， 并且更新口^; (7)将粒子按照代价函数值进行排序，用群体中最好一半粒子的速度和位置替 换最差一半的速度和位置，同时保留每个个体的最优值；（8)选出每个种群中的最优粒子和 最差粒子，3个最优粒子进行以上步骤的迭代选出最佳粒子，然后替换每个种群中的最差粒 子;（9)满足迭代次数，输出最优参数，确定轨迹。
[0019] 本发明与现有技术相比具有以下有益效果:本发明提出了一种合适的解决方案对 于冗余空间机械臂基座扰动最小轨迹规划问题，从冗余空间机械臂的设计开始，并成功地 将实际应用问题转化为数学求解问题，直到利用改进的混合粒子群算法快速有效的解决了 最优参数的求解，得到了良好的规划轨迹，提高了精确度和稳定性。
【附图说明】
[0020] 以下通过附图及具体实施例对本发明进行详细说明。
[0021] 图1本发明技术方案流程图；
[0022] 图2简化设计模型图；
[0023] 图3三维模型图；
[0024]图4改进混合粒子群算法流程图；
[0025]图5基座姿态曲线图；
[0026] 图6关节曲线图；
[0027]图7关节角速度曲线图；
[0028]图8关节角加速度曲线图。
【具体实施方式】
[0029]下面结合附图对本发明作进一步说明，如图1所示。
[0030]本发明的步骤如下：
[0031] (1)基于SolidWorks 2013设计了7自由度冗余空间机械臂的三维模型；
[0032] (2)采用经典的D-H参数法建立冗余空间机械臂的运动学模型；
[0033] (3)利用正运动学方程与广义雅克比矩阵推导出冗余空间机械臂的运动学方程；
[0034] (4)采用5阶正弦多项式函数法对关节角进行参数化；
[0035] (5)在基座姿态扰动最小的约束条件下建立代价函数方程；
[0036] (6)提出一种改进的混合粒子群算法；
[0037] (7)利用改进的混合粒子群算法求解最优参数，得到最优规划轨迹。
[0038] 实施例1
[0039]本发明的实施例是在以本发明技术方案为前提下进行实施的，给出了详细的实施 方式和具体的操作过程，但本发明的保护范围不限于下述实施例。
[0040] 具体实施步骤如下：
[0041] 步骤1:本发明的研究对象是冗余空间机械臂，它的7个自由度均采用旋转关节，能 够满足复杂空间环境下的在轨服务。其常见的结构有四种，本文采用的S-R-S结构被证明在 运动学上是最优的。其简化的设计模型如图2所示。根据冗余空间机械臂的设计模型，基于 SolidWorks 2013进行三维建模。在建模过程中采用模块化和自顶向下的设计原则，即从草 图设计_工作特征设计-零件设计-装配体。冗余空间机械臂的三维模型如图3所示。冗余空 间机械臂的三维可视化建模不仅形象生动，更能让人们有直观的了解和认知。
[0042] 步骤2:本发明采用D-H参数法建立机械臂的运动学模型。冗余空间机械臂的D-H参 数如表1所示。
[0043] 表1冗余空间机械臂的D-H参数表
[0045] 其中，ai-hdi和ai-邊三个固定不变的连杆参数，0i是关节变量。 ai-:表示沿Xi-i轴， 从Zi-i移动到Zi的距离;di表示沿Zi轴，从Xi-i移动到Xi的距离;表示绕Xi-4S，从Zi-i旋转 到冗:的角度，逆时针为正;0:表示绕Z4S，从Xh旋转到&的角度，逆时针为正。
[0046] 空间机器人系统由空间机械臂、基座和卫星组成，本文所采用的空间机器人质量 特性参数如表2所示。
[0047]表2空间机器人质量特性参数
[0048]
[0049] 步骤3:冗余空间机械臂的运动学分析是其他空间机械臂技术研究的基础和前提。 正运动学分析是在已知空间机械臂连杆参数和关节变量的情况下，求末端执行器相对于参 考坐标系的位姿解。冗余空间机械臂相邻坐标系之间转换矩阵的一般表达式为：
[0051]将7自由度空间机械臂和基座看作一个系统，在惯性坐标系内机械臂末端执行器 的位置矢量可以表不为如下式子：
[0053]其中，Pe G R3X3表示末端执行器相对于惯性坐标系的位置矢量，rQ G R3X3表示基座 质心相对于惯性坐标系的位置矢量，bQeR3x3表示从基座质心指向第一个关节的位置矢量， PleR3x3表示第i个关节相对于惯性坐标系的位置矢量，po表示基座相对于惯性坐标系的位 置矢量。
[0054]对上式进行微分可以得到末端执行器的线速度，如下式所示：
[0056]其中，V(5 G R3表示某一时刻末端执行器的线速度，VQ G R3表示某一时刻基座的线速 度，《QGR3表示某一时刻基座的角速度，kiGR3表示在惯性坐标系里第i个坐标系Z轴的方 向向量。
[0057]对上式进行微分可以得到末端执行器的角速度，如下式所示：
[0059] 其中，c〇eGR3表示某一时刻末端执行器的角速度。
[0060] 利用广义雅克比矩阵对7自由度空间机械臂进行运动学建模。机械臂的雅克比矩 阵如下式所示：
[0062]其中，Jm表示机械臂的雅克比矩阵。
[0063]基座的雅克比矩阵下式所示：
[0065] 其中，Jb表示基座的雅克比矩阵，E表示单位矩阵，pQe = pe-r0。
[0066]结合以上式子可推导出7自由度空间机械臂的运动学方程，如下式所示：
[0068]其中，0表示关节角矩阵。
[0069]由于自由漂浮空间机械臂的整个系统不受外力，假设系统初始的线动量和角动量 都为〇，则根据动量守恒定律可以推导出如下式所示：
[0071 ]其中，Ib表示基座的惯性矩阵，Im表示机械臂的惯性矩阵。
[0072]则自由漂浮空间机械臂系统的广义雅克比矩阵可表示为如下式所示：
[0074] 其中，Ibm表示耦合惯性矩阵，J(K 9，mi，Ii)表示系统的广义雅克比矩阵池表示基 座的姿态，mi表示第i个连杆的质量，Ii表示第i个连杆的惯量矩阵。
[0075] 步骤4:在本发明中5阶正弦函数多项式法被用来进行关节角ejt)的参数化，如下 式所述：
[0076] 9i(t) = A iisin(ai5t5+ai4t4+ai3t3+ai2t2+aiit+ai〇) +A i2
[0077] 其中，i = l，2,…，7，aiQ~&15是多项式系数，A u与A i2的表达如下式所示：
[0079] 其中，0^_为第i个关节角的最小值，为第i个关节角的最大值。
[0080] 则关节角速度、角加速度可分别表示为如下式子：
[0083] 在运动过程中，空间机械臂的约束条件如下式所示：
[0084] 0i(t〇) = 0i〇，0i(tf) = 9if
[0085] 0： (/") = 6^ (/; ) = 0,0, (/,) ：= 0, (/, ) = 0
[0086]其中，01()为初始关节角，0^为期望关节角。
[0087]将以上约束条件式代入式关节角、角速度和角加速度中得：
[0089 ] ai3 = 5/3ai5tf2, ai4 = -5/2aistf
[0090] 由以上式子可知，只有ai5为未知变量，其可以表达为如下式子：
[0091] a=[ai5，...，a75]
[0092] 因此当未知变量a确定时，冗余空间机械臂的轨迹就可以被规划了。这样，一个实 际的应用问题就被转化成了数学求解问题。
[0093] 步骤5:本发明采用四元数法对基座的姿态进行描述与表达。四元数法的方程如下 式所示：
[0094] Qb= [n,qi]
[0095] 其中，n为四元数的标量部分，qi(i = l，2,3)为四元数的矢量部分。它们可以表达 为如下式所示：
[0096] /； = cos(^>/2),q = ksin(^/2)
[0097] 其中，梦为坐标系绕着欧拉轴所转过的角度，k为欧拉轴单位方向的向量。
[0098] 四元数法的约束条件如下式所示：
[0099] q2+qi2+q22+q3 2= 1
[0100] 通过四元数与角速度之间的关系推导出用四元数表示的基座姿态的误差，如下式 所示：
[0102] 本发明是求解在基座姿态扰动最小的约束条件下冗余空间机械臂的轨迹规划，则 代价函数如下式所示。
[0103] 9i(t) = A (t)
[0105]其中，范数I |5qb| I =5qbT5qb，Oq为姿态误差的权重系数。
[0106] 步骤6:在实际应用中，粒子群算法的缺陷是易陷入局部最优或求解精度不高。为 了克服易陷入局部最优的缺点，提出一种多群体协同进化的方法。此方法受自然界生物系 统中"适者生存"的原则，具体策略包括3步，分别是（1)选择三个种群进行初始化，分别选出 每个种群中最优与最差的粒子；（2)选出的三个最优粒子组成一个新的种群并选出最佳粒 子；（3)用最佳粒子替换三个种群的最差粒子。这样群体之间的信息共享与交流在一定程度 上就避免了陷入局部最优的危险。为了提高求解精度，采用自然选择机理与粒子群算法相 结合。自然选择就是生物在生存斗争中适者生存，不适者被淘汰现象的体现。具体应用时的 基本思想是每次迭代时将粒子按代价函数值进行排序，用群体中最好一半粒子的速度和位 置替换最差一半的速度和位置，同时保留每个个体的最优值。
[0107] 步骤7:利用改进混合粒子群算法求解冗余空间机械臂最小基座姿态扰动轨迹规 划问题的具体实验步骤如下：
[0108] (1)设置参数:粒子数目为N，惯性权重为《，学习因子分别为cdPc2，最大迭代次数 为M，自变量的个数为D;
[0109] (2)随机初始化三个种群中粒子的速度和位置；
[0110] (3)每个种群计算每个粒子的代价函数值，将各个粒子自身最优的位置和代价函 数值存储在PU中，将种群的最优位置和代价函数值存储在Pg中； (4)更新粒子的速度和位置；
[0112] (5)对每个粒子自身当前的位置与之前最优位置进行比较，如果比较好，则将其更 新为最好的位置，反之，则保留之前最优位置；
[0113] (6)比较当前所有的pij和pgj，并且更新pgj;
[0114] (7)将粒子按照代价函数值进行排序，用用群体中最好一半粒子的速度和位置替 换最差一半的速度和位置，同时保留每个个体的最优值；
[0115] (8)选出每个种群中的最优粒子和最差粒子，3个最优粒子进行以上步骤的迭代选 出最佳粒子，然后替换每个种群中的最差粒子；
[0116] (9)满足迭代次数，输出最优位置a = [a15，…，a75 ]，确定轨迹。
[0117] 基于改进混合粒子群算法的冗余空间机械臂最小基座姿态扰动轨迹规划的算法 流程如图4所示。
[0118] 为了验证本发明所提出方法的有效性，以步骤1所描述的冗余空间机械臂为研究 对象，利用平台MATLAB R2013b对其进行实验与仿真。参数设置如下：
[0119] 基座的初始与期望姿态相等如下式所示：
[0120] Qb〇 = Qbf=[l 0 0 0]T
[0121] 冗余空间机械臂关节角的范围如下式所示：
[0124]冗余空间机械臂关节角速度与关节角加速度的范围如下式所示：
[0126] 代价函数中的权重系数如下式所示：
[0127] coq = sin(3i/360)
[0128] 本发明提出改进混合粒子群算法的参数设置下式所示：
[0129] N=30,ci = C2 = 2, ? = 0.5,D = 7,M=100
[0130] 为了公平的比较本文改进混合粒子群算法、粒子群算法和线性递减权重粒子群算 法的有效性，所有实验独立运行50次。通过实验仿真，求得的最优A、最差A、A的平均值和 方差如表3所示。
[0131] 表3仿真结果
[0132]
[0133] 在表3中，加粗表示最好的结果，最优结果表示50次运行中得到的最小值，最差结 果表示得到的最大值，平均值表示了整体水平值，方差表示了结果的稳定性。通过表3中的 数据可以表明，采用本发明改进混合粒子群算法得到的结果比其他两种算法均好。因此，本 发明所提出的改进混合粒子群算法在求解冗余空间机械臂最小基座姿态扰动轨迹规划问 题上更加有优势。
[0134] 通过本发明所得到的优化参数a和最优值A如下式所示：
[0136] 在整个轨迹规划过程中，基座的姿态变化如图2所示，冗余空间机械臂的关节角、 角速度和角加速度分别如图3、图4和图5所示。基座的姿态值如下式所示：
[0137] Qba=[l 1.07e-13 7.7e-14 1.18e-13]T
[0138] 仿真实验结果表明本发明所提方法的有效性。该方法有效的解决了冗余空间机械 臂最小基座姿态扰动轨迹规划问题，并且约束了关节角、角速度和角加速度。该方法有较强 的实用价值，对于其他类型的轨迹规划问题具有一定的指导意义。同时，与粒子群算法及线 性递减权重粒子群算法相比较，本发明提出的改进混合粒子群算法在优化求解时有着更高 精度和更加稳定的优势。
[0139] 以上所述，仅为本发明较佳的【具体实施方式】，但本发明的保护范围并不局限于此， 任何熟悉本技术领域的技术人员在本发明披露的技术范围内，根据本发明的技术方案及其 发明构思加以等同替换或改变，都应涵盖在本发明的保护范围内。
【主权项】
1. 冗余空间机械臂最小基座姿态扰动的轨迹规划方法，其特征在于:此方法包括如下 步骤： 步骤一:基于Sol idWorks进行冗余空间机械臂的三维模型设计； 步骤二:采用D-H参数法建立冗余空间机械臂的运动学模型，利用广义雅克比矩阵进行 运动学方程的建立，采用5阶正弦多项式函数进行关节的参数化； 步骤三:在基座姿态扰动最小的约束下建立代价函数方程； 步骤四：提出一种改进的混合粒子群算法，其具体策略为：首先，选择三个种群进行初 始化，选出每个种群中最优与最差的粒子;其次，选出的三个最优粒子组成一个新的种群并 选出最佳粒子;最后，利用最佳粒子替换三个种群的最差粒子； 步骤五:采用改进的混合粒子群算法求解最优参数，得到利用本方法求得在基座姿态 扰动最小下的规划轨迹。2. 根据权利要求1所述的冗余空间机械臂最小基座姿态扰动的轨迹规划方法，其特征 在于:所述步骤一为基于SolidWorks进行了7自由度冗余空间机械臂的设计并且建立了三 维模型，其构型采用S-R-S结构，7个自由度均为旋转关节，大臂与小臂采用黄金分割比例即 为1:0.618。3. 根据权利要求1所述的冗余空间机械臂最小基座姿态扰动的轨迹规划方法，其特征 在于:所述步骤三中在基座姿态扰动最小的约束下建立代价函数方程为： 0i(t)= A (t)其中，范数| |Sqb| | =5qbTSqb，为姿态误差的权重系数。4. 根据权利要求1所述的冗余空间机械臂最小基座姿态扰动的轨迹规划方法，其特征 在于:所述步骤五中改进的混合粒子群算法具体应用时的基本思想是每次迭代时将粒子按 代价函数值进行排序，用群体中最好一半粒子的速度和位置替换最差一半的速度和位置， 同时保留每个个体的最优值。5. 根据权利要求1所述的冗余空间机械臂最小基座姿态扰动的轨迹规划方法，其特征 在于:所述步骤五中改进的混合粒子群算法具体实验方案包括：（1)设置参数；（2)随机初始 化三个种群中粒子的速度和位置；（3)每个种群计算每个粒子的代价函数值，将各个粒子自 身最优的位置和代价函数值存储在PU中，将种群的最优位置和代价函数值存储在Pm中； (4)更新粒子的速度和位置；（5)对每个粒子自身当前的位置与之前最优位置进行比较，如 果比较好，则将其更新为最好的位置，反之，则保留之前最优位置；（6)比较当前所有的 PlJ和 pgJ，并且更新Pfu; (7)将粒子按照代价函数值进行排序，用群体中最好一半粒子的速度和位 置替换最差一半的速度和位置，同时保留每个个体的最优值；（8)选出每个种群中的最优粒 子和最差粒子，3个最优粒子进行以上步骤的迭代选出最佳粒子，然后替换每个种群中的最 差粒子;（9)满足迭代次数，输出最优参数，确定轨迹。
【文档编号】G06F17/50GK106055522SQ201610503870
【公开日】2016年10月26日
【申请日】2016年6月30日
【发明人】张强, 张建霞, 周东生, 魏小鹏
【申请人】大连大学