CN110695988A - 双机械臂协同运动方法及*** - Google Patents

Abstract

本发明公开了一种双机械臂协同运动方法及***，该方法包括：根据实体双机械臂构建虚拟双机械臂三维空间模型；对机械臂进行运动学建模；确定双机械臂的协作范围；根据实际需求控制虚拟双机械臂运动至目标位姿，并在运动过程中结合协作范围实时进行路径规划和碰撞检测，并将可行的路点保存至工控机的数据库中；建立工控机与实际双机械臂的通信；工控机加载数据库中可行的路点至实际双机械臂，控制其运动至目标位姿。***包括：工控机模块、通信模块、显示模块。本发明通过控制虚拟机械臂协同运动，并记录不会发生机械臂碰撞等问题的优选路点，根据这些路点控制实际机械臂运动至目标位姿，不会造成实际机械臂损伤，在机器人教学与实训等领域具有很好的应用价值。

Description

双机械臂协同运动方法及***

技术领域

本发明属于双机械臂机器人技术领域，特别涉及一种双机械臂协同运动方法及***。

背景技术

从1954年世界上第一台可编程机器人的诞生，到如今机器人与我们的生产生活息息相关，相关技术日益成熟。随着人们对各个领域机器人应用需求不断提高，双臂机器人应运而生。由于实现双机械臂协同运动难度较大，大部分技术还没有稳定、成熟，许多双机械臂的应用研究仍停留于实验室阶段，距离实际应用的普及还有一段距离。

在研究过程中，若直接操作机械臂，对操作人员要求较高，专业度不高或者新手很难直接上手操作，此外，双机械臂运动过程中存在一定的危险，直接操作实际双机械臂可能对操作人员的安全造成危害。

此外，双机械臂不同于单机械臂的特性，若操作人员将未经验证的算法或者路点直接加载到实际双机械臂，双机械臂在运行过程中很容易出现碰撞、损坏情况，造成经济损失。

发明内容

本发明的目的在于提供一种利用虚拟现实技术实现双机械臂协同运动的方法及***，避免直接操作实际机械臂造成实际机械臂损伤。

实现本发明目的的技术解决方案为：一种双机械臂协同运动方法，包括以下步骤：

步骤1、工控机根据实体双机械臂进行建模，构建包括周围环境信息的虚拟双机械臂三维空间模型，该模型的相关数据存至工控机模型库，并将虚拟双机械臂三维空间模型送至显示模块显示；

步骤2、工控机对机械臂进行运动学建模；

步骤3、工控机确定双机械臂的协作范围；

步骤4、工控机根据实际需求控制虚拟双机械臂运动至目标位姿，且在显示模块上实时显示虚拟双机械臂的关节角度和运动状态，并在运动过程中结合协作范围实时进行路径规划和碰撞检测，并将可行的路点保存至工控机的数据库中；

步骤5、建立工控机与实际双机械臂的通信；

步骤6、工控机加载数据库中可行的路点至实际双机械臂，控制其运动至所述目标位姿，在运动过程中实时将位姿信息反馈至工控机。

实现上述双机械臂协同运动方法的***，包括：

工控机模块，用于对双机械臂进行建模，并控制虚拟和实际双机械臂的运动；

通信模块，用于实现工控机模块与实际双机械臂的通信；

显示模块，用于显示虚拟双机械臂及其关节角度和运动状态。

本发明与现有技术相比，其显著优点为：1)***为双机械臂***运动控制的研究提供了直观的三维仿真环境，***仿真精确、动态性能好；2)利用不同的建模方法对实际双机械臂进行建模，减少了工作量，提高了操作人员的工作效率；3)工控机与实际双机械臂建立通信，可以自由切换虚拟双机械臂和实际双机械臂，将在虚拟仿真平台上试验得到的数据应用到实际***中，可以很好地解决实际应用中的路径规划、避障、防碰撞等问题；4)本发明可以应用在双机械臂研究或者实训教学等领域，后期也可以根据双机械臂不同的应用场景进行调试，应用范围广。

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

附图说明

图1为本发明双机械臂协同运动方法的流程图。

图2为本发明显示模块显示的虚拟双机械臂三维空间模型示意图。

具体实施方式

结合图1，本发明双机械臂协同运动方法，包括以下步骤：

步骤1、工控机根据实体双机械臂进行建模，构建包括周围环境信息的虚拟双机械臂三维空间模型，该模型的相关数据存至工控机模型库，并将虚拟双机械臂三维空间模型送至显示模块显示；

步骤2、工控机对机械臂进行运动学建模；

步骤3、工控机确定双机械臂的协作范围；

步骤4、工控机根据实际需求控制虚拟双机械臂运动至目标位姿，且在显示模块上实时显示虚拟双机械臂的关节角度和运动状态，并在运动过程中结合协作范围实时进行路径规划和碰撞检测，并将可行的路点保存至工控机的数据库中；

步骤5、建立工控机与实际双机械臂的通信；

步骤6、工控机加载数据库中可行的路点至实际双机械臂，控制其运动至所述目标位姿，在运动过程中实时将位姿信息反馈至工控机。

进一步地，步骤1中工控机根据实体双机械臂进行建模，包括：

(1)针对用户自定义设计的双机械臂，采用基于模型的建模方法进行建模；

(2)针对现有的双机械臂，利用其已有的模型数据直接进行建模。

进一步地，步骤2中工控机对机械臂进行运动学建模，具体为：基于齐次矩阵原理并结合机械臂D-H参数获取正逆运动学解，包括：

步骤2-1、基于D-H参数法，建立机械臂各连杆i的固连坐标系{O_i}；

步骤2-2、建立机械臂的平移变换矩阵与旋转变换矩阵，具体为：

坐标系{O_i}的原点相对于坐标系{O_i-1}的平移变换矩阵^i-1p_i为：

式中，ΔX,ΔY,ΔZ分别为坐标系{O_i}的原点相对于坐标系{O_i-1}的原点在X轴，Y轴，Z轴的平移距离；

坐标系{O_i}相对于坐标系{O_i-1}的旋转变换矩阵为

R，包括：

绕X轴的旋转变换矩阵为：

绕Y轴的旋转变换矩阵为：

绕Z轴的旋转变换矩阵为：

式中，θ为旋转角度，sθ,cθ分别为sinθ,cosθ的简写；

步骤2-3、根据步骤2-2中的平移变换矩阵与旋转变换矩阵坐标系获取{O_i}相对于坐标系{O_i-1}的位姿变换矩阵

为：

步骤2-4、根据步骤2-3的位姿变换矩阵

获得机械臂的正运动学方程为：

式中，θ_i为i关节的角度，{O_origin}为机械臂基座坐标系，

为机械臂基座到机械臂末端的位姿变换矩阵；

步骤2-5、基于解析法，求解机械臂的逆运动学方程的有效解，即

θ＝[θ₁,θ₂,…,θ_n-1,θ_n]。

进一步地，步骤3中工控机确定双机械臂的协作范围，具体为：

步骤3-1、针对每一个机械臂的每一关节i，对应的关节角度θ_i取值为：

θ_i＝θ_{i_min}+(θ_{i_max}-θ_{i_min})·Rand(n)

式中，θ_{i_max}、θ_{i_min}分别为i关节角度的最大、最小取值，Rand(n)为[0,1]的随机数；

步骤3-2、对步骤3-1中的Rand(n)取任意N个随机数，由此获得该机械臂末端的N组空间位置坐标；

步骤3-3、以某一个机械臂末端的N组空间位置坐标为基准，校正另一个机械臂末端的N组空间位置坐标，具体为：将所述某一个机械臂末端的N组空间位置坐标与双机械臂基座间的位置偏差相加，获得校正后的另一个机械臂末端的N组空间位置坐标；

步骤3-4、根据步骤3-3获得的每个机械臂对应的所有空间位置坐标，分别绘制点云图，点云图的交集即为协作范围。

进一步地，步骤4中工控机根据实际需求控制虚拟双机械臂运动至目标位姿，具体为：

步骤4-1、根据目标位姿，通过步骤2的逆运动学求解获得目标位姿各关节的角度；

步骤4-2、根据目标位姿各关节的角度，基于轨迹插值，实现各关节平稳运动至目标位姿。

进一步地，在其中一个实施例中，步骤4-2机械臂平稳运动过程中，由工控机设置位姿闭环控制器不断修正机械臂运动过程中产生的偏差。

进一步地，在其中一个实施例中，步骤4-2所述轨迹插值采用五次多项式插值法或B样条插值法。

进一步地，在其中一个实施例中，步骤5中工控机与实际双机械臂通过TCP/IP协议建立通信。

实现上述双机械臂协同运动方法的***，包括：

工控机模块，用于对双机械臂进行建模，并控制虚拟和实际双机械臂的运动；

通信模块，用于实现工控机模块与实际双机械臂的通信；

显示模块，用于显示虚拟双机械臂及其关节角度和运动状态；

进一步地，工控机模块包括：

机械臂建模单元，用于构建包括周围环境信息的虚拟双机械臂模型、进行运动学建模以及确定双机械臂协作范围；

机械臂控制单元，用于根据用户设定的目标位姿对虚拟双机械臂进行路径规划、碰撞检测，进而控制虚拟双机械臂运动；

存储单元，用于存储包括周围环境信息的虚拟双机械臂模型的数据、虚拟双机械臂仿真过程中的有效路点以及实际双机械臂的反馈数据。

本发明通过建立虚拟双机械臂模型，以三维动画模型形式全方位展示了机械臂的运动过程，通过人机交互控制虚拟机械臂协同运动，并记录不会发生机械臂碰撞等问题的优选路点，根据这些路点控制实际机械臂运动至目标位姿，不会造成实际机械臂损伤。本发明为双机械臂协同运动的研究奠定了良好的基础，在机器人教学与实训等领域具有很好的应用价值。

Claims (10)

1.一种双机械臂协同运动方法，其特征在于，包括以下步骤：

步骤1、工控机根据实体双机械臂进行建模，构建包括周围环境信息的虚拟双机械臂三维空间模型，该模型的相关数据存至工控机模型库，并将虚拟双机械臂三维空间模型送至显示模块显示；

步骤2、工控机对机械臂进行运动学建模；

步骤3、工控机确定双机械臂的协作范围；

步骤4、工控机根据实际需求控制虚拟双机械臂运动至目标位姿，且在显示模块上实时显示虚拟双机械臂的关节角度和运动状态，并在运动过程中结合协作范围实时进行路径规划和碰撞检测，并将可行的路点保存至工控机的数据库中；

步骤5、建立工控机与实际双机械臂的通信；

步骤6、工控机加载数据库中可行的路点至实际双机械臂，控制其运动至所述目标位姿，在运动过程中实时将位姿信息反馈至工控机。

2.根据权利要求1所述的双机械臂协同运动方法，其特征在于，步骤1所述工控机根据实体双机械臂进行建模，包括：

(1)针对用户自定义设计的双机械臂，采用基于模型的建模方法进行建模；

(2)针对现有的双机械臂，利用其已有的模型数据直接进行建模。

3.根据权利要求1所述的双机械臂协同运动方法，其特征在于，步骤2所述工控机对机械臂进行运动学建模，具体为：基于齐次矩阵原理并结合机械臂D-H参数获取正逆运动学解，包括：

步骤2-1、基于D-H参数法，建立机械臂各连杆i的固连坐标系{O_i}；

步骤2-2、建立机械臂的平移变换矩阵与旋转变换矩阵，具体为：

坐标系{O_i}的原点相对于坐标系{O_i-1}的平移变换矩阵^i-1p_i为：

式中，ΔX,ΔY,ΔZ分别为坐标系{O_i}的原点相对于坐标系{O_i-1}的原点在X轴，Y轴，Z轴的平移距离；

坐标系{O_i}相对于坐标系{O_i-1}的旋转变换矩阵为

包括：

绕X轴的旋转变换矩阵为：

绕Y轴的旋转变换矩阵为：

绕Z轴的旋转变换矩阵为：

式中，θ为旋转角度，sθ,cθ分别为sinθ,cosθ的简写；

步骤2-3、根据步骤2-2中的平移变换矩阵与旋转变换矩阵坐标系获取{O_i}相对于坐标系{O_i-1}的位姿变换矩阵

为：

步骤2-4、根据步骤2-3的位姿变换矩阵

获得机械臂的正运动学方程为：

式中，θ_i为i关节的角度，{O_origin}为机械臂基座坐标系，

为机械臂基座到机械臂末端的位姿变换矩阵；

步骤2-5、基于解析法，求解机械臂的逆运动学方程的有效解，即

θ＝[θ₁,θ₂,…,θ_n-1,θ_n]

式中，θ₁,θ₂,…,θ_n-1,θ_n分别为机械臂n个关节的角度。

4.根据权利要求1所述的双机械臂协同运动方法，其特征在于，步骤3所述工控机确定双机械臂的协作范围，具体为：

步骤3-1、针对每一个机械臂的每一关节i，对应的关节角度θ_i取值为：

θ_i＝θ_{i_min}+(θ_{i_max}-θ_{i_min})·Rand(n)

式中，θ_{i_max}、θ_{i_min}分别为i关节角度的最大、最小取值，Rand(n)为[0,1]的随机数；

步骤3-2、对步骤3-1中的Rand(n)取任意N个随机数，由此获得该机械臂末端的N组空间位置坐标；

步骤3-3、以某一个机械臂末端的N组空间位置坐标为基准，校正另一个机械臂末端的N组空间位置坐标，具体为：将所述某一个机械臂末端的N组空间位置坐标与双机械臂基座间的位置偏差相加，获得校正后的另一个机械臂末端的N组空间位置坐标；

步骤3-4、根据步骤3-3获得的每个机械臂对应的所有空间位置坐标，分别绘制点云图，点云图的交集即为协作范围。

5.根据权利要求1或3所述的双机械臂协同运动方法，其特征在于，步骤4所述工控机根据实际需求控制虚拟双机械臂运动至目标位姿，具体为：

步骤4-1、根据目标位姿，通过步骤2的逆运动学求解获得目标位姿各关节的角度；

步骤4-2、根据目标位姿各关节的角度，基于轨迹插值，实现各关节平稳运动至目标位姿。

6.根据权利要求5所述的双机械臂协同运动方法，其特征在于，步骤4-2机械臂平稳运动过程中，由工控机设置位姿闭环控制器不断修正机械臂运动过程中产生的偏差。

7.根据权利要求5所述的双机械臂协同运动方法，其特征在于，步骤4-2所述轨迹插值采用五次多项式插值法或B样条插值法。

8.根据权利要求1所述的双机械臂协同运动方法，其特征在于，步骤5中工控机与实际双机械臂通过TCP/IP协议建立通信。

9.实现权利要求1-8任意一项所述的双机械臂协同运动方法的***，其特征在于，包括：

工控机模块，用于对双机械臂进行建模，并控制虚拟和实际双机械臂的运动；

通信模块，用于实现工控机模块与实际双机械臂的通信；

显示模块，用于显示虚拟双机械臂及其关节角度和运动状态。

10.根据权利要求9所述的双机械臂协同运动***，其特征在于，所述工控机模块包括：

机械臂建模单元，用于构建包括周围环境信息的虚拟双机械臂模型、进行运动学建模以及确定双机械臂协作范围；

机械臂控制单元，用于根据用户设定的目标位姿对虚拟双机械臂进行路径规划、碰撞检测，进而控制虚拟双机械臂运动；

存储单元，用于存储包括周围环境信息的虚拟双机械臂模型的数据、虚拟双机械臂仿真过程中的有效路点以及实际双机械臂的反馈数据。

Images