CN113341731A - 一种基于序列凸优化的空间机器人轨迹规划方法 - Google Patents

Abstract

本发明公开了一种基于序列凸优化的空间机器人轨迹规划方法。首先建立了空间机器人的运动学模型，并考虑性能指标和约束条件，建立了空间机器人最优轨迹规划模型，接着提出了一种基于序列凸优化的最优轨迹快速求解方法。先通过离散化及对非凸约束的线性化处理，将空间机器人的非凸轨迹优化问题转换为近似凸优化问题，再利用该序列凸优化方法对该近似凸优化问题进行迭代求解，得到满足约束条件的最优轨迹。为提升序列凸优化方法求解时的收敛速度，本发明序列凸优化方法相较于伪谱法求解实时性更好，收敛性也有较大的提升，验证了本发明所提方法的有效性。

Description

一种基于序列凸优化的空间机器人轨迹规划方法

技术领域

本发明属于空间机器人系统技术领域，涉及一种基于序列凸优化的空间机器人轨迹规划方法。

背景技术

随着人类对于空间探索的深入，空间机器人执行的任务也变得复杂多样，例如空间碎片清除、卫星在轨服务、大型空间结构在轨装配和空间科学实验等等。自由漂浮空间机器人作为空间机器人的典型应用，由于基座和机械臂存在严重的动力学耦合，大大增加了轨迹规划的复杂性(注：下文中的空间机器人特指自由漂浮空间机器人)。而且随着空间任务复杂性的提高，对空间机器人进行轨迹规划的最优性和实时性要求也更加严苛，这些约束基本都是存在着强非线性的非凸约束，给空间机器人轨迹规划问题的求解增加了极大的计算量，严重制约了求解的实时性和快速性，因此需要寻求更加高效的最优求解方法。

传统的空间机器人最优轨迹规划问题的求解方法分为两类:间接法和直接法，但两种方法由于计算量或收敛性的原因无法满足实时性和快速性的要求。几十年来，由于凸优化方法以及伪谱离散方法在轨迹规划问题上求解速度的显著优势，其在航天器领域应用越来越多。无论是在轨道交会还是行星着陆，凸优化方法都能快速有效地规划轨迹。

在求解火星着陆器下降轨迹的问题中，Acikmese通过推力无损凸化准确完成了燃料消耗最少的着陆轨迹优化问题，而后总结了过去的凸化方法，并提出了燃料最优轨迹优化的二阶锥规划问题，并进一步明确了方法的应用场景。除此之外，BLACKMOR证明了最小着陆轨迹生成问题可以作为凸优化问题提出，并在已知收敛时间的条件下求解到全局最优，UTKU基于动力下降着陆制导问题的无损凸化，以及凸优化和计算几何方法，对给定飞行器的规定着陆精度的可行性和行星着陆任务下降阶段的预期散布进行详细分析。

在高超声速飞行器再入轨道的问题中，Wang在以往再入轨迹优化序列凸优化方法的基础上，开发了一种不同的连续算法，用于在线生成和跟踪从当前位置到期望目标的参考轨迹，将与高超音速轨迹优化相关的高度非线性最优控制问题公式化为一系列有限维凸优化问题，提出了一种近似最优的参考跟踪再入制导算法首先，并证明了在设计的跟踪制导律的控制下，轨迹能够满足路径约束，引入模型预测控制方法并采用改进的逐次凸化算法优化运载火箭着陆轨迹。

在航天器轨道交会和转移的问题中，Yang利用数学假设对航天器导航算法的扩展卡尔曼滤波器(EKF)的协方差矩阵元素进行线性化，并通过逐次线性化和凸优化给出了导航最优轨迹的快速生成算法，从而规划小天体着陆的时间最优轨迹。Zhou利用线性化的相对平移动力学和基于修正罗德里格参数的离散旋转方程，将原最优制导和控制问题的非线性系统动力学转化为凸系统动力学，接着将姿态和视场要求的非凸约束近似放松为凸标准二阶锥，进而求解航天器六自由度接近问题。Foust在轨装配轨迹规划问题中改变了对接和避碰准则，并对约束进行凸化及求解。Liu利用无损凸化方法将原凸问题凸化为二阶锥规划问题，再利用迭代求解凸问题，从而解决轨道转移中的非凸规划问题，并将交会对接和轨道转移问题设为应用场景。Bruijn研究了轨道卫星编队构型维持问题。Wang将自由末段小推力弹道优化问题转化为一系列凸优化问题，求解了小推力航天器平面内轨道转移问题。以上求解都采用了凸优化方法，都能求解设定任务背景下的最优轨迹规划问题。

另外，凸优化也被用于求解其他类型的优化问题。例如，Wang将凸优化方法扩展到求解具有非线性动力学的无人机最小时间协同轨迹规划。Daniel将序列凸优化方法应用于航天器编队集群制导，约束包含从一个不变轨道转移到另一个轨道，同时确保避障和燃料最优。Liu研究了以攻角和倾斜角作为控制输入的气动控制导弹在末段的最优飞行轨迹：考虑了攻角和动压的实际约束，并对高超飞行器的最优飞行模型设计了精确凸松弛化方法。

在以上目前已知的研究中，凸优化方法都能够很好地将问题高效优化求解，其主要研究都在问题的凸化上，即如何将原非凸轨迹规划问题转化为等价或近似的凸优化问题。然而，由于空间机器人自身的非完整性和复杂的动力学约束，导致不仅模型为非凸，而且转化为凸问题比较困难。在航天航空工程问题的非凸优化方法中，如何针对空间机器人规划问题应用凸化方法的研究也较少，相关的后续分析和证明也十分欠缺。Misra采用序列凸优化方法求解了自由飞行空间机器人的轨迹规划问题，但对于应用更加广泛的自由漂浮空间机器人并未深入研究。因此寻求一种收敛性和实时性均较好的自由漂浮空间机器人最优轨迹求解方法显得尤为必要。

发明内容

本发明的目的在于解决现有技术中的问题，提供一种基于序列凸优化的空间机器人轨迹规划方法。

为达到上述目的，本发明采用以下技术方案予以实现：

一种基于改进序列凸优化方法的空间机器人轨迹规划方法，包括以下步骤：

步骤1，建立自由漂浮空间机器人最优轨迹规划模型并将模型凸化；

首先将空间机器人系统假设为基座与机械臂组成的无根多刚体系统，推导了空间机器人运动学方程，并针对空间机器人点对点轨迹规划建立模型；接着给出了一般最优轨迹规划问题描述形式，引入优化指标和避障约束，建立了末端时刻和捕获位置均固定的最优轨迹规划问题模型；最后对建立的非凸轨迹优化模型进行进行凸化处理；

步骤2，设计基于信赖域更新策略的序列凸优化方法，并对自由漂浮空间机器人轨迹规划问题进行求解验证；

首先设计了针对空间机器人轨迹规划问题的序列凸优化求解算法，并基于信赖域更新策略对序列凸优化方法进行改进，最后利用改进的序列凸优化方法与传统Guass伪谱法进行求解仿真和对比分析。

所述步骤1建立自由漂浮空间机器人最优轨迹规划模型并将模型凸化的具体方法如下：

自由漂浮空间机器人的运动学方程：

其中，定义J_g为广义雅克比矩阵，D_g为广义动量。

对于本发明研究的问题，可以建模为一个最优控制问题，其具有如下的一般性形式(P0)：

其中t∈[t₀,t_f]，t₀表示任务初始时刻，t_f表示任务末时刻；x为空间机器人轨迹规划过程中的状态变量，u为过程中的控制变量；式s₁(x(t),u(t),t)≤0和s₂(x(t),u(t),t)＝0为空间机器人路径约束；ψ₁(x(t_f),t_f)≥0，ψ₂(x(t_f),t_f)＝0表示末时刻的等式约束和不等式约束。

以能量为优化指标选取目标函数:

综上所述，最优轨迹规划问题表述为P1：

q_i(0)＝q_i0,

q_{i_min}≤q_i(t)≤q_{i_max}

选用连续线性化方式：连续线性化是指在前次迭代获得的已知解处重复性地线性化选定的非线性项的过程。这是一种有效转化非线性项的凸化方法。具体来说，非线性项f(x)能够被近似为：

那么运动学方程中

相可以线性化为：

接着将连续系统离散化处理，对问题P1进行离散，时间步长为Δt，步数为N，则t_f＝NΔt。另外，将关节角度q和角速度

作为状态量，关节角加速度

作为控制量u，即

则u_t＝u_k,

那么离散状态方程为：

因此

x_k+1＝A_Δtx_k+B_Δtu_k (18)

其中

进一步，上述问题表述为离散后的最优控制问题P2：

subject to x_k+1＝A_Δtx_k+B_Δtu_k

x^min≤x_k≤x^max

u^min≤u_k≤u^max

P_e(k＝N)＝P_e(t_f)

x(k＝0)＝[0 0]^T

χ(x_k)≤0

(19)

其中，χ(x_k)≤0为避障约束。

将式(5)带入避障约束和末端位置约束中，可获得避障约束和末端位置约束的线性形式。由于序列凸优化框架下的凸近似仅在信赖域上有效，因此这是一个在信任域上具有线性等式的凸二次形式。该形式符合凸优化方法求解的问题形式，可以直接带入序列凸优化框架中进行求解，但需要在求解时需要设立合适的信赖域。

因此问题可进一步表示为P3:

其中，|x-x^k|≤ρ表示信赖域约束，ρ表示信赖域半径。

步骤2建立自由漂浮空间机器人轨迹规划模型并将模型凸化的具体方法如下：

传统上序列凸优化算法的具体流程如表1所示。利用序列凸优化方法，可以获得序列解x₁,x₂,...,x_k,...，直至达到停止条件，获得最优解。

表1序列凸优化方法

信赖更新策略具体如下：在序列凸优化迭代过程中，计算每次迭代实际性能指标函数ψ′与预测性能指标函数ψ的下降量分别为Δψ＝|ψ(x'^k+1,u'^k+1)-ψ(x'^k,u'^k)|和Δψ′＝|ψ′(x'^k+1,u'^k+1)-ψ′(x'^k,u'^k)|。

定义Δψ′与Δψ的比值

一般来说，预测性能指标函数ψ的下降量通常为正值，即Δψ＞0。因此，如果r_k＜0，则说明Δψ＜0，则△f：<0，原信赖域步长不能作为下一个迭代点，需要缩小信赖域半径重新求解子问题.若r_k比较接近1，说明二次模型与目标函数在信赖域范围内有很好的近似，此时原信赖域步长可以作为新的迭代点，同时下一次迭代时可以增大信赖域半径。对于其他情况，信赖域半径可以保持不变。具体信赖域更新策略见表2。

表2信赖域更新策略

基于传统序列凸优化方法和信赖域更新策略，设计的改进序列凸优化方法的伪代码如表3所示。

表3改进序列凸优化方法伪代码

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

本发明提出了一种基于序列凸优化的空间机器人轨迹规划方法。首先建立了空间机器人的运动学模型，并考虑性能指标和约束条件，建立了空间机器人最优轨迹规划模型，接着提出了一种基于序列凸优化的最优轨迹快速求解方法。具体地，先通过离散化及对非凸约束的线性化处理，将空间机器人的非凸轨迹优化问题转换为近似凸优化问题，再利用该序列凸优化方法对该近似凸优化问题进行迭代求解，得到满足约束条件的最优轨迹。为提升序列凸优化方法求解时的收敛速度，本发明基于信赖域更新策略对序列凸优化方法进行改进并将此方法与Guass伪谱法进行仿真和对比分析。结果表明：序列凸优化方法相较于伪谱法求解实时性更好，收敛性也有较大的提升，验证了本发明所提方法的有效性。

附图说明

图1七关节自由漂浮空间机器人示意图

图2机械臂与障碍物距离关系示意图

图3Gauss伪谱法与序列凸优化方法优化轨迹对比(红色轨迹为Gauss伪谱法优化结果，黑色轨迹为序列凸优化结果。下同)

图4Gauss伪谱法优化与序列凸优化下关节角变化情况

图5Gauss伪谱法优化与序列凸优化下关节角速度变化情况对比

图6Gauss伪谱法优化与序列凸优化下关节角加速度变化情况对比

具体实施方式

下面结合附图对本发明做进一步详细描述：

本发明建立自由漂浮空间机器人最优轨迹规划模型并将模型凸化。首先将空间机器人系统假设为基座与机械臂组成的无根多刚体系统，推导了空间机器人运动学方程，并针对空间机器人点对点轨迹规划建立模型；接着给出了一般最优轨迹规划问题描述形式，引入优化指标和避障约束，建立了末端时刻和捕获位置均固定的最优轨迹规划问题模型；最后对建立的非凸轨迹优化模型进行进行凸化处理；

设计基于信赖域更新策略的序列凸优化方法，并对自由漂浮空间机器人轨迹规划问题进行求解验证。首先设计了针对空间机器人轨迹规划问题的序列凸优化求解算法，并基于信赖域更新策略对序列凸优化方法进行改进，最后利用改进的序列凸优化方法与传统Guass伪谱法进行求解仿真和对比分析。

本发明基于改进序列凸优化方法的空间机器人轨迹规划方法，包括以下步骤：

步骤1，建立自由漂浮空间机器人最优轨迹规划模型并将模型凸化；

首先将空间机器人系统假设为基座与机械臂组成的无根多刚体系统，推导了空间机器人运动学方程，并针对空间机器人点对点轨迹规划建立模型；接着给出了一般最优轨迹规划问题描述形式，引入优化指标和避障约束，建立了末端时刻和捕获位置均固定的最优轨迹规划问题模型；最后对建立的非凸轨迹优化模型进行进行凸化处理；

1.1运动学建模：

1.1.1基本假设

为简化模型，将空间机器人系统中的所有柔性部件简化为刚体部件，因此，可以把空间机器人系统当做是由基座与机械臂组成的无根多刚体系统。当机械臂末端带有负载时，把负载和其接触的连杆当作同一个连杆。在建立空间机器人系统的数学模型时，作以下假设：

(1)空间机器人的机械臂各关节均仅能绕一轴转动，一个关节只具有一个自由度；

(2)忽略空间微重力，忽略空间各种阻力及摩擦力；

(3)忽略燃料消耗等质量变化，假设其质量不变。

1.1.2运动学方程推导

机械臂各连杆及端执行器的位置矢量表达式为：

对式(1)和式(2)求导，并利用泊松公式

展开，得到机械臂连杆和末端执行器的线速度表达式：

此外，机械臂各连杆质心及末端执行器的角速度表达式为：

将式(3)和式(5)写成矩阵形式，可得机械臂末端线速度和角度度表达式为：

其中，J_b和J_m分别为与基座和机械臂运动有关的雅可比矩阵：

式(7)中，J_bv,J_bω是J_b的分块矩阵，分别对应于基座线速度和角速度。同理，式(8)中，J_mν,J_mω为J_m的分块矩阵。

对于自由漂浮空间机器人，引入动量守恒定理，其中线动量表达式为：

角动量表达式为：

把式(9)和式(10)一起联立求解：

其中，H_υ,H_υω,H_ωυ,H_ω为H_b的分块矩阵，表达式分别为：

由式(11)可得：

令

则

其中

将式(13)代入(6)，可得到形式上类似于雅克比矩阵的自由漂浮空间机器人的运动学方程：

其中，定义J_g为广义雅克比矩阵，D_g为广义动量。

1.1.3空间机器人点到点轨迹规划

空间机器人末端执行器位姿是与基座姿态，机械臂关节角有关，这里将其表示为X_e，那么

若将空间机器人末端执行器的初始位姿、末时位姿以及期望状态分别表示为X_e0、X_ef和X_ed，则点到点路径规划可表示为

q_i(t)＝f_i(t),i＝1,2,…,n (16)

X_e(t₀)＝X_e0,X_e(t_f)＝X_ed, (17)

满足如下的等式约束

q_m(t₀)＝[q₁(t₀) q₂(t₀) q₃(t₀),...,q_n(t₀)]^T

q_m(t_f)＝[q₁(t_f) q₂(t_f) q₃(t_f),...,q_n(t_f)]^T

以及不等式约束

q_{i_min}≤q_i(t)≤q_{i_max},

其中，q_{i_min}、q_{i_max}，

分别为机械臂关节角度、关节角速度和关节角加速度的范围。

由运动学公式可知，

因而

因此，通过数值积分，基座姿态和末端执行器姿态在t时刻为

t时刻末端执行器的位置

由式(19)和式(20)可知，t_f时刻末端执行器的末端位姿与期望位姿之差

其中{η_ef,q_ef}和{η_ed,q_ed}分别为t_f时刻的末端姿态与期望姿态的四元数表示。

1.2空间机器人最优轨迹规划模型建立

1.2.1一般最优轨迹规划问题描述

对于本发明研究的问题，可以建模为一个最优控制问题，其具有如下的一般性形式(P0)：

其中t∈[t₀，t_f]，t₀表示任务初始时刻，t_f表示任务末时刻；x为空间机器人轨迹规划过程中的状态变量，u为过程中的控制变量；式s₁(x(t),u(t),t)≤0和s₂(x(t),u(t),t)＝0为空间机器人路径约束；ψ₁(x(t_f),t_f)≥0，ψ₂(x(t_f),t_f)＝0表示末时刻的等式约束和不等式约束。

1.2.2优化指标

考虑约束情况下对空间机器人轨迹规划问题进行求解的时候，也会设置一些优化指标，常见的有时间最优、基座无扰、能量最优等等，这类问题可以轨迹归结为空间机器人最优轨迹规划问题。本发明以能量为优化指标选取目标函数:

1.2.3空间机器人最优轨迹规划模型建立

空间机器人抓捕任务可以转化为含有固定末端时刻和捕获点位置约束的轨迹规划问题。此外，还需满足关节角、角速度、角加速度限制的约束。同时，将能量最优作为性能指标。

此外，考虑避障约束，本发明选择采用速度阻尼法来建立避障不等式约束。该方法的基本思想时：在轨迹规划过程中，当机械臂与障碍物的距离小于设置的安全距离时，规划算法会给关节一个方向相反的速度，驱动机械臂远离障碍物。

如图2所示为机械臂与障碍物之间的相对距离关系示意图，P₁与P₁'为两者表面距离最近的两个点，d表示间距，n₁是连接两点的单位方向向量，d_if、d_sr和d_uf分别为影响距离、临界距离与危险距离。

避障约束不等式表示为：

机械臂表面最近点P₁速度可表示为

这里本发明假设障碍物为球形，则障碍物表面最近点P₁'的速度为

在点P₁'处的切向速度，则有

式中φ为向量

与向量n₁之间的夹角。

进一步：

综上所述，最优轨迹规划问题表述为P1：

1.3模型凸化

采用凸优化方法求解轨迹规划的的重点是考虑如何将原非凸轨迹规划问题转化为满足凸优化形式的近似或等价问题。本发明具体的凸化过程是选用连续线性化方式：连续线性化是指在前次迭代获得的已知解处重复性地线性化选定的非线性项的过程。这是一种有效转化非线性项的凸化方法。具体来说，非线性项f(x)能够被近似为：

那么运动学方程中

相可以线性化为：

接着将连续系统离散化处理，对问题P1进行离散，时间步长为Δt，步数为N，则t_f＝NΔt。另外，将关节角度q和角速度

作为状态量，关节角加速度

作为控制量u，即

则u_t＝u_k,

那么离散状态方程为：

因此

x_k+1＝A_Δtx_k+B_Δtu_k (36)

其中

进一步，上述问题表述为离散后的最优控制问题P2：

其中，χ(x_k)≤0为避障约束。

将式(5)带入避障约束和末端位置约束中，可获得避障约束和末端位置约束的线性形式。由于序列凸优化框架下的凸近似仅在信赖域上有效，因此这是一个在信任域上具有线性等式的凸二次形式。该形式符合凸优化方法求解的问题形式，可以直接带入序列凸优化框架中进行求解，但需要在求解时需要设立合适的信赖域。

因此问题可进一步表示为P3:

其中，|x-x^k|≤ρ表示信赖域约束，ρ表示信赖域半径。

步骤2，设计基于信赖域更新策略的序列凸优化方法，并对自由漂浮空间机器人轨迹规划问题进行求解验证；

首先设计了针对空间机器人轨迹规划问题的序列凸优化求解算法，并基于信赖域更新策略对序列凸优化方法进行改进，最后利用改进的序列凸优化方法与传统Guass伪谱法进行求解仿真和对比分析。

2.1序列凸优化方法

序列凸优化方法采用轨迹线性化、离散化处理将空间机器人轨迹规划问题中的非线性动态约束转化为凸约束，同时为保证线性化系统对原非线性系统的有效近似，增加了信赖域域约束。一般来说，在求解空间机器人轨迹规划问题时，相对经典的基于非线性规划的直接法，凸优化方法计算效率能提升一个数量级以上。

传统上序列凸优化算法的具体流程如表1所示。利用序列凸优化方法，可以获得序列解x₁,x₂,...,x_k,...，直至达到停止条件，获得最优解。

表1序列凸优化方法

2.2信赖域更新策略

一般来说,在序列凸优化方法求解中，信赖域半径大小与计算效率密切相关。如果信赖域过大，则问题可能会偏离原问题而难以收敛；如果信赖域过小，则会因为迭代步长受限而导致收敛速度比较慢。因此如果在迭代过程中采用定值的信赖域，即便收敛性有保证，但可能会失去提升收敛速度的机会。因此在序列凸优化迭代求解过程中，考虑到信赖域大小对于收敛性和精度的影响，本发明根据每次迭代性能指标下降情况，设计了信赖更新策略。具体如下：

在序列凸优化迭代过程中，计算每次迭代实际性能指标函数ψ′与预测性能指标函数ψ的下降量分别为Δψ＝|ψ(x'^k+1,u'^k+1)-ψ(x'^k,u'^k)|和Δψ′＝|ψ′(x'^k+1,u'^k+1)-ψ′(x'^k,u'^k)|。

定义Δψ′与Δψ的比值

一般来说，预测性能指标函数ψ的下降量通常为正值，即Δψ＞0。因此，如果r_k＜0，则说明Δψ＜0，则△f：<0，原信赖域步长不能作为下一个迭代点，需要缩小信赖域半径重新求解子问题.若r_k比较接近1，说明二次模型与目标函数在信赖域范围内有很好的近似，此时原信赖域步长可以作为新的迭代点，同时下一次迭代时可以增大信赖域半径。对于其他情况，信赖域半径可以保持不变。具体信赖域更新策略见表2。

表2信赖域更新策略

2.3算法流程

基于传统序列凸优化方法和信赖域更新策略，设计的改进序列凸优化方法的伪代码如表3所示。

表3改进序列凸优化方法伪代码

实施例：

本发明的仿真对象为一个具有7关节的空间机器人，系统机械臂构型参考加拿大二号机械臂，如图1所示，系统的物理参数如表4所示。不失一般性，假设系统的初始线动量和角动量为零，选取基座质心初始时刻的位置为惯性参考系原点。基座的连体坐标系的原点O₀取在基座的质心处，坐标轴与其惯量主轴方向一致。假定初始状态下基座的连体坐标系O₀-x₀y₀z₀和惯性坐标系O_I-x_Iy_Iz_I是重合的。

表4空间机器人动力学参数

初始时刻空间机器人末端位置为P_e0＝(0,0,0)，单位m；空间机器人末端期望到达的位置为P_ef＝(0.3,0.1,5.5)，单位m；关节角度上下限：1.2≤q_i(t)≤1.2，单位rad；关节角速度上下限：

单位rad/s；节角加速度上下限：

单位rad/s²。规定规划时间t_f＝3s，离散点总数为200，为保证计算的效率，设定最高迭代次数k_max为100次，避障安全距离选为15cm。

本发明仿真在处理器配置为Intel Core i5-10400CPU2.90GHz和win10操作系统的计算机上进行。序列凸优化方法处理子凸问题采用MATLAB上的YALMIP优化环境和MOSEK求解子凸问题的求解器，Gauss伪谱法采用GPOPS-II工具箱求解。仿真结果如下：

表5 Gauss伪谱法与序列凸优化求解效率比较(信赖域ρ＝π/2)

由表5可知，仿真优化过程中，Gauss伪谱法迭代56次，用时23.56秒；而序列凸优化方法迭代11次，用时仅3.37秒。此外，由图3可知，Gauss伪谱法优化轨迹与序列凸优化轨迹几乎吻合，表明二者优化性能基本一致。

由图4——图6可知，Gauss伪谱法优化与序列凸优化下的空间机器人关节角度、角速度、角加速度轨迹平滑，且均满足约束要求，能够在实际工程中应用。

仿真结果表明，本发明所采用的序列凸优化方法相比于传统Gauss伪谱法，可以在不降低优化性能的前提下，极大地提升空间机器人轨迹优化问题的求解效率，进而达到实时性和快速性的要求。

以上内容仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明权利要求书的保护范围之内。

Claims (3)

1.一种基于改进序列凸优化方法的空间机器人轨迹规划方法，其特征在于，包括以下步骤：

步骤1，建立自由漂浮空间机器人最优轨迹规划模型并将模型凸化；

首先将空间机器人系统假设为基座与机械臂组成的无根多刚体系统，推导了空间机器人运动学方程，并针对空间机器人点对点轨迹规划建立模型；接着给出了一般最优轨迹规划问题描述形式，引入优化指标和避障约束，建立了末端时刻和捕获位置均固定的最优轨迹规划问题模型；最后对建立的非凸轨迹优化模型进行进行凸化处理；

步骤2，设计基于信赖域更新策略的序列凸优化方法，并对自由漂浮空间机器人轨迹规划问题进行求解验证；

首先设计了针对空间机器人轨迹规划问题的序列凸优化求解算法，并基于信赖域更新策略对序列凸优化方法进行改进，最后利用改进的序列凸优化方法与传统Guass伪谱法进行求解仿真和对比分析。

2.根据权利要求1所述的基于改进序列凸优化方法的空间机器人轨迹规划方法，其特征在于，步骤1建立自由漂浮空间机器人最优轨迹规划模型并将模型凸化的具体方法如下：

自由漂浮空间机器人的运动学方程：

其中，定义J_g为广义雅克比矩阵，D_g为广义动量；

对于本发明研究的问题，可以建模为一个最优控制问题，其具有如下的一般性形式(P0)：

其中t∈[t₀,t_f]，t₀表示任务初始时刻，t_f表示任务末时刻；x为空间机器人轨迹规划过程中的状态变量，u为过程中的控制变量；式s₁(x(t),u(t),t)≤0和s₂(x(t),u(t),t)＝0为空间机器人路径约束；ψ₁(x(t_f),t_f)≥0，ψ₂(x(t_f),t_f)＝0表示末时刻的等式约束和不等式约束；

以能量为优化指标选取目标函数:

综上所述，最优轨迹规划问题表述为P1：

选用连续线性化方式：连续线性化是指在前次迭代获得的已知解处重复性地线性化选定的非线性项的过程，这是一种有效转化非线性项的凸化方法，具体来说，非线性项f(x)能够被近似为：

那么运动学方程中

相可以线性化为：

接着将连续系统离散化处理，对问题P1进行离散，时间步长为Δt，步数为N，则t_f＝NΔt，另外，将关节角度q和角速度

作为状态量，关节角加速度

作为控制量u，即

则u_t＝u_k,

那么离散状态方程为：

因此

x_k+1＝A_Δtx_k+B_Δtu_k (8)

其中

进一步，上述问题表述为离散后的最优控制问题P2：

subject to x_k+1＝A_Δtx_k+B_Δtu_k

x^min≤x_k≤x^max

u^min≤u_k≤u^max

P_e(k＝N)＝P_e(t_f)

x(k＝0)＝[0 0]^T

χ(x_k)≤0

(9)

其中，χ(x_k)≤0为避障约束；

将式(5)带入避障约束和末端位置约束中，可获得避障约束和末端位置约束的线性形式，由于序列凸优化框架下的凸近似仅在信赖域上有效，因此这是一个在信任域上具有线性等式的凸二次形式，该形式符合凸优化方法求解的问题形式，可以直接带入序列凸优化框架中进行求解，但需要在求解时需要设立合适的信赖域；

因此问题可进一步表示为P3:

subject to x_k+1＝A_Δtx_k+B_Δtu_k

x^min≤x_k≤x^max

u^min≤u_k≤u^max

P_e(k＝N)＝P_e(t_f)

x(k＝0)＝[0 0]^T

χ(x_k)≤0

|x-x^k|≤ρ

(10)

其中，|x-x^k|≤ρ表示信赖域约束，ρ表示信赖域半径。

3.根据权利要求1所述的基于序列凸优化方法的空间机器人轨迹规划方法，其特征在于，步骤2建立自由漂浮空间机器人轨迹规划模型并将模型凸化的具体方法如下：

传统上序列凸优化算法的具体流程如表1所示，利用序列凸优化方法，可以获得序列解x₁,x₂,...,x_k,...，直至达到停止条件，获得最优解；

表1 序列凸优化方法

信赖更新策略具体如下：在序列凸优化迭代过程中，计算每次迭代实际性能指标函数ψ′与预测性能指标函数ψ的下降量分别为Δψ＝|ψ(x'^k+1,u'^k+1)-ψ(x'^k,u'^k)|和Δψ′＝|ψ′(x'^k+1,u'^k+1)-ψ′(x'^k,u'^k)|；

定义Δψ′与Δψ的比值

一般来说，预测性能指标函数ψ的下降量通常为正值，即Δψ＞0，因此，如果r_k＜0，则说明Δψ＜0，则△f：<0，原信赖域步长不能作为下一个迭代点，需要缩小信赖域半径重新求解子问题，若r_k比较接近1，说明二次模型与目标函数在信赖域范围内有很好的近似，此时原信赖域步长可以作为新的迭代点，同时下一次迭代时可以增大信赖域半径，对于其他情况，信赖域半径可以保持不变，具体信赖域更新策略见表2；

表2 信赖域更新策略

基于传统序列凸优化方法和信赖域更新策略，设计的改进序列凸优化方法的伪代码如表3所示；

表3 改进序列凸优化方法伪代码

Images