CN105138000A - 最优化基座位姿扰动的七自由度空间机械臂轨迹规划方法 - Google Patents

Abstract

本发明设计了一种七自由度空间机械臂最优化基座位姿扰动的路径规划方法。建立了自由漂浮基座和七自由度机械臂组成的模型，在该模型下进行仿真实验。包括以下步骤：首先，对空间机械臂建立运动学方程；然后利用正弦五阶多项式函数对关节轨迹参数化，根据基座位姿控制精度指标和机械臂的运动约束设计目标函数；最后采用量子行为粒子群优化算法求解非线性优化问题，搜索机械臂关节运动的最优轨迹，使其对基座的位置和姿态产生的扰动最小，从而达到最优化基座位姿的目的。发明能较快的找到全局最优值，所需的相关参数少，可使关节路径平滑并且满足关节角、角速度以及角加速度的范围。本发明最优化七自由度机械臂基座的位置和姿态问题是可行有效的。

Description

最优化基座位姿扰动的七自由度空间机械臂轨迹规划方法

技术领域

本发明属于航空航天控制领域，更具体地说，涉及最优化基座位姿扰动的七自由度空间机械臂轨迹规划方法。

背景技术

随着国家国民经济与国防工业技术的飞速发展，航天器卫星等数量越来越多，人们越来越关注空间机器人在空间技术的发展。但是由于各种原因如卫星故障、失效或任务结束而被放弃等，卫星被留在空中成为了空中废品，对轨道资源以及其他航天器安全都会产生巨大影响。

目前，越来越多的学者不断地研究空间机器人的姿态稳定问题。VAFA等关于自由漂浮系统的非完整路径规划提出了自校正运动法和扰动图法。但自校正运动法只能调整基座姿态，其最终状态改变不了。M.ATorres等提出增强干扰图(EDM)方法，这种方法减小了姿态干扰，但其计算需占用较大存储空间，计算慢且效率太低。P.FHuang在2006年提出最小基座反作用干扰的最优路径规划方法，它是基于遗传算法的，可以稳定控制基座姿态，但遗传算法的实现较粒子群算法复杂。戈新生等提出基于遗传算法的最优轨迹规划，解决了机械臂系统运动规划问题。2011年，史也等提出了量子粒子群优化算法进行关节轨迹优化，控制基座姿态。以上均不是以七自由度的机械臂为对象同时规划基座的位置和姿态，本发明以七自由度空间机械臂为仿真对象，采用量子行为粒子群优化算法同时规划基座的位置和姿态，使基座的位姿扰动最小。

发明内容

鉴于已有生产方法存在的缺陷，本发明提供最优化基座位姿扰动的七自由度空间机械臂轨迹规划方法，该方法是规划七自由度空间机械臂基座的位置和姿态，利用量子行为粒子群优化算法(QPSO算法)搜索关节运动的最优轨迹，达到机械臂运动对基座位姿产生的扰动最小的目的。

为实现上述目的，本发明所采用的技术方案是：

包括以下步骤：(1)对七自由度空间机械臂建模，定义机械臂的连杆长度、连杆以及基座的质量等几何参数，利用广义雅可比矩阵建立七自由度机械臂的运动学方程。(2)利用四元数描述机械臂的基座姿态，采用正弦函数五阶多项式参数化空间机械臂的关节轨迹，根据空间机械臂基座的位置和姿态建立系统状态方程，以上均要满足关节角、角速度和角加速度的约束条件。(3)根据基座位置和姿态控制精度指标和机械臂的运动约束建立适应度函数。(4)采用量子行为粒子群优化算法优化目标函数，求解出适应度函数中的最优解，代入参数化后的关节轨迹方程，实现对基座位置和姿态的最优化。

另外，目标函数就是适应度函数，定义的适应度函数为：

式中，b指基座，q，p分别表示基座姿态，基座位置。δq_b是基座姿态的四元数误差，δp_b是基座的位置误差，分别是关节速度和角加速度的约束。J_q,J_p分别是姿态误差和位置误差的权重系数，分别是关节角速度、角加速度限制的权重系数，它们均由精度要求所决定，此处

利用QPSO算法搜索空间机械臂关节运动的最优轨迹包括以下步骤：

步骤1：置时间t＝0,在问题空间中初始化M个粒子的当前位置X_i(0)，(粒子i＝1,2,...M)，并置个体最好位置P_i(0)＝X_i(0)。

步骤2：根据下式计算粒子的平均最好位置(j指空间维数，j＝1,2,...7)：

步骤3：根据适应度函数计算每个粒子的适应度值F_i(a)，并更新粒子的个体

最好位置，即若F[X_i(t)]＜F[P_i(t-1)]，则置P_i(t)＝X_i(t)；否则，

P_i(t)＝P_i(t-1)。

步骤4：更新全局最优位置G(t)，即将P_i(t)的适应值与全局最好位置G(t-1)的适应值比较，若F[P_i(t)]＜F[G(t-1)]，则置G(t)＝P_i(t)；否则G(t)＝G(t-1)。

步骤5：根据下式计算得到随机点的位置：

式中，服从(0,1)上的均匀分布。

步骤6：根据下式更新每个粒子的新位置：

式中，u_i,j(t)服从(0,1)上的均匀分布，α称为QPSO的收缩扩张系数，它是算法除群体规模和迭代次数以外的唯一控制参数。α按如下方式动态变化：

参数α随着迭代线性地从m递减到n，通常取m＝1，n＝0.5。N表示最大迭代次数。

步骤7：若算法达到迭代次数，算法结束；否则，置t＝t+1,返回步骤2。

本发明同时规划了七自由度空间机械臂基座的位置和姿态，最优化基座位姿，使其受到的扰动最小，采用QPSO算法可以较快地找到全局最优解，收敛速度快，所需相关参数少，规划的关节路径满足关节角、角速度及角加速度的范围且规划的关节路径平滑，适合机械臂的控制，本发明提高了计算效率和求解精度。

附图说明

图1基座位置变化曲线；

图2基座姿态变化曲线；

图3关节角的运动轨迹曲线；

图4关节角速度曲线；

图5关节角加速度曲线；

图6本发明的技术方案简图。

具体实施方式

本发明的实施例是在以本发明技术方案为前提下进行实施的，给出了详细的实施方式和具体的操作过程，针对七自由度空间机械臂进行研究，使机械臂的基座扰动最小。但本发明的保护范围不限于下述实施例。

实施例1

步骤1：定义如表一七自由度空间机械臂的D-H参数和表二的机械臂各连杆的质量参数；

表一系统的D-H参数

表二空间机械臂的质量特性

根据以上参数建立七自由度空间机械臂的运动学模型。七自由度即空间机械臂中具有七个独立位置变量。本发明将雅可比矩阵扩展为广义雅可比矩阵来描述空间机械臂的系统状态。该空间机械臂的雅可比矩阵为：

其中：v₀，ω₀分别为机械臂初始的线速度和角速度，I_b为基座的惯性矩阵，I_bm为基座与机械臂之间的耦合惯性矩阵，为关节角组成的关节角矩阵，J_vb为关于v₀的雅可比矩阵部分，J_ωb为关于ω₀的雅可比矩阵部分。

该自由漂浮模式下空间机械臂的广义雅可比公式为：

其中：v_e，ω_e分别为空间机械臂末端执行器的线速度和角速度，J_m为机械臂的雅可比矩阵，J_b为基座的雅可比矩阵，I_b为基座的惯性矩阵，I_bm为基座与机械臂之间的耦合惯性矩阵，为机械臂关节角组成的关节角矩阵，J^*为广义雅可比的符号，Ψ_b为基座的姿态，m_i为机械臂每个连杆的质量，I_i为惯性矩阵。步骤2：采用四元数法描述空间机械臂的基座姿态，根据机械臂基座的位置和姿态建立系统状态方程，使用正弦函数五阶多项式对七自由度空间机械臂的关节轨迹参数化，同时满足关节角、角速度和角加速度的限制条件；

步骤3：定义适应度函数；

步骤4：定义基座位置和姿态的初始状态和期望状态以及定义关节角、角速度、角加速度范围；

步骤5：利用QPSO算法对建立的七自由度空间机械臂的适应度函数求解，进行仿真实验；

所述QPSO算法步骤如下：

步骤1：置时间t＝0,在问题空间中初始化M个粒子的当前位置X_i(0)，(粒子i＝1,2,...M)，并置个体最好位置P_i(0)＝X_i(0)。

步骤2：根据下式计算粒子的平均最好位置(j指空间维数，j＝1,2,...7)：

步骤3：根据适应度函数计算每个粒子的适应度值F_i(a)，并更新粒子的个体最好位置，即若F[X_i(t)]＜F[P_i(t-1)]，则置P_i(t)＝X_i(t)；否则，P_i(t)＝P_i(t-1)。

步骤4：更新全局最优位置G(t)，即将P_i(t)的适应值与全局最好位置G(t-1)的适应值比较，若F[P_i(t)]＜F[G(t-1)]，则置G(t)＝P_i(t)；否则G(t)＝G(t-1)。

步骤5：根据下式计算得到随机点的位置：

式中，服从(0,1)上的均匀分布。

步骤6：根据下式更新每个粒子的新位置：

式中，u_i,j(t)服从(0,1)上的均匀分布，α称为QPSO的收缩扩张系数，它是算法除群体规模和迭代次数以外的唯一控制参数。α按如下方式动态变化：

参数α随着迭代线性地从m递减到n，通常取m＝1，n＝0.5。N表示最大迭代次数。

步骤7：若算法达到迭代次数N，算法结束；否则，置t＝t+1,返回步骤2。

图1至图5的曲线均在程序中设置得到各自的变化曲线。图1位置曲线是在程序中画出三个坐标轴x，y和z的变化曲线。

实施例2

首先，四元数描述机械臂的基座姿态，四元数方程定义为：

其中：η＝cos(Ψ/2)表示四元数中的标量部分；q＝ksin(Ψ/2)表示四元数中的矢

量部分；Ψ表示两个坐标系绕欧拉轴转过的角度；k表示欧拉轴的单位方向向量。如图2为基座的姿态变化曲线。

系统状态方程定义如下：

系统状态方程可以通过数值积分计算：

其中，为关节的角速度，J_{bm_ω}为关于角速度的基座与机械臂的耦合矩阵，J_{bm_v}为关于线速度的基座与机械臂的耦合矩阵。

运动规划的目的是使最终状态接近于期望状态(即初始状态)：

X_b0-X_bf→0

其中，X_b0为基座的初始状态，X_bf为基座的最终状态。

对机械臂关节角函数利用正弦函数五阶多项式进行如下参数化,得到参数化的关节轨迹方程：

式中，i＝1,2,…,7，[a_i1,a_i2,…a_i5]是多项式系数，Δ_i1及Δ_i2根据关节角的范围分别定义为：

将约束条件θ_i(0)＝θ_i0,代入θ_i(t)表达式，得到多项式系数：

此时，关节角、角速度及角加速度可表示为：

最终每个关节参数化方程中仅包含参数a_i5，只要确定了a_i5，空间机械臂的运动轨迹也就确定下来。

如图3和图4和图5所示分别为关节角，角速度和角加速度曲线。

适应度函数定义为：

式中，δq_b是基座姿态的四元数误差，δp_b是基座的位置误差，分别是关节角速度和角加速度的约束。J_q,J_p分别是姿态误差和位置误差的权重系数，分别是关节角速度、角加速度限制的权重系数，它们均由精度要求所决定，此处

将关节角速度、角加速度的范围定义为：

定义基座的初始关节角与期望关节角分别为：

Θ_b0＝[0,0,0,0,0,0,0]，Θ_bd＝[0,0,0,0,0,0,0]

基座的初始位姿定义为：

[P_b0Q_b0]＝[0-0.0813-0.18801000]

期望的系统位姿为：

[P_b0Q_b0]＝[0-0.0813-0.18801000]

机械臂基座位置和姿态部分的加权系数根据其精度要求确定，分别定义为：

J_p＝2×10^-3，

最后，设置粒子数为40，迭代次数为100次，自变量个数为7采用QPSO算法得到的最优a值和最优目标函数值为：

本发明通过QPSO算法同时规划七自由度空间机械臂基座的位置和姿态，仅规划关节角轨迹最优化基座的位置和姿态，规划的关节路径满足关节角、角速度及角加速度的范围，且规划出的关节角轨迹连续平滑，便于机械臂的控制。本发明收敛速度快，所需调整参数少，该算法是可行有效的。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围内。

Claims (9)

1.最优化基座位姿扰动的七自由度空间机械臂轨迹规划方法，其特征在于，包括以下步骤：

1)采用自由漂浮基座和七自由度空间机械臂模型；

2)用四元数法描述机械臂的基座姿态，采用正弦函数五阶多项式对关节轨迹方程参数化；

3)建立适应度函数；

4)利用量子行为粒子群优化算法优化适应度函数，求解适应度函数的最优解，将最优解代入参数化后的关节轨迹方程，得到最优化基座位姿。

2.根据权利要求1所述的最优化基座位姿扰动的七自由度空间机械臂轨迹规划方法，其特征在于，所述的方法包括以下具体步骤：

步骤1：对七自由度空间机械臂建立机械臂模型，定义机械臂的连杆长度、连杆以及基座的质量、关节的数量和类型，利用广义雅可比矩阵建立七自由度机械臂的运动学方程；

步骤2：利用四元数描述机械臂的基座姿态，采用正弦函数五阶多项式参数化空间机械臂的关节轨迹，根据空间机械臂基座的位置和姿态建立系统状态方程，以上均要满足关节角、角速度和角加速度的约束条件；

步骤3：根据基座位置和姿态控制精度指标和机械臂的运动约束建立适应度函数；

步骤4：采用量子行为粒子群优化算法优化目标函数，求解出适应度函数中的最优解，代入参数化后的关节轨迹方程，实现对基座位置和姿态的最优化。

3.根据权利要求1或2所述的最优化基座位姿扰动的七自由度空间机械臂轨迹规划方法，其特征在于，所述的适应度函数为：

式中，b指基座，q，p分别表示基座姿态，基座位置；δq_b是基座姿态的四元数误差，δp_b是基座的位置误差，分别是关节速度和角加速度的约束；J_q,J_p分别是姿态误差和位置误差的权重系数，分别是关节角速度、角加速度限制的权重系数，它们均由精度要求所决定，此处

4.根据权利要求1或2所述的最优化基座位姿扰动的七自由度空间机械臂轨迹规划方法，其特征在于，采用量子行为粒子群优化算法解决七自由度空间机械臂基座的位置和姿态扰动最小问题。

5.根据权利要求2所述的最优化基座位姿扰动的七自由度空间机械臂轨迹规划方法，其特征在于，所述步骤1中通过广义雅可比矩阵方程建立空间机械臂的运动学模型，自由漂浮模式下的广义雅可比矩阵公式为：

其中：v_e，ω_e分别为空间机械臂末端执行器的线速度和角速度，J_m为机械臂的雅可比矩阵，J_b为基座的雅可比矩阵，I_b为基座的惯性矩阵，I_bm为基座与机械臂之间的耦合惯性矩阵，为机械臂关节角组成的关节角矩阵，J^*为广义雅可比的符号，Ψ_b为基座的姿态，m_i为机械臂每个连杆的质量，I_i为惯性矩阵。

6.根据权利要求1或2所述的最优化基座位姿扰动的七自由度空间机械臂轨迹规划方法，其特征在于：所述机械臂的基座姿态为：

其中：η＝cos(Ψ/2)表示四元数中的标量部分；q＝ksin(Ψ/2)表示四元数中的矢量部分；Ψ表示两个坐标系绕欧拉轴转过的角度；k表示欧拉轴的单位方向向量。

7.根据权利要求1或2所述的最优化基座位姿扰动的七自由度空间机械臂轨迹规划方法，其特征在于：所述的关节轨迹方程为：

式中，i＝1,2,...,7，[a_i1,a_i2,…a_i5]是多项式系数，Δ_i1及Δ_i2根据关节角的范围分别定义为：

8.根据权利要求2所述的最优化基座位姿扰动的七自由度空间机械臂轨迹规划方法，其特征在于：所述的系统状态方程为：

其中，b指基座，X_b指基座位姿，Q_b，P_b分别表示基座姿态和基座位置。

9.根据权利要求1或2所述的最优化基座位姿扰动的七自由度空间机械臂轨迹规划方法，其特征在于，利用所述量子行为粒子群优化算法搜索空间机械臂关节运动的最优轨迹，其具体过程如下：

步骤1：置时间t＝0,在问题空间中初始化M个粒子的当前位置X_i(0)，(粒子i＝1,2,...M)，并置个体最好位置P_i(0)＝X_i(0)；

步骤2：根据下式计算粒子的平均最好位置(j指空间维数，j＝1,2,...7)：

步骤3：根据适应度函数计算每个粒子的适应度值F_i(a)，并更新粒子的个体最好位置，即若F[X_i(t)]＜F[P_i(t-1)]，则置P_i(t)＝X_i(t)；否则，P_i(t)＝P_i(t-1)；

步骤4：更新全局最优位置G(t)，即将P_i(t)的适应值与全局最好位置G(t-1)的适应值比较，若F[P_i(t)]＜F[G(t-1)]，则置G(t)＝P_i(t)；否则G(t)＝G(t-1)；

步骤5：根据下式计算得到随机点的位置：

式中，服从(0,1)上的均匀分布；

步骤6：根据下式更新每个粒子的新位置：

式中，u_i,j(t)服从(0,1)上的均匀分布，α称为量子行为粒子群优化的收缩扩张系数，它是算法除群体规模和迭代次数以外的唯一控制参数；α按如下方式动态变化：

参数α随着迭代线性地从m递减到n，通常取m＝1，n＝0.5；N表示最大迭代次数；

步骤7：若算法达到迭代次数，算法结束；否则，置t＝t+1,返回步骤2。