(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210777798.5 (22)申请日 2022.07.04 (71)申请人 中国中煤能源集团有限公司 地址 100120 北京市朝阳区黄 寺大街1号 申请人 中煤陕西榆林能源化工有限公司 　 中煤电气有限公司 (72)发明人 王金国　杨会龙　姚蔚利　王立辉　 陈惟彪　 (74)专利代理 机构 北京君慧知识产权代理事务 所(普通合伙) 11716 专利代理师 韩思思 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于倾角传感器的机械臂位姿控制方 法 (57)摘要 本申请涉及工程机械技术领域， 公开了一种 基于倾角传感器的机械臂位姿控制方法， 包括以 下步骤： S1、 构建根据所述倾角传感器测量所述 机械臂旋转角度的数学模型； S2、 获取所述倾角 传感器的输出值， 所述倾角传感器安装在所述机 械臂的关节处； S3、 通过所述倾角传感器的输出 值和所述数学模 型解算所述机械臂的旋转角度； S4、 根据所述机械臂的旋转角度解算所述机械臂 的偏航角； S5、 根据所述机械臂的偏航角， 优化所 述机械臂的位姿控制。 本申请的机械臂位姿控制 方法测量结果稳定、 可靠， 给工程人员带来了极 大的方便 。 权利要求书2页 说明书7页 附图2页 CN 115042182 A 2022.09.13 CN 115042182 A 1.一种基于倾角传感器的机 械臂位姿控制方法， 其特 征在于， 包括以下步骤： S1、 构建根据所述 倾角传感器测量所述机 械臂旋转角度的数 学模型； S2、 获取所述倾角传感器的输出值， 所述 倾角传感器安装在所述机 械臂的关节处； S3、 通过所述倾角传感器的输出值和所述数 学模型解 算所述机 械臂的旋转角度； S4、 根据所述机 械臂的旋转角度解 算所述机 械臂的偏航角； S5、 根据所述机 械臂的偏航角， 优化所述机 械臂的位姿控制。 2.根据权利要求1所述的一种基于倾角传感器的机械臂位姿控制方法， 其特征在于， 所 述倾角传感器为双轴倾角传感器， 所述 步骤S1包括： 设置水平面为所述倾角传感器测量轴X、 Y轴的参考平面， 所述倾角传感器的测量轴X、 Y 轴互相垂直； 根据所述参考平面， 建立第一坐标系oxyz， y轴为所述机械臂的旋转轴在水平面上的投 影， x轴垂直于y轴且与y轴同处于水平面内， z轴垂直于水平面并与x、 y轴相交于o 点； 将机械臂旋转一定角度后， 建立第二坐标系ouvw， ou轴与x轴重合， ov轴与所述机械臂 的旋转轴重合， o w轴为过点 o的uov平面的法向量； 根据所述第一坐标系oxyz和第二坐标系ouvw， 建立所述倾角传感器的测量值和所述机 械臂旋转角度的数 学模型。 3.根据权利要求2所述的一种基于倾角传感器的机械臂位姿控制方法， 其特征在于， 所 述步骤S1中， 根据所述第一坐标系 oxyz和第二坐标系ouvw， 建立所述倾角传感器的输出值 和所述机 械臂旋转角度的数 学模型具体如下： tx＝arcsi n(sinθ cosβ )    (1) ty＝β  (2) 其中， tx为所述机械臂转动角度θ后所述倾角传感器测量轴X轴输出值， ty为所述倾角 传感器测量轴Y轴输出值， θ为所述机械臂的旋转角度， β 为所述机械臂的旋转轴与水平 面的 夹角。 4.根据权利要求3所述的一种基于倾角传感器的机械臂位姿控制方法， 其特征在于， 所 述步骤S4中， 根据所述机 械臂的旋转角度解 算所述机 械臂的偏航角的具体方法如下： 由公式(1)得 所述机械臂未旋转时初始旋转角度 θ1′表达式如下： 其中， θ1′为未旋转的情况下因所述机械臂载体倾斜计算得到的旋转角度， tx1为所述倾 角传感器的测量轴X轴未旋转时的输出值， ty1为所述倾角传感器Y1轴与平面的夹角； 所述倾角传感器旋转角度 θ后， 旋转角度 θ1″的表达式如下： 其中， θ1″为所述机械臂旋转角度 θ计算得到的旋转角度， tx1′为所述倾角传感器的测量 轴X轴的输出值 为； 根据机械臂的旋转角度解 算偏航角的表达式为：权　利　要　求　书 1/2 页 2 CN 115042182 A 2其中， α'为所述机械臂未旋转时相对于水平面的偏航角， α ”为所述机械臂旋转角度θ后 相对于水平面的偏航角； 对(3)、 (4)、 (5)、 (6)式进行处 理得: α ＝α” ‑α' 其中， α 为所述机 械臂实际的偏航角。 5.根据权利要求1所述的一种基于倾角传感器的机械臂位姿控制方法， 其特征在于， 所 述步骤S2中， 所述 倾角传感器的测量轴与机 械臂的旋转轴平行且处于同一方向。 6.根据权利要求5所述的一种基于倾角传感器的机械臂位姿控制方法， 其特征在于， 所 述机械臂的旋转轴与水平面的夹角为β， β 的取值范围为β ∈(0,90 °)。权　利　要　求　书 2/2 页 3 CN 115042182 A 3