[发明专利]平面3R操作臂逆运动学实现方法

权利要求书

1.平面3R操作臂逆运动学实现方法，其特征在于：给定大臂、小臂、工装的长度l₁、l₂、l₃和目标点D的位置以及工装在目标点的姿态α，求肩关节O1、肘关节A和腕关节O2的角度θ₁、θ₂、θ₃，具体计算方法包括以下步骤：

步骤1：计算腕关节O2的坐标和转角θ₃，目标点D的坐标(x_D，y_D)和工装在目标点D的姿态α是给定的，可以确定腕关节O2的坐标(x_O2，y_O2)=( x_D +l₃cosα，y_D +l₃sinα)，腕关节转角θ₃=α+π；

步骤2：计算肘关节A的坐标和转角θ₂；此问题归结为圆O1(半径l₁)与圆O2(半径l₂)相交，求得两个交点之一A点坐标(x_A，y_A)，如图2所示采用代数法与几何法相结合、静动坐标系相互转换的方法，包括以下步骤：

步骤2.1：在静坐标系XOY中，两个圆的方程是

⊙O1：(x- x_O1)²+(y- y_O1)²= l₁²，⊙O2：(x- x_O2)²+(y- y_O2)²= l₂²；

步骤2.2：计算两圆的圆心距l=((y_O2- y _O1)²+(x_O2- x _O1) ²)^1/2，如果l l₁+ l₂则两圆相离，无交点，计算结束，否则进入下一步；

步骤2.3：建立动坐标系X1O1Y1，原点位于静坐标系XOY中的O1点，横轴O1X1通过连心线O1O2，与静坐标系的X轴夹角θ=atan2(y_O2- y _O1，x_O2- x _O1)；

步骤2.4：在动坐标系中，两个圆的方程是

⊙O1：u ²+v ²= l₁²，⊙O2：(u- l)²+v ²= l₂²；

步骤2.5：在动坐标系中，两个圆相交的公共弦为一条垂直于横轴O1X1的直线，与横轴O1X1交点为：u₀=( l₁²- l₂²+ l²)/(2l)，即O1C= u₀。根据勾股定理，AC=( l₁²-u₀ ²)^1/2，因此A点坐标(u_A，v_A)=( u₀，( l₁²-u₀ ²)^1/2)；同理，B点坐标(u_B，v_B)=( u₀，-( l₁²-u₀ ²)^1/2)；

步骤2.6：在静坐标系XOY中，通过坐标变换，计算A点的坐标为

(x_A，y_A)= (u_Acosθ+ v_A sinθ+ x_O1，v_Acosθ- u_A sinθ+y_O1)

B点坐标计算方法相同，(x_B，y_B)= (u_Bcosθ+ v_Bsinθ+ x_O1，v_Bcosθ- u_B sinθ+y_O1)；

步骤2.7：求得肘关节转角θ₂=atan2(y_O2- y_A，x_O2- x_A)；其中atan2为C语言中反正切函数，返回值的单位为弧度，取值范围为(-π，π]；

步骤3：计算肩关节的转角θ₁；肩关节O1的坐标(x_O1，y_O1)是已知的，因此肩关节转角θ₁=atan2(y_A- y_O1，x_A- x_O1)。