TCPdsfsdfd

最新推荐文章于 2024-06-14 23:48:57 发布

Zenker0418

最新推荐文章于 2024-06-14 23:48:57 发布

阅读量973

点赞数 22

文章标签： python

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

版权

void RunDefination::TcpCalibration(float DH[6], vector Point, double T_tool[NUM_TRAJECTORY_ARRARY], double Error[3]){
vector Pointtemp;
int pointNum = Point.size() / 6;
int Comb_no = 0;
int Comb_num = 0;
Comb_num =Combination(pointNum, 4);
int Comb_array[Comb_num][4] = {{0}};
double Comb_error[Comb_num] = {0.0};
int Comb_error_min_no = 0;
double Comb_error_min = 100000000000;
// 从点集中抽出四个点
for (int i = 0; i < pointNum - 3; i++)
{
for (int j = i + 1; j < pointNum - 2; j++)
{
for (int k = j + 1; k < pointNum - 1; k++)
{
for (int l = k + 1; l < pointNum; l++)
{
Comb_array[Comb_no][0] = i;
Comb_array[Comb_no][1] = j;
Comb_array[Comb_no][2] = k;
Comb_array[Comb_no][3] = l;
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * i, Point.begin() + 6 * (i + 1));
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * j, Point.begin() + 6 * (j + 1));
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * k, Point.begin() + 6 * (k + 1));
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * l, Point.begin() + 6 * (l + 1));

                TcpCalibration_1(DH, Pointtemp, T_tool, Error);
                Comb_error[Comb_no] = Error[0];
                qDebug()<<i<<" "<<j<<" "<<k<<" "<<l<<" "<<Error[0]<<" "<<Error[1]<<" "<<Error[2];
                if (Comb_error[Comb_no] < Comb_error_min)
                {
                    Comb_error_min = Comb_error[Comb_no];
                    Comb_error_min_no = Comb_no;
                }
                Comb_no++;
                Pointtemp.clear();
            }
        }
    }
}
// 给出误差较小的标定点
Comb_no = Comb_error_min_no;
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * Comb_array[Comb_no][0], Point.begin() + 6 * (Comb_array[Comb_no][0] + 1));
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * Comb_array[Comb_no][1], Point.begin() + 6 * (Comb_array[Comb_no][1] + 1));
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * Comb_array[Comb_no][2], Point.begin() + 6 * (Comb_array[Comb_no][2] + 1));
Pointtemp.insert(Pointtemp.begin(), Point.begin() + 6 * Comb_array[Comb_no][3], Point.begin() + 6 * (Comb_array[Comb_no][3] + 1));
TcpCalibration_1(DH, Pointtemp, T_tool, Error);

}

void RunDefination::TcpCalibration_1(float DH[6], vector Point, double T_tool[NUM_TRAJECTORY_ARRARY], double Error[3])
{
#if 1
int SizeOfVector = Point.size();
int pointNum = 0;
int Dimension = 0;
pointNum = SizeOfVector/6; //标定点数量
Dimension = 3*(pointNum-1); //方程数量
double PointAngle[pointNum][6] = {0};
double Tx[pointNum][16] = {0};
Eigen::MatrixXd CoefficientMatrix(Dimension,3); //系数矩阵
Eigen::VectorXd B(Dimension); //余项
Eigen::MatrixXd GeneralizedInverseOfCoefficientMatrix(3,Dimension); //系数矩阵的广义逆
Eigen::Matrix3d RotationMatrix; //旋转矩阵
Eigen::Matrix3d RotationMatrix1;
Eigen::Matrix3d RotationMatrix2;
Eigen::Vector3d PositionVector; //位置矢量
Eigen::Vector3d PositionVector1;
Eigen::Vector3d PositionVector2;
Eigen::Vector3d TcpVector; //TCP
Eigen::Matrix3d Temp;
for(int i=0;i<pointNum;i++)
{
for(int j=0;j<6;j++)
{
PointAngle[i][j] = Point[i6+j];
}
forward(DH,PointAngle[i],Tx[i]);
}
for(int i=0;i<pointNum-1;i++)
{
for(int m=0;m<3;m++)
{
for(int n=0;n<3;n++)
{
RotationMatrix1(m,n) = Tx[i][m4+n];
RotationMatrix2(m,n) = Tx[i+1][m4+n];
}
PositionVector1[m] = Tx[i][m4+3];
PositionVector2[m] = Tx[i+1][m4+3];
B[i3+m] = PositionVector2[m] - PositionVector1[m];
}
RotationMatrix = RotationMatrix1 - RotationMatrix2;
CoefficientMatrix.block(i3,0,3,3) = RotationMatrix;
PositionVector = PositionVector2 - PositionVector1;
// B.block(i3,(i+1)*3-1) = PositionVector;
}
Temp = CoefficientMatrix.transpose() * CoefficientMatrix;
GeneralizedInverseOfCoefficientMatrix = Temp.inverse() * CoefficientMatrix.transpose();
TcpVector = GeneralizedInverseOfCoefficientMatrix * B;
// TcpVector = Temp.inverse() * CoefficientMatrix.transpose() * B;

T_tool[3]   =   static_cast<float>(TcpVector[0]);
T_tool[7]   =   static_cast<float>(TcpVector[1]);
T_tool[11]  =   static_cast<float>(TcpVector[2]);
Eigen::VectorXd CalibrationError(Dimension);
Eigen::VectorXd CalibrationErrorSum(pointNum-1);
CalibrationError   =   CoefficientMatrix   *   TcpVector   -   B;
double  MaxError    =   0;
double  MinError    =   100;
double  AveError    =   0;
double  SumError    =   0;
for(int i=0;i<pointNum-1;i++)
{
    CalibrationErrorSum[i]  =   sqrt(pow(CalibrationError[i*3],2)+pow(CalibrationError[i*3+1],2)+pow(CalibrationError[i*3+2],2));
    if(CalibrationErrorSum[i]>=MaxError)
        MaxError    =   CalibrationErrorSum[i];
    if(CalibrationErrorSum[i]<=MinError)
        MinError    =   CalibrationErrorSum[i];
    SumError        +=  CalibrationErrorSum[i];
}
AveError    =   SumError/pointNum;
Error[0]    =   MaxError;
Error[1]    =   MinError;
Error[2]    =   AveError;

#endif

// for(int i=0;i<16;i++)
// {
// if(i%5==0)
// T_tool[i] = 1;
// else
// T_tool[i] = 0;
// }
// double Tx[4][16] = {0};
// double temp[6] = {0};
// for (int i=0;i<4;i++)
// {
// for(int j=0;j<6;j++)
// {
// temp[j] = cal_pos[i][j];
// }
// forward(DH,temp,Tx[i]);
// }
// int CaliNum = 4;
// float Ptcp[3] = {0};
// float Tx_float[4][16] = {0};
// for(int i = 0;i<4;i++)
// {
// for(int j = 0;j<16;j++)
// {
// Tx_float[i][j] = static_cast(Tx[i][j]);
// }
// }
// TCP(Tx_float[0],Tx_float[1],Tx_float[2],Tx_float[3],CaliNum,Ptcp);
// T_tool[3] = Ptcp[0];
// T_tool[7] = Ptcp[1];
// T_tool[11] = Ptcp[2];
// qDebug()<<“NEWW”<<Ptcp[0]<<Ptcp[1]<<Ptcp[2];
}