機械手基坐標系和工具坐標系的相互轉換，主要是通過歐拉角來完成的。

double D2R(double D) {return D / 180 * M_PI; }double R2D(double R) {return R / M_PI * 180; }void Rx(cv::Mat& mat, double dAngel) {mat = cv::Mat(3, 3, CV_64FC1);double dAngle_ = D2R(dAngel);mat.at<double>(0, 0) = 1;mat.at<double>(0, 1) = 0;mat.at<double>(0, 2) = 0;mat.at<double>(1, 0) = 0;mat.at<double>(1, 1) = std::cos(dAngle_);mat.at<double>(1, 2) = std::sin(dAngle_)*(-1);mat.at<double>(2, 0) = 0;mat.at<double>(2, 1) = std::sin(dAngle_);mat.at<double>(2, 2) = std::cos(dAngle_); }void Ry(cv::Mat& mat, double dAngel) {mat = cv::Mat(3, 3, CV_64FC1);double dAngle_ = D2R(dAngel);mat.at<double>(0, 0) = std::cos(dAngle_);mat.at<double>(0, 1) = 0;mat.at<double>(0, 2) = std::sin(dAngle_);mat.at<double>(1, 0) = 0;mat.at<double>(1, 1) = 1;mat.at<double>(1, 2) = 0;mat.at<double>(2, 0) = std::sin(dAngle_)*(-1);mat.at<double>(2, 1) = 0;mat.at<double>(2, 2) = std::cos(dAngle_); }void Rz(cv::Mat& mat, double dAngel) {mat = cv::Mat(3, 3, CV_64FC1);double dAngle_ = D2R(dAngel);mat.at<double>(0, 0) = std::cos(dAngle_);mat.at<double>(0, 1) = std::sin(dAngle_)*(-1);mat.at<double>(0, 2) = 0;mat.at<double>(1, 0) = std::sin(dAngle_);mat.at<double>(1, 1) = std::cos(dAngle_);mat.at<double>(1, 2) = 0;mat.at<double>(2, 0) = 0;mat.at<double>(2, 1) = 0;mat.at<double>(2, 2) = 1; }/* 說明：從基坐標系到工具坐標系 依據歐拉角換算旋轉矩陣 1. EulerAngle工具坐標系相對于基坐標系的歐拉角 2. matR 基坐標系到工具坐標系的旋轉矩陣 */ void EulerAngleToRotateMat_B2T(RobotEulerAngle EulerAngle, cv::Mat& matR) {cv::Mat rx, ry, rz;Rx(rx, EulerAngle.rx);Ry(ry, EulerAngle.ry);Rz(rz, EulerAngle.rz);matR = rz*ry*rx; }/* 說明：從基坐標系到工具坐標系 依據旋轉矩陣換算歐拉角 1. matR 基坐標系到工具坐標系的旋轉矩陣 2. EulerAngle工具坐標系相對于基坐標系的歐拉角 */ void RotateMatToEulerAngle_B2T(cv::Mat matR, RobotEulerAngle& EulerAngle) {EulerAngle.ry =std::atan2((-1)*matR.at<double>(2, 0),std::sqrt(std::pow(matR.at<double>(2, 1), 2) +std::pow(matR.at<double>(2, 2), 2)));EulerAngle.rx =std::atan2(matR.at<double>(2, 1) / std::cos(EulerAngle.ry),matR.at<double>(2, 2) / std::cos(EulerAngle.ry));EulerAngle.rz =std::atan2(matR.at<double>(1, 0) / std::cos(EulerAngle.ry),matR.at<double>(0, 0) / std::cos(EulerAngle.ry));EulerAngle.rx = R2D(EulerAngle.rx);EulerAngle.ry = R2D(EulerAngle.ry);EulerAngle.rz = R2D(EulerAngle.rz); }/* 說明：基坐標系上的點（向量）換算到工具坐標系 1. 基坐標系上的點： robotData.posInBase 2. 工具坐標系的姿態： robotData.EulerAngleTakePhoto 3. 換算的結果： robotData.posInTool */ void BasePointToTool(RobotStruct & robotData) {cv::Mat matR, matRes;EulerAngleToRotateMat_B2T(robotData.EulerAngleTakePhoto, matR);matRes = (cv::Mat_<double>(1, 3) << robotData.posInBase.x, robotData.posInBase.y, robotData.posInBase.z) * matR;robotData.posInTool.x = matRes.at<double>(0, 0);robotData.posInTool.y = matRes.at<double>(0, 1);robotData.posInTool.z = matRes.at<double>(0, 2); }/* 說明：從工具坐標系到基坐標系 依據歐拉角換算旋轉矩陣 1. EulerAngle工具坐標系相對于基坐標系的歐拉角 2. matR 基坐標系到工具坐標系的旋轉矩陣 */ void EulerAngleToRotateMat_T2B(RobotEulerAngle EulerAngle, cv::Mat& matR) {cv::Mat rx, ry, rz;Rx(rx, EulerAngle.rx*(-1));Ry(ry, EulerAngle.ry*(-1));Rz(rz, EulerAngle.rz*(-1));matR = rx*ry*rz; }/* 說明：從工具坐標系到基坐標系 依據旋轉矩陣換算歐拉角 1. matR 基坐標系到工具坐標系的旋轉矩陣 2. EulerAngle工具坐標系相對于基坐標系的歐拉角 */ void RotateMatToEulerAngle_T2B(cv::Mat matR, RobotEulerAngle& EulerAngle) {cv::Mat matR_T;cv::transpose(matR, matR_T);RotateMatToEulerAngle_B2T(matR_T, EulerAngle); }/* 說明：工具坐標系上的點（向量）換算到基坐標系 1. 工具坐標系上的點： robotData.posInTool 2. 工具坐標系的姿態： robotData.EulerAngleTakePhoto 3. 換算的結果： robotData.posInBase */ void ToolPointToBase(RobotStruct & robotData) {cv::Mat matR, matRes;EulerAngleToRotateMat_T2B(robotData.EulerAngleTakePhoto, matR);matRes = (cv::Mat_<double>(1, 3) << robotData.posInTool.x, robotData.posInTool.y, robotData.posInTool.z) * matR;robotData.posInBase.x = matRes.at<double>(0, 0);robotData.posInBase.y = matRes.at<double>(0, 1);robotData.posInBase.z = matRes.at<double>(0, 2); }/* 說明：相對基坐標系，先有兩個工具坐標的歐拉姿態 EulerAngleA EulerAngleB需要得到第三個歐拉姿態 EulerAngleC，可以直接從 EulerAngleA 轉換到 EulerAngleB EulerAngleA 歐拉姿態，相對基坐標系 EulerAngleB 歐拉姿態，相對基坐標系 EulerAngleC 歐拉姿態，相對 EulerAngleA */ void EulerAToEulerB(RobotEulerAngle EulerAngleA, RobotEulerAngle EulerAngleB, RobotEulerAngle & EulerAngleC) {cv::Mat matRA, matRAinv, matRB, matRC;EulerAngleToRotateMat_B2T(EulerAngleA, matRA);EulerAngleToRotateMat_B2T(EulerAngleB, matRB);cv::invert(matRA, matRAinv);matRC = matRAinv * matRB;RotateMatToEulerAngle_B2T(matRC, EulerAngleC); }/* 說明：工具坐標系相對偏移運動，根據此時的歐拉姿態，將工具端的相對運動量轉換到基坐標系 1. robotCurPos.posInBase 此時基坐標系的點坐標 2. robotCurPos.EulerAngleTakePhoto 此時工具坐標系的歐拉姿態（相對基坐標系） 3. posRelative 工具坐標系的相對偏移量 4. robotCurPos.posInBase 最后結果更新到這個變量中 */ void RobotToolRelativeMove(RobotStruct& robotCurPos, RobotPos posRelative) {// 交換數據，借用變量 RobotPos posInBase, posInTool;robotCurPos.posInBase.CopyTo(posInBase);robotCurPos.posInTool.CopyTo(posInTool);posRelative.CopyTo(robotCurPos.posInTool);// 投影向量ToolPointToBase(robotCurPos);// 還原數據robotCurPos.posInBase.x += posInBase.x;robotCurPos.posInBase.y += posInBase.y;robotCurPos.posInBase.z += posInBase.z;posInTool.CopyTo(robotCurPos.posInTool); }/* 說明：工具坐標系相對旋轉運動，根據此時的歐拉姿態，工具坐標系繼續做一個三軸旋轉并將 robotCurPos.EulerAngleTakePhoto 和 EulerAngle 合成一個新的歐拉角 robotCurPos.EulerAngleTakePhoto 工具末端初始歐拉姿態 EulerAngle 相對旋轉量 robotCurPos.EulerAngleTakePhoto 最后結果更新到這個變量中 */ void RobotToolRelativeRotate(RobotStruct& robotCurPos, RobotEulerAngle EulerAngle) {// 計算旋轉矩陣cv::Mat matR1, matR2, matR;EulerAngleToRotateMat_B2T(robotCurPos.EulerAngleTakePhoto, matR1);EulerAngleToRotateMat_B2T(EulerAngle, matR2);matR = matR1*matR2;// 反算此時歐拉角RotateMatToEulerAngle_B2T(matR, robotCurPos.EulerAngleTakePhoto); }/* 說明：相對法蘭中心的空間某點，做一個相對的平移和旋轉。 1. RelativePos 相對法蘭中心的空間某點 A 2. RelativeMove 相對 A 點做偏移運動，結果輸出在robotCurPos.EulerAngleTakePhoto 3. RelativeRotate 相對 A 點做旋轉運動，結果輸出在robotCurPos.posInBase 4. */ void RobotToolRelativeMoveRotate(RobotPos RelativePos, RobotPos RelativeMove, RobotEulerAngle RelativeRotate, RobotStruct& robotCurPos) {// 保持當前姿態下，計算工具端相對偏移量，相當于法蘭中心移到了工具末端位置RobotToolRelativeMove(robotCurPos, { RelativePos.x + RelativeMove.x, RelativePos.y + RelativeMove.y, RelativePos.z + RelativeMove.z });// 調整姿態RobotToolRelativeRotate(robotCurPos, RelativeRotate);// 反向調整工具端位置偏移RobotToolRelativeMove(robotCurPos, { RelativePos.x *(-1), RelativePos.y *(-1), RelativePos.z *(-1) }); }

Matlab中歐拉角和四元素的互換

R_All = Rx(rx)' * Ry(ry)' * Rz(rz)'; [Qur_W, Qur_X, Qur_Y, Qur_Z] = GetQurFromRotateMat(R_All); Qur_Norm = norm([Qur_X, Qur_Y, Qur_Z, Qur_W]);% =1 Qur_R = ...[1 - 2*Qur_Y^2 - 2*Qur_Z^2, 2*Qur_X*Qur_Y - 2*Qur_Z*Qur_W, 2*Qur_X*Qur_Z + 2*Qur_Y*Qur_W;2*Qur_X*Qur_Y + 2*Qur_Z*Qur_W, 1 - 2*Qur_X^2 - 2*Qur_Z^2, 2*Qur_Y*Qur_Z - 2*Qur_X*Qur_W;2*Qur_X*Qur_Z - 2*Qur_Y*Qur_W, 2*Qur_Y*Qur_Z + 2*Qur_X*Qur_W, 1 - 2*Qur_X^2 - 2*Qur_Y^2]; % R_All=Qur_R 就對了，本質上都是旋轉矩陣function [Qur_W, Qur_X, Qur_Y, Qur_Z] = GetQurFromRotateMat(Mat)Qur_W = sqrt(1+Mat(1,1)+Mat(2,2)+Mat(3,3))/2;Qur_X = (Mat(3,2)-Mat(2,3))/(4*Qur_W);Qur_Y = (Mat(1,3)-Mat(3,1))/(4*Qur_W);Qur_Z = (Mat(2,1)-Mat(1,2))/(4*Qur_W); end

總結

以上是生活随笔為你收集整理的机械手基坐标系和工具坐标系的相互转换的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。