CN109483529B - 一种基于螺旋理论的机械臂伺服控制方法、系统及装置 - Google Patents
一种基于螺旋理论的机械臂伺服控制方法、系统及装置 Download PDFInfo
- Publication number
- CN109483529B CN109483529B CN201811190619.8A CN201811190619A CN109483529B CN 109483529 B CN109483529 B CN 109483529B CN 201811190619 A CN201811190619 A CN 201811190619A CN 109483529 B CN109483529 B CN 109483529B
- Authority
- CN
- China
- Prior art keywords
- matrix
- mechanical arm
- joint
- error
- given target
- 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/16—Programme controls
- B25J9/1628—Programme controls characterised by the control loop
-
- 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
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
本发明公开了一种基于螺旋理论的机械臂伺服控制方法、系统及装置,其中所述方法包括:检测机械臂的关节转角数据,对关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿矩阵;根据实际末端位姿矩阵和给定目标位姿矩阵进行矩阵运算,获取机械臂的位置误差和姿态误差;获取位置误差和姿态误差中的最大值作为最大误差值,并判断最大误差值是否小于预设误差阈值;若否,进行计算,获取纠正偏差所需要的关节角转动量;获取机械臂关节的关节角更新数据;根据关节角更新数据控制机械臂关节进行位置更新。在本发明实施例中,采用本发明的实施方法,具有计算量小、实时性好、跟踪准确的优势,可有效提高对机械臂的控制精度。
Description
技术领域
本发明涉及机器人的机械臂伺服控制技术领域,尤其涉及一种基于螺旋理论的机械臂伺服控制方法、系统及装置。
背景技术
电机的伺服控制是通过对电机的电压、电流和频率等输入量进行控制,来改变电机输出的位移、速度、加速度和扭矩等机械量,从而带动机械装置按照人们预期的规律运行,以满足生产和生活中各种动作自动化的需要;服务机器人的机械臂一般为多螺旋关节串联型式,其电机的伺服控制一般要求有较高的协调性、实时性和准确性;电机的伺服控制包括位置控制、力控制和力/位置混合控制等方式,这些控制方式的实际效果与所采用的控制算法密切相关;高效、精准、稳健性强的控制算法能最大限度地抵消外界干扰的影响,保证机器人的执行机构按照正确的轨迹高效运动。
现有的机械臂伺服控制方法大多依赖于运动学逆解的解算,其过程一般是根据给定的机械臂末端位姿和结构参数求解逆运动学方程,进而推导出机械臂各个关节所需的螺旋角度;对于多自由度机械臂,由于其运动学逆解的多解性,每次计算后均需要根据一定的原则对多组逆解进行择优,导致一个伺服控制周期内运算量较大,造成机械臂响应慢、动作迟缓。
发明内容
本发明的目的在于克服现有技术的不足,本发明提供了一种基于螺旋理论的机械臂伺服控制方法、系统及装置,避免了运动学逆解的反复解算和多组逆解的择优过程,具有计算量小、实时性好、跟踪准确的优势,可有效的提高对机械臂的控制精度;并提高机械臂的动态性能,可有效提高机械臂的运行效率。
为了解决上述技术问题,本发明实施例提供了一种基于螺旋理论的机械臂伺服控制方法,所述机械臂伺服控制方法包括:
检测机械臂的关节转角数据,对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿矩阵;
根据所述实际末端位姿矩阵和给定目标位姿矩阵进行矩阵运算,获取所述机械臂的位置误差和姿态误差;
获取所述位置误差和所述姿态误差中的最大值作为最大误差值,并判断所述最大误差值是否小于预设误差阈值;
若否,根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行计算,获取纠正偏差所需要的关节角转动量;
将所述纠正偏差所需要的关节角转动量与当前关节角转动量进行叠加,获取机械臂关节的关节角更新数据;
根据所述关节角更新数据控制所述机械臂关节进行位置更新。
可选的,所述检测机械臂的关节转角数据,对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿,包括:
采用安装在关节电机轴上的光电编码器检测所述机械臂上关节的关节转角数据;
对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿矩阵;
所述指数积正向运动学计算的计算公式如下:
其中,Tc表示机械臂的实际末端位姿矩阵,xsn为机械臂第n个自由度的速度螺旋量,θsn表示检测到的机械臂第n个自由度的关节转角数据,i表示机械减速比,n表示自由度的个数,g0表示机械臂末端初始位姿矩阵。
可选的,所述根据所述实际末端位姿矩阵和给定目标位姿矩阵进行矩阵运算,获取所述机械臂的位置误差和姿态误差,包括:
对所述实际末端位姿矩阵和所述给定目标位姿矩阵进行矩阵分块处理,获取实际末端姿态矩阵、实际末端位置向量、给定目标姿态矩阵和给定目标位置向量;
提取实际末端姿态矩阵、给定目标姿态矩阵、实际末端位置向量和给定目标位置向量进行矩阵运算,获取所述机械臂的位置误差和姿态误差;
其中,对所述实际末端位姿矩阵进行矩阵分块处理,获取实际末端姿态矩阵和实际末端位置向量的表示如下:
对所述给定目标位姿矩阵进行矩阵分块处理,获取给定目标姿态矩阵和给定目标位置向量的表示如下:
其中,Tc为实际末端位姿矩阵,Rc为实际末端姿态矩阵,Pc为实际末端位置向量,Tt为给定目标位姿矩阵,Rt为给定目标姿态矩阵,Pt为给定目标位置向量;Tc和Tt为4×4矩阵,Rc和Rt为3×3矩阵,Pc和Pt为3×1列向量。
可选的,所述提取实际末端姿态矩阵、给定目标姿态矩阵、实际末端位置向量和给定目标位置向量进行矩阵运算,获取所述机械臂的位置误差和姿态误差,包括:
采用所述给定目标位置向量与所述实际末端位置向量进行矩阵运算,获取所述机械臂的位置误差;
采用所述给定目标姿态矩阵与所述实际末端姿态矩阵进行矩阵运算,获取所述机械臂的姿态误差;
其中,采用所述给定目标位置向量与所述实际末端位置向量进行矩阵运算的计算公式如下:
eP=||Pt-Pc||;
采用所述给定目标姿态矩阵与所述实际末端姿态矩阵进行矩阵运算的计算公式如下:
可选的,所述机械臂伺服控制方法还包括:
若判断所述最大误差值小于预设误差阈值时,返回检测机械臂的关节转角数据,对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿矩阵。
可选的,所述根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行计算,获取纠正偏差所需要的关节角转动量,包括:
根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行速度旋量计算,获取速度旋量;
采用所述速度旋量左乘雅克比矩阵的Moore-Penrose伪逆,获取关节空间速度;
采用所述关节空间速度与给定的时间微元相乘,获取纠正偏差所需要的关节角转动量。
可选的,所述根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行速度旋量计算的计算公式如下:
其中,x表示速度旋量,Pc表示实际末端位置向量,Pt表示给定目标位置向量,表示实际末端姿态矩阵的转置,Rt表示给定目标姿态矩阵,v表示线速度列向量,ω表示角速度列向量,vlim表示线速度限值,ωlim表示角速度限值。
可选的,所述将所述纠正偏差所需要的关节角转动量与当前关节角转动量进行叠加的计算公式如下:
另外,本发明实施例还提供了一种基于螺旋理论的机械臂伺服控制系统,所述机械臂伺服控制系统包括:
第一计算模块:用于检测机械臂的关节转角数据,对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿矩阵;
矩阵运算模块:用于根据所述实际末端位姿矩阵和给定目标位姿矩阵进行矩阵运算,获取所述机械臂的位置误差和姿态误差;
判断模块:用于获取所述位置误差和所述姿态误差中的最大值作为最大误差值,并判断所述最大误差值是否小于预设误差阈值;
第二计算模块:用于若判断为否时,根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行计算,获取纠正偏差所需要的关节角转动量;
叠加模块:用于将所述纠正偏差所需要的关节角转动量与当前关节角转动量进行叠加,获取机械臂关节的关节角更新数据;
更新模块:用于根据所述关节角更新数据控制所述机械臂关节进行位置更新。
另外,本发明实施例还提供了一种基于螺旋理论的机械臂伺服控制装置,其包括:
一个或多个处理器;
存储器;
一个或多个应用程序,其中所述一个或多个应用程序被存储在所述存储器中并被配置为由所述一个或多个处理器执行,所述一个或多个应用程序配置用于:执行上述中任意一项机械臂伺服控制方法。
在本发明具体实施例中,采用基于螺旋理论的数值算法进行机械臂的伺服控制,避免了运动学逆解的反复解算和多组逆解的择优过程,具有计算量小、实时性好、跟踪准确的优势,有效的提高对机械臂的控制精度;并提高机械臂的动态性能,可有效提高机械臂的运行效率。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见的,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其它的附图。
图1是本发明实施例中的基于螺旋理论的机械臂伺服控制方法的流程示意图;
图2是本发明实施例中的基于螺旋理论的机械臂伺服控制系统的结构组成示意图;
图3是本发明实施例中的基于螺旋理论的机械臂伺服控制装置的组成结构示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其它实施例,都属于本发明保护的范围。
实施例:
请参阅图1,图1是本发明实施例中的基于螺旋理论的机械臂伺服控制方法的流程示意图。
如图1所示,一种基于螺旋理论的机械臂伺服控制方法,所述机械臂伺服控制方法包括:
S11:检测机械臂的关节转角数据,对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿矩阵;
在本发明具体实施过程中,所述检测机械臂的关节转角数据,对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿,包括:采用安装在关节电机轴上的光电编码器检测所述机械臂上关节的关节转角数据;对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿矩阵;所述指数积正向运动学计算的计算公式如下:
其中,Tc表示机械臂的实际末端位姿矩阵,xsn为机械臂第n个自由度的速度螺旋量,θsn表示检测到的机械臂第n个自由度的关节转角数据,i表示机械减速比,n表示自由度的个数,g0表示机械臂末端初始位姿矩阵。
具体的,通过安装在关节电机轴上的光电编码器可以检测到机械臂上的各个自由度的关节转角数据,在检测到各个自由度的关节转角数据之后,采用这些关节转角数据进行指数积正向运动学求解计算,从而得到机械臂相应的实际末端位姿矩阵,其中具体的计算公式如下:
其中,Tc表示机械臂的实际末端位姿矩阵,xsn为机械臂第n个自由度的速度螺旋量,θsn表示检测到的机械臂第n个自由度的关节转角数据,i表示机械减速比,n表示自由度的个数,g0表示机械臂末端初始位姿矩阵。
其中,n表示该机械臂包含的自由度的个数一个关节可包含1~3个自由度,关节数不等于自由度数,因此,n为大于或等于1的整数,n的取值具体由机械臂的自由度所决定,若有3个自由度,则n=3,四个自由度,则n=4,以此类推;机械减速比为电机输出轴转速与关节轴转速的比值,比如电机输出轴转速为1000转/分,机械减速比为100,则相应的关节轴转速为10转/分。
S12:根据所述实际末端位姿矩阵和给定目标位姿矩阵进行矩阵运算,获取所述机械臂的位置误差和姿态误差;
在本发明具体实施过程中,所述根据所述实际末端位姿矩阵和给定目标位姿矩阵进行矩阵运算,获取所述机械臂的位置误差和姿态误差,包括:
对所述实际末端位姿矩阵和所述给定目标位姿矩阵进行矩阵分块处理,获取实际末端姿态矩阵、实际末端位置向量、给定目标姿态矩阵和给定目标位置向量;
提取实际末端姿态矩阵、给定目标姿态矩阵、实际末端位置向量和给定目标位置向量进行矩阵运算,获取所述机械臂的位置误差和姿态误差;
其中,对所述实际末端位姿矩阵进行矩阵分块处理,获取实际末端姿态矩阵和实际末端位置向量的表示如下:
对所述给定目标位姿矩阵进行矩阵分块处理,获取给定目标姿态矩阵和给定目标位置向量的表示如下:
其中,Tc为实际末端位姿矩阵,Rc为实际末端姿态矩阵,Pc为实际末端位置向量,Tt为给定目标位姿矩阵,Rt为给定目标姿态矩阵,Pt为给定目标位置向量;Tc和Tt为4×4矩阵,Rc和Rt为3×3矩阵,Pc和Pt为3×1列向量。
进一步的,所述提取实际末端姿态矩阵、给定目标姿态矩阵、实际末端位置向量和给定目标位置向量进行矩阵运算,获取所述机械臂的位置误差和姿态误差,包括:
采用所述给定目标位置向量与所述实际末端位置向量进行矩阵运算,获取所述机械臂的位置误差;
采用所述给定目标姿态矩阵与所述实际末端姿态矩阵进行矩阵运算,获取所述机械臂的姿态误差;
其中,采用所述给定目标位置向量与所述实际末端位置向量进行矩阵运算的计算公式如下:
eP=||Pt-Pc||;
采用所述给定目标姿态矩阵与所述实际末端姿态矩阵进行矩阵运算的计算公式如下:
具体的,根据实际末端位姿矩阵和给定目标位姿矩阵进行矩阵运算,分别求出机械臂的位置误差和姿态误差;在具体的计算过程中,从实际末端位姿矩阵左上角取出3×3个元素组成实际末端姿态矩阵,再从实际末端位姿矩阵右上角取出3×1个元素组成实际末端位置向量;采用同样的方法,在给定目标位姿矩阵左上角取出3×3个元素组成给定目标姿态矩阵,在给定目标位姿矩阵右上角取出取出3×1个元素组成给定目标位置向量;然后通过既定的计算公式进行计算,获取机械臂的位置误差和姿态误差;具体的计算公式如下:
eP=||Pt-Pc||;
S13:获取所述位置误差和所述姿态误差中的最大值作为最大误差值;
在本发明具体实施过程中,从位置误差和姿态误差中选取最大值作为最大误差值,其中选取最大值的公式如下:
其中,e表示最大误差值,eP表示位置误差,er表示姿态误差。
S14:判断所述最大误差值是否小于预设误差阈值;
在本发明具体实施过程中,若判断所述最大误差值小于预设误差阈值时,返回检测机械臂的关节转角数据,对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿矩阵;若判断为否时,进入下一步骤。
具体的,在本发明实施过程中,预设有允许存在的误差值,但允许存在的误差值应该在可接受的范围内,且误差值可根据控制精度的要求进行调整,在具体实施过程中,采用最大误差值与允许存在的误差值进行比较,判断最大误差值是否大于允许存在的误差值。
S15:若否,根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行计算,获取纠正偏差所需要的关节角转动量;
在本发明具体实施过程中,所述根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行计算,获取纠正偏差所需要的关节角转动量,包括:根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行速度旋量计算,获取速度旋量;采用所述速度旋量左乘雅克比矩阵的Moore-Penrose伪逆,获取关节空间速度;采用所述关节空间速度与给定的时间微元相乘,获取纠正偏差所需要的关节角转动量。
进一步的,所述根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行速度旋量计算的计算公式如下:
其中,x表示速度旋量,Pc表示实际末端位置向量,Pt表示给定目标位置向量,表示实际末端姿态矩阵的转置,Rt表示给定目标姿态矩阵,v表示线速度列向量,ω表示角速度列向量,vlim表示线速度限值,ωlim表示角速度限值。
具体的,在知道所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值的情况下,通过以下公式计算速度旋量,计算公式如下:
其中,x表示速度旋量,Pc表示实际末端位置向量,Pt表示给定目标位置向量,表示实际末端姿态矩阵的转置,Rt表示给定目标姿态矩阵,v表示线速度列向量,ω表示角速度列向量,vlim表示线速度限值,ωlim表示角速度限值。
在获取到速度旋量之后,采用速度旋量左乘雅克比矩阵的Moore-Penrose伪逆,即可获取关节空间速度;采用所述关节空间速度与给定的时间微元相乘,即可获取纠正偏差所需要的关节角转动量。
具体的给定的时间微元根据具体的控制精度要求而确定。
S16:将所述纠正偏差所需要的关节角转动量与当前关节角转动量进行叠加,获取机械臂关节的关节角跟新数据;
在本发明具体实施过程中,所述将所述纠正偏差所需要的关节角转动量与当前关节角转动量进行叠加的计算公式如下:
通过上述叠加公式,可以计算机械臂关节的关节角更新数据。
S17:根据所述关节角更新数据控制所述机械臂关节进行位置更新。
在本发明具体实施过程中,在获取到关节角更新数据之后,将关节角更新数据输入到机器人控制装置中的驱动器上,驱动器将关节角更新数据转化为控制脉冲序列,并将控制脉冲序列通过电机控制端口输入电机中,实现位置更新。
在本发明具体实施例中,采用基于螺旋理论的数值算法进行机械臂的伺服控制,避免了运动学逆解的反复解算和多组逆解的择优过程,具有计算量小、实时性好、跟踪准确的优势,有效的提高对机械臂的控制精度;并提高机械臂的动态性能,可有效提高机械臂的运行效率。
实施例:
请参阅图2,图2是本发明实施例中的基于螺旋理论的机械臂伺服控制系统的结构组成示意图;
如图2所示,一种基于螺旋理论的机械臂伺服控制系统,所述机械臂伺服控制系统包括:
第一计算模块11:用于检测机械臂的关节转角数据,对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿矩阵;
在本发明具体实施过程中,所述检测机械臂的关节转角数据,对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿,包括:采用安装在关节电机轴上的光电编码器检测所述机械臂上关节的关节转角数据;对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿矩阵;所述指数积正向运动学计算的计算公式如下:
其中,Tc表示机械臂的实际末端位姿矩阵,xsn为机械臂第n个自由度的速度螺旋量,θsn表示检测到的机械臂第n个自由度的关节转角数据,i表示机械减速比,n表示自由度的个数,g0表示机械臂末端初始位姿矩阵。
具体的,通过安装在关节电机轴上的光电编码器可以检测到机械臂上的各个自由度的关节转角数据,在检测到各个自由度的关节转角数据之后,采用这些关节转角数据进行指数积正向运动学求解计算,从而得到机械臂相应的实际末端位姿矩阵,其中具体的计算公式如下:
其中,Tc表示机械臂的实际末端位姿矩阵,xsn为机械臂第n个自由度的速度螺旋量,θsn表示检测到的机械臂第n个自由度的关节转角数据,i表示机械减速比,n表示自由度的个数,g0表示机械臂末端初始位姿矩阵。
其中,n表示该机械臂包含的自由度的个数一个关节可包含1~3个自由度,关节数不等于自由度数,因此,n为大于或等于1的整数,n的取值具体由机械臂的自由度所决定,若有3个自由度,则n=3,四个自由度,则n=4,以此类推。
矩阵运算模块12:用于根据所述实际末端位姿矩阵和给定目标位姿矩阵进行矩阵运算,获取所述机械臂的位置误差和姿态误差;
在本发明具体实施过程中,所述根据所述实际末端位姿矩阵和给定目标位姿矩阵进行矩阵运算,获取所述机械臂的位置误差和姿态误差,包括:
对所述实际末端位姿矩阵和所述给定目标位姿矩阵进行矩阵分块处理,获取实际末端姿态矩阵、实际末端位置向量、给定目标姿态矩阵和给定目标位置向量;
提取实际末端姿态矩阵、给定目标姿态矩阵、实际末端位置向量和给定目标位置向量进行矩阵运算,获取所述机械臂的位置误差和姿态误差;
其中,对所述实际末端位姿矩阵进行矩阵分块处理,获取实际末端姿态矩阵和实际末端位置向量的表示如下:
对所述给定目标位姿矩阵进行矩阵分块处理,获取给定目标姿态矩阵和给定目标位置向量的表示如下:
其中,Tc为实际末端位姿矩阵,Rc为实际末端姿态矩阵,Pc为实际末端位置向量,Tt为给定目标位姿矩阵,Rt为给定目标姿态矩阵,Pt为给定目标位置向量;Tc和Tt为4×4矩阵,Rc和Rt为3×3矩阵,Pc和Pt为3×1列向量。
进一步的,所述提取实际末端姿态矩阵、给定目标姿态矩阵、实际末端位置向量和给定目标位置向量进行矩阵运算,获取所述机械臂的位置误差和姿态误差,包括:
采用所述给定目标位置向量与所述实际末端位置向量进行矩阵运算,获取所述机械臂的位置误差;
采用所述给定目标姿态矩阵与所述实际末端姿态矩阵进行矩阵运算,获取所述机械臂的姿态误差;
其中,采用所述给定目标位置向量与所述实际末端位置向量进行矩阵运算的计算公式如下:
eP=||Pt-Pc||;
采用所述给定目标姿态矩阵与所述实际末端姿态矩阵进行矩阵运算的计算公式如下:
具体的,根据实际末端位姿矩阵和给定目标位姿矩阵进行矩阵运算,分别求出机械臂的位置误差和姿态误差;在具体的计算过程中,从实际末端位姿矩阵左上角取出3×3个元素组成实际末端姿态矩阵,再从实际末端位姿矩阵右上角取出3×1个元素组成实际末端位置向量;采用同样的方法,在给定目标位姿矩阵左上角取出3×3个元素组成给定目标姿态矩阵,在给定目标位姿矩阵右上角取出取出3×1个元素组成给定目标位置向量;然后通过既定的计算公式进行计算,获取机械臂的位置误差和姿态误差;具体的计算公式如下:
eP=||Pt-Pc||;
判断模块13:用于获取所述位置误差和所述姿态误差中的最大值作为最大误差值,并判断所述最大误差值是否小于预设误差阈值;
在本发明具体实施过程中,从位置误差和姿态误差中选取最大值作为最大误差值,其中选取最大值的公式如下:
其中,e表示最大误差值,eP表示位置误差,er表示姿态误差。
在本发明具体实施过程中,若判断所述最大误差值小于预设误差阈值时,返回检测机械臂的关节转角数据,对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿矩阵;若判断为否时,进入下一步骤。
具体的,在本发明实施过程中,预设有允许存在的误差值,但允许存在的误差值应该在可接受的范围内,且误差值可根据控制精度的要求进行调整,在具体实施过程中,采用最大误差值与允许存在的误差值进行比较,判断最大误差值是否大于允许存在的误差值。
第二计算模块14:用于若判断为否时,根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行计算,获取纠正偏差所需要的关节角转动量;
在本发明具体实施过程中,所述根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行计算,获取纠正偏差所需要的关节角转动量,包括:根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行速度旋量计算,获取速度旋量;采用所述速度旋量左乘雅克比矩阵的Moore-Penrose伪逆,获取关节空间速度;采用所述关节空间速度与给定的时间微元相乘,获取纠正偏差所需要的关节角转动量。
进一步的,所述根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行速度旋量计算的计算公式如下:
其中,x表示速度旋量,Pc表示实际末端位置向量,Pt表示给定目标位置向量,表示实际末端姿态矩阵的转置,Rt表示给定目标姿态矩阵,v表示线速度列向量,ω表示角速度列向量,vlim表示线速度限值,ωlim表示角速度限值。
具体的,在知道所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值的情况下,通过以下公式计算速度旋量,计算公式如下:
其中,x表示速度旋量,Pc表示实际末端位置向量,Pt表示给定目标位置向量,表示实际末端姿态矩阵的转置,Rt表示给定目标姿态矩阵,v表示线速度列向量,ω表示角速度列向量,vlim表示线速度限值,ωlim表示角速度限值。
在获取到速度旋量之后,采用速度旋量左乘雅克比矩阵的Moore-Penrose伪逆,即可获取关节空间速度;采用所述关节空间速度与给定的时间微元相乘,即可获取纠正偏差所需要的关节角转动量。
雅克比矩阵的Moore-Penrose伪逆的表示方式为J+,其中,J+=JT(JJT)-1,因此θ=J+x;
具体的给定的时间微元根据具体的控制精度要求而确定。
叠加模块15:用于将所述纠正偏差所需要的关节角转动量与当前关节角转动量进行叠加,获取机械臂关节的关节角跟新数据;
在本发明具体实施过程中,所述将所述纠正偏差所需要的关节角转动量与当前关节角转动量进行叠加的计算公式如下:
通过上述叠加公式,可以计算机械臂关节的关节角更新数据。
更新模块16:用于根据所述关节角跟新数据控制所述机械臂关节进行位置更新。
在本发明具体实施过程中,在获取到关节角更新数据之后,将关节角更新数据输入到机器人控制装置中的驱动器上,驱动器将关节角更新数据转化为控制脉冲序列,并将控制脉冲序列通过电机控制端口输入电机中,实现位置更新。
在本发明具体实施例中,采用基于旋量理论的数值算法进行机械臂的伺服控制,避免了运动学逆解的反复解算和多组逆解的择优过程,具有计算量小、实时性好、跟踪准确的优势,有效的提高对机械臂的控制精度;并提高机械臂的动态性能,可有效提高机械臂的运行效率。
实施例:
请参阅图3,图3是本发明实施例中的基于螺旋理论的机械臂伺服控制装置的组成结构示意图。
如图3所示,一种基于螺旋理论的机械臂伺服控制装置,其组成包括:处理器302、存储器303、驱动器306、输入单元304以及显示单元305等器件。图3所示的结构器件并不构成对所有的基于螺旋理论的机械臂伺服控制装置的限定,可以比图3使用更少或更多部件,或者组合某些部件。
存储器303可用于存储应用程序301以及各功能模块。处理器302运行存储在存储器303的应用程序301,从而执行设备的各种功能应用以及数据处理。存储器可以是内存储器或外存储器,或者包括内存储器和外存储器两者。内存储器可以包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦写可编程ROM(EEPROM)、快闪存储器、或者随机存储器。外存储器可以包括硬盘、软盘、ZIP盘、U盘、磁带等。本发明所公开的存储器包括但不限于这些类型的存储器。本发明所公开的存储器只作为例子而非作为限定。
驱动器306用于接收处理器302处理之后产生的相应的驱动指令,并相应该指令,驱动机器人上的对应电机进行相应的工作。
输入单元304用于接收给定的机械臂末端目标轨迹曲线的输入,以及接收用户输入的关键字。输入单元304可包括触控面板以及其它输入设备。显示单元305可用于显示用户输入的信息或提供给用户的信息以及终端设备的各种菜单。显示单元305可采用液晶显示器、有机发光二极管等形式。处理器302是终端设备的控制中心,利用各种接口和线路连接整个设备的各个部分,通过运行或执行存储在存储器302内的软件程序和/或模块,以及调用存储在存储器内的数据,执行各种功能和处理数据。
作为一个实施例,所述机械臂伺服控制装置包括:一个或多个处理器(上302,存储器303,一个或多个应用程序301,其中所述一个或多个应用程序301被存储在存储器303中并被配置为由所述一个或多个处理器302执行,所述一个或多个应用程序301配置用于执行上述实施例中的机械臂伺服控制方法。
本发明实施例提供的基于螺旋理论的机械臂伺服控制装置可实现上述提供的机械臂伺服控制方法的实施例,具体功能实现请参详方法实施例中的说明,在此不再赘述。
在本发明具体实施例中,采用基于旋量理论的数值算法进行机械臂的伺服控制,避免了运动学逆解的反复解算和多组逆解的择优过程,具有计算量小、实时性好、跟踪准确的优势,有效的提高对机械臂的控制精度;并提高机械臂的动态性能,可有效提高机械臂的运行效率。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成,该程序可以存储于一计算机可读存储介质中,存储介质可以包括:只读存储器(ROM,ReadOnly Memory)、随机存取存储器(RAM,Random AccessMemory)、磁盘或光盘等。
另外,以上对本发明实施例所提供的一种基于螺旋理论的机械臂伺服控制方法、系统及装置进行了详细介绍,本文中应采用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想;同时,对于本领域的一般技术人员,依据本发明的思想,在具体实施方式及应用范围上均会有改变之处,综上所述,本说明书内容不应理解为对本发明的限制。
Claims (9)
1.一种基于螺旋理论的机械臂伺服控制方法,其特征在于,所述机械臂伺服控制方法包括:
检测机械臂的关节转角数据,对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿矩阵;
根据所述实际末端位姿矩阵和给定目标位姿矩阵进行矩阵运算,获取所述机械臂的位置误差和姿态误差;
获取所述位置误差和所述姿态误差中的最大值作为最大误差值,并判断所述最大误差值是否小于预设误差阈值;
若否,根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行计算,获取纠正偏差所需要的关节角转动量;
将所述纠正偏差所需要的关节角转动量与当前关节角转动量进行叠加,获取机械臂关节的关节角更新数据;
根据所述关节角更新数据控制所述机械臂关节进行位置更新;
所述根据所述实际末端位姿矩阵和给定目标位姿矩阵进行矩阵运算,获取所述机械臂的位置误差和姿态误差,包括:
对所述实际末端位姿矩阵和所述给定目标位姿矩阵进行矩阵分块处理,获取实际末端姿态矩阵、实际末端位置向量、给定目标姿态矩阵和给定目标位置向量;
提取实际末端姿态矩阵、给定目标姿态矩阵、实际末端位置向量和给定目标位置向量进行矩阵运算,获取所述机械臂的位置误差和姿态误差;
其中,对所述实际末端位姿矩阵进行矩阵分块处理,获取实际末端姿态矩阵和实际末端位置向量的表示如下:
对所述给定目标位姿矩阵进行矩阵分块处理,获取给定目标姿态矩阵和给定目标位置向量的表示如下:
其中,Tc为实际末端位姿矩阵,Rc为实际末端姿态矩阵,Pc为实际末端位置向量,Tt为给定目标位姿矩阵,Rt为给定目标姿态矩阵,Pt为给定目标位置向量;Tc和Tt为4×4矩阵,Rc和Rt为3×3矩阵,Pc和Pt为3×1列向量。
3.根据权利要求1所述的机械臂伺服控制方法,其特征在于,所述提取实际末端姿态矩阵、给定目标姿态矩阵、实际末端位置向量和给定目标位置向量进行矩阵运算,获取所述机械臂的位置误差和姿态误差,包括:
采用所述给定目标位置向量与所述实际末端位置向量进行矩阵运算,获取所述机械臂的位置误差;
采用所述给定目标姿态矩阵与所述实际末端姿态矩阵进行矩阵运算,获取所述机械臂的姿态误差;
其中,采用所述给定目标位置向量与所述实际末端位置向量进行矩阵运算的计算公式如下:
eP=||Pt-Pc||;
采用所述给定目标姿态矩阵与所述实际末端姿态矩阵进行矩阵运算的计算公式如下:
4.根据权利要求1所述的机械臂伺服控制方法,其特征在于,所述机械臂伺服控制方法还包括:
若判断所述最大误差值小于预设误差阈值时,返回检测机械臂的关节转角数据,对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿矩阵。
5.根据权利要求1所述的机械臂伺服控制方法,其特征在于,所述根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行计算,获取纠正偏差所需要的关节角转动量,包括:
根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行速度旋量计算,获取速度旋量;
采用所述速度旋量左乘雅克比矩阵的Moore-Penrose伪逆,获取关节空间速度;
采用所述关节空间速度与给定的时间微元相乘,获取纠正偏差所需要的关节角转动量。
8.一种基于螺旋理论的机械臂伺服控制系统,其特征在于,所述机械臂伺服控制系统包括:
第一计算模块:用于检测机械臂的关节转角数据,对所述关节转角数据进行指数积正向运动学计算,获取机械臂的实际末端位姿矩阵;
矩阵运算模块:用于根据所述实际末端位姿矩阵和给定目标位姿矩阵进行矩阵运算,获取所述机械臂的位置误差和姿态误差;
判断模块:用于获取所述位置误差和所述姿态误差中的最大值作为最大误差值,并判断所述最大误差值是否小于预设误差阈值;
第二计算模块:用于若判断为否时,根据所述位置误差、所述姿态误差、机械臂的线速度限值和机械臂的角速度限值进行计算,获取纠正偏差所需要的关节角转动量;
叠加模块:用于将所述纠正偏差所需要的关节角转动量与当前关节角转动量进行叠加,获取机械臂关节的关节角更新数据;
更新模块:用于根据所述关节角更新数据控制所述机械臂关节进行位置更新;
矩阵运算模块:还用于对所述实际末端位姿矩阵和所述给定目标位姿矩阵进行矩阵分块处理,获取实际末端姿态矩阵、实际末端位置向量、给定目标姿态矩阵和给定目标位置向量;提取实际末端姿态矩阵、给定目标姿态矩阵、实际末端位置向量和给定目标位置向量进行矩阵运算,获取所述机械臂的位置误差和姿态误差;其中,对所述实际末端位姿矩阵进行矩阵分块处理,获取实际末端姿态矩阵和实际末端位置向量的表示如下:
对所述给定目标位姿矩阵进行矩阵分块处理,获取给定目标姿态矩阵和给定目标位置向量的表示如下:
其中,Tc为实际末端位姿矩阵,Rc为实际末端姿态矩阵,Pc为实际末端位置向量,Tt为给定目标位姿矩阵,Rt为给定目标姿态矩阵,Pt为给定目标位置向量;Tc和Tt为4×4矩阵,Rc和Rt为3×3矩阵,Pc和Pt为3×1列向量。
9.一种基于螺旋理论的机械臂伺服控制装置,其特在在于,其包括:
一个或多个处理器;
存储器;
一个或多个应用程序,其中所述一个或多个应用程序被存储在所述存储器中并被配置为由所述一个或多个处理器执行,所述一个或多个应用程序配置用于:执行根据权利要求1至7中任意一项机械臂伺服控制方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811190619.8A CN109483529B (zh) | 2018-10-12 | 2018-10-12 | 一种基于螺旋理论的机械臂伺服控制方法、系统及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811190619.8A CN109483529B (zh) | 2018-10-12 | 2018-10-12 | 一种基于螺旋理论的机械臂伺服控制方法、系统及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109483529A CN109483529A (zh) | 2019-03-19 |
CN109483529B true CN109483529B (zh) | 2021-02-26 |
Family
ID=65689669
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811190619.8A Active CN109483529B (zh) | 2018-10-12 | 2018-10-12 | 一种基于螺旋理论的机械臂伺服控制方法、系统及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109483529B (zh) |
Families Citing this family (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110801238B (zh) * | 2019-11-20 | 2022-07-01 | 上海交通大学 | 一种下肢多关节等速训练测试装置 |
CN110801239B (zh) * | 2019-11-20 | 2022-06-17 | 上海交通大学 | 一种上肢多关节等速训练测试装置 |
CN113146616B (zh) * | 2021-02-25 | 2023-08-04 | 北京空间飞行器总体设计部 | 一种四自由度机械臂视觉伺服控制方法 |
CN113021408B (zh) * | 2021-03-23 | 2022-08-05 | 中联重科股份有限公司 | 补偿臂架挠度的方法和装置及控制臂架的方法和装置 |
CN113268055B (zh) * | 2021-04-07 | 2023-01-13 | 北京拓疆者智能科技有限公司 | 工程车辆避障控制方法、装置及机械设备 |
CN114290331B (zh) * | 2021-12-14 | 2023-06-23 | 中国科学院深圳先进技术研究院 | 机器人运动控制方法、机器人及计算机可读存储装置 |
CN114800523B (zh) * | 2022-05-26 | 2023-12-01 | 江西省智能产业技术创新研究院 | 机械臂轨迹修正方法、系统、计算机及可读存储介质 |
CN115674207B (zh) * | 2022-12-14 | 2023-03-14 | 杭州景业智能科技股份有限公司 | 机械臂关节耦合误差补偿方法、装置、设备和存储介质 |
CN117123520A (zh) * | 2023-02-06 | 2023-11-28 | 荣耀终端有限公司 | 实现对目标工件擦胶的方法及擦胶设备 |
CN116512278B (zh) * | 2023-06-26 | 2023-12-01 | 极限人工智能(北京)有限公司 | 基于虚拟目标点的机械臂末端直线运动控制方法及系统 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9205555B2 (en) * | 2011-03-22 | 2015-12-08 | Harris Corporation | Manipulator joint-limit handling algorithm |
CN106647282A (zh) * | 2017-01-19 | 2017-05-10 | 北京工业大学 | 一种考虑末端运动误差的六自由度机器人轨迹规划方法 |
CN107038275A (zh) * | 2016-12-19 | 2017-08-11 | 中国科学院沈阳自动化研究所 | 一种机械臂误差分析方法 |
CN107756400A (zh) * | 2017-10-13 | 2018-03-06 | 北京工业大学 | 一种基于旋量理论的6r机器人逆运动学几何求解方法 |
CN108621162A (zh) * | 2018-05-09 | 2018-10-09 | 广西科技大学 | 一种机械臂运动规划方法 |
-
2018
- 2018-10-12 CN CN201811190619.8A patent/CN109483529B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9205555B2 (en) * | 2011-03-22 | 2015-12-08 | Harris Corporation | Manipulator joint-limit handling algorithm |
CN107038275A (zh) * | 2016-12-19 | 2017-08-11 | 中国科学院沈阳自动化研究所 | 一种机械臂误差分析方法 |
CN106647282A (zh) * | 2017-01-19 | 2017-05-10 | 北京工业大学 | 一种考虑末端运动误差的六自由度机器人轨迹规划方法 |
CN107756400A (zh) * | 2017-10-13 | 2018-03-06 | 北京工业大学 | 一种基于旋量理论的6r机器人逆运动学几何求解方法 |
CN108621162A (zh) * | 2018-05-09 | 2018-10-09 | 广西科技大学 | 一种机械臂运动规划方法 |
Non-Patent Citations (1)
Title |
---|
基于旋量理论的农用自动化系统末端执行构件运动精度分析;程秀玲;《山东农业大学学报(自然科学版)》;20141215;第45卷(第5期);720-723 * |
Also Published As
Publication number | Publication date |
---|---|
CN109483529A (zh) | 2019-03-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN109483529B (zh) | 一种基于螺旋理论的机械臂伺服控制方法、系统及装置 | |
CN109159151B (zh) | 一种机械臂空间轨迹跟踪动态补偿方法和系统 | |
US10259118B2 (en) | Robot system having function of simplifying teaching operation and improving operating performance by learning | |
EP3740352B1 (en) | Vision-based sensor system and control method for robot arms | |
CN111230867B (zh) | 机器人运动控制方法、运动控制设备及机器人 | |
US11059181B2 (en) | Control apparatus, robot, and robot system | |
CN108527375B (zh) | 控制系统、控制器、控制方法及记录媒体 | |
Oikawa et al. | Reinforcement learning for robotic assembly using non-diagonal stiffness matrix | |
CN109571481A (zh) | 一种关节奇异点处理方法、装置、设备和存储介质 | |
CN114131611B (zh) | 机器人重力位姿分解的关节误差离线补偿方法、系统及终端 | |
JP2566665B2 (ja) | 慣性系におけるロボットの制御装置 | |
US10035264B1 (en) | Real time robot implementation of state machine | |
CN110891741A (zh) | 对机器人单元的控制 | |
JP2015003348A (ja) | ロボット制御システム、制御装置、ロボット、ロボット制御システムの制御方法及びロボットの制御方法 | |
CN114378830B (zh) | 一种机器人腕关节奇异规避方法及系统 | |
CN112894801B (zh) | 一种双轴同导轨设备回零方法和装置 | |
CN109634273A (zh) | 可移动设备的跟随控制方法及装置 | |
US20180361573A1 (en) | Anti-shake method of robot and robot thereof | |
CN113927603A (zh) | 机械臂拖动控制方法、装置、计算机设备以及存储介质 | |
KR101411503B1 (ko) | 로봇 제어 장치 및 그 방법 | |
CN112440282A (zh) | 一种舵机控制方法、装置、电子设备及存储介质 | |
Huang et al. | Development and analysis of 5-DOF manipulator kinematics | |
CN113459103B (zh) | 一种机械手自动运行时的转角轨迹控制方法及装置 | |
CN108000520A (zh) | 机器人的运动控制方法及系统 | |
CN116852377A (zh) | 机器人关节的抖动数据的确定方法、装置和计算机设备 |
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 |