CN118143934A - 一种用于空间机械臂作业的改进动态基元方法 - Google Patents

Abstract

本发明涉及一种用于空间机械臂作业的改进动态基元方法，属于空间机械臂技术领域。首先针对空间环境，在圆柱坐标系下进行基元轨迹学习，并通过引入避障项来实现机械臂末端与搭载机械臂的航天器主体的避障；将圆柱坐标系轨迹计算转化到笛卡尔坐标系，并通过基于雅克比的逆运动学计算得到笛卡尔空间轨迹；针对笛卡尔空间可能存在的不平滑现象和末端误差问题，通过二次规划方法，将末端精确构型作为约束，将轨迹平滑性和与原轨迹的相似性作为指标，对轨迹进行优化。利用二次规划进行轨迹优化，求解快速，能够实现轨迹实时优化。最终克服动态基元的方法缺陷，实现其在空间机械臂作业中的应用。

Description

一种用于空间机械臂作业的改进动态基元方法

技术领域

本发明属于空间机械臂技术领域，涉及一种空间机械臂轨迹生成方法，特别是一种基于改进动态基元的模仿学习轨迹生成方法。

背景技术

空间机械臂运动规划要求轨迹具有安全性、稳定性、确定性、高精度等特点。传统基于采样和优化的运动规划方法规划结果不具有确定性，即相同条件下规划结果可能不同，难以应用于空间机械臂作业。文献“Ijspeert AJ,Nakanishi J,Hoffmann H,etal.Dynamical movement primitives:Learning attractor models for motorbehaviors.Neural Computation,2013,25(2):328-373”提出了一种基于模仿学习的动态基元(Dynamical movement primitives,DMP)方法，该方法基于二阶弹簧阻尼动力学系统加入非线性项实现轨迹学习，具有规划快速、泛化能力强等优点。然而文献所述方法规划得到的轨迹存在末端误差，轨迹精度无法达到空间机械臂作业要求，且泛化轨迹映射到机械臂关节空间可能存在不平滑现象，无法直接应用在空间任务中。

发明内容

本发明所要解决的技术问题是：

为了实现空间机械臂的快速轨迹规划，考虑机械臂作业轨迹的安全性、稳定性和高精度要求，结合空间机械臂作业环境特点，本发明提供一种用于空间机械臂作业的改进动态基元方法。

为了解决上述技术问题，本发明采用的技术方案为：

一种用于空间机械臂作业的改进动态基元方法，其特征在于，包括：

在圆柱坐标系下对动态基元方法公式进行改进，在ρ坐标方向引入避障项来实现机械臂末端与搭载机械臂的航天器主体的避障；

将基于改进动态基元方法获得的圆柱坐标系轨迹转化到笛卡尔坐标系，基于雅克比的逆运动学计算得到笛卡尔空间轨迹；

对笛卡尔空间轨迹进行二次规划：将末端精确位姿对应的构型作为末端约束，以相似度指标、平滑性指标作为优化指标对笛卡尔空间轨迹进行优化。

本发明进一步的技术方案：所述改进后的动态基元方法公式为：

其中，y和z分别表示位置量和引入的系统速度相关量，g表示目标位置，τ是时间常数，α_z和β_z是正常数且β_z＝α_z/4，f为非线性强迫项，为避障项，ρ表示到航天器中轴距离，k为常值系数，相变量s使得避障项能够随着运动过程衰减，从而保证系统的目标收敛特性。

本发明进一步的技术方案：所述将基于改进动态基元方法获得的圆柱坐标系轨迹转化到笛卡尔坐标系，基于雅克比的逆运动学计算得到笛卡尔空间轨迹，具体为：

在机器人领域有雅克比矩阵相关定义，其具有如下性质：

其中，x_e表示末端位姿，表示末端速度和角速度，θ表示机械臂关节构型，/>表示机械臂关节角速度，J为雅克比矩阵；

假定机械臂当前位姿为x₀，当前构型为θ₀，下一步目标位姿为x₁，每一步长的运动为小量，即Δx＝x₁-x₀是一个小量；则

Δθ＝pinv(J)Δx

利用上式，通过多次迭代求出到达目标位姿时机器人的关节角度变化量Δθ，进而得到对应目标位姿x₁的目标构型：

θ₁＝θ₀+Δθ

获得机械臂关节空间轨迹Q＝[θ₀,θ₁,...,θ_n]，n为该轨迹对应的步长数。

本发明进一步的技术方案：将优化轨迹与参考轨迹的差值平方作为相似度指标，形式如下：

其中，x＝[θ₁₁,θ₂₁,...,θ₇₁,...,θ_1n,θ_2n,...,θ_7n]^T为优化变量，为原关节空间参考轨迹记，A₁是7n×7n的单位矩阵，线性项的系数是常数向量h＝-2x^r，c是仅与参考轨迹x^r相关的常数项。

本发明进一步的技术方案：所述平滑性指标为：

其中，0_6×1表示6行1列的0元素组成的向量。

本发明进一步的技术方案：所述末端约束为：

其中，θ_init和θ_end分别为初末构型，0_{(n-14)×(n-14)}表示(n-14)行(n-14)列的0元素组成的矩阵、0_(n-14)×1表示(n-14)行1列的0元素组成的向量。

一种计算机系统，其特征在于包括：一个或多个处理器，计算机可读存储介质，用于存储一个或多个程序，其中，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现上述的方法。

一种计算机可读存储介质，其特征在于存储有计算机可执行指令，所述指令在被执行时用于实现上述的方法。

本发明的有益效果在于：

本发明提供的一种用于空间机械臂作业的改进动态基元方法，通过在动态基元方法中添加避障项，实现机械臂末端与航天器主体的避障，通过二次规划，克服了当前动态基元方法末端误差缺陷。

给定示教轨迹，所提方法可以快速学习，并泛化应用于相似场景中。利用本方法可以方便实现机械臂末端与航天器主体的避障，同时本方法消除了关节空间轨迹的不平滑与泛化轨迹的末端误差。

附图说明

附图仅用于示出具体实施例的目的，而并不认为是对本发明的限制，在整个附图中，相同的参考符号表示相同的部件。

图1是本发明方法的总体计算流程图。

图2是笛卡尔坐标系与圆柱坐标系转化关系图。

图3是本发明方法中的避障项作用效果图。

图4是本发明方法中利用二次规划进行轨迹优化前后关节空间轨迹对比图：(a)优化前的关节空间轨迹；(b)优化后的关节空间轨迹。

图5是本发明方法对动态基元的末端误差消除效果图：(a)优化前末端位置误差；(b)优化后末端位置误差；(c)优化前末端姿态误差；(d)优化后末端姿态误差。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

本发明提供了一种用于空间机械臂作业的改进动态基元方法，首先针对空间环境，在圆柱坐标系下进行基元轨迹学习，并通过引入避障项来实现机械臂末端与搭载机械臂的航天器主体的避障；将圆柱坐标系轨迹计算转化到笛卡尔坐标系，并通过基于雅克比的逆运动学计算得到笛卡尔空间轨迹；针对笛卡尔空间可能存在的不平滑现象和末端误差问题，通过二次规划方法，将末端精确构型作为约束，将轨迹平滑性和与原轨迹的相似性作为指标，对轨迹进行优化。利用二次规划进行轨迹优化，求解快速，能够实现轨迹实时优化。最终克服动态基元的方法缺陷，实现其在空间机械臂作业中的应用。

如图1所示，包括以下步骤：

步骤一、动态基元方法公式改进

传统位置级DMP(Dynamic Movement Primitives)方法基于如下公式：

其中，y和z分别表示位置量和引入的系统速度相关量，g表示目标位置。τ是时间常数，α_z和β_z是正常数且β_z＝α_z/4，式中的f项称为非线性强迫项，通过设计f项，达到对系统演化过程进行调制的目的。由于f项的干扰作用，传统DMP方法无法严格收敛到目标状态。f项如下：

其中，s为相位变量，满足ψ_i(·)表示第i个基函数，ω_i表示第i个基函数的权重。

其中，且存在关系/>四元数的共轭定义为：

基于四元数的姿态DMP公式如下：

式中的常数参数与位置DMP方法相同，式中的η是一个标量部分为0的四元数，即它是与目标角速度相关的量。g_o为四元数形式表示的目标姿态，q表示当前姿态。f_o(x)具有如下形式：

式中给定示教轨迹，同样通过LWR计算式中权重系数向量，实现姿态轨迹学习。

空间场景中，通过航天器搭载机械臂执行空间任务。如图2所示，可在航天器主轴建立圆柱坐标系。相比笛卡尔坐标系，圆柱系坐标参数(x,ρ,φ)具有更加直观的物理意义。x表示轴向位置坐标，ρ表示到航天器中轴距离，φ表示与给定参考轴的夹角。在圆柱系下进行机械臂末端轨迹学习，能够获得更加自然的泛化效果。圆柱系与笛卡尔系具有简洁的转化关系，给定笛卡尔空间坐标(x,y,z)，其在圆柱系下对应的坐标(x,ρ,φ)为：

同理给定圆柱系下的位置(x,ρ,φ)，可以计算笛卡尔系下的位置：

其中，ρ表示机械臂末端到航天器中轴距离，在ρ坐标方向的原DMP公式中加入避障项F，可实现机械臂末端对航天器主体的避碰功能。参考自然界万有引力形式，避障项F项设计如下：

其中，k为常值系数，相变量s使得避障项能够随着运动过程衰减，从而保证系统的目标收敛特性。在避障项的作用下，机械臂末端表现出远离航天器主体的趋势，从而保证机械臂末端与航天器主体无碰。改进后的ρ坐标方向DMP公式如下：

其余坐标方向及姿态的DMP公式不做改变。

步骤二、获取基于雅克比迭代的关节空间轨迹

基于改进DMP方法在获得圆柱系下的轨迹后，即可转化得到其笛卡尔空间轨迹。在机器人领域有雅克比矩阵相关定义，其具有如下性质：

其中，x_e表示末端位姿，表示末端速度和角速度。θ表示机械臂关节构型，/>表示机械臂关节角速度。通过雅克比矩阵J，笛卡尔空间和关节空间建立起联系。

为满足复杂空间任务要求，空间机械臂通常为7自由度冗余机械臂。假定机械臂当前位姿为x₀，当前构型为θ₀，下一步目标位姿为x₁，每一步长的运动为小量，即Δx＝x₁-x₀是一个小量。则

Δθ＝pinv(J)Δx

利用上式，通过多次迭代求出到达目标位姿时机器人的关节角度变化量Δθ，进而得到对应目标位姿x₁的目标构型：

θ₁＝θ₀+Δθ

按照上述过程可以得到机械臂关节空间轨迹Q，其可以表示为一组关节构型的集合，即Q＝[θ₀,θ₁,...,θ_n]，n为该轨迹对应的步长数，θ_i＝[θ_i1,θ_i2,...,θ_i7]。

笛卡尔空间平滑的轨迹，其相应的关节空间轨迹不一定平滑。

步骤三、关节空间轨迹二次规划

针对轨迹末端误差问题和关节空间轨迹不一定平滑问题，利用二次规划方法对规划结果进行优化。优化变量x＝[θ₁₁,θ₂₁,...,θ₇₁,...,θ_1n,θ_2n,...,θ_7n]^T，原关节空间参考轨迹记为优化指标包括与原轨迹的相似性，以及轨迹平滑性。同时将末端精确位姿对应的构型作为末端约束，以实现末端误差的消除。

以优化轨迹与参考轨迹的差值平方作为相似度指标，形式如下：

整理为二次型形式如下：

其中，A₁是7n×7n的单位矩阵，线性项的系数是常数向量h＝-2x^r，c是仅与参考轨迹x^r相关的常数项，不影响性能指标的求解，因此在优化问题中可以舍去。这一指标保证优化结果与参考轨迹的相似程度，使得优化后的轨迹保留原轨迹特征。

轨迹的平滑性实质上取决于轨迹运动速度的稳定性，若轨迹速度连续变化且单位时间其变化量小，则其相应位置轨迹自然平滑。机械臂某一关节相邻三个轨迹点的值分别记为y_i、y_i+1和y_i+2，以单位步长的位置差值来代表速度，相邻两步长速度差可以记为y_i+2-2y_i+1+y_i。通过优化整条轨迹任意三个相邻轨迹点之间的速度差值，可以得到平滑稳定的轨迹。对于7自由度的空间机械臂来说，利用上述原理对7个关节轨迹的速度差进行优化，可以获得平滑的关节空间轨迹。因此设计平滑性指标为：

式中矩阵A₂为如下形式：

为保证优化结果初末位姿的准确性，将初末构型的精确解作为优化问题中的约束。记初末构型分别为θ_init和θ_end，设计如下等式约束：

上式左侧系数矩阵记为A₃，右侧记为b。等式约束简写为A₃x＝b。

由上述分析获得如下优化问题：

subject to A₃x＝b

其中，α₁和α₂为两个正常数，表示相似性和平滑性性能指标的权重系数。

采用如图2所示的空间站场景作为仿真场景，利用本发明方法进行运动规划。机械臂固连在航天器上，基座位姿为机械臂从初始构型θ₀＝[1.5708,0.7854,-2.3562,0.5236,1.5708,-0.7854,-1.5708]出发，移动到目标位姿算法涉及的公式中相关参数设置如下：τ＝7.5，α_z＝25，β_z＝6.25，α_s＝2，N＝15，k＝1000，α₁＝1，α₂＝1000。

如图3所示，加入斥力项之后的轨迹能够有效远离原点，在空间场景中表现为机械臂末端在运动过程中呈现出远离舱段轴心的趋势。如图4所示，通过二次规划方法优化后的轨迹与原轨迹形状高度相似，保留了DMP方法模仿得到的示教轨迹特征，同时原轨迹中不平滑位置得以消除，且在其附近过渡自然。如图5所示，原始DMP方法得到的轨迹存在明显的末端误差，在本例中末端位置误差在3cm左右。优化后的轨迹末端误差在10^-8m量级，为机器人运动学的计算误差，可见本方法消除了DMP方法的末端误差。本例中原轨迹末端姿态误差较小，但可以看出优化后的姿态变化过程更加平滑自然。同时本发明方法基于改进DMP公式和二次规划方法，给定场景能够得到唯一确定的轨迹，能够满足空间机械臂运动的确定性要求。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明公开的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。

Claims (8)

1.一种用于空间机械臂作业的改进动态基元方法，其特征在于，包括：

在圆柱坐标系下对动态基元方法公式进行改进，在ρ坐标方向引入避障项来实现机械臂末端与搭载机械臂的航天器主体的避障；

将基于改进动态基元方法获得的圆柱坐标系轨迹转化到笛卡尔坐标系，基于雅克比的逆运动学计算得到笛卡尔空间轨迹；

对笛卡尔空间轨迹进行二次规划：将末端精确位姿对应的构型作为末端约束，以相似度指标、平滑性指标作为优化指标对笛卡尔空间轨迹进行优化。

2.根据权利要求1所述一种用于空间机械臂作业的改进动态基元方法，其特征在于，所述改进后的动态基元方法公式为：

其中，y和z分别表示位置量和引入的系统速度相关量，g表示目标位置，τ是时间常数，α_z和β_z是正常数且β_z＝α_z/4，f为非线性强迫项，为避障项，ρ表示到航天器中轴距离，k为常值系数，相变量s使得避障项能够随着运动过程衰减，从而保证系统的目标收敛特性。

3.根据权利要求1所述一种用于空间机械臂作业的改进动态基元方法，其特征在于，所述将基于改进动态基元方法获得的圆柱坐标系轨迹转化到笛卡尔坐标系，基于雅克比的逆运动学计算得到笛卡尔空间轨迹，具体为：

在机器人领域有雅克比矩阵相关定义，其具有如下性质：

其中，x_e表示末端位姿，表示末端速度和角速度，θ表示机械臂关节构型，/>表示机械臂关节角速度，J为雅克比矩阵；

假定机械臂当前位姿为x₀，当前构型为θ₀，下一步目标位姿为x₁，每一步长的运动为小量，即Δx＝x₁-x₀是一个小量；则

Δθ＝pinv(J)Δx

利用上式，通过多次迭代求出到达目标位姿时机器人的关节角度变化量Δθ，进而得到对应目标位姿x₁的目标构型：

θ₁＝θ₀+Δθ

获得机械臂关节空间轨迹Q＝[θ₀,θ₁,...,θ_n]，n为该轨迹对应的步长数。

4.根据权利要求1所述一种用于空间机械臂作业的改进动态基元方法，其特征在于，将优化轨迹与参考轨迹的差值平方作为相似度指标，形式如下：

其中，x＝[θ₁₁,θ₂₁,...,θ₇₁,...,θ_1n,θ_2n,...,θ_7n]^T为优化变量，为原关节空间参考轨迹记，A₁是7n×7n的单位矩阵，线性项的系数是常数向量h＝-2x^r，c是仅与参考轨迹x^r相关的常数项。

5.根据权利要求1所述一种用于空间机械臂作业的改进动态基元方法，其特征在于，所述平滑性指标为：

其中，0_6×1表示6行1列的0元素组成的向量。

6.根据权利要求1所述一种用于空间机械臂作业的改进动态基元方法，其特征在于，所述末端约束为：

其中，θ_init和θ_end分别为初末构型，0_{(n-14)×(n-14)}表示(n-14)行(n-14)列的0元素组成的矩阵、0_(n-14)×1表示(n-14)行1列的0元素组成的向量。

7.一种计算机系统，其特征在于包括：一个或多个处理器，计算机可读存储介质，用于存储一个或多个程序，其中，当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现权利要求1-6任一项所述的方法。

8.一种计算机可读存储介质，其特征在于存储有计算机可执行指令，所述指令在被执行时用于实现权利要求1-6任一项所述的方法。