8_机械臂工作台坐标系标定及验证

1、机械臂实际数据

AUBO 机械臂xOxy方式标定用户坐标系：

X轴正半轴一点：

XOY象限任意一点(还是有一些要求的):

一些坐标点的验证：

2、如何根据上述3点，计算work1坐标系与base坐标系的关系？

最开始在网上没找到相关的资料(论资料搜索对程序员的重要性)，唯一一篇有参考价值的还需要充会员，最后不得不冲了会员，太想知道了。之后就有了向量叉积那篇笔记。有了前两篇笔记的铺垫，本次笔记就纯是将思路代码化，是个体力活，要注意的是坐标系之间的相对关系，那块调了好一会儿。

部分代码(通过work1坐标系X轴向量和work1 XOY象限向量的叉积求work1坐标系的Z轴向量，并单位化)：

//po  origin          (-0.163555, 0.396348, 0.071506, -90.148331, 45.409718, -90.103142)
//px  x-axis positive (-0.326204, 0.396016, 0.071179, -90.148407, 45.409279, -90.103485)
//pxy first quadrant  (-0.324761, 0.231210, 0.071491, -90.148285, 45.409794, -90.103714)
    //px-po,  px向量减去po向量，基于base坐标系
    double user_px_x = -0.326204+0.163555;
    double user_px_y = 0.396016-0.396348;
    double user_px_z = 0.071179-0.071506;


    //pxy-po, pxy向量减去po向量，基于base坐标系
    double user_pxy_x = -0.324761+0.163555;
    double user_pxy_y = 0.231210-0.396348;
    double user_pxy_z = 0.071491-0.071506;


    double user_px_modulus = sqrt(user_px_x*user_px_x +
                                  user_px_y*user_px_y +
                                  user_px_z*user_px_z);


    double user_pxy_modulus = sqrt(user_pxy_x*user_pxy_x +
                                  user_pxy_y*user_pxy_y +
                                  user_pxy_z*user_pxy_z);     


    //px normal,  px向量单位化
    double user_px_x_mormal = user_px_x/user_px_modulus;
    double user_px_y_mormal = user_px_y/user_px_modulus;
    double user_px_z_mormal = user_px_z/user_px_modulus;
    //pxy normal,  pxy向量单位化
    double user_pxy_x_mormal = user_pxy_x/user_pxy_modulus;
    double user_pxy_y_mormal = user_pxy_y/user_pxy_modulus;
    double user_pxy_z_mormal = user_pxy_z/user_pxy_modulus;


    Eigen::Vector3d user_px_axis_normal(user_px_x_mormal, user_px_y_mormal, user_px_z_mormal);
    Eigen::Vector3d user_pxy_axis_normal(user_pxy_x_mormal, user_pxy_y_mormal, user_pxy_z_mormal);    
          
    //a = {ax,ay,az), b = (bx,by,bz),  根据px和pxy向量的叉积求pz
    //CrossProduct -->> poz axb =(aybz -azby,azbx -axbz,axby -aybx}
    double user_pz_x = user_px_y_mormal*user_pxy_z_mormal - user_px_z_mormal*user_pxy_y_mormal;
    double user_pz_y = user_px_z_mormal*user_pxy_x_mormal - user_px_x_mormal*user_pxy_z_mormal;
    double user_pz_z = user_px_x_mormal*user_pxy_y_mormal - user_px_y_mormal*user_pxy_x_mormal;


    double user_pz_modulus = sqrt(user_pz_x*user_pz_x+
                                  user_pz_y*user_pz_y +
                                  user_pz_z*user_pz_z);


    //pz 向量单位化
    double user_pz_x_normal = user_pz_x/user_pz_modulus;
    double user_pz_y_normal = user_pz_y/user_pz_modulus;
    double user_pz_z_normal = user_pz_z/user_pz_modulus;


根据px和pz可求py，并将其单位化。
工作台坐标系相对于base坐标系的旋转矩阵：
    R << user_px_x_mormal, user_py_x_normal, user_pz_x_normal,
         user_px_y_mormal, user_py_y_normal, user_pz_y_normal,
         user_px_z_mormal, user_py_z_normal, user_pz_z_normal;

根据《7_机械臂工作台坐标系理论_一般坐标系的映射_2》笔记，构建齐次矩阵，代码如下：

Eigen::MatrixXd pos(3,1);  
    pos(0,0) = -0.163555;  
    pos(1,0) = 0.396348;  
    pos(2,0) = 0.071506;  


    Eigen::MatrixXd homogeneous(1,4);    
    homogeneous(0,0)=0.0f;  
    homogeneous(0,1)=0.0f;  
    homogeneous(0,2)=0.0f;  
    homogeneous(0,3)=1.0f;  


    Eigen::MatrixXd posX(3,1);  
    posX(0,0) = -0.326204;  
    posX(1,0) = 0.396016;  
    posX(2,0) = 0.071179;  


    Eigen::MatrixXd posTest(3,1);  
    posTest(0,0) = 0.214300;  
    posTest(1,0) = -0.138937;  
    posTest(2,0) = 0.200473;  


    Eigen::Matrix     <double,            </double,3, 4> m3x4;    
    Eigen::Matrix     <double,            </double,4, 4> m4x4;    


    m3x4 << R, pos;  
    m4x4 << m3x4, homogeneous;