CN109397271B - 一种7自由度拟人机械臂及其控制方法和系统 - Google Patents
一种7自由度拟人机械臂及其控制方法和系统 Download PDFInfo
- Publication number
- CN109397271B CN109397271B CN201811415296.8A CN201811415296A CN109397271B CN 109397271 B CN109397271 B CN 109397271B CN 201811415296 A CN201811415296 A CN 201811415296A CN 109397271 B CN109397271 B CN 109397271B
- Authority
- CN
- China
- Prior art keywords
- joint motor
- joint
- freedom
- mechanical arm
- shoulder
- 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.)
- Active
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/02—Programme-controlled manipulators characterised by movement of the arms, e.g. cartesian coordinate type
-
- 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/10—Programme-controlled manipulators characterised by positioning means for manipulator elements
- B25J9/12—Programme-controlled manipulators characterised by positioning means for manipulator elements electric
-
- 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
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
本发明公开了一种7自由度拟人机械臂及其控制方法和系统，系统包括：7自由度拟人机械臂和控制模块，所述控制模块与7自由度拟人机械臂通信连接，所述控制模块，用于根据7自由度拟人机械臂的起始点和目标点，得到7自由度拟人机械臂的7个关节角度，然后进行路径规划，得到从起始点到目标点的一系列路径点，使用最小二乘法平滑多个路径点，再进行路径点之间的插补计算，进行轨迹生成，对每个关节电机进行轨迹跟踪，实时控制7自由度拟人机械臂运动。本发明的7自由度拟人机械臂具有冗余自由度，相比6自由度机械臂具有更多的解空间，可以得到可灵活避障的路径，保证人机协同作业时，具有更好的运动柔性、灵活性和适应性。
Description
技术领域
本发明属于机械臂领域，更具体地，涉及一种7自由度拟人机械臂及其控制方法和系统。
背景技术
随着现代工业应用场景对机械臂的工作空间限制及灵活度的要求越来越高，6个自由度是具有完成空间定位(3个位置量、3个姿态角)能力的最小自由度数，在越来越多的人机协同作业场合下，6自由度机械臂已无法满足更高的柔性和灵活性要求，对于运动学特性方面，6个自由度机械臂可能存在奇异构型、关节位移超限以及工作环境中存在障碍三大问题，使得机械臂的运动受到较大的限制；对于动力学特性方面，6个自由度机械臂存在关节力矩的分配可能不合理的情况；在容错性方面，若6个自由度机械臂中有一关节失效，则机械臂便无法继续完成工作。因此，增加自由度可改善机器人的运动学和动力学特性。
7自由度机械臂相比6自由度机械臂，多了1个冗余自由度，可以实现工作空间范围内的任意位置均可达且有无穷解，相比6自由度机械臂具有更多的解空间，改善了机器人的运动学和动力学特性，可以保证人机协同作业时，实现避障及人体安全防护等功能，对动态障碍物如运动的工作人员肢体等进行避让，因此，7自由度机械臂具有更好的运动柔性、灵活性和适应性。
目前，7自由度是可以实现灵活避障及解决奇异问题的最少自由度数要求。当7自由度机械臂机械结构不符合PIEPER准则(三个相邻关节轴相交于一点或三个相邻关节轴相互平行)时，则求解逆运动学时无法得到封闭解，即无法通过解析方法求解。由此可见，对于不符合PIEPER准则的7自由度机械臂无法通过解析方法求解、无法在保证机械臂在人机协同作业时，具有良好的运动柔性、灵活性和适应性。
同时，现有的机械臂路径规划后，直接利用得到的轨迹点，对7自由度机械臂进行实时运动控制，机械臂的运动过程不够平稳。
发明内容
针对现有技术的以上缺陷或改进需求，本发明提供了一种7自由度拟人机械臂及其控制方法和系统，由此解决对于不符合PIEPER准则的7自由度机械臂存在无法通过解析方法求解、无法在保证机械臂人机协同作业时，具有良好的运动柔性、灵活性和适应性的技术问题。
为实现上述目的，按照本发明的一个方面，提供了一种7自由度拟人机械臂，包括依次连接的夹爪、腕部、小臂连杆、肘部、大臂连杆和肩部，其中，肩部由肩部第一关节电机、肩部第二关节电机、肩部第三关节电机组成，肘部由肘部第一关节电机和肘部第二关节电机组成，腕部由腕部第一关节电机和腕部第二关节电机组成，相邻两关节电机轴线垂直相交。
腕部由2个关节电机组成，而不是人类腕部的3个自由度关节，解决了腕部关节电机自重造成的机械臂末端负载能力降低的问题，保证了机械臂的负载及自重的分布均衡化，既可以满足7自由度拟人机械臂的灵活运动要求，又可以保证其末端具有较大的负载能力。
进一步地，7自由度拟人机械臂在不包含夹爪时的总长度为600mm～700mm。
进一步地，7自由度拟人机械臂在不包含夹爪时的总重量为6kg～7kg。
进一步地，所述肩部第一关节电机的轴线垂直于地面，肩部第二关节电机垂直于肩部第一关节电机，肩部第二关节电机位于肩部第一关节电机的末端；肩部第三关节电机连接于肩部第二关节电机的末端，垂直于肩部第二关节电机安装，肩部第一关节电机的轴线与肩部第三关节电机的轴线之间的距离是肩部第二关节电机的减速器长度及与肩部第三关节电机连接法兰厚度之和；肩部第三关节电机连接大臂连杆，大臂连杆与肩部第三关节电机的轴共线；大臂连杆末端连接肘部第一关节电机，肘部第一关节电机的轴线与肩部第三关节电机的轴线垂直；肘部第二关节电机连接于肘部第一关节电机的末端，轴线垂直于肘部第一关节电机的轴线，肘部第二关节电机的轴线与肩部第三关节电机的轴线之间的距离是肘部第一关节电机的减速器长度及与肘部第二关节电机连接法兰厚度之和；小臂连杆连接于肘部第二关节电机末端，小臂连杆和肘部第二关节电机的轴线共线；小臂连杆末端连接腕部第一关节电机，腕部第一关节电机的轴线与肘部第二关节电机的轴线垂直；腕部第二关节电机连接于腕部第一关节电机的末端，轴线垂直于腕部第一关节电机的轴线，腕部第二关节电机轴线与肘部第二关节电机轴线之间的距离是腕部第一关节电机的减速器长度及与腕部第二关节电机连接法兰厚度之和。
进一步地，腕部第二关节电机末端有法兰，法兰上面有定位孔，夹爪通过定位孔与腕部第二关节电机固定。
进一步地，肩部的关节电机的连续扭矩大于等于20N·m，所述肘部的关节电机的连续扭矩范围为5N·m～25N·m，所述腕部的关节电机的连续扭矩小于等于5N·m。
按照本发明的另一个方面，提供了一种7自由度拟人机械臂的控制系统，包括：7自由度拟人机械臂和控制模块，所述控制模块与7自由度拟人机械臂通信连接，
所述控制模块，用于对7自由度拟人机械臂的起始点和目标点进行逆运动学求解，得到对应起始位姿和目标位姿下的7自由度拟人机械臂的7个关节角度，然后进行路径规划，得到从起始点到目标点的多个路径点，使用最小二乘法平滑多个路径点，在多个路径点之间进行插补计算，得到7自由度拟人机械臂的运行轨迹，接着对每个关节电机进行轨迹跟踪，实时控制7自由度拟人机械臂运动。
进一步地，控制模块包括底层控制层和PC机控制层，所述底层控制层与7自由度拟人机械臂之间通过EtherCAT、CAN或RS485连接，所述底层控制层和PC机控制层通过以太网、RS232或RS485连接；
所述PC机控制层，用于机械臂逆运动学求解和路径规划的实时计算，根据7自由度拟人机械臂的起始点和目标点进行逆运动学求解，得到对应起始位姿和目标位姿下的7自由度拟人机械臂的7个关节角度，然后基于采样的RRT算法进行路径规划，得到从起始点到目标点的多个路径点，使用最小二乘法平滑多个路径点，在多个路径点之间进行插补计算，得到与每个插补点对应的轨迹点，每个轨迹点包含轨迹时间、7个关节角度、7个关节速度和7个关节加速度；
所述底层控制层，用于利用多个轨迹点，同步对每个关节电机进行轨迹跟踪，对7自由度拟人机械臂进行实时运动控制。
进一步地，逆运动学求解时使用基于数值解的TRAC-IK求解器，TRAC-IK求解器包括并列运行的KDL-RR算法和SQP-SS算法，将KDL-RR算法和SQP-SS算法中最快得到的结果作为TRAC-IK求解器的结果。
所述KDL-RR算法和SQP-SS算法都是基于数值解的逆运动学求解算法。
进一步地，KDL-RR算法的循环中止条件为最大循环时间，当KDL-RR算法的循环时间小于最大循环时间时，利用rand函数生成关节向量的随机种子后继续运行KDL-RR算法。
进一步地，SQP-SS算法的循环中止条件为最大循环次数，SQP-SS算法以起始点的位姿和目标点的位姿之间的笛卡尔误差向量的平方和最小为目标函数。
按照本发明的另一个方面，提供了一种7自由度拟人机械臂的控制方法，其特征在于，包括：
根据7自由度拟人机械臂的起始点和目标点，得到对应起始位姿和目标位姿下的7自由度拟人机械臂的7个关节角度，然后进行路径规划，得到从起始点到目标点的多个路径点，使用最小二乘法平滑多个路径点，再在多个路径点之间进行插补计算，得到与每个插补点对应的轨迹点，每个轨迹点包含轨迹时间、7个关节角度、7个关节速度和7个关节加速度；
利用多个轨迹点，同步对每个关节电机进行轨迹跟踪，对7自由度拟人机械臂进行实时运动控制。
总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：
(1)本发明的7自由度拟人机械臂具有冗余自由度，相比6自由度机械臂具有更多的解空间，可以实现工作空间范围内的任意位置均可达且有无穷解，因此可以保证人机协同作业时，实现避障及人体安全防护等功能，具有更好的运动柔性、灵活性和适应性。
(2)本发明的7自由度拟人机械臂系统机械结构参照人体手臂的结构进行设计，符合人类手臂的运动过程，适合于人机协作场合，但本发明的7自由度拟人机械臂不符合PIEPER准则，求解逆运动学时无法得到封闭解，即无法通过解析方法求解，因此本发明采用基于数值解的算法进行逆运动学求解，以保证了7自由度拟人机械臂人机协同作业时，具有更好的运动柔性、灵活性和适应性。
(3)本发明的7自由度拟人机械臂的关节电机布局形式相对人类手臂结构进行了优化，将组成人类腕部的1个自由度关节电机调整到肘部，即将人类腕部的3个自由度关节电机调整为2个自由度关节电机，从而解决了腕部关节电机自重造成的机械臂末端负载能力降低的问题，保证了机械臂的负载及自重的分布均衡化，既可以满足7自由度机械臂的灵活运动要求，又可以保证其末端具有较大的负载能力。
(4)本发明的7自由度拟人机械臂控制系统采用分层控制策略，将控制模块分为底层控制层和PC机控制层，通过将上层(PC机控制层)路径规划与底层运动控制分布在两个平台上进行，实现了7自由度拟人机械臂运动控制的实时性与准确性。
(5)本发明的7自由度拟人机械臂采用基于数值解的逆运动学求解方法。逆运动学求解方法使用TRAC-IK求解器，包括两个解算线程，一个线程运行增强KDL算法即KDL-RR算法，另一个线程运行优化SQP即SQP-SS算法求逆算法，一旦其中的一个解算线程计算完成，两个线程立即同时停止，并返回已完成的那个解算线程的结果作为TRAC-IK求解器的计算结果，保证了逆运动学求解的实时性。
(6)本发明的7自由度拟人机械臂进行路径规划，得到从机械臂的起始点到目标点的一系列中间过程路径点，然后使用最小二乘法平滑多个路径点，进行插补计算，再将得到的轨迹点发送给底层控制层，对7自由度机械臂进行实时运动控制，使得机械臂的运动过程更平滑且连续。
附图说明
图1为本发明实施例提供的一种基于数值解的7自由度拟人机械臂控制系统的结构图；
图2为本发明实施例提供的一种基于数值解的7自由度拟人机械臂系统架构框图；
图3为本发明实施例提供的一种基于数值解的7自由度拟人机械臂控制方法流程图；
图4为本发明实施例提供的一种基于数值解的7自由度拟人机械臂逆运动学求解算法流程图；
图5为本发明实施例提供的一种基于数值解的7自由度拟人机械臂路径规划算法流程图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
如图1所示，一种基于数值解的7自由度拟人机械臂控制系统，包括：7自由度拟人机械臂和控制模块，所述控制模块与7自由度拟人机械臂通信连接，
所述控制模块，用于对7自由度拟人机械臂的起始点和目标点进行逆运动学求解，得到对应起始位姿和目标位姿下的7自由度拟人机械臂的7个关节角度，然后进行路径规划，得到从起始点到目标点的多个路径点，使用最小二乘法平滑多个路径点，在多个路径点之间进行插补计算，得到7自由度拟人机械臂的运行轨迹，接着对每个关节电机进行轨迹跟踪，实时控制7自由度拟人机械臂运动。
如图1所示，图中虚线用于示意机械臂各关节的轴线，关节电机编号为1～7。其中，相邻两关节轴线垂直相交，但相邻三个关节轴未满足相交于一点或相互平行的要求。因此无法满足PIEPER准则，无法得到封闭解。
具体地，本发明的7自由度拟人机械臂相邻两关节电机轴均互相垂直，7自由度拟人机械臂的7个关节电机包括肩部第一关节电机1、肩部第二关节电机2、肩部第三关节电机3、肘部第一关节电机4、肘部第二关节电机5、腕部第一关节电机6和腕部第二关节电机7，所述肩部第一关节电机1的轴线垂直于地面，肩部第二关节电机2垂直于肩部第一关节电机1安装，且安装在肩部第一关节电机1的末端；肩部第三关节电机3连接于肩部第二关节电机2的末端，垂直于肩部第二关节电机2安装，且保证肩部第一关节电机1的轴线与肩部第三关节电机3的轴线之间的距离是肩部第二关节电机2的减速器长度及与肩部第三关节电机3连接法兰厚度之和；肩部第三关节电机3连接有大臂连杆，大臂连杆轴与肩部第三关节电机3轴共线；大臂连杆末端连接肘部第一关节电机4，且保证肘部第一关节电机4的轴线与肩部第三关节电机3的轴线垂直；肘部第二关节电机5连接于肘部第一关节电机4的末端，轴线垂直于肘部第一关节电机4的轴线，且保证肘部第二关节电机5的轴线与肩部第三关节电机3的轴线之间的距离是肘部第一关节电机4的减速器长度及与肘部第二关节电机5连接法兰厚度之和；小臂连杆连接于肘部第二关节电机5末端，且保证两者轴线共线；小臂连杆末端连接腕部第一关节电机6，且保证腕部第一关节电机6的轴线与肘部第二关节电机5的轴线垂直；腕部第二关节电机7连接于腕部第一关节电机6的末端，轴线垂直于腕部第一关节电机6的轴线，且保证腕部第二关节电机7轴线与肘部第二关节电机5轴线之间的距离是腕部第一关节电机6的减速器长度及与腕部第二关节电机7连接法兰厚度之和。
相邻两关节轴线垂直相交，但相邻三个关节轴未满足相交于一点或相互平行的要求。因此无法满足PIEPER准则，无法得到封闭解。
7自由度拟人机械臂的机械结构通过参照正常成人手臂，来设计机械臂的大臂和小臂长度尺寸及各关节选型，包括依次连接的夹爪8、腕部、小臂连杆9、肘部、大臂连杆10和肩部，其中，肩部由肩部第一关节电机1、肩部第二关节电机2、肩部第三关节电机组成3，肘部由肘部第一关节电机4和肘部第二关节电机5组成，腕部由腕部第一关节电机6和腕部第二关节电机7组成，相邻两关节电机轴线垂直相交。关节电机布局形式相对人类手臂结构进行优化，将组成人类腕部的1个自由度关节电机调整到肘部，将人类腕部的3个自由度关节电机调整为2个自由度关节电机，肘部包含2个自由度关节电机，提高机械臂末端的负载能力；所述7自由度拟人机械臂在不包含夹爪时的总长度为600mm～700mm，所述7自由度拟人机械臂在不包含夹爪时的总重量为6kg～7kg。
具体地，本发明的机械臂总长700mm(不含夹爪)，总重量为7kg。其中，大臂长度为350mm，小臂长度为350mm。大臂连杆是指肩部第二关节电机2的轴线到肘部第一关节电机4的轴线的距离，小臂连杆是指肘部第一关节电机4的轴线到腕部第一关节电机6的轴线的距离。
进一步地，腕部第二关节电机末端有法兰，法兰上面有定位孔，夹爪通过定位孔与腕部第二关节电机固定。
所述肩部的关节电机的连续扭矩大于等于20N·m，所述肘部的关节电机的连续扭矩范围为5N·m～25N·m，所述腕部的关节电机的连续扭矩小于等于5N·m。
如图1所示，各关节电机选型方案一：肩部第一关节电机1、肩部第二关节电机2、肩部第三关节电机3和肘部第一关节电机4选扭矩较大的电机，肘部第二关节电机5选扭矩中等的电机，腕部第一关节电机6和腕部第二关节电机7选扭矩较小的电机。各电机参数如下：肩部第一关节电机1、肩部第二关节电机2、肩部第三关节电机3和肘部第一关节电机4的电机额定功率为70W，连续扭矩(指经减速器之后的连续扭矩，下同)为21.9N·m；肘部第二关节电机5的电机额定功率为30W，连续扭矩为8N·m；腕部第一关节电机6和腕部第二关节电机7的电机额定功率为15W，连续扭矩为3N·m。
如图1所示，各关节电机选型方案二：肩部第一关节电机1、肩部第二关节电机2和肩部第三关节电机3选扭矩较大的电机，肘部第一关节电机4和肘部第二关节电机5选扭矩中等的电机，腕部第一关节电机6和腕部第二关节电机7选扭矩较小的电机。各电机参数如下：肩部第一关节电机1、肩部第二关节电机2和肩部第三关节电机3的电机额定功率为70W，连续扭矩为21.9N·m；肘部第一关节电机4和肘部第二关节电机5的电机额定功率为30W，连续扭矩为8N·m；腕部第一关节电机6和腕部第二关节电机7的电机额定功率为15W，连续扭矩为3N·m。
这2种电机选型方案都可以保证7自由度拟人机械臂末端的负载能力适合用于人机协作的场合。
如图2所示，7自由度(DOF)拟人机械臂本体由7个关节、夹爪以及大臂连杆和小臂连杆组成，包括3个关节组成的肩部、2个关节组成的肘部、2个关节组成的腕部。机械臂控制系统采用分层结构，包括底层控制层和PC机控制层。底层控制层采用基于STM32F407微控制器的STM32控制板，与7自由度拟人机械臂本体内部的各个关节驱动控制板通信，通信接口方式为EtherCAT、CAN或RS485，用于保证机械臂的实时运动控制。PC机控制层与STM32控制板通过以太网、RS232或RS485通信，进行机械臂逆运动学求解和路径规划的实时计算，并读取7自由度机械臂的运行状态参数。
图3所示一种基于数值解的7自由度拟人机械臂控制方法流程图，包括：
设置7自由度拟人机械臂的起始点和目标点，采集7自由度拟人机械臂在起始点的状态，采用TRAC-IK求解器，得到起始点和目标点对应起始位姿和目标位姿下的7自由度拟人机械臂的7个关节角度；在关节状态空间下，通过RRT算法进行路径规划得到起始位姿到目标位姿之间的一系列路径点，每个路径点包含7个关节角度；然后使用最小二乘法平滑多个路径点，进行路径点之间的插补计算，得到多个插补点，并结合关节电机的最大速度、最大加速度等约束，进行轨迹生成，得到与每个插补点对应的轨迹点，每个轨迹点包含轨迹时间和7个关节的角度、速度及加速度。
利用一系列轨迹点，底层控制层对每个关节电机进行轨迹跟踪，实时控制7自由度拟人机械臂运动。
具体地，首先设置机械臂末端起始点的位姿(Xs，Ys，Zs，RXs，RYs，RZs)和目标点的位姿(Xe，Ye，Ze，RXe，RYe，RZe)，通过TRAC-IK求解器，分别得到对应起始位姿和目标位姿下的7自由度拟人机械臂的7个关节角度(A1s，A2s，A3s，A4s，A5s，A6s，A7s)和(A1e，A2e，A3e，A4e，A5e，A6e，A7e)；在关节空间下，通过RRT算法进行路径规划，得到从起始位姿到目标位姿之间的一系列路径点；然后使用最小二乘法平滑多个路径点，再对这一系列路径点采用插补算法进行插补，并结合机械臂关节电机的最大速度、最大加速度等约束，进行轨迹生成，得到包含轨迹时间和7个关节的角度、速度及加速度的轨迹点。在底层控制层利用路径规划生成的一系列的轨迹点，对每个关节电机进行轨迹跟踪，实时控制7自由度拟人机械臂运动，使机械臂末端运动过程平滑且连续。
进一步的，Xs，Ys，Zs代表机械臂末端起始点的空间位置，RXs，RYs，RZs代表机械臂末端起始点的空间姿态；Xe，Ye，Ze代表机械臂末端目标点的空间位置，RXe，RYe，RZe代表机械臂末端目标点的空间姿态。
图4所示一种基于数值解的7自由度拟人机械臂逆运动学求解算法流程图，本发明采用的逆运动学求解算法通过TRAC-IK(逆运动学，IK：Inverse Kinematics)求解器，提高了计算的实时性。根据图4，TRAC-IK求解器会创建两个解算线程，一个线程运行基于雅可比矩阵的优化迭代求逆算法：KDL-RR(KDL with Random Restarts)，一种增强KDL(机器人操作系统ROS中的运动学和动力学库，KDL：Kinematics and Dynamics Library)算法；另一个线程运行基于牛顿迭代法的优化序列二次规划算法：SQP-SS(SQP with Sum of Squares)，一种优化后的SQP(序列二次规划，SQP：Sequential Quadratic Programming)求逆算法。一旦其中的一个解算线程计算完成，两个线程会同时立即停止，并返回已完成的那个解算线程的结果作为TRAC-IK求解器的计算结果，因而求解效率更高。例如机械臂某个姿态下，利用增强KDL算法求解需要5ms，利用优化SQP求逆算法求解需要8ms，则将增强KDL算法求解的结果作为机械臂逆运动学求解结果。本发明的基于数值计算的方法，通过不断迭代，找寻最合适的解，具有运算快，稳定性好的特点。
进一步的，TRAC-IK求解器包含2种逆运动学求解算法，也可以解决当某种求解算法失败时，造成逆运动学无解的缺点，提高了求解成功率。
KDL-RR算法是对经典KDL算法进行优化，得到的增强型运动学求逆算法。经典KDL算法的主要公式如公式(1)所示：
qnext＝qprev+J-1perr (1)
其中，qnext、qprev是后一状态、前一状态的关节向量，perr是机械臂当前位姿和目标位姿之间的笛卡尔误差向量，J-1是雅可比矩阵的逆。
在KDL-RR算法中，主要对经典KDL进行了两点改进：1)解算器的循环中止条件将最大迭代次数改为最大循环时间；2)增加关节向量的随机种子，以防止出现局部最优解的情况。
如图4中的KDL-RR算法模块所示，其中初始化包括：1)初始的笛卡尔坐标系下的位姿；2)初始位姿和目标位姿之间的笛卡尔误差向量perr；3)雅可比矩阵的逆J-1；初始化完成后开始循环迭代计算qnext；若机械臂各关节角度误差||dθ||小于设定误差阈值epsl，则将随机种子作为qnext，继续进行循环计算，以防止出现局部最优解的情况。
进一步的，为了使算法可控，设定循环时间上限，如果循环时间t超过最大循环时间tset，则中止循环，此时的qnext作为KDL的计算结果。
SQP算法是非线性约束优化问题中常用的方法，这种算法可以解决KDL系列算法在存在关节约束条件下求解失败的缺点，可提供更高的求解成功率。基本SQP算法的最优化问题如公式(2)所示：
其中，qseed是关节的n维初始值，不等式约束fi(q)是关节限制、欧几里德距离误差和角距离误差。
SQP-SS相比于基本SQP算法优化了SQP中的最小化目标函数，即将最小化平方和作为目标函数，如公式(3)所示：
如图4中的SQP-SS算法模块所示，初始化完成初始关节向量qseed及不等式约束fi(q)；再通过序列二次规划不断迭代循环；若机械臂末端位姿误差||perr||小于设定误差阈值eps2，则结束循环。
进一步的，为了使算法可控，设定循环次数上限，如果循环次数c达到设定值cset，则结束循环。
本实施例的SQP-SS算法相比基本SQP算法具有更好的运动学求逆效果。
图5所示一种基于数值解的7自由度拟人机械臂路径规划算法流程图。RRT(快速遍历随机树，或快速扩展随机树，RRT：Rapidly-exp1oring Random Tree)是一种树形数据存储结构和算法，通过递增的方法建立，并快速减小随机选择点同树的距离。本发明采用基于采样的RRT路径规划算法，能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径。搜索时，通过对状态空间的采样点进行碰撞检测，避免了对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题，适合于解决多自由度机器人在复杂环境下和动态环境中的路径规划。
本发明的路径规划算法具体流程如图5所示，初始化过程包括：随机树T的初始化、起始点qorigin、目标点qgoal，初始化的随机数T只包含一个节点：根节点qinit。首先从状态空间中随机选择一个采样点qtarget；然后在随机树T中选择距离qtarget最近的节点qnearest；再通过从qnearest向qtarget扩展一段距离，得到一个新的节点qnew；如果qnew与障碍物发生碰撞，则放弃此次生长，等待下次迭代循环，否则将新节点qnew加入到随机树中；再判断随机树T中的子节点qnearest与目标点qgoal的距离，若距离小于设定的阈值，则退出循环，返回计算结果，否则继续循环。
进一步的，在上述算法中，当随机树的叶子节点进入了目标区域时，便可以在随机树中找到一条从根节点到叶子节点的路径，即从起始点到目标点的一系列路径，该路径中每个节点都包含关节空间下的每个关节电机对应的角度，然后使用最小二乘法平滑多个路径点，在相邻路径节点之间进行插补计算，得到多个插补点，并结合关节电机的最大速度、最大加速度等约束，进行轨迹生成，得到与每个插补点对应的轨迹点，每个轨迹点包含轨迹时间和7个关节的角度、速度及加速度。
进一步的，为了使算法可控，设定搜索次数上限，如果在限制次数内无法到达目标点，则算法返回失败。
进一步地，在路径规划算法中，由于随机采样的缘故，这些路径点构成的线段可能并不平滑，容易造成机械臂在运动的过程中出现不稳定的情况，因此，使用最小二乘法平滑多个路径点。具体方法是：使用最小二乘多项式拟合的方式对原始路径进行平滑处理，即使用多项式函数代替原数据，并保证拟合数据与原始数据的偏差的平方和最小。
设n对原始数据点(xi，yi)，(i＝1，2，...，n)，以及m次多项式
如果限定多项式的最高次数是固定的情况下，求系数ai(i＝1，2，...，m)，使得多项式拟合数据与原始数据的偏差的平方和Q最小，即满足
最小，则该多项式就是要寻找的拟合曲线。
进一步地，本发明选取最小二乘多项式的最高次数为3次。多项式的最高次数要选取恰当，太小会欠拟合，平滑效果不好；太大会过拟合，也会增加计算量。
本发明所述一种基于数值解的7自由度拟人机械臂系统与控制方法通过对机械臂的机械结构进行设计，对关节电机的选型及布局配置做出了详细的设计说明；结合本发明中的机械臂控制系统，底层采用STM32单片机、上层采用高性能PC机的分层控制策略，采用TRAC-IK求解器作为逆运动学解算器，基于数值计算的方法，通过不断迭代，找寻最合适的解，具有运算快，稳定性好的特点。同时，基于采样的RRT路径规划算法，以及插补计算，进行轨迹生成，使机械臂末端运动过程平滑且连续。
保证了7自由度拟人机械臂人机协同作业时，具有更好的运动柔性、灵活性和适应性。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
Claims (6)
1.一种7自由度拟人机械臂，其特征在于，包括依次连接的夹爪(8)、腕部、小臂连杆(9)、肘部、大臂连杆(10)和肩部，其中，肩部由肩部第一关节电机(1)、肩部第二关节电机(2)、肩部第三关节电机(3)组成，肘部由肘部第一关节电机(4)和肘部第二关节电机(5)组成，腕部由腕部第一关节电机(6)和腕部第二关节电机(7)组成，相邻两关节电机轴线垂直相交；
所述肩部第一关节电机(1)的轴线垂直于地面，肩部第二关节电机(2)垂直于肩部第一关节电机(1)，肩部第二关节电机(2)位于肩部第一关节电机(1)的末端；肩部第三关节电机(3)连接于肩部第二关节电机(2)的末端，垂直于肩部第二关节电机(2)安装，肩部第一关节电机(1)的轴线与肩部第三关节电机(3)的轴线之间的距离是肩部第二关节电机(2)的减速器长度及与肩部第三关节电机(3)连接法兰厚度之和；肩部第三关节电机(3)连接大臂连杆，大臂连杆与肩部第三关节电机(3)的轴共线；
大臂连杆末端连接肘部第一关节电机(4)，肘部第一关节电机(4)的轴线与肩部第三关节电机(3)的轴线垂直；肘部第二关节电机(5)连接于肘部第一关节电机(4)的末端，轴线垂直于肘部第一关节电机(4)的轴线，肘部第二关节电机(5)的轴线与肩部第三关节电机(3)的轴线之间的距离是肘部第一关节电机(4)的减速器长度及与肘部第二关节电机(5)连接法兰厚度之和；小臂连杆连接于肘部第二关节电机(5)末端，小臂连杆和肘部第二关节电机(5)的轴线共线；
小臂连杆末端连接腕部第一关节电机(6)，腕部第一关节电机(6)的轴线与肘部第二关节电机(5)的轴线垂直；腕部第二关节电机(7)连接于腕部第一关节电机(6)的末端，轴线垂直于腕部第一关节电机(6)的轴线，腕部第二关节电机(7)轴线与肘部第二关节电机(5)轴线之间的距离是腕部第一关节电机(6)的减速器长度及与腕部第二关节电机(7)连接法兰厚度之和；
所述肩部的关节电机的连续扭矩大于等于20N·m，所述肘部的关节电机的连续扭矩范围为5N·m～25N·m，所述腕部的关节电机的连续扭矩小于等于5N·m。
2.如权利要求1所述的一种7自由度拟人机械臂，其特征在于，所述7自由度拟人机械臂在不包含夹爪时的总长度为600mm～700mm。
3.如权利要求1或2所述的一种7自由度拟人机械臂，其特征在于，所述7自由度拟人机械臂在不包含夹爪时的总重量为6kg～7kg。
4.如权利要求1-3任一所述的一种7自由度拟人机械臂的控制系统，其特征在于，包括：7自由度拟人机械臂和控制模块，所述控制模块与7自由度拟人机械臂通信连接，
所述控制模块，用于对7自由度拟人机械臂的起始点和目标点进行逆运动学求解，得到起始点和目标点对应的起始位姿和目标位姿下的7自由度拟人机械臂的7个关节角度，然后进行路径规划，得到从起始点到目标点的多个路径点，使用最小二乘法平滑多个路径点，在多个路径点之间进行插补计算，得到7自由度拟人机械臂的运行轨迹，利用运行轨迹对每个关节电机进行轨迹跟踪，实时控制7自由度拟人机械臂运动；
所述逆运动学求解时使用基于数值解的TRAC-IK求解器，所述TRAC-IK求解器包括并列运行的KDL-RR算法和SQP-SS算法，将KDL-RR算法和SQP-SS算法中最快得到的结果作为TRAC-IK求解器的结果；
所述KDL-RR算法的循环中止条件为最大循环时间，当KDL-RR算法的循环时间小于最大循环时间时，利用rand函数生成关节向量的随机种子后继续运行KDL-RR算法；
所述SQP-SS算法的循环中止条件为最大循环次数，SQP-SS算法以起始点的位姿和目标点的位姿之间的笛卡尔误差向量的平方和最小为目标函数。
5.如权利要求4所述的一种7自由度拟人机械臂的控制系统，其特征在于，所述控制模块包括底层控制层和PC机控制层，所述底层控制层与7自由度拟人机械臂之间通过EtherCAT、CAN或RS485连接，所述底层控制层和PC机控制层通过以太网、RS232或RS485连接；
所述PC机控制层，用于根据7自由度拟人机械臂的起始点和目标点进行逆运动学求解，得到起始点和目标点对应的起始位姿和目标位姿下的7自由度拟人机械臂的7个关节角度，然后进行路径规划，得到从起始点到目标点的多个路径点，使用最小二乘法平滑多个路径点，在多个路径点之间进行插补计算，得到与每个插补点对应的轨迹点，每个轨迹点包含轨迹时间、7个关节角度、7个关节速度和7个关节加速度；
所述底层控制层，用于利用多个轨迹点，同步对每个关节电机进行轨迹跟踪，对7自由度拟人机械臂进行实时运动控制。
6.如权利要求1-3任一所述的一种7自由度拟人机械臂的控制方法，其特征在于，包括：
根据7自由度拟人机械臂的起始点和目标点，得到起始点和目标点对应的起始位姿和目标位姿下的7自由度拟人机械臂的7个关节角度，然后进行路径规划，得到从起始点到目标点的多个路径点，使用最小二乘法平滑多个路径点，再在多个路径点之间进行插补计算，得到与每个插补点对应的轨迹点，每个轨迹点包含轨迹时间、7个关节角度、7个关节速度和7个关节加速度；
利用多个轨迹点，同步对每个关节电机进行轨迹跟踪，对7自由度拟人机械臂进行实时运动控制。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811415296.8A CN109397271B (zh) | 2018-11-23 | 2018-11-23 | 一种7自由度拟人机械臂及其控制方法和系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811415296.8A CN109397271B (zh) | 2018-11-23 | 2018-11-23 | 一种7自由度拟人机械臂及其控制方法和系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109397271A CN109397271A (zh) | 2019-03-01 |
| CN109397271B true CN109397271B (zh) | 2020-07-24 |
Family
ID=65455518
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811415296.8A Active CN109397271B (zh) | 2018-11-23 | 2018-11-23 | 一种7自由度拟人机械臂及其控制方法和系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109397271B (zh) |
Families Citing this family (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109986564A (zh) * | 2019-05-20 | 2019-07-09 | 上海应用技术大学 | 工业机械臂路径规划方法 |
| CN110480657A (zh) * | 2019-08-13 | 2019-11-22 | 北京控制工程研究所 | 一种复杂结构环境空间机器人天地遥操作系统 |
| CN113084791B (zh) * | 2019-12-23 | 2022-09-23 | 深圳市优必选科技股份有限公司 | 机械臂控制方法、机械臂控制装置及终端设备 |
| CN112511759B (zh) * | 2021-02-08 | 2021-05-11 | 常州微亿智造科技有限公司 | 飞拍控制方法和装置 |
| CN113730178A (zh) * | 2021-07-27 | 2021-12-03 | 深圳华鹊景医疗科技有限公司 | 上肢等速训练康复机器人系统及机器人控制方法 |
| CN114714327A (zh) * | 2022-03-14 | 2022-07-08 | 北京精密机电控制设备研究所 | 一种机械臂与灵巧手的融合系统及运动控制方法 |
| CN114952868B (zh) * | 2022-07-26 | 2022-11-11 | 之江实验室 | 7自由度srs型机械臂控制方法及装置、弹琴机器人 |
| CN115256400B (zh) * | 2022-08-26 | 2024-05-28 | 北京理工大学 | 机器人三自由度电驱动耦合关节的运动可行范围线性界定方法 |
| CN115755592B (zh) * | 2023-01-10 | 2023-05-12 | 常熟理工学院 | 调整三自由度外骨骼运动状态的多模态控制方法及外骨骼 |
| CN116038708A (zh) * | 2023-01-30 | 2023-05-02 | 北京大学 | 基于内部预演的机械手臂趋近规划方法及装置 |
| CN117270381B (zh) * | 2023-11-06 | 2024-03-12 | 深圳市鸿栢科技实业有限公司 | 工业机器人外环控制器及控制方法 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH0550386A (ja) * | 1991-08-16 | 1993-03-02 | Fanuc Ltd | ７自由度マニプレータの位置教示方式 |
| TW200930523A (en) * | 2008-01-11 | 2009-07-16 | Han-Pang Huang | 7 degrees of freedom humanoid robot arm (including a gripper) |
| CN202146362U (zh) * | 2010-12-30 | 2012-02-22 | 上海交通大学医学院附属第九人民医院 | 一种基于光学导航七自由度颅颌面手术辅助机械臂 |
| CN102837322B (zh) * | 2012-09-26 | 2015-04-15 | 黄刚 | 一种仿人型七自由度机械臂 |
| US9862090B2 (en) * | 2014-07-25 | 2018-01-09 | California Institute Of Technology | Surrogate: a body-dexterous mobile manipulation robot with a tracked base |
| CN104385266A (zh) * | 2014-08-28 | 2015-03-04 | 北京邮电大学 | 七自由度外骨骼式遥操作主手 |
-
2018
- 2018-11-23 CN CN201811415296.8A patent/CN109397271B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN109397271A (zh) | 2019-03-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109397271B (zh) | 一种7自由度拟人机械臂及其控制方法和系统 | |
| CN109397244B (zh) | 一种一体化双7自由度机械臂全向移动机器人系统与控制方法 | |
| CN109397292B (zh) | 一种基于解析解的7自由度机械臂控制方法与系统 | |
| CN113352319B (zh) | 基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法 | |
| US9411335B2 (en) | Method and apparatus to plan motion path of robot | |
| CN104965517B (zh) | 一种机器人笛卡尔空间轨迹的规划方法 | |
| US8825209B2 (en) | Method and apparatus to plan motion path of robot | |
| US8483874B2 (en) | Path planning apparatus of robot and method and computer-readable medium thereof | |
| WO2023024317A1 (zh) | 一种机器人避障方法、装置和机器人 | |
| CN110561419B (zh) | 臂型线约束柔性机器人轨迹规划方法及装置 | |
| Chen et al. | An integrated two-level self-calibration method for a cable-driven humanoid arm | |
| Wen et al. | Obstacle avoidance path planning of manipulator based on improved RRT algorithm | |
| Gochev et al. | Motion planning for robotic manipulators with independent wrist joints | |
| CN111791234A (zh) | 一种狭窄空间内多机器人空间工作位置防撞控制算法 | |
| Hou et al. | Kinematics analysis and self-collision detection of Truss type multi-robot cooperative welding platform | |
| Ma et al. | 6R serial manipulator space path planning based on RRT | |
| Li et al. | Stiffness-maximum trajectory planning of a hybrid kinematic-redundant robot machine | |
| Tang et al. | Coordinated motion planning of dual-arm space robot with deep reinforcement learning | |
| Seyboldt et al. | Sampling-based path planning to cartesian goal positions for a mobile manipulator exploiting kinematic redundancy | |
| Zhang et al. | Kuka youBot arm shortest path planning based on geodesics | |
| Qian et al. | Path planning approach for redundant manipulator based on Jacobian pseudoinverse-RRT algorithm | |
| Xu et al. | A cable-driven hyperredundant manipulator: Obstacle-avoidance path planning and tension optimization | |
| Zhang et al. | Robot motion planning with orientational constraints based on offline sampling datasets | |
| Hu et al. | Trajectory planning method of 6-DOF modular manipulator based on polynomial interpolation | |
| Lu et al. | An optimization algorithm for trajectory planning of a 7-DOF redundant manipulator |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |