CN107490965B - 一种空间自由漂浮机械臂的多约束轨迹规划方法 - Google Patents

Abstract

本发明提供一种空间自由漂浮机械臂的多约束轨迹规划方法，能够满足空间机器人的动力学耦合、关节驱动能力受限、抓捕时机以及多优化指标的要求，驱动关节电机实现对非合作目标的自主抓捕任务。所述方法包括：建立空间机器人的多体动力学模型；基于贝塞尔曲线构建机械臂各关节的轨迹，将关节的轨迹设计过程中出现的约束和优化指标函数均转化为贝塞尔曲线控制点的函数，使得轨迹规划最终表示为贝塞尔曲线控制点作为优化参数的非线性优化问题；空间机器人的轨迹规划问题被转化为一个多约束和多目标的最优化问题，利用非线性优化方法对其进行搜索，得到最优解，完成空间自由漂浮机械臂的多约束轨迹规划。

Description

一种空间自由漂浮机械臂的多约束轨迹规划方法

技术领域

本发明涉及空间机器人的轨迹规划技术，具体为一种空间自由漂浮机械 臂的多约束轨迹规划方法。

背景技术

对空间目标的安全可靠抓捕，是后续执行航天器在轨维护、在轨加注、 在轨制造、在轨维修等任务的前提条件，主要目的在于延长航天器的可用寿 命、增强航天器的性能，以及构建新的空间飞行器。统计显示，国外航天机 构通过ROTEX、ETS-VII、OE、DEOS、FREND等项目完成了针对空间合 作目标的快速自主交会、近距编队飞行、利用机械臂捕获合作目标等技术验 证。但是针对空间非合作目标，由于其运动状态、质量特性等均为未知且不 具备抓捕对接的标准接口，导致抓捕与对接操作的难度大为提升，并且空间 机械臂的路径规划受到动力学耦合、关节输入、输出等多种约束的影响，至 今尚无针对非合作目标成功的抓捕案例。因此亟待探索新的空间机械臂自主 路径规划方法，以解决空间机械臂安全、自主抓捕非合作目标的关键技术问 题。

同时，随着空间机器人科学研究的深入，对于空间机器人的自主性要求 越来越高，特别是针对空间非合作目标的自主抓捕任务。传统的空间机器人 抓捕的均为合作目标，为了应对日益增多的空间碎片以及空间安全形势，利 用空间机器人完成自主抓捕任务成为研究与应用的热点。由于机械臂与基座 之间存在动力学干扰，并且机械臂的驱动能力、输出受限，因此，如何设计 一种能够满足任务需求与能力约束的机械臂轨迹十分必要。

空间机械臂本身作为一个运行在微重力环境中的多输入、多输出、非线 性、强耦合的复杂系统，其轨迹规划问题起源于计算几何学的相关研究课题。 根据对环境信息的掌握程度，轨迹规划问题可以分为基于环境先验信息的全 局轨迹规划与基于不确定环境测量信息的局部路径规划。Yoshida等学者在 “Flight Validation with ETS-VII SpaceRobot and Extension to Kinematically Redundant Arm”中提出了零反作用空间的概念并在ETS-VII项目中进行了 飞行验证，特别是针对运动冗余机械臂，在此空间内设计的机械臂轨迹可以 最小化基座姿态干扰并移除操作过程中的关节速度约束。参考有限时域滚动 优化及在线反馈的思想，基于智能搜索的遗传算法，Huang等人在 “Minimum-TorquePath Planning of Space Robots using Genetic Algorithms” 中设计了基座最小力矩干扰的非完整路径规划。徐文福等人在“自由漂浮空 间机器人的笛卡尔连续路径规划”中针对自由漂浮空间机器人提出了速度级 逆运动学方程的方法，用于实现机械臂末端执行器连续位姿跟踪、基座姿态 调整等任务。史士财等在“空间机械臂全局反作用优化及其地面试验研究” 中采用4-3-4分段关节轨迹描述，设计了空间机械臂全局反作用优化方法，并基于气浮轴承法利用地面试验系统进行了验证。引入动态抓取域的概念并 应用关节主动阻尼控制，通过在零反作用空间中利用多项式函数构建参数化 关节轨迹，Kaigom等在“Optimal Motion Planning of a Space Robot with Base Disturbance Minimization”中采用粒子群算法搜索参数化空间构建了基 座姿态最小干扰的空间机械臂轨迹。Lampariello等在“Generating Feasible Trajectories for Autonomous On-OrbitGrasping of Spinning Debris in a Useful Time”中则应用B样条曲线描述关节参数化轨迹并调用非线性优化中的序 列二次规划法进行搜索。

上述有关空间自由漂浮机器人的轨迹规划方法虽然能够最小基座姿态干 扰的任务目标，但是对于空间自主抓捕而言，不仅仅要考虑到对基座的干扰， 同时要考虑到机械臂运动过程中优化其拓扑构型，便于后期抓捕的实现。对 于抓捕的时机问题，上述研究均直接给出轨迹规划的执行时间，未能充分考 虑机械臂关节的能力问题。

发明内容

针对现有技术中存在的问题，本发明提供一种空间自由漂浮机械臂的多 约束轨迹规划方法，能够满足空间机器人的动力学耦合、关节驱动能力受限、 抓捕时机以及多优化指标的要求，生成机械臂各关节的运动规律，驱动关节 电机实现对非合作目标的自主抓捕任务。

本发明是通过以下技术方案来实现：

一种空间自由漂浮机械臂的多约束轨迹规划方法，包括如下步骤，

首先，根据空间机器人的构型建立其多体动力学模型，多体动力学模型 中包括空间机器人的机械臂与基座的动力学耦合；

其次，选择机械臂末端效应器所到达点的位姿，基于贝塞尔曲线构建机 械臂各关节的轨迹，并将各关节的驱动能力约束和输出受限约束进行转化， 根据需求设计不同的优化指标函数；将关节的轨迹设计过程中出现的约束和 优化指标函数均转化为贝塞尔曲线控制点的函数，使得轨迹规划最终表示为 贝塞尔曲线控制点作为优化参数的非线性优化问题；

最后，空间机器人的轨迹规划问题被转化为一个多约束和多目标的最优 化问题，利用非线性优化方法对其进行搜索，得到最优解，确定轨迹规划时 间，构建机械臂各关节的轨迹，完成空间自由漂浮机械臂的多约束轨迹规划。

优选的，具体步骤如下，

步骤1，构建空间自由漂浮机器人动力学模型；

步骤1.1，空间机器人多体动力学建模；

空间机器人系统包括n+1个运动部件，参考拉格朗日力学体系，空间 机器人的动力学方程表示如下：

其中，H_b、H_bm和H_m分别表示基座、耦合、机械臂惯量矩阵；

为基 座运动加速度，

为关节角加速度；c_b和c_m表示基座和机械臂的科里奥利力 和离心力；τ表示关节电机的控制力矩；f_b和f_c分别表示作用于基座与机械臂 末端执行器的广义外力；J_b和J_e为基座和末端执行器的雅可比矩阵；

当没有外力作用在末端执行器，即f_e＝0，也没有外力作用在基座上时， 即f_b＝0，整个系统被称为空间自由漂浮机器人；

根据动量守恒定律，空间机器人系统的总动量表示如下：

假设初始动量L₀＝0，由于H_b可逆，基座的运动表示如下：

末端执行器的运动表示如下：

其中，J_g为广义雅可比矩阵；

步骤1.2，对机械臂末端效应器所到达点的位姿进行姿态描述；

利用单位四元数

描述姿态，其中η是标量部分，ε是矢量部分； 四元素满足η²+ε^Tε＝1的约束关系；

步骤2，根据空间机器人多体动力学模型和机械臂末端效应器的位姿描 述，进行关节轨迹设计，确定轨迹规划；

步骤2.1，确定约束表示，

在生成关节轨迹的过程中，确定终端约束，以及速度和加速度约束，根 据任务要求确定等式约束，根据机械臂硬件条件确定不等式约束，得到如下 的各种等式和不等式约束：

其中，上标s，f，d的x分别表示起始、终端以及期望的位姿，设计的 关节轨迹要求末端执行器的位姿趋向于期望的位姿，亦即

步骤2.2，确定指标函数，描述轨迹规划问题；

根据约束条件，确定设计需求的至少一个指标函数；

根据需要的指标函数，在一系列等式约束和不等式约束下的轨迹规划问 题表示为一个非线性优化问题，并描述为：

subject to：g_i(θ(t))＜0

h_i(θ(t))＝0；

步骤2.3，将每个关节轨迹θ_i(t)采用贝塞尔曲线表示；

用贝塞尔曲线描述第i个关节的轨迹如下，

多项式b_j，m(τ)被称为伯恩斯坦基准多项式，

为二项式系数，P_ij是构 造贝塞尔曲线的控制点，τ为归一化时间；使得轨迹规划最终表示为贝塞尔 曲线控制点作为优化参数的非线性优化问题，即轨迹规划问题转变为以p为 参数的非线性最优化问题：

subject to：g_i(p)＜0

h_i(p)＝0

p_min≤p≤p_max；

步骤3，采用非线性优化方法解决以p为参数的非线性最优化问题，获 得相应的可行解，从而确定各关节的轨迹，完成空间自由漂浮机械臂的多约 束轨迹规划。

进一步的，步骤1.1中，将基座的运动学表示式代入机械臂末端执行器 的动力学映射方程

得到末端执行器的运动表示方程。

进一步的，步骤1.2中，利用单位四元数描述姿态时，

假设q₁＝{η₁，ε₁}和q₂＝{η₂，ε₂}表示两个旋转坐标系的姿态，那么这两 个坐标之间的差别能够进行如下计算：

其中，

为反对称矩阵，当δε＝0表示两个旋转坐标系完全重合。

进一步的，步骤2.1中，生成关节轨迹的过程中包括如下计算，

对基座与末端执行器的运动，进行计算如下：

对基座与末端执行器的最终位姿与期望的位姿之间的相对差值计算如下：

进一步的，步骤2.2中，所述的指标函数包括如下类型，

定义轨迹规划时间内基座干扰最小的指标函数，

在整个轨迹规划时间内定义关节角度变化指标函数，

定义终端的最大可操作度指标函数，

进一步的，步骤2.3中，使得轨迹规划最终表示为贝塞尔曲线控制点作 为优化参数的非线性优化问题时，

对用贝塞尔曲线描述第i个关节的轨迹表达式进行求导，

由此，轨迹规划的时间通过关节的速度和加速度约束确定：

并且，由等式约束可得：

得到设计的第i个关节的轨迹依赖于参数P_i5，定义p＝[P₁₅，...，P_n5]^T作 为设计变量，确定p，即可确定各关节的轨迹，因此轨迹规划问题转变为以 p为参数的非线性最优化问题：

subject to：g_i(p)＜0

h_i(p)＝0

p_min≤p≤p_max。

与现有技术相比，本发明具有以下有益的技术效果：

本发明基于曲线设计和非线性最优化理论，提出空间自由漂浮机器人的 轨迹规划方法，实现运动学冗余机械臂在空间环境下的关节轨迹规划，实现 空间自由漂浮运动学冗余机械臂的多约束轨迹规划问题，生成机械臂各关节 的运动规律，进而驱动关节电机执行自主抓捕任务，满足关节的驱动能力、 输出约束以及多种优化指标的要求。首先，根据空间机器人的构型建立其多 体动力学模型，并在此基础上分析机械臂与基座的动力学耦合问题；其次， 选择机械臂末端效应器所到达点的位姿，基于Bézier曲线构建机械臂各关 节的轨迹，并将各关节的驱动能力、输出受限等约束进行转化，设计不同的 优化指标函数；最后，空间机器人的轨迹规划问题被转化为一个多约束、多 目标的最优化问题，利用非线性优化方法——粒子群算法对其进行搜索，得 到最优解，构建机械臂各关节的轨迹，进而驱动各关节电机执行抓捕任务。

附图说明

图1为本发明实例中所述空间自由漂浮机械臂轨迹规划流程图。

图2为本发明实例中所述多目标优化的Pareto前沿。

图3为本发明实例中所述空间自由漂浮机械的初始构型。

图4为本发明实例中所述空间自由漂浮机械在仿真1空间的终端构型。

图5为本发明实例中所述空间自由漂浮机械在仿真2空间的终端构型。

图6为本发明实例中所述空间自由漂浮机械在仿真3空间的终端构型。

具体实施方式

下面结合具体的实施例对本发明做进一步的详细说明，所述是对本发明 的解释而不是限定。

本发明提出并设计了一种针对空间自由漂浮机械臂的基于贝塞尔 (Bézier)曲线的多约束轨迹规划算法，满足空间机械臂实现自主抓捕非合 作目标的需求，综合考虑动力学耦合、抓捕时机、机械臂驱动能力约束以及 多优化指标的轨迹规划算法，从而实现对空间非合作目标的自主抓捕任务。

空间机器人由多个单自由度关节构成的机械臂系统以及搭载机械臂系统 的基座组成，假设机械臂共有n个关节，那么空间机器人系统共有n+1个可 以运动的部件，由于基座的运动自由度为6，因此，整个系统的自由度为 n+6。对于空间机器人系统，为了减少能量消耗，在机械臂运动过程中，基 座的姿轨控系统不被启用，因此整个系统为一个自由漂浮系统，满足动量守 恒定律。

如图1所示，本发明设计的轨迹规划算法主要由三部分组成，一是多体 动力学建模与姿态表示，由于机械臂与基座之间存在动力学耦合，因此需要 建立空间自由漂浮机械臂的动力学模型，并采用合适的位姿表示；二是关节 轨迹设计，主要涉及到三方面的内容，约束表示，指标函数以及Bézier曲 线表示，主要对轨迹规划过程中出现的约束、优化指标进行描述，并设计 Bézier曲线，将关节的轨迹设计过程中出现的约束、优化指标均转化为Bézier曲线控制点的函数，最终表示Bézier曲线控制点作为优化参数的非线 性优化问题；三是非线性优化算法，主要用于对转化后的非线性优化问题进 行求解，本发明中利用粒子群优化算法进行搜索，从而得到构建Bézier曲 线的最优控制点。

本发明空间自由漂浮机械臂的多约束轨迹规划方法的具体步骤如下。

第一步：空间自由漂浮机器人动力学建模。

1)空间机器人动力学建模；

空间机器人系统包括n+1个运动部件，参考拉格朗日(Lagrange)力学 体系，空间机器人的动力学方程可以表示如下：

其中，H_b、H_bm和H_m分别表示基座、耦合、机械臂惯量矩阵；

为基 座运动加速度，

为关节角加速度；c_b和c_m表示基座和机械臂的科里奥利力 和离心力；τ表示关节电机的控制力矩；f_b和f_e分别表示作用于基座与机械臂 末端执行器的广义外力；J_b和J_e为基座和末端执行器的雅可比(Jacobian)矩 阵。当没有外力作用在末端执行器f_e＝0，也没有外力作用在基座上时 f_b＝0，整个系统被称为空间自由漂浮机器人。根据动量守恒定律，空间机器人系统的总动量可以表示如下：

一般假设初始动量L₀＝0，由于H_b可逆，基座的运动可以被表示如下：

将上式代入机械臂末端执行器的动力学映射方程

关于 末端执行器的运动可以表示如下：

J_g被称为广义Jacobian矩阵。如上所示，机械臂的运动将直接影响基座 的位姿，因此，在设计机械臂关节轨迹时，必须考虑动力学耦合的影响。

2)姿态描述；

机械臂末端执行器到达相应点位，不仅要考虑其位置信息，同时要考虑 机械臂末端的姿态信息。对于姿态的描述一般采用Euler角、选择轴/角度、 或者单位四元数法。Euler角姿态描述方法由于奇异点的存在，在此不予采 用，利用单位四元数

描述姿态，其中η是标量部分，ε是矢量部分。 四元素满足η²+ε^Tε＝1的约束关系，假设q₁＝{η₁，ε₁}和q₂＝{η₂，ε₂}表示两个 选择坐标的姿态，那么这两个坐标之间的差别可以如下计算：

其中为反对称矩阵，注意到当δε＝0表示两个旋转坐标完全重合。

第二步：轨迹规划方法。

1)约束处理

轨迹规划的目的是生成可行的机械臂关节轨迹以完成要求的空间任务， 一般情况下，轨迹规划应满足如下要求：指定的指标函数应在可行域内进行 优化；保证关节位置、速度的连续性以及最小化不期望干扰。

空间自由漂浮机器人的轨迹规划问题不同于地面固定基座机器人，其非 完整特性导致了空间机器人的末端运动不仅依赖于逆运动学，而且与动力学 耦合效应有关。针对基座与末端执行器的运动，可以计算如下：

基座与末端执行器的最终位姿与期望的位姿之间的相对差值可以计算如 下：

生成关节轨迹的过程中，必须要考虑到终端约束，以及速度和加速度的 约束限制，根据任务要求确定等式约束，根据机械臂硬件条件确定不等式约 束，具体为如下的各种等式和不等式约束：

其中，上标s，f，d的x分别表示起始、终端以及期望的位姿。设计的 关节轨迹要求末端执行器的位姿趋向于期望的位姿，亦即

2)指标函数

空间机器人轨迹规划的过程除了要求完成末端执行器的任务之外，对于 运动学冗余机械臂而言，可以定义其他的优化指标，能够根据约束条件，确 定设计需求的指标函数；例如，定义轨迹规划时间内基座干扰最小：

为了控制各关节移动距离总和，在整个轨迹规划时间内定义关节角度变 化：

为了得到最优构型，定义终端的最大可操作度：

因此，结合需要的指标函数在一系列等式约束和不等式约束下的轨迹规 划问题可以描述为：

subject to：g_i(θ(t))＜0

h_i(θ(t))＝0

3)Bézier曲线

如前所示，轨迹规划问题可以表示为一个非线性优化问题。每个关节轨 迹θ_i(t)可以采用Bézier曲线来表示，本发明中采用5阶Bézier曲线描述第i 个关节的轨迹

多项式b_j，m(τ)被称为伯恩斯坦(Bernstein)基准多项式，

为二项式系 数。P_ij是构造Bézier曲线的控制点，τ为归一化时间，对上式进行求导：

由此，轨迹规划的时间可以通过关节的速度和加速度约束确定：

并且，由等式约束可得：

因此，设计的第i个关节的轨迹主要依赖于参数P_i5，定义 p＝[P₁₅，…，P_n5]^T作为设计变量，一旦其确定，即可确定各关节的轨迹，因 此轨迹规划问题转变为以p为参数的非线性最优化问题：

subject to：g_i(p)＜0

h_i(p)＝0

p_min≤p≤p_max

针对上述问题，可以采用非线性优化方法解决，例如序贯二次规划法、 遗传算法、粒子群算法等，获得相应的可行解，从而确定各关节的轨迹。

在三种仿真场景下，采用本发明所给出的针对7自由度机械臂轨迹规划 效果分别如下。

1)轨迹规划从机械臂初始构型运动到指定的末端执行器位姿

仿真1的机械臂及基座的初始条件为 其中，初始构型以及基座的初始状态如上式 所示，要求规划7自由度机械臂的关节轨迹使得在满足约束的条件下保证机 械臂的末端执行器位姿到达期望的值

利 用粒子群算法搜索获得如下结果，

T＝30s，p^*＝(0.503,0.788,2.303,-1.334,0.612,1.184,4.005)。

其中，轨迹规划时间为30s，获得构造Bézier曲线的控制点后，即可完 成轨迹规划任务。

2)要求机械臂执行如仿真1的任务同时最小化基座姿态干扰

仿真2采用和仿真1相同的机械臂与基座初始构型，并要求如仿真1一 样实现末端执行器的期望位姿，但是还需要考虑最小化基座姿态干扰。通过 利用粒子群算法进行搜索，获得满足任务要求的构建Bézier曲线的最优控 制点为p^*＝(-2.392，-0.436，-1.639，-0.884,0.810,0.980,3.319)，所规划轨迹的末端执行器 的位姿误差为δx_e＝[-0.412,-0.140,-1.503,13.572,2.239,-1.706]×10^-3，其中，轨迹规划 时间为30s，获得构造Bézier曲线的控制点后，即可完成轨迹规划任务。

3)要求机械臂执行如仿真1的任务同时最小化基座姿态干扰和最大化 机械臂可操作度

仿真3采用和仿真1相同的机械臂与基座初始构型，并要求如仿真1一 样实现末端执行器的期望位姿，但是还需要考虑最小化基座姿态干扰。通过 利用粒子群算法进行搜索，获得满足任务要求的构建Bézier曲线的最优控 制点为p^*＝(-2.392,-0.436,-1.639,-0.884,0.810,1.980,3.319)，所规划轨迹的末端执行器 的位姿误差为δx_e＝[-0.412,0.140,-1.503,13.572,2.239,-1.706]×10^-3，其中，轨迹规划时 间为30s，获得构造Bézier曲线的控制点后，即可完成轨迹规划任务。

计算结果表明针对存在多约束的情形，基于Bézier曲线构建的机械臂 关节轨迹可以有效的进行约束转化，根据设计的优化指标，利用粒子群搜索 算法可以获得可行解并构建满足需求的关节轨迹。通过分析计算结果，设计 的关节轨迹驱动末端执行器完成任务后，终端的位置误差小于5mm，姿态 误差小于1deg。对于多目标优化问题，由于存在多个可行解，搜索获得了 多目标优化的Pareto前沿，图2显示了针对多目标优化的帕雷托(Pareto) 前沿以及三组仿真在其中的分布。

可以看出，针对所述的三种仿真场景，采用本发明设计的轨迹规划方案， 其终端的位姿误差都很小，并且可以根据不同的优化指标选择有效的可行解 来构建关节轨迹，由此说明本发明所给出的轨迹规划方案对于空间复杂机械 臂执行抓捕任务的有效性，同时验证了本发明所提出的轨迹规划的可行性。

本发明具有实用性和可实施性。已通过仿真验证其轨迹规划的终端精度 满足位置误差<5mm，姿态误差<1deg。图4-图6显示了本发明提到的三种 仿真情形。图3为空间机械臂的初始构型；图4为仿真1中仅考虑终端位姿 执行轨迹规划的情形，可以看出，由于未对基座的姿态干扰进行优化，从而 导致虽能够完成终端的任务要求，但是对基座干扰较大；图5显示了仿真2 中既考虑终端要求又优化基座干扰的情形，可以看出，规划的路径与仿真1 不同，终端位姿达到要求，同时对基座的干扰大大缩小；图6显示了仿真3 中考虑终端要求，同时考虑基座干扰和构型可操作度的情形，可以看出，规 划的轨迹由于多优化指标的影响，得到了完全不同于仿真1和仿真2的轨迹， 对基座的干扰大于仿真2的情形，这是由于多优化指标对轨迹规划的影响， 这可以从图2中得到印证。

Claims (6)

1.一种空间自由漂浮机械臂的多约束轨迹规划方法，其特征在于，包括如下步骤，

首先，根据空间机器人的构型建立其多体动力学模型，多体动力学模型中包括空间机器人的机械臂与基座的动力学耦合；

其次，选择机械臂末端效应器所到达点的位姿，基于贝塞尔曲线构建机械臂各关节的轨迹，并将各关节的驱动能力约束和输出受限约束进行转化，根据需求设计不同的优化指标函数；将关节的轨迹设计过程中出现的约束和优化指标函数均转化为贝塞尔曲线控制点的函数，使得轨迹规划最终表示为贝塞尔曲线控制点作为优化参数的非线性优化问题；

最后，空间机器人的轨迹规划问题被转化为一个多约束和多目标的最优化问题，利用非线性优化方法对其进行搜索，得到最优解，确定轨迹规划时间，构建机械臂各关节的轨迹，完成空间自由漂浮机械臂的多约束轨迹规划；

具体步骤如下，

步骤1，构建空间自由漂浮机器人动力学模型；

步骤1.1，空间机器人多体动力学建模；

空间机器人系统包括n+1个运动部件，参考拉格朗日力学体系，空间机器人的动力学方程表示如下：

其中，H_b、H_bm和H_m分别表示基座、耦合、机械臂惯量矩阵；为基座运动加速度，

为关节角加速度；c_b和c_m表示基座和机械臂的科里奥利力和离心力；τ表示关节电机的控制力矩；f_b和f_c分别表示作用于基座与机械臂末端执行器的广义外力；J_b和J_e为基座和末端执行器的雅可比矩阵；

当没有外力作用在末端执行器，即f_e＝0，也没有外力作用在基座上时，即f_b＝0，整个系统被称为空间自由漂浮机器人；

根据动量守恒定律，空间机器人系统的总动量表示如下：

假设初始动量L₀＝0，由于H_b可逆，基座的运动表示如下：

末端执行器的运动表示如下：

其中，J_g为广义雅可比矩阵；

步骤1.2，对机械臂末端效应器所到达点的位姿进行姿态描述；

利用单位四元数

描述姿态，其中η是标量部分，ε是矢量部分；单位四元数 满足η²+ε^Tε＝1的约束关系；

步骤2，根据空间机器人多体动力学模型和机械臂末端效应器的位姿描述，进行关节轨迹设计，确定轨迹规划；

步骤2.1，确定约束表示，

在生成关节轨迹的过程中，确定终端约束，以及速度和加速度约束，根据任务要求确定等式约束，根据机械臂硬件条件确定不等式约束，得到如下的各种等式和不等式约束：

其中，角标s，f，d分别表示起始、终端以及期望，设计的关节轨迹要求末端执行器的位姿趋向于期望的位姿，亦即

步骤2.2，确定指标函数，描述轨迹规划问题；

根据约束条件，确定设计需求的至少一个指标函数；

根据需要的指标函数，在一系列等式约束和不等式约束下的轨迹规划问题表示为一个非线性优化问题，并描述为：

subject to：g_i(θ(t))＜0

h_i(θ(t))＝0；

步骤2.3，将每个关节轨迹θ_i(t)采用贝塞尔曲线表示；

用贝塞尔曲线描述第i个关节的轨迹如下，

多项式b_j，m(u)被称为伯恩斯坦基准多项式，为二项式系数，P_ij是构造贝塞尔曲线的控制点，u为归一化时间；使得轨迹规划最终表示为贝塞尔曲线控制点作为优化参数的非线性优化问题，即轨迹规划问题转变为以p为参数的非线性最优化问题：

subject to：g_i(p)＜0

h_i(p)＝0

p_min≤p≤p_max；

步骤3，采用非线性优化方法解决以p为参数的非线性最优化问题，获得相应的可行解，从而确定各关节的轨迹，完成空间自由漂浮机械臂的多约束轨迹规划。

2.根据权利要求1所述的一种空间自由漂浮机械臂的多约束轨迹规划方法，其特征在于，步骤1.1中，将基座的运动学表示式代入机械臂末端执行器的动力学映射方程

得到末端执行器的运动表示方程。

3.根据权利要求1所述的一种空间自由漂浮机械臂的多约束轨迹规划方法，其特征在于，步骤1.2中，利用单位四元数描述姿态时，

假设q₁＝{η₁，ε₁}和q₂＝{η₂，ε₂}表示两个旋转坐标系的姿态，那么这两个坐标之间的差别能够进行如下计算：

其中，

为反对称矩阵，当δε＝0表示两个旋转坐标系完全重合。

4.根据权利要求1所述的一种空间自由漂浮机械臂的多约束轨迹规划方法，其特征在于，步骤2.1中，生成关节轨迹的过程中包括如下计算，

对基座与末端执行器的运动，进行计算如下：

对基座与末端执行器的最终位姿与期望的位姿之间的相对差值计算如下：

5.根据权利要求1所述的一种空间自由漂浮机械臂的多约束轨迹规划方法，其特征在于，步骤2.2中，所述的指标函数包括如下类型，

定义轨迹规划时间内基座干扰最小的指标函数，

在整个轨迹规划时间内定义关节角度变化指标函数，

定义终端的最大可操作度指标函数，

6.根据权利要求1所述的一种空间自由漂浮机械臂的多约束轨迹规划方法，其特征在于，步骤2.3中，使得轨迹规划最终表示为贝塞尔曲线控制点作为优化参数的非线性优化问题时，

对用贝塞尔曲线描述第i个关节的轨迹表达式进行求导，

由此，轨迹规划的时间通过关节的速度和加速度约束确定：

并且，由等式约束可得：

得到设计的第i个关节的轨迹依赖于参数P_i5，定义p＝[P₁₅，…，P_n5]^T作为设计变量，确定p，即可确定各关节的轨迹，因此轨迹规划问题转变为以p为参数的非线性最优化问题：

subject to：g_i(p)＜0

h_i(p)＝0

p_min≤p≤p_max。

Images