CN106054876B - 一种针对空间多任务的避障路径最优逐次操作规划方法 - Google Patents

Abstract

本发明公开了一种针对空间多任务的避障路径最优逐次操作规划方法，通过寻找路径最优任务序列，然后寻找相邻任务点之间的最短避障路径，最后寻找对本体姿态扰动最小的各关节运动，实现空间多任务的路径最优逐次操作的规划；本发明利用自由漂浮空间机器人系统的运动学方程，借助混合粒子群优化算法，对空间多项任务进行路径最优的逐次操作规划，算法简单、计算量较少、且运行结果精度较高。本发明适用于空间操作系统连续执行多项任务，节省时间、节约燃料，进而有助于提高航天器寿命，为空间操作系统连续执行多项任务问题提供了一种新的思路。下面结合附图和实例对本发明进一步说明。

Description

一种针对空间多任务的避障路径最优逐次操作规划方法

【技术领域】

本发明属于航天器在轨服务技术领域，涉及一种针对空间多任务的避障路径最优逐次操作规划方法。

【背景技术】

近年来，随着空间技术的发展，空机器人被广泛应用于在轨服务中，如在轨维修、燃料补给、清理碎片等。进行离线操作规划是执行任务的先决条件，文献“Liu X,Baoyin Hand Ma X.Optimal path planning of redundant free-floating revolute-jointedspace manipulators with seven links”利用粒子群优化算法分析了空间机器人的任务操作规划问题，约束条件是规划过程中实现对本体姿态的扰动最小化。文献“Wang M,Luo Jand Walter U.Trajectory planning of free-floating space robot using ParticleSwarm Optimization”借助四元数考虑了不同约束条件下的任务操作规划问题，如最小化对本体姿态的扰动、最大化系统的可操作性，或两者均包括。但以上文献主要是根据空间机器人和任务点的位姿进行点到点的操作规划，即实现在轨服务的基本途径是利用空间机器人执行单一任务。文献“王俊龙,张国良,羊帆.改进人工势场法的机械臂避障路径规划”利用改进的人工势场法研究了机器人避障路径规划问题。工业上，为提高生产效率，常利用机器人代替人类进行生产线工作。较为典型的是利用机器人连续完成多项任务。文献“Alatartsev S,Stellmacher S,Ortmeier F.Robotic task sequencing problem:Asurvey”综述了近年来地面机器人连续完成多项任务的操作规划问题进展。根据综述，多数文献只考虑了多任务的序列问题。少数文献将机器人的各关节运动情况考虑进来，但机器人大多只有二或三个关节(至多不超过六个)，且由于是地面机器人，其本体是固定的。

由以上可知，目前对机器人任务路径规划问题的研究主要分为两类：外太空单一任务路径规划问题和地面多任务路径规划问题。但各有不足，在外太空环境中燃料十分宝贵，只执行单一任务提高了燃料成本，大大降低了航天器的寿命。地面虽考虑了执行多任务情况，但由于地面和外太空的环境差别很大，且操作器系统种类不同，地面操作器系统本体是固定的且关节自由度非冗余，空间机器人系统(此处空间操作器系主要包括两部分：操作系统和本体)的本体处于自由漂浮状态且关节自由度冗余。此外，文献中提到的避障方法比较复杂。为节省时间、节约燃料，利用带有冗余自由度的自由漂浮空间机器人系统执行空间多项操作任务，进行相应的多任务避障路径规划是十分有必要的。

近年来，在轨服务任务日益增多，借助自由漂浮空间机器人系统连续完成多项任务有助于节省时间、节约燃料，进而提高航天器寿命，必然成为一种趋势。本发明主要用于解决如下问题：根据各任务的位置和姿态，利用带有冗余度的自由漂浮空间机器人系统逐次执行各任务，成功避开各相邻任务间的不规则障碍物，执行完任务后回到初始任务位置，整个过程中末端执行器避障路径最优，且操作系统对本体姿态扰动为零。

【发明内容】

本发明的目的在于克服上述现有技术的缺点，提供一种

为节约时间、节省燃料，利用自由漂浮空间机器人系统连续执行多项任务，本发明提出了一种针对空间多任务的路径最优逐次操作规划方法，首先得到路径最短的最优任务序列，然后给出相邻两任务间的路径最短避障任务操作规划，最后基于操作系统末端执行器的轨迹及自由漂浮空间机器人系统的运动学方程给出系统各关节的变化情况，约束条件是实现对本体零姿态扰动。

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

一种针对空间多任务的避障路径最优逐次操作规划方法，包括以下步骤：

步骤一、寻找路径最优任务序列

根据各任务点的位置，寻找一条最优路径；即根据加权图G＝(C,L)，寻找一条花费最小的哈密顿圈；其中，C＝{c₁,...,c_n}为顶点集，即各任务点的位置，此处指三维坐标系下的坐标，单位为m，L＝{l_ij|c_i,c_j∈C}为边集；D＝{d_ij|c_i,c_j∈C}为加权集，此处指相邻任务位置点之间的距离，单位为m；若T＝{t₁,...,t_n+1}为可行任务序列，其中，t_i∈C,i＝1,...,n,t_n+1＝t₁，则寻找距离最短的最优任务序列模型为：

采用混合粒子群优化算法HPSO对寻找路径最短的任务点序列进行求解；粒子群优化算法通过模拟鸟群捕食行为来寻找全局最优解，根据如下公式更新每个粒子的速度和位置：

其中k表示第k次迭代，表示第k次迭代得到的第i个粒子的位置，表示第k次迭代得到的第i个粒子的速度，表示经k次迭代后得到的第i个粒子的最好位置，表示经k次迭代后得到的群体所经历的最好位置；w为惯性因子，c₁和c₂为学习因子，η₁和η₂为[0,1]范围内的均匀随机数；

步骤二、寻找相邻任务点之间的最短避障路径

若两相邻任务之间存在不规则障碍物，则首先用最优尺寸的方体逼近该障碍物，然后寻找相邻任务点之间距离最短的避障路径，具体步骤如下：

2-1)用最优尺寸的方体逼近该不规则障碍物；

2-2)旋转两任务点，使旋转后的新点分别位于方体的上、下、左和右四个侧面内；

2-3)用直线连接旋转后得到的两新点，共得到上、下、左、右四个平面内的四条直线，分别与相对应的平面内的方形(逼近障碍物的最优方体在四个平面内的投影)有两个交点；

2-4)用直线依次连接原任务点、步骤2-3)中得到的与方形的两个交点，即得到一条相邻任务间的避障路径；计算得到的四条路径，选择距离最短的一条，即为两相邻任务点之间的最短避障路径；

步骤三、寻找对本体姿态扰动最小的各关节运动

通过自由漂浮空间机器人系统的运动学方程得到各关节角变化情况，其运动学方程表示如下：

其中v_e和ω_e分别表示操作系统末端执行器的线速度和角速度，单位分别为m/s和deg/s；v_b和ω_b分别表示操作系统本体的线速度和角速度，单位分别为m/s和deg/s；φ表示操作系统的关节角向量，单位为deg；J_g表示广义雅可比矩阵；对公式(3)求伪逆，然后进行积分，得到各关节角；此外，在求解各关节角过程中，需要实时保证运动对本体的姿态扰动为零。

本发明进一步的改进在于：

所述步骤一中，用混合粒子群优化算法HPSO的具体步骤如下：

1-1)初始化粒子群，包括粒子个数、最大迭代次数和初始解；

1-2)根据上一次迭代得到的每个粒子的位置、局部最优位置和全局最优位置，计算该次该粒子的局部最优位置，其中包括交叉和变异；

1-3)计算全局最优位置和全局最优值，若满足条件，则输出最优适应度值，否则返回步骤1-2)。

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

本发明利用自由漂浮空间机器人系统的运动学方程，借助混合粒子群优化算法，对空间多项任务进行路径最优的逐次操作规划，算法简单、计算量较少、且运行结果精度较高。本发明适用于空间操作系统连续执行多项任务，节省时间、节约燃料，进而有助于提高航天器寿命，为空间操作系统连续执行多项任务问题提供了一种新的思路。下面结合附图和实例对本发明进一步说明。

【附图说明】

图1为三维空间中操作系统的路径最短运动轨迹；

图2为任务点B和D之间障碍物附近的路径；

图3为任务点A和C之间障碍物附近的路径；

图4为任务点B和D之间障碍物附近操作系统机械臂构型；

图5为三维空间中操作系统末端执行器离线规划路径与实际路径；

图6为末端执行器离线规划路径与实际路径在二维平面内随时间变化的情况；

图7为末端执行器离线规划姿态与实际姿态；

图8为操作系统各关节角变化情况；

图9为系统本体姿态变化情况。

【具体实施方式】

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

参见图1-图9，本发明针对空间多任务的避障路径最优逐次操作规划方法，包括以下步骤：

步骤一、寻找路径最优任务序列

根据各任务点的位置，寻找一条最优路径。即根据加权图G＝(C,L)，寻找一条花费最小的哈密顿圈。其中，C＝{c₁,...,c_n}为顶点集，即各任务点的位置(此处指三维坐标系下的坐标，单位为m)，L＝{l_ij|c_i,c_j∈C}为边集。D＝{d_ij|c_i,c_j∈C}为加权集，此处指相邻任务位置点之间的距离，单位为m。若T＝{t₁,...,t_n+1}(t_i∈C,i＝1,...,n,t_n+1＝t₁)为可行任务序列，则寻找距离最短的最优任务序列模型为：

寻找路径最短的任务点序列是一个典型的NP问题，此处采用混合粒子群优化算法(HPSO)对该问题进行求解。粒子群优化算法(PSO)通过模拟鸟群捕食行为来寻找全局最优解，根据如下公式更新每个粒子的速度和位置：

其中k表示第k次迭代，表示第k次迭代得到的第i个粒子的位置，表示第k次迭代得到的第i个粒子的速度，表示经k次迭代后得到的第i个粒子的最好位置，表示经k次迭代后得到的群体所经历的最好位置。w为惯性因子，c₁和c₂为学习因子，η₁和η₂为[0,1]范围内的均匀随机数。

HPSO结合了PSO和遗传算法(GA)的优点，用HPSO解决该问题的具体步骤如下：

1)初始化粒子群，包括粒子个数、最大迭代次数和初始解；

2)根据上一次迭代得到的每个粒子的位置、局部最优位置和全局最优位置，计算该次该粒子的局部最优位置，其中包括交叉和变异；

3)计算全局最优位置和全局最优值(即最优适应度值)，若满足条件，则输出最优适应度值，否则返回步骤2)。

步骤二、寻找相邻任务点之间的最短避障路径

若两相邻任务之间存在不规则障碍物，则首先用最优尺寸的方体逼近该障碍物，然后寻找相邻任务点之间距离最短的避障路径，具体步骤如下：

1)用最优尺寸的方体逼近该不规则障碍物；

2)旋转两任务点，使旋转后的新点分别位于方体的上、下、左和右四个侧面内；

3)用直线连接旋转后得到的两新点，共得到上、下、左、右四个平面内的四条直线，分别与相对应的平面内的方形(逼近障碍物的最优方体在四个平面内的投影)有两个交点；

4)用直线依次连接原任务点、3)中得到的与方形的两个交点，即得到一条相邻任务间的避障路径。计算得到的四条路径，选择距离最短的一条，即为两相邻任务点之间的最短避障路径。

步骤三、寻找对本体姿态扰动最小的各关节运动

主要通过自由漂浮空间机器人系统的运动学方程得到各关节角变化情况，其运动学方程表示如下：

其中v_e和ω_e分别表示操作系统末端执行器的线速度和角速度(单位分别为m/s和deg/s)，v_b和ω_b分别表示操作系统本体的线速度和角速度(单位分别为m/s和deg/s)，φ表示操作系统的关节角向量(单位为deg)，J_g表示广义雅可比矩阵。对式(3)求伪逆，然后进行积分，可得各关节角。此外，在求解各关节角过程中，需要实时保证运动对本体的姿态扰动为零。

实施例：

要求自由漂浮空间机器人系统(物理参数见表1)末端执行器经过四任务点(位置和姿态数据见表2)，相邻任务点A和C、B和D之间均之间存在不规则障碍物，包围不规则障碍物的最优方体尺寸见表3，操作系统需避开障碍物并逐次执行各任务，执行完任务后回到初始任务点处，要求末端执行器的逐次操作路径最优，且对系统本体的姿态扰动为零。

表1四项任务点的位置和姿态

表2包围任务点间不规则障碍物的最优方体尺寸

表3自由漂浮空间机器人系统物理参数

图1至9为操作系统执行任务的路径最优逐次操作规划示意图。由图1可得最优路径有两条：A→C→D→B→A和A→B→D→C→A，以第一条路径为例进行分析。图1描述了操作系统末端执行器的离线规划轨迹及各关节的运动情况；图2、3和4为两障碍物附近路径和相关关节运动变化的放大图，可知操作系统成功地避开了障碍物；图5和6分别描述了操作系统末端执行器在三维空间和相对应的二维平面中的连线规划路径和实际路径；图7描述了操作系统末端执行器的离线规划姿态和实际姿态变化情况；图8描述了各关节角的变化情况，可知变化比较平稳；图9描述了操作系统执行任务过程中对本体的姿态扰动情况，在10^-6(deg)量级，表明规划出的路径对本体姿态扰动比接近于零。此外，由图5、图6和图7可知操作系统成功地逐次达到个任务点的位置和姿态。

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

Claims (1)

1.一种针对空间多任务的避障路径最优逐次操作规划方法，其特征在于，包括以下步骤：

步骤一、寻找路径最优任务序列

根据各任务点的位置，寻找一条最优路径；即根据加权图G＝(C,L)，寻找一条花费最小的哈密顿圈；其中，C＝{c₁,...,c_n}为顶点集，即各任务点的位置，此处指三维坐标系下的坐标，单位为m，L＝{l_ij|c_i,c_j∈C}为边集；D＝{d_ij|c_i,c_j∈C}为加权集，此处指相邻任务位置点之间的距离，单位为m；若T＝{t₁,...,t_n+1}为可行任务序列，其中，t_i∈C,i＝1,...,n,t_n+1＝t₁，则寻找距离最短的最优任务序列模型为：

采用混合粒子群优化算法HPSO对寻找路径最短的任务点序列进行求解；粒子群优化算法通过模拟鸟群捕食行为来寻找全局最优解，根据如下公式更新每个粒子的速度和位置：

其中k表示第k次迭代，表示第k次迭代得到的第i个粒子的位置，表示第k次迭代得到的第i个粒子的速度，表示经k次迭代后得到的第i个粒子的最好位置，表示经k次迭代后得到的群体所经历的最好位置；w为惯性因子，c1和c₂为学习因子，η₁和η₂为[0,1]范围内的均匀随机数；

用混合粒子群优化算法HPSO的具体步骤如下：

1-1)初始化粒子群，包括粒子个数、最大迭代次数和初始解；

1-2)根据上一次迭代得到的每个粒子的位置、局部最优位置和全局最优位置，计算该次该粒子的局部最优位置，其中包括交叉和变异；

1-3)计算全局最优位置和全局最优值，若满足条件，则输出最优适应度值，否则返回步骤1-2)；

步骤二、寻找相邻任务点之间的最短避障路径

若两相邻任务之间存在不规则障碍物，则首先用最优尺寸的方体逼近该障碍物，然后寻找相邻任务点之间距离最短的避障路径，具体步骤如下：

2-1)用最优尺寸的方体逼近该不规则障碍物；

2-2)旋转两任务点，使旋转后的新点分别位于方体的上、下、左和右四个侧面内；

2-3)用直线连接旋转后得到的两新点，共得到上、下、左、右四个平面内的四条直线，分别与相对应的平面内的方形有两个交点，所述方形为逼近障碍物的最优方体在四个平面内的投影；

2-4)用直线依次连接原任务点、步骤2-3)中得到的与方形的两个交点，即得到一条相邻任务间的避障路径；计算得到的四条路径，选择距离最短的一条，即为两相邻任务点之间的最短避障路径；

步骤三、寻找对本体姿态扰动最小的各关节运动

通过自由漂浮空间机器人系统的运动学方程得到各关节角变化情况，其运动学方程表示如下：

其中v_e和ω_e分别表示操作系统末端执行器的线速度和角速度，单位分别为m/s和deg/s；v_b和ω_b分别表示操作系统本体的线速度和角速度，单位分别为m/s和deg/s；φ表示操作系统的关节角向量，单位为deg；J_g表示广义雅可比矩阵；对公式(3)求伪逆，然后进行积分，得到各关节角；此外，在求解各关节角过程中，需要实时保证运动对本体的姿态扰动为零。