CN113414761B - 一种冗余机械臂运动轨迹优化的方法 - Google Patents

Abstract

本申请公开了一种冗余机械臂运动轨迹优化的方法，该方法包括：建立空间障碍物环境模型以及机械臂模型，根据空间障碍物环境模型、机械臂模型以及预设的A^*算法在障碍物环境中确定出机械臂从预设的起始点运动到预设的目标点的初始避障路径；基于预设的梯度投影算法建立机械臂运动优化数学模型，以及根据预设的任务需求确定轨迹优化向量，其中，数学模型用于对机械臂的运动速度进行优化；根据数学模型确定初始避障路径中任意相邻两个节点之间的机械臂关节运动规律，根据运动规律以及轨迹优化向量对初始避障路径进行优化得到优化后的路径。本申请解决了现有技术机械臂的轨迹优化无法满足实际需求的技术问题。

Description

一种冗余机械臂运动轨迹优化的方法

技术领域

本申请涉及机械臂运动轨迹优化技术领域，尤其涉及一种冗余机械臂运动轨迹优化的方法。

背景技术

随着航天器技术的发展，航天器被广泛应用于多个领域，机械臂作为航天器中一个重要的部件。空间机械臂在执行在轨操作任务的过程中，由于受到外界环境约束及自身结构限制，可能与环境物体或自身发生碰撞，严重影响机械臂的安全运行。特别是，当基座处于自由漂浮状态时，空间机械臂执行任务过程中与障碍物发生碰撞，导致机械臂偏移、基座失稳等严重后果。因此，空间机械臂避障轨迹规划对在轨操作任务的顺利完成和机械臂的安全稳定运行至关重要。

目前，常见的机械臂避障轨迹优化方法主要包括A*、D*或者人工势场法等，现有的避障轨迹规划方法主要自主搜索到一条从起始位姿达到目标位姿的避障路径，但是，在空间机械臂路径优化过程中，不仅要考虑到避障需求，还需要考虑到空间机械臂自身运动学性能或动力学性能，故在空间机械臂轨迹优化过程中需要将避障、关节之间的奇异性、基座姿态保持等因素结合起来对空间机械臂路径进行规划，因此，现有技术机械臂的轨迹优化无法满足实际需求。

发明内容

本申请解决的技术问题是：针对现有技术机械臂的轨迹优化无法满足实际需求。本申请提供了一种冗余机械臂运动轨迹优化的方法，在本申请实施例所提供的方案中，通过A^*算法构建初始避障路径，然后再根据基于预设的梯度投影算法建立机械臂运动优化数学模型，以及根据预设的任务需求确定轨迹优化向量，再根据数学模型确定初始避障路径中任意相邻两个节点之间的机械臂关节运动规律，根据运动规律以及轨迹优化向量对初始避障路径进行优化得到优化后的路径，即在空间机械臂轨迹优化过程中需要将避障、关节之间的奇异性、基座姿态保持等因素结合起来对空间机械臂路径进行规划，使得机械臂的轨迹优化能够满足实际需求。

第一方面，本申请实施例提供一种冗余机械臂运动轨迹优化的方法，该方法包括：

建立空间障碍物环境模型以及机械臂模型，根据所述空间障碍物环境模型、所述机械臂模型以及预设的A^*算法在障碍物环境中确定出机械臂从预设的起始点运动到预设的目标点的初始避障路径，其中，所述初始避障路径包括多个节点；

基于预设的梯度投影算法建立机械臂运动优化数学模型，以及根据预设的任务需求确定轨迹优化向量，其中，所述数学模型用于对机械臂的运动速度进行优化；

根据所述数学模型确定所述初始避障路径中任意相邻两个节点之间的机械臂关节运动规律，根据所述运动规律以及所述轨迹优化向量对所述初始避障路径进行优化得到优化后的路径。

在本申请实施例所提供的方案中，通过A^*算法构建初始避障路径，然后再根据基于预设的梯度投影算法建立机械臂运动优化数学模型，以及根据预设的任务需求确定轨迹优化向量，再根据数学模型确定初始避障路径中任意相邻两个节点之间的机械臂关节运动规律，根据运动规律以及轨迹优化向量对初始避障路径进行优化得到优化后的路径，即在空间机械臂轨迹优化过程中需要将避障、关节之间的奇异性、基座姿态保持等因素结合起来对空间机械臂路径进行规划，使得机械臂的轨迹优化能够满足实际需求。

可选地，根据所述空间障碍物环境模型、所述机械臂模型以及预设的A^*算法在障碍物环境中确定出机械臂从起始点运动到目标点的初始避障路径，包括：

创建OPEN表和CLOSE表，设置搜索步长和设置所述CLOSE表为空表，并初始化预设的代价估计函数，其中，所述OPEN表用于存储已生成且待访问的节点，所述CLOSE表用于存储访问过且代价值最小的节点；

将所述起始点插入到所述OPEN表中，根据初始化后的代价估计函数从所述OPEN表中选取具有最小代价值的节点为最佳节点，并将所述最佳节点放入所述CLOSE表，以及将所述最佳节点从所述OPEN表中删除，并判断所述最佳节点是否为目标点；

若不是，则根据所述搜索步长以及所述最佳节点确定下一节点，计算所述下一节点的代价值，以及根据所述下一节点的代价值更新所述CLOSE表，直到确定出所述目标点为止，根据所述更新后的CLOSE表确定从所述起始点运动到所述目标点的多个节点，根据所述起始点、所述目标点以及所述多个节点确定出所述初始避障路径。

可选地，计算所述下一节点的代价值，以及根据所述下一节点的代价值更新所述CLOSE表，包括：

判断所述下一节点是否在空间机械臂预设的自由运动空间；

若是，则计算所述起始点到所述下一节点的代价值，以及判断所述下一节点是否在所述OPEN表中；

若在，则根据所述最佳节点的代价值和所述下一节点的代价值的比较结果更新所述最佳节点；

根据更新后的最佳节点更新所述起始点到所述目标点的代价值以及所述CLOSE表。

可选地，基于预设的梯度投影算法建立机械臂运动优化数学模型，包括：

通过下式建立所述数学模型：

其中，

表示机械臂关节角速度；

表示机械臂末端速度；E表示单位矩阵；J表示空间机械臂的广义雅克比矩阵；

表示J的伪逆，

表示矩阵J的零空间；Φ表示优化向量。

可选地，所述轨迹优化向量，包括：避奇异优化向量、避障优化向量、基座保持优化向量和复位优化向量。

可选地，所述避奇异优化向量为：

其中，Φ_sg表示所述避奇异优化向量；κ表示预设的优化系数；q₁…q_n表示具有n个自由度的机械臂各关节角度。

可选地，所述避障优化向量为：

其中，Φ_qm表示所述避障优化向量；q_imax和q_imin分别表示第i个关节考虑避障后的最大角度和最小角度。

可选地，所述基座保持优化向量为：

其中，Φ_b表示所述基座保持优化向量；[J_TωH_ωq]^T表示空间机械臂与基座的耦合惯量矩阵；r_0g表示基座质心指向空间机械臂系统质心的矢量；·^×表示矩阵算子，对于一个三维向量r＝[r_x r_y r_z]^T，其矩阵算子可表示为

可选地，所述复位优化向量为：

Φ_r＝κ_J·Φ_J+κ_a·Φ_a

Φ_a＝J_ωω_e

其中，Φ_r表示所述复位优化向量；κ_J和κ_a分别表示预设的关节复位优化系数和姿态复位优化系数；Φ_J和Φ_a分别表示关节复位优化向量和姿态复位优化向量；q_i和q_{i_int}分别表示空间机械臂当前时刻第i个关节的角度和初始角度；ω_e表示当前基座姿态与初始基座姿态偏差的角速度值；J_ω表示取空间机械臂广义雅克比矩阵J的后三行；M表示基座的质量；H_ω表示基座的惯量矩阵。

可选地，根据所述数学模型确定所述初始避障路径中任意相邻两个节点之间的机械臂关节运动规律，包括：

确定机械臂当前关节角度，根据所述当前关节角度确定机械臂末端经过所述起始点、所述目标点以及所述任意相邻节点的样条曲线轨迹，采用带圆弧过渡的梯形规则以及所述样条曲线轨迹规划所述机械臂末端速度；

根据所述机械臂末端速度确定当前规划时长，并判断当前规划时长是否达到预设的规划总时长；

若否，则根据所述机械臂末端速度、所述当前关节角度以及所述数学模型计算当前关节角速度，并根据预设的控制周期时长以及所述当前关节角速度调整所述当前规划时长，直到达到所述规划总时长为止。

附图说明

图1为本申请实施例所提供的一种冗余机械臂运动轨迹优化的方法的流程示意图；

图2为本申请实施例所提供的一种平滑曲线系数随时间的变化曲线。

具体实施方式

本申请实施例提供的方案中，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其它实施例，都属于本申请保护的范围。

为了更好的理解上述技术方案，下面通过附图以及具体实施例对本申请技术方案做详细的说明，应当理解本申请实施例以及实施例中的具体特征是对本申请技术方案的详细的说明，而不是对本申请技术方案的限定，在不冲突的情况下，本申请实施例以及实施例中的技术特征可以相互组合。

以下结合说明书附图对本申请实施例所提供的一种冗余机械臂运动轨迹优化的方法做进一步详细的说明，该方法具体实现方式可以包括以下步骤(方法流程如图1所示)：

步骤101，建立空间障碍物环境模型以及机械臂模型，根据所述空间障碍物环境模型、所述机械臂模型以及预设的A^*算法在障碍物环境中确定出机械臂从预设的起始点运动到预设的目标点的初始避障路径，其中，所述初始避障路径包括多个节点。

具体的，在本申请实施例所提供的方案中，空间障碍物模型以及机械臂模型是在三维空间环境中建立的，其中，所述空间障碍物模型采用球形包络描述。在建立机械臂模型之后，还需要在障碍物环境中确定出从起始点到目标点的避障路径。在本申请实施例所提供的方案中，采用预设的A^*算在障碍物环境中确定出从起始点到目标点的初始路径的方式有多种，其中，初始避障路径为避障路径，下面以一种较佳的方式为例进行说明。

在一种可能实现的方式中，根据所述空间障碍物环境模型、所述机械臂模型以及预设的A^*算法在障碍物环境中确定出机械臂从起始点运动到目标点的初始避障路径，包括：

创建OPEN表和CLOSE表，设置搜索步长和设置所述CLOSE表为空表，并初始化预设的代价估计函数，其中，所述OPEN表用于存储已生成且待访问的节点，所述CLOSE表用于存储访问过且代价值最小的节点；

将所述起始点插入到所述OPEN表中，根据初始化后的代价估计函数从所述OPEN表中选取具有最小代价值的节点为最佳节点，并将所述最佳节点放入所述CLOSE表，以及将所述最佳节点从所述OPEN表中删除，并判断所述最佳节点是否为目标点；

若不是，则根据所述搜索步长以及所述最佳节点确定下一节点，计算所述下一节点的代价值，以及根据所述下一节点的代价值更新所述CLOSE表，直到确定出所述目标点为止，根据所述更新后的CLOSE表确定从所述起始点运动到所述目标点的多个节点，根据所述起始点、所述目标点以及所述多个节点确定出所述初始避障路径。

进一步，在一种可能实现的方式中，计算所述下一节点的代价值，以及根据所述下一节点的代价值更新所述CLOSE表，包括：

判断所述下一节点是否在空间机械臂预设的自由运动空间；

若是，则计算所述起始点到所述下一节点的代价值，以及判断所述下一节点是否在所述OPEN表中；

若在，则根据所述最佳节点的代价值和所述下一节点的代价值的比较结果更新所述最佳节点；

根据更新后的最佳节点更新所述起始点到所述目标点的代价值以及所述CLOSE表。

为了便于理解，下面对A^*算法原理进行简要介绍。

A*算法是典型的启发式搜索方法，被广泛应用于最优路径的求解，其核心在于估价函数的设计。在选择当前节点的下一个评估节点时引入估价函数f(x)

f(x)＝g(x)+h(x)

式中，g(x)是从起始点到节点x最小代价路径的实际代价；h(x)是从节点x到目标点路径的估计代价。

为便于利用A*算法，定义空间机械臂的7个关节角度为一个七维数组，起始关节角度为

当前关节角度为

第i步最佳关节角度为

关节角度搜索步长为

第i步的后继节点关节角度为

(由关节角度搜索步长确定)，期望关节角度为

定义估价函数

其中，q_{i m}为第i步机械臂关节角度中的第m个关节的关节角度，q_{des m}为机械臂期望关节角度中的第m个关节的关节角度。由此可以求出第i步后继节点关节角度中f_i(q)值最小的节点作为此步最优关节角度，根据步长生成第i+1步的后继节点关节角度，重复此过程可保证每一步都可以得到最优关节角度，因此通过该估价函数可以实现空间机械臂从起始关节角度到目标关节角度的构型空间最优路径规划。

为了便于分析，设定3个表，即OPEN表、CLOSE表和后继节点表。把起始点q_ini放入OPEN表，初始化f₀(q)＝h₀(q)，置CLOSE表为空表。

重复下列过程，直到找到目标点为止。若OPEN表为空表，表明空间机械臂路径规划失败；否则，开始以下循环。

Step1选取OPEN表中未设置过的具有最小f值的节点为最佳节点q_{best_i}，并将其放入CLOSE表，同时将其从OPEN表中删除；

Step2判断q_{best_i}是否为目标点q_des，若是，则求解成功，循环结束；否则进行下一步；

Step3根据设定的步长l_step值，计算后继节点q_{suc_i}；

Step4按下述流程对每个后继节点q_{suc_i}进行处理：

a.判断q_{suc_i}是否在空间机械臂自由运动空间内，若是，建立从q_{suc_i}返回q_{best_i}的指针；否则转至Step3；

b.计算

c.判断q_{suc_i}是否在OPEN表中，若是转至d；否则转至e；

d.定义数组q_{old_i}＝q_{suc_i}，将q_{old_i}填到q_{best_i}的后继节点表中，进而比较新旧轨迹代价：若g(q_{old_i})＞g(q_{suc_i})，更新q_{old_i}的父辈节点为q_{best_i}，并更新g(q_{old_i})＝g(q_{suc_i})，进一步修正f(q_{old_i})值，同时对OPEN表重新排序；否则转至g；

e.判断q_{suc_i}是否在CLOSE表中，若是，转至d；否则转至f；

f.将q_{suc_i}放入OPEN表，并添入到q_{best_i}的后继表中，转至g；

g.计算f(q_{suc_i})，并转至a。

步骤102，基于预设的梯度投影算法建立机械臂运动优化数学模型，以及根据预设的任务需求确定轨迹优化向量，其中，所述数学模型用于对机械臂的运动速度进行优化。

具体的，在本申请实施例所提供的方案中，机械臂是具有多个自由度的冗余机械臂，该机械臂的运动学方程可通过如下公式表示：

其中，x_e表示空间机械臂末端位姿，

m表示操作空间自由度数；q表示空间机械臂关节角度，

n表示机械臂关节自由度数；

表示基座位姿；

为基座雅克比矩阵；

为固定基座机械臂雅克比矩阵；

为广义雅克比矩阵。

冗余空间机械臂在广义雅克比矩阵的零空间运行时，机械臂的速度值满足

可以在保证机械臂末端位姿不发生改变的情况下进行自运动。根据这一特点，可在满足末端运动状态的同时，通过空间机械臂自运动改变空间机械臂在关节空间的运动轨迹，从而实现考虑避奇异、避障等多种运动特性下的运动轨迹优化。

进一步，在本申请实施例所提供的方案中，采用梯度投影法对机械臂的运动轨迹优化，其中，梯度投影法通过如下公式表示：

其中，q表示机械臂关节角度；x_e表示机械臂末端位姿；E表示单位矩阵；J表示空间机械臂的广义雅克比矩阵；

表示J的伪逆，

表示矩阵J的零空间；Φ表示优化向量。

进一步，在本申请实施例所提供的方案中，基于梯度投影算法建立机械臂优化的数学模型的方式有多种，下面以一种较佳的方式为例进行说明。

在一种可能实现方式中，基于预设的梯度投影算法建立机械臂运动优化数学模型，包括：

通过下式建立所述数学模型：

其中，

表示机械臂关节角速度；

表示机械臂末端速度；E表示单位矩阵；J表示空间机械臂的广义雅克比矩阵；

表示J的伪逆，

表示矩阵J的零空间；Φ表示优化向量。

在一种可能实现方式中，所述轨迹优化向量，包括：避奇异优化向量、避障优化向量、基座保持优化向量和复位优化向量。

在一种可能实现方式中，所述避奇异优化向量为：

其中，Φ_sg表示所述避奇异优化向量；k表示预设的优化系数；q₁…q_n表示具有n个自由度的机械臂各关节角度。

在一种可能实现方式中，所述避障优化向量为：

其中，Φ_qm表示所述避障优化向量；q_imax和q_imin分别表示第i个关节考虑避障后的最大角度和最小角度。

在一种可能实现方式中，所述基座保持优化向量为：

其中，Φ_b表示所述基座保持优化向量；[J_Tω H_ωq]^T表示空间机械臂与基座的耦合惯量矩阵；r_0g表示基座质心指向空间机械臂系统质心的矢量；·^×表示矩阵算子，对于一个三维向量r＝[r_x r_y r_z]^T，其矩阵算子可表示为

在一种可能实现方式中，所述复位优化向量为：

Φ_r＝κ_J·Φ_J+λ_a·Φ_a

Φ_a＝J_ωω_e

其中，Φ_r表示所述复位优化向量；κ_J和κ_a分别表示预设的关节复位优化系数和姿态复位优化系数；Φ_J和Φ_a分别表示关节复位优化向量和姿态复位优化向量；q_i和q_{i_int}分别表示空间机械臂当前时刻第i个关节的角度和初始角度；ω_e表示当前基座姿态与初始基座姿态偏差的角速度值；J_ω表示取空间机械臂广义雅克比矩阵J的后三行；C表示基座的质量；H_ω表示基座的惯量矩阵。

具体的，在本申请实施例所提供的方案中，任务项包括主任务、从任务以及优化任务，其中，机械臂末端执行器轨迹跟踪通常被设定为主任务，机械臂关节构型及基座位姿重复运动规划设定为从任务，将从任务引入主任务零空间作为优化任务。在确定出机械臂初始避障路径之后，需要确定机械臂根据初始避障路径运动过程，机械臂的任务项。

步骤103，根据所述数学模型确定所述初始避障路径中任意相邻两个节点之间的机械臂关节运动规律，根据所述运动规律以及所述轨迹优化向量对所述初始避障路径进行优化得到优化后的路径。

在一种可能实现的方式中，根据所述机械臂优化的数学模型规划相邻所述任务项之间的关节运动规律，包括：确定机械臂当前关节角度，根据所述当前关节角度确定机械臂末端经过所述起始点、所述目标点以及所述任意相邻节点的样条曲线轨迹，采用带圆弧过渡的梯形规则以及所述样条曲线轨迹规划所述机械臂末端速度；根据所述机械臂末端速度确定当前规划时长，并判断当前规划时长是否达到预设的规划总时长；若否，则根据所述机械臂末端速度、所述当前关节角度以及所述数学模型计算当前关节角速度，并根据预设的控制周期时长以及所述当前关节角速度调整所述当前规划时长，直到达到所述规划总时长为止。

具体的，空间机械臂执行在轨操作任务过程中，主从任务可能存在冲突，考虑到重复运动规划要求终止时刻关节构型以及基座姿态复位，因此，在任务执行的后半程引入基座复位优化向量即可。同时考虑到优化能力和基座姿态对空间机械臂系统稳定运行影响，在任务前半程引入基座姿态保持优化向量。

为了便于理解，下面对基座姿态保持优化向量以及基座复位优化向量计算过程进行简要介绍。

具体的，在无外力/力矩作用时，空间机械臂在运动过程中动量守恒，有如下关系式：

其中，

为在空间机械臂系统质心坐标系下表示的基座惯性矩阵；

为机械臂与基座的耦合惯性矩阵；

为空间机械臂基座在惯性系下的速度。

空间机械臂基座在惯性系下的线速度v₀可通过如下公式表示：

进一步，上述空间机械臂在运动过程中动量守恒关系可转换为：

其中，

进一步，为了实现基座姿态保持优化，上式可转换为：

根据该公式可解得：

考虑到基座姿态保持要求，设ω₀＝[0,0,0]^T，可得基座姿态保持优化向量为：

任务前半程考虑基座姿态保持优化，任务后半程考虑基座姿态复位优化的空间机械臂运动规划为：

在空间机械臂运行过程的前半程引入基座姿态保持优化算子，后半程引入重复运动算子，为了保证空间机械臂运行过程中速度连续平滑，设计平滑系数ρ_＝与ρ_b分别为

式中，

T_m、T_R、T_Z和T_A分别为基座姿态复位优化开始时刻、基座姿态保持优化终止时刻、基座姿态复位优化终止的开始时刻与总规划时间。具体的，系数ρ_＝与ρ_b的变化曲线如图2所示。

在给定空间机械臂末端轨迹情况下，采用上述空间机械臂重复运动规划对关节角速度进行规划，可保证空间机械臂在完成末端轨迹跟踪的同时，使关节构型及基座位姿在终止时刻复位，实现空间机械臂的重复运动规划。

在本申请实施例所提供的方案中，通过A^*算法构建初始避障路径，然后再根据基于预设的梯度投影算法建立机械臂运动优化数学模型，以及根据预设的任务需求确定轨迹优化向量，再根据数学模型确定初始避障路径中任意相邻两个节点之间的机械臂关节运动规律，根据运动规律以及轨迹优化向量对初始避障路径进行优化得到优化后的路径，即在空间机械臂轨迹优化过程中需要将避障、关节之间的奇异性、基座姿态保持等因素结合起来对空间机械臂路径进行规划，使得机械臂的轨迹优化能够满足实际需求。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器和光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

Claims (1)

1.一种冗余机械臂运动轨迹优化的方法，其特征在于，包括：

建立空间障碍物环境模型以及机械臂模型，根据所述空间障碍物环境模型、所述机械臂模型以及预设的A*算法在障碍物环境中确定出机械臂从预设的起始点运动到预设的目标点的初始避障路径，其中，所述初始避障路径包括多个节点；

基于预设的梯度投影算法建立机械臂运动优化数学模型，以及根据预设的任务需求确定轨迹优化向量，其中，所述数学模型用于对机械臂的运动速度进行优化；

根据所述数学模型确定所述初始避障路径中任意相邻两个节点之间的机械臂关节运动规律，根据所述运动规律以及所述轨迹优化向量对所述初始避障路径进行优化得到优化后的路径，其中，通过下式建立所述数学模型：

其中，

表示机械臂关节角速度；

表示机械臂末端速度；E表示单位矩阵；J表示空间机械臂的广义雅克比矩阵；J⁺表示J的伪逆，J⁺＝J^T(JJ^T+E)^-1；(E-J⁺J)表示矩阵J的零空间；Φ表示优化向量；

所述轨迹优化向量，包括：避奇异优化向量、避障优化向量、基座保持优化向量和复位优化向量；其中，所述避奇异优化向量为：

其中，Φ_sg表示所述避奇异优化向量；k表示预设的优化系数；q₁…q_n表示具有n个自由度的机械臂各关节角度；

根据所述数学模型确定所述初始避障路径中任意相邻两个节点之间的机械臂关节运动规律，包括：

确定机械臂当前关节角度，根据所述当前关节角度确定机械臂末端经过所述起始点、所述目标点以及所述任意相邻两个 节点的样条曲线轨迹，采用带圆弧过渡的梯形规则以及所述样条曲线轨迹规划所述机械臂末端速度；

根据所述机械臂末端速度确定当前规划时长，并判断当前规划时长是否达到预设的规划总时长；

若否，则根据所述机械臂末端速度、所述当前关节角度以及所述数学模型计算当前关节角速度，并根据预设的控制周期时长以及所述当前关节角速度调整所述当前规划时长，直到达到所述规划总时长为止；

根据所述空间障碍物环境模型、所述机械臂模型以及预设的A*算法在障碍物环境中确定出机械臂从起始点运动到目标点的初始避障路径，包括：

创建OPEN表和CLOSE表，设置搜索步长和设置所述CLOSE表为空表，并初始化预设的代价估计函数，其中，所述OPEN表用于存储已生成且待访问的节点，所述CLOSE表用于存储访问过且代价值最小的节点；

将所述起始点插入到所述OPEN表中，根据初始化后的代价估计函数从所述OPEN表中选取具有最小代价值的节点为最佳节点，并将所述最佳节点放入所述CLOSE表，以及将所述最佳节点从所述OPEN表中删除，并判断所述最佳节点是否为目标点；

若不是，则根据所述搜索步长以及所述最佳节点确定下一节点，计算所述下一节点的代价值，以及根据所述下一节点的代价值更新所述CLOSE表，直到确定出所述目标点为止，根据所述更新后的CLOSE表确定从所述起始点运动到所述目标点的多个节点，根据所述起始点、所述目标点以及所述多个节点确定出所述初始避障路径；

计算所述下一节点的代价值，以及根据所述下一节点的代价值更新所述CLOSE表，包括：

判断所述下一节点是否在空间机械臂预设的自由运动空间；

若是，则计算所述起始点到所述下一节点的代价值，以及判断所述下一节点是否在所述OPEN表中；

若在，则根据所述最佳节点的代价值和所述下一节点的代价值的比较结果更新所述最佳节点；

所述避障优化向量为：

其中，Φ_qm表示所述避障优化向量；q_imax和q_imin分别表示第i个关节考虑避障后的最大角度和最小角度；

所述基座保持优化向量为：

其中，Φ_b表示所述基座保持优化向量；[J_Tw H_wq]^T表示空间机械臂与基座的耦合惯量矩阵；r_0g表示基座质心指向空间机械臂系统质心的矢量；·×表示矩阵算子，对于一个三维向量r＝[r_x r_y r_z]^T，其矩阵算子可表示为

所述复位优化向量为：

Φ_r＝k_J·Φ_J+k_a·Φ_a

Φ_a＝J_ω·ω_e

其中，Φ_r表示所述复位优化向量；k_J和k_a分别表示预设的关节复位优化系数和姿态复位优化系数；Φ_J和Φ_a分别表示关节复位优化向量和姿态复位优化向量；q_i和q_{i_int}分别表示空间机械臂当前时刻第i个关节的角度和初始角度；ω_e表示当前基座姿态与初始基座姿态偏差的角速度值；J_ω表示取空间机械臂广义雅克比矩阵J的后三行；M表示基座的质量；H_ω表示基座的惯量矩阵；

空间机械臂执行在轨操作任务过程中，在任务执行的后半程引入基座复位优化向量即可，同时考虑到优化能力和基座姿态对空间机械臂系统稳定运行影响，在任务前半程引入基座姿态保持优化向量。

Images