CN114800534A - 一种机械臂控制方法及装置 - Google Patents
一种机械臂控制方法及装置 Download PDFInfo
- Publication number
- CN114800534A CN114800534A CN202210746684.4A CN202210746684A CN114800534A CN 114800534 A CN114800534 A CN 114800534A CN 202210746684 A CN202210746684 A CN 202210746684A CN 114800534 A CN114800534 A CN 114800534A
- Authority
- CN
- China
- Prior art keywords
- joint
- angle
- point
- coordinate
- mechanical arm
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 56
- 230000014509 gene expression Effects 0.000 claims description 73
- 239000013598 vector Substances 0.000 claims description 44
- 238000004364 calculation method Methods 0.000 claims description 26
- 238000004891 communication Methods 0.000 claims description 19
- 238000004590 computer program Methods 0.000 claims description 13
- 101100161752 Mus musculus Acot11 gene Proteins 0.000 claims description 10
- 239000011159 matrix material Substances 0.000 claims description 5
- 230000008569 process Effects 0.000 description 13
- 238000006243 chemical reaction Methods 0.000 description 10
- 238000010586 diagram Methods 0.000 description 10
- 230000009471 action Effects 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 238000009795 derivation Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 2
- 241000764238 Isis Species 0.000 description 1
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 238000013500 data storage Methods 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 239000000758 substrate Substances 0.000 description 1
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
- B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Numerical Control (AREA)
- Manipulator (AREA)
Abstract
本发明实施例提供了一种机械臂控制方法及装置,涉及数据处理技术邻域,上述方法包括:在机械臂的基座对应的基座坐标系下建立机械臂的正运动学模型;根据正运动学模型确定机械臂末端在基座坐标系下的位姿数据与机械臂中各个关节的转动角度之间的第一关系;获得机械臂末端在基座坐标系下的期望位姿数据;根据正运动学模型、第一关系以及期望位姿数据,计算对机械臂中各个关节进行转动的目标角度;控制各个关节按照对应的目标角度转动,以使得机械臂末端在基座坐标系下的位姿数据为期望位姿数据。应用本发明实施例提供的方案控制机械臂,能够提高控制机械臂的效率。
Description
技术领域
本发明涉及数据处理技术领域,特别是涉及一种机械臂控制方法及装置。
背景技术
机器人可以安装有多关节的机械臂,机械臂中每一关节可绕自身轴线进行转动,在机器人工作过程中,机器人可以获得机械臂末端的期望位姿数据,通过控制关节转动来调整机械臂的位姿,使得机械臂末端的位姿数据为所设置的期望位姿数据,从而实现对机械臂的控制。
机器人控制机械臂的关节转动时,通常需要预先建立各个关节对应的关节坐标系,并计算各个相邻关节对应的关节坐标系之间的转换关系,根据上述期望位姿数据以及各个转换关系,计算各个关节相对于预先设置的基准位置之间的目标转动角度,控制各个关节按照对应的目标转动角度转动,实现对机械臂的控制。
然而,计算每一关节的目标转动角度均需考虑各个关节坐标系之间的转换关系,这样计算目标转动角度的计算量较大,获得各个关节的目标转动角度的效率较低,从而导致对机械臂进行控制的效率较低。
发明内容
本发明实施例的目的在于提供一种机器人控制方法及装置,以提高控制机械臂的效率。具体技术方案如下:
第一方面,本发明实施例提供了一种机械臂控制方法,所述方法包括:
在机械臂的基座对应的基座坐标系下建立机械臂的正运动学模型,其中,在所述正运动学模型中所述机械臂各个关节相对于预设基准位置的转动角度为零;
根据所述正运动学模型确定所述机械臂末端在基座坐标系下的位姿数据与所述机械臂中各个关节的转动角度之间的第一关系;
获得所述机械臂末端在所述基座坐标系下的期望位姿数据;
根据所述正运动学模型、第一关系以及期望位姿数据,计算对所述机械臂中各个关节进行转动的目标角度;
控制各个关节按照对应的目标角度转动,以使得所述机械臂末端在基座坐标系下的位姿数据为所述期望位姿数据。
本发明的一个实施例中,所述第一关系按照以下表达式表示:
其中,所述为所述机械臂末端在所述基座坐标系下的位姿数据,所述N为所述机械臂中关节总数量,所述为自然常数,所述为第n个关节在所述正运动学模型中的运动旋量,所述为第n个关节相对于所述预设基准位置的转动角度,所述为所述机械臂末端在所述正运动学模型中的初始位姿数据。
本发明的一个实施例中,所述机械臂配置有六个关节,第一关节、第二关节、第三关节、第四关节、第五关节和第六关节通过连杆依次连接,所述第一关节的一端与所述机械臂基座相连,所述第六关节的一端与所述机械臂末端相连,所述第二关节、第三关节以及第四关节的轴线平行;
所述根据所述正运动学模型、第一关系以及期望位姿数据,计算对所述机械臂中各个关节进行转动的目标角度,包括:
在所述正运动学模型中获得所述第五关节和第六关节的轴线交点的交点坐标以及平行关节的关节轴线向量,其中,所述平行关节为:所述第二关节、第三关节或第四关节;
根据所述第一关系、期望位姿数据、交点坐标以及关节轴线向量,获得对所述第一关节进行转动的第一角度;
根据所述期望位姿数据和所述第一角度,计算所述机械臂末端在转动第一角度后的第一关节对应的目标关节坐标系下的期望坐标;
基于预先获得的第四关系,根据计算得到的期望坐标,获得对所述第五关节进行转动的第五角度,其中,所述第四关系为:所述机械臂末端在所述目标关节坐标系下的坐标与所述第五关节的转动角度之间的关系;
根据所述第一角度、第五角度以及期望位姿数据,获得对所述第六关节进行转动的第六角度;
根据所述第一角度、第五角度、第六角度、第一关系以及所述第四关节在所述正运动学模型中的关节坐标,预测所述第四关节经所述第二关节和第三关节转动后在所述基座坐标系下的关节坐标;
根据所述第二关节、第三关节、第四关节在所述正运动学模型中的关节坐标以及预测得到的关节坐标,获得对所述第二关节进行转动的第二角度和对所述第三关节进行转动的第三角度;
基于预先获得的所述第二关节、第三关节以及第四关节的转动角度之间的第五关系,根据计算得到的第二角度和第三角度,获得对所述第四关节进行转动的第四角度。
本发明的一个实施例中,所述根据所述第一关系、期望位姿数据、交点坐标以及关节轴线向量,获得对所述第一关节进行转动的第一角度,包括:
根据所述第一关系、期望位姿数据和交点坐标,预测所述轴线交点经所述第一关节、第二关节、第三关节以及第四关节转动后在基座坐标系统下对应的第一坐标点的第一坐标;
根据所述交点坐标、关节轴线向量、第一坐标以及预先获得的点轴位置关系,预测所述轴线交点经所述第二关节、第三关节以及第四关节转动后在所述基座坐标系下对应的第二坐标点的第二坐标,其中,所述点轴位置关系为:所述轴线交点、第一坐标点、第二坐标点以及所述第一关节的轴线之间的位置关系;
基于预先获得的第三关系,根据所述第一坐标、第二坐标,获得对所述第一关节进行转动的第一角度,其中,所述第三关系为:所述第一坐标点的坐标、第二坐标点的坐标以及所述第一关节的转动角度之间的对应关系。
本发明的一个实施例中,所述点轴位置关系按照以下表达式表示:
其中,所述为所述第一坐标点在所述基座坐标系下的坐标,所述为所述第一关节的轴线上任一点在所述基座坐标系下的坐标,所述为所述第二坐标点在所述基座坐标系下的坐标,所述为所述交点坐标,所述为所述关节轴线向量。
本发明的一个实施例中,所述第三关系按照以下表达式表示:
本发明的一个实施例中,所述目标关节坐标系的第一坐标轴方向与所述第二关节的轴线方向之间的夹角为所述第一角度;
所述第四关系按照以下表达式表示:
其中,所述为所述机械臂末端在所述第一坐标轴上的坐标,所述为第一端点与第二端点之间的距离,所述第一端点为所述第五关节中与所述第六关节相连的端点,所述第二端点为所述第六关节末端端点,所述为所述第五角度,所述为所述第三端点与第四端点之间的距离,所述第三端点为所述第四关节中与所述第三关节相连的端点,所述第四端点为所述第四关节中与所述第五关节相连的端点。
本发明的一个实施例中,所述根据所述第一角度、第五角度、第六角度、第一关系以及所述第四关节在所述正运动学模型中的关节坐标,预测所述第四关节经所述第二关节和第三关节转动后在所述基座坐标系下的关节坐标,包括:
按照以下表达式进行关节坐标预测:
其中,所述为所述第四关节经所述第二关节和第三关节转动后在所述基座坐标系下的关节坐标,所述为所述第四关节的关节坐标,所述为自然常数,所述至分别为:所述第一关节至第六关节的轴线在正运动学模型中的运动旋量,所述至分别为第一角度至第六角度,所述为所述期望位姿数据,所述为所述机械臂末端在所述正运动学模型中的初始位姿数据的逆矩阵。
第二方面,本发明实施例还提供了一种机械臂控制装置,所述装置包括:
模型建立模块,用于在机械臂的基座对应的基座坐标系下建立机械臂的正运动学模型,其中,在所述正运动学模型中所述机械臂各个关节相对于预设基准位置的转动角度为零;
关系确定模块,用于根据所述正运动学模型确定所述机械臂末端在基座坐标系下的位姿数据与所述机械臂中各个关节的转动角度之间的第一关系;
数据获得模块,用于获得所述机械臂末端在所述基座坐标系下的期望位姿数据;
角度计算模块,用于根据所述正运动学模型、第一关系以及期望位姿数据,计算对所述机械臂中各个关节进行转动的目标角度;
关节控制模块,用于控制各个关节按照对应的目标角度转动,以使得所述机械臂末端在基座坐标系下的位姿数据为所述期望位姿数据。
本发明的一个实施例中,所述第一关系按照以下表达式表示:
其中,所述为所述机械臂末端在所述基座坐标系下的位姿数据,所述N为所述机械臂中关节总数量,所述为自然常数,所述为第n个关节在所述正运动学模型中的运动旋量,所述为第n个关节相对于所述预设基准位置的转动角度,所述为所述机械臂末端在所述正运动学模型中的初始位姿数据。
本发明的一个实施例中,所述机械臂配置有六个关节,第一关节、第二关节、第三关节、第四关节、第五关节和第六关节通过连杆依次连接,所述第一关节的一端与所述机械臂基座相连,所述第六关节的一端与所述机械臂末端相连,所述第二关节、第三关节以及第四关节的轴线平行;
所述角度计算模块,包括:
数据获得子模块,用于在所述正运动学模型中获得所述第五关节和第六关节的轴线交点的交点坐标以及平行关节的关节轴线向量,其中,所述平行关节为:所述第二关节、第三关节或第四关节;
第一获得子模块,用于根据所述第一关系、期望位姿数据、交点坐标以及关节轴线向量,获得对所述第一关节进行转动的第一角度;
第一计算模块,用于根据所述期望位姿数据和所述第一角度,计算所述机械臂末端在转动第一角度后的第一关节对应的目标关节坐标系下的期望坐标;
第二获得子模块,用于于预先获得的第四关系,根据计算得到的期望坐标,获得对所述第五关节进行转动的第五角度,其中,所述第四关系为:所述机械臂末端在所述目标关节坐标系下的坐标与所述第五关节的转动角度之间的关系;
第三获得子模块,用于根据所述第一角度、第五角度以及期望位姿数据,获得对所述第六关节进行转动的第六角度;
坐标预测子模块,用于根据所述第一角度、第五角度、第六角度、第一关系以及所述第四关节在所述正运动学模型中的关节坐标,预测所述第四关节经所述第二关节和第三关节转动后在所述基座坐标系下的关节坐标;
第四获得子模块,用于根据所述第二关节、第三关节、第四关节在所述正运动学模型中的关节坐标以及预测得到的关节坐标,获得对所述第二关节进行转动的第二角度和对所述第三关节进行转动的第三角度;
第五获得子模块,用于基于预先获得的所述第二关节、第三关节以及第四关节的转动角度之间的第五关系,根据计算得到的第二角度和第三角度,获得对所述第四关节进行转动的第四角度。
本发明的一个实施例中,所述第一获得子模块,具体用于:
根据所述第一关系、期望位姿数据和交点坐标,预测所述轴线交点经所述第一关节、第二关节、第三关节以及第四关节转动后在基座坐标系统下对应的第一坐标点的第一坐标;
根据所述交点坐标、关节轴线向量、第一坐标以及预先获得的点轴位置关系,预测所述轴线交点经所述第二关节、第三关节以及第四关节转动后在所述基座坐标系下对应的第二坐标点的第二坐标,其中,所述点轴位置关系为:所述轴线交点、第一坐标点、第二坐标点以及所述第一关节的轴线之间的位置关系;
基于预先获得的第三关系,根据所述第一坐标、第二坐标,获得对所述第一关节进行转动的第一角度,其中,所述第三关系为:所述第一坐标点的坐标、第二坐标点的坐标以及所述第一关节的转动角度之间的对应关系。
本发明的一个实施例中,所述点轴位置关系按照以下表达式表示:
其中,所述为所述第一坐标点在所述基座坐标系下的坐标,所述为所述第一关节的轴线上任一点在所述基座坐标系下的坐标,所述为所述第二坐标点在所述基座坐标系下的坐标,所述为所述交点坐标,所述为所述关节轴线向量。
本发明的一个实施例中,所述第三关系按照以下表达式表示:
本发明的一个实施例中,所述目标关节坐标系的第一坐标轴方向与所述第二关节的轴线方向之间的夹角为所述第一角度;
所述第四关系按照以下表达式表示:
其中,所述为所述机械臂末端在所述第一坐标轴上的坐标,所述为第一端点与第二端点之间的距离,所述第一端点为所述第五关节中与所述第六关节相连的端点,所述第二端点为所述第六关节末端端点,所述为所述第五角度,所述为所述第三端点与第四端点之间的距离,所述第三端点为所述第四关节中与所述第三关节相连的端点,所述第四端点为所述第四关节中与所述第五关节相连的端点。
本发明的一个实施例中,所述坐标预测子模块,具体用于:
按照以下表达式进行关节坐标预测:
其中,所述为所述第四关节经所述第二关节和第三关节转动后在所述基座坐标系下的关节坐标,所述为所述第四关节的关节坐标,所述为自然常数,所述至分别为:所述第一关节至第六关节的轴线在正运动学模型中的运动旋量,所述至分别为第一角度至第六角度,所述为所述期望位姿数据,所述为所述机械臂末端在所述正运动学模型中的初始位姿数据的逆矩阵。
第三方面,本发明实施例还提供了一种电子设备,包括处理器、通信接口、存储器和通信总线,其中,处理器,通信接口,存储器通过通信总线完成相互间的通信;
存储器,用于存放计算机程序;
处理器,用于执行存储器上所存放的程序时,实现上述第一方面任一所述的方法步骤。
第四方面,本发明实施例还提供了一种计算机可读存储介质,所述计算机可读存储介质内存储有计算机程序,所述计算机程序被处理器执行时实现上述第一方面任一所述的方法步骤。
本发明实施例有益效果:
由以上可见,应用本发明实施例提供的方案控制机械臂时,由于上述正运动学模型、第一关系以及期望位姿数据均为在基座坐标系下的信息,因此,在计算对各个关节进行转动的目标角度时,可以直接利用同一坐标系下的上述信息进行计算,而无需通过一系列坐标系转换,将上述信息转换至同一坐标系下,这样可以减小计算上述目标角度的计算量,提高计算效率,从而提高控制机械臂的控制效率以及控制实时性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,还可以根据这些附图获得其他的实施例。
图1a为本发明实施例提供的第一种机械臂控制方法的流程示意图;
图1b为本发明实施例提供的一种机械臂的正运动学模型的结构示意图;
图2a为本发明实施例提供的一种机械臂的结构示意图;
图2b为本发明实施例提供的第二种机械臂控制方法的流程示意图;
图2c为本发明实施例提供的一种基座坐标系和关节坐标系之间的位置示意图;
图2d为本发明实施例提供的一种绕轴转动的转动示意图;
图3为本发明实施例提供的第三种机械臂控制方法的流程示意图;
图4为本发明实施例提供的第一种机械臂控制装置的结构示意图;
图5为本发明实施例提供的第二种机械臂控制装置的结构示意图;
图6为本发明实施例提供的一种电子设备的结构示意图。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员基于本发明所获得的所有其他实施例,都属于本发明保护的范围。
参见图1a,图1a为本发明实施例提供的第一种机械臂控制方法的流程示意图,本实施例中,上述方法包括以下步骤S101-S105。
步骤S101:在机械臂的基座对应的基座坐标系下建立机械臂的正运动学模型。
其中,上述机械臂配置有机械臂基座、机械臂末端以及关节,上述关节的数量可以是多个。
例如,上述机械臂中可以配置有5个、6个或者其他数量个关节。
在正运动学模型中机械臂各个关节相对于预设基准位置的转动角度为零。
上述预设基准位置为人为预先设定的位置。
每一关节对应一个预设基准位置。当关节处于其所对应的预设基准位置时,该关节对应的转动角度为零。
机械臂关节可以绕自身轴线进行自转,在机械臂关节自转时,关节自身与其预设基准位置之间的角度偏差即为关节相对于预设基准位置的转动角度。
具体的,可以以机械臂的基座所在位置为坐标原点建立基座坐标系,并在所建立的基座坐标系中,确定当各个关节相对于预设基准位置的转动角度为零时各个关节的角度数据,从而在基座坐标系中,根据所确定的各个关节的角度数据,建立机械臂的正运动学模型,其中,关节的角度数据可以包括关节相对于预设基准位置的转动角度。
图1b为一种机械臂的正运动学模型的结构示意图,图1b中的机械臂为配置有六个关节的机械臂,其中,-分别表示这六个关节的轴线的轴线向量,机械臂基座与对应的关节相连,d1为对应的关节的一端点与轴线之间的距离,a2为轴线与轴线之间的距离,a3为轴线与轴线之间的距离,d4为对应的关节的一端点与轴线之间的距离,d5为轴线与轴线的交点与轴线之间的距离,d6为对应的关节两端点之间的距离。在建立基座坐标系时,可以以对应的关节中与机械臂基座相连的端点为坐标原点建立基座坐标系,在图1b中,水平向左的箭头方向表示基座坐标系的X轴方向,竖直向上的箭头方向表示基座坐标系的z轴方向,指向左下角的箭头方向表示基座坐标系的Y轴方向。
各关节在基座坐标系下的坐标可以以各关节的端点的坐标表示,这样各个关节在基座坐标系下的坐标可以按照以下表达式表示:
在上述表达式中,每一列的三个数值分别表示基座坐标系下的三个坐标值,列数与关节相对应。例如,的第三列中的、0、分别表示对应的关节在基座坐标系下的三个坐标值,又例如,第五列中的、、分别表示对应的关节在基座坐标系下的三个坐标值。
步骤S102:根据正运动学模型确定机械臂末端在基座坐标系下的位姿数据与机械臂中各个关节的转动角度之间的第一关系。
具体的,在上述正运动学模型中,由于各个关节相对于预设基准位置的转动角度为零,即各个关节位于预设基准位置,这样可以根据各个关节之间的相对位置关系,确定机械臂末端在基座坐标系下的初始位姿数据,该初始位姿数据为各个关节对应的转动角度均为零的情况下机械臂末端的位姿数据,因此,当各个关节中一个或多个关节转动时,可以根据所确定的初始位姿数据以及转动的关节对应的转动角度,确定转动后的机械臂末端在基座坐标系下的位姿数据,这样也就利用上述初始位姿数据,确定了机械臂末端在基座坐标系下的位姿数据与机械臂中各个关节的转动角度之间的第一关系。
步骤S103:获得机械臂末端在基座坐标系下的期望位姿数据。
上述期望位姿数据包括位置数据和姿态数据。
上述期望位姿数据可以是人为设定的位姿数据。
本发明的一个实施例中,上述期望位姿数据可以按照以下表达式表示:
步骤S104:根据正运动学模型、第一关系以及期望位姿数据,计算对机械臂中各个关节进行转动的目标角度。
由于不同机械臂中关节的数量、连接方式等不同,因此,计算对不同机械臂中各个关节进行转动的目标角度的方式也就不同。
后续图2b所示实施例中步骤S104A-S104H提及了一种计算对三轴平行结构的机械臂中各个关节进行转动的目标角度的方式,针对其他结构的机械臂,可以采用与后续图2b所示实施例中步骤S104A-S104H类似的方式计算目标角度,这里暂不详述。
步骤S105:控制各个关节按照对应的目标角度转动,以使得机械臂末端在基座坐标系下的位姿数据为期望位姿数据。
具体的,在获得对各个关节进行转动的目标角度之后,针对每一关节,可以将该关节对应的转动角度调节为该关节对应的目标角度,这样能够使得机械臂末端在基座坐标系下的位姿数据为期望位姿数据。
本发明的一个实施例中,针对每一关节,可以获得该关节当前对应的转动角度,并计算当前转动角度与所对应目标角度之间的差值,从而基于计算得到的差值,控制各个关节转动。
由以上可见,应用本发明实施例提供的方案控制机械臂时,由于上述正运动学模型、第一关系以及期望位姿数据均为在基座坐标系下的信息,因此,在计算对各个关节进行转动的目标角度时,可以直接利用同一坐标系下的上述信息进行计算,而无需通过一系列坐标系转换,将上述信息转换至同一坐标系下,这样可以减小计算上述目标角度的计算量,提高计算效率,从而提高控制机械臂的控制效率以及控制实时性。
另外,本方案中,机械臂的正运动学模型为在基座坐标系下建立的,这样在建模时仅需考虑机械臂各个关节与机械臂基座之间的位置关系即可,无需进行坐标系转换,从而能够提高建模的便捷性,进一步提高控制机械臂的控制效率。
本发明的一个实施例中,上述第一关系按照以下表达式表示:
其中,为机械臂末端在基座坐标系下的位姿数据,N为机械臂中关节总数量,为自然常数,为第n个关节在正运动学模型中的运动旋量,为第n个关节相对于预设基准位置的转动角度,为机械臂末端在正运动学模型中的初始位姿数据。
从上述简化后的表达式中可以看出,在将第k个关节从预设基准位置转动至相对于其所对应预设基准位置的转动角度为的位置、且其他关节位于所对应的预设基准位置时,所获得的机械臂末端的位姿数据可以看做是机械臂末端的初始位姿绕关节k的轴线转动后的位姿数据,这样上述未简化的表达式可以理解为在各个关节均位于其所对应的预设基准位置的前提下,将各个关节均转动一定角度,所获得的机械臂末端的位姿数据可以看做是机械臂末端的初始位姿绕各个关节的轴线转动相对应的角度后的位姿数据。
例如,针对图1b中的机械臂,上述第一关系可以表示为:
该表达式可以理解为在各个关节均位于其所对应的预设基准位置的前提下,将对应的关节旋转、对应的关节旋转、对应的关节旋转、对应的关节旋转、对应的关节旋转、对应的关节旋转,这样所获得的机械臂末端的位姿数据为机械臂末端的初始位姿绕对应的关节的轴线旋转、绕对应的关节的轴线旋转、绕对应的关节的轴线旋转、绕对应的关节的轴线旋转、绕对应的关节的轴线旋转、绕对应的关节的轴线旋转后的位姿数据。
由以上可见,应用本发明实施例提供的方案控制机械臂时,上述表达式可以准确表示第一关系,这样根据上述表达式、正运动学模型以及期望位姿数据,能够准确计算对机械臂中各个关节进行转动的目标角度,从而准确控制各个关节转动,提高机械臂控制的准确性。
下面对计算对三轴平行结构的机械臂中各个关节进行转动的目标角度的实现方式进行说明。
首先对三轴平行结构的机械臂进行说明。
参见图2a,图2a为三轴平行结构的机械臂的结构示意图,图2a中,机械臂的右下方的圆台区域表示机械臂基座,机械臂配置有六个关节,第一关节的一端与机械臂基座相连,第一关节的另一端与第二关节的一端通过连杆相连,第二关节的另一端与第三关节的一端通过连杆相连,第三关节的另一端与第四关节的一端通过连杆相连,第四关节的另一端与第五关节的一端通过连杆相连,第五关节的另一端与第六关节的一端通过连杆相连,第六关节的另一端与机械臂末端相连。第二关节、第三关节以及第四关节的轴线平行。图中的三条虚线为基座坐标系的三条坐标轴线。
本发明的一个实施例中,参见图2b,提供了第二种机械臂控制方法的流程示意图,本实施例中,可以通过以下步骤S104A-S104H实现上述步骤S104。
上述图1b可以看做是上述图2a所示机械臂的正运动学模型的结构示意图,下面结合上述图1b以及图2a,对步骤S104A-S104H进行说明。
步骤S104A:在正运动学模型中获得第五关节和第六关节的轴线交点的交点坐标以及平行关节的关节轴线向量。
其中,平行关节为:第二关节、第三关节或第四关节。
从上述图1b中可以看出,与基座坐标系的z轴平行,与基座坐标系的Y轴平行,因此,可以根据第五关节和第六关节的坐标,得到上述轴线交点的交点坐标为(,,),并获得第二关节、第三关节或第四关节的关节轴线向量为。
步骤S104B:根据第一关系、期望位姿数据、交点坐标以及关节轴线向量,获得对第一关节进行转动的第一角度。
当空间中一点M绕一条旋转轴进行旋转,将旋转后的点M作为点N,则点M和点N所在直线与该旋转轴垂直,从点M指向点N的向量与该旋转轴的轴线向量的乘积为零。
鉴于此,在获得上述第一角度时,在上述正运动学模型中,转动第二关节、第三关节以及第四关节中一个或多个关节,可以将上述轴线交点转动至另一位置。为表述清楚,这里将轴线交点的原位置称为第一位置,将上述另一位置称为第二位置。由于第二关节、第三关节以及第四关节平行,因此,第二关节、第三关节以及第四关节进行任意转动,均存在根据上述第一位置和第二位置构成的向量与上述关节轴线向量的乘积为零。
另外,空间中的一条线段绕一条旋转轴进行旋转,旋转后的该线段的长度不变。
鉴于此,在上述轴线交点位于上述第二位置时,可以选取第一关节的轴线上的任一点O,点O与第二位置上的轴线交点可以构成一条线段R,转动第一关节,可以将上述轴线交点再次转动至又一位置,这里将该又一位置称为第三位置,此时,根据上述内容的描述,可以得知第三位置上的轴线交点与点O组成的线段S与上述线段R的长度相同。
基于上述两种特性,可以根据第一关系、期望位姿数据、交点坐标以及关节轴线向量,获得对第一关节进行转动的第一角度。
步骤S104C:根据期望位姿数据和第一角度,计算机械臂末端在转动第一角度后的第一关节对应的目标关节坐标系下的期望坐标。
具体的,可以在上述正运动学模型中,建立各个关节对应的关节坐标系,在获得上述第一角度后,控制第一关节按照第一角度转动,这样各个关节对应的关节坐标系的位置也随之发生变化。由于在正运动学模型中,各个关节的相对位置关系是已知的,又由于第一角度是已知的,因此,在控制第一关节转动后,可以根据第一角度,确定基座坐标系与目标关节坐标系之间的转换关系。另外,上述期望位姿数据中包括机械臂末端在基座坐标系下的期望位置坐标,因此,根据期望位姿数据中包括的期望位置坐标以及所确定的转换关系,可以计算得到机械臂末端在目标关节坐标系下的期望坐标。
如图2c所示,图2c为从与基座坐标系的Z轴方向相反的方向上的各个坐标系之间的位置示意图,上述图2c可以理解各个坐标系的俯视图,图2c中,、分别表示基座坐标系的X轴、Y轴,、分别表示目标关节坐标系的X轴、Z轴,表示第二关节对应的关节坐标系的Z轴,表示第三关节对应的关节坐标系的Z轴,表示第四关节对应的关节坐标系的X轴,表示第四关节对应的关节坐标系的Y轴,表示第六关节对应的关节坐标系的Z轴,表示第五关节对应的关节坐标系的X轴,表示第五关节对应的关节坐标系的Z轴。为机械臂末端的期望位置坐标中的X轴坐标,为机械臂末端的期望位置坐标中的Y轴坐标,为机械臂末端与基座坐标系原点之间的距离,该距离可以根据、计算得到,表示,为对第五关节进行转动的第五角度。
步骤S104D:基于预先获得的第四关系,根据计算得到的期望坐标,获得对第五关节进行转动的第五角度。
其中,第四关系为:机械臂末端在目标关节坐标系下的坐标与第五关节的转动角度之间的关系。
具体的,上述图2c可以预先根据各个关节的相对位置推导得到,根据上述图2c,可以确定机械臂末端在目标关节坐标系下的坐标与第五关节的转动角度之间的第四关系,从而结合目标关节坐标系下的期望坐标以及第四关系,获得对第五关节进行转动的第五角度。
本发明的一个实施例中,目标关节坐标系的第一坐标轴方向与第二关节的轴线方向之间的夹角为第一角度。
上述第四关系按照以下表达式表示:
其中,为机械臂末端在第一坐标轴上的坐标,为第一端点与第二端点之间的距离,第一端点为第五关节中与第六关节相连的端点,第二端点为第六关节末端端点,为第五角度,为第三端点与第四端点之间的距离,第三端点为第四关节中与第三关节相连的端点,第四端点为第四关节中与第五关节相连的端点。
本方案中,在获得上述期望坐标之后,通过上述表达式,可以准确计算出对第五关节进行转动的第五角度,从而能够按照第五角度,准确控制第五关节转动,因此,应用本发明实施例提供的机械臂控制方案,能够提高机械臂控制的准确性。
步骤S104E:根据第一角度、第五角度以及期望位姿数据,获得对第六关节进行转动的第六角度。
具体的,在获得上述第一角度、第五角度以及期望位姿数据之后,可以将所获得的上述参数代入表示上述第一关系的表达式中,通过现有的消元技术,获得对第六关节进行转动的第六角度。
将上述获得的参数代入表示上述第一关系的表达式中,可以得到第六角度为:
步骤S104F:根据第一角度、第五角度、第六角度、第一关系以及第四关节在正运动学模型中的关节坐标,预测第四关节经第二关节和第三关节转动后在基座坐标系下的关节坐标。
具体的,在已知机械臂末端在基座坐标系下的期望位姿数据的情况下,可以对表示上述第一关系的表达式进行转换,得到以下表达式:
其中,T为上述期望位姿数据。
在获得上述第一角度、第五角度、第六角度之后,可以对上述转换后的表达式进行变形,得到以下表达式:
根据上述变形后的表达式,可以预测第四关节经第二关节、第三关节以及第四关节转动后在基座坐标系下的关节坐标:
由于第四关节在绕其自身的轴线转动后,关节位置不变,因此,可以得到以下表达式:
步骤S104G:根据第二关节、第三关节、第四关节在正运动学模型中的关节坐标以及预测得到的关节坐标,获得对第二关节进行转动的第二角度和对第三关节进行转动的第三角度。
参见图2d,图2d为第四关节绕第三关节和第二关节的轴线转动的转动示意图。图2d中的左图为在垂直于第二关节和第三关节的方向上的转动示意图,图2d中的左图中,两个椭圆形表示的平面垂直于第二关节和第三关节的轴线,为第二关节的轴线,为第三关节的轴线,为第四关节正运动学模型中的关节坐标,坐标点绕第三关节的轴线旋转到达一点,这里将该点称为坐标点,坐标点绕第二关节的轴线旋转到达坐标点,为预测的第四关节经第二关节和第三关节转动后在基座坐标系下的关节坐标,u为基座坐标系中点与第三关节轴线上任一点构成的向量,v为基座坐标系中点与第二关节轴线上任一点构成的向量。图2d中的右图为在与第二关节或第三关节的轴线方向相反的方向上的转动示意图,图2d中的右图中,为u在椭圆形平面上的投影,为v在椭圆形平面上的投影, 为第二关节的关节坐标,为第三关节的关节坐标,w为根据、构成的向量,为与 之间的夹角,为由坐标点与构成的线段与w之间的夹角,为与之间的夹角,为由坐标点与构成的线段与w之间的夹角。
从图2d中可以看出,可以按照以下表达式获得上述第二角度和第三角度:
步骤S104H:基于预先获得的第二关节、第三关节以及第四关节的转动角度之间的第五关系,根据计算得到的第二角度和第三角度,获得对第四关节进行转动的第四角度。
具体的,可以通过以下三种实现方式中的任一种获得上述第四角度。
第一种实现方式中,对表示上述第一关系的表达式进行转换,得到以下表达式:
其中,表示机械臂末端从初始位姿经第五关节和第六关节转动后在基座坐标系下的位姿数据,表示位姿数据中的位置坐标,表示机械臂末端从初始位姿经第二关节至第六关节转动后在基座坐标系下的位姿数据,表示位姿数据中的位置坐标。
由于第二关节、第三关节以及第四关节平行,因此,可以将第二关节、第三关节和第四关节的转动过程看做是绕一个公共轴线转动的过程,该公共轴线为第四关节的轴线经第三关节转动第三角度、第二关节转动第二角度后得到的轴线。
在计算第四角度时,可以首先计算坐标点经第三关节转动第三角度、第二关节转动第二角度后的坐标,这里将转动后的坐标点称为,这样根据坐标点和坐标点这两个坐标点在基座坐标系下的坐标以及公共轴线在基座坐标系下的轴线向量,可以计算出坐标点绕公共轴线转动到坐标点的公共角度。另外,上述公共轴线为第四关节的轴线经第三关节转动第三角度、第二关节转动第二角度后得到的轴线,而第二角度和第三角度是已知量,因此,可以确定得到的公共轴线、第二关节轴线和第三关节轴线之间的相对位置关系,从而基于所确定的相对位置关系,根据计算得到的公共角度、第二角度以第三角度,获得对第四关节进行转动的第四角度。
第二种实现方式中,在获得上述第五角度和第六角度之后,可以根据上述期望位姿数据、第五角度以及第六角度,预测当机械臂末端在基座坐标系下的位姿数据为期望位姿数据时第五关节的位姿数据,在获得上述第一角度、第二角度以及第三角度之后,根据基座的位姿数据、第一角度、第二角度以及第三角度,预测当机械臂末端在基座坐标系下的位姿数据为期望位姿数据时第三关节的位姿数据,这样可以根据与第四关节相连的第三关节的位姿数据和第五关节的位姿数据,获得第四关节的位姿数据,通过对比所获得的第四关节的位姿数据以及第四关节对应的预期基准位置,得到对第四关节进行转动的第四角度。
第三种实现方式中,可以将第一角度、第二角度、第三角度、第五角度和第六角度代入表示第一关系的表达式,从而直接计算出第四角度。
由以上可见,应用本发明实施例提供的方案控制上述结构的机械臂时,根据多个关节的相对位置关系、关节坐标以及多个关节对应的转动角度之间的关系,结合所获得的第一关系、期望位姿数据、正运动学模型等参数,能够准确获得机械臂中配置的六个关节对应的目标角度,从而控制这六个关节按照对应的目标角度转动,能够提高机械臂控制的准确性。
各个关节之间的转动过程均通过关节或轴线的交点在基准坐标系下的坐标以及关节对应的转动角度进行描述,这样在计算目标角度时,各个参数的几何意义明确,从而简化了对关节转动的分析和求解过程,使得机械臂的逆运动学推导过程更加直观、清晰、便于理解。另外,应用本发明实施例提供的方案,可以获得第二角度和第三角度的多种解,因此,本方案在控制机械臂关节转动时,可以通过多种不同转动方案实现关节转动,这样能够提高机械臂控制的适用范围。
本发明实施例提供的方案中,在计算对各个关节进行转动的角度时,考虑了关节几何位置关系、三轴平行结构的结构特性,并融合现有的几何法以及Panden-kahan子问题解法,创造性的提出了一种新的针对三维平行结构的机械臂的逆运动学解法。
本发明的一个实施例中,参见图3,提供了第三种机械臂控制方法的流程示意图,本实施例中,可以通过以下步骤S104B1-S104B3实现上述步骤S104B。
步骤S104B1:根据第一关系、期望位姿数据和交点坐标,预测轴线交点经第一关节、第二关节、第三关节以及第四关节转动后在基座坐标系统下对应的第一坐标点的第一坐标。
具体的,由于轴线交点位于第五关节和第六关节的轴线上,因此,在第五关节和第六关节进行任意转动时,轴线交点的交点坐标不变,因此,根据第一关系、期望位姿数据和交点坐标,可以得出以下表达式:
步骤S104B2:根据交点坐标、关节轴线向量、第一坐标以及预先获得的点轴位置关系,预测轴线交点经第二关节、第三关节以及第四关节转动后在基座坐标系下对应的第二坐标点的第二坐标。
其中,点轴位置关系为:轴线交点、第一坐标点、第二坐标点以及第一关节的轴线之间的位置关系。
又由于空间中的一条线段绕一条旋转轴进行旋转,旋转后的该线段的长度不变,因此:
同理可得:
另外,由于第二关节、第三关节和第四关节平行,因此,由轴线交点与第二坐标点构成向量与平行关节的关节轴线向量的乘积为零,即:
联立简化后得到的两个表达式以及利用平行性质得到的表达式这三个表达式,可以准确计算出上述第二坐标点的第二坐标。
步骤S104B3:基于预先获得的第三关系,根据第一坐标、第二坐标,获得对第一关节进行转动的第一角度。
其中,第三关系为:第一坐标点的坐标、第二坐标点的坐标以及第一关节的转动角度之间的对应关系。
具体的,第一坐标点为轴线交点经第一关节、第二关节、第三关节以及第四关节转动后在基座坐标系统下对应的坐标点,第二坐标点为轴线交点经第二关节、第三关节以及第四关节转动后在基座坐标系下对应的坐标点,因此,可以认为第一坐标点为第二坐标点经第一关节转动后在基座坐标系下对应的坐标点。
第三关系为第一坐标点的坐标、第二坐标点的坐标以及第一关节的转动角度之间的对应关系,这样在获得上述第一坐标点的坐标、第二坐标点的坐标后,可以根据第三关系,获得对第一关节进行转动的第一角度。
本发明的一个实施例中,第三关系按照以下表达式表示:
由以上可见,应用本发明实施例提供的方案控制机械臂时,预测上述第一坐标点的第一坐标之后,根据交点坐标、关节轴线向量、第一坐标以及预先获得的点轴位置关系,预测第二坐标点的第二坐标,由于点轴位置关系为轴线交点、第一坐标点、第二坐标点以及第一关节的轴线之间的位置关系,因此,根据点轴位置关系,可以准确预测出第二坐标点的第二坐标,另外,第三关系为第一坐标点、第二坐标点这两个坐标点的坐标以及第一关节的转动角度之间的对应关系,因此,基于第三关系,根据第一坐标和第二坐标,能够准确计算出上述第一角度,这样可以根据第一角度,准确控制机械臂中第一关节转动,从而提高机械臂控制的准确性。
与上述机械臂控制方法相对应,本发明实施例还提供了一种机械臂控制装置。
本发明的一个实施例中,参见图4,提供了第一种机械臂控制装置的结构示意图,所述装置包括:
模型建立模块401,用于在机械臂的基座对应的基座坐标系下建立机械臂的正运动学模型,其中,在所述正运动学模型中所述机械臂各个关节相对于预设基准位置的转动角度为零;
关系确定模块402,用于根据所述正运动学模型确定所述机械臂末端在基座坐标系下的位姿数据与所述机械臂中各个关节的转动角度之间的第一关系;
数据获得模块403,用于获得所述机械臂末端在所述基座坐标系下的期望位姿数据;
角度计算模块404,用于根据所述正运动学模型、第一关系以及期望位姿数据,计算对所述机械臂中各个关节进行转动的目标角度;
关节控制模块405,用于控制各个关节按照对应的目标角度转动,以使得所述机械臂末端在基座坐标系下的位姿数据为所述期望位姿数据。
由以上可见,应用本发明实施例提供的方案控制机械臂时,由于上述正运动学模型、第一关系以及期望位姿数据均为在基座坐标系下的信息,因此,在计算对各个关节进行转动的目标角度时,可以直接利用同一坐标系下的上述信息进行计算,而无需通过一系列坐标系转换,将上述信息转换至同一坐标系下,这样可以减小计算上述目标角度的计算量,提高计算效率,从而提高控制机械臂的控制效率以及控制实时性。
另外,本方案中,机械臂的正运动学模型为在基座坐标系下建立的,这样在建模时仅需考虑机械臂各个关节与机械臂基座之间的位置关系即可,无需进行坐标系转换,从而能够提高建模的便捷性,进一步提高控制机械臂的控制效率。
本发明的一个实施例中,所述第一关系按照以下表达式表示:
其中,所述为所述机械臂末端在所述基座坐标系下的位姿数据,所述N为所述机械臂中关节总数量,所述为自然常数,所述为第n个关节在所述正运动学模型中的运动旋量,所述为第n个关节相对于所述预设基准位置的转动角度,所述为所述机械臂末端在所述正运动学模型中的初始位姿数据。
由以上可见,应用本发明实施例提供的方案控制机械臂时,上述表达式可以准确表示第一关系,这样根据上述表达式、正运动学模型以及期望位姿数据,能够准确计算对所述机械臂中各个关节进行转动的目标角度,从而准确控制各个关节转动,提高机械臂控制的准确性。
本发明的一个实施例中,参见图5,提供了第二种机械臂控制装置的结构示意图,本实施例中,所述机械臂配置有六个关节,第一关节、第二关节、第三关节、第四关节、第五关节和第六关节通过连杆依次连接,所述第一关节的一端与所述机械臂基座相连,所述第六关节的一端与所述机械臂末端相连,所述第二关节、第三关节以及第四关节的轴线平行;
所述角度计算模块404,包括:
数据获得子模块404A,用于在所述正运动学模型中获得所述第五关节和第六关节的轴线交点的交点坐标以及平行关节的关节轴线向量,其中,所述平行关节为:所述第二关节、第三关节或第四关节;
第一获得子模块404B,用于根据所述第一关系、期望位姿数据、交点坐标以及关节轴线向量,获得对所述第一关节进行转动的第一角度;
第一计算模块404C,用于根据所述期望位姿数据和所述第一角度,计算所述机械臂末端在转动第一角度后的第一关节对应的目标关节坐标系下的期望坐标;
第二获得子模块404D,用于于预先获得的第四关系,根据计算得到的期望坐标,获得对所述第五关节进行转动的第五角度,其中,所述第四关系为:所述机械臂末端在所述目标关节坐标系下的坐标与所述第五关节的转动角度之间的关系;
第三获得子模块404E,用于根据所述第一角度、第五角度以及期望位姿数据,获得对所述第六关节进行转动的第六角度;
坐标预测子模块404F,用于根据所述第一角度、第五角度、第六角度、第一关系以及所述第四关节在所述正运动学模型中的关节坐标,预测所述第四关节经所述第二关节和第三关节转动后在所述基座坐标系下的关节坐标;
第四获得子模块404G,用于根据所述第二关节、第三关节、第四关节在所述正运动学模型中的关节坐标以及预测得到的关节坐标,获得对所述第二关节进行转动的第二角度和对所述第三关节进行转动的第三角度;
第五获得子模块404H,用于基于预先获得的所述第二关节、第三关节以及第四关节的转动角度之间的第五关系,根据计算得到的第二角度和第三角度,获得对所述第四关节进行转动的第四角度。
由以上可见,应用本发明实施例提供的方案控制上述结构的机械臂时,根据多个关节的相对位置关系、关节坐标以及多个关节对应的转动角度之间的关系,结合所获得的第一关系、期望位姿数据、正运动学模型等参数,能够准确获得机械臂中配置的六个关节对应的目标角度,从而控制这六个关节按照对应的目标角度转动,能够提高机械臂控制的准确性。
各个关节之间的转动过程均通过关节或轴线的交点在基准坐标系下的坐标以及关节对应的转动角度进行描述,这样在计算目标角度时,各个参数的几何意义明确,从而简化了对关节转动的分析和求解过程,使得机械臂的逆运动学推导过程更加直观、清晰、便于理解。另外,应用本发明实施例提供的方案,可以获得第二角度和第三角度的多种解,因此,本方案在控制机械臂关节转动时,可以通过多种不同转动方案实现关节转动,这样能够提高机械臂控制的适用范围。
本发明实施例提供的方案中,在计算对各个关节进行转动的角度时,考虑了关节几何位置关系、三轴平行结构的结构特性,并融合现有的几何法以及Panden-kahan子问题解法,创造性的提出了一种新的针对三维平行结构的机械臂的逆运动学解法。
本发明的一个实施例中,所述第一获得子模块404B,具体用于:
根据所述第一关系、期望位姿数据和交点坐标,预测所述轴线交点经所述第一关节、第二关节、第三关节以及第四关节转动后在基座坐标系统下对应的第一坐标点的第一坐标;
根据所述交点坐标、关节轴线向量、第一坐标以及预先获得的点轴位置关系,预测所述轴线交点经所述第二关节、第三关节以及第四关节转动后在所述基座坐标系下对应的第二坐标点的第二坐标,其中,所述点轴位置关系为:所述轴线交点、第一坐标点、第二坐标点以及所述第一关节的轴线之间的位置关系;
基于预先获得的第三关系,根据所述第一坐标、第二坐标,获得对所述第一关节进行转动的第一角度,其中,所述第三关系为:所述第一坐标点的坐标、第二坐标点的坐标以及所述第一关节的转动角度之间的对应关系。
由以上可见,应用本发明实施例提供的方案控制机械臂时,预测上述第一坐标点的第一坐标之后,根据交点坐标、关节轴线向量、第一坐标以及预先获得的点轴位置关系,预测第二坐标点的第二坐标,由于点轴位置关系为轴线交点、第一坐标点、第二坐标点以及第一关节的轴线之间的位置关系,因此,根据点轴位置关系,可以准确预测出第二坐标点的第二坐标,另外,第三关系为第一坐标点、第二坐标点这两个坐标点的坐标以及第一关节的转动角度之间的对应关系,因此,基于第三关系,根据第一坐标和第二坐标,能够准确计算出上述第一角度,这样可以根据第一角度,准确控制机械臂中第一关节转动,从而提高机械臂控制的准确性。
本发明的一个实施例中,所述点轴位置关系按照以下表达式表示:
其中,所述为所述第一坐标点在所述基座坐标系下的坐标,所述为所述第一关节的轴线上任一点在所述基座坐标系下的坐标,所述为所述第二坐标点在所述基座坐标系下的坐标,所述为所述交点坐标,所述为所述关节轴线向量。
本方案中,根据上述表达式表示的点轴位置关系,能够准确计算出上述第二坐标点的第二坐标,从而能够提高第一角度的准确性,进而提高机械臂控制的准确性。
本发明的一个实施例中,所述第三关系按照以下表达式表示:
本方案中,上述表达式可以理解为第二坐标点绕第一关节的轴线转动,到达第一坐标点,即第一坐标点为第二坐标点经第一关节转动后在基座坐标系下对应的坐标点,因此,通过上述表达式,可以准确计算出第一角度,这样可以根据第一角度,准确控制机械臂中第一关节转动,从而提高机械臂控制的准确性。
本发明的一个实施例中,所述目标关节坐标系的第一坐标轴方向与所述第二关节的轴线方向之间的夹角为所述第一角度;
所述第四关系按照以下表达式表示:
其中,所述为所述机械臂末端在所述第一坐标轴上的坐标,所述为第一端点与第二端点之间的距离,所述第一端点为所述第五关节中与所述第六关节相连的端点,所述第二端点为所述第六关节末端端点,所述为所述第五角度,所述为所述第三端点与第四端点之间的距离,所述第三端点为所述第四关节中与所述第三关节相连的端点,所述第四端点为所述第四关节中与所述第五关节相连的端点。
本方案中,在获得上述期望坐标之后,通过上述表达式,可以准确计算出对第五关节进行转动的第五角度,从而能够按照第五角度,准确控制第五关节转动,因此,应用本发明实施例提供的机械臂控制方案,能够提高机械臂控制的准确性。
本发明的一个实施例中,所述坐标预测子模块404F,具体用于:
按照以下表达式进行关节坐标预测:
其中,所述为所述第四关节经所述第二关节和第三关节转动后在所述基座坐标系下的关节坐标,所述为所述第四关节的关节坐标,所述为自然常数,所述至分别为:所述第一关节至第六关节的轴线在正运动学模型中的运动旋量,所述至分别为第一角度至第六角度,所述为所述期望位姿数据,所述为所述机械臂末端在所述正运动学模型中的初始位姿数据的逆矩阵。
本发明实施例还提供了一种电子设备,如图6所示,包括处理器601、通信接口602、存储器603和通信总线604,其中,处理器601,通信接口602,存储器603通过通信总线604完成相互间的通信,
存储器603,用于存放计算机程序;
处理器601,用于执行存储器603上所存放的程序时,实现如下步骤:
在机械臂的基座对应的基座坐标系下建立机械臂的正运动学模型,其中,在所述正运动学模型中所述机械臂各个关节相对于预设基准位置的转动角度为零;
根据所述正运动学模型确定所述机械臂末端在基座坐标系下的位姿数据与所述机械臂中各个关节的转动角度之间的第一关系;
获得所述机械臂末端在所述基座坐标系下的期望位姿数据;
根据所述正运动学模型、第一关系以及期望位姿数据,计算对所述机械臂中各个关节进行转动的目标角度;
控制各个关节按照对应的目标角度转动,以使得所述机械臂末端在基座坐标系下的位姿数据为所述期望位姿数据。
上述处理器601执行存储器603上存放的程序而实现机械臂控制的其他方案,与前述方法实施例中提及的方案相同,这里不再赘述。
上述电子设备提到的通信总线可以是外设部件互连标准(Peripheral ComponentInterconnect,PCI)总线或扩展工业标准结构(Extended Industry StandardArchitecture,EISA)总线等。该通信总线可以分为地址总线、数据总线、控制总线等。为便于表示,图中仅用一条粗线表示,但并不表示仅有一根总线或一种类型的总线。
通信接口用于上述电子设备与其他设备之间的通信。
存储器可以包括随机存取存储器(Random Access Memory,RAM),也可以包括非易失性存储器(Non-Volatile Memory,NVM),例如至少一个磁盘存储器。可选的,存储器还可以是至少一个位于远离前述处理器的存储装置。
上述的处理器可以是通用处理器,包括中央处理器(Central Processing Unit,CPU)、网络处理器(Network Processor,NP)等;还可以是数字信号处理器(Digital SignalProcessor,DSP)、专用集成电路(Application Specific Integrated Circuit,ASIC)、现场可编程门阵列(Field-Programmable Gate Array,FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。
在本发明提供的又一实施例中,还提供了一种计算机可读存储介质,该计算机可读存储介质内存储有计算机程序,所述计算机程序被处理器执行时实现上述任一机械臂控制方法的步骤。
在本发明提供的又一实施例中,还提供了一种包含指令的计算机程序产品,当其在计算机上运行时,使得计算机执行上述实施例中任一机械臂控制方法。
在上述实施例中,可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用软件实现时,可以全部或部分地以计算机程序产品的形式实现。所述计算机程序产品包括一个或多个计算机指令。在计算机上加载和执行所述计算机程序指令时,全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中,或者从一个计算机可读存储介质向另一个计算机可读存储介质传输,例如,所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(DSL))或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输。所述计算机可读存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质,(例如,软盘、硬盘、磁带)、光介质(例如,DVD)、或者半导体介质(例如固态硬盘Solid State Disk (SSD))等。
需要说明的是,在本文中,诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来,而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
本说明书中的各个实施例均采用相关的方式描述,各个实施例之间相同相似的部分互相参见即可,每个实施例重点说明的都是与其他实施例的不同之处。尤其,对于装置、电子设备、计算机可读存储介质以及计算机程序产品实施例而言,由于其基本相似于方法实施例,所以描述的比较简单,相关之处参见方法实施例的部分说明即可。
以上所述仅为本发明的较佳实施例,并非用于限定本发明的保护范围。凡在本发明的精神和原则之内所作的任何修改、等同替换、改进等,均包含在本发明的保护范围内。
Claims (18)
1.一种机械臂控制方法,其特征在于,所述方法包括:
在机械臂的基座对应的基座坐标系下建立机械臂的正运动学模型,其中,在所述正运动学模型中所述机械臂各个关节相对于预设基准位置的转动角度为零;
根据所述正运动学模型确定所述机械臂末端在基座坐标系下的位姿数据与所述机械臂中各个关节的转动角度之间的第一关系;
获得所述机械臂末端在所述基座坐标系下的期望位姿数据;
根据所述正运动学模型、第一关系以及期望位姿数据,计算对所述机械臂中各个关节进行转动的目标角度;
控制各个关节按照对应的目标角度转动,以使得所述机械臂末端在基座坐标系下的位姿数据为所述期望位姿数据。
3.根据权利要求1或2所述的方法,其特征在于,所述机械臂配置有六个关节,第一关节、第二关节、第三关节、第四关节、第五关节和第六关节通过连杆依次连接,所述第一关节的一端与所述机械臂基座相连,所述第六关节的一端与所述机械臂末端相连,所述第二关节、第三关节以及第四关节的轴线平行;
所述根据所述正运动学模型、第一关系以及期望位姿数据,计算对所述机械臂中各个关节进行转动的目标角度,包括:
在所述正运动学模型中获得所述第五关节和第六关节的轴线交点的交点坐标以及平行关节的关节轴线向量,其中,所述平行关节为:所述第二关节、第三关节或第四关节;
根据所述第一关系、期望位姿数据、交点坐标以及关节轴线向量,获得对所述第一关节进行转动的第一角度;
根据所述期望位姿数据和所述第一角度,计算所述机械臂末端在转动第一角度后的第一关节对应的目标关节坐标系下的期望坐标;
基于预先获得的第四关系,根据计算得到的期望坐标,获得对所述第五关节进行转动的第五角度,其中,所述第四关系为:所述机械臂末端在所述目标关节坐标系下的坐标与所述第五关节的转动角度之间的关系;
根据所述第一角度、第五角度以及期望位姿数据,获得对所述第六关节进行转动的第六角度;
根据所述第一角度、第五角度、第六角度、第一关系以及所述第四关节在所述正运动学模型中的关节坐标,预测所述第四关节经所述第二关节和第三关节转动后在所述基座坐标系下的关节坐标;
根据所述第二关节、第三关节、第四关节在所述正运动学模型中的关节坐标以及预测得到的关节坐标,获得对所述第二关节进行转动的第二角度和对所述第三关节进行转动的第三角度;
基于预先获得的所述第二关节、第三关节以及第四关节的转动角度之间的第五关系,根据计算得到的第二角度和第三角度,获得对所述第四关节进行转动的第四角度。
4.根据权利要求3所述的方法,其特征在于,所述根据所述第一关系、期望位姿数据、交点坐标以及关节轴线向量,获得对所述第一关节进行转动的第一角度,包括:
根据所述第一关系、期望位姿数据和交点坐标,预测所述轴线交点经所述第一关节、第二关节、第三关节以及第四关节转动后在基座坐标系统下对应的第一坐标点的第一坐标;
根据所述交点坐标、关节轴线向量、第一坐标以及预先获得的点轴位置关系,预测所述轴线交点经所述第二关节、第三关节以及第四关节转动后在所述基座坐标系下对应的第二坐标点的第二坐标,其中,所述点轴位置关系为:所述轴线交点、第一坐标点、第二坐标点以及所述第一关节的轴线之间的位置关系;
基于预先获得的第三关系,根据所述第一坐标、第二坐标,获得对所述第一关节进行转动的第一角度,其中,所述第三关系为:所述第一坐标点的坐标、第二坐标点的坐标以及所述第一关节的转动角度之间的对应关系。
9.一种机械臂控制装置,其特征在于,所述装置包括:
模型建立模块,用于在机械臂的基座对应的基座坐标系下建立机械臂的正运动学模型,其中,在所述正运动学模型中所述机械臂各个关节相对于预设基准位置的转动角度为零;
关系确定模块,用于根据所述正运动学模型确定所述机械臂末端在基座坐标系下的位姿数据与所述机械臂中各个关节的转动角度之间的第一关系;
数据获得模块,用于获得所述机械臂末端在所述基座坐标系下的期望位姿数据;
角度计算模块,用于根据所述正运动学模型、第一关系以及期望位姿数据,计算对所述机械臂中各个关节进行转动的目标角度;
关节控制模块,用于控制各个关节按照对应的目标角度转动,以使得所述机械臂末端在基座坐标系下的位姿数据为所述期望位姿数据。
11.根据权利要求9或10所述的装置,其特征在于,所述机械臂配置有六个关节,第一关节、第二关节、第三关节、第四关节、第五关节和第六关节通过连杆依次连接,所述第一关节的一端与所述机械臂基座相连,所述第六关节的一端与所述机械臂末端相连,所述第二关节、第三关节以及第四关节的轴线平行;
所述角度计算模块,包括:
数据获得子模块,用于在所述正运动学模型中获得所述第五关节和第六关节的轴线交点的交点坐标以及平行关节的关节轴线向量,其中,所述平行关节为:所述第二关节、第三关节或第四关节;
第一获得子模块,用于根据所述第一关系、期望位姿数据、交点坐标以及关节轴线向量,获得对所述第一关节进行转动的第一角度;
第一计算模块,用于根据所述期望位姿数据和所述第一角度,计算所述机械臂末端在转动第一角度后的第一关节对应的目标关节坐标系下的期望坐标;
第二获得子模块,用于于预先获得的第四关系,根据计算得到的期望坐标,获得对所述第五关节进行转动的第五角度,其中,所述第四关系为:所述机械臂末端在所述目标关节坐标系下的坐标与所述第五关节的转动角度之间的关系;
第三获得子模块,用于根据所述第一角度、第五角度以及期望位姿数据,获得对所述第六关节进行转动的第六角度;
坐标预测子模块,用于根据所述第一角度、第五角度、第六角度、第一关系以及所述第四关节在所述正运动学模型中的关节坐标,预测所述第四关节经所述第二关节和第三关节转动后在所述基座坐标系下的关节坐标;
第四获得子模块,用于根据所述第二关节、第三关节、第四关节在所述正运动学模型中的关节坐标以及预测得到的关节坐标,获得对所述第二关节进行转动的第二角度和对所述第三关节进行转动的第三角度;
第五获得子模块,用于基于预先获得的所述第二关节、第三关节以及第四关节的转动角度之间的第五关系,根据计算得到的第二角度和第三角度,获得对所述第四关节进行转动的第四角度。
12.根据权利要求11所述的装置,其特征在于,所述第一获得子模块,具体用于:
根据所述第一关系、期望位姿数据和交点坐标,预测所述轴线交点经所述第一关节、第二关节、第三关节以及第四关节转动后在基座坐标系统下对应的第一坐标点的第一坐标;
根据所述交点坐标、关节轴线向量、第一坐标以及预先获得的点轴位置关系,预测所述轴线交点经所述第二关节、第三关节以及第四关节转动后在所述基座坐标系下对应的第二坐标点的第二坐标,其中,所述点轴位置关系为:所述轴线交点、第一坐标点、第二坐标点以及所述第一关节的轴线之间的位置关系;
基于预先获得的第三关系,根据所述第一坐标、第二坐标,获得对所述第一关节进行转动的第一角度,其中,所述第三关系为:所述第一坐标点的坐标、第二坐标点的坐标以及所述第一关节的转动角度之间的对应关系。
17.一种电子设备,其特征在于,包括处理器、通信接口、存储器和通信总线,其中,处理器,通信接口,存储器通过通信总线完成相互间的通信;
存储器,用于存放计算机程序;
处理器,用于执行存储器上所存放的程序时,实现权利要求1-8任一所述的方法步骤。
18.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质内存储有计算机程序,所述计算机程序被处理器执行时实现权利要求1-8任一所述的方法步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210746684.4A CN114800534B (zh) | 2022-06-29 | 2022-06-29 | 一种机械臂控制方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210746684.4A CN114800534B (zh) | 2022-06-29 | 2022-06-29 | 一种机械臂控制方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114800534A true CN114800534A (zh) | 2022-07-29 |
CN114800534B CN114800534B (zh) | 2022-10-14 |
Family
ID=82522261
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210746684.4A Active CN114800534B (zh) | 2022-06-29 | 2022-06-29 | 一种机械臂控制方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114800534B (zh) |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115227407A (zh) * | 2022-09-19 | 2022-10-25 | 杭州三坛医疗科技有限公司 | 一种手术机器人控制方法、装置、系统、设备及存储介质 |
CN115305980A (zh) * | 2022-08-16 | 2022-11-08 | 湖南中联重科智能高空作业机械有限公司 | 用于控制折叠臂式臂架的方法、处理器、装置及工程设备 |
CN116141338A (zh) * | 2023-04-18 | 2023-05-23 | 广东隆崎机器人有限公司 | 双臂机器人的控制方法、计算机存储介质及双臂机器人 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108044651A (zh) * | 2017-10-19 | 2018-05-18 | 北京航空航天大学 | 一种基于双目视觉的空间机械臂运动学参数在轨标定方法 |
CN109895101A (zh) * | 2019-04-09 | 2019-06-18 | 大连理工大学 | 一种关节型机械臂逆运动学数值唯一解求取方法 |
CN110434851A (zh) * | 2019-06-24 | 2019-11-12 | 浙江工业大学 | 一种5自由度机械臂逆运动学求解方法 |
US20200215690A1 (en) * | 2018-12-29 | 2020-07-09 | Cloudminds (Shenzhen) Robotics Systems Co., Ltd. | Motion planning method for robot arms, computing device and robot |
CN111890349A (zh) * | 2020-05-25 | 2020-11-06 | 北京空间飞行器总体设计部 | 一种四自由度机械臂运动规划方法 |
US20210212777A1 (en) * | 2016-03-04 | 2021-07-15 | Covidien Lp | Inverse kinematic control systems for robotic surgical system |
CN113127989A (zh) * | 2021-04-22 | 2021-07-16 | 中国科学院沈阳自动化研究所 | 一种六自由度机械臂逆运动学解析求解控制方法 |
-
2022
- 2022-06-29 CN CN202210746684.4A patent/CN114800534B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20210212777A1 (en) * | 2016-03-04 | 2021-07-15 | Covidien Lp | Inverse kinematic control systems for robotic surgical system |
CN108044651A (zh) * | 2017-10-19 | 2018-05-18 | 北京航空航天大学 | 一种基于双目视觉的空间机械臂运动学参数在轨标定方法 |
US20200215690A1 (en) * | 2018-12-29 | 2020-07-09 | Cloudminds (Shenzhen) Robotics Systems Co., Ltd. | Motion planning method for robot arms, computing device and robot |
CN109895101A (zh) * | 2019-04-09 | 2019-06-18 | 大连理工大学 | 一种关节型机械臂逆运动学数值唯一解求取方法 |
CN110434851A (zh) * | 2019-06-24 | 2019-11-12 | 浙江工业大学 | 一种5自由度机械臂逆运动学求解方法 |
CN111890349A (zh) * | 2020-05-25 | 2020-11-06 | 北京空间飞行器总体设计部 | 一种四自由度机械臂运动规划方法 |
CN113127989A (zh) * | 2021-04-22 | 2021-07-16 | 中国科学院沈阳自动化研究所 | 一种六自由度机械臂逆运动学解析求解控制方法 |
Cited By (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115305980A (zh) * | 2022-08-16 | 2022-11-08 | 湖南中联重科智能高空作业机械有限公司 | 用于控制折叠臂式臂架的方法、处理器、装置及工程设备 |
CN115305980B (zh) * | 2022-08-16 | 2023-09-15 | 湖南中联重科智能高空作业机械有限公司 | 用于控制折叠臂式臂架的方法、处理器、装置及工程设备 |
CN115227407A (zh) * | 2022-09-19 | 2022-10-25 | 杭州三坛医疗科技有限公司 | 一种手术机器人控制方法、装置、系统、设备及存储介质 |
CN115227407B (zh) * | 2022-09-19 | 2023-03-03 | 杭州三坛医疗科技有限公司 | 一种手术机器人控制方法、装置、系统、设备及存储介质 |
CN116141338A (zh) * | 2023-04-18 | 2023-05-23 | 广东隆崎机器人有限公司 | 双臂机器人的控制方法、计算机存储介质及双臂机器人 |
Also Published As
Publication number | Publication date |
---|---|
CN114800534B (zh) | 2022-10-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114800534B (zh) | 一种机械臂控制方法及装置 | |
US20200338730A1 (en) | Trajectory planning device, trajectory planning method and program | |
CN110640747B (zh) | 机器人的手眼标定方法、系统、电子设备和存储介质 | |
CN112318506A (zh) | 机械臂自动标定方法、装置、设备、机械臂和介质 | |
CN111844130B (zh) | 一种修正机器人末端工具位姿的方法和装置 | |
CN109571481A (zh) | 一种关节奇异点处理方法、装置、设备和存储介质 | |
Geng et al. | A novel welding path planning method based on point cloud for robotic welding of impeller blades | |
WO2024037658A1 (zh) | 机器人指向动作控制方法、装置、电子设备和存储介质 | |
CN115179297A (zh) | 手术机器人位置姿态控制联合避障关节极限的方法及系统 | |
CN116125906A (zh) | 一种数控加工的运动规划方法、装置、设备和存储介质 | |
CN113814988B (zh) | 7自由度srs型机械臂逆解的解析方法、装置及电子设备 | |
CN112476435B (zh) | 重力加速度方向的标定方法、标定装置及存储介质 | |
CN117207200B (zh) | 机械臂的工作空间生成方法、装置和计算机设备 | |
CN112536792A (zh) | 一种基于球面几何解析法的机器臂逆解方法 | |
CN115805587A (zh) | 七轴机器人的运动解析方法、装置及电子设备 | |
CN114851190B (zh) | 面向低频驱控一体的机械臂轨迹规划方法及系统 | |
CN115771146A (zh) | 一种机器人姿态插补方法、装置及存储介质 | |
CN116141302A (zh) | 七自由度机械臂运动的控制方法、装置及电子设备 | |
CN112847441B (zh) | 基于梯度下降法的六轴机器人坐标偏移检测方法和装置 | |
CN111716345B (zh) | 一种运动控制方法、运动控制装置及机械臂 | |
CN112917479A (zh) | 五轴机器人的近似位姿计算方法、装置和存储介质 | |
CN117464667B (zh) | 七轴机械臂的运动规划方法、装置及机械臂 | |
CN114290331B (zh) | 机器人运动控制方法、机器人及计算机可读存储装置 | |
TWI856570B (zh) | 加工路徑規劃模擬裝置以及方法 | |
CN118143948B (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 |