五自由度机器人运动控制方法及系统与流程

1.本发明涉及机器人运动控制技术领域，具体地，涉及五自由度机器人运动控制方法及系统。


背景技术：

2.专利文献cn104690735a(申请号：201310662919.2)公开了一种五自由度开放式机器人自动控制系统，属于机器人控制技术领域。本发明的五自由度开放式机器人自动控制系统，其特征在于：包括工控机、系统复位模块、操纵杆输入端、上层软件、运动控制卡、存储模块、显示模块、下层软件、原点和限位开关信号检测模块、限位开关组、驱动模块、大臂电机、小臂电机、腕部电机、主臂电机、手抓电机、减速器a、减速器b。
3.现有技术中，已有的酒店清洁机器人，多以单独移动机器人为主。少数带有机械臂的清洁机器人使用现有的协作机械臂，工作空间有限，体积较大。很难适应酒店房间环境。如图1的五自由度机器人包含2个移动轴和3个转动副，能够配合移动地盘，完成酒店清洁任务，而且机器人更加轻便小巧，工作范围大。
4.因此，有必要提出一种五自由度机器人运动控制方法。


技术实现要素：

5.针对现有技术中的缺陷，本发明的目的是提供一种五自由度机器人运动控制方法及系统。
6.根据本发明提供的一种五自由度机器人运动控制方法，包括：
7.步骤s1：根据dh模型建立机器人连杆坐标系，得到五自由度机器人dh参数；
8.步骤s2：确定相邻连杆的坐标变化关系，得到机器人正运动学表达式；
9.步骤s3：基于正运动学表达式获取机器人运动路径中的位姿点；
10.步骤s4：根据目标位姿点和机器人运动学模型依次计算机器人5个轴的关节角；
11.步骤s5：将5个关节角转化为每个电机的编码器值，并发送到机器人的驱动器；
12.步骤s6：驱动器根据接收的每个轴的编码器值驱动电机运动。
13.优选地，所述步骤s2采用：
14.步骤s2.1：根据机器人运动学模型建立机器人连杆坐标系；
15.步骤s2.2：根据机器人连杆坐标系确定相邻连杆的坐标变换关系，得到机器人正运动学表达式；
16.t
0,t
＝t
0,1
*t
1,2
*t
2,3
*t
3,4
*t
4,5
*tool
ꢀꢀꢀ
(1)
[0017][0018]
其中，t
0,1
表示坐标系1相对于坐标系0的齐次变换矩阵；t
1,2
表示坐标系2相对于坐标系1的齐次变换矩阵；t
2,3
表示坐标系3相对于坐标系2的齐次变换矩阵；t
3,4
表示坐标系4
相对于坐标系3的齐次变换矩阵；t
4,5
表示坐标系5相对于坐标系4的齐次变换矩阵；t
y
表示工具中心y方向偏移，t
z
表示工具中心z方向偏移；
[0019]
步骤s2.3：基于dh参数转换机器人正运动学表达式；
[0020][0021]
其中，s1＝sinθ1，c1＝cosθ1，s3＝sinθ3，c3＝cosθ3，s5＝sinθ5，c5＝cosθ5；[θ1,d2,θ3,d4,θ5]表示关节变量；p
x
、p
y
、p
z
表示机器人目标位姿。
[0022]
优选地，所述步骤s3采用：
[0023][0024]
roll＝θ3[0025]
yaw＝θ1 θ5[0026]
其中,p
x
、p
y
、p
z
、roll、yaw表示机器人目标位姿；s1＝sinθ1，c1＝cosθ1，s3＝sinθ3，c3＝cosθ3，s5＝sinθ5，c5＝cosθ5；[θ1,d2,θ3,d4,θ5]表示关节变量；a2和d3表示连杆的长度；t
y
表示工具中心y方向偏移；t
z
表示工具中心z方向偏移。
[0027]
优选地，所述步骤s4采用：
[0028]
机器人俯仰姿态由转动关节3确定：θ3＝roll；其中，roll表示机器人的俯仰姿态；
[0029]
机器人偏航姿态由转动关节1和转动关节5确定：θ1 θ5＝yaw；yaw表示机器人的偏航姿态；
[0030]
根据(p
x
‑
t
y
c
yaw
)s1 (
‑
p
y
t
y
s
yaw
)c1 d3＝0，计算转动关节1θ1以及转动关节5θ5；其中，c
yaw
表示cos(yaw)，偏航姿态的余弦值；s
yaw
表示sin(yaw)，偏航姿态的正弦值；
[0031]
移动关节4d4表示为：：
[0032]
移动关节2d2表示为：d2＝p
z
d4s3 t
y
s3c5‑
t
z
c3。
[0033]
优选地，所述步骤s5采用：根据关节角和电机编码器的分辨率得到电机的编码器值。
[0034]
根据本发明提供的一种五自由度机器人运动控制系统，包括：
[0035]
模块m1：根据dh模型建立机器人连杆坐标系，得到五自由度机器人dh参数；
[0036]
模块m2：确定相邻连杆的坐标变化关系，得到机器人正运动学表达式；
[0037]
模块m3：基于正运动学表达式获取机器人运动路径中的位姿点；
[0038]
模块m4：根据目标位姿点和机器人运动学模型依次计算机器人5个轴的关节角；
[0039]
模块m5：将5个关节角转化为每个电机的编码器值，并发送到机器人的驱动器；
[0040]
模块m6：驱动器根据接收的每个轴的编码器值驱动电机运动。
[0041]
优选地，所述模块m2采用：
[0042]
模块m2.1：根据机器人运动学模型建立机器人连杆坐标系；
[0043]
模块m2.2：根据机器人连杆坐标系确定相邻连杆的坐标变换关系，得到机器人正运动学表达式；
[0044]
t
0,t
＝t
0,1
*t
1,2
*t
2,3
*t
3,4
*t
4,5
*tool
ꢀꢀꢀ
(1)
[0045][0046]
其中，t
0,1
表示坐标系1相对于坐标系0的齐次变换矩阵；t
1,2
表示坐标系2相对于坐标系1的齐次变换矩阵；t
2,3
表示坐标系3相对于坐标系2的齐次变换矩阵；t
3,4
表示坐标系4相对于坐标系3的齐次变换矩阵；t
4,5
表示坐标系5相对于坐标系4的齐次变换矩阵；t
y
表示工具中心y方向偏移，t
z
表示工具中心z方向偏移；
[0047]
模块m2.3：基于dh参数转换机器人正运动学表达式；
[0048][0049]
其中，s1＝sinθ1，c1＝cosθ1，s3＝sinθ3，c3＝cosθ3，s5＝sinθ5，c5＝cosθ5；[θ1,d2,θ3,d4,θ5]表示关节变量；p
x
、p
y
、p
z
表示机器人目标位姿。
[0050]
优选地，所述模块m3采用：
[0051][0052]
roll＝θ3[0053]
yaw＝θ1 θ5[0054]
其中,p
x
、p
y
、p
z
、roll、yaw表示机器人目标位姿；s1＝sinθ1，c1＝cosθ1，s3＝sinθ3，c3＝cosθ3，s5＝sinθ5，c5＝cosθ5；[θ1,d2,θ3,d4,θ5]表示关节变量；a2和d3表示连杆的长度；t
y
表示工具中心y方向偏移；t
z
表示工具中心z方向偏移。
[0055]
优选地，所述模块m4采用：
[0056]
机器人俯仰姿态由转动关节3确定：θ3＝roll；其中，roll表示机器人的俯仰姿态；
[0057]
机器人偏航姿态由转动关节1和转动关节5确定：θ1 θ5＝yaw；yaw表示机器人的偏航姿态；
[0058]
根据(p
x
‑
t
y
c
yaw
)s1 (
‑
p
y
t
y
s
yaw
)c1 d3＝0，计算转动关节1θ1以及转动关节5θ5；其中，c
yaw
表示cos(yaw)，偏航姿态的余弦值；s
yaw
表示sin(yaw)，偏航姿态的正弦值；
[0059]
移动关节4d4表示为：：
[0060]
移动关节2d2表示为：d2＝p
z
d4s3 t
y
s3c5‑
t
z
c3。
[0061]
优选地，所述模块m5采用：根据关节角和电机编码器的分辨率得到电机的编码器值。
[0062]
与现有技术相比，本发明具有如下的有益效果：
[0063]
本发明对五自由度机器人的逆解采用解析解，从而实现对5自由度机器人的运动控制，运算速度快，计算效率高。
附图说明
[0064]
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
[0065]
图1为五自由度机器人示意图。
[0066]
图2为机器人连杆坐标系示意图。
[0067]
图3为五自由度机器人dh参数。
具体实施方式
[0068]
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
[0069]
实施例1
[0070]
本发明建立机器人运动学模型，根据目标位姿和运动学表达式，反解机器人5个关节角。5个关节角转化为每个电机的编码器值，发送到机器人的驱动器。驱动器根据接收的每个轴的编码器值，驱动电机运动。机器人运动学模型包括图2机器人运动学模型图和图3机器人运动学参数，其中，pi为180度的弧度。
[0071]
根据本发明提供的一种五自由度机器人运动控制方法，包括：
[0072]
步骤s1：根据dh模型建立机器人连杆坐标系，得到五自由度机器人dh参数，如图2至3所示；
[0073]
步骤s2：确定相邻连杆的坐标变化关系，得到机器人正运动学表达式；
[0074]
步骤s3：基于正运动学表达式获取机器人运动路径中的位姿点；
[0075]
步骤s4：根据目标位姿点和机器人运动学模型依次计算机器人5个轴的关节角；
[0076]
步骤s5：将5个关节角转化为每个电机的编码器值，并发送到机器人的驱动器；
[0077]
步骤s6：驱动器根据接收的每个轴的编码器值驱动电机运动；
[0078]
步骤s7：机器人带动末端工具完成清洁任务。
[0079]
所述正运动学表达式表示工具坐标系与机器人基坐标系的坐标变换关系。
[0080]
如图1所示，所述五自由度机器人是由2个移动副与3个转动副组成的五自由度机器臂，其中，关节1转动，关节2上下移动，关节3俯仰摆动，关节4伸缩移动，关节5转动。确保机器人能够快速移动到制定位置，完成清洁工作。
[0081]
具体地，所述步骤s2采用：
[0082]
步骤s2.1：根据机器人运动学模型建立机器人连杆坐标系；
[0083]
步骤s2.2：根据机器人连杆坐标系确定相邻连杆的坐标变换关系，得到机器人正运动学表达式；
[0084]
t
0,t
＝t
0,1
*t
1,2
*t
2,3
*t
3,4
*t
4,5
*tool
ꢀꢀꢀ
(1)
[0085]
[0086]
其中，t
0,1
表示坐标系1相对于坐标系0的齐次变换矩阵；t
1,2
表示坐标系2相对于坐标系1的齐次变换矩阵；t
2,3
表示坐标系3相对于坐标系2的齐次变换矩阵；t
3,4
表示坐标系4相对于坐标系3的齐次变换矩阵；t
4,5
表示坐标系5相对于坐标系4的齐次变换矩阵；t
y
表示工具中心y方向偏移，t
z
表示工具中心z方向偏移；tool表示工具坐标系相对于法兰坐标系关系；
[0087]
步骤s2.3：基于dh参数转换机器人正运动学表达式；
[0088][0089]
其中，s1＝sinθ1，c1＝cosθ1，s3＝sinθ3，c3＝cosθ3，s5＝sinθ5，c5＝cosθ5；[θ1,d2,θ3,d4,θ5]表示关节变量；p
x
、p
y
、p
z
表示机器人目标位姿。
[0090]
具体地，所述步骤s3采用：
[0091][0092]
roll＝θ3[0093]
yaw＝θ1 θ5[0094]
其中,p
x
、p
y
、p
z
、roll、yaw表示机器人目标位姿；s1＝sinθ1，c1＝cosθ1，s3＝sinθ3，c3＝cosθ3，s5＝sinθ5，c5＝cosθ5；[θ1,d2,θ3,d4,θ5]表示关节变量；a2和d3表示连杆的长度；t
y
表示工具中心y方向偏移；t
z
表示工具中心z方向偏移。
[0095]
具体地，所述步骤s4采用：
[0096]
机器人俯仰姿态由转动关节3确定：θ3＝roll；其中，roll表示机器人的俯仰姿态；
[0097]
机器人偏航姿态由转动关节1和转动关节5确定：θ1 θ5＝yaw；yaw表示机器人的偏航姿态；
[0098]
根据(p
x
‑
t
y
c
yaw
)s1 (
‑
p
y
t
y
s
yaw
)c1 d3＝0，计算转动关节1θ1以及转动关节5θ5；其中，c
yaw
表示cos(yaw)，偏航姿态的余弦值；s
yaw
表示sin(yaw)，偏航姿态的正弦值；
[0099]
移动关节4d4表示为：：
[0100]
移动关节2d2表示为：d2＝p
z
d4s3 t
y
s3c5‑
t
z
c3。
[0101]
具体地，所述步骤s4采用：根据关节角和电机编码器的分辨率得到电机的编码器值。
[0102]
更为具体地，关节角乘以电机编码器的分辨率得到电机的编码器值。
[0103]
根据本发明提供的一种五自由度机器人运动控制系统，包括：
[0104]
模块m1：根据dh模型建立机器人连杆坐标系，得到五自由度机器人dh参数，如图2至3所示；
[0105]
模块m2：确定相邻连杆的坐标变化关系，得到机器人正运动学表达式；
[0106]
模块m3：基于正运动学表达式获取机器人运动路径中的位姿点；
[0107]
模块m4：根据目标位姿点和机器人运动学模型依次计算机器人5个轴的关节角；
[0108]
模块m5：将5个关节角转化为每个电机的编码器值，并发送到机器人的驱动器；
[0109]
模块m6：驱动器根据接收的每个轴的编码器值驱动电机运动；
[0110]
模块m7：机器人带动末端工具完成清洁任务。
[0111]
所述正运动学表达式表示工具坐标系与机器人基坐标系的坐标变换关系。
[0112]
如图1所示，所述五自由度机器人是由2个移动副与3个转动副组成的五自由度机器臂，其中，关节1转动，关节2上下移动，关节3俯仰摆动，关节4伸缩移动，关节5转动。确保机器人能够快速移动到制定位置，完成清洁工作。
[0113]
具体地，所述模块m2采用：
[0114]
模块m2.1：根据机器人运动学模型建立机器人连杆坐标系；
[0115]
模块m2.2：根据机器人连杆坐标系确定相邻连杆的坐标变换关系，得到机器人正运动学表达式；
[0116]
t
0,t
＝t
0,1
*t
1,2
*t
2,3
*t
3,4
*t
4,5
*tool
ꢀꢀꢀ
(1)
[0117][0118]
其中，t
0,1
表示坐标系1相对于坐标系0的齐次变换矩阵；t
1,2
表示坐标系2相对于坐标系1的齐次变换矩阵；t
2,3
表示坐标系3相对于坐标系2的齐次变换矩阵；t
3,4
表示坐标系4相对于坐标系3的齐次变换矩阵；t
4,5
表示坐标系5相对于坐标系4的齐次变换矩阵；t
y
表示工具中心y方向偏移，t
z
表示工具中心z方向偏移；tool表示工具坐标系相对于法兰坐标系关系；
[0119]
模块m2.3：基于dh参数转换机器人正运动学表达式；
[0120][0121]
其中，s1＝sinθ1，c1＝cosθ1，s3＝sinθ3，c3＝cosθ3，s5＝sinθ5，c5＝cosθ5；[θ1,d2,θ3,d4,θ5]表示关节变量；p
x
、p
y
、p
z
表示机器人目标位姿。
[0122]
具体地，所述模块m3采用：
[0123][0124]
roll＝θ3[0125]
yaw＝θ1 θ5[0126]
其中,p
x
、p
y
、p
z
、roll、yaw表示机器人目标位姿；s1＝sinθ1，c1＝cosθ1，s3＝sinθ3，c3＝cosθ3，s5＝sinθ5，c5＝cosθ5；[θ1,d2,θ3,d4,θ5]表示关节变量；a2和d3表示连杆的长度；t
y
表示工具中心y方向偏移；t
z
表示工具中心z方向偏移。
[0127]
具体地，所述模块m4采用：
[0128]
机器人俯仰姿态由转动关节3确定：θ3＝roll；其中，roll表示机器人的俯仰姿态；
[0129]
机器人偏航姿态由转动关节1和转动关节5确定：θ1 θ5＝yaw；yaw表示机器人的偏
航姿态；
[0130]
根据(p
x
‑
t
y
c
yaw
)s1 (
‑
p
y
t
y
s
yaw
)c1 d3＝0，计算转动关节1θ1以及转动关节5θ5；其中，c
yaw
表示cos(yaw)，偏航姿态的余弦值；s
yaw
表示sin(yaw)，偏航姿态的正弦值；
[0131]
移动关节4d4表示为：：
[0132]
移动关节2d2表示为：d2＝p
z
d4s3 t
y
s3c5‑
t
z
c3。
[0133]
具体地，所述模块m4采用：根据关节角和电机编码器的分辨率得到电机的编码器值。
[0134]
更为具体地，关节角乘以电机编码器的分辨率得到电机的编码器值。
[0135]
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
[0136]
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本技术的实施例和实施例中的特征可以任意相互组合。