一种关节空间规划的冗余机械臂碰撞回避及路径规划方法技术

一种关节空间规划的冗余机械臂碰撞回避及路径规划方法，涉及冗余机械臂碰撞回避和路径规划技术领域，针对现有技术中在机械臂路径规划时，并不能兼顾回避复杂障碍物的碰撞和冗余机械臂的自碰撞，进而导致规划的路径会发生机械臂碰撞的问题，本申请舍弃了实时规划的需求，通过对关节空间之间旋转矩阵的合理利用，不仅增加了碰撞障碍物的类型，使其可以进行稍复杂障碍物的碰撞检测，同时也实现了运动过程中自碰撞的检测，得以兼顾，进而避免了机械臂的碰撞。的碰撞。的碰撞。

【技术实现步骤摘要】
一种关节空间规划的冗余机械臂碰撞回避及路径规划方法


[0001]本专利技术涉及冗余机械臂碰撞回避和路径规划
，具体为一种关节空间规划的冗余机械臂碰撞回避及路径规划方法。

技术介绍

[0002]机械臂的路径规划是使机械臂末端从起始点运动至目标点，运动过程中避免产生碰撞，得出各关节运动路径。路径规划可以分为关节空间路径规划和笛卡尔空间路径规划，非冗余机械臂可以采用笛卡尔空间路径规划，先计算出末端路径，再进行运动学求逆，得出各关节角度，常用的有位置级求逆和速度级求逆。笛卡尔空间路径规划广泛使用快速拓展随机树RRT算法，该算法可以考虑到机械臂的非完整性约束限制，可以搜索机械臂的整个工作空间，快速得到笛卡尔空间的路径。
[0003]冗余机械臂无法通过单个末端位姿得到机械臂各个关节角，需要额外给定条件，本质是限制自由度。目前的路径规划方法有限制自由度法、投影梯度法、人工势场法，投影梯度法需要建立性能指标函数，需要确定的避障性能准则函数，其在碰撞检测可以函数表达时，即只考虑简单障碍物(球和椭球)回避中可以实现，但是在冗余机械臂的工作环境中，通常需要考虑的障碍物可以包络为：球体、立方体、圆台、圆柱体，同时仍需考虑坐标实时改变的自碰撞，无法建立行之有效的性能指标函数；人工势场法严重依赖好的势能启发函数，但在冗余多自由度机械臂的势场建立时，会产生极其庞大的计算量，实现困难。因此，现有技术中在机械臂路径规划时，并不能兼顾回避复杂障碍物的碰撞和冗余机械臂的自碰撞，进而导致规划的路径会发生碰撞。

技术实现思路

[0004]本专利技术的目的是：针对现有技术中在机械臂路径规划时，并不能兼顾回避复杂障碍物的碰撞和冗余机械臂的自碰撞，进而导致规划的路径会发生机械臂碰撞的问题，提出一种关节空间规划的冗余机械臂碰撞回避及路径规划方法。
[0005]本专利技术为了解决上述技术问题采取的技术方案是：
[0006]一种关节空间规划的冗余机械臂碰撞回避及路径规划方法，包括以下步骤：
[0007]步骤一：确定机械臂运动路径中的关节起点P
start
和终点P
goal
，所述起点P
start
和终点P
goal
为机械臂各个关节的角度；其中，P
start
和P
goal
都是长度为n的向量，n是冗余机械臂的关节数，n不小于7；
[0008]步骤二：确定机械臂工作环境中的障碍物，并将障碍物包络为规则形状，之后将包络后的障碍物表示在机械臂基坐标系下，所述规则形状包括圆柱体、球体、圆台、立方体；
[0009]步骤三：确定机械臂关节和臂杆的尺寸参数，以及机械臂上会产生自碰撞的部件，进而得到碰撞点集PSET
cc
；
[0010]步骤四：在关节和臂杆各自所在的关节空间中表示所需进行自碰撞检测的点，进而得到自碰撞点集PSET
csc
，自碰撞是指在运动过程中臂杆和关节与自碰撞部件产生碰撞；
[0011]步骤五：运用正运动学将PSET
cc
表示在机械臂基坐标系下，运用正运动学将PSET
csc
表示在自碰撞的障碍物所在的关节坐标系中，并通过快速拓展随机树RRT算法得到关节空间的路径。
[0012]进一步的，所述步骤三的具体步骤为：
[0013]步骤三一：确定机械臂关节和臂杆的尺寸参数，进而得到DH参数，之后获取自碰撞部件的尺寸参数；
[0014]步骤三二：根据DH参数以及自碰撞部件的尺寸参数将关节、臂杆以及自碰撞部件包络为圆柱体；
[0015]步骤三三：在关节、臂杆以及自碰撞部件各自所在的关节空间中表示需进行碰撞检测的点，即在圆柱体外表面上均匀等步长地取若干点表示在所在关节空间中，并记录于碰撞点集PSET
cc
中，PSET
cc
中前三行为笛卡尔坐标，第四行为1，第五行为对应点所在的关节号，取值范围(1，n)：
[0016][0017]其中，P
x
、P
y
、P
z
为检测点的坐标，j为该点所在的关节号，P
x1
、P
y1
、P
z1
为第一个点的坐标，P
xp
、P
yp
、P
zp
为最后一个点的坐标，碰撞点集PSET
cc
为5*p的矩阵，p为点的个数。
[0018]进一步的，所述步骤四的具体步骤为：在关节和臂杆各自所在的关节空间中表示所需进行自碰撞检测的点，即在圆柱体外表面上均匀等步长地取若干点表示在所在关节空间中，记录于自碰撞的点集PSET
csc
中，其中，自碰撞点集PSET
csc
为5*q的矩阵，q为点的个数，自碰撞是指在运动过程中臂杆和关节与自碰撞部件产生碰撞。
[0019]进一步的，所述机械臂上会产生自碰撞的部件包括末端操作装置、手眼相机。
[0020]进一步的，所述步骤五中通过快速拓展随机树RRT算法得到关节空间的路径的具体步骤为：
[0021]步骤五一：构建路径随机树矩阵RRT，并将机械臂运动路径的起点P
start
表示为[P
start
0]，之后将[P
start 0]输入RRT中，其中，RRT为m行n+1列，其中m为RRT中n维向量的个数，RRT中最后一列的元素为索引号；
[0022]步骤五二：以RRT中随机一行形成一个点X
branch
，并将X
branch
作为起点，其中X
branch
为1行n列的向量，之后设定概率，并通过设定的概率生成一个指向随机的n维生长向量或生成一个朝向P
goal
的n维生长向量，生成一个指向随机的n维生长向量的概率和生成一个朝向P
goal
的n维生长向量的概率之和为1，最后根据P
goal
并结合X
branch
生成生长向量的末端点X
rand
，X
rand
表示为：
[0023][0024]其中，X
rand
为即将生成的随机点，rand(1，n)为生成1行n列的大小在0
‑
1之间的随机数，X
branch
为RRT中的随机一点，P为设定的概率；
[0025]步骤五三：在RRT中寻找距离X
rand
最近的点，并将距离X
rand
最近的点作为新路径点
的父节点X
near
，在RRT中寻找距离X
rand
最近的点具体表示为：
[0026][0027]其中，Dis为RRT中点距离X
rand
的n维欧氏距离，i为RRT的行索引，将距离最近的父节点X...

【技术保护点】

【技术特征摘要】
1.一种关节空间规划的冗余机械臂碰撞回避及路径规划方法，其特征在于包括以下步骤：步骤一：确定机械臂运动路径中的关节起点P
start
和终点P
goal
，所述起点P
start
和终点P
goal
为机械臂各个关节的角度；其中，P
start
和P
goal
都是长度为n的向量，n是冗余机械臂的关节数，n不小于7；步骤二：确定机械臂工作环境中的障碍物，并将障碍物包络为规则形状，之后将包络后的障碍物表示在机械臂基坐标系下，所述规则形状包括圆柱体、球体、圆台、立方体；步骤三：确定机械臂关节和臂杆的尺寸参数，以及机械臂上会产生自碰撞的部件，进而得到碰撞点集PSET
cc
；步骤四：在关节和臂杆各自所在的关节空间中表示所需进行自碰撞检测的点，进而得到自碰撞点集PSET
csc
，自碰撞是指在运动过程中臂杆和关节与自碰撞部件产生碰撞；步骤五：运用正运动学将PSET
cc
表示在机械臂基坐标系下，运用正运动学将PSET
csc
表示在自碰撞的障碍物所在的关节坐标系中，并通过快速拓展随机树RRT算法得到关节空间的路径。2.根据权利要求1所述的一种关节空间规划的冗余机械臂碰撞回避及路径规划方法，其特征在于所述步骤三的具体步骤为：步骤三一：确定机械臂关节和臂杆的尺寸参数，进而得到DH参数，之后获取自碰撞部件的尺寸参数；步骤三二：根据DH参数以及自碰撞部件的尺寸参数将关节、臂杆以及自碰撞部件包络为圆柱体；步骤三三：在关节、臂杆以及自碰撞部件各自所在的关节空间中表示需进行碰撞检测的点，即在圆柱体外表面上均匀等步长地取若干点表示在所在关节空间中，并记录于碰撞点集PSET
cc
中，PSET
cc
中前三行为笛卡尔坐标，第四行为1，第五行为对应点所在的关节号，取值范围(1,n)：其中，P
x
、P
y
、P
z
为检测点的坐标，j为该点所在的关节号，P
x1
、P
y1
、P
z1
为第一个点的坐标，P
xp
、P
yp
、P
zp
为最后一个点的坐标，碰撞点集PSET
cc
为5*p的矩阵，p为点的个数。3.根据权利要求2所述的一种关节空间规划的冗余机械臂碰撞回避及路径规划方法，其特征在于所述步骤四的具体步骤为：在关节和臂杆各自所在的关节空间中表示所需进行自碰撞检测的点，即在圆柱体外表面上均匀等步长地取若干点表示在所在关节空间中，记录于自碰撞的点集PSET
csc
中，其中，自碰撞点集PSET
csc
为5*q的矩阵，q为点的个数，自碰撞是指在运动过程中臂杆和关节与自碰撞部件产生碰撞。4.根据权利要求3所述的一种关节空间规划的冗余机械臂碰撞回避及路径规划方法，其特征在于所述机械臂上会产生自碰撞的部件包括末端操作装置、手眼相机。5.根据权利要求4所述的一种关节空间规划的冗余机械臂碰撞回避及路径规划方法，
其特征在于所述步骤五中通过快速拓展随机树RRT算法得到关节空间的路径的具体步骤为：步骤五一：构建路径随机树矩阵RRT，并将机械臂运动路径的起点P
start
表示为[P
start
0]，之后将[P
start 0]输入RRT中,其中，RRT为m行n+1列，其中m为RRT中n维向量的个数，RRT中最后一列的元素为索引号；步骤五二：以RRT中随机一行形成一个点X
branch
，并将X
branch
作为起点，其中X
branch
为1行n列的向量，之后设定概率，并通过设定的概率生成一个指向随机的n维生长向量或生成一个朝向P
goal
的n维生长向量，生成一个指向随机的n维生长向量的概率和生成一个朝向P
goal
的n维生长向量的概率之和为1，最后根据P
goal
并结合X
branch
生成生长向量的末端点X
rand
，X
rand
表示为：其中，X
rand
为即将生成的随机点，rand(1,n)为生成1行n列的大小在0
‑
1之间的随机数，X
branch
为RRT中的随机一点，P为设定的概率；步骤五三：在RRT中寻找距离X
rand
最近的点，并将距离X
rand
最近的点作为新路径点的父节点X
near
，在RRT中寻找距离X
rand
最近的点具体表示为：其中，Dis为RRT中点距离X
rand
的n维欧氏距离，i为RRT的行索引，将距离最近的父节点X
near
的行号记录为i
near
；步骤五四：以X
near
、X
rand
和Step确定一个新的点X
new
，X
new
表示为：X
...

【专利技术属性】
技术研发人员：杨国财，韩旭，史士财，金明河，刘宏，曲希蕾，
申请(专利权)人：哈尔滨工业大学，
类型：发明
国别省市：