[0054] 下面结合具体实施例对本发明做进一步的分析。
[0055] 以一具体机械臂为例，如图1任务空间启发式搜索优化方法的实施步骤如下：
[0056] 步骤(1)、初始化机械臂系统及障碍物参数，具体操作如下：
[0057] 1‑1获得机械臂结构参数。即机械臂所对应的D‑H参数表。
[0058] 表1：机械臂的D‑H参数表
[0059]
[0060] 其中，0代表机械臂基座，1…6数字代表第1…6个关节。θ表示旋转关节的关节变量，d表示滑动关节的关节变量。q表示关节之间的距离。α表示关节之间的姿态变化量。
[0061] 1‑2对机械臂进行建模,将机械臂抽象为关节与关节间的圆柱形连杆组成的构型L。关节坐标由D‑H正运动学计算得出,构型连杆的半径为rl。
[0062] 1‑3对障碍物进行球形包络,得到障碍物球心坐标obs,障碍物半径为ro。
[0063] 步骤(2)、采用D‑H法(Denavit‑Hartenberg)建立机械臂的正运动学模型，并构建障碍物模型；将障碍物的位置数据保存至不可行集合Closed表，其中障碍物的位置数据包括障碍物的中心、半径信息；同时根据机械臂的正运动学模型计算出初始状态的末端执行器位姿pf，其中位姿包括姿态信息和坐标信息。
[0064] 通过机械臂的每个关节Ji的构型与运动信息获得各关节的运动变换矩阵Ai，通过公式(1)得到变换矩阵
[0065]
[0066]
[0067] 其中1≤i≤j，j表示机械臂关节自由度； 表示pf的三维姿态信息；(xf，yf，zf)表示pf的三维坐标信息；
[0068] 步骤(3)、将pf的三维姿态信息采用欧拉角表示。初始化时将机械臂当前末端执行器的位姿pc设置为pf的位姿信息如下：
[0069] pf＝(xf，yf，zf，αf，βf，γf) 式(3)
[0070] 其中xf，yf，zf表示三维坐标信息，αf，βf，γf表示pf的欧拉角；
[0071] 步骤(4)、使用任务空间下的启发式搜索算法ARA*(Anytime RepairingA*)对机械臂末端执行器位姿进行规划：
[0072] 4‑1初始化：将待扩展节点列表PNEXT设置为空列表，PNEXT节点数目u＝0，循环参数v＝1。
[0073] 4‑2将机械臂当前末端执行器位姿的6个自由度各自加减一个单位获得12个周边节点信息；
[0074] p1＝(xc+1，yc，zc，αc，βc，γc)
[0075] p2＝(xc‑1，yc，zc，αc，βc，γc)
[0076] p3＝(xc，yc+1，zc，αc，βc，γc)
[0077] p4＝(xc，yc‑1，zc，αc，βc，γc)
[0078] p5＝(xc，yc，zc+1，αc，βc，γc)
[0079] p6＝(xc，yc，zc‑1，αc，βc，γc)
[0080] p7＝(xc，yc，zc，αc+1，βc，γc)
[0081] p8＝(xc，yc，zc，αc‑1，βc，γc)
[0082] p9＝(xc，yc，zc，αc，βc+1，γc)
[0083] p10＝(xc，yc，zc，αc，βc‑1，γc)
[0084] p11＝(xc，yc，zc，αc，βc，γc+1)
[0085] p12＝(xc，yc，zc，αc，βc，γc‑1)
[0086] 将上述12个周边节点中不在不可行集合Closed表中的节点加入到待扩展节点列表PNEXT；
[0087] PNEXT＝{p1，p′1；p2，p′2；...；pv，p′v；...；pu，p′u}，u≤12 式(4)[0088] pv表示周边节点，p′v表示pv的父节点。
[0089] 同时将待扩展节点列表PNEXT中不在可行集合OPEN表的节点进行初始化操作：将父节点设为空，原有代价函数F(pv)old设为无穷大；然后计算待扩展节点列表PNEXT所有节点的新代价函数F(pv)new；
[0090] F(pv)new＝G(pv)+H(pv) 式(5)
[0091] F(pv)old＝G(pv)old+H(pv) 式(6)
[0092] 其中G(pv)表示从初始节点到pc的代价值加一，H(pv)表示pv到目标节点的欧式距离，初始节点为初始状态的末端执行器位姿pf，目标节点为机械臂末端执行器最终要到达的位姿；G(pv)old表示初始节点到p′v的代价值加一。
[0093] 4‑3比较待扩展节点列表PNEXT中所有节点的F(pv)old和F(pv)new，如果F(pv)old>F(pv)new，将父节点p′v改为当前节点pc，同时将pv的F(pv)old赋值为F(pv)new，反之不做变化。
[0094] 步骤(5)、判断是否遍历列表PNEXT中所有节点
[0095] 比较v和u的大小，若v≤u则对列表PNEXT的第v个节点设为子节点pnext，执行步骤(6)‑(7)，若v＞u则将可行集合OPEN表中代价函数最小的节点更新为机械臂当前末端执行器位姿pc，返回步骤(4)。
[0096] 步骤(6)、对子节点pnext进行碰撞检测：
[0097] 6‑1对节点pnext采用梯度投影法进行逆运动学计算，得到一组最优的逆运动学解，即为关节角解；将这组关节角解通过D‑H正运动学计算得到机械臂所有关节的三维坐标。
[0098] 6‑2根据6‑1得到的机械臂所有关节的三维坐标构建各关节的机械臂构型；然后对各关节的机械臂构型进行碰撞检测，判断节点pnext是否会与障碍物发生碰撞，具体步骤如下：
[0099] 6‑2‑1选取相邻两个关节Ja，Jb之间的机械臂构型L，设安全距离r为障碍物半径加上ε倍的机械臂构型半径：
[0100] r＝ro+ε·rl 式(7)
[0101] 其中ro表示障碍物半径，rl表示机械臂构型半径，ε为人为设定，ε＞1。
[0102] 6‑2‑2计算构型L到障碍物中心的距离s；如果s＞r，表示未发生碰撞，则将节点pnext加入到可行集合OPEN表中，执行步骤(8)；若s≤r，则首先计算以障碍物中心为顶角，障碍物中心与两个关节连接的线段为边的夹角∠MON(如图2)，并计算构型L距离障碍物中心obs为s时的最大安全角δ，若夹角∠MON小于最大安全角δ则认为未发生碰撞，则将节点pnext加入到可行集合OPEN表中，执行步骤(8)；若夹角∠MON大于等于最大安全角δ则发生碰撞，进行步骤(7)。
[0103]
[0104] 其中M为关节Ja的中心点，N是关节Jb的中心点，O是障碍物的中心，a是障碍物中心到关节Ja的距离；b是障碍物中心到关节Jb的距离；l为机械臂构型L的长度。
[0105]
[0106] 其中s为障碍物中心到构型L的距离，r表示距离障碍物中心的安全距离，l表示构型L的长度。
[0107] 步骤(7)、发生碰撞后需要判断是否会出现机械臂构型无法绕开障碍物的情况，进行局部最小检测，具体步骤如下：
[0108] 7‑1建立以当前末端执行器位姿pc的三维坐标信息(xc，yc，zc)，目标位置pt的三维坐标信息(xt，yt，zt)，以机械臂的基座中心为原点po(0，0，0)构成末端工作平面OCT。
[0109] Ax+By+Cz+D＝0 式(10)
[0110] 满足如下约束：
[0111] A×0+B×0+C×0+D＝0 式(11)
[0112] Axt+Byt+Czt+D＝0 式(12)
[0113] Axc+Byc+Czc+D＝0 式(13)
[0114] 令A＝1，得D＝0，
[0115] 7‑2如图3将障碍物及机械臂构型投影到该平面OCT中，球形障碍物在平面中的投影是以障碍中心的投影点po′为圆心，以障碍物半径ro为半径的圆形障碍物obs’。机械臂构型在平面中的投影是各关节角在平面的投影点所组成的新的构型L‘。
[0116] 7‑3观察投影后的机械臂构型L‘是否与投影后的障碍物obs’发生碰撞：对L‘和obs’使用步骤(6)的方法进行碰撞检测。若构型L‘与obs’发生碰撞，表示机械臂构型无法绕开障碍物。将待扩展节点pnext加入到不可行集合Closed表中，更新v＝v+1，返回步骤(4)。若未发生碰撞，判断机械臂构型L‘是否将障碍物obs’包围，步骤如下：
[0117] a)根据目标位置pt和障碍物中心po′得向量 计算在 垂直方向上的单位向量[0118]
[0119] b)根据障碍物中心po′和原点Po得向量 根据障碍物中心po′和当前节点坐标pc得向量 计算 和 的数量积与 和 的数量积的乘积F：
[0120]
[0121] 若F＜0，则认为机械臂构型L‘包围了障碍物obs’，这种情况表示机械臂构型无法绕开障碍物。因此将这些会造成局部最小问题的待扩展节点pnext加入到不可行集合Closed表中，更新v＝v+1，返回步骤(5)。若F≥0，则认为机械臂构型L‘未包围障碍物obs’，表示机械臂构型可以绕开障碍物。将节点pnext舍去，当作未查询过的节点，更新v＝v+1，返回步骤(5)。
[0122] 步骤(8)、判断可行集合OPEN表中含有目标节点，若是则在可行集合OPEN表中采用回溯父节点的方式从目标节点回溯到初始节点得到一条安全的机械臂运动路径，反之则更新v＝v+1，返回步骤(5)。
