机械手基坐标系和工具坐标系的相互转换

机械手基坐标系和工具坐标系的相互转换，主要是通过欧拉角来完成的。

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

机械手基坐标系和工具坐标系的相互转换相关推荐

机械手使用者坐标系和工具坐标系_EPSON机械手工具坐标系的标定
概念 ------------------------------------------ 工具坐标系是把机器人腕部法兰盘所握工具的有效方向定为Z轴,把坐标定义在工具尖端点,所以工具坐标的方向随腕部的 ...
机械手使用者坐标系和工具坐标系_【安川】安川机器人XRC控制柜慨述、操作及编程详解（上）...
外部轴:本体俯焊好,不能仰焊,要增加手臂自由度,成本太高,带外部轴可增加功能. XRC控制柜慨述主电源开关和门锁位于XRC控制柜的面板上,示教盒挂在控制柜的右上方,再现面板位于控制柜的柜门上,如图所 ...
FANUC机器人6点示教法设定工具坐标系的具体步骤
FANUC机器人6点示教法设定工具坐标系的具体步骤前些天发现了一个巨牛的人工智能学习网站,通俗易懂,风趣幽默,忍不住分享一下给大家.点击跳转到网站. 设定坐标系的目的:为了确定机器人的位置和姿势而在 ...
工业机器人三点工具定位法图文_工业机器人工具坐标系的设置
何淼摘要:该文以FANUC工业机器人为例,从工具坐标系设置的意义出发,分析了不同机器人设置工具坐标系的方法,成功建立了机器人新的工具坐标系,为机器人精确的运动控制奠定基础. 关键词:工业机器人;坐标 ...
工业机器人工具坐标系与用户坐标系标定方法
一.工具坐标系 1.定义:即安装在机器人末端的工具坐标系,原点及方向都是随着末端位置与角度不断变化的,该座标系实际是将基础座标系通过旋转及位移变化而来的.工具中心点(TCP)的位置和工具姿态的坐标系. ...
halocn标定找旋转中心_机器人工具坐标系标定原理
工业机器人使用过程中经常在机器人末端法兰面安装不同的工具来满足实际生产需求,为了准确控制工具运动的位置与姿态,需要对工具所在坐标系进行标定. 对于工业机器人来说,基坐标与末端法兰面所在坐标系之间 ...
坐标轨迹计算_工业机器人工具坐标系和用户坐标系的区别与联系
记得关注我哦~ 有问有答碰到过fanuc机器人程序调用用户坐标和工具坐标两个坐标系,请问到底以谁为主? 问题分析最近有网友在我们的学习课程交流QQ群中提出了上述疑问,这也是很多同学在初学时会遇到的 ...
库卡机器人坐标手势_在工具坐标系中移动库卡机器人
 在工具坐标系中手动移动时,可根据之前所测工具的坐标方向移动机器人. 因此,坐标系并非固定不变(例如:世界坐标系或基坐标系),而是由机器人引导.在此过程中,所有需要的机器人轴也会自行移动.哪些轴会自 ...
机器人学（五）：机器人工具坐标系标定
一.问题 1.1 工具坐标系的位置标定已知机械臂末端坐标系End相对于机械臂基坐标系Base的位姿关系可以实时测量得到,求机械臂末端连接的工具TCP相对于End坐标系的平移变换. 1.2 工具坐标系 ...

机械手基坐标系和工具坐标系的相互转换

机械手基坐标系和工具坐标系的相互转换相关推荐

最新文章

热门文章