CN103984230A - 一种空间机械臂基座零扰动优化控制方法 - Google Patents

Abstract

本发明公开了一种空间机械臂基座零扰动优化控制方法，包括以下步骤：S1：对空间机械臂建立运动学模型：S2：等效固定基空间机械臂的运动学建模：S3：使用七元七次多项式参数化空间机械臂的关节轨迹，采用四元数方程表示空间机械臂的基座姿态，根据空间机械臂基座的位置和姿态建立系统状态方程；S4：建立优化算法的目标函数，根据求解系统状态方程以及对机械臂的关节速度参数和角加速度参数的限定化简该目标函数；S5：通过优化算法解决运动规划问题。发明提高了寻找最优的基座无扰动空间机械臂路径的机会，节约了粒子群算法评价目标函数值的计算，还针对并行计算进行优化，可以通过并行计算成倍的提高运算速度。

Description

一种空间机械臂基座零扰动优化控制方法

技术领域

本发明涉及一种空间机械臂基座零扰动优化控制方法。

背景技术

为了使太空探索更具有高效性，近几年来研究人员不断的对空间机械臂技术问题进行研究，随着研究的深入，空间机械的主要研究热点和难点也逐渐清楚。根据动量守恒定律，空间机械臂在运动的过程中不可避免的会使装载它的人造卫星发生姿态和位置的改变。一般情况下，传统的方法是使用互相正交的飞轮、力平衡陀螺仪或者磁力矩器，甚至推进器来平衡这些扰动，不过这不仅增加了空间机器人的制造难度还耗费了空间机器人能够携带的有限的燃料。在这种前提下，研究基座最小扰动空间机械臂运动规划有其工程上的实际意义。

相关的国内外研究有：哈尔滨工业大学2008年发表的“The Cartesian PathPlanning of Free-Floating Space Robot using Particle Swarm Optimization”和“Non-holonomic Path Planning of a Free-Floating Space Robotic SystemUsing Genetic Algorithms”，日本2001年发表的“Zero reaction maneuver:flight validation with ETS-VII space robot and extension to kinematicallyredundant arm”但是上述的方法在实际应用过程中具有计算量大，难以实际应用、有一定随机性的缺点。

发明内容

根据现有技术存在的问题，本发明公开了一种空间机械臂基座零扰动优化控制方法，包括以下步骤：

S1：对空间机械臂建立运动学模型：采用逆运动学螺旋理论建立空间机械臂的运动学方程的广义雅可比矩阵，将机械臂的运动从笛卡尔空间映射到关节空间；

S2：等效固定基空间机械臂的运动学建模：采用指数坐标系来描绘空间机械臂的运动学特征；

S3：使用七元七次多项式参数化空间机械臂的关节轨迹，采用四元数方程表示空间机械臂的基座姿态，根据空间机械臂基座的位置和姿态建立系统状态方程；

S4：建立优化算法的目标函数，根据求解系统状态方程以及对机械臂的关节速度参数和角加速度参数的限定化简该目标函数；

S5：通过优化算法解决运动规划问题：采用粒子群优化算法的方法对目标函数进行规划，求出目标函数中未知数的最优解，将求出的最优解代入使用七元七次多项式参数化空间机械臂的关节轨迹方程，实现对空间机械臂的控制。

进一步的，所述优化算法的目标函数为：

$F\left(a\right)=\frac{\left|\right|{\mathrm{\δq}}_b\left|\right|}{K_q}+\frac{\left|\right|{\mathrm{\δp}}_b\left|\right|}{K_p}+\frac{L_{\stackrel{\·}{\mathrm{\Θ}}}}{L_{\stackrel{\·}{\mathrm{\Θ}}}}+\frac{L_{\stackrel{\·\·}{\mathrm{\Θ}}}}{K_{\stackrel{\·\·}{\mathrm{\Θ}}}}$

其中：δq_b、δp_b、分别是空间机械臂的基座姿态的四元数误差参数、基座的位置误差参数、关节速度参数和角加速度参数，上述这四个参数均为待优化参数a的函数，K_q、K_p、由精度要求所决定，并且

进一步的，采用多粒子群优化算法的方法对目标函数进行规划时包括以下步骤：

步骤1：反复计算目标函数初始化全部的子种群，基于目标函数的值对全部的粒子进行重新排序，选择前N个子粒初始化精英种群，评估精英种群选出精英粒子；

步骤2：分发精英粒子到每一个子种群，在分发过程中，精英粒子的速度由以下公式确定：

$v_i^{k+1}={\mathrm{\ωv}}_i^k+c_1{r}_1(p_{\mathrm{best}\_i}^k-a_i^k)+c_2{r}_2({g_{\mathrm{best}}^k}^{\text{'}}-a_i^k)+c_3{r}_3(e^k-a_i^k)$

其中：c₁、c₂、c₃为学习因子，为第i个粒子在k时刻的位置，p_{best_i}、g_best、e分别为第i个粒子的历史最佳位置、种群最佳位置和精英粒子位置，上角标k均表示k时刻；

步骤3：独立从各个种群中选出最优粒子取代精英种群中最差的粒子，当吸收了由普通种群贡献的精英粒子之后，精英种群进行一代繁殖；

$a_i^{k+1}=a_i^k+v_i^{k+1}+\mathrm{\δ}$

其中δ为变异因子，由下式决定：

$\left\{\begin{array}{c}\mathrm{\δ}\∈U(a,b),{\mathrm{\Δf}}_i\≤\mathrm{\ϵ}\\ \mathrm{\δ}\∈N(0,\mathrm{\σ}),{\mathrm{\Δf}}_i>\mathrm{\ϵ}\end{array}\right.$

其中 ${\mathrm{\Δf}}_i=|f\left(a_i^k\right)-f\left(a_i^{k-1}\right)|,$ ε是误差的边界值；

步骤4：选出精英种群的最优粒子，判断是否满足收敛条件，如果不满足则返回步骤3，满足进入如下的步骤5；

步骤5：算法结束，输出结果。

由于采用了上述技术方案，本发明公开的一种空间机械臂基座零扰动优化控制方法，通过对空间机械臂的运动学建模、等效固定基空间机械臂的运动学建模、建立空间机械臂的关节轨迹方程以及目标函数，通过优化算法解决运动规划问题对目标函数进行求解最后实现对空间机械臂的控制。本发明在建立目标函数时将机械臂的基座的空间位置也列为考虑项，进一步增加了空间机械臂的稳定性，在优化目标函数时改变了基础粒子群算法评估目标函数的步骤，减去了1/3的底层子种群的评估目标函数的计算，使得计算的效率大大提高。本发明提高了寻找最优的基座无扰动空间机械臂路径的机会，节约了粒子群算法评价目标函数值的计算时间，还针对并行计算进行优化，可以通过并行计算成倍的提高运算速度。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明公开的优化控制方法的拓扑结构示意图；

图2为指数坐标系统的示意图；

图3为指数坐标系统从p到p₄的粒子轨迹示意图；

图4为优化目标函数时的流程图；

图5为粒子群优化算法的流程图；

图6为自由度空间机械臂模型的结构示意图；

图7为各粒子群的最佳适应变化示意图；

图8为机械臂的末端器路径的示意图。

具体实施方式

为使本发明的技术方案和优点更加清楚，下面结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚完整的描述：

步骤1：根据如表一所示的6自由度空间机械臂的D-H参数，

表一

由于太空环境中动量守恒，空间机械臂存在动力学冗余特性，空间机械臂末端器的位姿不仅和关节角有关还和机械臂运动的历史有关。因此本发明结合动力学方程将雅可比矩阵扩展为广义雅可比矩阵来描述空间机械臂的系统状态。根据这些参数建立空间机械臂的运动学模型。该机械臂的广义雅可比矩阵公式为：

$\left[\begin{array}{c}{v}_e\\ {\mathrm{\ω}}_e\end{array}\right]=[J_m-J_b{I}_b^{-1}{I}_{\mathrm{bm}}]\stackrel{\·}{\mathrm{\Θ}}=J^{*}({\mathrm{\ψ}}_b,\mathrm{\Θ},m_i,I_i)\stackrel{\·}{\mathrm{\Θ}}$

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

步骤2：采用指数坐标系来描绘空间机械臂的运动，如图2和图3所示，在一般的直角坐标系下，我们可以把线位移视为一个向量，我们把角位移也视为一个向量就很难理解，但是角位移确实是一个向量，并且很明显平行四边形法则不能计算角位移向量的运算，因为角位移符合的是洛仑兹变换。因此我们可以使用指数坐标系来描绘空间机械臂的运动特征。

步骤3：如图4和图5和图6所示，使用七元七次多项式参数化空间机械臂的关节轨迹，采用四元数方程表示空间机械臂的基座姿态，根据空间机械臂基座的位置和姿态建立系统状态方程。通常描述一个刚体姿态的方式有等效轴角、旋转矩阵、欧拉角三种方法，但这三种方法均有奇异性和信息冗余的特性。因此本发明使用全局无奇异的最小表法，四元数方程来表示空间机械臂卫星的基座姿态。

为了得到平滑的轨迹曲线，我们使用多项式来描述关节运动，并且多项式的阶数必须大于或等于约束的数量，因此我们使用七阶多项式来进行参数化空间机械臂的关节轨迹，如下所示：

θ_i(t)＝Δ_i1sin(a_i7t⁷+a_i6t⁶+a_i5t⁵+a_i4t⁴+a_i3t³+a_i2t²+a_i1t¹+a_i0)+Δ_i2   (7)

此处： ${\mathrm{\Δ}}_{i1}=\frac{{\mathrm{\θ}}_{i\max }-{\mathrm{\θ}}_{i\min }}{2},{\mathrm{\Δ}}_{i2}=\frac{{\mathrm{\θ}}_{i\max }+{\mathrm{\θ}}_{i\min }}{2}$

其中，a_i0-a_i7是多项式系数，Δ_i1、Δ_i2用来使每一个关节在其约束范围内θ_i∈[θ_i min,θ_i max]

相应的角速度对应的七阶多项式为：

$\begin{array}{c}{\stackrel{\·}{\mathrm{\θ}}}_i\left(t\right)={\mathrm{\Δ}}_{i1}\cos (a_{i7}{t}^7+a_{i6}{t}^6+a_{i5}{t}^5+a_{i4}{t}^4+a_{i3}{t}^3+a_{i2}{t}^2+a_{i1}{t}^1+a_{i0})\\ \·(7a_{i7}{t}^6+{6a}_{i6}{t}^5+{5a}_{i5}{t}^4+{4a}_{i4}{t}^3+{3a}_{i3}{t}^2+{2a}_{i2}t+a_{i1})---\left(8\right)\end{array}$

相应的角加速度为：

$\begin{array}{c}\stackrel{\·\·}{\mathrm{\θ}}\left(t\right)={-\mathrm{\Δ}}_{i1}\sin (a_{i7}{t}^7+a_{i6}{t}^6+a_{i5}{t}^5+a_{i4}{t}^4+a_{i3}{t}^3+a_{i2}{t}^2+a_{i1}{t}^1+a_{i0})\\ \·{({7a}_{i7}{t}^6+{6a}_{i6}{t}^5+{5a}_{i5}{t}^4+{4a}_{i4}{t}^3+{3a}_{i3}{t}^2+{2a}_{i2}t+a_{i1})}^2\\ +{\mathrm{\Δ}}_{i1}\cos (a_{i7}{t}^7+a_{i6}{t}^6+a_{i5}{t}^5+a_{i4}{t}^4+a_{i3}{t}^3+a_{i2}{t}^2+a_{i1}{t}^1+a_{i0})\\ \·({42a}_{i7}{t}^5+{30a}_{i6}{t}^4+{20a}_{i5}{t}^3+{12a}_{i4}{t}^2+{6a}_{i3}t+{2a}_{i2})---\left(9\right)\end{array}$

很明显θ,应该满足下列方程

$\mathrm{\Θ}\left(t_0\right)={\mathrm{\Θ}}_0,\stackrel{\·}{\mathrm{\Θ}}\left(t_0\right)=0,\stackrel{\·\·}{\mathrm{\Θ}}\left(t_0\right)=0---\left(10\right)$

$\mathrm{\Θ}\left(t_f\right)={\mathrm{\Θ}}_d,\stackrel{\·}{\mathrm{\Θ}}\left(t_f\right)=0,\stackrel{\·\·}{\mathrm{\Θ}}\left(t_f\right)=0---\left(11\right)$

t₀、t_f分别为初始时间和中指时间，

将(11)代入(7)-(10)得：

$a_{i0}=\arcsin \left(\frac{{\mathrm{\θ}}_{i0}-{\mathrm{\Δ}}_{i2}}{{\mathrm{\Δ}}_{i1}}\right)$

a_i1＝a_i2＝0

$a_{i3}=-({3a}_{i7}{t}_f^7+a_{i6}{t}_f^6-10[\arcsin \left(\frac{{\mathrm{\θ}}_{\mathrm{id}}-{\mathrm{\Δ}}_{i2}}{{\mathrm{\Δ}}_{i1}}\right)-\arcsin \left(\frac{{\mathrm{\θ}}_{i0}-{\mathrm{\Δ}}_{i2}}{{\mathrm{\Δ}}_{i1}}\right)\left]\right)/t_f^3$

$a_{i4}=-({8a}_{i7}{t}_f^7+3a_{i6}{t}_f^6-15[\arcsin \left(\frac{{\mathrm{\θ}}_{\mathrm{id}}-{\mathrm{\Δ}}_{i2}}{{\mathrm{\Δ}}_{i1}}\right)-\arcsin \left(\frac{{\mathrm{\θ}}_{i0}-{\mathrm{\Δ}}_{i2}}{{\mathrm{\Δ}}_{i1}}\right)\left]\right)/t_f^4$

$a_{i5}=-({6a}_{i7}{t}_f^7+3a_{i6}{t}_f^6-6[\arcsin \left(\frac{{\mathrm{\θ}}_{\mathrm{id}}-{\mathrm{\Δ}}_{i2}}{{\mathrm{\Δ}}_{i1}}\right)-\arcsin \left(\frac{{\mathrm{\θ}}_{i0}-{\mathrm{\Δ}}_{i2}}{{\mathrm{\Δ}}_{i1}}\right)\left]\right)/t_f^5$

此处θ_i0,θ_id分别是关节i初始和期望的关节角；

最终每个关节参数化方程中只有两个参数(a_i6,a_i7)剩下来，

a＝[a₁₆,a₁₇,a₂₆,a₂₇,...,a_n6,a_n7]∈R²ⁿ

在优化算法中，a也叫做拉格朗日乘子，当a解出来了，空间机械臂的运动也就确定了下来。

本发明中规划的目的：让空间机械臂期望的终点和实际运动的终点重合，系统状态方程可以表达如下：

$X=\left(\begin{array}{c}{Q}_b\\ P_b\end{array}\right)\∈R^7$

其中：Q_b和P_b分别是特定的空间机械臂的基座姿态的四元数误差参数和基座的位置误差参数，δq_b和δp_b是通用的空间机械臂的基座姿态的四元数误差参数和基座的位置误差参数，在通常的公式中都采用小写字母的形式进行表示。

系统状态方程可以通过数值积分进行求解：

$Q_b\left(t\right)={\∫}_0^t\frac{1}{2}\left[\begin{array}{c}{-q}_b^T\\ {\mathrm{\η}}_{b}I-{\tilde{q}}_b\end{array}\right]J_{\mathrm{bm}\_\mathrm{\ω}}\stackrel{\·}{\mathrm{\θ}}\mathrm{dt}$

$P_b={\∫}_0^t{J}_{\mathrm{bm}\_v}\stackrel{\·}{\mathrm{\θ}}\mathrm{dt}$

相应的，最终的系统方程和期望的系统状态方程如下：

$X_0=X_d=\left(\begin{array}{c}{Q}_{b0}\\ P_{b0}\end{array}\right);$ $X_f=\left(\begin{array}{c}{Q}_{\mathrm{bf}}\\ P_{\mathrm{bf}}\end{array}\right)$

最终的运动规划是让终止系统状态方程和期望系统状态方程的差为零，即：

X_b0-X_bf→0

因为这种方法不需要计算J_bm的逆，因此我们称这种方法为正向运动学规划方法。

为了使该方法更加具有实用价值，机械臂的各关节的角速度和角加速度被限制如下：

$\left|{\stackrel{\·}{\mathrm{\θ}}}_i\right|\≤{\stackrel{\·}{\mathrm{\θ}}}_{i\_\mathrm{limit}},\left|{\stackrel{\·\·}{\mathrm{\θ}}}_i\right|\≤{\stackrel{\·\·}{\mathrm{\θ}}}_{i\_\mathrm{limit}}$

此种情况下，我们可以定义两个函数：

$L_{\stackrel{\·}{\mathrm{\Θ}}}=\underset{0\≤i\≤n}{\max }\left(J_{{\stackrel{\·}{\mathrm{\θ}}}_i}\right),L_{\stackrel{\·\·}{\mathrm{\Θ}}}=\underset{0\≤i\≤n}{\max }\left(J_{{\stackrel{\·\·}{\mathrm{\θ}}}_i}\right)$

此处

$J_{{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_i}=\left\{\begin{array}{cc}0& \mathrm{if}{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{i\_\max }<{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{i\_\mathrm{limit}}\\ ({\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{i\_\max }-{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{i\_\mathrm{limit}})/{\stackrel{\&CenterDot;}{\mathrm{\&theta;}}}_{i\_\mathrm{limit}}& \mathrm{else}\end{array}\right.$

$J_{{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_i}=\left\{\begin{array}{cc}0& \mathrm{if}{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_{i\_\max }<{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_{i\_\mathrm{limit}}\\ ({\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_{i\_\max }-{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_{i\_\mathrm{limit}})/{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&theta;}}}_{i\_\mathrm{limit}}& \mathrm{else}\end{array}\right.$

此处是的最大值，并且是他们超出约束的比例。这样我们将δq_b、δp_b、代入优化算法的目标函数中，该目标函数即为以参数a为未知数的函数，我们需要求解a的数值来控制空间机械臂的运动情况。根据计算出的参数a,可以简单的确定。

通过优化算法解决运动规划问题

优化算法的目标函数为：

$F\left(a\right)=\frac{\left|\right|{\mathrm{\&delta;q}}_b\left|\right|}{K_q}+\frac{\left|\right|{\mathrm{\&delta;p}}_b\left|\right|}{K_p}+\frac{L_{\stackrel{\&CenterDot;}{\mathrm{\&Theta;}}}}{L_{\stackrel{\&CenterDot;}{\mathrm{\&Theta;}}}}+\frac{L_{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&Theta;}}}}{K_{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&Theta;}}}}$

其中：δq_b、δp_b、分别是空间机械臂的基座姿态的四元数误差参数、基座的位置误差参数、关节速度参数和角加速度参数，上述这四个参数均为待优化参数a的函数，K_q、K_p、由精度要求所决定；并且

在上述目标函数中我们需要求解a，使F(a)越小越好。

本发明采用多粒子群优化算法的方法对目标函数进行求解，求解出a值，该粒子群优化算法的拓扑结构如图1所示，粒子群优化算法的具体处理步骤如下：

步骤1：反复计算目标函数初始化全部的子种群，基于目标函数的值对全部的粒子进行重新排序，选择前N个子粒初始化精英种群，评估精英种群选出精英粒子；

步骤2：分发精英粒子到每一个子种群，在分发过程中，精英粒子的速度由以下公式确定：

$v_i^{k+1}={\mathrm{\&omega;v}}_i^k+c_1{r}_1(p_{\mathrm{best}\_i}^k-a_i^k)+c_2{r}_2({g_{\mathrm{best}}^k}^{\text{'}}-a_i^k)+c_3{r}_3(e^k-a_i^k)$

其中：c₁、c₂、c₃为学习因子，为第i个粒子在k时刻的位置，p_{best_i}、g_best、e分别为第i个粒子的历史最佳位置、种群最佳位置和精英粒子位置，上角标k均表示k时刻；因为所有的粒子都有速度，所谓精英种群是把每一个种群中好的那些粒子选出来组成的。

步骤3：独立从各个种群中选出最优粒子取代精英种群中最差的粒子，当吸收了由普通种群贡献的精英粒子之后，精英种群进行一代繁殖；

$a_i^{k+1}=a_i^k+v_i^{k+1}+\mathrm{\&delta;}$

其中δ为变异因子，由下式决定：

$\left\{\begin{array}{c}\mathrm{\&delta;}\&Element;U(a,b),{\mathrm{\&Delta;f}}_i\&le;\mathrm{\&epsiv;}\\ \mathrm{\&delta;}\&Element;N(0,\mathrm{\&sigma;}),{\mathrm{\&Delta;f}}_i>\mathrm{\&epsiv;}\end{array}\right.$

其中 ${\mathrm{\&Delta;f}}_i=|f\left(a_i^k\right)-f\left(a_i^{k-1}\right)|,$ ε是误差的边界值；

步骤4：选出精英种群的最优粒子，判断是否满足收敛条件，如果不满足则返回步骤3，满足进入如下的步骤5；

步骤5：算法结束，输出结果。

实施例：本发明是针对六自由度空间机械臂来进行研究，从而使机械臂的基座扰动降低至零。如表二所示机械臂各个连杆的质量参数：

表二

每个关节角速度和角加速度范围设定为：

精度系数为： $\left\{\begin{array}{c}{K}_q=\sin (\mathrm{\&pi;}/360)\\ K_p=2\&CenterDot;{10}^{-3},K_{\stackrel{\&CenterDot;}{\mathrm{\&Theta;}}}=K_{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&Theta;}}}=0.01\end{array}\right.$

初始的系统位姿和末端器位置为：

$\left\{\begin{array}{c}\left[\begin{array}{cc}{P}_{b0}& Q_{b0}\end{array}\right]=\left[\begin{array}{ccccccc}0& -0.3333& -3.0966& 1& 0& 0& 0\end{array}\right]\\ P_{e0}=\left[\begin{array}{ccc}0& 0.6666& -0.0966\end{array}\right]\end{array}\right.$

期望的系统位姿和末端器位置：

$\left\{\begin{array}{c}\left[\begin{array}{cc}{P}_{\mathrm{bd}}& Q_{\mathrm{bd}}\end{array}\right]=\left[\begin{array}{ccccccc}0& -0.3333& -3.0966& 1& 0& 0& 0\end{array}\right]\\ P_{\mathrm{ed}}=\left[\begin{array}{ccc}0.6981& 1.1462& 0.1714\end{array}\right]\end{array}\right.$

算法的参数设定为

[N_e nS N_s c₁ c₂ c₃ ω_n M D δ]

＝[40 5 40 2.05 2.05 2 ω_n 40 12 0.05]

此处

ω_n＝[0.2 0.4 0.6 0.8 1]

计算结果如图7所示，显示了目标函数的值和繁殖代数之间的关系，

最优的a和最优的目标函数为：

$\left\{\begin{array}{c}a=\left[\begin{array}{cccc}-0.1754& 0.2040& 0.0322& 0.1530\end{array}\right.\\ \begin{array}{cccc}0.2502& 0.1389& -0.0579& 0.0757\end{array}\\ \left.\begin{array}{cccc}0.0480& 0.2341& -0.1018& -0.0546\end{array}\right]\&times;{10}^{-5}\\ F\left(a\right)=4.8309\end{array}\right.$

附图8是末端器的路径示意图，经过校正之后的末端器位置为：

P_em＝[0.7325 1.1171 0.1915]

其中系统总误差为：

P_{e_error}＝[0.0344 -0.0291 0.0201]

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

Claims (3)

1.一种空间机械臂基座零扰动优化控制方法，其特征在于：包括以下步骤：

S1：对空间机械臂建立运动学模型：采用逆运动学螺旋理论建立空间机械臂的运动学方程的广义雅可比矩阵，将机械臂的运动从笛卡尔空间映射到关节空间；

S2：等效固定基空间机械臂的运动学建模：采用指数坐标系来描绘空间机械臂的运动学特征；

S3：使用七元七次多项式参数化空间机械臂的关节轨迹，采用四元数方程表示空间机械臂的基座姿态，根据空间机械臂基座的位置和姿态建立系统状态方程；

S4：建立优化算法的目标函数，根据求解系统状态方程以及对机械臂的关节速度参数和角加速度参数的限定化简该目标函数；

S5：通过优化算法解决运动规划问题：采用粒子群优化算法的方法对目标函数进行规划，求出目标函数中未知数的最优解，将求出的最优解代入使用七元七次多项式参数化空间机械臂的关节轨迹方程，实现对空间机械臂的控制。

2.根据权利要求1所述的一种空间机械臂基座零扰动优化控制方法，其特征还在于：所述优化算法的目标函数为：

$F\left(a\right)=\frac{\left|\right|{\mathrm{\&delta;q}}_b\left|\right|}{K_q}+\frac{\left|\right|{\mathrm{\&delta;p}}_b\left|\right|}{K_p}+\frac{L_{\stackrel{\&CenterDot;}{\mathrm{\&Theta;}}}}{L_{\stackrel{\&CenterDot;}{\mathrm{\&Theta;}}}}+\frac{L_{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&Theta;}}}}{K_{\stackrel{\&CenterDot;\&CenterDot;}{\mathrm{\&Theta;}}}}$

其中：δq_b、δp_b、分别是空间机械臂的基座姿态的四元数误差参数、基座的位置误差参数、关节速度参数和角加速度参数，上述这四个参数均为待优化参数a的函数，K_q、K_p、由精度要求所决定，并且

3.根据权利要求1所述的一种空间机械臂基座零扰动优化控制方法，其特征还在于：采用多粒子群优化算法的方法对目标函数进行规划时包括以下步骤：

步骤1：反复计算目标函数初始化全部的子种群，基于目标函数的值对全部的粒子进行重新排序，选择前N个子粒初始化精英种群，评估精英种群选出精英粒子；

步骤2：分发精英粒子到每一个子种群，在分发过程中，精英粒子的速度由以下公式确定：

$v_i^{k+1}={\mathrm{\&omega;v}}_i^k+c_1{r}_1(p_{\mathrm{best}\_i}^k-a_i^k)+c_2{r}_2({g_{\mathrm{best}}^k}^{\text{'}}-a_i^k)+c_3{r}_3(e^k-a_i^k)$

其中：c₁、c₂、c₃为学习因子，为第i个粒子在k时刻的位置，p_{best_i}、g_best、e分别为第i个粒子的历史最佳位置、种群最佳位置和精英粒子位置，上角标k均表示k时刻；

步骤3：独立从各个种群中选出最优粒子取代精英种群中最差的粒子，当吸收了由普通种群贡献的精英粒子之后，精英种群进行一代繁殖；

$a_i^{k+1}=a_i^k+v_i^{k+1}+\mathrm{\&delta;}$

其中δ为变异因子，由下式决定：

$\left\{\begin{array}{c}\mathrm{\&delta;}\&Element;U(a,b),{\mathrm{\&Delta;f}}_i\&le;\mathrm{\&epsiv;}\\ \mathrm{\&delta;}\&Element;N(0,\mathrm{\&sigma;}),{\mathrm{\&Delta;f}}_i>\mathrm{\&epsiv;}\end{array}\right.$

其中 ${\mathrm{\&Delta;f}}_i=|f\left(a_i^k\right)-f\left(a_i^{k-1}\right)|,$ ε是误差的边界值；

步骤4：选出精英种群的最优粒子，判断是否满足收敛条件，如果不满足则返回步骤3，满足进入如下的步骤5；

步骤5：算法结束，输出结果。