基于MPU6050的虚拟现实机械臂控制方法技术

本发明专利技术公开了一种基于MPU6050的虚拟现实机械臂控制方法，包括：S1.确定机械臂的运动学方程；S2.基于MPU6050的手势姿态信息，确定机械臂的数学模型；具体对应关系：X＝r*cos(pitch)*cos(yaw)、Y＝r*cos(pitch)*sin(yaw)、Z＝r*sin(pitch)、θ5＝roll；S3.根据机械臂末端的位置以及手部腕关节的转角解算运动学方程，得到机械臂各关节的转角；S4.根据各关节的转角控制机械臂完成指定动作。本发明专利技术能够实现较好的人机交互，保证了控制的稳定性，同时降低了投入成本。同时降低了投入成本。同时降低了投入成本。

【技术实现步骤摘要】
基于MPU6050的虚拟现实机械臂控制方法


[0001]本专利技术涉及自动控制领域，具体涉及一种基于MPU6050的虚拟现实机械臂控制方法。

技术介绍

[0002]根据人口普查，预计2033年我国即将进入超老龄化社会，目前，在生活辅助、情感陪伴和日常护理等方面，机器人的研发尚处于探索阶段，鲜见研发出相关成熟产品。为了护理和照看老年人、残疾人、传染性疾病患者等行动不便的人士，非接触性无人作业成为发展趋势，开发具有良好人机交互性能，具备常见目标物体抓取与放置功能的室内人机共融生活支援机器人，具有很强的现实必要性。
[0003]上述机器人的操作作业，实际上是对机械臂进行控制来实现的。而现阶段，基于低成本传感器的机械臂控制方法，在交互性、稳定性以及功能丰富程度等方面，尚存在较大缺陷，很难实现真正的生活支援功能。因此，为解决以上问题，需要一种基于MPU6050的虚拟现实机械臂控制方法，不仅能够做到利用低成本传感器来降低机械臂的成本，同时利用虚拟现实手臂控制实现了良好的人机交互性。

技术实现思路

[0004]有鉴于此，本专利技术的目的是克服现有技术中的缺陷，提供基于MPU6050的虚拟现实机械臂控制方法，能够实现较好的人机交互，保证了控制的稳定性，同时降低了投入成本。
[0005]本专利技术的基于MPU6050的虚拟现实机械臂控制方法，包括如下步骤：
[0006]S1.确定机械臂的运动学方程；
[0007]S2.基于MPU6050的手势姿态信息，确定机械臂末端的位置；
[0008]S3.根据机械臂末端的位置以及手部腕关节的转角解算运动学方程，得到机械臂各关节的转角；
[0009]S4.根据各关节的转角控制机械臂完成指定动作。
[0010]进一步，确定机械臂的运动学方程，具体包括：
[0011]S11.构建机械臂的D
‑
H坐标系；
[0012]S12.根据所述D
‑
H坐标系确定连杆的D
‑
H参数以及关节变量；
[0013]S13.以连杆的D
‑
H参数以及关节变量为数据源，计算得到机械臂的运动学方程。
[0014]进一步，根据如下公式确定机械臂末端的位置(X、Y、Z)：
[0015]X＝r*cos(pitch)*cos(yaw)；
[0016]Y＝r*cos(pitch)*sin(yaw)；
[0017]Z＝r*sin(pitch)；
[0018]其中，r为机械臂末端到机械臂原点的长度，pitch为手势姿态的俯仰角，yaw为手势姿态的航向角。
[0019]进一步，将MPU6050的roll值作为手部腕关节的转角；其中，roll为手势姿态的横
滚角。
[0020]进一步，根据如下公式确定机械臂各关节的转角：
[0021]θ1＝j1＝arctan(P
y
/P
x
)；
[0022]θ2＝pi/2
‑
j2＝pi/2
‑
arctan(((l2+l3cosj3)sj2+l3sinj3cosj2)/((l2+l3cosj3)cosj2‑
l3sinj3sinj2))
‑
arctan((l2+l3cosj3)/(l3sinj3))；
[0023]θ3＝
‑
j3＝
‑
arctan(sinj3/cosj3)；
[0024]θ4＝
‑
j4‑
arctan(a4/l4)＝j2+j3‑
φ
‑
arctan(a4/l4)；
[0025]θ5＝roll；
[0026]其中，θ1为D
‑
H坐标系中连杆1对应的绕Z0轴旋转的角度，θ2为D
‑
H坐标系中连杆2对应的绕Z1轴旋转的角度，θ3为D
‑
H坐标系中连杆3对应的绕Z2轴旋转的角度，θ4为D
‑
H坐标系中连杆4对应的绕Z3轴旋转的角度；θ5为D
‑
H坐标系中连杆5对应的绕Z4轴旋转的角度；j1表示俯视时机械臂绕腰关节从P
x
轴逆时针转过的角度，j2表示连杆2绕肩关节从Pz轴方向顺时针转过的角度，j3表示连杆3绕肘关节1从连杆2的方向顺时针转过的角度，j4表示连杆4绕肘关节2从连杆3方向顺时针转过的角度；l1表示在D
‑
H坐标系中，使腰关节坐标系的原点与肩关节坐标系原点重合在肩关节坐标系X方向上的偏移量，l2表示连杆2的长度，l3表示连杆3的长度，l4表示连杆4和连杆5的长度之和；P
y
、P
x
分别为机械臂末端三维空间坐标系中Y、X的值；l1+l3sin(j2+j3)+l2sinj2+l4sin(j2+j3+j4)≠0；a4表示在D
‑
H坐标系中，使肘关节2坐标系的原点与手部腕关节坐标系原点重合在肘关节2坐标系X方向上的偏移量；所述φ＝j2+j3+j4；
[0027]所述连杆1为腰关节与肩关节之间的连杆，所述连杆2为肩关节与肘关节1之间的连杆，所述连杆3为肘关节1与肘关节2之间的连杆，所述连杆4为肘关节2与手部腕关节之间的连杆；所述连杆5为手部腕关节与手抓开合关节之间的连杆。
[0028]进一步，根据如下公式确定手势姿态的俯仰角pitch：
[0029]pitch＝arcsin(
‑
2*q1*q3+2*q0*q2)*57.3；
[0030]其中，q0、q1、q2以及q3为四元数。
[0031]进一步，根据如下公式确定手势姿态的航向角yaw：
[0032]yaw＝arctan((2*(q1*q2+q0*q3))/(q0*q0+q1*q1
‑
q2*q2
‑
q3*q3))*57.3；
[0033]其中，q0、q1、q2以及q3为四元数。
[0034]进一步，根据如下公式确定手势姿态的横滚角roll：
[0035]roll＝arctan((2*q2*q3+2*q0*q1)/(
‑
2*q1*q1
‑
2*q2*q2+1))*57.3；
[0036]其中，q0、q1、q2以及q3为四元数。
[0037]本专利技术的有益效果是：本专利技术公开的一种基于MPU6050的虚拟现实机械臂控制方法，利用MPU6050传感器获取操作者的手势姿态信息，再进行运动学求解，进而驱动机械臂的各个关节转动到相应角度，实现了对机械臂的运动控制，解决了现阶段低成本机械臂交互性差的技术难题。
附图说明
[0038]下面结合附图和实施例对本专利技术作进一步描述：
[0039]图1为本专利技术的机械臂的D
‑
H坐标系示意图；
[0040]图2为本专利技术的传感器安装本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于MPU6050的虚拟现实机械臂控制方法，其特征在于：包括如下步骤：S1.确定机械臂的运动学方程；S2.基于MPU6050采集到的手势姿态信息，确定机械臂的抓取姿态；S3.根据机械臂末端的位置以及手部腕关节的转角解算运动学方程，得到机械臂各关节的转角；S4.根据各关节的转角控制机械臂完成指定动作。2.根据权利要求1所述的基于MPU6050的虚拟现实机械臂控制方法，其特征在于：确定机械臂的运动学方程，具体包括：S11.构建机械臂的D
‑
H坐标系；S12.根据所述D
‑
H坐标系确定连杆的D
‑
H参数以及关节变量；S13.以连杆的D
‑
H参数以及关节变量为数据源，计算得到机械臂的运动学方程。3.根据权利要求1所述的基于MPU6050的虚拟现实机械臂控制方法，其特征在于：根据如下公式确定机械臂末端的位置(X、Y、Z)：X＝r*cos(pitch)*cos(yaw)；Y＝r*cos(pitch)*sin(yaw)；Z＝r*sin(pitch)；其中，r为机械臂末端到机械臂原点的长度，pitch为手势姿态的俯仰角，yaw为手势姿态的航向角。4.根据权利要求1所述的基于MPU6050的虚拟现实机械臂控制方法，其特征在于：将MPU6050的roll值作为手部腕关节的转角；其中，roll为手势姿态的横滚角。5.根据权利要求1所述的基于MPU6050的虚拟现实机械臂控制方法，其特征在于：根据如下公式确定机械臂各关节的转角：θ1＝j1＝arctan(P
y
/P
x
)；θ2＝pi/2
‑
j2＝pi/2
‑
arctan(((l2+l3cosj3)sj2+l3sinj3cosj2)/((l2+l3cosj3)cosj2‑
l3sinj3sinj2))
‑
arctan((l2+l3cosj3)/(l3sinj3))；θ3＝
‑
j3＝
‑
arctan(sinj3/cosj3)；θ4＝
‑
j4‑
arctan(a4/l4)＝j2+j3‑
φ
‑
arctan(a4/l4)；θ5＝roll；其中，θ1为D
‑
H坐标系中连杆1绕对应的Z0轴旋转的角度，θ2为D
‑
H坐标系中连杆2绕对应的Z1轴旋转的...

【专利技术属性】
技术研发人员：刘宗敏，文浩，李祖锋，敖文刚，王松，王吉睿，张龙发，黄国羊，
申请(专利权)人：重庆凯瑞机器人技术有限公司，
类型：发明
国别省市：