之前我有一篇文章是介绍利用 向量法对Delta并联机械手的正解。今天我来分享代数法的正解，同时在代数法的基础上计算雅可比矩阵。
Delta并联机械手代数法正解
Delta并联机械手的从动臂的上下节点的距离即为从动臂长，因此，每个主动臂可以得到相应三元二次方程组。三个主动臂就有3个三元二次方程。
△r 是 静平台与动平台的半径差值。α主动臂垂直角度，θ是静平台主动臂水平角度，w是主动臂长度，l是从动臂长度。
根据这个方程，我们会发现，3个主动臂上节点和动平台中心，是球表面3个点与球心的关系，而且已知球半径。借助《C# 求取圆心/球心坐标 ∈ C# 编程笔记_已知空间上四点坐标,求球心坐标c#代码-CSDN博客》，我们可以求出球心坐标，也就是动平台的坐标。具体实现，下面以下是代码展示。
(*
#@Copyright:
#@License:
#@Birth: created by Along on 2023-05-25
#@Content: Final_DirectTripod3A_Algebraic-代数法-正解Tripod机械手子程序FB
#@Version: 0.0.1
#@Revision: last revised by Along on 2023-05-25
#@brief 代数法-正解Tripod机械手子程序 ， Final不可继承
根据《Delta并联机器人运动空间的几何作图求解》 https://www.docin.com/p-912524846.html 的方法进行计算。
即：
1，根据 Delta并联机械手的结构特点，从动臂上节点与下节点 之间的距离 = 从动臂， 构建 3个球方程。
2，由于动平台中心点，到3个从动臂上节点的距离都是 从动臂长。即已知 球表面3个点和球半径，求 球心坐标。
3，根据 https://blog.csdn.net/Gou_Hailong/article/details/106686355 的3点+半径的 算法，算出球心。
*)
FUNCTION_BLOCK PUBLIC FINAL Final_DirectTripod3A_Algebraic
VAR_INPUT
lr_ag21 :LREAL; // 静平台 臂1 水平度角
lr_ag22 :LREAL; // 静平台 臂2 水平度角
lr_ag23 :LREAL; // 静平台 臂3 水平度角
lr_ag1 :LREAL; // 静平台 臂1 （水平180度角） 主动臂角度
lr_ag2 :LREAL; // 静平台 臂2 （水平60度角） 主动臂角度
lr_ag3 :LREAL; // 静平台 臂3 （水平-60度角） 主动臂角度
lr_r_top :LREAL; // 静平台 半径
lr_r_plate :LREAL; // 动平台 半径
lr_driven_arm :LREAL; // 主动臂长度
lr_passtive_arm :LREAL; // 从动臂长度
END_VAR
VAR_OUTPUT
lr_out_x :LREAL;
lr_out_y :LREAL;
lr_out_z :LREAL;
END_VAR
VAR
lr_cos1 :LREAL; //臂1 垂直角度（伺服） COS
lr_sin1 :LREAL; //臂1 垂直角度（伺服） SIN
lr_cos2 :LREAL; //臂2 垂直角度（伺服） COS
lr_sin2 :LREAL; //臂2 垂直角度（伺服） SIN
lr_cos3 :LREAL; //臂3 垂直角度（伺服） COS
lr_sin3 :LREAL; //臂3 垂直角度（伺服） SIN
lr_cos21 :LREAL; //臂1 水平角度（静平台） COS
lr_sin21 :LREAL; //臂1 水平角度（静平台） SIN
lr_cos22 :LREAL; //臂2 水平角度（静平台） COS
lr_sin22 :LREAL; //臂2 水平角度（静平台） SIN
lr_cos23 :LREAL; //臂3 水平角度（静平台） COS
lr_sin23 :LREAL; //臂3 水平角度（静平台） SIN
lr_l1 :LREAL;
lr_l2 :LREAL;
lr_l3 :LREAL;
lr_l21 :LREAL;
lr_l22 :LREAL;
lr_l23 :LREAL;
lr_x1 :LREAL;
lr_y1 :LREAL;
lr_z1 :LREAL;
lr_x2 :LREAL;
lr_y2 :LREAL;
lr_z2 :LREAL;
lr_x3 :LREAL;
lr_y3 :LREAL;
lr_z3 :LREAL;
lr_M1 :LREAL;
lr_M2 :LREAL;
lr_M3 :LREAL;
lr_C12 :LREAL;
lr_C23 :LREAL;
lr_C1 :LREAL;
lr_C2 :LREAL;
lr_D1 :LREAL;
lr_D2 :LREAL;
lr_A :LREAL;
lr_B :LREAL;
lr_C :LREAL;
lr_x21 :LREAL;
lr_y21 :LREAL;
lr_z21 :LREAL;
lr_x32 :LREAL;
lr_y32 :LREAL;
lr_z32 :LREAL;
lr_B4AC :LREAL;
lr_cal_x :LREAL;
lr_cal_y :LREAL;
lr_cal_z :LREAL;
lr_cal_x2 :LREAL;
lr_cal_y2 :LREAL;
lr_cal_z2 :LREAL;
END_VAR
//垂直角三角函数
lr_cos1 := COS( lr_ag1 * glvars.const_lr_pi / 180 ) ;
lr_sin1 := SIN( lr_ag1 * glvars.const_lr_pi / 180 ) ;
lr_cos2 := COS( lr_ag2 * glvars.const_lr_pi / 180 ) ;
lr_sin2 := SIN( lr_ag2 * glvars.const_lr_pi / 180 ) ;
lr_cos3 := COS( lr_ag3 * glvars.const_lr_pi / 180 ) ;
lr_sin3 := SIN( lr_ag3 * glvars.const_lr_pi / 180 ) ;
//水平角三角函数
lr_cos21 := COS( lr_ag21 * glvars.const_lr_pi / 180 ) ;
lr_sin21 := SIN( lr_ag21 * glvars.const_lr_pi / 180 ) ;
lr_cos22 := COS( lr_ag22 * glvars.const_lr_pi / 180 ) ;
lr_sin22 := SIN( lr_ag22 * glvars.const_lr_pi / 180 ) ;
lr_cos23 := COS( lr_ag23 * glvars.const_lr_pi / 180 ) ;
lr_sin23 := SIN( lr_ag23 * glvars.const_lr_pi / 180 ) ;
//构建3个球方程组。
lr_x1 := (lr_r_top - lr_r_plate + lr_driven_arm * lr_cos1) * lr_cos21;
lr_y1 := (lr_r_top - lr_r_plate + lr_driven_arm * lr_cos1) * lr_sin21;
lr_z1 := lr_driven_arm * lr_sin1 + glvars.const_lr_z_zerohei;
lr_x2 := (lr_r_top - lr_r_plate + lr_driven_arm * lr_cos2) * lr_cos22;
lr_y2 := (lr_r_top - lr_r_plate + lr_driven_arm * lr_cos2) * lr_sin22;
lr_z2 := lr_driven_arm * lr_sin2 + glvars.const_lr_z_zerohei;
lr_x3 := (lr_r_top - lr_r_plate + lr_driven_arm * lr_cos3) * lr_cos23;
lr_y3 := (lr_r_top - lr_r_plate + lr_driven_arm * lr_cos3) * lr_sin23;
lr_z3 := lr_driven_arm * lr_sin3 + glvars.const_lr_z_zerohei;
lr_M1 := lr_x1 * lr_x1 + lr_y1 * lr_y1 + lr_z1 * lr_z1 ;
lr_M2 := lr_x2 * lr_x2 + lr_y2 * lr_y2 + lr_z2 * lr_z2 ;
lr_M3 := lr_x3 * lr_x3 + lr_y3 * lr_y3 + lr_z3 * lr_z3 ;
lr_C12 := lr_M1 - lr_M2 ;
lr_C23 := lr_M2 - lr_M3 ;
lr_x21 := lr_x2 - lr_x1 ;
lr_y21 := lr_y2 - lr_y1 ;
lr_z21 := lr_z2 - lr_z1 ;
lr_x32 := lr_x3 - lr_x2 ;
lr_y32 := lr_y3 - lr_y2 ;
lr_z32 := lr_z3 - lr_z2 ;
IF lr_x32 * lr_z21 - lr_x21 * lr_z32 = 0 THEN
lr_C1 := 0 ;
ELSE
lr_C1 := (lr_C23 * lr_x21 - lr_C12 * lr_x32 ) / (2 * ( lr_x32 * lr_z21 - lr_x21 * lr_z32 ) );
END_IF
IF lr_x32 * lr_z21 - lr_x21 * lr_z32 = 0 THEN
lr_C2 := 0 ;
ELSE
lr_C2 := - ( lr_x32 * lr_y21 - lr_x21 * lr_y32 ) / ( lr_x32 * lr_z21 - lr_x21 * lr_z32 ) ;
END_IF
IF lr_y21 + lr_z21 * lr_C2 = 0 THEN
lr_D1 := 0;
ELSE
lr_D1 := - (lr_C12 + 2 * lr_C1 * lr_z21 ) / (2 * ( lr_y21 + lr_z21 * lr_C2 )) ;
END_IF
IF lr_y21 + lr_z21 * lr_C2 = 0 THEN
lr_D2 := 0;
ELSE
lr_D2 := -( lr_x21 / (lr_y21 + lr_z21 * lr_C2) );
END_IF
(*
通过消元法 获得的 3个 三元一次方程组，其是一个 三维直线，有无数解。
只有把这个直线的 xyz的关系，代入球方程，才能求出 直线与球的交点，才是我们所需要的解。
*)
//把 y0,z0带入 球方程，获得 一元二次方程。
lr_A := 1 + lr_D2 * lr_D2 + lr_C2 * lr_C2 * lr_D2 * lr_D2 ;
lr_B := -2 * lr_x1 - 2 * lr_y1 * lr_D2 - 2 * lr_z1 * lr_C2 * lr_D2 + 2 * lr_D1 * lr_D2 + 2 * (lr_C1 + lr_C2 * lr_D1) * lr_C2 * lr_D2 ;
lr_C := lr_x1 * lr_x1 + lr_y1 * lr_y1 + lr_z1 * lr_z1 - 2 * lr_y1 * lr_D1 - 2 * lr_z1 * (lr_C1 + lr_C2 * lr_D1) + lr_D1 * lr_D1 + (lr_C1 + lr_C2 * lr_D1) * (lr_C1 + lr_C2 * lr_D1) - lr_passtive_arm * lr_passtive_arm ;
//判断二元一次方程,解的类型
lr_B4AC := lr_B * lr_B - 4 * lr_A * lr_C ;
IF lr_B4AC > 0 THEN
//有2个解
lr_cal_x := (-lr_B + SQRT( lr_B4AC )) / (2 * lr_A) ;
lr_cal_y := lr_cal_x * lr_D2 + lr_D1 ;
lr_cal_z := lr_C1 + lr_C2 * lr_cal_y ;
lr_cal_x2 := (-lr_B - SQRT( lr_B4AC )) / (2 * lr_A) ;
lr_cal_y2 := lr_cal_x2 * lr_D2 + lr_D1 ;
lr_cal_z2 := lr_C1 + lr_C2 * lr_cal_y2 ;
ELSE
IF lr_B4AC = 0 THEN
//唯一解
lr_cal_x := -lr_B / (2 * lr_A) ;
lr_cal_y := lr_cal_x * lr_D2 + lr_D1 ;
lr_cal_z := lr_C1 + lr_C2 * lr_cal_y ;
lr_cal_x2 := lr_cal_x ;
lr_cal_y2 := lr_cal_Y ;
lr_cal_z2 := lr_cal_z ;
ELSE
//无解
lr_cal_x := 0 ;
lr_cal_y := 0 ;
lr_cal_z := 0 ;
lr_cal_x2 := lr_cal_x ;
lr_cal_y2 := lr_cal_Y ;
lr_cal_z2 := lr_cal_z ;
END_IF
END_IF
//根据 2个解的Z位置，判断合适的解 并输出
IF
lr_cal_z < glvars.const_lr_z_zerohei
THEN
lr_out_x := lr_cal_x;
lr_out_y := lr_cal_y;
lr_out_z := lr_cal_z;
ELSE
lr_out_x := lr_cal_x2;
lr_out_y := lr_cal_y2;
lr_out_z := lr_cal_z2;
END_IF
Delta并联机械手雅可比矩阵
首先感谢刘大佬的技术指导。
利用上文的三元二次方程，两边分别求导dx/dα , dx/dα，dz/dα。
通过3个角度用矩阵表示
(*
#@Copyright:
#@License:
#@Birth: created by Along on 2023-05-18
#@Content: Final_Jacobian-代数法-正解Tripod机械手-- 雅可比矩阵 子程序FB
#@Version: 0.0.1
#@Revision: last revised by Along on 2023-05-18
#@brief 代数法-正解Tripod机械手-- 雅可比矩阵 子程序FB ， Final不可继承
根据《Delta并联机器人运动空间的几何作图求解》 https://www.docin.com/p-912524846.html 的方法进行计算。
即：
1，根据 Delta并联机械手的结构特点，从动臂上节点与下节点 之间的距离 = 从动臂， 构建 3个球方程。
2，由于动平台中心点，到3个从动臂上节点的距离都是 从动臂长。即已知 球表面3个点和球半径，求 球心坐标。
3，根据 https://blog.csdn.net/Gou_Hailong/article/details/106686355 的3点+半径的 算法，算出球心。
4，根据 3点+半径的 球心算法，进行求导和矩阵运算，得出 雅可比矩阵。
*)
FUNCTION_BLOCK PUBLIC FINAL Final_Jacobian
VAR_INPUT
lr_x :LREAL; //终端X坐标
lr_y :LREAL; //终端Y坐标
lr_z :LREAL; //终端Z坐标
lr_ag_11 :LREAL; //静平台 臂1 水平角度
lr_ag_12 :LREAL; //静平台 臂2 水平角度
lr_ag_13 :LREAL; //静平台 臂3 水平角度
lr_ag_21 :LREAL; //静平台 臂1 主动臂角度
lr_ag_22 :LREAL; //静平台 臂2 主动臂角度
lr_ag_23 :LREAL; //静平台 臂3 主动臂角度
lr_driven_arm :LREAL; //主动臂长
lr_passive_arm :LREAL; //从动臂长
lr_r_top :LREAL; //静平台 半径
lr_r_plate :LREAL; //动平台 半径
END_VAR
VAR_OUTPUT
matrix_jaco :ARRAY[0..2,0..2] OF LREAL; //雅可比矩阵
bs_singular_point :BOOL; //信号 机械手奇异点
END_VAR
VAR
j_d_1 :Stru_tripod_Jp; // 臂1 的 Jp结构体实例
j_d_2 :Stru_tripod_Jp; // 臂2 的 Jp结构体实例
j_d_3 :Stru_tripod_Jp; // 臂3 的 Jp结构体实例
matrix_jp :ARRAY[0..2,0..2] OF LREAL; // 矩阵 jp
matrix_jq :ARRAY[0..2,0..2] OF LREAL; // 矩阵 jq
matrix_jp_inv :ARRAY[0..2,0..2] OF LREAL; // 矩阵 jp的逆矩阵
END_VAR
//求3个臂的 Jp结构体实例
j_d_1 := METH_Jp(lr_hori_ag:= lr_ag_11, lr_arm_ag:= lr_ag_21);
j_d_2 := METH_Jp(lr_hori_ag:= lr_ag_12, lr_arm_ag:= lr_ag_22);
j_d_3 := METH_Jp(lr_hori_ag:= lr_ag_13, lr_arm_ag:= lr_ag_23);
//构建 矩阵 jp
matrix_jp := Matrix.Fn_Matrix_Init(
j_d_1.lr_Jx,j_d_1.lr_Jy,j_d_1.lr_Jz,
j_d_2.lr_Jx,j_d_2.lr_Jy,j_d_2.lr_Jz,
j_d_3.lr_Jx,j_d_3.lr_Jy,j_d_3.lr_Jz
);
//构建 矩阵 jq
matrix_jq := Matrix.Fn_Matrix_Init(
j_d_1.lr_Jq,0,0,
0,j_d_2.lr_Jq,0,
0,0,j_d_3.lr_Jq
);
//jp的逆矩阵
matrix_jp_inv := Matrix.Fn_Matrix_33_Inverse(matrix_jp);
//雅可比矩阵
matrix_jaco := Matrix.Fn_Matrix_33_Cross(matrix_jp_inv,matrix_jq);
//信号 机械手奇异点
bs_singular_point := Matrix.Fn_Matrix_33_Determinant(matrix_jaco) = 0 ;
(*
#@Copyright:
#@License:
#@Birth: created by Along on 2023-05-18
#@Content: meth_Jp-求导的变量函数 method
#@Version: 0.0.1
#@Revision: last revised by Along on 2023-05-18
#@brief 求导的变量函数 method
#@requirement
*)
METHOD PRIVATE meth_Jp : Stru_tripod_Jp
VAR_INPUT
lr_hori_ag :LREAL; //水平角度
lr_arm_ag :LREAL; //臂 垂直角度
END_VAR
VAR
lr_hori_cos :LREAL; //水平角度 cos
lr_hori_sin :LREAL; //水平角度 sin
lr_vert_cos :LREAL; //垂直角度 cos
lr_vert_sin :LREAL; //垂直角度 sin
lr_x_v1 :LREAL; //公式变量x
lr_y_v1 :LREAL; //公式变量y
lr_z_v1 :LREAL; //公式变量z
END_VAR
//水平角三角函数
lr_hori_cos := COS( lr_hori_ag * glvars.const_lr_pi / 180 ) ;
IF lr_hori_ag = 0 OR lr_hori_ag = 180 THEN
lr_hori_sin := 0;
ELSE
lr_hori_sin := SIN( lr_hori_ag * glvars.const_lr_pi / 180 ) ;
END_IF
//垂直角三角函数
lr_vert_cos := COS( lr_arm_ag * glvars.const_lr_pi / 180 ) ;
lr_vert_sin := SIN( lr_arm_ag * glvars.const_lr_pi / 180 ) ;
METH_Jp.lr_Jx := lr_x - ( lr_r_top - lr_r_plate + lr_driven_arm * lr_vert_cos ) * lr_hori_cos;
METH_Jp.lr_Jy := lr_y - ( lr_r_top - lr_r_plate + lr_driven_arm * lr_vert_cos ) * lr_hori_sin ;
METH_Jp.lr_Jz := lr_z - lr_driven_arm * lr_vert_sin - glvars.const_lr_z_zerohei;
METH_Jp.lr_Jq := METH_Jp.lr_Jx * ( -lr_driven_arm * lr_vert_sin * lr_hori_cos )
+ METH_Jp.lr_Jy * ( -lr_driven_arm * lr_vert_sin * lr_hori_sin )
+ METH_Jp.lr_Jz * ( lr_driven_arm * lr_vert_cos );
代码里面用到矩阵运算函数，可以到【免费】原创CODESYS操作Matrix33矩阵运算的功能块库文件资源-CSDN文库下载使用。
以上。
1895
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
