本发明属于机器人逆运动学领域,具体涉及一种任意关系的二阶子问题逆运动学求解方法。
背景技术:
paden-kanhan子问题在机器人逆运动学应用非常广泛,因为它具有几何意义和数值稳定性,能够灵活的为多种机器人提供封闭解。paden-kanhan子问题主要分为三类:一阶子问题,二阶子问题,三阶子问题。其中一阶子问题是针对单关节的转动r或平移t运动的逆解问题;二阶子问题是针对两个关节逆解问题,包含了3种情况:rr,tt,rt/tr,其中rr又分为相交、平行、异面垂直等不同的类型;三阶子问题是针对三个关节的逆解问题,包含了6种情况。在实际中,由于加工、装配很多几何关系很难保证,比如:相交、平行,而且不同的结构需要选择不同的公式,这为实际应用带来很多不便。
技术实现要素:
针对现有技术中存在的上述技术问题,本发明提出了一种任意关系的二阶子问题逆运动学求解方法,设计合理,克服了现有技术的不足,具有良好的效果。
为了实现上述目的,本发明采用如下技术方案:
一种任意关系的二阶子问题逆运动学求解方法,包括如下步骤:
步骤1:求θ1
二阶子问题rr可用公式表示为
其中,
||c-r2||=||p-r2||(5);
将
x1sinθ1+y1cosθ1=z1(9);
其中
上式中需要通过调整r1和r2来保证
步骤2:求θ2
根据已知的θ1可得c的值,而c还可表示为:
将
x2sinθ2+y2cosθ2=z2(14);
其中
θ2角度的具体象限由
本发明所带来的有益技术效果:
1、计算效率高,给出了关节角度的封闭解,可利反三角函数直接求出,具有很高的计算效率;2、实现简单,每个关节的表达形式非常简单易懂,只需求解一次反三角函数即可;3、应用范围广,可应用于任意2r机器人中,不需要考虑其轴线之间的几何关系。
附图说明
图1为任意关系的rr结构图。
具体实施方式
下面结合附图以及具体实施方式对本发明作进一步详细说明:
一种任意关系的二阶子问题逆运动学求解方法,包括如下步骤:
步骤1:求θ1
如图1所示,二阶子问题rr可用公式表示为
其中,
其中,i3×3为3×3的单位矩阵,
其中,
根据旋量理论的距离相等原则可知:
||c-r2||=||p-r2||(5);
根据旋量理论的基本原理可知:
上述两式相减可得:
将指数积公式
将公式(7)带入公式(5)可得:
再将
x1sinθ1+y1cosθ1=z1(9);
其中
设x1=ρcosφ,y1=ρsinφ,则
其中,
则关节角度θ1可表示为:
上式中需要通过调整r1和r2来保证
步骤2:求θ2
将θ1的值带入公式(7)中可得c的值,而c还可表示为:
将
x2sinθ2+y2cosθ2=z2(14);
其中
由于
则θ2可表示为:
θ2角度的具体象限由
当然,上述说明并非是对本发明的限制,本发明也并不仅限于上述举例,本技术领域的技术人员在本发明的实质范围内所做出的变化、改型、添加或替换,也应属于本发明的保护范围。