六自由度机械臂避障路径规划方法技术

本发明专利技术提出一种六自由度机械臂避障路径规划方法，在已知起始位置，目标位置和障碍物信息的情况下，利用双树结构探索空间，辅以目标偏置和metropolis接受准则优化随机采样点的生成，加快算法收敛于目标位置，采用动态步长，根据节点扩展情况来评判节点周围障碍物情况，依据障碍物多少动态调节节点扩展步长，使其适应不同的障碍物环境，并采用势力场修正策略，加快算法远离起始点，收敛目标位置，有效提高了算法避障路径规划的实时性，最后用重选父节点优化路径节点，减少路径长度。前述设计的路径规划算法可以很好的适用于六自由度机械臂路径规划问题，在不同的障碍物环境中都可以完成路径规划任务。完成路径规划任务。完成路径规划任务。

【技术实现步骤摘要】
六自由度机械臂避障路径规划方法


[0001]本专利技术属于机械臂运动规划
，涉及一种六自由度机械臂避障路径规划方法，包括一种用于六自由度机械臂在复杂障碍物环境下避障路径规划的改进RRT
‑
Connect算法。。

技术介绍

[0002]机械臂应用是现代工业自动化、智能化的一个重要体现，而作为机械臂应用过程中所要解决的一个重要问题——路径规划问题一直颇受人们的重视。机械臂路径规划方法的好坏直接影响机械臂的整体工作性能及效率。
[0003]目前，解决机械臂路径规划问题的常用的方法有：A
*
算法、人工势场法、快速随机树算法、RRT
‑
Connect算法、RRT*算法等。其中，A
*
算法和人工势场法主要在移动机器人路径规划中具有较好的表现，但随着维度的增加，空间环境复杂度的提升将导致前述算法的计算量陡增，并可能会导致算法陷入局部极小值问题。基于采样的快速随机树算法虽可减少障碍物描述以及用搜索树来探索工作空间，适用于不同维度的机械臂路径规划问题，但该算法采取完全随机的采样本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种六自由度机械臂避障路径规划方法，其特征在于：在已知起始位置，目标位置和障碍物信息的情况下，利用双树结构探索空间，辅以目标偏置和metropolis接受准则优化随机采样点的生成，以加快算法收敛于目标位置；并依据势力场修正策略，调整节点扩展位置，加快算法远离起始点，收敛目标位置；采用动态步长，根据节点扩展情况评判节点周围障碍物情况，依据障碍物多少动态调节节点扩展步长，使其适应不同的障碍物环境；最后采用重选父节点优化路径节点，减少路径长度。2.根据权利要求1所述的六自由度机械臂避障路径规划方法，其特征在于，包括以下步骤：步骤S1：设六自由度机械臂末端操作器在系统基坐标系O
‑
xyz下的位置矢量为P＝(x,y,z)
T
；其末端操作器连体坐标系O
e
‑
x
e
y
e
z
e
的姿态通过基坐标系O
‑
xyz进行Z
‑
Y
‑
X欧拉角q＝(γ,β,α)
T
旋转操作确定，即以基坐标系O
‑
xyz为起始坐标系，先绕基坐标系O
‑
xyz的z轴旋转α角，获得坐标系O'
‑
x'y'z'；再绕旋转后的坐标系O'
‑
x'y'z'的y'轴旋转β角，获得坐标系O
”‑
x”y”z”；最后绕坐标系O
”‑
x”y”z”的x”轴旋转γ角，最终确定出机械臂末端操作器的连体坐标系O
e
‑
x
e
y
e
z
e
；由此，将机械臂末端操作器位姿相对于系统基坐标系O
‑
xyz的齐次变换矩阵描述为：其中，为末端操作器连体坐标系O
e
‑
x
e
y
e
z
e
姿态相对于基坐标系O
‑
xyz的旋转变换矩阵，O＝(0,0,0)为零向量，为机械臂连杆i连体坐标系相对于连杆i
‑
1连体坐标系的齐次变换矩阵，θ
i
表示机械臂连杆i相对于连杆i
‑
1的关节转角；利用上式及反变换法对系统进行运动学反解，可求得机械臂各连杆关节角θ
i
(i＝1,2,
…
,6)；利用符号表示机械臂在位置*时的系统节点；其中，表示机械臂各连杆在位置*时所对应的关节角；将系统起始位置节点用符号Q
init
表示，目标位置节点用Q
goal
表示，相应的以机械臂关节角行程作为系统的路径代价度量；在此基础上，获取包括尺寸、形状、起始位置Q
init
、目标位置Q
goal
的机械臂信息及包括障碍物的尺寸、形状及位置的机械臂工作环境信息；步骤S2：以机械臂起始位置Q
init
作为根节点构建起始搜索树StartT，以机械臂目标位置Q
goal
作为根节点构建目标搜索树GoalT；所述起始搜索树是一个以Q
init
为根节点且可以不断扩展的节点集motionS；所述目标搜索树是一个以Q
goal
为根节点且可以不断扩展的节点集motionG；起始搜索树的扩展步长StartT_step和目标搜索树的扩展步长GoalT_step初始时都设为相同的步长值init_step；
步骤S3：采用目标偏置和metropolis接受准则在工作空间筛选随机采样点Q
rand
；设rand为在[0,1]内随机生成的数值，P
goal
为目标偏置概率；若rand＜P
goal
，则直接将目标点Q
goal
作为随机采样点Q
rand
；若rand≥P
goal
，则采用metropolis接受准则来筛选随机采样点Q
rand
，即若采样点满足P＝1，则接受此采样点，继续执行步骤S4；若P＝0，则此采样点不被算法接受，重新进行随机采样，直至采样点满足P＝1的情况；其中，metropolis接受准则如下：其中，Q
nearest
表示起始搜索树StartT中距离采样点Q
rand
最近的节点，Q
end
表示起始搜索树StartT上一次扩展时节点集motionS添加的节点，符号d(Q
A
,Q
B
)表示节点A和节点B的路径代价，即λ为采样点接受范围调节参数；λ值越大，则采样点接受范围越小；λ值越小，则采样点接受范围越大；步骤S4：在起始搜索树StartT上搜寻节点集motionS中距离Q
rand
最近的节点Q
nearest
；设起始搜索树StartT的步长为StartT_step，若d(Q
nearest
,Q

【专利技术属性】
技术研发人员：陈志勇，黄泽麟，曾德财，
申请(专利权)人：福州大学，
类型：发明
国别省市：