CN113664829A - 一种空间机械臂避障路径规划系统、方法、计算机设备及存储介质 - Google Patents

Abstract

本发明属于空间机械臂技术领域，公开了一种空间机械臂避障路径规划方法，包括以下步骤：步骤1：设计用于空间机械臂避障的强化学习算法；步骤2：将每节机械臂都视为智能体，用强化学习算法对各节机械臂进行离线训练，获取各个智能体的状态‑动作值函数表；步骤3：空间机械臂在作业过程中，将各个关节角变化转换为利用逆运动学求出的关节角度值；当空间机械臂靠近障碍物时，相应的臂杆进入到危险状态，这时智能体自动根据状态‑动作值函数表选择相应的动作对障碍物进行避障。使用该方法能使空间机械臂在对固定目标进行跟踪的同时，实现对固定或移动障碍物的有效避障，具有在复杂动态环境下较强的适应性和决策能力。

Description

一种空间机械臂避障路径规划系统、方法、计算机设备及存储 介质

技术领域

本发明属于空间机械臂技术领域，特别涉及一种空间机械臂避障路径规划系统、方法、计算机设备及存储介质。

背景技术

随着人类对地外空间的不断探索，大量的航天任务不断出现，在轨服务技术得到了快速的发展。而空间机器人对于实现在轨服务，如对航天器的在轨维修，空间碎片的在轨捕获，空间站的在轨建造与维护等任务有着重要作用。为此各国研究人员对空间机器人进行了深入的研究，空间机器人根据执行机构的数目分为单臂和多臂空间机器人，本发明的研究对象为单臂空间机器人。空间机械臂固联在空间机器人上，是空间机器人的执行机构，空间机器人的大部分操作任务都由空间机械臂完成。空间机械臂具有冗余的自由度，这使得它在对目标进行捕获的同时可以对障碍物进行躲避，与其他执行机构相比也可以实现更精细的操作，这使得它具有广阔的应用前景。很多专家对空间机械臂的避障路径规划方面做了深入的研究，并取得了丰硕的成果。

姜力在文献《七自由度冗余机械臂避障控制》中提出了一种基于冗余机械手的零空间运动方程的七自由度在轨自保持冗余机械手逆动力学避障控制方法，该方法可以在确保机械臂末端执行器有效地跟踪最终执行轨迹的同时实现避障。贾庆轩在文献《基于A～*算法的空间机械臂避障路径规划》中使用A*启发式搜索算法来实现空间机械臂的无碰撞路径规划。祁若龙在文献《一种基于遗传算法的空间机械臂避障轨迹规划方法》中通过建立理想轨迹的分段描述方程，将空间机械臂轨迹规划问题转变为一个多目标优化求解问题，并通过加权系数法建立遗传算法适应度评定函数，最后利用遗传算法在关节空间下规划出一条无碰撞理想轨迹。以上的方法都属于传统方法，通常针对特定的目标和障碍对轨迹进行最优规划，但更复杂的环境将会增加计算成本并大幅降低鲁棒性，进一步增加轨迹规划成功的难度，甚至使任务失败。

发明内容

本发明的目的在于提供一种空间机械臂避障路径规划方法，解决传统避障方法在复杂环境中计算成本大、鲁棒性差的问题。

本发明是通过以下技术方案来实现：

一种空间机械臂避障路径规划方法，包括以下步骤：

步骤1：设计用于空间机械臂避障的强化学习算法；

步骤2：将每节机械臂都视为智能体，用强化学习算法对各节机械臂进行离线训练，获取各个智能体的状态-动作值函数表；

步骤3：空间机械臂在作业过程中，将各个关节角变化转换为利用逆运动学求出的关节角度值；

当空间机械臂靠近障碍物时，相应的臂杆进入到危险状态，这时智能体自动根据状态-动作值函数表选择相应的动作对障碍物进行避障。

进一步，强化学习算法采用Q学习算法。

进一步，步骤1具体为：

1.1、设计智能体的状态值与动作值，根据不同的状态智能体采取不同的动作；

1.2、设计智能体选择动作策略，即在当前状态下选取能使得动作价值函数值最大的动作；

1.3、设计奖惩函数来对选取动作后智能体所处的状态进行评价，进而对动作价值函数值进行更新。

进一步，步骤3中，将各个关节角变化转换为利用逆运动学求出的关节角度值，具体为：

以3连杆6自由度空间机械臂为研究对象，三段连杆的臂长分别设为l₁、l₂、l₃，机械臂的六个关节都是旋转副，分别称为第一关节、第二关节、第三关节、第四关节、第五关节及第六关节；第一关节的关节角为θ₁，第二关节的关节角为θ₂，第三关节的关节角为θ₃，第四关节的关节角为θ₄，第五关节的关节角为θ₅，第六关节的关节角为θ₆；

第一关节的变换矩阵为

第二关节的变换矩阵为

第三关节的变换矩阵为

第四关节的变换矩阵为

第五关节的变换矩阵为

第六关节的变换矩阵为

将六个关节的变换矩阵依次相乘，得到空间机械臂的运动学方程：

用逆变换

左乘运动学方程(1)求解θ₁和θ₃：

用逆变换

左乘运动学方程(1)求解θ₂和θ₄：

θ₂＝θ₂₃-θ₃ (4)

其中，

θ₄＝Atan2(-a_xsinθ₁+a_ycosθ₁，-a_xcosθ₁cosθ₂₃-a_ysinθ1 cosθ₂₃+a_zsinθ₂₃) (5)

用逆变换

左乘运动学方程(1)求解θ₅：

θ₅＝Atan2(sθ₅，cθ₅) (6)

用逆变换

左乘运动学方程(1)求解θ₆：

θ₆＝Atan2(sθ₆，cθ₆) (7)。

进一步，步骤2具体为：

S1、首先对智能体参数进行初始化；

S2、设置随机障碍物位置与机械臂各关节角初始值；

S3、智能体感知状态s(0)，依据公式(9)选择动作a(0)并执行，智能体继续感知状态s(1)，K++，根据奖惩函数反馈奖励R，根据公式(8)更新动作价值函数Q(s，a)；

S4、判断智能体是否到达安全环境，若否，则返回S3；若是，则I++，K＝0；I为训练周期数、K为试探次数；

S5、然后判断I是否到达上限，若否，则返回S3；若是则流程结束。

进一步，步骤3中，利用状态-动作值函数表进行避障的具体过程为：

S3.1、设置空间机械臂各关节角初始值；

S3.2、判断θ_i是否等于θ_i’，若等于，则θ_i不变，各智能体感知状态s；若θ_i小于θ_i’，则θ_i＝θ_i’+0.1，若θ_i大于θ_i’，则θ_i＝θ_i’-0.1，各智能体感知状态s；

θ_i为机械臂关节角当前位置，θ_i’为机械臂关节角期望位置；

S3.3、判断智能体是否在危险区，若不在危险区，则智能体不执行工作，判断空间机械臂捕获到固定目标或与障碍物相撞；若在危险区，则智能体根据状态-动作函数表选择当前状态下，状态-动作值最高的动作a并执行，判断空间机械臂捕获到固定目标或与障碍物相撞；

S3.4、若空间机械臂捕获到固定目标或与障碍物相撞，该流程结束；若空间机械臂没有捕获到固定目标或与没有障碍物相撞，则返回S3.2循环往复，直至空间机械臂捕获到固定目标或与障碍物相撞。

本发明还公开了一种空间机械臂避障路径规划系统，包括关节角度计算模块、训练模块、避障模块；

关节角度计算模块，用于利用逆运动学求出空间机械臂末端到达目标点所需的各个关节角度值；

强化学习模块，用于负载空间机械臂避障的强化学习算法；

训练模块，用于对各节机械臂进行离线训练，获取各个智能体的状态-动作值函数表；

避障模块，用于在空间机械臂在作业过程中，当空间机械臂靠近障碍物时，相应的臂杆进入到危险状态，这时智能体自动根据状态-动作值函数表选择相应的动作对障碍物进行避障。

本发明还公开了一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现所述空间机械臂避障路径规划方法的步骤。

本发明还公开了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现所述空间机械臂避障路径规划方法的步骤。

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

本发明公开了一种有效的空间机械臂避障路径规划方法，采用强化学习来解决空间机械臂作业过程中的避障问题，强化学习是一种应用广泛的机器学习方法。强化学习的重点在于对智能体的训练，在强化学习中，智能体(agent)通过与所在环境的不断交互来进行学习，智能体根据所处环境选择动作，而做出的动作又对环境造成影响并通过设计奖惩函数来对该影响进行评价，引导智能体向着预想的方向进行学习，而智能体的目的就是学习最优策略来最优化状态-动作值函数。强化学习无需环境模型与先验知识，只需智能体不断与环境交互就能完成学习，具有很强的鲁棒性与智能性。使用该方法能使空间机械臂在对固定目标进行跟踪的同时，实现对固定或移动障碍物的有效避障，具有在复杂动态环境下较强的适应性和决策能力。

附图说明

图1为本发明所使用的空间机械臂示意图；

图2为本发明设计智能体状态时所使用的椭圆形障碍距离程度判据示意图；

图3为用强化学习算法对各节机械臂进行离线训练的算法流程图；

图4为机械臂第一节臂杆进行离线训练一定周期后，其运动过程图；

图5为空间机械臂作业时利用状态-动作值函数表进行避障的算法流程图；

图6为利用本发明提出的空间机械臂避障路径规划方法对固定障碍实现的空间机械臂避障路径规划；

图7为利用本发明提出的空间机械臂避障路径规划方法对移动障碍实现的空间机械臂避障路径规划。

具体实施方式

下面结合具体的实施例对本发明做进一步的详细说明，所述是对本发明的解释而不是限定。

本发明提供了一种基于强化学习的空间机械臂避障规划方法，用以解决空间机械臂在作业过程中的避障问题，该方法分为如下几个步骤：

1)利用逆运动学对无障碍条件下的空间机械臂进行轨迹规划

本发明以图1所示的3连杆6自由度空间机械臂进行研究，机械臂臂长分别设为l₁,l₂,l₃，每个坐标系代表一个关节角，z轴为关节角的旋转轴。由于工作过程中基座固定，因此不需要考虑基座自由漂浮情况下机械臂的动力学特性。

在工程应用中，逆运动学十分重要，这是机械臂运动规划的基础之一。给定空间机械臂末端位置及姿态，使用逆运动学即可求出相应的关节角。捕获目标时，空间机械臂末端位置即为目标位置。故空间机械臂在作业过程中，只需要将各个关节角变化为利用逆运动学求出的关节角度值，即可捕获到目标。

机械臂的六个关节都是旋转副，根据相应的DH参数表，可算出各个连杆变换矩阵：

将上述连杆变换矩阵依次相乘，可得到空间机械臂的运动学方程：

用逆变换

左乘运动学方程(1)求解θ₁和θ₃：

用逆变换

左乘运动学方程(1)求解θ₂和θ₄：

θ₂＝θ₂₃-θ₃ (4)

其中，

θ₄＝Atan2(-a_xsinθ₁+a_ycosθ₁,-a_xcosθ₁cosθ₂₃-a_ysinθ₁cosθ₂₃+a_zsinθ₂₃) (5)

用逆变换

左乘运动学方程(1)求解θ₅：

θ₅＝Atan2(sθ₅，cθ₅) (6)

用逆变换

左乘运动学方程(1)求解θ₆：

θ₆＝Atan2(sθ₆，cθ₆) (7)

2)设计用于空间机械臂避障的强化学习算法

在强化学习所有思想中，时序差分学习是最核心的思想，时序差分学习结合了蒙特卡洛方法和动态规划方法的思想，可以直接从环境互动的经验中学习策略，而不需要构建关于环境动态特性的模型。而离轨策略下的时序差分控制算法的提出是强化学习的一个重要突破，这一算法由Watkins提出，被称为Q学习算法，其基本形式为：

Q(S_t，A_t)←Q(S_t，A_t)+α[R_t+1+γmax_aQ(S_t+1，a)-Q(S_t，A_t))] (8)

式中，Q(S_t，A_t)为待学习的动作价值函数，α是步长，为常数值，γ为折扣率，R_t+1为状态转移奖赏，S_t为智能体所处状态，A_t为智能体在该状态下选择的动作。

如图3所示，首先初始化Q值，智能体在s状态根据一定的策略π选择动作a，随后得到新的状态值s′和奖励R，得到经验知识和训练例＜s，a，s′，R＞，根据此经验知识用上式(8)修改Q值，当智能体达到目标，循环终止，算法继续从初始状态开始新的迭代循环，直到学习结束。

在一定条件下Q学习算法只需采用贪心策略就可保证收敛，因此Q学习是目前最有效的模型无关强化学习算法，只需要所有的“状态-动作”二元组可以持续更新，整个学习过程就能够正确地收敛。

本发明采用ε-贪心策略来选择智能体的动作，公式如下：

将每节机械臂都视为智能体，每节智能体的动作分别由转角θ₂，θ₃，θ₅的变化来实现。障碍物O与臂杆的距离应考虑到整条线段的长度，因此本发明考虑将障碍物与各节机械臂的距离用椭圆形状的障碍距离程度评价。图2为本发明设计智能体状态时所使用的椭圆形障碍距离程度判据示意图，机械臂两端为椭圆焦点，当障碍物O于运动过程中位于同一个椭圆上时，其对该节臂杆的危险程度视为相等。

故各个臂杆所处环境的障碍距离由下式所示：

图2中，大写的O可以理解为障碍物，小写的o、a、b、c可以理解为机械臂上的关节点，该式即为各节机械臂在所处环境的状态值，本发明为有限马尔可夫环境，为了避免维数灾难，状态和动作的集合只有有限个元素，故要将状态值离散化。根据障碍物与机械臂的距离和方位，将状态划分为六段，如表1所示：

表1环境状态划分表

每次智能体选择动作进行状态转移后，都会有奖惩函数对该行为做出评价，并更新动作价值函数Q(s，a),为了解决避障任务，使用人工势场对障碍物设置排斥势，空间机械臂进入靠近障碍物的危险区时会获得惩罚，惩罚大小反比于障碍物到该臂的距离平方，当接触到障碍物时会获得更高的惩罚，奖惩函数如下所示：

式中，l_danger为危险区距离，l_touch为接触距离，d_{i min}为当前状态值,c₁,c₂为奖惩系数。

3)用强化学习算法对各节机械臂进行离线训练，获取各个智能体的状态-动作值函数表

图3为用强化学习算法对各节机械臂进行离线训练的算法流程图，利用该流程图对空间机械臂的各节臂杆进行离线训练，获取相应的状态-动作值函数表。具体过程为：

S1、首先对智能体参数进行初始化，I＝0，K＝0；I为训练周期数、K为试探次数；

S2、设置随机障碍物位置与机械臂各关节角初始值；

S3、智能体感知状态s(0)，依据公式(9)选择动作a(0)并执行，智能体继续感知状态s(1)，K++，根据奖惩函数反馈奖励R，根据公式(8)更新动作价值函数Q(s，a)；

S4、判断智能体是否到达安全环境，若否，则返回S3；若是，则I++，K＝0；

S5、然后判断I是否到达上限，若否，则返回S3；若是则流程结束。

本发明将空间机械臂的每一节都视为智能体，故每节机械臂都需要进行离线训练。要想完成对单节机械臂的离线避障训练，本发明选择的方法是不断在这节机械臂周围施加障碍物来构造避障环境，通过大量的练习训练智能体避障。

障碍物对单节机械臂的距离要覆盖智能体的各个状态，并用ε-贪心策略选择动作a，只要时间足够长，就可以使不同状态下的每一个动作都无限次被采样。这样就可以使所有的“状态-动作”二元组可以持续更新，整个学习过程就能够正确地收敛，从而确保动作价值函数Q(S，A)收敛到动作价值函数最优值q_*，并获得各个智能体的状态-动作值函数表。

图4为机械臂第一节臂杆进行离线训练一定周期后，该智能体的运动过程。机械臂初始状态与y轴重合，开始时在其周围近距离设置一个障碍物，智能体与环境交互后选择动作，直到远离障碍物。从第一幅图到第四幅图关节角θ₂逐渐增大，机械臂缓慢向下转动，直到远离障碍物。

4)利用逆运动学和训练好的状态-动作值函数表解决空间机械臂的避障路径规划问题

利用逆运动学来解决空间机械臂对固定目标作业的路径规划问题：只要获取给定空间机械臂末端位置及姿态，使用逆运动学即可求出相应的关节角。捕获目标时，空间机械臂末端位置即为目标位置。故空间机械臂在作业过程中，只需要将各个关节角变化为利用逆运动学求出的关节角度值，即可捕获到目标。

利用强化学习算法训练好的状态-动作值函数表来解决空间机械臂作业过程中的避障问题，在空间机械臂运动的过程中，当空间机械臂靠近固定或移动障碍物时，相应的臂杆会进入到危险状态，这时智能体会自动根据状态-动作值函数表选择相应的动作对障碍物进行避障，保证空间机械臂顺利完成任务。

图5为空间机械臂作业时利用状态-动作值函数表进行避障的算法流程图，利用该流程图，可以使空间机械臂在对固定目标进行作业的同时具有对固定或移动障碍物进行规避的能力。具体的过程为：

S1、设置空间机械臂各关节角初始值；

S2、判断θ_i是否等于θ_i’，若等于，则θ_i不变，各智能体感知状态s；若θ_i小于θ_i’，则θ_i＝θ_i’+0.1，若θ_i大于θ_i’，则θ_i＝θ_i’-0.1，各智能体感知状态s；θ_i为机械臂关节角当前位置，θ_i’为机械臂关节角期望位置；

S3、判断智能体是否在危险区，若不在危险区，则智能体不执行工作，判断空间机械臂捕获到固定目标或与障碍物相撞；若在危险区，则智能体根据状态-动作函数表选择当前状态下，状态-动作值最高的动作a并执行，判断空间机械臂捕获到固定目标或与障碍物相撞；

S4、若空间机械臂捕获到固定目标或与障碍物相撞，该流程结束；若空间机械臂没有捕获到固定目标或与没有障碍物相撞，则返回S2循环往复，直至空间机械臂捕获到固定目标或与障碍物相撞。

如图6所示，利用本发明提出的空间机械臂避障路径规划方法对固定障碍实现的空间机械臂避障路径规划。图7为利用本发明提出的空间机械臂避障路径规划方法对移动障碍实现的空间机械臂避障路径规划。

图6和图7表明本发明利用强化学习算法实现了在捕获固定目标的同时对固定或移动障碍物的躲避，提高了空间机械臂的鲁棒性，使其能适应复杂的空间环境。

本发明的空间机械臂避障路径规划方法可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本发明的空间机械臂避障路径规划方法如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。计算机可读存储介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。其中，所述计算机存储介质可以是计算机能够存取的任何可用介质或数据存储设备，包括但不限于磁性存储器(例如软盘、硬盘、磁带、磁光盘(MO)等)、光学存储器(例如CD、DVD、BD、HVD等)、以及半导体存储器(例如ROM、EPROM、EEPROM、非易失性存储器(NANDFLASH)、固态硬盘(SSD))等。

在示例性实施例中，还提供计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现所述空间机械臂避障路径规划方法的步骤。处理器可能是中央处理单元(CentralProcessingUnit，CPU)，还可以是其他通用处理器、数字信号处理器(DigitalSignalProcessor，DSP)、专用集成电路(ApplicationSpecificIntegratedCircuit，ASIC)、现成可编程门阵列(Field-ProgrammableGateArray，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。

本发明首次提出利用强化学习解决空间机械臂对固定目标进行轨迹规划时的避障问题。虽然本发明在陈述过程中利用逆运动学求解空间机械臂在无障碍条件下的轨迹规划问题，但在实际过程中可利用不同的轨迹规划方法对空间机械臂末端进行轨迹规划，均可使用本方法解决轨迹规划过程中的避障问题。

相比于传统的空间机械臂轨迹规划方法，本发明在实际应用时，可以先在地面上对空间机械臂进行离线训练，得到相应的状态—动作值函数表，在线应用阶段利用得到的函数表进行实时的轨迹调整，这使得空间机械臂具有了一定的智能性。本发明设计的方法只需要在任务开始时利用逆运动学计算空间机械臂各关节所需角度，在任务过程中不需要进行其他运动学方面的计算，轨迹调整由各智能体自行完成，大大减少了计算量，有利于潜在的工程应用。

以上所述，仅是本发明的一般实施例而已，并未对本发明做任何形式上的限制，虽然本发明以一般实施例证明了本发明有效解决了空间机械臂对固定目标进行作业时的避障路径规划问题，但是其可以轻易的推广到其他不同任务的路径规划问题。因此，任何熟悉本专业的技术人员，在不脱离本发明技术方案范围内，均可轻易利用上述揭示的方法及技术内容做出些许的更动或修饰得到等同变化的等效实施例。但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上一般实施例或类似工作所做的任何简单修改、等同变化与修饰，仍属本发明技术方案的范围内。

Claims (9)

1.一种空间机械臂避障路径规划方法，其特征在于，包括以下步骤：

步骤1：设计用于空间机械臂避障的强化学习算法；

步骤2：将每节机械臂都视为智能体，用强化学习算法对各节机械臂进行离线训练，获取各个智能体的状态-动作值函数表；

步骤3：空间机械臂在作业过程中，将各个关节角变化转换为利用逆运动学求出的关节角度值；

当空间机械臂靠近障碍物时，相应的臂杆进入到危险状态，这时智能体自动根据状态-动作值函数表选择相应的动作对障碍物进行避障。

2.根据权利要求1所述的一种空间机械臂避障路径规划方法，其特征在于，强化学习算法采用Q学习算法。

3.根据权利要求1所述的一种空间机械臂避障路径规划方法，其特征在于，步骤1具体为：

1.1、设计智能体的状态值与动作值，根据不同的状态智能体采取不同的动作；

1.2、设计智能体选择动作策略，即在当前状态下选取能使得动作价值函数值最大的动作；

1.3、设计奖惩函数来对选取动作后智能体所处的状态进行评价，进而对动作价值函数值进行更新。

4.根据权利要求1所述的一种空间机械臂避障路径规划方法，其特征在于，步骤3中，将各个关节角变化转换为利用逆运动学求出的关节角度值，具体为：

以3连杆6自由度空间机械臂为研究对象，三段连杆的臂长分别设为l₁、l₂、l₃，机械臂的六个关节都是旋转副，分别称为第一关节、第二关节、第三关节、第四关节、第五关节及第六关节；第一关节的关节角为θ₁，第二关节的关节角为θ₂，第三关节的关节角为θ₃，第四关节的关节角为θ₄，第五关节的关节角为θ₅，第六关节的关节角为θ₆；

第一关节的变换矩阵为

第二关节的变换矩阵为

第三关节的变换矩阵为

第四关节的变换矩阵为

第五关节的变换矩阵为

第六关节的变换矩阵为

将六个关节的变换矩阵依次相乘，得到空间机械臂的运动学方程：

用逆变换

左乘运动学方程(1)求解θ₁和θ₃：

用逆变换

左乘运动学方程(1)求解θ₂和θ₄：

θ₂＝θ₂₃-θ₃ (4)

其中，

θ₄＝Atan2(-a_xsinθ₁+a_ycosθ₁，-a_xcosθ₁cosθ₂₃-a_ysinθ₁cosθ₂₃+a_zsinθ₂₃) (5)

用逆变换

左乘运动学方程(1)求解θ₅：

θ₅＝Atan2(sθ₅，cθ₅) (6)

用逆变换

左乘运动学方程(1)求解θ₆：

θ₆＝Atan2(sθ₆，cθ₆) (7)。

5.根据权利要求1所述的一种空间机械臂避障路径规划方法，其特征在于，步骤2具体为：

S1、首先对智能体参数进行初始化；

S2、设置随机障碍物位置与机械臂各关节角初始值；

S3、智能体感知状态s(0)，依据公式(9)选择动作a(0)并执行，智能体继续感知状态s(1)，K++，根据奖惩函数反馈奖励R，根据公式(8)更新动作价值函数Q(s，a)；

S4、判断智能体是否到达安全环境，若否，则返回S3；若是，则I++，K＝0；I为训练周期数、K为试探次数；

S5、然后判断I是否到达上限，若否，则返回S3；若是则流程结束。

6.根据权利要求1所述的一种空间机械臂避障路径规划方法，其特征在于，步骤3中，利用状态-动作值函数表进行避障的具体过程为：

S3.1、设置空间机械臂各关节角初始值；

S3.2、判断θ_i是否等于θ_i’，若等于，则θ_i不变，各智能体感知状态s；若θ_i小于θ_i’，则θ_i＝θ_i’+0.1，若θ_i大于θ_i’，则θ_i＝θ_i’-0.1，各智能体感知状态s；

θ_i为机械臂关节角当前位置，θ_i’为机械臂关节角期望位置；

S3.3、判断智能体是否在危险区，若不在危险区，则智能体不执行工作，判断空间机械臂捕获到固定目标或与障碍物相撞；若在危险区，则智能体根据状态-动作函数表选择当前状态下，状态-动作值最高的动作a并执行，判断空间机械臂捕获到固定目标或与障碍物相撞；

S3.4、若空间机械臂捕获到固定目标或与障碍物相撞，该流程结束；若空间机械臂没有捕获到固定目标或与没有障碍物相撞，则返回S3.2循环往复，直至空间机械臂捕获到固定目标或与障碍物相撞。

7.一种空间机械臂避障路径规划系统，其特征在于，包括关节角度计算模块、训练模块、避障模块；

关节角度计算模块，用于利用逆运动学求出空间机械臂末端到达目标点所需的各个关节角度值；

强化学习模块，用于负载空间机械臂避障的强化学习算法；

训练模块，用于对各节机械臂进行离线训练，获取各个智能体的状态-动作值函数表；

避障模块，用于在空间机械臂在作业过程中，当空间机械臂靠近障碍物时，相应的臂杆进入到危险状态，这时智能体自动根据状态-动作值函数表选择相应的动作对障碍物进行避障。

8.一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至6任意一项所述空间机械臂避障路径规划方法的步骤。

9.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至6任意一项所述空间机械臂避障路径规划方法的步骤。

Images