[发明专利]双机械臂协同运动方法及系统

权利要求书

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个关节的角度。