【轨迹规划】机械臂末端姿态轴角插补_机械臂关节角度插补-优快云博客

本文介绍了一种将机器人末端执行器的姿态从一个旋转矩阵平滑过渡到另一个的方法。通过计算两个姿态之间的轴角差，并将其均匀分布在指定的时间段内，实现了姿态的线性插值。该方法适用于机器人运动规划中的平滑控制。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

对转动角度进行平滑规划，从零到θ
t时刻角度θt
转为旋转矩阵：

根据末端位姿应用IK
得到关节空间q
直接位置控制或者
根据上一时刻qlast
计算delta q，×dt
作为微分进行速度控制

test：

vector<rbdlmath::Vector3d> axis_traj(rbdlmath::Matrix3d ori1,rbdlmath::Matrix3d ori2, int N)
{
    vector<rbdlmath::Vector3d> oriall;
    rbdlmath::Vector3d axs;
    rbdlmath::Matrix3d midori;
    rbdlmath::Matrix3d ori1i;
    rbdlmath::Vector3d pos;
    double theta;
    ori1i =ori1.transpose();
    midori =ori2*ori1i;
//                      mat =mat0_t*mat1;
    theta = acos((midori(0,0)+midori(1,1)+midori(2,2)-1)/2);
    double axs_x = (midori(2,1)-midori(1,2))/2*sin(theta);
    double axs_y = (midori(0,2)-midori(2,0))/2*sin(theta);
    double axs_z = (midori(1,0)-midori(0,1))/2*sin(theta);
    double axs_norm = sqrt(pow(axs_x,2)+pow(axs_y,2)+pow(axs_z,2));
    axs << axs_x/axs_norm,axs_y/axs_norm,axs_z/axs_norm;
    cout << "theta : " << theta*180/M_PI << endl;
    cout<< "axs:"<< axs <<endl;
    cout<< "mat:"<< midori <<endl;

    double dtheta = theta/N;
    double newtheta = 0;
    rbdlmath::Matrix3d mat_c;
    rbdlmath::Matrix3d mat_out;
    for(int i = 0;i < N;i++)
    {
        newtheta = newtheta + dtheta;
        cout << "newtheta is " << newtheta << endl;
        mat_c(0,0)=pow(axs[0],2)*(1-cos(newtheta)) + cos(newtheta);
        mat_c(0,1)=axs[0]*axs[1]*(1-cos(newtheta)) - axs[2]*sin(newtheta);
        mat_c(0,2)=axs[0]*axs[2]*(1-cos(newtheta)) + axs[1]*sin(newtheta);

        mat_c(1,0)=axs[0]*axs[1]*(1-cos(newtheta)) + axs[2]*sin(newtheta);
        mat_c(1,1)=pow(axs[1],2)*(1-cos(newtheta)) + cos(newtheta);
        mat_c(1,2)=axs[1]*axs[2]*(1-cos(newtheta)) - axs[0]*sin(newtheta);

        mat_c(2,0)=axs[0]*axs[2]*(1-cos(newtheta)) - axs[1]*sin(newtheta);
        mat_c(2,1)=axs[1]*axs[2]*(1-cos(newtheta)) + axs[0]*sin(newtheta);
        mat_c(2,2)=pow(axs[2],2)*(1-cos(newtheta)) + cos(newtheta);

        mat_out = mat_c*ori1;
        oriall.push_back(mat_out.eulerAngles(0,1,2));
    }
    cout << "oriall 's size is " << oriall.size() << endl;

    return oriall;
}