一种基于B样条曲线的双机器人协调装配的轨迹规划方法.pdf

本发明提供了一种基于B样条曲线的双机器人协调装配轨迹规划方法，属于轨迹规划领域；其包括有任务模块、主‑从机器人关节轨迹规划模块、避障策略模块、多目标优化模块、工业机器人驱动器。所述主‑从机器人关节轨迹规划模块发明了一种基于B样条曲线的双机器人协调装配关节轨迹规划方法和协调装配误差补偿的方法；所述避障策略模块发明了一种双机器人深度搜索避障策略；多目标优化模块发明了一种基于群体智能算法求解双机器人协调装配时间较少，装配轨迹平滑的运动学约束条件下的多目标规划方法；采用本发明的技术方案，可以得到装配时间较短，装配段关节轨迹平滑，成功避开障碍物且无干涉的双机器人协调装配轨迹，降低了成本，避免了装配件的损坏。

1.一种基于B样条曲线的双机器人协调装配的轨迹规划方法，其特征在于以下几个步
骤：
步骤1、建立双机器人的运动学模型，求得主-从机器人的正反解；
步骤2、由所述的任务模块获取主-从机器人的任务点位姿信息；
步骤3、建立主-从机器人协调系统的坐标系，通过坐标转换的方法得到主-从机器人的
末端坐标系相对于自身基座标系的变换矩阵；
步骤4、采用B样条曲线插值的方法对主-从机器人协调装配关节位置点进行插值，求得
双机器人协调装配关节轨迹，并进行误差补偿，即关节轨迹规划模块；
步骤5、执行双机器人深度搜索避障策略模块；
步骤6、确定协调装配轨迹的优化目标和约束条件，结合量子粒子群算法和避障策略进
行求解，即多目标规划模块；
步骤7、将多目标规划模块得到的关节空间信息发送给主-从机器人的驱动器。
2.根据权利要求1中所述的基于B样条曲线的双机器人协调装配的轨迹规划方法，其特
征在于：所述步骤4中，机器人协调装配轨迹规划的方法是：利用德布尔k次非均匀有理B样
条递推公式及r阶导矢变换公式构造主-从机器人B样条关节轨迹随时间变化的关节位移、
速度、加速度和加加速度曲线；此外，由于装配体之间的位姿约束和相对运动约束难以保
证，会产生装配误差，因此在基于B样条曲线插值后需要进行装配误差补偿。
3.根据权利要求1中所述的基于B样条曲线的双机器人协调装配的轨迹规划方法，其特
征在于：所述步骤5中，机器人与障碍物之间的避障策略如下：在机器人的连杆轴线上每隔
一定间距设置一个干涉检测点，并将干涉检测点各个区间段连杆的最大包络半径叠加到障
碍物的厚度上，从而将机器人与障碍物之间的碰撞转化为由一系列干涉检测点与扩充后的
障碍物间的位置判定关系。
4.根据权利要求1中所述的基于B样条曲线的双机器人协调装配的轨迹规划方法，其特
征在于：所述步骤5中，机器人与机器人之间的避障策略如下：将机器人简化为由连杆轴组
成的直线段，从而将机器人与机器人之间的碰撞简化为判断整个工作空间内空间中两条直
线段的最短距离与相应两条连杆的最大包络半径之和的大小判定关系。
5.在根据权利要求1中所述的基于B样条曲线的双机器人协调装配的轨迹规划方法，其
特征在于：所述步骤6中，利用量子粒子群算法优化双机器人轨迹方法如下：为了提高机器
人的装配效率、减小关节振动，保护机器人的驱动器及减速器等部件，建立在满足运动学约
束和双机器人无碰撞的条件下，装配效率最优和最优平滑轨迹问题的多目标优化模型；采
用罚函数法将有约束的目标函数转化为无约束目标函数；利用群体智能算法求解多目标优
化模型。
6.根据权利要求1中所述的基于B样条曲线的双机器人协调装配的轨迹规划方法，其特
征在于：所述步骤6中，双机器人深度搜索避障策略如下步骤：
S1、设置主-从机器人各自的起始点与目标点，令num1=num2=0；
S2、根据任务空间中机器人与障碍物存在的位置关系人为确定大致的避障中间点位
置，num1=num1+1；
S3、在避障中间点附近指定的搜索范围内进行启发式搜索，确定避障中间点，num2=
num2+1；
S4、分别对主-从机器人的起始点、中间点和目标点进行B样条插值，生成主-从机器人
的关节轨迹；
S5、通过避障检测方法判断整个工作时间内机器人和障碍物、双机器人之间是否发生
碰撞，若否，则输出主-从机器人的无碰关节轨迹，终止算法；若是，则执行步骤S6；
S6、判断num2是否小于num2max，若是，则执行步骤S3，若否，则执行步骤S7；
S7、判断num1是否小于num1max，若是，则执行步骤S2，若否，则没有成功搜索到主-从机
器人的无碰撞关节轨迹，，算法结束；
其中num1max为人为确定避障中间点位置的尝试次数，num2max为在避障中间点附近范
围内随机确定避障中间点的最大允许次数。
7.根据权利要求2中所述的基于B样条曲线的双机器人协调装配的轨迹规划方法，其特
征在于：所述装配误差定义如下：在定义装配的误差前首先建立装配体之间的配合坐标系，
以装配体A的轴线为Z轴，装配体A在轴线方向装配路径段的起始点O1为原点建立装配体A的
坐标系；以无装配误差时装配体B的配合面轴线为Z轴，装配体B在轴线方向装配路径段的起
始点O2为原点建立装配体B的坐标系；建立轴孔简化模型，可参考图2；
无误差装配情况下装配体B坐标系的Z轴始终与装配体A坐标系的Z轴重合，装配体A和
装配体B之间相对运动约束为：装配体A沿着装配体B坐标系的Z轴做直线运动；
根据装配任务要求在笛卡尔空间给定一系列相对世界坐标系的装配任务点序列；开始
进入协调装配时O1与O2重合；装配完成后O1与Pn重合，O2与P1重合；图2装配体A的原点O1在
装配体B的Z轴上的垂足；F为装配体B的原点O2在WP1的Z轴上的垂足；为了在双机器人协调
装配策略中消除装配误差，提高装配成功率，定义装配位置误差和装配姿态误差；
在装配路径段，选取两垂线（O1，E）至（O2，F）距离的最大值定义为装配位置误差
PositionError；将当前装配体A和装配体B之间姿态变换的欧拉角与理想欧拉角的差值取
绝对值求和定义为装配体定义装配姿态误差AttitudeError；
理想装配情况下，装配体B坐标系的Z轴始终与装配体A坐标系的Z轴重合，所以装配位
置误差PositionError为0；当前装配体A和装配体B之间装配姿态误差AttitudeError也为
0。
8.根据权利要求2中所述的基于B样条曲线的双机器人协调装配的轨迹规划方法，其特
征在于：所述装配误差补偿方法为：在协调装配路径段，主机器人始终按照B样条插值关节
轨迹运动；遍历从机器人最大装配误差，若小于预设精度，从机器人按照从机器人的B样条
插值关节轨迹运动；若大于预设精度，则从机器人按照运动学约束随从主机器人运动，进行
误差补偿，以免损坏装配体，并更新从机器人装配误差最大值。

一种基于B样条曲线的双机器人协调装配的轨迹规划方法

技术领域

本发明涉及轨迹规划领域。更具体地，涉及一种基于B样条曲线的双机器人协调装
配的轨迹规划方法。

背景技术

由于单机器人装配存在工作空间有限，容易存在装配盲区，灵活性不够，难以实现
全自动化装配作业等不足。因此，为了适应装配任务的复杂性和智能型，弥补单机器人装配
的不足，提出双机器人协调装配系统。双机器人协调装配系统虽然具有灵活性好、适应性高
的优点，但其复杂性也随之增加，双机器人系统的协调运动方案、机器人之间的避障算法、
双机器人的轨迹规划及其优化算法都是亟待解决的问题。

发明内容

为了解决上述问题，本发明旨在公开一种双机器人协调装配轨迹规划方法，尤指
一种基于B样条曲线获得运行时间最少、装配轨迹平滑、避开障碍物的多目标双机器人装配
轨迹规划的方法。

为实现上述目的，本发明所采用的技术方案主要包括以下步骤：

步骤1、建立双工业机器人的运动学模型，求得主-从工业机器人的正反解；

所述主机器人是指在协调装配阶段，关节位移、速度和加速度根据用运动学约束
来确定的夹持一个装配体的工业机器人；从机器人是指在协调装配阶段，关节运动随主机
器人的关节运动而变化的夹持另一个装配体的工业机器人。

步骤2、由所述的任务模块获取主-从机器人的任务点位姿信息；

所述任务模块是指给定主-从机器人具体的离散示教路径点，利用步骤1的结果可
以得到主-从机器人的任务点位姿信息。

步骤3、建立主-从机器人协调系统的坐标系，通过坐标转换的方法得到主-从机器
人的末端坐标系相对于自身基座标系的变换矩阵。

步骤4、采用B样条曲线插值的方法对主-从机器人协调装配关节位置点进行插值，
求得双机器人协调装配关节轨迹，并进行协调装配误差的补偿，即关节轨迹规划模块；

所述B样条曲线插值方法的数学模型为：

公式(1)中：Q₁(u)∈R^M×1和Q₂(u)∈R^M×1分别为归一化时刻u处主-从机器人关节的
位移，其中M为机器人的关节数目，d_j,1(u)∈R^M×1和d_j,2(u)∈R^M×1分别为主-从机器人B样条
关节轨迹曲线的控制点；为主从机器人B样条关
节轨迹曲线的归一化时间向量，n₁+1和n₂+1分别为主从机器人的任务插值点个数，N_j,k,1(u)
和N_j,k,2(u)分别为主-从机器人B样条关节轨迹曲线的k次规范B样条基函数，按照递推公式
(2)定义：

公式(2)中，R＝1表示主机器人，R＝2表示从机器人，i为样条序号，B样条基函数
N_i,k,R(u)的支承区间为[u_i,u_i+k+1],i＝0,1,...n；若运行时间为t_total，根据主-从机器人关
节轨迹的B样条插值曲线的控制点和节点矢量，将的德布尔递推公式变换可以得到曲线上
时刻t的r阶导矢，即：

当公式(3)中R＝1,2、r＝0,1,2,3时可以分别得到主-从机器人B样条关节轨迹随
时间变化的关节位移、速度、加速度和加加速度曲线；

由于装配体之间的位姿约束和相对运动约束难以保证，会产生装配误差，因此基
于B样条曲线插值后需要进行装配误差补偿；

所述协调装配误差补偿方法为：

公式(4)中PositionError为位置误差，AttitudeError为姿态误差，分别
为两垂线距离(可参考图2)，α、βγ、α、β^*、γ^*、^*为欧拉角。

步骤5、执行双机器人深度搜索避障策略模块；

所述双机器人深度搜索避障策略中机器人与障碍物之间的避障策略如下：在机器
人的连杆轴线上每隔一定间距设置一个干涉检测点，并将干涉检测点各个区间段连杆的最
大包络半径叠加到障碍物的厚度上，从而将机器人与障碍物之间的碰撞转化为由一系列干
涉检测点与扩充后的障碍物间的位置关系判定。

所述一系列干涉检测点与扩充后的障碍物间的位置关系判定方法如下：将机器人
简化为一系列干涉检测点后，判断整个工作时间内干涉检测点是否在扩充后的障碍物区域
内；若是，则连杆和障碍物有干涉情况，判定机器人与障碍物发生碰撞；否则继续判断其他
干涉检测点在整个工作时间内是否在扩充后的障碍物区域内；若所有干涉检测点在整个工
作时间内都不在扩充后的障碍物区域内，则判定机器人与障碍物没有发生碰撞，否则机器
人与障碍物发生碰撞(参考图3)。

所述双机器人深度搜索避障策略中机器人与机器人之间的避障策略如下：将机器
人简化为由连杆轴组成的直线段，从而将机器人与机器人之间的碰撞简化为判断整个工作
空间内空间中两条直线段的最短距离与相应两条连杆的最大包络半径之和的大小判定关
系。

所述简化判断整个工作空间内空间中两条直线段的最短距离与相应两条连杆的
最大包络半径之和的大小判定关系方法如下：计算主机器人连杆i的简化直线段与从机器
人连杆j的简化直线段之间的最短距离(计算方法参考空间中两条线段之间的最短距离的
计算方法)。判断在整个工作时间内最短距离是否小于连杆i和连杆j的最大包络半径之和，
若是，则连杆i与连杆j发生碰撞，判定双机器人之间发生碰撞；否则连杆i与连杆j没有发生
碰撞，继续判断主机器人其他连杆i与从机器人的连杆是否发生碰撞；当主机器人所有连杆
与从机器人所有连杆都没有发生碰撞，则判定双机器人之间没有发生碰撞(参考图4)。

所述双机器人深度搜索避障策略如下步骤：

S1、设置主-从机器人各自的起始点与目标点，令num1＝num2＝0；

S2、根据任务空间中机器人与障碍物存在的位置关系人为确定大致的避障中间点
位置，num1＝num1+1；

S3、在避障中间点附近指定的搜索范围内进行启发式搜索，确定避障中间点，num2
＝num2+1；

S4、分别对主-从机器人的起始点、中间点和目标点进行B样条插值，生成主-从机
器人的关节轨迹；

S5、通过避障检测方法判断整个工作时间内机器人和障碍物、双机器人之间是否
发生碰撞，若否，则输出主-从机器人的无碰关节轨迹，f_ob＝0终止算法；若是，则执行步骤
S6；

S6、判断num2是否小于num2max，若是，则执行步骤S3，若否，则执行步骤S7；

S7、判断num1是否小于num1max，若是，则执行步骤S2，若否，则没有成功搜索到主-
从机器人的无碰撞关节轨迹，f_ob＝+∞，算法结束；

其中num1max为人为确定避障中间点位置的尝试次数，num2max为在避障中间点附
近范围内随机确定避障中间点的最大允许次数，具体的算法执行流程参考图5。

步骤6、确定协调装配轨迹的优化目标和约束条件，结合量子粒子群算法和避障策
略进行求解，即多目标规划模块；

所述多目标数学模型为：

目标函数：F_G＝ω₁.f_t+ω₂.f_j+ω₃.f_ob (5)

约束条件：

公式(5～9)中：f_t为运行时间评价指标，衡量装配效率；f_j为关节平均脉动，作为轨
迹的平滑度评价指标；f_ob为碰撞评价指标；ω₁、ω₂、ω₃分别为f_t、f_j、f_ob三个评价指标的权
值系数。

所述量子粒子群算法数学模型为：

假设N是种群粒子数，粒子i的当前位置记为X_i＝(x_i1,x_i2,…,x_iD)，D是粒子的维
数。在每一次迭代中，粒子i搜索的最好位置记为pbest_i＝(pbest_i1,pbest_i2,…,pbest_iD)，
整个种群搜索的最好位置记为gbest_i＝(gbest_i1,gbest_i2,…,gbest_iD)。其中：

公式(10～11)中t是算法的当前迭代次数，r_1d(t)和r_2d(t)是(0,1)间的随机数；c₁
和c₂为PSO算法的学习因子，一般取c₁＝c₂，此时是均匀分布在(0,1)上的随机数；在粒
子i的当前最优位置pbest_i和群体的全局最优位置gbest_i为顶点构成的超矩形中按比
例随机选取一个位置作为粒子i的下一代的最好位置；此方法称为粒子群算法。

假设粒子群中粒子具有量子动力学的行为，由波函数ψ来确定每个粒子的位置，|ψ
|2是粒子位置的概率密度函数。假定在第t次迭代，粒子i在D维空间运动，该粒子在第d维的
势阱为pbestid(t)，则可以得到粒子i在第(t+1)次迭代的波函数为：

则粒子的概率密度函数Q为：

概率分布函数T为：

采用蒙特卡罗方法对粒子位置进行随机采样，得到在第(t+1)次迭代时，第i个粒
子第d维的位置分量：

公式(15)中uid(t)是均匀分布在(0,1)上的随机数。Lid(t)的值由公式(16)确定:

L_id(t)＝2α(t)|mbest_d(t)-x_id(t)| (16)

其中mbest称为平均最优位置，它是所有粒子自身最优位置的中心点，可由式(17)
计算得到：

则粒子的位置更新公式为：

x_id(t+1)＝pbest_id(t)±α(t)|mbest_id(t)-x_id(t)|ln[1/u_id(t)] (18)

公式(18)中，参数α称为压缩-扩张因子，直接影响粒子的收敛速度。该参数的值可
以是固定的，也可以是动态变化的，通常由下式确定：

公式(19)中a<b，b和a分别是控制参数α的初始值和终止值。

粒子的当前最优位置pbest_i的更新方式为：

全局最优位置gbest的更新方式为：

公式(21)中f为目标函数。将公式(18)作为粒子位置更新公式的粒子群算法即为
具有量子粒子群优化算法。

所述结合量子粒子群算法和避障策略进行求解方法流程参考图6。

步骤7、将多目标规划模块得到的关节空间信息发送给主-从机器人的驱动器。

采用上述双机器人协调装配轨迹规划方法，可以得到一条速度、加速度、加加速度
连续，装配时间较短，装配段关节轨迹平滑，成功避开障碍物且无干涉的双机器人协调装配
轨迹，降低了成本，避免了装配件的损坏。

附图说明

图1是本发明提出的基于量子粒子群算法和B样条曲线的双机器人协调装配的轨
迹规划方法的流程图；

图2是本发明提出的基于B样条曲线的双机器人协调装配的轨迹规划方法的协调
装配误差定义示意图；

图3是本发明提出的基于B样条曲线的双机器人协调装配的轨迹规划方法的机器
人与障碍物之间避障简化示意图；

图4是本发明提出的基于B样条曲线的双机器人协调装配的轨迹规划方法的机器
人与机器人之间避障简化示意图；

图5是本发明提出的基于B样条曲线的双机器人协调装配的轨迹规划方法的双机
器人深度搜索避障策略流程图；

图6是本发明提出的基于B样条曲线的双机器人协调装配的轨迹规划方法的量子
粒子群算法执行的流程图；

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面结合附图详细说明本发明
的具体实施方式：

参考图1，本发明公开了一种基于B样条曲线的双工业机器人协调装配的轨迹规划
方法，其包括有任务模块、主-从机器人关节轨迹规划模块、避障策略模块、多目标优化模
块、工业机器人驱动器。以下就以任务为实现典型的双工业机器人(六自由度)轴孔协调装
配任务为例进行描述。其他符合要求的任务均可按照本方案实施，具体的实施主要步骤如
下：

步骤1、建立双工业机器人的运动学模型，求得主-从工业机器人的正反解；

所述运动学模型是指可以通过D-H方法分别建立主-从机器人各个连杆坐标系，通
过D-H参数表和坐标系变化矩阵推导出运动学正解，然后通过PIPER法推导出运动学逆解。

步骤2、由所述的任务模块获取主-从机器人的任务点位姿信息。

双机器人协调装配任务一般分为两个阶段：第一阶段为夹取工件；第二阶段为协
调装配阶段；第一阶段只需要双机器人各自绕过障碍物夹取装配体并且运动到指定的装配
位置；第二阶段需要双机器人在保持既定的相对装配约束下必须严格经过离散的装配路径
点。通过示教获取离散装配路径点，并通过步骤1得到的双机器人运动学逆解公式去求任务
点姿态。

步骤3、建立主-从机器人协调系统的坐标系，通过坐标转换的方法得到主-从机器
人的末端坐标系相对于自身基座标系的变换矩阵。

所述主-从机器人协调系统包括双机器人的基座标系、末端坐标系、工具坐标系、
装配任务点坐标系；主-从机器人组成两条运动链所形成的封闭环链：一条是“主机器人的
基座-主机器人末端—装配体A-装配任务点”的运动链，另外一条是“主机器人基座-从机器
人基座-从机器人末端-装配体B-装配任务点”的运动链，根据两条运动链，可以得到主-从
机器人末端坐标系相对于自身基座标系的变换矩阵，进而根据主-从机器人的逆运动学求
解出相应的关节角，从而驱使双机器人进行协调装配运动。

步骤4、采用B样条曲线插值的方法对主-从机器人协调装配关节位置点进行插值，
求得双机器人协调装配关节轨迹，并进行误差补偿，即关节轨迹规划模块；

根据步骤1～3可以得到主-从机器人装配任务路径点相对应的关节角度，为了实
现主-从机器人关节轨迹的平滑性，采用B样条曲线插值的方法进行插值。具体插值步骤为：

S1、采用累积弦长法对节点时间归一化，得到主机器人k次B样条轨迹曲线的节点
向量：其中

S2、将带入求
得主机器人k次B样条轨迹曲线满足插值条件的n₁+1个方程；

S3、结合主机器人切矢边界条件，确定主机器人的B样条关节轨迹；

S4、同步骤S1，采用累积弦长法，得到从机器人k次B样条轨迹曲线的节点向量；

S5、同S2，将S3得到节点向量带入向量
求得主机器人k次B样条轨迹曲线满足插值条件的n₂+1个方程；

S6、同步骤S3，结合从机器人切矢边界条件，确定从机器人的B样条关节轨迹；

S7、将主-从机器人运动学约束转化为B样条控制点的约束；

S8、遍历从机器人B样条轨迹的时间区间内的最大装配位置误差，若小于预设的精
度，则执行从机器人的B样条曲线插值功能；若小于预设精度，则从机器人根据运动学约束
随从主机器人运动。

步骤5、执行双机器人深度搜索避障策略模块；

S1、设置主-从机器人各自的起始点与目标点，令num1＝num2＝0；

S2、根据任务空间中机器人与障碍物存在的位置关系人为确定大致的避障中间点
位置，num1＝num1+1；

S3、在避障中间点附近指定的搜索范围内进行启发式搜索，确定避障中间点，num2
＝num2+1；

S4、分别对主-从机器人的起始点、中间点和目标点进行B样条插值，生成主-从机
器人的关节轨迹；

S5、通过避障检测方法判断整个工作时间内机器人和障碍物、双机器人之间是否
发生碰撞，若否，则输出主-从机器人的无碰关节轨迹，f_ob＝0终止算法；若是，则执行步骤
S6；

S6、判断num2是否小于num2max，若是，则执行步骤S3，若否，则执行步骤S7；

S7、判断num1是否小于num1max，若是，则执行步骤S2，若否，则没有成功搜索到主-
从机器人的无碰撞关节轨迹，f_ob＝+∞，算法结束；

其中num1max为人为确定避障中间点位置的尝试次数，num2max为在避障中间点附
近范围内随机确定避障中间点的最大允许次数，具体的实现算法流程参考图5。

步骤6、确定协调装配轨迹的优化目标和约束条件，结合量子粒子群算法和避障策
略进行求解，即多目标规划模块；

所述结合量子粒子群算法和避障策略进行求解方法涉及到算法，可参考图6，具体
的算法执行步骤如下：

S1、初始化设置：在给定范围内对所有粒子的位置进行随机赋值；

S2、判断是否达到最大迭代次数，若是，则跳出算法；若否，则至S3；

S3、执行双机器人协调装配轨迹规划方法所构造的主-从B样条插值轨迹，即步骤
4；

S4、执行双机器人深度搜索策略算法，即步骤5；

S5、判断碰撞检测指标f_ob是否为0,；若是，则令粒子群算法适应度值为无穷大，至
S9；若否，至S6；

S6、用公式计算总运行时间；

S7、用公式计算平均脉动值；

S8、用公式F_G＝ω₁.f_t+ω₂.f_j+ω₃.f_ob计算适应度函数值；

S9、用公式x_id(t+1)＝pbest_id(t)±α(t)|mbest_id(t)-x_id(t)|ln[1/u_id(t)]更新粒
子位置，至S2；

通过算法的循环迭代，将会得到一条速度、加速度、加加速度连续，执行时间较短，
关节轨迹平滑的关节轨迹。

步骤7、将多目标规划模块得到的关节空间信息发送给主-从机器人的驱动器。