基于自适应阈值伪谱法的空间机器人轨迹优化

doi:10.11918/202508063

doi: 10.11918/202508063

倪浩¹ ，刘壮¹ ，马晓龙^2,3 ，张欧阳¹ ，陈萌^2,3 ，刘健行¹ ，吴立刚¹

1. 哈尔滨工业大学航天学院,哈尔滨 150090

2. 上海宇航系统工程研究所,上海 201109

3. 宇航空间机构全国重点实验室,上海 201109

基金项目: 国家自然科学基金(62373127,62033005,62503137) ；国家资助博士后研究人员计划(GZB20250957) ；中国博士后科学基金(2024M764189) ；中国航天科技集团公司第八研究院产学研合作基金(SAST2024-018,SAST2024-019)

详细信息

作者简介

倪浩(2004—),男,硕士研究生;

刘健行(1986—),男,教授,博士生导师；

吴立刚(1977—),男,教授，博士生导师

通讯作者

刘健行,jx.liu@hit.edu.cn

中图分类号: TP242.3

文献标识码: A

文章编号: 0367-6234(2026)01-0001-11

Adaptive threshold pseudo-spectral method for space robot trajectory optimization

NI Hao¹ ， LIU Zhuang¹ ， MA Xiaolong^2,3 ， ZHANG Ouyang¹ ， CHEN Meng^2,3 ， LIU Jianxing¹ ， WU Ligang¹

1. School of Astronautics, Harbin Institute of Technology, Harbin 150090 , China

2. Aerospace System Engineering Shanghai, Shanghai 201109 , China

3. National Key Laboratory of Aerospace Mechanism, Shanghai 201109 , China

摘要

为解决传统Radau伪谱法在轨迹优化求解效率与轨迹解可行性之间存在的矛盾,本文提出一种基于自适应阈值的分段升阶伪谱法，用于提升非线性优化问题的求解精度，加快收敛速度。该方法通过动态比较迭代过程中的误差矩阵极大值与标准时间步长,在误差超过自适应阈值的区间设置新分段点,并在误差小于偏差阈值的分段内增加配点数,这种策略对轨迹解的平滑/非平滑区间进行针对性优化,从而以较少的分段数和配点数达成轨迹规划目标。相比于传统Radau伪谱法，自适应分段升阶方法能够以较少迭代轮次收敛到期望结果,实现数值精度与求解效率的有效平衡。为验证算法性能，本研究基于自由飞行空间机器人非合作目标捕获场景进行了对比仿真实验。实验结果表明，本文提出的自适应分段升阶方法，在求解耗时和轨迹解的末端精度方面均优于对比方法,显示出更好的综合表现。

关键词

自由飞行空间机器人 / 轨迹规划 / 最优控制 / 伪谱法 / 自适应阈值

Abstract

To address the trade-off between solution efficiency and trajectory feasibility in traditional Radau Pseudo-spectral Method (RPM) for trajectory optimization, this paper proposes an adaptive threshold-based segmented polynomial-degree elevation pseudo-spectral method (AT-RPM). This approach aims to enhance nonlinear optimization accuracy and accelerate convergence. By dynamically comparing the maximum value of the error matrix with a standard time step during iterative process, the method establishes new segment points in intervals where the error exceeds an adaptive threshold. Furthermore, the number of collocation points within segments is also increased when the error falls below a deviation threshold. This strategy selectively optimizes both smooth and non-smooth regions of the trajectory solution, thereby enabling achievement of trajectory planning goals with fewer segments and collocation points. Compared with the traditional Radau Pseudo-spectral Method, AT-RPM can converge to the desired result in fewer iteration rounds, effectively balancing numerical accuracy and solution efficiency. To validate performance, comparative simulation experiments have been conducted in a non-cooperative target capture scenario involving a free-flying space robot. The results indicate that the proposed method outperforms the existing approaches in both computation time and terminal accuracy, demonstrating superior overall performance.

Keywords

free-flying space robot / trajectory planning / optimal control / pseudo-spectral method / adaptive threshold

1 FFSR系统建模 2 轨迹规划任务分析 2.1 最优控制问题一般数学表述 2.2 面向目标捕获任务的约束条件与性能指标 3 Radau伪谱法改进 3.1 全过程离散化 3.2 自适应分段升阶策略 4 仿真实验 5 结论

随着太空探索的规模扩大、复杂性提升，传统依赖人工操作的执行方式已难以满足高频次、高精度的在轨服务需求，空间机器人作为一种替代方案而日益受到重视^[1]。自由飞行空间机器人（free-flying space robot，FFSR）可通过推进器和动量轮对基座、机械臂的位姿进行同步控制，如图1所示。FFSR具备高度灵活性，可替代宇航员完成危险性工作，在目标捕获、碎片清理等任务中具有广阔应用前景^[2-3]。

图1自由飞行空间机器人在轨服务^[1]

Fig.1A free-flying space robot performing on-orbit servicing^[1]

然而，保障FFSR正常工作面临诸多挑战，其中轨迹优化策略直接影响机器人太空操作的稳定性、快速性和精确度^[4-5]。本质上，空间机器人轨迹优化是有约束条件下的指标最优问题，机器人需要在视角受限、规避障碍等前提下完成任务^[6]。解决此类问题有直接法和间接法两种思路^[7]。间接法利用极小值原理将问题转化为两点边值问题，通过求解微分代数方程获得最优轨迹。采用间接法有助于深入理解系统行为，但其数学推导过程常十分复杂，求解速度也非常缓慢，实际应用中更多采用直接法求解。直接法采用配点思想对问题作离散化并求数值解，通常求解速度快但数值精度低。国内外大量研究人员致力于改进直接法求解轨迹优化问题^[8-9]。

伪谱法是直接法中的一种经典思路，其采用正交多项式作为基函数，利用积分在离散点上实现微分或代数方程近似，将最优控制问题转化为非线性规划（non-linear programing，NLP）问题进行求解^[10]，具有较高的收敛阶数，利用少量配点即可逼近最优解。2011年Grag等^[11]最早将Radau伪谱法应用于求解有限时间和无限时间下的一般轨迹优化问题。2012年Li等^[12]利用伪谱法完成了漂浮基座空间机器人的轨迹优化求解，轨迹解同时兼顾了时间消耗和能量积分。2023年Zhang等^[13]提出依据误差曲率密度积分计算合理分段位置，在FFSR轨迹优化任务中取得了良好表现，其以更少配点取得了几乎相同的数值精度。尽管已有一些研究工作对伪谱法作出改进^[14-15]，但其在推广应用中始终面临矛盾:若全程采用高阶多项式，计算量将显著增加并引发数值稳定性问题; 若仅采用低阶多项式，则难以保证解的精度。增加拟合多项式阶数可提高数值精度，但将显著增加求解时间; 增加分段数能够加快收敛速度，但常导致精度下降较多。

为了解决上述矛盾，在分析现有伪谱法改进思路的基础上，本文提出了一种计算阈值自适应更新离散网格迭代的改进Radau伪谱法，该方法通过优化分段区间配置和段内配点数，兼顾求解精度提升、计算效率提高。该方法将在FFSR目标捕获轨迹优化问题中通过仿真实验进行验证。

1 FFSR系统建模

FFSR工作于太空微重力环境，其动力学应符合如下假设^[13]:

1）FFSR是由刚性基座和刚性多连杆组成的多刚体系统，且基座和连杆质量均匀分布；

2）考虑均由旋转关节组成的FFSR构型；

3）接近-绕飞阶段运动过程中FFSR与目标之间相互作用力几乎无变化，且不相互影响运动；

4）忽略地心引力对FFSR系统的影响。

基于上述预设，可在惯性系{I}下描述FFSR基座、机械臂的位置与姿态。FFSR系统示意图如图2所示，其整体由基座与多连杆串联组成，{b}为基座系，{i}为连杆i系，J_i为关节i，惯性系原点指向关节i和连杆i质心的矢量分别为p_i，r_i，惯性系原点指向基座和末端的矢量分别为r_b，r_e，基座指向关节J₁的矢量为b₀，关节J_i指向连杆i质心C_i的矢量为a_i，连杆i质心C_i指向关节J_i₊₁的矢量为b_i。

图2自由飞行空间机器人结构示意图

Fig.2Schematic diagram of the free-flying space robot

根据图2可推导得到{I}系下连杆i质心的位置矢量

^{I} r_{i}

和末端e的位置矢量

^{I} r_{e}

满足:

^{I} r_{i} =^{I} r_{b} +^{I} b_{0} + \sum_{j = 1}^{i}^{I} a_{j} + \sum_{j = 1}^{i - 1}^{I} b_{j}

(1)

^{I} r_{e} =^{I} r_{b} +^{I} b_{0} + \sum_{j = 1}^{i}^{I} a_{j} + \sum_{j = 1}^{i}^{I} b_{j}

(2)

对位置矢量求时间导数可得到连杆 i 质心速

^{I} v_{i}

和末端 e 速度

^{I} v_{e}

^{I} v_{i} =^{I} v_{b} +^{I} w_{b} \times (^{I} r_{i} -^{I} r_{b}) + \sum_{j = 1}^{i} [^{I} z_{j} \times {\dot{θ}}_{j} (^{I} r_{i} -^{I} p_{j})]

(3)

^{I} v_{e} =^{I} v_{b} +^{I} w_{b} \times (^{I} r_{e} -^{I} r_{b}) + \sum_{j = 1}^{n} [^{I} z_{j} \times {\dot{θ}}_{j} (^{I} r_{e} -^{I} p_{j})]

(4)

式中:

^{I} w_{b}

为{I}系下基座角速度，^Iz_j 为关节 J_i 轴线方向的单位矢量，

{\dot{θ}}_{j}

为关节 J_i 角速度。连杆 i 质心角速度^Iw_i、末端 e 的角速度^Iw_e 可通过下式求得:

^{I} w_{i} =^{I} w_{b} +^{b} w_{i} =^{I} w_{b} + \sum_{j = 1} ({\dot{θ}}_{j}^{I} z_{j})

(5)

^{I} w_{e} =^{I} w_{b} +^{b} w_{e} =^{I} w_{b} + \sum_{j = 1}^{n} ({\dot{θ}}_{j}^{I} z_{j})

(6)

将式（3）~式（6）整理为矩阵方程:

[\begin{matrix} ^{I} v_{i} \\ ^{I} w_{i} \end{matrix}] = [\begin{matrix} E_{3 \times 3} & ^{I} r_{i b}^{\times} \\ 0_{3 \times 3} & E_{3 \times 3} \end{matrix}] [\begin{matrix} ^{I} v_{b} \\ ^{I} w_{b} \end{matrix}] + [\begin{matrix} J_{T i} \\ J_{R i} \end{matrix}] {\dot{q}}_{m}

[\begin{matrix} ^{I} v_{e} \\ ^{I} w_{e} \end{matrix}] = [\begin{matrix} E_{3 \times 3} & ^{I} r_{e b}^{\times} \\ 0_{3 \times 3} & E_{3 \times 3} \end{matrix}] [\begin{matrix} ^{I} v_{b} \\ ^{I} w_{b} \end{matrix}] + [\begin{matrix} J_{T n} \\ J_{R n} \end{matrix}] {\dot{q}}_{m}

(7)

式中:

^{I} r_{i b}^{\times}

为向量

^{I} r_{i b}^{\times}

的叉乘操作数，即^Ir_i_b与任意向量 a 作叉乘运算，可以等效为作点乘运算

:^{I} r_{i b} \times a =^{I} r_{i b}^{\times} a_{\circ}

关节量导数

{\dot{q}}_{m} = {[\begin{matrix} {\dot{θ}}_{1} {\dot{θ}}_{2} \dots {\dot{θ}}_{n} \end{matrix}]}^{T} 。

矩阵

J_{T i} \in R^{3 \times n} ， J_{R i} \in R^{3 \times n} ， J_{T n} \in R^{3 \times n} ， J_{R n} \in R^{3 \times n}

满足:

J_{T i} = [\begin{matrix} ^{I} z_{1} \times (^{I} r_{i} -^{I} p_{1}) \dots^{I} z_{i} \times (^{I} r_{i} -^{I} p_{i}) 0_{3 \times 1} \dots 0_{3 \times 1} \end{matrix}]

J_{R i} = [\begin{matrix} ^{I} z_{1} \dots^{I} z_{i} 0_{3 \times 1} \dots 0_{3 \times 1} \end{matrix}]

J_{T n} = [\begin{matrix} ^{I} z_{1} \times (^{I} r_{i} -^{I} p_{1}) \dots^{I} z_{n} \times (^{I} r_{i} -^{I} p_{n}) \end{matrix}]

J_{R n} = [\begin{matrix} ^{I} z_{1}^{I} z_{2} \dots^{I} z_{n} \end{matrix}]

(8)

定义 FFSR 系统拉格朗日函数 L 为

L = W - P

(9)

式中：W，P分别为系统动能和势能。考虑假设4），势能近似为零，总能量可近似为动能。满足如下表达式:

W = W_{v} + W_{w} = \frac{1}{2} \sum_{i = 0}^{n} m_{i}^{I} v_{i}^{T I} v_{i} + \frac{1}{2} \sum_{i = 0}^{n}^{I} w_{i}^{T} I_{i}^{I} w_{i}

(10)

式中：W_v，W_w分别描述系统的平动动能和转动动能，m为质量，I为转动惯量，下标i=0代表基座的质量、速度、角速度。

W_{v} = \frac{1}{2} \sum_{i = 0}^{n} {(^{I} w_{b} + J_{R i} {\dot{q}}_{m})}^{T} I_{i} (^{I} w_{b} + J_{R i} {\dot{q}}_{m})

W_{w} = \frac{1}{2} \sum_{i = 0}^{n} {([\begin{matrix} E_{3}^{I} r_{i b}^{\times} \end{matrix}] [\begin{matrix} ^{I} v_{b} \\ ^{I} w_{b} \end{matrix}] + J_{T i} {\dot{q}}_{m})}^{T} .

I_{i} ([\begin{matrix} E_{3}^{I} r_{i b}^{\times} \end{matrix}] [\begin{matrix} ^{I} v_{b} \\ ^{I} w_{b} \end{matrix}] + J_{T i} {\dot{q}}_{m})

(11)

故可将FFSR系统的拉格朗日函数L整理为

L = W = \frac{1}{2} {\dot{q}}^{T} D \dot{q}

(12)

式中:

q = [\begin{matrix} r_{b}^{T} q_{b}^{T} q_{m}^{T} \end{matrix}]

为广义坐标，包含基座位置、基座转角和关节转角信息。对其求导可得到运动信息

\dot{q} = [\begin{matrix} v_{b}^{T} w_{b}^{T} w_{m}^{T} \end{matrix}] 。

D为惯性矩阵，具体形式为:

D = [\begin{matrix} D_{11} D_{12} D_{13} \\ D_{21} D_{22} D_{23} \\ D_{31} D_{32} D_{33} \end{matrix}]

(13)

其中，矩阵各项为

D_{11} = \sum_{i = 0}^{n} m_{i} E_{3} D_{12} = \sum_{i = 0}^{n} m_{i} r_{i b}^{\times} D_{13} = \sum_{i = 0}^{n} m_{i} J_{T i}

D_{21} = \sum_{i = 0}^{n} m_{i} r_{b i}^{\times} D_{22} = \sum_{i = 0}^{n} (I_{i} - m_{i} r_{b i}^{\times} r_{b i}^{\times})

D_{23} = \sum_{i = 0}^{n} (I_{i} J_{R i} + m_{i} r_{b i}^{\times} J_{T i}) D_{31} = \sum_{i = 0}^{n} m_{i} J_{T i}^{T}

D_{32} = \sum_{i = 0}^{n} (J_{R i}^{T} I_{i} + m_{i} J_{T i}^{T} r_{i b}^{\times})

D_{33} = \sum_{i = 0}^{n} (J_{R i}^{T} I_{i} J_{R i} + m_{i} J_{T i}^{T} J_{T i})

(14)

惯性矩阵D是一个正定矩阵，描述了机器人的质量分布和几何特性。将式（12）代入拉格朗日方程可得:

D (q) \ddot{q} + C (q, \dot{q}) \dot{q} = u

(15)

式中：控制输入

u = {[\begin{matrix} F_{b}^{T} τ_{b}^{T} τ_{m}^{T} \end{matrix}]}^{T} ， F_{b} ， τ_{b} ， τ_{m}

分别为施加到基座上的力、力矩效果，以及施加在关节上的力矩效果，

C （ q ， \dot{q} ）

为科里奥利和离心力矩阵，其形式不唯一，

C （ q ， \dot{q} ） \dot{q}

对应科氏力项，满足如下等式:

C (q, \dot{q}) \dot{q} = \frac{d D (q)}{d t} \dot{q} - \frac{\partial L}{\partial q}

(16)

2 轨迹规划任务分析

2.1 最优控制问题一般数学表述

FFSR的轨迹规划问题实际是一个非线性连续时变系统的最优控制问题。Bolza型最优控制问题的性能指标一般形式为^[16]

m i n J_{b} = Φ (x (t_{0}), x (t_{f}), t_{0}, t_{f}) + \int_{t_{0}}^{t_{f}} L (x (t), u (t), t) d t

(17)

式中：Φ（·）为边值性能指标映射，L（·）为积分性能指标映射，两者共同评估解的表现。

系统迭代更新应满足状态方程:

\dot{x} (t) = f (x (t), u (t), t)

(18)

运动中始终满足等式、不等式约束:

\begin{matrix} C_{1} (x (t), u (t), t) = 0 \\ C_{2} (x (t), u (t), t) ⩽ 0 \end{matrix}

(19)

系统的边界条件为:

C_{3} (x (t_{0}), x (t_{f}), t_{0}, t_{f}) = 0

(20)

联立式（17）~（20）可得到轨迹规划问题的一般数学描述。具体到目标捕获任务，约束包括运动过程中的视角跟随、动态避障、状态方程约束和边界条件限制，性能指标通常选择最小化末端距离和电机能量消耗。

2.2 面向目标捕获任务的约束条件与性能指标

将式（15）FFSR动力学方程改写为如下形式:

[\begin{matrix} \dot{q} \\ \ddot{q} \end{matrix}] = [\begin{matrix} 0 & E \\ 0 & - D (q)^{- 1} C (q, \dot{q}) \end{matrix}] [\begin{matrix} q \\ \dot{q} \end{matrix}] + [\begin{matrix} 0 \\ D (q)^{- 1} \end{matrix}] u

(21)

令

X = {[\begin{matrix} q \dot{q} \end{matrix}]}^{T} ， U = u ， \dot{X} = {[\begin{matrix} \dot{q} \ddot{q} \end{matrix}]}^{T} ，

则系统矩阵A和输入矩阵B分别为:

A = [\begin{matrix} 0 & E \\ 0 & - D (q)^{- 1} C (q, \dot{q}) \end{matrix}], B = [\begin{matrix} 0 \\ D (q)^{- 1} \end{matrix}]

(22)

即得到状态方程一般形式，并可进一步写作:

\dot{X} = A X + B U = f (X (t), U (t), t)

(23)

状态方程反映了系统的动力学特性，直接与控制输入相关联，也是状态解所需满足的最基本约束。FFSR在捕获目标时，还需要保证目视目标，而传感器工作范围有限，故需定义视场约束，如图3所示。

图3视场约束示意图

Fig.3Illustration of the field-of-view constraint

图中各参数上标I，b，t分别表示惯性系、基座系、目标系，r_b，r_t分别表示FFSR基座和目标在惯性系中的位置，r_ct为空间机器人传感器中心指向目标质心的矢量，v_c，v_g分别表示传感器和目标捕获点的主轴方向。视场约束包括目视和跟随两方面，分别用目视角和跟随角进行描述。

定义目视角θ_fov为矢量r_ct，v_c所成夹角，跟随角θ_rot为矢量 r_tc，v_g 所成夹角。在捕获过程中，两角度需要保证在一定范围内:

\begin{matrix} c o s θ_{f o v} ⩾ c o s θ_{f o v M a x} \\ c o s θ_{r o t} ⩾ c o s θ_{r o t M a x} \end{matrix}

(24)

FFSR捕获目标时，不能对目标本体造成碰撞破坏。通常目标构型包括细长的太阳能板等设备。若仅使用一个包络圆包裹所有设备，障碍区会非常冗余，导致规划的约束条件非常严格，最终可能无法求解得到可行解。本研究中采用多个相交圆来设置避障约束，如图4所示。

图4避障约束示意图

Fig.4Illustration of the obstacle avoidance constraint

当两种障碍区圆不相交，可认为约束被满足。将自身障碍区圆个数记作N_obsR，第i个圆的圆心位置记作r_obsR_i，直径记作d_obsR_i（i=1，2，···，N_obsR）。将目标障碍区圆个数记作N_obsT，第i个圆的圆心位置记作r_obsT_i，直径记作d_obsT_i（i=1，2，···，N_obsT）。对于第i个目标障碍区圆，其圆心指向第j个自身障碍区圆的位矢r_dis（i，j）为:

r_{d i s (i, j)} = r_{o b s T i} - r_{o b s R j}

(25)

位矢模长将大于两圆半径之和，确保两障碍区无交点:

‖r_{d i s (i, j)}‖ ⩾ \frac{1}{2} (d_{o b s R j} + d_{o b s T i}),

(i = 1,2, \dots, N_{o b s T}, j = 1,2, \dots, N_{o b s R})

(26)

障碍圆的设置基于机器人与目标的实际几何外接圆与关键特征点（如太阳能板端点、基座角点），通过四等分或等间距投影方式确定圆心位置，半径则根据实际构件尺寸和安全裕度确定。

FFSR由电机进行驱动，所能产生的驱动力、力矩的大小有限，故运动过程中的控制律U应始终满足:

U_{m i n} ⩽ U ⩽ U_{m a x}

(27)

式中：U_min=[U_1min，U_2min，···，U_kmin]^T，U_max=[U_1max，U_2max，···，U_kmax]^T分别对应各关节电机驱动力/力矩的输出最小值和最大值，k为电机个数。

FFSR由给定初位置运动到期望末位置，自然形成状态量的边界条件约束:

X (t_{0}) = [\begin{matrix} r_{b} (t_{0}) q_{b} (t_{0}) q_{m} (t_{0}) \end{matrix} {{\dot{r}}_{b} (t_{0}) {\dot{q}}_{b} (t_{0}) {\dot{q}}_{m} (t_{0})]}^{T}

X (t_{f}) = [\begin{matrix} r_{b} (t_{f}) & q_{b} (t_{f}) & q_{m} (t_{f}) \end{matrix} {{\dot{r}}_{b} (t_{f}) {\dot{q}}_{b} (t_{f}) {\dot{q}}_{m} (t_{f})]}^{T}

(28)

式中：t₀，t_f分别为运动始末时刻，

r_{b}

，

{\dot{r}}_{b}

分别为基座的位置和线速度，

q_{b}

，

{\dot{q}}_{b}

分别为基座的转角和角速度，

q_{m}

，

{\dot{q}}_{m}

分别为关节的角位置和角速度。

对于目标捕获任务，通常有初始时刻推进器开机和终止时刻推进器关机的操作，因此控制律U的边界条件为:

U (t_{0}) = {[\begin{matrix} F_{b} (t_{0}) τ_{b} (t_{0}) τ_{m} (t_{0}) \end{matrix}]}^{T}

U (t_{f}) = {[\begin{matrix} F_{b} (t_{f}) τ_{b} (t_{f}) τ_{m} (t_{f}) \end{matrix}]}^{T}

(29)

轨迹规划需设定优化方向，评估求解改进效果。在目标捕获任务中，末端捕获误差、推进电机耗能是重要的优化对象，因而可写出性能指标:

J_{b} = k_{1} ‖r_{ef} - r_{eg}‖ + k_{2} \int_{t_{0}}^{t_{f}} U (t)^{T} Q_{u} U (t) d t

(30)

式中：

r_{eg}

和

r_{ef}

分别为期望到达和最终实际位置，可分别由关节量解算得到；

Q_{u}

为输入权重矩阵，衡量了不同控制输入的耗能权重；k₁，k₂分别为边值性能和积分性能的关注度，其数值选取基于任务优先级（如均衡考虑末端精度、能量消耗）与指标实际值。

综上分析，可给出FFSR目标捕获任务轨迹优化问题的完整描述:

m i n J_{b} = k_{1} ‖r_{ef} - r_{eg}‖ + k_{2} \int_{t_{0}}^{t_{f}} U (t)^{T} Q_{u} U (t) d t

s. t. \dot{X} = A X + B U = f (X (t), U (t), t)

X (t_{0}) = [\begin{matrix} r_{b} (t_{0}) q_{b} (t_{0}) q_{m} (t_{0}) \end{matrix} {{\dot{r}}_{b} (t_{0}) {\dot{q}}_{b} (t_{0}) {\dot{q}}_{m} (t_{0})]}^{T}

X (t_{f}) = [\begin{matrix} r_{b} (t_{f}) q_{b} (t_{f}) q_{m} (t_{f}) \end{matrix} {{\dot{r}}_{b} (t_{f}) {\dot{q}}_{b} (t_{f}) {\dot{q}}_{m} (t_{f})]}^{T}

U (t_{0}) = {[\begin{matrix} F_{b} (t_{0}) τ_{b} (t_{0}) τ_{m} (t_{0}) \end{matrix}]}^{T}

U (t_{f}) = {[\begin{matrix} F_{b} (t_{f}) τ_{b} (t_{f}) τ_{m} (t_{f}) \end{matrix}]}^{T}

c o s θ_{f o v} ⩾ c o s θ_{f o v M a x}

c o s θ_{r o t} ⩾ c o s θ_{r o t M a x}

‖r_{d i s (i, j)}‖ ⩾ \frac{1}{2} (d_{o b s R j} + d_{o b s T i}),

(i = 1, 2, \dots, N_{o b s T}, j = 1, 2, \dots, N_{o b s R})

U_{m i n} ⩽ U ⩽ U_{m a x}

(31)

3 Radau伪谱法改进

3.1 全过程离散化

式（31）给出的非线性连续时变系统最优控制问题，直接求解析解过于复杂，考虑将其转化为有限离散点的规划问题求数值解。Radau伪谱法采用勒让德-高斯-拉道（legendre-gauss-radau，LGR）正交配点对状态量和控制律同步离散化，再调用优化器求解。

首先将原始问题的时间区间t∈[t₀，t_f]通过下述方式映射到标准时间区间τ∈[-1，1]:

τ = \frac{2}{t_{f} - t_{0}} t - \frac{t_{f} + t_{0}}{t_{f} - t_{0}}

t = \frac{t_{f} - t_{0}}{2} τ + \frac{t_{f} + t_{0}}{2}

(32)

在标准时间区间τ∈[-1，1]内，将LGR函数零点作为配点:

P_{n - 1} (τ) - P_{n} (τ) = 0

(33)

式中P_n（τ）为n阶Legendre多项式，其表达式为:

P_{n} (x) = \sum_{m = 0}^{M} (- 1)^{m} \frac{(2 n - 2 m)!}{2^{n} m! (n - m)! (n - 2 m)!} x^{n - 2 m}

(34)

式中参数M满足:

M = \{\begin{matrix} \frac{n}{2}, n = 2 k, k \in Z \\ \frac{n - 1}{2}, n = 2 k - 1, k \in Z \end{matrix}

(35)

式（33）方程有N个根，将其记作τ₁，τ₂，···，τ_N，这些点的范围是τ_i∈（-1，1]。补充定义一个起点τ₀=-1，这N+1个点共同构成规划问题的求解目标点。由N+1个点可写出i配点处的N阶Lagrange插值多项式为:

L_{i} (τ) = \prod_{j = 0, j \neq i}^{N} \frac{τ - τ_{j}}{τ_{i} - τ_{j}}, (i = 0,1, \dots, N)

(36)

系统状态序列可近似离散为:

X (τ) \approx \sum_{i = 0}^{N} X_{i} \cdot L_{i} (τ)

(37)

式中X_i=X（τ_i），表示在时间τ_i处采样状态量。

系统的控制序列可近似离散为:

U (τ) \approx \sum_{i = 1}^{N} U_{i} \cdot {\hat{L}}_{i} (τ)

(38)

式中Lagrange插值多项式

{\hat{L}}_{i} （ τ ）

不使用初始时刻τ₀=-1节点，

{\hat{L}}_{i} （ τ ）

写作:

{\hat{L}}_{i} (τ) = \prod_{j = 1, j \neq i}^{N} \frac{τ - τ_{j}}{τ_{i} - τ_{j}}, (i = 0,1, \dots, N)

(39)

系统性能指标积分可用求和形式表示:

\int_{- 1}^{1} L (X (τ), U (τ), τ) d τ = \sum_{i = 1}^{N} w_{i} L (X_{i}, U_{i}, τ_{i})

(40)

式中w_i为高斯积分权重，衡量了离散点处性能指标对规划全过程的贡献，计算公式为:

w_{i} = \int_{- 1}^{1} {\hat{L}}_{i} (τ) d τ

(41)

系统状态方程可改写为离散形式:

\dot{X} (τ_{j}) \approx \sum_{i = 0}^{N} {\dot{L}}_{i} (τ_{j}) X_{i} = \sum_{i = 0}^{N} W_{j i} X_{i}, (j = 1,2, \dots, N)

(42)

式中W_ji为N×（N+1）的微分权重矩阵，每一行代表一个时刻，每一列代表所有配点的状态。

综上所述，式（17）~式（20）描述的连续Bolza形式最优控制问题，经离散化可转化为如下NLP问题:

\begin{matrix} m i n J_{b} = Φ (X (- 1), X (1), - 1,1) + \\ \frac{t_{f} - t_{0}}{2} \sum_{i = 1}^{N} w_{i} L (X_{i}, U_{i}, τ_{i}) \\ s . t . \sum_{i = 0}^{N} W_{j i} X_{i} - \frac{t_{f} - t_{0}}{2} f (X (τ_{j}), U (τ_{j}), τ_{j}) = 0, \\ (j = 1,2, \dots, N) \\ C_{1} (X (τ), U (τ), τ) = 0 \\ C_{2} (X (τ), U (τ), τ) ⩽ 0 \\ C_{3} (X (- 1), X (1), - 1,1) = 0 \end{matrix}

(43)

全局Radau伪谱法转化后，优化器待求解的变量包括:标准离散时间τ₁，τ₂，···，τ_N、离散状态量X₁，X₂，···，X_N、离散控制量U₁，U₂，···，U_N。

3.2 自适应分段升阶策略

全局伪谱法缺乏局部适应性，对于具有非平滑特性（如控制量饱和、非线性约束）的轨迹，全局高阶近似效率低下，易产生吉布斯现象。在全局方法的背景下，增加配点能够提高拟合多项式的阶数，可行性误差往往减小，但求解耗时也相应变长。已有研究^[17]指出:增加配点可以使轨迹平滑处呈现指数级收敛趋势; 在轨迹不平滑位置添加分段点可以构造平滑区间。当前应用较多的分段伪谱法依赖曲率和积分运算确定分段位置，计算代价较大。本研究提出一种基于阈值的自适应策略，旨在通过阈值判断实现更高效的分段和升阶决策，提升优化问题求解精度、加快求解速度。

在问题时间区间[t₀，t_f]之间插入P-1个分段点，将时间域整体细分为[t₀t₁···t_i_-1t_i···t_f]的P个小段，第i段时间区间为[t_i_-1，t_i]，结合式（34），可将第i段时间区间变换到标准时间区间:

τ = \frac{2}{t_{i} - t_{i - 1}} t - \frac{t_{i} + t_{i - 1}}{t_{i} - t_{i - 1}}, t \in [t_{i - 1}, t_{i}]

(44)

设第i段内有N_i个LGR配点，并补充初始节点

τ_{0}^{（ i ）} = - 1 ，

即共有N_i+1个LGR节点:

τ_{0}^{（ i ）} ， τ_{1}^{（ i ）} ， \dots ， τ_{N}^{（ i ）}

。第i段内的状态量、控制律分别用X^（ⁱ^），U^（ⁱ^）进行表示:

\begin{matrix} X^{(i)} (τ) \approx \sum_{j = 0}^{N_{i}} X_{j}^{(i)} \cdot L_{j}^{(i)} (τ) \\ L_{j}^{(i)} (τ) = \prod_{k = 0, k \neq j}^{N_{i}} \frac{τ - τ_{k}^{(i)}}{τ_{j}^{(i)} - τ_{k}^{(i)}}, (j = 0,1, \dots, N_{i}) \end{matrix}

(45)

\begin{matrix} U^{(i)} (τ) \approx \sum_{j = 0}^{N_{i}} U_{j}^{(i)} \cdot {\hat{L}}_{j}^{(i)} (τ) \\ {\hat{L}}_{j}^{(i)} (τ) = \prod_{k = 1, k \neq j}^{N_{i}} \frac{τ - τ_{k}^{(i)}}{τ_{j}^{(i)} - τ_{k}^{(i)}}, (j = 1,2 \dots, N_{i}) \end{matrix}

(46)

子段i内的性能指标为:

\begin{matrix} J_{b}^{(i)} = Φ (X_{0}^{(i)}, X_{N_{i}}^{(i)}, τ_{0}^{(i)}, τ_{N_{i}}^{(i)}) + \\ \frac{t_{i} - t_{i - 1}}{2} \sum_{j = 1}^{N_{i}} w_{j}^{(i)} L (X_{j}^{(i)}, U_{j}^{(i)}, τ_{j}^{(i)}) \end{matrix}

(47)

子段i内的状态方程约束为:

\begin{matrix} \sum_{j = 0}^{N_{i}} W_{k j}^{(i)} X_{j}^{(i)} - \frac{t_{i} - t_{i - 1}}{2} f (X_{k}^{(i)}, U_{k}^{(i)}, τ_{k}^{(i)}) = 0, \\ (k = 1,2, \dots, N_{i}) \\ W_{k j}^{(i)} = {\dot{L}}_{j}^{(i)} (τ_{k}^{(i)}), (k = 1,2, \dots, N_{i}) \end{matrix}

(48)

分段伪谱法通过引入分段点，将各段分别映射到标准时间区间进行规划，需要保证相邻两子段的状态量和控制量连续。分段伪谱法离散化过程如图5所示。

考虑第i个子段的LGR节点

τ_{0}^{(i)} ， τ_{1}^{(i)} ， \dots ， τ_{N_{i}}^{(i)}

，τ_j节点处的状态导数误差可以写作:

\begin{matrix} e_{τ_{j}}^{(i)} = |{\dot{X}}^{(i)} (τ_{j}^{(i)}) - \frac{t_{i} - t_{i - 1}}{2} f (X^{(i)} (τ_{j}^{(i)}), \\ U^{(i)} (τ_{j}^{(i)}), τ_{j}^{(i)}) ∣, (j = 1,2, \dots, N_{i}) \end{matrix}

(49)

式中：

e_{τ_{j}}^{（ i ）} 为 1 \times N_{x}

行向量，N_x为FFSR系统的状态维度，

X^{（ i ）} (τ_{j}^{（ i ）}) ， {\dot{X}}^{（ i ）} (τ_{j}^{（ i ）})

分别表示

τ_{j}^{（ i ）}

节点处的状态量和其导数值，

U^{（ i ）} (τ_{j}^{（ i ）})

表示

τ_{j}^{（ i ）}

节点处的控制律。绝对值中的第一项由插值得到，第二项根据自身动力学方程计算得到，两者之差衡量了轨迹规划的精度水平和可优化位置。直接根据LGR节点计算偏差，作用于插值多项式的导数

{\dot{L}}_{j}^{（ i ）} （ τ ）

进一步引入节点中间时刻的插值状态及其导数偏差，可将插值多项式

L_{j}^{（ i ）} （ τ ）

自身也纳入到最优化网格加细过程。LGR节点中间时刻为:

{\bar{τ}}_{j}^{(i)} = \frac{τ_{j - 1}^{(i)} + τ_{j}^{(i)}}{2}, (j = 1,2, \dots, N_{i})

(50)

中点处偏差计算方式同式（48）。统计段内所有节点偏差可堆叠得到形状为2N_i×N_x的矩阵:

e^{(i)} = [\begin{matrix} {(e_{{\tilde{τ}}_{1}}^{(i)})}^{T} {(e_{τ_{1}}^{(i)})}^{T} {(e_{{\bar{τ}}_{2}}^{(i)})}^{T} {(e_{τ_{2}}^{(i)})}^{T} \dots \end{matrix} {{(e_{{\tilde{τ}}_{N_{i}}}^{(i)})}^{T} {(e_{τ_{N_{i}}}^{(i)})}^{T}]}^{T}

(51)

式中e^（ⁱ^）矩阵每一行代表同一时刻下不同分量的偏差，每一列代表不同时刻状态分量变化。从任一状态分量的视角去看，误差的数值分布可能呈现出阶段性平缓或陡峭的特征，如图6所示。

图5分段伪谱法示意图

Fig.5Illustration of the segmented pseudo-spectral method

图6自适应分段升阶改进策略示意图

Fig.6Illustration of the adaptive segmented upgrading strategy

记录偏差矩阵e^（ⁱ^）中的最大元素

e_{m a x}^{(i)}

，将其与偏差阈值ε作比较，如果有

e_{m a x}^{(i)}

＜ε，且N_i＞N_min（图6中粉色P1区域），则认为该分段内精度达到要求，无需对网格继续加细优化，N_min表示段内配点下限。如果

e_{m a x}^{(i)}

＜ε，而N_i＜N_min（图6粉色P2圆形区域），则说明该段呈现平滑特征，且提升段内配点数可以提高收敛速度，分段点位置无需变动，该段内配点数增加为:

N_{i}^{k + 1} = m i n \{N_{i}^{k} + M_{1}, N_{m a x}\}

(52)

式中：M₁为一个固定值，表示光滑情况下的新增配点数，k为数值迭代进入第k轮，N_max为段内配点上限，min{·}函数取两者较小值，实现上限截断，避免单段内配点过多、拟合多项式阶数过高导致计算效率低下。

如果

e_{m a x}^{(i)}

＞ε，说明轨迹出现了较大偏差，应插入分段点构造分段光滑区域进行优化。记录

e_{m a x}^{(i)}

出现的列数为m，代表状态量X的第m个分量

X_{m}^{(i)}

，图6即可被理解为

X_{m}^{(i)}

在一系列配点处计算得出的状态导数偏差。记录

X_{m}^{(i)}

的2N_i-1个偏差点中超过阈值的极大值位置，设这些点对应标准时间为

τ_{m_{1}}^{（ i ）} ， τ_{m_{2}}^{（ i ）} ， \dots ， τ_{m_{n}}^{（ i ）} ， (n ⩽ N_{i} ， m_{1} < m_{2} < \dots < m_{n}) ，

对应偏差值为

e_{m}^{（ i ）} (τ_{m_{1}}^{（ i ）}) ， e_{m}^{（ i ）} (τ_{m_{2}}^{（ i ）}) ， \dots ， e_{m}^{（ i ）} (τ_{m_{n}}^{（ i ）})

。

定义伪谱标准时间自适应阈值η:

η (m_{a}) = λ (l o g \frac{e_{m}^{(i)} (τ_{m_{a}}^{(i)})}{ε} + l o g \frac{e_{m}^{(i)} (τ_{α}^{(i)})}{ε}), (a = 1,2, \dots, N_{i})

(53)

式中自适应阈值增益λ可调，用于控制分段策略的敏感程度，较大的λ值会更倾向于采用分段方式优化迭代过程。α记录了上一分段点的设置位置，一定有

τ_{α}^{（ i ）} < τ_{m_{a}}^{（ i ）}

。对于第一个猜测分段点

τ_{m_{1}}^{（ i ）}

，设置

τ_{α}^{（ i ）} = τ_{0}^{（ i ）} = - 1 \circ η (m_{a})

的设计基于相邻分段点的误差超限量之和，其目的是将局部误差映射为时间间隔上的分段需求，选择执行分段的判定条件为:

η (m_{a}) > τ_{m_{a}}^{(i)} - τ_{α}^{(i)}, (a = 1,2, \dots, N_{i})

(54)

即当前点

τ_{m_{a}}^{(i)}

与上一分段点

τ_{α}^{(i)}

的自适应阈值 η 超过自身标准时间间隔，才执行分段。分段数和新段内配点数的选择策略为:

\begin{matrix} N B_{i}^{k + 1} = m a x \{c e i l (\frac{η + N_{i}^{k}}{N_{m i n}}), M_{2}\} \\ N_{j}^{k + 1} = N_{m i n}, j = 1, \dots, N B_{i}^{k + 1} \end{matrix}

(55)

式中：

N_{i}^{k}

表示在第k轮迭代中i分段内的配点数，M₂为固定值，表示新增分段数的下限，ceil（·）表示向上取整数，

N B_{i}^{k + 1}

为下轮迭代中i分段拆分出的新分段数，新段内均设置配点数为下限值N_min。完成i段内新分段和配点数设置后，更新上一分段位置α。完整的自适应分段升阶Radau伪谱法的求解示意如图7所示。

图7自适应阈值分段升阶伪谱法求解示意图

Fig.7Schematic diagram of the adaptive threshold segmented upgrading pseudo-spectral method

4 仿真实验

本文以平面二连杆FFSR作为实验对象，应用自适应阈值分段升阶伪谱法规划目标捕获轨迹，并与全局Radau伪谱法、自适应曲率分段Radau伪谱法^[14]的求解结果进行比较。FFSR构型参数如表1所示。

表1FFSR构型系数

Tab.1Configuration parameters of the FFSR

表1中基座的转动惯量相对于基座质心转轴，连杆的转动惯量相对于关节转轴。

捕获任务设定如下。

1）仿真开始时间t₀=0.0 s，结束时间t_f=60.0 s。

2）非合作目标初始位置[3.5，2.0]^T，自转角速度π/60 rad/s，逆时针方向旋转。

3）FFSR初始状态r_b（t₀）=[1.0，2.0]^T，q_b（t₀）=0.0，q_m（t₀）=[π/2，π/2]^T，

{\dot{r}}_{b}

（t₀）=[0.0，0.0]^T，

{\dot{q}}_{b}

（t₀）=0.0，

{\dot{q}}_{m}

（t₀）=[0.0，0.0]^T。

4）FFSR结束状态

r_{b} (t_{f}) = {[\begin{matrix} 4 15 ， 2 . 0 \end{matrix}]}^{T} ， q_{b} (t_{f}) = π ， q_{m} (t_{f}) = [0.8 ， - 1.87]^{T} ， {\dot{r}}_{b} (t_{f}) = [0.0，0.0]^{T} ， {\dot{q}}_{b} (t_{f}) = 0.0 ， {\dot{q}}_{m} (t_{f}) = [0.0，0.0]^{T} 。

5）视场约束目视角 θ_fovMax = 15°，跟随角 θ_rotMax= 25°。

6）目标障碍区圆圆心在目标系中向两侧太阳能板间隔 0.2 m 处设置，中心障碍圆半径为 0.14 m，太阳能板上障碍圆半径为 0.1 m。自身障碍区依据基座外接圆和连杆的四等分点设置。

7）电机输入 U 满足:U≤U_max = [1. 0，1. 0，0.1，1.5，1.5]^T，U≥U_min = [-1. 0，-1. 0，-0.1，-1.5，-1.5]^T。

8）输入权重矩阵 Q_u = diag（ 1，1，300，300，300），边值性能指标 k₁ = 100，积分性能指标 k₂ = 1。

应用本文方法求解上述优化问题，可得轨迹可行解如图8所示。

图8FFSR目标捕获运动可行解

Fig.8Feasible trajectory solution for the FFSR target capture maneuver

图8展示了FFSR的目标捕获过程，红色目标星进行非合作自旋运动，黑色虚线圆为优化问题设置的障碍区，蓝色机器人为平面二连杆FFSR，蓝色虚线代表机器人的末端轨迹。FFSR的末端执行器不直接进行控制，而是由基座和连杆传递运动。由图8可见，FFSR运动曲线平滑，有效遵循了视场、避障等约束，展现出接近-绕飞的过程特征，最终实现目标点位重合，顺利完成捕获任务，验证了改进策略求解的有效性。为进一步体现改进策略的优越性，与全局Radau伪满法、自适应曲率分段Radau伪谱法进行对比实验。不同算法规划FFSR轨迹，运动过程中各状态量与控制律变化如图9所示。

图9中的不同线型代表不同的状态量/控制律，黄色、紫色、绿色线分别代表全局方法、曲率方法和本文提出的阈值方法。由图9可见，基座和关节运动均展现出良好的平滑性，而提出的改进策略倾向于使用更少的配点完成规划任务，因而表现更优。本文方法与全局Radau伪谱法、自适应曲率分段Radau伪谱法的定量对比结果如表2所示。

由表2结果可见，本文提出的阈值法在优化轨迹时使用的配点数最少、求解时间最短，并成功保留了轨迹解的核心变化趋势，展现出最小的末端捕获误差，相比全局法和曲率法更优越。在约束违反方面，本文方法的约束违反量处于10^-13量级，虽略高于曲率方法，但此数量级已可视为数值计算误差的范畴，远低于实际物理系统的容错范围，因此对解的可行性、最优性影响可忽略不计。实际系统在对指令轨迹进行跟踪时，可通过设计控制律抑制微小的约束违反与未知的扰动噪声，该结论已在多个空间机器人轨迹优化研究中得到验证^[16]。

为进一步明确自适应阈值策略的应用方法，本研究对其参数选择原则进行如下阐述:

1）偏差阈值ε，根据系统动力学精度要求设定，通常取10^-5~10^-3数量级，过大易导致优化求解精度降低，过小则使优化问题求解困难;

2）自适应阈值增益λ是自适应分段策略的核心，本研究对其进行了参数分析实验，结果见表3。

表3结果表明，较小的λ值倾向于通过增加段内配点数实现轨迹优化，较大的λ值则更容易越过时间步长限制，采用分段策略优化。选取合适的增益λ，可实现更小的末端误差、约束违反量与更快的计算速度。

3）分段内配点数上限N_max，下限N_min，根据伪谱法多项式拟合经验值设定，通常单段内配点数取3~10之间的整数。配点数过少会倾向于过度分段，引入不必要的边界条件; 配点数过多，则可能导致数值病态（插值矩阵条件数恶化），计算代价也会急剧上升。

图9FFSR运动过程状态量与控制律变化

Fig.9Variation of FFSR states and control laws during motion

表2FFSR轨迹优化求解效果对比

Tab.2Comparison of trajectory optimization performance for the FFSR

表3自适应阈值增益λ实验分析

Tab.3Experimental analysis of adaptive threshold gain λ

4）M₁为光滑区间追求加速收敛增加的配点数。由文献^[17]可知，增加配点可以使轨迹平滑处呈现指数级收敛趋势，因而M1取较小值即可显著加速收敛，通常取1~3之间的整数。M₂为新增分段下限数，本文中取M₂=2，即激活分段策略后，至少将原区间分为两段。

5 结论

1）本文基于FFSR目标捕获任务背景，针对Radau伪谱法求解轨迹优化问题的均衡矛盾，提出了一种基于阈值的自适应分段升阶改进策略。通过在迭代过程中计算偏差矩阵、比较时间阈值，实现了对分段位置、配点数的有效分级决策，为机器人快速规划行动路径提供了支撑。

2）与全局伪谱法不同，阈值法充分发挥了分段策略的优势，避免了单一区间配点过多的问题，从而显著减少计算耗时。与自适应曲率分段伪谱法相比，阈值法采用相对保守的分段策略，最大限度地利用了解曲线在光滑区域的特点，通过提升多项式阶数实现了更快的收敛速度和更小的末端误差。

3）将本文提出的AT-RPM方法应用于平面二连杆FFSR目标捕获任务，仿真结果表明AT-RPM方法相比全局方法和曲率方法，计算耗时缩短至13.19 s，末端误差减小至3 mm，具有最好的性能表现。未来将考虑研究受到外部扰动下的FFSR路径优化问题，进一步提升算法的鲁棒性和适用性。

图1自由飞行空间机器人在轨服务^[1]

Fig.1A free-flying space robot performing on-orbit servicing^[1]

下载: 全尺寸图片

图2自由飞行空间机器人结构示意图

Fig.2Schematic diagram of the free-flying space robot

下载: 全尺寸图片

图3视场约束示意图

Fig.3Illustration of the field-of-view constraint

下载: 全尺寸图片

图4避障约束示意图

Fig.4Illustration of the obstacle avoidance constraint

下载: 全尺寸图片

图5分段伪谱法示意图

Fig.5Illustration of the segmented pseudo-spectral method

下载: 全尺寸图片

图6自适应分段升阶改进策略示意图

Fig.6Illustration of the adaptive segmented upgrading strategy

下载: 全尺寸图片

图7自适应阈值分段升阶伪谱法求解示意图

Fig.7Schematic diagram of the adaptive threshold segmented upgrading pseudo-spectral method

下载: 全尺寸图片

图8FFSR目标捕获运动可行解

Fig.8Feasible trajectory solution for the FFSR target capture maneuver

下载: 全尺寸图片

图9FFSR运动过程状态量与控制律变化

Fig.9Variation of FFSR states and control laws during motion

下载: 全尺寸图片

表1FFSR构型系数

Tab.1Configuration parameters of the FFSR

下载: 全尺寸图片

表2FFSR轨迹优化求解效果对比

Tab.2Comparison of trajectory optimization performance for the FFSR

下载: 全尺寸图片

表3自适应阈值增益λ实验分析

Tab.3Experimental analysis of adaptive threshold gain λ

下载: 全尺寸图片

图1自由飞行空间机器人在轨服务^[1]

Fig.1A free-flying space robot performing on-orbit servicing^[1]

图2自由飞行空间机器人结构示意图

Fig.2Schematic diagram of the free-flying space robot

图3视场约束示意图

Fig.3Illustration of the field-of-view constraint

图4避障约束示意图

Fig.4Illustration of the obstacle avoidance constraint

图5分段伪谱法示意图

Fig.5Illustration of the segmented pseudo-spectral method

图6自适应分段升阶改进策略示意图

Fig.6Illustration of the adaptive segmented upgrading strategy

图7自适应阈值分段升阶伪谱法求解示意图

Fig.7Schematic diagram of the adaptive threshold segmented upgrading pseudo-spectral method

图8FFSR目标捕获运动可行解

Fig.8Feasible trajectory solution for the FFSR target capture maneuver

图9FFSR运动过程状态量与控制律变化

Fig.9Variation of FFSR states and control laws during motion

表1FFSR构型系数

Tab.1Configuration parameters of the FFSR

表2FFSR轨迹优化求解效果对比

Tab.2Comparison of trajectory optimization performance for the FFSR

表3自适应阈值增益λ实验分析

Tab.3Experimental analysis of adaptive threshold gain λ

图1自由飞行空间机器人在轨服务^[1]

Fig.1A free-flying space robot performing on-orbit servicing^[1]

图2自由飞行空间机器人结构示意图

Fig.2Schematic diagram of the free-flying space robot

图3视场约束示意图

Fig.3Illustration of the field-of-view constraint

图4避障约束示意图

Fig.4Illustration of the obstacle avoidance constraint

图5分段伪谱法示意图

Fig.5Illustration of the segmented pseudo-spectral method

图6自适应分段升阶改进策略示意图

Fig.6Illustration of the adaptive segmented upgrading strategy

图7自适应阈值分段升阶伪谱法求解示意图

Fig.7Schematic diagram of the adaptive threshold segmented upgrading pseudo-spectral method

图8FFSR目标捕获运动可行解

Fig.8Feasible trajectory solution for the FFSR target capture maneuver

图9FFSR运动过程状态量与控制律变化

Fig.9Variation of FFSR states and control laws during motion

表1FFSR构型系数

Tab.1Configuration parameters of the FFSR

表2FFSR轨迹优化求解效果对比

Tab.2Comparison of trajectory optimization performance for the FFSR

表3自适应阈值增益λ实验分析

Tab.3Experimental analysis of adaptive threshold gain λ

赵亮亮, 李雪皑, 赵京东, 等. 面向航天器自主维护的空间机器人发展战略研究[J]. 中国工程科学.2024,26(1):149.ZHAO Liangliang, LI Xuekai, ZHAO Jingdong,et al. Development strategy of space robots for autonomous repair and maintenance of spacecraft[J]. Strategic Study of CAE,2024,26(1):149. DOI:10.15302/J-SSCAE-2024.01.014.

GAO Y, CHIEN S. Review on space robotics: Toward top-level science through space exploration[J]. Science Robotics,2017,2(7):eaan5074. DOI:10.1126/scirobotics.aan5074.

闫弘旭. 面向空间抓捕任务的机械臂运动规划算法设计[D]. 成都: 电子科技大学,2024.YAN Hongxu. Motion planning algorithm design for space capture tasks using robotic arms[D]. Chengdu: University of Electronic Science and Technology of China,2024.

AL A A, SHI J, ZHU Z H. Path planning of 6-dof free-floating space robotic manipulators using reinforcement learning[J]. Acta Astronautica,2024,224:367. DOI:10.1016/j.actaastro.2024.08.015.

HONG M, WANG L, LIU L,et al. Trajectory planning of a free-floating dual-arm space robot with minimal base disturbance in obstacle environments[J]. Advances in Space Research,2024,74(3):1410. DOI:10.1016/j.asr.2024.04.050.

TANG Xiaojun, CHEN Jie. Direct trajectory optimization and costate estimation of infinite-horizon optimal control problems using collocation at the flipped legendre-gauss-radau points[J]. IEEE/CAA Journal of Automatica Sinica,2016,3(2):174. DOI:10.1109/JAS.2016.7451105.

崔乃刚, 郭冬子, 李坤原, 等. 飞行器轨迹优化数值解法综述[J]. 战术导弹技术,2020(5):37.CUI Naigang, GUO Dongzi, LI Kunyuan,et al. A review of numerical methods for flight vehicle trajectory optimization[J]. Tactical Missile Technology,2020(5):37. DOI:10.16358/j.issn.1009-1300.2020.1.536.

ADU C E, CHUQUIURE C E R, ZHANG B,et al. Bring the heat: Rapid trajectory optimization with pseudospectral techniques and the affine geometric heat flow equation[J]. IEEE Robotics and Automation Letters,2025,10(4):4148. DOI:10.1109/LRA.2025.3547299.

梅杰, 马广富, 杨博. 基于Legendre伪谱法的卫星轨道转移燃料最优控制[J]. 哈尔滨工业大学学报,2010,42(3):352.MEI Jie, MA Guangfu, YANG Bo. Optimal fuel control for satellite orbit transfer based on legendre pseudospectral method[J]. Journal of Harbin Institute of Technology,2010,42(3):352. DOI:10.11918/j.issn.0367-6234.2010.03.004.

REDDIEN G W. Collocation at gauss points as a discretization in optimal control[J]. SIAM Journal on Control and Optimization,1979,17(2):298. DOI:10.1137/031702.

GARG D, PATTERSON M A, FRANCOLIN C,et al. Direct trajectory optimization and costate estimation of finite-horizon and infinite-horizon optimal control problems using a radau pseudospectral method[J]. Computational Optimization and Applications,2011,49(2):335. DOI:10.1007/s10589-009-9291-0.

LI Shi, DUAN Guangren. A pseudospectral method for trajectory optimization of free-floating space manipulator[C]//Proceedings of the 31st Chinese Control Conference. Heifei, China: IEEE,2012:2395.

ZHANG Ouyang, YAO Weiran, DU Desong,et al. Trajectory optimization and tracking control of free-flying space robots for capturing non-cooperative tumbling objects[J]. Aerospace Science and Technology,2023,143:108718. DOI:10.1016/j.ast.2023.108718.

仲小清, 邵翔宇, 许林杨, 等. 自适应Radau伪谱法自由漂浮空间机器人轨迹规划[J]. 哈尔滨工业大学学报,2018,50(4):49.ZHONG Xiaoqing, SHAO Xiangyu, XU Linyang,et al. Trajectory planning for free-floating space robots using adaptive radau pseudospectral method[J]. Journal of Harbin Institute of Technology,2018,50(4):49. DOI:10.11918/j.issn.0367-6234.201708114.

ZHANG O, LIU Z, SHAO X,et al. Learning-based task space trajectory planning framework with preplanning and postprocessing for uncertain free-floating space robots[J]. IEEE Transactions on Aerospace and Electronic Systems,2025,61(3):6325. DOI:10.1109/TAES.2025.3527428.

SHAO X, YAO W, LI X,et al. Direct trajectory optimization of free-floating space manipulator for reducing spacecraft variation[J]. IEEE Robotics and Automation Letters,2022,7(2):2795. DOI:10.1109/LRA.2022.3143586.

DARBY C L, HAGER W W, RAO A V. An hp-adaptive pseudospectral method for solving optimal control problems[J]. Optimal Control Applications and Methods,2011,32(4):476. DOI:10.1002/oca.957.

出版声明

期刊订阅

1 FFSR系统建模

2 轨迹规划任务分析

3 Radau伪谱法改进

4 仿真实验

5 结论