清洁机械手运动控制方法及系统技术方案

本发明专利技术提供了一种清洁机械手运动控制方法及系统，包括如下步骤：计算步骤：计算机械手多个关节的关节角；驱动步骤：对多个关节角进行处理，进而机械手进行清洁工作。对清洁机械手的逆解采用解析解，从而实现对清洁机械手的运动控制，运算速度快，计算效率高。计算效率高。计算效率高。

【技术实现步骤摘要】
清洁机械手运动控制方法及系统


[0001]本专利技术涉及机器人运动控制领域的
，具体地，涉及一种清洁机械手运动控制方法及系统，尤其是涉及一种酒店清洁机械手运动控制方法。

技术介绍

[0002]已有的酒店清洁机器人，多以单独移动机器人为主，能够完成地面清洁任务。卫生间台盆和马桶清洁需要机械手完成清洁任务。清洁机械手是六自由度机械臂和电动夹爪组成。清洁机械手轻便小巧，工作范围大，适应酒店卫生间清洁任务。
[0003]公开号为CN105500361A的中国专利技术专利文献公开了一种连杆结构机械手的运动控制方法及系统，方法包括：获取用于标识所述机械手空间位置的多个轨迹点，所述轨迹点包括用于标识抓料位置的抓料点、用于标识放料位置的放料点、以及所述抓料点和放料点之间的过渡点；生成空间轨迹曲线；将所述空间轨迹曲线上的多个空间点转换为对应的电机角度，生成基于时间轴的凸轮曲线；确定所述空间点所对应的电机参数，并将对应的电机参数与空间点对应的电机角度关联；根据所述凸轮曲线及凸轮曲线上的点所对应的电机参数，控制所述电机转动，从而带动所述机械手运动。
[0004]针对上述中的相关技术，专利技术人认为上述方法运算速度较慢、计算效率较低，导致对清洁机械手的运动控制性较差。

技术实现思路

[0005]针对现有技术中的缺陷，本专利技术的目的是提供一种清洁机械手运动控制方法及系统。
[0006]根据本专利技术提供的一种清洁机械手运动控制方法，包括如下步骤：
[0007]计算步骤：计算机械手多个关节的关节角；
[0008]驱动步骤：对多个关节角进行处理，进而驱动机械手进行清洁工作。
[0009]优选的，所述计算步骤包括如下步骤：
[0010]步骤S1.1：采用多自由度机器人，根据DH模型建立机器人连杆坐标系，得到多自由度机器人DH参数，建立机器人运动学模型；
[0011]步骤S1.2：多自由度机器人包括视觉系统，由视觉系统获取清洁任务中的位姿点；
[0012]步骤S1.3：多自由度机器人包括机械手，根据机械手位姿点和机器人运动学模型计算机械手多个关节的关节角。
[0013]优选的，所述驱动步骤包括如下步骤：
[0014]步骤S2.1：将多个关节角分别转换为对应电机的编码器值，并将编码器值发送给机械手的驱动器；
[0015]步骤S2.2：驱动器根据每个编码器值驱动对应的电机运动；
[0016]步骤S2.3：电机带动机械手运动，机械手根据清洁任务进行清洁工作。
[0017]优选的，在所述步骤S1.1中，所述多自由度机器人DH参数包括关节变量[q1，q2，q3，
q4，q5，q6]、连杆参数[a3，a4，d1，d4，d5，d6]；机械手包括机械臂和夹爪，将夹爪安装在机械臂的端部，夹爪中心定义为工具坐标系，机械臂末端定义为法兰坐标系；
[0018]其中，q1表示第一关节的角度，即第一轴关节角；q2表示第二关节的角度，即第二轴关节角；q3表示第三关节的角度，即第三轴关节角；q4表示第四关节的角度，即第四轴关节角；q4表示第五关节的角度，即第五轴关节角；q6表示第六关节的角度，即第六轴关节角；a3表示第三连杆长度；a4表示第四连杆长度；d1表示第一连杆偏置长度；d4表示第四连杆偏置长度；d5表示第五连杆偏置长度；d6表示第六连杆偏置长度。
[0019]优选的，所述步骤S1.3包括如下步骤：
[0020]步骤S1.3.1：计算出法兰坐标系相对于基坐标系的位姿其中，表示法兰坐标系相对于机械手基坐标系的齐次变换矩阵；表示清洁任务位姿点相对于机械手基坐标系的齐次变换矩阵；表示工具坐标系相对于法兰坐标系的齐次变换矩阵的逆矩阵；*表示乘法；
[0021][0022]其中，p
x
表示x坐标的位置分量；p
y
表示y坐标的位置分量；p
z
表示z坐标的位置分量；n
x
、o
x
和a
x
均表示x坐标轴的姿态分量；n
y
、o
y
和a
y
均表示y坐标轴的姿态分量；n
z
、o
z
和a
z
分别表示z坐标轴的姿态分量；
[0023]步骤S1.3.2：计算由
[0024][0025]其中，表示第五坐标系相对于第一坐标系的齐次变换矩阵；表示第一坐标系相对于机械手基坐标系的齐次变换矩阵矩阵的逆矩阵；表示法兰坐标系相对于第五坐标系的齐次变换矩阵的逆矩阵；表示第二坐标系相对于第一坐标系的齐次变换矩阵；表示第三坐标系相对于第二坐标系的齐次变换矩阵；表示第四坐标系相对于第三坐标系的齐次变换矩阵；表示第五坐标系相对于第四坐标系的齐次变换矩阵；
[0026]得：
[0027][0028]其中，q1的余弦值c1＝cos(q1)，q1的正弦值s1＝sin(q1)，q6的余弦值c6＝cos(q6)，q6的正弦值s6＝sin(q6)，q2的余弦值c2＝cos(q2)，q2的正弦值s2＝sin(q2)，q5的余弦值c5＝cos(q5)，q5的正弦值s5＝sin(q5)，q2+q3+q4的余弦值c
234
＝cos(q
234
)，q2+q3+q4的正弦值s
234
＝sin(q
234
)，q2+q3的余弦值c
23
＝cos(q
23
)，q2+q3的正弦值s
23
＝sin(q
23
)；q
234
表示q2+q3+q4；q
23
表示q2+q3；
[0029]由
‑
p
x
s1+p
y
c1+a
x
d6s1‑
a
y
d6c1得
[0030](a
x
d6‑
p
x
)s1+(p
y
‑
a
y
d6)c1‑
d4＝0
[0031]求解方程得到q1；
[0032]由a
x
s1‑
a
y
c1得
[0033]c5＝a
y
c1‑
a
x
s1[0034]q5＝
±
arccos(a
y
c1‑
a
x
s1)
[0035]其中，arccos表示反余弦函数；
[0036]得到q5；
[0037]由
‑
n
x
s1s6+n
y
c1s6‑
o
x
s1c6+o
y
c1c6得
[0038](
‑
n
x
...

【技术保护点】

【技术特征摘要】
1.一种清洁机械手运动控制方法，其特征在于，包括如下步骤：计算步骤：计算机械手多个关节的关节角；驱动步骤：对多个关节角进行处理，进而驱动机械手进行清洁工作。2.根据权利要求1所述的清洁机械手运动控制方法，其特征在于，所述计算步骤包括如下步骤：步骤S1.1：采用多自由度机器人，根据DH模型建立机器人连杆坐标系，得到多自由度机器人DH参数，建立机器人运动学模型；步骤S1.2：多自由度机器人包括视觉系统，由视觉系统获取清洁任务中的位姿点；步骤S1.3：多自由度机器人包括机械手，根据机械手位姿点和机器人运动学模型计算机械手多个关节的关节角。3.根据权利要求2所述的清洁机械手运动控制方法，其特征在于，所述驱动步骤包括如下步骤：步骤S2.1：将多个关节角分别转换为对应电机的编码器值，并将编码器值发送给机械手的驱动器；步骤S2.2：驱动器根据每个编码器值驱动对应的电机运动；步骤S2.3：电机带动机械手运动，机械手根据清洁任务进行清洁工作。4.根据权利要求2所述的清洁机械手运动控制方法，其特征在于，在所述步骤S1.1中，所述多自由度机器人DH参数包括关节变量[q1，q2，q3，q4，q5，q6]、连杆参数[a3，a4，d1，d4，d5，d6]；机械手包括机械臂和夹爪，将夹爪安装在机械臂的端部，夹爪中心定义为工具坐标系，机械臂末端定义为法兰坐标系；其中，q1表示第一关节的角度；q2表示第二关节的角度；q3表示第三关节的角度；q4表示第四关节的角度；q4表示第五关节的角度；q6表示第六关节的角度；a3表示第三连杆长度；a4表示第四连杆长度；d1表示第一连杆偏置长度；d4表示第四连杆偏置长度；d5表示第五连杆偏置长度；d6表示第六连杆偏置长度。5.根据权利要求2所述的清洁机械手运动控制方法，其特征在于，所述步骤S1.3包括如下步骤：步骤S1.3.1：计算出法兰坐标系相对于基坐标系的位姿其中，表示法兰坐标系相对于机械手基坐标系的齐次变换矩阵；表示清洁任务位姿点相对于机械手基坐标系的齐次变换矩阵；表示工具坐标系相对于法兰坐标系的齐次变换矩阵的逆矩阵；*表示乘法；其中，p
x
表示x坐标的位置分量；p
y
表示y坐标的位置分量；p
z
表示z坐标的位置分量；n
x
、o
x
和a
x
均表示x坐标轴的姿态分量；n
y
、o
y
和a
y
均表示y坐标轴的姿态分量；n
z
、o
z
和a
z
分别表示z坐标轴的姿态分量；步骤S1.3.2：计算由
其中，表示第五坐标系相对于第一坐标系的齐次变换矩阵；表示第一坐标系相对于机械手基坐标系的齐次变换矩阵矩阵的逆矩阵；表示法兰坐标系相对于第五坐标系的齐次变换矩阵的逆矩阵；表示第二坐标系相对于第一坐标系的齐次变换矩阵；表示第三坐标系相对于第二坐标系的齐次变换矩阵；表示第四坐标系相对于第三坐标系的齐次变换矩阵；表示第五坐标系相对于第四坐标系的齐次变换矩阵；得：其中，q1的余弦值c1＝cos(q1)，q1的正弦值s1＝sin(q1)，q6的余弦值c6＝cos(q6)，q6的正弦值s6＝sin(q6)，q2的余弦值c2＝cos(q2)，q2的正弦值s2＝sin(q2)，q5的余弦值c5＝cos(q5)，q5的正弦值s5＝sin(q5)，q2+q3+q4的余弦值c
234
＝cos(q
234
)，q2+q3+q4的正弦值s
234
＝sin(q
234
)，q2+q3的余弦值c
23
＝cos(q
23
)，q2+q3的正弦值s
23
＝sin(q
23
)；q
234
表示q2+q3+q4；q
23
表示q2+q3；由
‑
p
x
s1+p
y
c1+a
x
d6s1‑
a
y
d6c1得(a
x
d6‑
p
x
)s1+(py
‑
a
y
d6)c1‑
d4＝0求解方程得到q1；由a
x
s1‑
a
y
c1得c5＝a
y
c1‑
a
x
s1q5＝
±
arccos(a
y
c1‑
a
x
s1)其中，arccos表示反余弦函数；得到q5；由
‑
n
x
s1s6+n
y
c1s6‑
o
x
s1c6+o
y
c1c6得(
‑
n
x
s1+n
y
c1)s6+(
‑
o
x
s1+o
y
c1)c6＝0当q5≠0时，求得q6；步骤S1.3.3：计算由其中，表示第四坐标系相对于第一坐标系的齐次变换矩阵；表示第一坐标系相对于机械手基坐标系的齐次变换矩阵的逆矩阵；表示第五坐标系相对于第四坐标系的齐次变换矩阵的逆矩阵；得：
由p
x
c1+p
y
s1‑
d6(a
y
s1+a
x
c1)
‑
d5s6(n
x
c1+n
y
s1)
‑
d5c6(o
x
c1+o
y
s1)和p
z
‑
d1‑
a
z
d6‑
d5(o
z
c6+n
z
s6)得其中，m3表示p
x
c1+p
y
s1‑
d6(a
y
s1+a
x
c1)
‑
d5s6(n
x
c1+n
y
s1)
‑
d5c6(o
x
c1+o
y
s1)的结果；n3表示p
z
‑
d1‑
a
z
d6‑
d5(o
z
c6+n
z
s6)的结果；X和Y分别表示等式记号；由X2+Y2得得其中，s3表示q3的正弦值sin(q3)；arcsin表示反正弦函数；得到q3；展开c
23
，s
23
；其中，c3表示q3的余弦值cos(q3)；令令其中，a3‑
a4s3记为A；a4c3记为B；得q2＝atan2(s2，c2)；其中，atan2表示反正切函数；得到q2；由n
x
c1c5c6+n
y
s1c5c6‑
o
x
c1c5s6‑
o
y
s1c5s6+a
x
c1s5+a
y
s1s5和n
z
c5c6‑
o
z
c5s6+a
z
s5得计算q4＝atan2(s
234
c
23
‑
c
234
s
23
，s
234
s
23
+c
234
c
23
)；得到q4。6.一种清洁机械手运动...

【专利技术属性】
技术研发人员：王永超，郭震，
申请(专利权)人：上海景吾酷租科技发展有限公司，
类型：发明
国别省市：