CN112894817A - 一种任务空间下的机械臂运动规划方法 - Google Patents

Abstract

本发明涉及一种任务空间下的机械臂运动规划方法。本发明首先根据机械臂的结构，采用D‑H方法建立正运动学模型，在任务空间直接进行末端位姿的启发式搜索，通过对选取的待扩展节点进行梯度投影逆运动学计算获得关节角数据，在关节空间对待扩展节点进行碰撞检测，判断机械臂能否安全到达待扩展节点。最后，将发生碰撞的位姿映射到末端工作平面中进行局部最小判断。本发明的机械臂任务空间启发式搜索优化方法弥补了机械臂任务空间规划方法的不足，是一种能在任务空间快速搜索出可行路径的方法。本发明所提方法可以使机械臂避免陷入局部最优陷阱。

Description

一种任务空间下的机械臂运动规划方法

技术领域

本发明属于自动化技术领域，涉及一种任务空间下的机械臂运动规划方法。

背景技术

机械臂是服务机器人对目标执行操作的核心部件，也是自动化生产线上的关键设备。在完成复杂任务时，机械臂的运动规划需要面临一些末端位置和姿态的约束。末端执行器在精准的抓取、切割、焊接等任务中均需满足末端位置和姿态约束。目前基于关节空间的运动规划方法，然而，在关节空间描述的任务约束下，难以进行复杂的运动规划。因此，在任务空间下构建机械臂的运动规划方法对于机器人执行末端约束下的任务具有重要意义。

发明内容

本发明的目标是构建任务空间下的机械臂启发式搜索运动规划方法,并对其优化。首先，在任务空间直接进行末端位姿的启发式搜索，然后，对球形包络后的障碍物与机械臂进行碰撞检测。最后将发生碰撞的位姿映射到末端工作平面中进行局部最小判断。该方法能有效避免机械臂在任务空间下采用启发式搜索规划时陷入局部最小状态的缺陷。在机械臂的任务空间中采用启发式搜索算法进行规划，具有规划速度快，能满足任务约束等优点。但是该方法容易陷入局部极小陷阱。本发明所提方法可以弥补该缺陷。

本发明方法的步骤包括：

步骤(1)、采用D-H法(Denavit-Hartenberg)建立机械臂的正运动学模型，并构建障碍物模型；将障碍物的位置数据保存至不可行集合Closed表，其中障碍物的位置数据包括障碍物的中心、半径信息；同时根据机械臂的正运动学模型计算出初始状态的末端执行器位姿p_f，其中位姿包括姿态信息和坐标信息。

通过机械臂的每个关节J_i的构型与运动信息获得各关节的运动变换矩阵A_i，通过公式(1)得到变换矩阵

其中1≤i≤j，j表示机械臂关节自由度；

表示p_f的三维姿态信息；(p_x，p_y，p_z)表示p_f的三维坐标信息；

步骤(2)、将p_f的三维姿态信息采用欧拉角表示。初始化时将机械臂当前末端执行器的位姿p_c设置为p_f的位姿信息如下：

p_f＝(x_f，y_f，z_f，α_f，β_f，γ_f) 式(3)

其中x_f，y_f，z_f表示p_f的三维坐标信息；α_f，β_f，γ_f表示p_f的欧拉角；

步骤(3)、使用任务空间下的启发式搜索算法ARA^*(Anytime RepairingA*)对机械臂末端执行器位姿进行规划：

3-1初始化：将待扩展节点列表P_NEXT设置为空列表，P_NEXT节点数目u＝0，循环参数v＝1。

3-2将机械臂当前末端执行器位姿的6个自由度各自加减一个单位获得12个周边节点信息；将上述12个周边节点中不在不可行集合Closed表中的节点加入到待扩展节点列表P_NEXT；

P_NEXT＝{p₁，p′₁；p₂，p′₂；...；p_v，p′_v；...；p_u，p′_u}，u≤12 式(4)

p_v表示周边节点，p′_v表示p_v的父节点。

同时将待扩展节点列表P_NEXT中不在可行集合OPEN表的节点进行初始化操作：将父节点设为空，原有代价函数F(p_v)_old设为无穷大；然后计算待扩展节点列表P_NEXT所有节点的新代价函数F(p_v)_new；

F(p_v)_new＝G(p_v)+H(p_v) 式(5)

F(p_v)_old＝G(p_v)_old+H(p_v) 式(6)

其中，G(p_v)表示从初始节点到p_c的代价值加一，H(p_v)表示p_v到目标节点的欧式距离，初始节点为初始状态的末端执行器位姿p_f，目标节点为机械臂末端执行器最终要到达的位姿；G(p_v)_old表示初始节点到p′_v的代价值加一。

3-3比较待扩展节点列表P_NEXT中所有节点的F(n)_old和F(n)_new，如果F(n)_old＞F(n)_new，将父节点p′_v改为当前节点p_c，同时将p_v的F(n)_old赋值为F(n)_new，反之不做变化。

步骤(4)、判断是否遍历列表P_NEXT中所有节点

比较v和u的大小，若v≤u则对列表P_NEXT的第v个节点设为子节点p_next，执行步骤(5)-(6)，若v＞u则将可行集合OPEN表中代价函数F(n)最小的节点更新为机械臂当前末端执行器位姿p_c，返回步骤(3)。

步骤(5)、对子节点p_next进行碰撞检测：

5-1对节点p_next采用梯度投影法进行逆运动学计算，得到一组最优的逆运动学解，即为关节角解；将这组关节角解通过D-H正运动学计算得到机械臂所有关节的三维坐标。

5-2根据5-1得到的机械臂所有关节的三维坐标构建各关节的机械臂构型；然后对各关节的机械臂构型进行碰撞检测，判断节点p_next是否会与障碍物发生碰撞，具体步骤如下：

5-2-1选取相邻两个关节J_a,J_b之间的机械臂构型L,设安全距离r为障碍物半径加上ε倍的机械臂构型半径：

r＝r_o+ε·r_l 式(7)

其中r_p表示障碍物半径，r_l表示机械臂构型半径，ε为人为设定，ε＞1。

5-2-2计算构型L到障碍物中心的距离s；如果s＞r，表示未发生碰撞，则将节点p_next加入到可行集合OPEN表中，执行步骤(7)；若s≤r,则首先计算以障碍物中心为顶角，障碍物中心与两个关节连接的线段为边的夹角∠MON，并计算构型L距离障碍物中心obs为s时的最大安全角δ，若夹角∠MON小于最大安全角δ则认为未发生碰撞，则将节点p_next加入到可行集合OPEN表中，执行步骤(7)；若夹角∠MON大于等于最大安全角δ则发生碰撞，进行步骤(6)。

其中M为关节J_a的中心点，N是关节J_b的中心点，O是障碍物的中心，a是障碍物中心到关节J_a的距离；b是障碍物中心到关节J_b的距离；l为机械臂构型L的长度。

其中s为障碍物中心到构型L的距离，r表示距离障碍物中心的安全距离，l表示构型L的长度。

步骤(6)、发生碰撞后需要判断是否会出现机械臂构型无法绕开障碍物的情况，进行局部最小检测，具体步骤如下：

6-1建立以当前末端执行器位姿p_c的三维坐标信息(x_c，y_c，z_c),目标位置p_t的三维坐标信息(x_t，y_t，z_t)，以机械臂的基座中心为原点p_o(0，0，0)构成末端工作平面OCT。

Ax+By+Cz+D＝0 式(10)

6-2将障碍物及机械臂构型投影到该平面OCT中，球形障碍物在平面中的投影是以障碍中心的投影点p_o′为圆心，以障碍物半径r_o为半径的圆形障碍物obs’。机械臂构型在平面中的投影是各关节角在平面的投影点所组成的新的构型L‘。

6-3观察投影后的机械臂构型L‘是否与投影后的障碍物obs’发生碰撞：对L‘和obs’使用步骤(5)的方法进行碰撞检测。若构型L‘与obs’发生碰撞，则表示机械臂构型无法绕开障碍物。将待扩展节点

加入到不可行集合Closed表中，更新v＝v+1,返回步骤(4)。若未发生碰撞，判断机械臂构型L‘是否将障碍物obs’包围,步骤如下：

a)根据目标位置p_t和障碍物中心p_o′得向量

计算在

垂直方向上的单位向量

b)根据障碍物中心p_o′和原点P_o得向量

根据障碍物中心p_o′和当前节点坐标p_c得向量

计算

和

的数量积与

和

的数量积的乘积F：

若F<0，则认为机械臂构型L‘包围了障碍物obs’，这种情况表示机械臂构型无法绕开障碍物。因此将这些会造成局部最小问题的待扩展节点p_next加入到不可行集合Closed表中，更新v＝v+1,返回步骤(4)。若F≥0，则认为机械臂构型L‘未包围障碍物obs’，机械臂构型可以绕开障碍物。将节点p_next舍去，当作未查询过的节点，更新v＝v+1,返回步骤(4)。

步骤(7)、，判断可行集合OPEN表中含有目标节点，若是则在可行集合OPEN表中采用回溯父节点的方式从目标节点回溯到初始节点得到一条安全的机械臂运动路径，反之则更新v＝v+1,返回步骤(4)。

本发明的另一个目的是提供一种计算机可读存储介质，其上存储有计算机程序，当所述计算机程序在计算机中执行时，令计算机执行上述的方法。

本发明的又一个目的是提供一种计算设备，包括存储器和处理器，所述存储器中存储有可执行代码，所述处理器执行所述可执行代码时，实现上述的方法。

本发明提出的机械臂任务空间运动规划优化方法弥补了现阶段机械臂关节空间规划方法很难描述任务约束的缺点,并有效地解决了任务空间启发式搜索规划算法中存在的末端执行器陷入局部最优陷阱的问题。

本发明提出的机械臂任务空间运动规划方法，能够有效地控制机械臂，完成具有末端位姿约束特点的任务。

附图说明

图1为本发明的流程图；

图2为夹角∠MON示意图；

图3为投影至OCT平面示意图。

具体实施方式

下面结合具体实施例对本发明做进一步的分析。

以一具体机械臂为例，如图1任务空间启发式搜索优化方法的实施步骤如下：

步骤(1)、初始化机械臂系统及障碍物参数，具体操作如下：

1-1获得机械臂结构参数。即机械臂所对应的D-H参数表。

表1：机械臂的D-H参数表

其中，0代表机械臂基座，1…6数字代表第1…6个关节。θ表示旋转关节的关节变量，d表示滑动关节的关节变量。q表示关节之间的距离。α表示关节之间的姿态变化量。

1-2对机械臂进行建模,将机械臂抽象为关节与关节间的圆柱形连杆组成的构型L。关节坐标由D-H正运动学计算得出,构型连杆的半径为r_l。

1-3对障碍物进行球形包络,得到障碍物球心坐标obs,障碍物半径为r_o。

步骤(2)、采用D-H法(Denavit-Hartenberg)建立机械臂的正运动学模型，并构建障碍物模型；将障碍物的位置数据保存至不可行集合Closed表，其中障碍物的位置数据包括障碍物的中心、半径信息；同时根据机械臂的正运动学模型计算出初始状态的末端执行器位姿p_f，其中位姿包括姿态信息和坐标信息。

通过机械臂的每个关节J_i的构型与运动信息获得各关节的运动变换矩阵A_i，通过公式(1)得到变换矩阵

其中1≤i≤j，j表示机械臂关节自由度；

表示p_f的三维姿态信息；(p_x，p_y，p_z)表示p_f的三维坐标信息；

步骤(3)、将p_f的三维姿态信息采用欧拉角表示。初始化时将机械臂当前末端执行器的位姿p_c设置为p_f的位姿信息如下：

p_f＝(x_f，y_f，z_f，α_f，β_f，γ_f) 式(3)

其中x_f，y_f，z_f表示三维坐标信息，α_f，β_f，γ_f表示p_f的欧拉角；

步骤(4)、使用任务空间下的启发式搜索算法ARA^*(Anytime RepairingA*)对机械臂末端执行器位姿进行规划：

4-1初始化：将待扩展节点列表P_NEXT设置为空列表，P_NEXT节点数目u＝0，循环参数v＝1。

4-2将机械臂当前末端执行器位姿的6个自由度各自加减一个单位获得12个周边节点信息；

p₁＝(x_c+1，y_c，z_c，α_c，β_c，γ_c)

p₂＝(x_c-1，y_c，z_c，α_c，β_c，γ_c)

p₃＝(x_c，y_c+1，z_c，α_c，β_c，γ_c)

p₄＝(x_c，y_c-1，z_c，α_c，β_c，γ_c)

p₅＝(x_c，y_c，z_c+1，α_c，β_c，γ_c)

p₆＝(x_c，y_c，z_c-1，α_c，β_c，γ_c)

p₇＝(x_c，y_c，z_c，α_c+1，β_c，γ_c)

p₈＝(x_c，y_c，z_c，α_c-1，β_c，γ_c)

p₉＝(x_c，y_c，z_c，α_c，β_c+1，γ_c)

p₁₀＝(x_c，y_c，z_c，α_c，β_c-1，γ_c)

p₁₁＝(x_c，y_c，z_c，α_c，β_c，γ_c+1)

p₁₂＝(x_c，y_c，z_c，α_c，β_c，γ_c-1)

将上述12个周边节点中不在不可行集合Closed表中的节点加入到待扩展节点列表P_NEXT；

P_NEXT＝{p₁，p′₁；p₂，p′₂；...；p_v，p′_v；…；p_u，p′_u}，u≤12 式(4)

p_v表示周边节点，p′_v表示p_v的父节点。

同时将待扩展节点列表P_NEXT中不在可行集合OPEN表的节点进行初始化操作：将父节点设为空，原有代价函数F(p_v)_old设为无穷大；然后计算待扩展节点列表P_NEXT所有节点的新代价函数F(p_v)_new；

F(p_v)_new＝G(p_v)+H(p_v) 式(5)

F(p_v)_old＝G(p_v)_old+H(p_v) 式(6)

其中G(p_v)表示从初始节点到p_c的代价值加一，H(p_v)表示p_v到目标节点的欧式距离，初始节点为初始状态的末端执行器位姿p_f，目标节点为机械臂末端执行器最终要到达的位姿；G(p_v)_old表示初始节点到p′_v的代价值加一。

4-3比较待扩展节点列表P_NEXT中所有节点的F(n)_old和F(n)_new，如果F(n)_old＞F(n)_new，将父节点p′_v改为当前节点p_c，同时将p_v的F(n)_old赋值为F(n)_new，反之不做变化。

步骤(5)、判断是否遍历列表P_NEXT中所有节点

比较v和u的大小，若v≤u则对列表P_NEXT的第v个节点设为子节点p_next，执行步骤(6)-(7)，若v＞u则将可行集合OPEN表中代价函数F(n)最小的节点更新为机械臂当前末端执行器位姿p_c，返回步骤(4)。

步骤(6)、对子节点p_next进行碰撞检测：

6-1对节点p_next采用梯度投影法进行逆运动学计算，得到一组最优的逆运动学解，即为关节角解；将这组关节角解通过D-H正运动学计算得到机械臂所有关节的三维坐标。

6-2根据6-1得到的机械臂所有关节的三维坐标构建各关节的机械臂构型；然后对各关节的机械臂构型进行碰撞检测，判断节点p_next是否会与障碍物发生碰撞，具体步骤如下：

6-2-1选取相邻两个关节J_a,J_b之间的机械臂构型L,设安全距离r为障碍物半径加上ε倍的机械臂构型半径：

r＝r_o+ε·r_l 式(7)

其中r_o表示障碍物半径，r_l表示机械臂构型半径，ε为人为设定，ε＞1。

6-2-2计算构型L到障碍物中心的距离s；如果s＞r，表示未发生碰撞，则将节点p_next加入到可行集合OPEN表中，执行步骤(8)；若s≤r,则首先计算以障碍物中心为顶角，障碍物中心与两个关节连接的线段为边的夹角∠MON(如图2)，并计算构型L距离障碍物中心obs为s时的最大安全角δ，若夹角∠MON小于最大安全角δ则认为未发生碰撞，则将节点p_next加入到可行集合OPEN表中，执行步骤(8)；若夹角∠MON大于等于最大安全角δ则发生碰撞，进行步骤(7)。

其中M为关节J_a的中心点，N是关节J_b的中心点，O是障碍物的中心，a是障碍物中心到关节J_a的距离；b是障碍物中心到关节J_b的距离；l为机械臂构型L的长度。

其中s为障碍物中心到构型L的距离，r表示距离障碍物中心的安全距离，l表示构型L的长度。

步骤(7)、发生碰撞后需要判断是否会出现机械臂构型无法绕开障碍物的情况，进行局部最小检测，具体步骤如下：

7-1建立以当前末端执行器位姿p_c的三维坐标信息(x_c，y_c，z_c),目标位置p_t的三维坐标信息(x_t，y_t，z_t)，以机械臂的基座中心为原点p_o(0，0，0)构成末端工作平面OCT。

Ax+By+Cz+D＝0 式(10)

满足如下约束：

A×0+B×0+C×0+D＝0 式(11)

Ax_t+By_t+Cz_t+D＝0 式(12)

Ax_c+By_c+Cz_c+D＝0 式(13)

令A＝1，得D＝0，

7-2如图3将障碍物及机械臂构型投影到该平面OCT中，球形障碍物在平面中的投影是以障碍中心的投影点p_o′为圆心，以障碍物半径r_o为半径的圆形障碍物obs’。机械臂构型在平面中的投影是各关节角在平面的投影点所组成的新的构型L‘。

7-3观察投影后的机械臂构型L‘是否与投影后的障碍物obs’发生碰撞：对L‘和obs’使用步骤(6)的方法进行碰撞检测。若构型L‘与obs’发生碰撞，表示机械臂构型无法绕开障碍物。将待扩展节点p_next加入到不可行集合Closed表中，更新v＝v+1,返回步骤(4)。若未发生碰撞，判断机械臂构型L‘是否将障碍物obs’包围,步骤如下：

a)根据目标位置p_t和障碍物中心p_o′得向量

计算在

垂直方向上的单位向量

b)根据障碍物中心p_o′和原点P_o得向量

根据障碍物中心p_o′和当前节点坐标

p_c得向量

计算

和

的数量积与

和

的数量积的乘积F：

若F<0，则认为机械臂构型L‘包围了障碍物obs’，这种情况表示机械臂构型无法绕开障碍物。因此将这些会造成局部最小问题的待扩展节点p_next加入到不可行集合Closed表中，更新v＝v+1,返回步骤(5)。若F≥0，则认为机械臂构型L‘未包围障碍物obs’，表示机械臂构型可以绕开障碍物。将节点p_next舍去，当作未查询过的节点，更新v＝v+1,返回步骤(5)。

步骤(8)、判断可行集合OPEN表中含有目标节点，若是则在可行集合OPEN表中采用回溯父节点的方式从目标节点回溯到初始节点得到一条安全的机械臂运动路径，反之则更新v＝v+1,返回步骤(5)。

Claims (7)

1.一种任务空间下的机械臂运动规划方法，该方法包括以下步骤：

步骤(1)、采用D-H法建立机械臂的正运动学模型，并构建障碍物模型；将障碍物的位置数据保存至不可行集合Closed表，其中障碍物的位置数据包括障碍物的中心、半径信息；同时根据机械臂的正运动学模型计算出初始状态的末端执行器位姿p_f，其中位姿包括姿态信息和位置坐标信息；

步骤(2)、将p_f的三维姿态信息采用欧拉角表示，初始化时将机械臂当前末端执行器的位姿p_c设置为p_f的位姿信息如下：

p_f＝(x_f，y_f，z_f，α_f，β_f，γ_f) 式(3)

其中x_f，y_f，z_f表示p_f的三维坐标信息，α_f，β_f，γ_f表示p_f的欧拉角；

步骤(3)、使用任务空间下的启发式搜索算法ARA*(Anytime Repairing A*)对机械臂末端执行器位姿进行规划，更新待扩展节点列表P_NEXT；

步骤(4)、判断是否遍历列表P_NEXT中所有节点

比较v和u的大小，若v≤u则将列表P_NEXT的第v个节点设为子节点p_next，执行步骤(5)-(6)，若v＞u则将可行集合OPEN表中代价函数F(n)最小的节点更新为机械臂当前末端执行器位姿p_c，返回步骤(3)；

步骤(5)、对子节点p_next进行碰撞检测：

5-1对节点p_next采用梯度投影法进行逆运动学计算，得到一组最优的逆运动学解，即为关节角解；将这组关节角解通过D-H正运动学计算得到机械臂所有关节的三维坐标；

5-2根据5-1得到的机械臂所有关节的三维坐标构建各关节的机械臂构型；然后对各关节的机械臂构型进行碰撞检测，判断节点p_next是否会与障碍物发生碰撞，具体步骤如下：

5-2-1选取相邻两个关节J_a，J_b之间的机械臂构型L，设安全距离r为障碍物半径加上ε倍的机械臂构型半径：

r＝r_o+ε·r_l 式(7)

其中r_o表示障碍物半径，r_l表示机械臂构型半径，ε为人为设定参数；

5-2-2计算构型L到障碍物中心的距离s；如果s＞r，表示未发生碰撞，则将节点p_next加入到可行集合OPEN表中，执行步骤(7)；若s≤r，则首先计算以障碍物中心为顶角，障碍物中心与两个关节连接的线段为边的夹角∠MON，并计算构型L距离障碍物中心obs为s时的最大安全角δ，若夹角∠MON小于最大安全角δ则认为未发生碰撞，则将节点p_next加入到可行集合OPEN表中，执行步骤(7)；若夹角∠MON大于等于最大安全角δ则发生碰撞，进行步骤(6)；

步骤(6)、发生碰撞后需要判断是否会出现机械臂构型无法绕开障碍物的情况，进行局部最小检测，具体步骤如下：

6-1建立以当前末端执行器位姿p_c的三维坐标信息(x_c，y_c，z_c)，目标位置p_t的三维坐标信息(x_t，y_t，z_t)，以机械臂的基座中心为原点p_o(0，0，0)构成末端工作平面OCT；

6-2将障碍物及机械臂构型投影到该平面OCT中，球形障碍物在平面中的投影是以障碍中心的投影点p_o′为圆心，以障碍物半径r_o为半径的圆形障碍物obs’；机械臂构型在平面中的投影是各关节角在平面的投影点所组成的新的构型L‘；

6-3观察投影后的机械臂构型L‘是否与投影后的障碍物obs’发生碰撞：对L‘和obs’使用步骤(5)的方法进行碰撞检测；若构型L‘与obs’发生碰撞，表示机械臂构型无法绕开障碍物；将待扩展节点Pp_next加入到不可行集合Closed表中，更新v＝v+1，返回步骤(4)；若未发生碰撞，判断机械臂构型L‘是否将障碍物obs’包围，步骤如下：

根据目标位置p_t和障碍物中心p_o′得向量

计算在

垂直方向上的单位向量

根据障碍物中心p_o′和原点P_o得向量

根据障碍物中心p_o′和当前节点坐标p_c得向量

计算

和

的数量积与

和

的数量积的乘积F：

若F＜0，则认为机械臂构型L‘包围了障碍物obs’，这种情况表示机械臂构型无法绕开障碍物；因此将这些会造成局部最小问题的待扩展节点p_next加入到不可行集合Closed表中，更新v＝v+1，返回步骤(4)；若F≥0，则认为机械臂构型L‘未包围障碍物obs’，表示机械臂构型可以绕开障碍物；将节点p_next舍去，当作未查询过的节点，更新v＝v+1，返回步骤(4)；

步骤(7)、判断可行集合OPEN表中含有目标节点，若是则在可行集合OPEN表中采用回溯父节点的方式从目标节点回溯到初始节点得到一条安全的机械臂运动路径，反之则更新v＝v+1，返回步骤(4)。

2.如权利要求1所述的一种任务空间下的机械臂运动规划方法，其特征在于步骤(1)通过机械臂的每个关节J_i的构型与运动信息获得各关节的运动变换矩阵A_i，通过公式(1)得到变换矩阵

其中1≤i≤j，j表示机械臂关节自由度；

表示p_f的三维姿态信息；(p_x，p_y，p_z)表示p_f的三维坐标信息。

3.如权利要求2所述的一种任务空间下的机械臂运动规划方法，其特征在于步骤(3)具体是：

3-1初始化：将待扩展节点列表P_NEXT设置为空列表，P_NEXT节点数目u＝0，循环参数v＝1；

3-2将机械臂当前末端执行器位姿p_c各自加减一个单位获得12个周边节点信息；将上述12个周边节点中不在不可行集合Closed表中的节点加入到待扩展节点列表P_NEXT；

P_NEXT＝{p₁，p′₁；p₂，p′₂；...；p_v，p′_v；...；p_u，p′_u}，u≤12 式(4)

其中p_v表示周边节点，p′_v表示p_v的父节点；

同时将待扩展节点列表P_NEXT中不在可行集合OPEN表的节点进行初始化操作：将父节点设为空，原有代价函数F(p_v)_old设为无穷大；然后计算待扩展节点列表P_NEXT所有节点的新代价函数F(p_v)_new；

F(p_v)_new＝G(p_v)+H(p_v) 式(5)

F(p_v)_old＝G(p_v)_old+H(p_v) 式(6)

其中G(p_v)表示从初始节点到p_c的代价值加一，H(p_v)表示p_v到目标节点的欧式距离，初始节点为初始状态的末端执行器位姿p_f，目标节点为机械臂末端执行器最终要到达的位姿；G(p_v)_old表示初始节点到p′_v的代价值加一；

3-3比较待扩展节点列表P_NEXT中所有节点的F(n)_old和F(n)_new，如果F(n)_old＞F(n)_new，将父节点p′_v改为当前节点p_c，同时将p_v的F(n)_old赋值为F(n)_new，反之不做变化。

4.如权利要求3所述的一种任务空间下的机械臂运动规划方法，其特征在于步骤(5-2-2)中以障碍物中心为顶角，障碍物中心与两个关节连接的线段为边的夹角∠MON具体是：

其中M为关节J_a的中心点，N是关节J_b的中心点，O是障碍物的中心，a是障碍物中心到关节J_a的距离；b是障碍物中心到关节J_b的距离；l为机械臂构型L的长度。

5.如权利要求3或4所述的一种任务空间下的机械臂运动规划方法，其特征在于步骤(5-2-2)中最大安全角δ具体是：

其中s为障碍物中心到构型L的距离，r表示距离障碍物中心的安全距离，l表示构型L的长度。

6.一种计算机可读存储介质，其上存储有计算机程序，当所述计算机程序在计算机中执行时，令计算机执行权利要求1-5中任一项所述的方法。

7.一种计算设备，包括存储器和处理器，所述存储器中存储有可执行代码，所述处理器执行所述可执行代码时，实现权利要求1-5中任一项所述的方法。

Images