trhyrnjtes

最新推荐文章于 2024-06-14 13:38:01 发布

Zenker0418

最新推荐文章于 2024-06-14 13:38:01 发布

阅读量18

点赞数

文章标签：算法

本文链接：https://blog.csdn.net/Zenker0418/article/details/134448606

版权

void ErrorCompensation_14(double Tx[16], double DiscartesPoint[6], double pos_start[6])
{
std::cout << endl;
std::cout << “ErrorCompensation_14”;
// 补偿后DH参数
double theta[6];
theta[0] = pos_start[0]; //;
theta[1] = pos_start[1]; //- 0.200247 / 180 * EIGEN_PI;
theta[2] = pos_start[2]; // - 0.0241131 / 180 * EIGEN_PI;
theta[3] = pos_start[3]; // 0.107099 / 180 * EIGEN_PI;
theta[4] = pos_start[4]; // - 5.535242 / 180 * EIGEN_PI;
theta[5] = pos_start[5]; // 1 + 0 / 180 * EIGEN_PI;
double D[6] = {137.5,
0.0,
0.0,
120.941,
118.061, /* */
88.5};
double A[6] = {0.0,
-400.811,
-373.927,
0.0,
0.0,
0.0};
double Alpha[6] = {90.0848 / 180 * EIGEN_PI,
0.499605 / 180 * EIGEN_PI,
0.251361 / 180 * EIGEN_PI,
89.9559 / 180 * EIGEN_PI,
-89.8948 / 180 * EIGEN_PI,
0.323088 / 180 * EIGEN_PI};
struct timeval tv, tv1, tv2, tv3, tv4, tv5, tv6, tv7;
double p[6], P[6];
double rpy[3];
double RPY[3];
double T[6][16] = {{0.0}};
double Theta[6] = {0};
// lilun mubiao
p[0] = Tx[3] * 1000;
p[1] = Tx[7] * 1000;
p[2] = Tx[11] * 1000;
double r[3][3] = {{Tx[0], Tx[1], Tx[2]}, {Tx[4], Tx[5], Tx[6]}, {Tx[8], Tx[9], Tx[10]}};
rotationMatrixToRPY(r, rpy);
p[3] = rpy[0];
p[4] = rpy[1];
p[5] = rpy[2];
// 最大迭代次数
int maxIter = 10;
// 阻尼系数
double u = 0.01;
// H矩阵维度
int Dim = 6;
int updateJ = 1;
double error, error_last, error_1, error_r;
Eigen::MatrixXd J_ad = Eigen::MatrixXd::Zero(6, 6);
Eigen::VectorXd delta_P = Eigen::VectorXd::Zero(6);
Eigen::VectorXd delta_theta = Eigen::VectorXd::Zero(6);
Eigen::VectorXd delta_theta_1 = Eigen::VectorXd::Zero(6);
Eigen::MatrixXd J = Eigen::MatrixXd::Zero(6, 6);

for (int i = 1; i <= maxIter; i++)
{
    ///
    if (updateJ == 1)
    {
        kinematics_1(theta, D, A, Alpha, T);
        P[0] = T[5][3];
        P[1] = T[5][7];
        P[2] = T[5][11];
        double R[3][3] = {{T[5][0], T[5][1], T[5][2]}, {T[5][4], T[5][5], T[5][6]}, {T[5][8], T[5][9], T[5][10]}};
        rotationMatrixToRPY(R, RPY);
        P[3] = RPY[0];
        P[4] = RPY[1];
        P[5] = RPY[2];

        delta_P(0) = p[0] - P[0];
        delta_P(1) = p[1] - P[1];
        delta_P(2) = p[2] - P[2];
        delta_P(3) = p[3] - P[3];
        delta_P(4) = p[4] - P[4];
        delta_P(5) = p[5] - P[5];

        J_ad = JacobiMatrix_1(theta, D, A, Alpha);

        for (int i = 0; i < 6; i++)
        {
            for (int j = 0; j < 6; j++)
            {
                J(i, j) = J_ad(i, j);
            }
        }

        error_last = (p[0] - P[0]) * (p[0] - P[0]) + (p[1] - P[1]) * (p[1] - P[1]) + (p[2] - P[2]) * (p[2] - P[2]); // + (p[3] - P[3]) * (p[3] - P[3]) + (p[4] - P[4]) * (p[4] - P[4]) + (p[5] - P[5]) * (p[5] - P[5]);
    }
    Eigen::MatrixXd temp = inv_6(J.transpose() * J + u * Eigen::MatrixXd::Identity(Dim, Dim));
    delta_theta = temp * J.transpose() * delta_P;

    Theta[0] = theta[0] + delta_theta(0);
    Theta[1] = theta[1] + delta_theta(1);
    Theta[2] = theta[2] + delta_theta(2);
    Theta[3] = theta[3] + delta_theta(3);
    Theta[4] = theta[4] + delta_theta(4);
    Theta[5] = theta[5] + delta_theta(5);
    kinematics_1(Theta, D, A, Alpha, T);
    P[0] = T[5][3];
    P[1] = T[5][7];
    P[2] = T[5][11];
    double R[3][3] = {{T[5][0], T[5][1], T[5][2]}, {T[5][4], T[5][5], T[5][6]}, {T[5][8], T[5][9], T[5][10]}};
    rotationMatrixToRPY(R, RPY);
    P[3] = RPY[0];
    P[4] = RPY[1];
    P[5] = RPY[2];

    delta_P(0) = p[0] - P[0];
    delta_P(1) = p[1] - P[1];
    delta_P(2) = p[2] - P[2];
    delta_P(3) = p[3] - P[3];
    delta_P(4) = p[4] - P[4];
    delta_P(5) = p[5] - P[5];

    error = (p[0] - P[0]) * (p[0] - P[0]) + (p[1] - P[1]) * (p[1] - P[1]) + (p[2] - P[2]) * (p[2] - P[2]); // + (p[3] - P[3]) * (p[3] - P[3]) + (p[4] - P[4]) * (p[4] - P[4]) + (p[5] - P[5]) * (p[5] - P[5]);

    // std::cout << delta_P << endl;

    if (error < error_last)
    {
        for (int k = 0; k < 6; k++)
        {
            theta[k] = Theta[k];
        }
        u = u / 10;

        error_last = error;
        // double err = sqrt(error_last);
        updateJ = 1;
    }
    else
    {
        u = u * 10;
        updateJ = 0;
    }

    if (error < 0.0001)
    {
        break;
    }
}
for (int i = 0; i < 6; i++)
{
    DiscartesPoint[i] = Theta[i];
}
cout << endl;

}