一种基于构型空间采样算法的机械臂运动规划方法技术

本发明专利技术公开了一种基于构型空间采样算法的机械臂运动规划方法，该方法包括：确定机械臂运动学模型；初始化构型空间，确定起始位姿与目标位姿对应的关节角；将笛卡尔空间中的地图向构型空间映射；根据优化后的采样算法在构型空间搜索并得到一条无碰撞路径

【技术实现步骤摘要】
一种基于构型空间采样算法的机械臂运动规划方法


[0001]本专利技术涉及机器人
，尤其涉及一种基于构型空间采样算法的机械臂运动规划方法
。

技术介绍

[0002]机械臂工作空间中，经常存在一些障碍物影响机械臂的正常运动
。
传统的示教方法在复杂场景下对机械臂进行路径规划效率低，因此需要引入智能路径规划算法来提高机械臂的路径规划效率
。
[0003]尽管常见的快速拓展随机树算法在机械臂路径规划中得到广泛应用，但该算法存在随机性强和无效搜索的问题，导致规划效率很低
。

技术实现思路

[0004]本专利技术所要解决的技术问题是克服现有技术的不足，提出一种基于构型空间采样算法的机械臂运动规划方法，能够解决传统快速扩展随机树算法规划效率低，路径代价大的技术问题
。
[0005]为达到上述目的，本专利技术是采用下述技术方案实现的：
[0006]第一方面，本专利技术提供了一种基于构型空间采样算法的机械臂运动规划方法，包括以下步骤：
[0007]步骤1，根据机械臂构型与几何参数进行运动学建模，建立运动学模型；
[0008]步骤2，初始化构型空间，基于建立的运动学模型，求解机械臂起始状态与目标状态对应的关节角，作为机械臂在构型空间中的起始节点与目标节点；
[0009]步骤3，通过蒙特卡洛法将笛卡尔空间中机械臂与障碍物的碰撞关系映射到构型空间；
[0010]步骤4，基于构型空间和机械臂在构型空间中的起始节点与目标节点，通过采样策略进行节点拓展，并对拓展的新节点进行父节点重选与区域重连，循环执行节点拓展，直到拓展的新节点进入目标节点的膨胀范围，生成无碰撞路径
。
[0011]进一步的，所述步骤1中，根据机械臂构型与几何参数进行运动学建模，建立运动学模型，包括：
[0012]建立机械臂
DH
运动学模型，将相邻杆件之间的位姿变换关系分解成两次旋转与两次平移运动，然后连乘得到机械臂末端相对于基坐标系的变换矩阵，包括：
[0013][0014][0015][0016][0017][0018][0019][0020]其中，关节与连杆参数
a
i
、
α
i
、
θ
i
、d
i
为
DH
参数
。
各个参数的定义与含义如下：
[0021]α
i
‑1：将
Z
i
‑1轴围绕
X
i
‑1轴旋转一定角度使得
Z
i
‑1与
Z
i
轴平行，该角度定义为连杆扭转角
α
i
；
[0022]a
i
‑1：将
Z
i
‑1轴沿
X
i
‑1轴移动一定距离使得
Z
i
‑1与
Z
i
轴重合，该距离定义为连杆长度
a
i
‑1；
[0023]θ
i
：将
X
i
‑1轴围绕
Z
i
旋转一定角度使得
X
i
‑1与
X
i
轴平行，该角度定义为关节角
θ
i
；
[0024]d
i
：将
X
i
‑1轴沿着
Z
i
移动一定距离使得
X
i
‑1与
X
i
轴重合，该距离定义为连杆偏距
d
i
。
[0025]s、c
是
sin、cos
的缩写
。
[0026]Rot(X,
α
)
：绕
X
轴旋转
α
角；
[0027]Trans(a,0,0)
：沿
X
轴平移
a。
[0028]其中，表示相邻杆件之间的位姿变换矩阵；
[0029]分别表示从基座到末端相邻杆件之间的位姿描述；
[0030]表示机械臂末端相对于底座基坐标系的描述；
[0031]r
11
、r
12
、r
13
、r
21
、r
22
、r
23
、r
31
、r
32
、r
33
、p
x
、p
y
、p
z
：分别对应于上述六个变换矩阵相乘后的结果，用于描述末端相对于基坐标系的位置与姿态
。
[0032]进一步的，所述步骤2中，初始化构型空间，基于建立的运动学模型，求解机械臂起始状态与目标状态对应的关节角，作为机械臂在构型空间中的起始节点与目标节点，包括：
[0033]确定机械臂在笛卡尔空间中的起始位姿与目标位姿；
[0034]将机械臂起始位姿与目标位姿通过所建立的运动学模型逆解到构型空间，求得对应的起始关节角与目标关节角，作为构型空间中的起始节点与目标节点，构型空间中的每一个节点坐标对应机械臂六个关节角数值
。
[0035]进一步的，所述步骤3中，通过蒙特卡洛法将笛卡尔空间中机械臂与障碍物的碰撞关系映射到构型空间，包括：
[0036]确定障碍物在笛卡尔空间中的位置，在笛卡尔空间通过蒙特卡洛法随机采集大量关节角样本并计算机械臂与障碍物的碰撞信息，然后将碰撞信息映射到机械臂构型空间
。
[0037]进一步的，所述步骤4中，基于构型空间和机械臂在构型空间中的起始节点与目标节点，通过采样策略进行节点拓展，并对拓展的新节点进行父节点重选与区域重连，循环执行节点拓展，直到拓展的新节点进入目标节点的膨胀范围，生成无碰撞路径，包括：
[0038]步骤
A
：从起始节点
q
start
开始在构型空间中随机采样获得随机节点
q
rand
，从距离随机节点最近的树节点
q
nearest
出发向
q
rand
方向经过一步长
λ
得到第一个新节点
q
new1
，判断当前路径是否发生碰撞，若未发生碰撞将该节点保存至节点树中；若发生碰撞，舍弃
q
new1
，重新采样；
[0039]步骤
B
：判断新节点
q
new1
是否进入目标节点
q
goal
的膨胀范围，进入膨胀范围后，直连
q
new1...

【技术保护点】

【技术特征摘要】
1.
一种基于构型空间采样算法的机械臂运动规划方法，其特征在于，包括以下步骤：根据机械臂构型与几何参数进行运动学建模，建立运动学模型；初始化构型空间，基于建立的运动学模型，求解机械臂起始状态与目标状态对应的关节角，作为机械臂在构型空间中的起始节点与目标节点；通过蒙特卡洛法将笛卡尔空间中机械臂与障碍物的碰撞关系映射到构型空间；基于构型空间和机械臂在构型空间中的起始节点与目标节点，通过采样策略进行节点拓展，并对拓展的新节点进行父节点重选与区域重连，循环执行节点拓展，直到拓展的新节点进入目标节点的膨胀范围，生成无碰撞路径
。2.
根据权利要求1所述的基于构型空间采样算法的机械臂运动规划方法，其特征在于，根据机械臂构型与几何参数进行运动学建模，建立运动学模型，包括：建立机械臂
DH
运动学模型，将相邻杆件之间的位姿变换关系分解成两次旋转与两次平移运动，然后连乘得到机械臂末端相对于基坐标系的变换矩阵，包括：移运动，然后连乘得到机械臂末端相对于基坐标系的变换矩阵，包括：移运动，然后连乘得到机械臂末端相对于基坐标系的变换矩阵，包括：移运动，然后连乘得到机械臂末端相对于基坐标系的变换矩阵，包括：移运动，然后连乘得到机械臂末端相对于基坐标系的变换矩阵，包括：
其中，关节与连杆参数
a
i
、
α
i
、
θ
i
、d
i
为
DH
参数；各个参数的定义与含义如下：
α
i
‑1：将
Z
i
‑1轴围绕
X
i
‑1轴旋转一定角度使得
Z
i
‑1与
Z
i
轴平行，该角度定义为连杆扭转角
α
i
；
a
i
‑1：将
Z
i
‑1轴沿
X
i
‑1轴移动一定距离使得
Z
i
‑1与
Z
i
轴重合，该距离定义为连杆长度
a
i
‑1；
θ
i
：将
X
i
‑1轴围绕
Z
i
旋转一定角度使得
X
i
‑1与
X
i
轴平行，该角度定义为关节角
θ
i
；
d
i
：将
X
i
‑1轴沿着
Z
i
移动一定距离使得
X
i
‑1与
X
i
轴重合，该距离定义为连杆偏距
d
i
；
s、c
是
sin、cos
的缩写；
Rot(X,
α
)
：绕
X
轴旋转
α
角；
Trans(a,0,0)
：沿
X
轴平移
a
；其中，表示相邻杆件之间的位姿变换矩阵；分别表示从基座到末端相邻杆件之间的位姿描述；表示机械臂末端相对于底座基坐标系的描述；
r
11
、r
12
、r
13
、r
21
、r
22
、r
23
、r
31
、r
32
、r
33
、p
x
、p
y
、p
z
：分别对应于上述六个变换矩阵相乘后的结果，用于描述末端相对于基坐标系的位置与姿态
。3.
根据权利要求1所述的基于构型空间采样算法的机械臂运动规划方法，其特征在于，初始化构型空间，基于建立的运动学模型，求解机械臂起始状态与目标状态对应的关节角，作为机械臂在构型空间中的起始节点与目标节点，包括：确定机械臂在笛卡尔空间中的起始位姿与目标位姿；将机械臂起始位姿与目标位姿通过所建立的运动学模型逆解到构型空间，求得对应的起始关节角与目标关节角，作为构型空间中的起始节点与目标节点，构型空间中的每一个节点坐标对应机械臂六个关节角数值
。4.
根据权利要求1所述的基于构型空间采样算法的机械臂运动规划方法，其特征在于，通过蒙特卡洛法将笛卡尔空间中机械臂与障碍物的碰撞关系映射到构型空间，包括：确定障碍物在笛卡尔空间中的位置，在笛卡尔空间通过蒙特卡洛法随机采集大量关节角样本并计算机械臂与障碍物的碰撞信息，然后将碰撞信息映射到机械臂构型空间
。5.
根据权利要求1所述的基于构型空间采样算法的机械臂运动规划方法，其特征在于，基于构型空间和机械臂在构型空间中的起始节点与目标节点，通过采样策略进行节点拓展，并对拓展的新节点进行父节点重选与区域重连，循环执行节点拓展，直到拓展的新节点
进入目标节点的膨胀范围，...

【专利技术属性】
技术研发人员：苑明海，舒瑞，孙英杰，梁振宇，顾文斌，裴凤雀，
申请(专利权)人：河海大学，
类型：发明
国别省市：