CN111216136A - 多自由度机械手臂控制系统、方法、存储介质、计算机 - Google Patents

多自由度机械手臂控制系统、方法、存储介质、计算机 Download PDF

Info

Publication number
CN111216136A
CN111216136A CN202010167333.9A CN202010167333A CN111216136A CN 111216136 A CN111216136 A CN 111216136A CN 202010167333 A CN202010167333 A CN 202010167333A CN 111216136 A CN111216136 A CN 111216136A
Authority
CN
China
Prior art keywords
cos
sin
rotation
mechanical arm
module
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202010167333.9A
Other languages
English (en)
Inventor
王罡
郭芳泽
吴定都
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Jimei University
Original Assignee
Jimei University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Jimei University filed Critical Jimei University
Priority to CN202010167333.9A priority Critical patent/CN111216136A/zh
Publication of CN111216136A publication Critical patent/CN111216136A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture

Abstract

本发明属于工业机器人技术领域，公开了一种多自由度机械手臂控制系统、方法、存储介质、计算机，建立坐标系；进行建立的坐标系之间的变换，将坐标点的参考系由上一个转换为下一个，确认末端点的位置坐标；确定ID6190旋转模块的旋转角度，使得机械手臂正向面对目标点，求解个各选转模块的旋转角度；通过对轨迹进行微分，逐段求解，然后通过调用机械手臂底层的控制接口实现整个联动控制过程；使用自带的EV‑MRobot三维仿真系统和Matlab的机器人工具箱，显示七自由度机械手臂的位姿。本发明避免机械臂出现同一空间坐标对应多个位姿的解；在联动运动过程进行优化，提高机械手臂联动的稳定性，减少抖动。

Description

多自由度机械手臂控制系统、方法、存储介质、计算机
技术领域
本发明属于工业机器人技术领域，尤其涉及一种多自由度机械手臂控制系统、方法、存储介质、计算机。
背景技术
目前在工业机器人领域中，广泛应用的机械手臂多是四自由度的。多自由度机械手臂在实际运用中普及度不高，存在求解复杂、联动运动抖动等问题。
综上所述，现有技术存在的问题是：目前多自由度机械手臂存在求解复杂、联动运动抖动。
发明内容
针对现有技术存在的问题，本发明提供了一种多自由度机械手臂控制系统、方法、存储介质、计算机。
本发明是这样实现的，一种多自由度机械手臂控制方法，所述多自由度机械手臂控制方法包括以下步骤：
第一步，统一3轴设置、统一坐标系建立坐标系；进行坐标系之间的变换，将坐标点的参考系由上一个转换为下一个，通过坐标系的叠加推导出末端点坐标方程组，确认末端点的位置坐标。推导过程详见表1，最终末端点位置方程组：在X轴上的等式：
X＝(-((L8+L7)*sin(f)*cos(e)*sin(d)+((L8+L7)*cos(f)+L6+L5)*cos(d)+L4+L3))*cos(a)-(-((-(((L8+L7)*sin(f)*cos(e)*cos(d)-((L8+L7)*cos(f)+L6+L5)*sin(d))*cos(c)-((L8+L7)*sin(f)*sin(e))*sin(c)))*cos(b)-((L8+L7)*sin(f)*cos(e)*sin(d)+((L8+L7)*cos(f)+L6+L5)*cos(d)+L4+L3)*sin(b)))*sin(a)
在Y轴上的等式：
Y＝(-((L8+L7)*sin(f)*cos(e)*sin(d)+((L8+L7)*cos(f)+L6+L5)*cos(d)+L4+L3))*sin(a)+(-((-(((L8+L7)*sin(f)*cos(e)*cos(d)-((L8+L7)*cos(f)+L6+L5)*sin(d))*cos(c)-((L8+L7)*sin(f)*sin(e))*sin(c)))*cos(b)-((L8+L7)*sin(f)*cos(e)*sin(d)+((L8+L7)*cos(f)+L6+L5)*cos(d)+L4+L3)*sin(b)))*cos(a)
在z轴上的等式：
z＝(-(((L8+L7)*sin(f)*cos(e)*cos(d)-((L8+L7)*cos(f)+L6+L5)*sin(d))*cos(c)-((L8+L7)*sin(f)*sin(e))*sin(c)))*sin(b)+((L8+L7)*sin(f)*cos(e)*sin(d)+((L8+L7)*cos(f)+L6+L5)*cos(d)+L4+L3)*cos(b)+L2+L1。
表1
Figure BDA0002407916890000021
Figure BDA0002407916890000031
表1：坐标解析推导过程
第二步，确定ID6190旋转模块的旋转角度，使得机械手臂正向面对目标点，求解个各选转模块的旋转角度；
空间定位逆推策略，分为两种：“分步法”、“分段法”。
分步法，将整个空间定位逆推过程分为“初步定位”和“精确定位”两步，初步定位过程完成机械手臂逼近目标点的过程，通过ID6190、ID6191旋转模块实现面对和相切像个环节，之后精确定位过程是实现剩余一个半径长度的空间距离的定位过程，通过ID6193、ID6194旋转模块调整长度，达到目标点。流程如图9分步法逆推策略流程图所示。
具体实现过程
2)初步定位1：根据末端手爪的位置点在XoY平面上的投影点P(X,Y)，通过三角关系，确定ID6190旋转模块的旋转角度：
Figure BDA0002407916890000032
使得机械手臂正向面对目标点，如图10初步定位1示意图。
初步定位2：获取末端手爪的位置点与基坐标原点距离，设置以末端手爪的位置点为球心，固定长度为半径的球面，然后通过空间位置关系确定ID6191旋转模块的旋转角度：
e＝d-c
即：
Figure BDA0002407916890000041
使得整个机械手臂与该球面相切，切点为ID6193的旋转中心，如图11初步定位2示意图。
3)精确定位：根据末端点位置方程组，带入已知旋转角度，得到三元二次方程组，用于控制机械手臂完成剩下一个半径长度的空间距离的定位，然后调用Matlab的solve()接口求解来获取ID6193、ID6194、ID6196旋转模块的旋转角度，最终实现逆推策略。如图12精确定位示意图。
设ID6193、ID6194、ID6196旋转模块的角度分别为d、f、e，可以列出方程组：
X＝(-(L7+L8)*sin(f)*sin(e))*cos(a)-(-((-((L7+L8)*sin(f)*cos(e)*cos(d)-((L7+L8)*cos(f)+L6+L5)*sin(d)))*cos(b)-((L7+L8)*sin(f)*cos(e)*sin(d)+((L7+L8)*cos(f)+L6+L5)*cos(d)+L4+L3)*sin(b)))*sin(a)
Y＝(-(L7+L8)*sin(f)*sin(e))*sin(a)+(-((-((L7+L8)*sin(f)*cos(e)*cos(d)-((L7+L8)*cos(f)+L6+L5)*sin(d)))*cos(b)-((L7+L8)*sin(f)*cos(e)*sin(d)+((L7+L8)*cos(f)+L6+L5)*cos(d)+L4+L3)*sin(b)))*cos(a)
z＝(-((L7+L8)*sin(f)*cos(e)*cos(d)-((L7+L8)*cos(f)+L6+L5)*sin(d)))*sin(b)+((L7+L8)*sin(f)*cos(e)*sin(d)+((L7+L8)*cos(f)+L6+L5)*cos(d)+L4+L3)*cos(b)+L2+L1
其中Li为各个旋转模块旋转中心之间的距离，a、b、c、g分别为ID6190、ID6191、ID6192、ID6195的旋转模块角度，都为常数。最后调用Matlab的solve()接口求解，从而得到全部模块的旋转角度，最终实现整个“分步法”逆推策略算法的实现。
进一步，分段法，首先是控制七自由度机械手臂，使其正向面对目标点位置，之后根据末端手爪的位置点与ID6191模块旋转中心的距离进行分区间分析，在各个区间内，控制某个旋转模块的旋转相应的角度，从而依次计算出剩余旋转模块的旋转角度。最后实现整个空间定位逆推算法。流程如图13分段法逆推流程图所示。
具体包括：
1)对末端手爪的位置点坐标进行投影到XOY平面上，得到相应的X,Y坐标，确定ID6190的旋转角度，控制机械手臂运动，使得整个机械手臂转到正向面对目标点的位置。与分步法的第一步一致。
2)根据末端手爪的位置点与ID6191模块旋转中心的距离进行分段分析，若在min到m1区间内，控制ID6194旋转模块为90°，然后根据空间位置关系，求解出ID6193和ID6191的旋转角度。若在m1到maX区间内，获取ID6191旋转模块的旋转角度，使得机械手臂指向目标位置，然后根据空间三角相对位置关系，依次求解出ID6193和ID6194的旋转角度，从而实现整个逆推过程，如图14“分段法”逆推示意图。
由图14可知，AF为七自由度机械手臂的初始位置，此时ID6191、ID6193、ID6194旋转模块的旋转角度为0°。a、b、d、c分别为ID6190到ID6191、ID6191到ID6193、ID6193到ID6194、ID6194到手爪之间的距离。
目标点距离ID6191模块旋转中心的最远距离maX：
Figure BDA0002407916890000051
即ID6193、ID6194旋转模块的旋转角度为0°时的距离，对应AF段。
目标点距离ID6191模块旋转中心的最近距离min：
Figure BDA0002407916890000052
即ID6193、ID6194旋转模块的旋转角度为90°时的距离，ABCDE段。
设置临界点J，目标点距离ID6191模块旋转中心的临界距离m1：
Figure BDA0002407916890000053
即ID6194旋转模块的旋转角度为90°时，机械手臂所能到达的最远距离，对应ABHIJ段。
第三步，通过对轨迹进行微分，逐段求解，然后通过调用机械手臂底层的控制接口MoveWithVel()和MoveToPos()实现整个联动控制过程，最后借助3D仿真系统验证正确性；联动控制绘制图形的效果中图15绘制正方形运行图一。图16绘制正方形效果图二。
第四步，使用自带的EV-MRobot三维仿真系统和Matlab的机器人工具箱，显示七自由度机械手臂的位姿，如图17仿真效果图。
进一步，所述第二步使用Matlab的solve接口求解三元二次方程组进行求解的。
进一步，所述第二步分段定位根据末端点距离ID6190中心距离进行分段求解。
进一步，所述第三步联动初步通过求解角度时间函数进行联动控制。
进一步，所述第三步使用逆推策略求解每一段的旋转模块角度，进一步优化联动控制。
本发明的另一目的在于提供一种接收用户输入程序存储介质，所存储的计算机程序使电子设备执行包括下列步骤：
第一步，统一3轴设置、统一坐标系建立坐标系；进行坐标系之间的变换，将坐标点的参考系由上一个转换为下一个，通过坐标系的叠加推导出末端点坐标方程组，确认末端点的位置坐标；
第二步，确定ID6190旋转模块的旋转角度，使得机械手臂正向面对目标点，求解个各选转模块的旋转角度；
第三步，通过对轨迹进行微分，逐段求解，然后通过调用机械手臂底层的控制接口MoveWithVel()和MoveToPos()实现整个联动控制过程，最后借助3D仿真系统验证正确性；
第四步，使用自带的EV-MRobot三维仿真系统和Matlab的机器人工具箱，显示七自由度机械手臂的位姿。
本发明的另一目的在于提供一种存储在计算机可读介质上的计算机程序产品，包括计算机可读程序，供于电子装置上执行时，提供用户输入接口以实施所述的多自由度机械手臂控制方法。
本发明的另一目的在于提供一种实施所述多自由度机械手臂控制方法的多自由度机械手臂控制系统，所述多自由度机械手臂控制系统包括：
空间定位正推模块，用于实现坐标系建立、坐标系交换、正推方程组；
空间定位逆推模块，用于得到七自由度机械手臂空间位姿状态；
空间定位联动模块，用于通过调用机械手臂底层的控制接口MoveWithVel()和MoveToPos()实现整个联动控制过程；
空间仿真模块，用于显示七自由度机械手臂的位姿，验证算法的正确性。
进一步，所述空间定位正推模块包括：
坐标系建立单元，用于实现统一3轴设置、统一坐标系的建立；
坐标系交换单元，用于进行坐标系之间的变换，将坐标点的参考系由上一个转换为下一个；
正推方程组处理单元，用于通过坐标系的叠加推导出末端点坐标方程组，确认末端点的位置坐标；
所述空间定位逆推模块包括：
分步处理单元，用于借助Matlab的solve接口求解三元二次方程组进行求解的；
分段处理单元，用于根据末端点距离ID6190中心距离进行分段求解；
所述空间定位联动模块包括：
联动初步处理单元，用于通过求解角度时间函数进行联动控制；
联动优化处理单元，用于使用逆推策略求解每一段的旋转模块角度。
本发明的另一目的在于提供一种搭载所述多自由度机械手臂控制系统的工业机器人。
综上所述，本发明的优点及积极效果为：本发明为机械手臂运动位姿求解提供一个快速计算的思路，通过约束条件限制机械臂的运动有效空间，避免机械臂出现同一空间坐标对应多个位姿的解；在联动运动过程进行优化，提高机械手臂联动的稳定性，减少抖动。
附图说明
图1是本发明实施例提供的多自由度机械手臂控制方法流程图。
图2是本发明实施例提供的多自由度机械手臂控制系统的结构示意图；
图中：1、空间定位正推模块；2、空间定位逆推模块；3、空间定位联动模块；4、空间仿真模块。
图3是本发明实施例提供的七自由度机械手臂的结构示意图。
图4是本发明实施例提供的空间定位正推模块示意图。
图5是本发明实施例提供的空间定位逆推模块示意图。
图6是本发明实施例提供的空间定位联动模块示意图。
图7是本发明实施例提供的空间仿真模块示意图。
图8是本发明实施例提供的多自由度机械手臂控制系统的原理图。
图9是本发明实施例提供的分步法逆推策略流程图。
图10是本发明实施例提供的初步定位1示意图。
图11是本发明实施例提供的初步定位2示意图。
图12是本发明实施例提供的精确定位示意图。
图13是本发明实施例提供的分段法逆推流程图。
图14是本发明实施例提供的分段法逆推示意图。
图15是本发明实施例提供的联动控制绘制图形的效果中绘制正方形运行图一。
图16是本发明实施例提供的绘制正方形效果图二。
图17是本发明实施例提供的仿真效果图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
针对现有技术存在的问题，本发明提供了一种多自由度机械手臂控制系统、方法、存储介质、计算机，下面结合附图对本发明作详细的描述。
如图1所示，本发明实施例提供的多自由度机械手臂控制方法包括以下步骤：
S101：统一3轴设置、统一坐标系建立坐标系；进行坐标系之间的变换，将坐标点的参考系由上一个转换为下一个，通过坐标系的叠加推导出末端点坐标方程组，确认末端点的位置坐标；
S102：确定ID6190旋转模块的旋转角度，使得机械手臂正向面对目标点，求解个各选转模块的旋转角度；借助Matlab的solve接口求解三元二次方程组来进行求解的，根据末端点距离ID6190中心距离进行分段求解的，得到七自由度机械手臂空间位姿状态；
S103：通过对轨迹进行微分，逐段求解，然后通过调用机械手臂底层的控制接口MoveWithVel()和MoveToPos()实现整个联动控制过程，最后借助3D仿真系统验证正确性；联动初步是通过求解角度时间函数进行联动控制，使用分段法逆推策略求解每一段的旋转模块角度，进一步优化联动控制；
S104：使用自带的EV-MRobot三维仿真系统和Matlab的机器人工具箱，显示七自由度机械手臂的位姿。
如图2所示，本发明实施例提供的多自由度机械手臂控制系统包括：
空间定位正推模块1，用于实现坐标系建立、坐标系交换、正推方程组。
空间定位逆推模块2，用于得到七自由度机械手臂空间位姿状态。
空间定位联动模块3，用于通过调用机械手臂底层的控制接口MoveWithVel()和MoveToPos()实现整个联动控制过程。
空间仿真模块4，用于显示七自由度机械手臂的位姿，验证算法的正确性。
下面结合附图对本发明的技术方案作进一步的描述。
本发明实施例提供的多自由度机械手臂控制系统包括：空间定位正推模块1、空间定位逆推模块2、空间定位联动模块3、空间仿真模块4。
空间定位正推模块1主要包括坐标系建立、坐标系交换、正推方程组三个小模块内容，其中坐标系建立有两个方法：统一3轴设置方法、统一坐标系的方法。
坐标系变换分为两个部分：平移变换和旋转变换，用于进行坐标系之间的变换，将坐标点的参考系由上一个转换为下一个，以此类推，通过坐标系的叠加来推导出末端点坐标方程组，确认末端点的位置坐标，最后借助3D仿真系统验证正确性。如图3。
定位逆推模块2主要包括“分步法”和“分段法”两个小模块，两个模块的共同基础是初步定位，即确定ID6190旋转模块的旋转角度，使得机械手臂正向面对目标点，最后去求解个各选转模块的旋转角度，“分步法”的精确定位模块借助Matlab的solve接口求解三元二次方程组来进行求解的，而“分段法”的分段定位是根据末端点距离ID6190中心距离来进行分段求解的。两种不同的方法都能得到七自由度机械手臂空间位姿状态，最后借助3D仿真系统来验证正确性。如图4。
空间定位联动模块3包括联动初步和联动优化两个小模块，两者的理论基础都是“微分”近似思想，通过对轨迹进行微分，逐段求解，然后通过调用机械手臂底层的控制接口MoveWithVel()和MoveToPos()来实现整个联动控制过程，最后借助3D仿真系统来验证正确性。
联动初步是通过求解角度时间函数来进行联动控制的，而联动优化是在此基础上，使用“分段法”逆推策略求解每一段的旋转模块角度，进一步优化联动控制，其稳定性能和分段的数量级相关，分段越小，稳定性越高。如图5。
空间仿真模块4主要包括使用自带的EV-MRobot三维仿真系统和Matlab的机器人工具箱，该模块用于显示七自由度机械手臂的位姿，验证算法的正确性。如图6。
本发明实施例提供的多自由度机械手臂控制系统包括：空间定位正推模块1、空间定位逆推模块2、空间定位联动模块3、空间仿真模块4。组成的模块关系图如图7。
图8是本发明实施例提供的多自由度机械手臂控制系统的原理图。
下面结合具体实施例对本发明作进一步描述。
实施例
第一步，统一3轴设置、统一坐标系建立坐标系；进行坐标系之间的变换，将坐标点的参考系由上一个转换为下一个，通过坐标系的叠加推导出末端点坐标方程组，确认末端点的位置坐标。推导过程详见表1，最终末端点位置方程组：在X轴上的等式：
X＝(-((L8+L7)*sin(f)*cos(e)*sin(d)+((L8+L7)*cos(f)+L6+L5)*cos(d)+L4+L3))*cos(a)-(-((-(((L8+L7)*sin(f)*cos(e)*cos(d)-((L8+L7)*cos(f)+L6+L5)*sin(d))*cos(c)-((L8+L7)*sin(f)*sin(e))*sin(c)))*cos(b)-((L8+L7)*sin(f)*cos(e)*sin(d)+((L8+L7)*cos(f)+L6+L5)*cos(d)+L4+L3)*sin(b)))*sin(a)
在Y轴上的等式：
Y＝(-((L8+L7)*sin(f)*cos(e)*sin(d)+((L8+L7)*cos(f)+L6+L5)*cos(d)+L4+L3))*sin(a)+(-((-(((L8+L7)*sin(f)*cos(e)*cos(d)-((L8+L7)*cos(f)+L6+L5)*sin(d))*cos(c)-((L8+L7)*sin(f)*sin(e))*sin(c)))*cos(b)-((L8+L7)*sin(f)*cos(e)*sin(d)+((L8+L7)*cos(f)+L6+L5)*cos(d)+L4+L3)*sin(b)))*cos(a)
在z轴上的等式：
z＝(-(((L8+L7)*sin(f)*cos(e)*cos(d)-((L8+L7)*cos(f)+L6+L5)*sin(d))*cos(c)-((L8+L7)*sin(f)*sin(e))*sin(c)))*sin(b)+((L8+L7)*sin(f)*cos(e)*sin(d)+((L8+L7)*cos(f)+L6+L5)*cos(d)+L4+L3)*cos(b)+L2+L1。
表1：坐标解析推导过程
Figure BDA0002407916890000121
Figure BDA0002407916890000131
第二步，确定ID6190旋转模块的旋转角度，使得机械手臂正向面对目标点，求解个各选转模块的旋转角度；
空间定位逆推策略，分为两种：“分步法”、“分段法”。
分步法，将整个空间定位逆推过程分为“初步定位”和“精确定位”两步，初步定位过程完成机械手臂逼近目标点的过程，通过ID6190、ID6191旋转模块实现面对和相切像个环节，之后精确定位过程是实现剩余一个半径长度的空间距离的定位过程，通过ID6193、ID6194旋转模块调整长度，达到目标点。流程如图9分步法逆推策略流程图所示。
具体实现过程：
初步定位1：根据末端手爪的位置点在XoY平面上的投影点P(X,Y)，通过三角关系，确定ID6190旋转模块的旋转角度：
Figure BDA0002407916890000132
使得机械手臂正向面对目标点，如图10。
初步定位2：获取末端手爪的位置点与基坐标原点距离，设置以末端手爪的位置点为球心，固定长度为半径的球面，然后通过空间位置关系确定ID6191旋转模块的旋转角度：
e＝d-c；
即：
Figure BDA0002407916890000133
使得整个机械手臂与该球面相切，切点为ID6193的旋转中心，如图11。
精确定位：根据末端点位置方程组，带入已知旋转角度，得到三元二次方程组，用于控制机械手臂完成剩下一个半径长度的空间距离的定位，然后调用Matlab的solve()接口求解来获取ID6193、ID6194、ID6196旋转模块的旋转角度，最终实现逆推策略。如图12。
设ID6193、ID6194、ID6196旋转模块的角度分别为d、f、e，可以列出方程组：
X＝(-(L7+L8)*sin(f)*sin(e))*cos(a)-(-((-((L7+L8)*sin(f)*cos(e)*cos(d)-((L7+L8)*cos(f)+L6+L5)*sin(d)))*cos(b)-((L7+L8)*sin(f)*cos(e)*sin(d)+((L7+L8)*cos(f)+L6+L5)*cos(d)+L4+L3)*sin(b)))*sin(a)
Y＝(-(L7+L8)*sin(f)*sin(e))*sin(a)+(-((-((L7+L8)*sin(f)*cos(e)*cos(d)-((L7+L8)*cos(f)+L6+L5)*sin(d)))*cos(b)-((L7+L8)*sin(f)*cos(e)*sin(d)+((L7+L8)*cos(f)+L6+L5)*cos(d)+L4+L3)*sin(b)))*cos(a)
z＝(-((L7+L8)*sin(f)*cos(e)*cos(d)-((L7+L8)*cos(f)+L6+L5)*sin(d)))*sin(b)+((L7+L8)*sin(f)*cos(e)*sin(d)+((L7+L8)*cos(f)+L6+L5)*cos(d)+L4+L3)*cos(b)+L2+L1
其中Li为各个旋转模块旋转中心之间的距离，a、b、c、g分别为ID6190、ID6191、ID6192、ID6195的旋转模块角度，都为常数。最后调用Matlab的solve()接口求解，从而得到全部模块的旋转角度，最终实现整个“分步法”逆推策略算法的实现。
分段法，首先是控制七自由度机械手臂，使其正向面对目标点位置，之后根据末端手爪的位置点与ID6191模块旋转中心的距离进行分区间分析，在各个区间内，控制某个旋转模块的旋转相应的角度，从而依次计算出剩余旋转模块的旋转角度。最后实现整个空间定位逆推算法。流程如图14。
具体实现过程
对末端手爪的位置点坐标进行投影到XOY平面上，得到相应的X,Y坐标，确定ID6190的旋转角度，控制机械手臂运动，使得整个机械手臂转到正向面对目标点的位置。与分步法的第一步一致。
根据末端手爪的位置点与ID6191模块旋转中心的距离进行分段分析，若在min到m1区间内，控制ID6194旋转模块为90°，然后根据空间位置关系，求解出ID6193和ID6191的旋转角度。若在m1到maX区间内，获取ID6191旋转模块的旋转角度，使得机械手臂指向目标位置，然后根据空间三角相对位置关系，依次求解出ID6193和ID6194的旋转角度，从而实现整个逆推过程，如图15。由图15可知，AF为七自由度机械手臂的初始位置，此时ID6191、ID6193、ID6194旋转模块的旋转角度为0°。a、b、d、c分别为ID6190到ID6191、ID6191到ID6193、ID6193到ID6194、ID6194到手爪之间的距离
目标点距离ID6191模块旋转中心的最远距离maX：
Figure BDA0002407916890000151
即ID6193、ID6194旋转模块的旋转角度为0°时的距离，对应AF段。
目标点距离ID6191模块旋转中心的最近距离min：
Figure BDA0002407916890000152
即ID6193、ID6194旋转模块的旋转角度为90°时的距离，ABCDE段。
设置临界点J，目标点距离ID6191模块旋转中心的临界距离m1：
Figure BDA0002407916890000153
即ID6194旋转模块的旋转角度为90°时，机械手臂所能到达的最远距离，对应ABHIJ段。
第三步，通过对轨迹进行微分，逐段求解，然后通过调用机械手臂底层的控制接口MoveWithVel()和MoveToPos()实现整个联动控制过程，最后借助3D仿真系统验证正确性；图16是联动控制绘制图形的效果图。
第四步，使用自带的EV-MRobot三维仿真系统和Matlab的机器人工具箱，显示七自由度机械手臂的位姿，如图17。
应当注意，本发明的实施方式可以通过硬件、软件或者软件和硬件的结合来实现。硬件部分可以利用专用逻辑来实现；软件部分可以存储在存储器中，由适当的指令执行系统，例如微处理器或者专用设计硬件来执行。本领域的普通技术人员可以理解上述的设备和方法可以使用计算机可执行指令和/或包含在处理器控制代码中来实现，例如在诸如磁盘、CD或DVD-ROM的载体介质、诸如只读存储器(固件)的可编程的存储器或者诸如光学或电子信号载体的数据载体上提供了这样的代码。本发明的设备及其模块可以由诸如超大规模集成电路或门阵列、诸如逻辑芯片、晶体管等的半导体、或者诸如现场可编程门阵列、可编程逻辑设备等的可编程硬件设备的硬件电路实现，也可以用由各种类型的处理器执行的软件实现，也可以由上述硬件电路和软件的结合例如固件来实现。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种多自由度机械手臂控制方法，其特征在于，所述多自由度机械手臂控制方法包括以下步骤：
第一步，统一3轴设置、统一坐标系建立坐标系；进行坐标系之间的变换，将坐标点的参考系由上一个转换为下一个，通过坐标系的叠加推导出末端点坐标方程组，确认末端点的位置坐标；
第二步，确定ID6190旋转模块的旋转角度，使得机械手臂正向面对目标点，求解个各选转模块的旋转角度；
第三步，通过对轨迹进行微分，逐段求解，然后通过调用机械手臂底层的控制接口MoveWithVel()和MoveToPos()实现整个联动控制过程，最后借助3D仿真系统验证正确性；
第四步，使用自带的EV-MRobot三维仿真系统和Matlab的机器人工具箱，显示七自由度机械手臂的位姿。
2.如权利要求1所述的多自由度机械手臂控制方法，其特征在于，所述第一步末端点位置方程组包括：
在X轴上的等式：
X＝(-((L8+L7)*sin(f)*cos(e)*sin(d)+((L8+L7)*cos(f)+L6+L5)*cos(d)+L4+L3))*cos(a)-(-((-(((L8+L7)*sin(f)*cos(e)*cos(d)-((L8+L7)*cos(f)+L6+L5)*sin(d))*cos(c)-((L8+L7)*sin(f)*sin(e))*sin(c)))*cos(b)-((L8+L7)*sin(f)*cos(e)*sin(d)+((L8+L7)*cos(f)+L6+L5)*cos(d)+L4+L3)*sin(b)))*sin(a)；
在Y轴上的等式：
Y＝(-((L8+L7)*sin(f)*cos(e)*sin(d)+((L8+L7)*cos(f)+L6+L5)*cos(d)+L4+L3))*sin(a)+(-((-(((L8+L7)*sin(f)*cos(e)*cos(d)-((L8+L7)*cos(f)+L6+L5)*sin(d))*cos(c)-((L8+L7)*sin(f)*sin(e))*sin(c)))*cos(b)-((L8+L7)*sin(f)*cos(e)*sin(d)+((L8+L7)*cos(f)+L6+L5)*cos(d)+L4+L3)*sin(b)))*cos(a)；
在z轴上的等式：
z＝(-(((L8+L7)*sin(f)*cos(e)*cos(d)-((L8+L7)*cos(f)+L6+L5)*sin(d))*cos(c)-((L8+L7)*sin(f)*sin(e))*sin(c)))*sin(b)+((L8+L7)*sin(f)*cos(e)*sin(d)+((L8+L7)*cos(f)+L6+L5)*cos(d)+L4+L3)*cos(b)+L2+L1。
3.如权利要求1所述的多自由度机械手臂控制方法，其特征在于，所述第二步求解个各选转模块的旋转角度中，采用空间定位逆推策略，包括：
分步法，将整个空间定位逆推过程分为初步定位和精确定位，初步定位过程完成机械手臂逼近目标点的过程，通过ID6190、ID6191旋转模块实现面对和相切像个环节，之后精确定位过程是实现剩余一个半径长度的空间距离的定位过程，通过ID6193、ID6194旋转模块调整长度，达到目标点；
分段法，首先是控制七自由度机械手臂，使正向面对目标点位置，之后根据末端手爪的位置点与ID6191模块旋转中心的距离进行分区间分析，在各个区间内，控制某个旋转模块的旋转相应的角度，从而依次计算出剩余旋转模块的旋转角度；最后实现整个空间定位逆推算法。
4.如权利要求3所述的多自由度机械手臂控制方法，其特征在于，所述分步法具体包括：1)初步定位1：根据末端手爪的位置点在XoY平面上的投影点P(X,Y)，通过三角关系，确定ID6190旋转模块的旋转角度：
Figure FDA0002407916880000021
使得机械手臂正向面对目标点；
初步定位2：获取末端手爪的位置点与基坐标原点距离，设置以末端手爪的位置点为球心，固定长度为半径的球面，然后通过空间位置关系确定ID6191旋转模块的旋转角度：
e＝d-c
即：
Figure FDA0002407916880000022
使得整个机械手臂与该球面相切，切点为ID6193的旋转中心；
1)精确定位：根据末端点位置方程组，带入已知旋转角度，得到三元二次方程组，用于控制机械手臂完成剩下一个半径长度的空间距离的定位，然后调用Matlab的solve()接口求解来获取ID6193、ID6194、ID6196旋转模块的旋转角度，最终实现逆推策略；
ID6193、ID6194、ID6196旋转模块的角度分别为d、f、e，列出方程组：
X＝(-(L7+L8)*sin(f)*sin(e))*cos(a)-(-((-((L7+L8)*sin(f)*cos(e)*cos(d)-((L7+L8)*cos(f)+L6+L5)*sin(d)))*cos(b)-((L7+L8)*sin(f)*cos(e)*sin(d)+((L7+L8)*cos(f)+L6+L5)*cos(d)+L4+L3)*sin(b)))*sin(a)；
Y＝(-(L7+L8)*sin(f)*sin(e))*sin(a)+(-((-((L7+L8)*sin(f)*cos(e)*cos(d)-((L7+L8)*cos(f)+L6+L5)*sin(d)))*cos(b)-((L7+L8)*sin(f)*cos(e)*sin(d)+((L7+L8)*cos(f)+L6+L5)*cos(d)+L4+L3)*sin(b)))*cos(a)；
z＝(-((L7+L8)*sin(f)*cos(e)*cos(d)-((L7+L8)*cos(f)+L6+L5)*sin(d)))*sin(b)+((L7+L8)*sin(f)*cos(e)*sin(d)+((L7+L8)*cos(f)+L6+L5)*cos(d)+L4+L3)*cos(b)+L2+L1；
其中Li为各个旋转模块旋转中心之间的距离，a、b、c、g分别为ID6190、ID6191、ID6192、ID6195的旋转模块角度，都为常数；最后调用Matlab的solve()接口求解，从而得到全部模块的旋转角度，最终实现整个分步法”逆推策略算法的实现。
5.如权利要求3所述的多自由度机械手臂控制方法，其特征在于，所述分段法具体包括：1)对末端手爪的位置点坐标进行投影到XOY平面上，得到相应的X,Y坐标，确定ID6190的旋转角度，控制机械手臂运动，使得整个机械手臂转到正向面对目标点的位置；
2)根据末端手爪的位置点与ID6191模块旋转中心的距离进行分段分析，若在min到m1区间内，控制ID6194旋转模块为90°，然后根据空间位置关系，求解出ID6193和ID6191的旋转角度；若在m1到maX区间内，获取ID6191旋转模块的旋转角度，使得机械手臂指向目标位置，然后根据空间三角相对位置关系，依次求解出ID6193和ID6194的旋转角度，从而实现整个逆推过程；
AF为七自由度机械手臂的初始位置，此时ID6191、ID6193、ID6194旋转模块的旋转角度为0°；a、b、d、c分别为ID6190到ID6191、ID6191到ID6193、ID6193到ID6194、ID6194到手爪之间的距离；
目标点距离ID6191模块旋转中心的最远距离maX：
Figure FDA0002407916880000041
即ID6193、ID6194旋转模块的旋转角度为0°时的距离，对应AF段；
目标点距离ID6191模块旋转中心的最近距离min：
Figure FDA0002407916880000042
即ID6193、ID6194旋转模块的旋转角度为90°时的距离，ABCDE段；
设置临界点J，目标点距离ID6191模块旋转中心的临界距离m1：
Figure FDA0002407916880000043
即ID6194旋转模块的旋转角度为90°时，机械手臂所能到达的最远距离，对应ABHIJ段。
6.如权利要求1所述的多自由度机械手臂控制方法，其特征在于，所述第三步，通过对轨迹进行微分，逐段求解，然后通过调用机械手臂底层的控制接口MoveWithVel()和MoveToPos()实现整个联动控制过程，最后利用3D仿真系统验证正确性；
所述第四步，使用自带的EV-MRobot三维仿真系统和Matlab的机器人工具箱，显示七自由度机械手臂的位姿。
7.一种接收用户输入程序存储介质，其特征在于，所存储的计算机程序使电子设备执行包括下列步骤：
第一步，统一3轴设置、统一坐标系建立坐标系；进行坐标系之间的变换，将坐标点的参考系由上一个转换为下一个，通过坐标系的叠加推导出末端点坐标方程组，确认末端点的位置坐标；
第二步，确定ID6190旋转模块的旋转角度，使得机械手臂正向面对目标点，求解个各选转模块的旋转角度；
第三步，通过对轨迹进行微分，逐段求解，然后通过调用机械手臂底层的控制接口MoveWithVel()和MoveToPos()实现整个联动控制过程，最后借助3D仿真系统验证正确性；
第四步，使用自带的EV-MRobot三维仿真系统和Matlab的机器人工具箱，显示七自由度机械手臂的位姿。
8.一种实施权利要求1～5任意一项所述多自由度机械手臂控制方法的多自由度机械手臂控制系统，其特征在于，所述多自由度机械手臂控制系统包括：
空间定位正推模块，用于实现坐标系建立、坐标系交换、正推方程组；
空间定位逆推模块，用于得到七自由度机械手臂空间位姿状态；
空间定位联动模块，用于通过调用机械手臂底层的控制接口MoveWithVel()和MoveToPos()实现整个联动控制过程；
空间仿真模块，用于显示七自由度机械手臂的位姿，验证算法的正确性。
9.如权利要求8所述的多自由度机械手臂控制系统，其特征在于，所述空间定位正推模块包括：
坐标系建立单元，用于实现统一3轴设置、统一坐标系的建立；
坐标系交换单元，用于进行坐标系之间的变换，将坐标点的参考系由上一个转换为下一个；
正推方程组处理单元，用于通过坐标系的叠加推导出末端点坐标方程组，确认末端点的位置坐标；
所述空间定位逆推模块包括：
分步处理单元，用于借助Matlab的solve接口求解三元二次方程组进行求解的；
分段处理单元，用于根据末端点距离ID6190中心距离进行分段求解；
所述空间定位联动模块包括：
联动初步处理单元，用于通过求解角度时间函数进行联动控制；
联动优化处理单元，用于使用逆推策略求解每一段的旋转模块角度。
10.一种搭载权利要求8所述多自由度机械手臂控制系统的工业机器人。
CN202010167333.9A 2020-03-11 2020-03-11 多自由度机械手臂控制系统、方法、存储介质、计算机 Pending CN111216136A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010167333.9A CN111216136A (zh) 2020-03-11 2020-03-11 多自由度机械手臂控制系统、方法、存储介质、计算机

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010167333.9A CN111216136A (zh) 2020-03-11 2020-03-11 多自由度机械手臂控制系统、方法、存储介质、计算机

Publications (1)

Publication Number Publication Date
CN111216136A true CN111216136A (zh) 2020-06-02

Family

ID=70807769

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010167333.9A Pending CN111216136A (zh) 2020-03-11 2020-03-11 多自由度机械手臂控制系统、方法、存储介质、计算机

Country Status (1)

Country Link
CN (1) CN111216136A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113580128A (zh) * 2021-07-09 2021-11-02 国网江西省电力有限公司电力科学研究院 四自由度机械臂控制方法及变电站消防机械臂控制方法
CN113715025A (zh) * 2021-09-10 2021-11-30 南京猫眼智能科技有限公司 自动拉伸机械手臂控制方法
CN114129392A (zh) * 2021-12-07 2022-03-04 山东大学 可调控末端指尖力的自适应冗余驱动外骨骼康复机器人

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2778285B2 (ja) * 1991-05-31 1998-07-23 松下電器産業株式会社 倣いセンサロボットシステム
CN103085069A (zh) * 2012-12-17 2013-05-08 北京邮电大学 一种新型机器人运动学建模方法
CN103968761A (zh) * 2014-05-28 2014-08-06 中科华赫（北京）科技有限责任公司 串联关节式机器人绝对定位误差校准方法及标定系统
CN104866722A (zh) * 2015-05-26 2015-08-26 宁波韦尔德斯凯勒智能科技有限公司 一种七轴工业机械臂的逆动学求解方法
CN105975795A (zh) * 2016-05-23 2016-09-28 湖北工业大学 一种高精度的多关节串联机械臂运动学反解解法
CN106426176A (zh) * 2016-11-08 2017-02-22 北京邮电大学 一种六自由度空间机械臂动态负载能力工作空间分析方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2778285B2 (ja) * 1991-05-31 1998-07-23 松下電器産業株式会社 倣いセンサロボットシステム
CN103085069A (zh) * 2012-12-17 2013-05-08 北京邮电大学 一种新型机器人运动学建模方法
CN103968761A (zh) * 2014-05-28 2014-08-06 中科华赫（北京）科技有限责任公司 串联关节式机器人绝对定位误差校准方法及标定系统
CN104866722A (zh) * 2015-05-26 2015-08-26 宁波韦尔德斯凯勒智能科技有限公司 一种七轴工业机械臂的逆动学求解方法
CN105975795A (zh) * 2016-05-23 2016-09-28 湖北工业大学 一种高精度的多关节串联机械臂运动学反解解法
CN106426176A (zh) * 2016-11-08 2017-02-22 北京邮电大学 一种六自由度空间机械臂动态负载能力工作空间分析方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
WANG GANG等: ""Analyses and Linkage Control Methods for Fast Implementation of Multiple Degree of Freedom Manipulators"", 《THE 11TH INTERNATIONAL CONFERENCE ON COMPUTER SCIENCE & EDUCATION (ICCSE 2016)》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113580128A (zh) * 2021-07-09 2021-11-02 国网江西省电力有限公司电力科学研究院 四自由度机械臂控制方法及变电站消防机械臂控制方法
CN113580128B (zh) * 2021-07-09 2022-08-16 国网江西省电力有限公司电力科学研究院 四自由度机械臂控制方法及变电站消防机械臂控制方法
CN113715025A (zh) * 2021-09-10 2021-11-30 南京猫眼智能科技有限公司 自动拉伸机械手臂控制方法
CN114129392A (zh) * 2021-12-07 2022-03-04 山东大学 可调控末端指尖力的自适应冗余驱动外骨骼康复机器人

Similar Documents

Publication Publication Date Title
CN108818535B (zh) 机器人3d视觉手眼标定方法
CN111216136A (zh) 多自由度机械手臂控制系统、方法、存储介质、计算机
WO2018086226A1 (zh) 机械臂的控制方法和装置
CN111369625B (zh) 定位方法、装置和存储介质
EP4045390B1 (en) Dynamic planning controller
EP3953114A1 (en) System and method for constraint management of one or more robots
CN113613850A (zh) 一种坐标系校准方法、装置和计算机可读介质
CN113715016B (zh) 一种基于3d视觉的机器人抓取方法、系统、装置及介质
CN113379849A (zh) 基于深度相机的机器人自主识别智能抓取方法及系统
WO2024037658A1 (zh) 机器人指向动作控制方法、装置、电子设备和存储介质
Corke et al. Robot arm kinematics
Cong et al. A new decoupled control law for image-based visual servoing control of robot manipulators
CN110561420A (zh) 臂型面约束柔性机器人轨迹规划方法及装置
CN112476435B (zh) 重力加速度方向的标定方法、标定装置及存储介质
Cong et al. A review and performance comparison of visual servoing controls
Kansal et al. Kinematic and dynamic analysis of a dexterous multi-fingered delta robot for object catching
EP3990233A1 (en) Inverse kinematic solver for wrist offset robots
Kim et al. Vision-force guided precise robotic assembly for 2.5 D components in a semistructured environment
He et al. A direct-drive SCARA robot for wafer&ceramic-substrate handling based on visual servoing
Feng et al. Research on SCARA robot sorting system based on CoDeSys
Zheng et al. An optimal kinematics calculation method for a multi-DOF manipulator
Borangiu et al. Constraints-based motion planning for an automatic, flexible laser scanning robotized platform
Li et al. Hard Disk Posture Recognition and Grasping Based on Depth Vision
CN109129558A (zh) 一种控制机械臂末端执行空间曲线动作的方法
Dong et al. A novel algorithm combined with single mapping workspace and Genetic Algorithm for solving inverse kinematic problem of redundant manipulators

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination