CN109910015A - 一种砂浆喷抹建筑机器人的末端路径规划算法 - Google Patents

Abstract

本发明涉及机器人运动规划技术领域，特别是一种砂浆喷抹建筑机器人的末端路径规划算法。包括以下过程：1)任务轨迹规划：确定机械臂执行末端从任务初始位置移动到任务终止位置的任务轨迹点，针对每个轨迹点求动力学逆解组，遍历每一个逆解，找到遍历成功率最高的路径作为最终的任务轨迹规划；2)自由轨迹规划：计算机械臂当前姿态的关节位移量，并利用任务轨迹规划计算得到初始姿态的关节位移量，建立机械臂执行末端从当前位置开始移动到任务初始位置的路径规划。采用上述方案，可以快速准确规划获得从任意机械臂当前位置到机械臂任务操作路径的运动轨迹，执行精度高，可以满足水泥砂浆喷抹任务需求。

Description

一种砂浆喷抹建筑机器人的末端路径规划算法

技术领域

本发明涉及机器人运动规划技术领域，特别是一种砂浆喷抹建筑机器人的末端路径规划算法。

背景技术

在常见的室内建筑施工中，原始的建筑墙面，例如砖墙以及混凝土墙面，需要通过喷抹水泥砂浆，从而达到墙面平面度、阴阳角垂直度等质量评估标准要求，便于后续的房屋装修处理。水泥砂浆的喷抹操作现在主要通过人工操作完成，施工效率低，劳动强度大。传统施工环境粉尘严重，工作环境恶劣，对施工工人造成严重的身体健康隐患。除此之外，因为专业技术水平差异，人工操作难以保证施工质量，存在墙面不平整，空鼓开裂等严重质量问题，给建筑施工的验收造成了极大的负面影响。

随着时代进步，工业技术不断发展，机器人在现代工业得到了越来越广泛的使用，本发明利用多自由度机械臂替代人工操作，完成建筑施工中的水泥砂浆喷抹任务。在利用机器人技术实现该任务的自动化过程中，一个亟需解决的问题就是高自由度机械臂在复杂建筑施工环境内的路径规划问题。在机械臂末端路径规划问题中，常见解决方法包括人工势场法、遗传算法、模拟退火算法、蚁群优化算法等。但是，在复杂操作环境内，上述方法复杂度较高，并且需要完成对工作环境的三维建模，不适合建筑施工环境内水泥砂浆喷抹任务的规划。

发明内容

本发明所要解决的技术问题是：针对上述存在的问题，提供了一种砂浆喷抹建筑机器人的末端路径规划算法。

本发明采用的技术方案如下：一种砂浆喷抹建筑机器人的末端路径规划算法，包括以下过程：

任务轨迹规划：确定机械臂执行末端从任务初始位置移动到任务终止位置的任务轨迹点，针对每个轨迹点求动力学逆解组，遍历每一个逆解，找到遍历成功率最高的路径作为最终的任务轨迹规划；

自由轨迹规划：计算机械臂当前姿态的关节位移量，并利用任务轨迹规划计算得到初始姿态的关节位移量，建立机械臂执行末端从当前位置开始移动到任务初始位置的路径规划。

进一步的，任务轨迹规划包括以下过程：

(a)根据墙面几何信息确定操作任务的路径，路径上的路径点表示为P＝[P_s,P₁,..P_i..,P_k,P_e]，P_s为路径上任务初始位置，P_e为路径上任务终止位置，P_i为第i个路径点，i∈[1,2…,k]；

(b)针对每一个路径点P_i，基于不同的位姿，得每个位姿对应的完整运动学逆解组C_i；

(c)遍历初始姿态的每一个逆解C_s,i，针对初始姿态的每一个逆解C_s,i，在路径点P₁的动力学逆解中寻找在关节空间内距离最近的动力学逆解C_1,j，其中j表示每个位姿对应的多个逆解的编号；依次类推，针对路径点P_i位姿的每一个逆解，在路径点P_i+1的动力学逆解中寻找在关节空间内距离最近的动力学逆解C_i+1,nn；如果距离最近的动力学逆解C_i+1,nn与上一个距离最近的动力学逆解C_i,j的距离超过阈值，则终止当前逆解序列的遍历；

(d)针对初始姿态的每一个逆解，按照步骤(c)的方法依次往后面的路径点遍历任务轨迹的路径中所有的位姿，直到所有路径点遍历完成或者中途终止机械臂运动；

(e)遍历的成功率表示为其中q为遍历终止时刻位置的索引数，n为动力学逆解组个数；选择遍历成功率高的路径作为最终的任务轨迹规划。

进一步的，所述步骤(c)中，利用二范数得到的关节空间内最近距离搜索方法为：

其中，C_i+1,nn表示在第i+1个姿态动力学逆解组C_i+1中找到的距离最近的动力学逆解，w_k表示第i+1个位置动力学逆解中第k个关节的位移量，表示第i个位姿中第j个动力学逆解C_i,j中第k个关节的配置。

进一步的，所述步骤(e)中，如果存在多个遍历成功率相同的路径，则选取初始关节配置与机械臂当前关节配置最接近的路径。

进一步的，自由轨迹规划包括以下过程：

(A)在六自由度空间内，首先获取当前姿态P_c的关节位移量w_c＝w_c,i其中i＝1,2,3,4,5,6，w_c,i表示各个关节的位移量，构造图数据结构对象G_c；然后利用任务轨迹规划计算得到的初始姿态P_s关节位移量w_s，构造图数据结构对象G_s；

(B)在关节空间中作随机点采样得到w_rand点，然后在图数据结构对象G_c中找到距离w_rand点最近的w_cn点；根据算法设定的关节空间移动步长λ，我们计算新位移量w_new如下：

并将其插入到图数据结构对象G_c中；

(C)按照步骤(B)，在G_s中找到距离w_rand点最近的w_sn点，然后根据同样的方法计算得到新未来量w_new并添加到G_s中；

(D)交替着顺序执行步骤(B)和步骤(C)的随机点采样扩展，直到图数据结构对象G_c和图数据结构对象G_s连接上为止，找出当前位置到初始位置的最短路径为最终的自由轨迹规划。

与现有技术相比，采用上述技术方案的有益效果为：采用本发明的技术方案，可以快速准确规划获得从任意机械臂初始位置到机械臂任务操作路径的运动轨迹，执行精度高，可以满足水泥砂浆喷抹任务需求。

附图说明

图1是喷抹轨迹的几字形轨迹示意图。

图2是喷抹轨迹的之字形轨迹示意图。

图3是对应一个末端的不同动力学逆解示意图。

图4是本发明规划自由轨迹示意图。

具体实施方式

下面结合附图对本发明做进一步描述。

一种砂浆喷抹建筑机器人的末端路径规划算法，对于不同的操作墙面和任务方式，先不管机械臂当前位置，首先生成满足工艺需求的任务执行轨迹；再从工作开始前机械臂的当前位置出发，完成当前位置到初始位置的自由路径规划：

任务轨迹规划：确定机械臂执行末端从任务初始位置移动到任务终止位置的任务轨迹点，针对每个轨迹点求动力学逆解组，遍历每一个逆解，找到遍历成功率最高的路径作为最终的任务轨迹规划；

自由轨迹规划：计算机械臂当前姿态的关节位移量，并利用任务轨迹规划计算得到初始姿态的关节位移量，建立机械臂执行末端从当前位置开始移动到任务初始位置的路径规划。

任务轨迹规划的实施例：

(a)根据墙面几何信息，墙面几何信息包括墙面的法向量n_wall，墙面相对于机械臂基座的距离d_wall，机器人在完成砂浆喷抹时机械臂末端需要垂直于墙面，即和法向量平行，而且砂浆喷抹时对机械臂基座离墙面的距离必须满足一定的要求，因此需要基于以上几何信息确定操作任务路径。以矩形的墙面为例，如图1所示，常见的喷抹轨迹有两种：之字形轨迹(如图1)与几字形轨迹(如图2)。图1和2中虚线表示机械臂末端需要遍历的任务路径，虚线上的点表示该轨迹上的路径点。路径上的路径点表示为P＝[P_s,P₁,..P_i..,P_k,P_e]，P_s为路径上任务初始位置，P_e为路径上任务终止位置，P_i为第i个路径点，i∈[1,2…,k]；

(b)针对每位置的路径点P_i，得到每个位姿对应的完整运动学逆解组C_i，由于多自由度机器人动力学模型特点，每个指定作业位姿均存在多个且数量未知的逆运动学解，即C_i＝C_i,1∶n，其中n为动力学逆解的数量，因此这里会得到多个逆解，得到的是一个动力学逆解组；对应一个末端位姿，会存在多个不同的动力学逆解并且差别显著。以图3中的两个动力学逆解为例，虚线圆圈标识出来的关节位置存在很大的差别，而类似的差别在不同的环境下将会对机器人的运动约束产生显著的影响。

(c)遍历初始姿态的每一个逆解C_s,i，针对初始姿态的每一个逆解C_s,i，在路径点P₁的动力学逆解中寻找在关节空间内距离最近的动力学逆解C_1,j，其中j表示每个位姿对应的多个逆解的编号；依次类推，针对路径点P_i位姿的每一个逆解，在路径点P_i+1的动力学逆解中寻找在关节空间内距离最近的动力学逆解C_i+1,nn；如果距离最近的动力学逆解C_i+1,nn与上一个距离最近的动力学逆解C_i,j的距离超过阈值，则终止当前逆解序列的遍历；

以六自由度机器人为例，利用二范数得到的关节空间内最近距离搜索方法为：

其中，C_i+1,nn表示在第i+1个姿态动力学逆解组C_i+1中找到的距离最近的动力学逆解，w_k表示第i+1个位置动力学逆解中第k个关节的位移量，表示第i个位姿中第j个动力学逆解C_i,j中第k个关节的配置。在搜索关节空间内最近邻的过程中，如果寻找出的最近邻动力学逆解C_i+1,nn与上一个距离最近的动力学逆解C_i,j距离过远，超过阈值σ_c，即：

‖C_i+1,nn-C_i,j‖₂>σ_c

则被认为无法完成两个动力学逆解之间的差异过大，、机械臂的运动无法继续。

(d)针对初始姿态的每一个逆解，按照步骤(c)的方法依次往后面的路径点遍历任务轨迹的路径中所有的位姿，直到所有路径点遍历完成或者中途终止机械臂运动；

(e)假设规划终止时刻的姿态索引为q，计算从C_s,i开始时，路径遍历的百分比为：

当q＝n时，则路径遍历的成功率为100％，遍历轨迹被完整执行。

将R_i,s的集合定义为R_s，包含从P_s开始所有路径的遍历成功率，寻找出R_s中成功率最高的路径，并将其定义为候选的任务路径。若R_s中存在多个成功率相同的路径，则额外考虑机械臂当前的关节配置C_c，在成功率相同的多个路径中选取初始关节配置和当前关节配置C_c最接近的路径，并将其定义为唯一的候选路径，作为任务轨迹规划的路径。

任务轨迹规划的程序实现过程为：

1.根据已知的任务轨迹点P_i，调用FindOIKSolutions函数求得其对应的所有逆解。注意每个位姿可能会求得不定数量的多个逆运动学解，称其为一个逆解组。

2.利用每个逆解组，调用KDTree函数构建对应的KD树对象，便于加速之后的最近邻搜索任务。

3.依次遍历起始位姿P_s的每个逆解，循环调用KD树对象中的query查询函数，找到一个动力学逆解序列，以完成对于任务轨迹上路径点的遍历并调用ComputeSuccessRate函数计算该动力学逆解序列S的成功率。

4.调用FindMaxSuccessRate函数选出其中成功率最高的动力学逆解序列。如果同时存在多个动力学逆解序列具有相同的最高成功率，需要取出这些序列中的首个逆解，构建KDTree对象，然后比较选出其中距离当前机器人位姿最近的那个动力学逆解。该动力学逆解即为完成作业任务所需的初始关节空间值，也就是自由轨迹规划的目标关节姿态。

自由轨迹规划的实施例：

在完成对墙面喷抹任务的机械臂规划后，我们需要完成从机器人的当前姿态到轨迹初始姿态的规划。与上一个轨迹规划任务不同，在自由轨迹规划中，需要在关节空间内完成机械臂的轨迹规划。考虑到任务要求的实时性和稳定性，我们利用双向快速探索随机树(Bidirectional Rapid-Exploring Random Tree,BiRRT)算法完成自由轨迹规划，具体步骤如下：

(A)在六自由度空间内，首先获取当前姿态P_c的关节位移量w_c＝w_c,i其中i＝1,2,3,4,5,6，w_c,i表示各个关节的位移量，构造图数据结构对象G_c；然后利用任务轨迹规划计算得到的初始姿态P_s关节位移量w_s，构造图数据结构对象G_s；图数据结构中的顶点(vertex)标识某组关节位移量，对应于特定的末端执行机构姿态。图数据结构中的边(edge)表示两个顶点(两组关节位移量)之间可以完成移动。

(B)在关节空间中作随机点采样得到w_rand点，然后在图数据结构对象G_c中找到距离w_rand点最近的w_cn点；根据算法设定的关节空间移动步长λ，我们计算新位移量w_new如下：

并将其插入到图数据结构对象G_c中；

(C)按照步骤(B)，在G_s中找到距离w_rand点最近的w_sn点，然后根据同样的方法计算得到新未来量w_new并添加到G_s中；

(D)在双向构建两棵树，需要考虑两棵树的深度平衡，交替着顺序执行步骤(B)和步骤(C)的随机点采样扩展，直到两棵树彼此连接上为止，并在建立起的完整树结构上寻找出最短路径，作为最终的自由轨迹规划的路径。如图4所示的虚线为最终得到的自由轨迹。

自由轨迹规划的程序实现过程为：

1.获取随机采样的点w_rand；

2.从图G_c中选出距离w_rand的最近邻w_nn；

3.根据步长，添加点w_new到图中；

4.扩展完G_c的新节点w_new之后，以w_new作为目标方向扩展G_s；

5.每次迭代还需要考虑G_c和G_s的平衡性，交替次序选择“小”的那棵树进行扩展。

本发明并不局限于前述的具体实施方式。本发明扩展到任何在本说明书中披露的新特征或任何新的组合，以及披露的任一新的方法或过程的步骤或任何新的组合。如果本领域技术人员，在不脱离本发明的精神所做的非实质性改变或改进，都应该属于本发明权利要求保护的范围。

Claims (5)

1.一种砂浆喷抹建筑机器人的末端路径规划算法，其特征在于，包括以下过程：

任务轨迹规划：确定机械臂执行末端从任务初始位置移动到任务终止位置的任务轨迹点，针对每个轨迹点求动力学逆解组，遍历每一个逆解，找到遍历成功率最高的路径作为最终的任务轨迹规划；

自由轨迹规划：计算机械臂当前姿态的关节位移量，并利用任务轨迹规划计算得到初始姿态的关节位移量，建立机械臂执行末端从当前位置开始移动到任务初始位置的路径规划。

2.如权利要求1所述的砂浆喷抹建筑机器人的末端路径规划算法，其特征在于，任务轨迹规划包括以下过程：

(a)根据墙面几何信息确定操作任务的路径，路径上的路径点表示为P＝[P_s,P₁,..P_i..,P_k,P_e]，P_s为路径上任务初始位置，P_e为路径上任务终止位置，P_i为第i个路径点，i∈[1,2,...,k]；

(b)针对每一个路径点P_i，基于不同的位姿，得每个位姿对应的完整运动学逆解组C_i；

(c)遍历初始姿态的每一个逆解C_s,i，针对初始姿态的每一个逆解C_s,i，在路径点P₁的动力学逆解中寻找在关节空间内距离最近的动力学逆解C_1,j，其中j表示每个位姿对应的多个逆解的编号；依次类推，针对路径点P_i位姿的每一个逆解，在路径点P_i+1的动力学逆解中寻找在关节空间内距离最近的动力学逆解C_i+1,nn；如果距离最近的动力学逆解C_i+1,nn与上一个距离最近的动力学逆解C_i,j的距离超过阈值，则终止当前逆解序列的遍历；

(d)针对初始姿态的每一个逆解，按照步骤(c)的方法依次往后面的路径点遍历任务轨迹的路径中所有的位姿，直到所有路径点遍历完成或者中途终止机械臂运动；

(e)遍历的成功率表示为其中q为遍历终止时刻位置的索引数，n为动力学逆解组个数；选择遍历成功率高的路径作为最终的任务轨迹规划。

3.如权利要求2所述的砂浆喷抹建筑机器人的末端路径规划算法，其特征在于，所述步骤(c)中，利用二范数得到的关节空间内最近距离搜索方法为：

其中，C_i+1,nn表示在第i+1个姿态动力学逆解组C_i+1中找到的距离最近的动力学逆解，w_k表示第i+1个位置动力学逆解中第k个关节的位移量，表示第i个位姿中第j个动力学逆解C_i,j中第k个关节的配置。

4.如权利要求3所述的砂浆喷抹建筑机器人的末端路径规划算法，其特征在于，所述步骤(e)中，如果存在多个遍历成功率相同的路径，则选取初始关节配置与机械臂当前关节配置最接近的路径。

5.如权利要求4所述的砂浆喷抹建筑机器人的末端路径规划算法，其特征在于，自由轨迹规划包括以下过程：

(A)在六自由度空间内，首先获取当前姿态P_c的关节位移量w_c＝w_c,i其中i＝1,2,3,4,5,6，w_c,i表示各个关节的位移量，构造图数据结构对象G_c；然后利用任务轨迹规划计算得到的初始姿态P_s关节位移量w_s，构造图数据结构对象G_s；

(B)在关节空间中作随机点采样得到w_rand点，然后在图数据结构对象G_c中找到距离w_rand点最近的w_cn点；根据算法设定的关节空间移动步长λ，我们计算新位移量w_new如下：

并将其插入到图数据结构对象G_c中；

(C)按照步骤(B)，在G_s中找到距离w_rand点最近的w_sn点，然后根据同样的方法计算得到新位移量w_new并添加到G_s中；

(D)交替着顺序执行步骤(B)和步骤(C)的随机点采样扩展，直到图数据结构对象G_c和图数据结构对象G_s连接上为止，找出当前位置到初始位置的最短路径为最终的自由轨迹规划。