(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210849027.2 (22)申请日 2022.07.19 (71)申请人 重庆工商大 学 地址 400067 重庆市南岸区学府大道19号 申请人 重庆凯瑞机 器人技术有限公司 (72)发明人 刘宗敏　文浩　李祖锋　敖文刚　 王松　王吉睿　张龙发　黄国羊　 (74)专利代理 机构 北京海虹嘉诚知识产权代理 有限公司 1 1129 专利代理师 胡博文 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 基于MPU6050的虚拟现实机 械臂控制方法 (57)摘要 本发明公开了一种基于MPU6050的虚拟现实 机械臂控制方法， 包括： S1.确定机械臂的运动学 方程； S2.基于MPU6050的手势姿态信息， 确定机 械臂的数学模型； 具体对应关系： X＝r*cos (pitch)*cos(yaw)、 Y＝r*cos(pitch)*sin (yaw)、 Z＝r*sin(pitch)、 θ5＝roll； S3.根据机 械臂末端的位置以及手部腕关节的转角解算运 动学方程， 得到机械臂各关节的转角； S4.根据各 关节的转角控制机械臂完成指定动作。 本发明能 够实现较好的人机交互， 保证了控制的稳定性， 同时降低了投入成本 。 权利要求书2页 说明书6页 附图2页 CN 115042187 A 2022.09.13 CN 115042187 A 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(pitc h)*cos(yaw)； Y＝r*cos(pitc h)*sin(yaw)； Z＝r*sin(pitch)； 其中， r为机械臂末端到机械臂原点的长度， pitch为手势姿态的俯仰角， yaw为手势姿 态的航向角。 4.根据权利要求1所述的基于MPU6050的虚拟现实机械臂控制方法， 其特征在于： 将 MPU6050的roll值作为手部腕关节的转角； 其中， ro ll为手势姿态的横滚角。 5.根据权利要求1所述的基于MPU6050的虚拟现实机械臂控制方法， 其特征在于： 根据 如下公式确定 机械臂各关节的转角： θ1＝j1＝arctan(Py/Px)； θ2＝pi/2‑j2＝pi/2‑arctan(((l2+l3cosj3)sj2+l3sinj3cosj2)/((l2+l3cosj3)cosj2‑ l3sinj3sinj2))‑arctan((l2+l3cosj3)/(l3sinj3))； θ3＝‑j3＝‑arctan(si nj3/cosj3)； θ4＝‑j4‑arctan(a4/l4)＝j2+j3‑φ‑arctan(a4/l4)； θ5＝roll； 其中， θ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表示俯 视时机械臂绕腰关节从Px轴逆时针 转过的角度， j2表示连杆2绕肩关节从Pz轴方向顺时针 转 过的角度， j3表示连杆3绕肘关节1从连杆2的方向顺时针转 过的角度， j4表示连杆4绕肘关节 2从连杆3方向顺时针转过的角度； l1表示在D‑H坐标系中， 使腰关节坐标系的原点与肩关节 坐标系原点重合在肩关节坐标系X方向上的偏移量， l2表示连杆2的长度， l3表示连杆3的长 度， l4表示连杆4和连杆5的长度之和； Py、 Px分别为机械臂末端三维空间坐标系中Y、 X的值； l1+l3sin(j2+j3)+l2sinj2+l4sin(j2+j3+j4)≠0； a4表示在D‑H坐标系中， 使肘关节2坐标系的 原点与手部腕关节坐标系原点重合在肘关节 2坐标系X 方向上的偏移量； 所述φ＝j2+j3+j4； 所述连杆1为腰关节与肩关节之间的连杆， 所述连杆2为肩关节与肘关节1之间的连杆，权　利　要　求　书 1/2 页 2 CN 115042187 A 2所述连杆3为肘关节1与肘关节2之间的连杆， 所述连杆4为肘关节2与手部腕关节之间的连 杆； 所述连 杆5为手部腕关节与手抓 开合关节之间的连 杆。 6.根据权利要求3所述的基于MPU6050的虚拟现实机械臂控制方法， 其特征在于： 根据 如下公式确定手势姿态的俯仰角pitc h： pitch＝arcsi n(‑2*q1*q3+2*q0 *q2)*57.3； 其中， q0、 q1、 q2以及q3为四元 数。 7.根据权利要求3所述的基于MPU6050的虚拟现实机械臂控制方法， 其特征在于： 根据 如下公式确定手势姿态的航向角yaw： yaw＝arctan( (2*(q1*q2+q0 *q3))/(q0*q0+q1*q1 ‑q2*q2‑q3*q3))*57.3； 其中， q0、 q1、 q2以及q3为四元 数。 8.根据权利要求4所述的基于MPU6050的虚拟现实机械臂控制方法， 其特征在于： 根据 如下公式确定手势姿态的横滚角ro ll： roll＝arctan( (2*q2*q3+2*q0 *q1)/(‑2*q1*q1‑2*q2*q2+1) )*57.3； 其中， q0、 q1、 q2以及q3为四元 数。权　利　要　求　书 2/2 页 3 CN 115042187 A 3