（三）手眼标定结果的应用

boss-dog

已于 2023-04-16 23:06:20 修改

阅读量7.8k

点赞数 19

分类专栏：手眼标定文章标签：手眼标定 c++

于 2022-04-06 13:17:34 首次发布

本文链接：https://blog.csdn.net/qq_45445740/article/details/123567627

版权

手眼标定专栏收录该内容

9 篇文章 28 订阅

订阅专栏

记录自己手眼标定（眼在手上）结果的应用过程，本人使用的是2D相机。

内容

1.核心公式
2.手眼标定的运算法则和对应的物理意义
3.场景需求

1.核心公式

1.1 命名规定

表示以base(机器臂基底)为基准，end(机械臂末端)在其下的位姿描述，即机械臂末端坐标系到机械臂基底坐标系的变换矩阵。
（本文所有公式中的变量都以这个为标准，有的资料中写的和这个相反，容易记混了，注意哪个是基准就行。）

1.2 欧拉角、四元数、旋转矩阵的关系

关于这几个的关系转换如下图所示，在之后的计算中，需要频繁进行转换。
在这里插入图片描述

1.3 公式

在这里插入图片描述
上面是三个4×4的矩阵相乘，得到的结果就是在机械臂基底坐标系base下，识别到的目标target的位姿。计算结果仍然是个4×4矩阵，包含一个旋转矩阵+一个平移向量，将旋转矩阵转为欧拉角，再通过运动学逆解计算将欧拉角转为6个关节角度值，将这6个值输入到示教器中，即可将机械臂末端运动到当前目标在基底下的位姿状态。
（PS：如果直接输入X、Y、Z、RX、RY、RZ可能会得到好几种不同的关节角度值，虽然它们的位置和欧拉角都相同，万一解出一个值机器人会撞到工作平面，所以需要我们通过运动学逆解直接输入关节角值，来避免这种情况。）

上式中各个参数的含义和计算方法：

1.T：变换矩阵，由旋转矩阵（3×3）和平移向量（3×1）组成，在旋转矩阵下面加一行[0, 0, 0, 1]构成齐次矩阵；格式如下：
2.
这个值由示教器面板直接读取或通过示教器的SDK获取，但获得的位姿形式是（X、Y、Z、RX、RY、RZ），需要将其转换为旋转矩阵R和平移向量t，然后再拼接为齐次矩阵（4×4），将欧拉角（RX、RY、RZ）转为旋转矩阵，这里要注意欧拉角的排列顺序（机械臂的位姿类型有zyx,yzx,zxy,yxz,xyz,xzy几种，需要区分），之后再和平移向量（XYZ）进行拼接。

举例：获取到当前机器人的位姿（m，弧度）和6个关节角（弧度）
（X、Y、Z、RX、RY、RZ）-> （0.243749, 0.508627, 0.369233, -3.136813, 0.000506, 2.746928）
关节角：（J1、J2、J3、J4、J5、J6）->（1.341016、-0.193995、1.538816、0.152183、1.571031 、0.166330）


// 欧拉角转换为旋转矩阵
cv::Mat eulerAngleToRotateMatrix(const cv::Mat& eulerAngle, const std::string& seq)
{
	cv::Matx13d m(eulerAngle);		//<double, 1, 3>

	auto rx = m(0, 0), ry = m(0, 1), rz = m(0, 2);
	auto rxs = sin(rx), rxc = cos(rx);
	auto rys = sin(ry), ryc = cos(ry);
	auto rzs = sin(rz), rzc = cos(rz);

	//XYZ方向的旋转矩阵
	cv::Mat RotX = (cv::Mat_<double>(3, 3) << 1, 0, 0,
		0, rxc, -rxs,
		0, rxs, rxc);
	cv::Mat RotY = (cv::Mat_<double>(3, 3) << ryc, 0, rys,
		0, 1, 0,
		-rys, 0, ryc);
	cv::Mat RotZ = (cv::Mat_<double>(3, 3) << rzc, -rzs, 0,
		rzs, rzc, 0,
		0, 0, 1);
	//按顺序合成后的旋转矩阵
	cv::Mat rotMat;

	if (seq == "zyx") rotMat = RotX * RotY * RotZ;
	else if (seq == "yzx") rotMat = RotX * RotZ * RotY;
	else if (seq == "zxy") rotMat = RotY * RotX * RotZ;
	else if (seq == "yxz") rotMat = RotZ * RotX * RotY;
	else if (seq == "xyz") rotMat = RotZ * RotY * RotX;
	else if (seq == "xzy") rotMat = RotY * RotZ * RotX;
	else
	{
		cout<< "Euler Angle Sequence string is wrong..." << endl;
	}

	return rotMat;
}

cv::Mat_<double> CalPose = (cv::Mat_<double>(1, 6) << 0.243749, 0.508627, 0.369233, -3.136813, 0.000506, 2.746928);
cv::Mat T_end2base;
T_end2base = attitudeVectorToMatrix(CalPose.row(0), false, "xyz"); 

上面的位姿转换为：
T_end2base：
[-0.9231254383299377, 0.3844964842228446, -0.001370667148604696, 0.243749;
 0.3844985943695793, 0.923114082176005, -0.004606757663443459, 0.5086270000000001;
 -0.000505999978407631, -0.004779634779333107, -0.9999884494609915, 0.369233;
 0, 0, 0, 1]

3.
这是手眼标定的结果。

关于手眼标定，参考：https://blog.csdn.net/qq_45445740/article/details/122170029

举例：T_cam2end：
[-0.99798, 0.0607526, -0.0185534, -0.017052;
 -0.0603958, -0.997989, -0.0192232, 0.132618;
 -0.019684, -0.0180638, 0.9996429, -0.026936;
 0, 0, 0, 1]

4.
根据自己选择的目标放在相机视野内进行拍照识别，我选择的是对aruco标记图进行识别，会得到目标（标记图）在相机坐标系下的位姿（X、Y、Z、w、x、y、z），需要将后面的四元数转为旋转矩阵，再和前面的（X、Y、Z）组成齐次矩阵。

四元数->旋转矩阵，参考：https://zhuanlan.zhihu.com/p/45404840

举例：
aruco的识别结果：
Find Board！
（X、Y、Z、w、x、y、z）->（-0.00257137, -0.0155088, 0.779451, 0.0245001, -0.702148, 0.711493, -0.0128749）

// 将四元数转换为旋转矩阵
// 归一化的四元数: q = q0 + q1 * i + q2 * j + q3 * k;
//                          q0=w, q1=x, q2=y, q3=z
cv::Mat quaternionToRotatedMatrix(const cv::Vec4d& q)
{
	double q0 = q[0], q1 = q[1], q2 = q[2], q3 = q[3];

	double q0q0 = q0 * q0, q1q1 = q1 * q1, q2q2 = q2 * q2, q3q3 = q3 * q3;
	double q0q1 = q0 * q1, q0q2 = q0 * q2, q0q3 = q0 * q3;
	double q1q2 = q1 * q2, q1q3 = q1 * q3;
	double q2q3 = q2 * q3;
	//根据公式得来
	cv::Mat RotMtr = (cv::Mat_<double>(3, 3) << (1 - 2 * (q2q2 + q3q3)), 2 * (q1q2 - q0q3), 2 * (q1q3 + q0q2),
		2 * (q1q2 + q0q3), 1 - 2 * (q1q1 + q3q3), 2 * (q2q3 - q0q1),
		2 * (q1q3 - q0q2), 2 * (q2q3 + q0q1), (1 - 2 * (q1q1 + q2q2)));

	return RotMtr;
}

四元数->旋转矩阵结果：
[-0.01277610419802011, -0.99851590125302, 0.052943469869;
 -0.99977764660298, 0.01364484609198002, 0.0160845899782;
 -0.0167831287282, -0.052726194881, -0.9984682059060002]

T_target2cam：
[-0.01277610419802011, -0.99851590125302, 0.052943469869, -0.002571;
 -0.99977764660298, 0.01364484609198002, 0.0160845899782, -0.015509;
 -0.0167831287282, -0.052726194881, -0.9984682059060002, 0.779451;
 0, 0, 0, 1]

2.手眼标定的运算法则和对应的物理意义

参考：https://zhuanlan.zhihu.com/p/388952714
在这里插入图片描述

3.场景需求

前提都是水平面上的目标移动，非水平面的还没做。。。若有大佬做过的迫切希望得到指导。

3.1 目标(aruco标记图)在水平面上不动，指定机械臂末端运动到目标上方的固定距离

PS：这里我没有直接将机械臂末端直接移动到目标点这个位置，因为我没有在机械臂末端加装机械手或者相关示教工具，所以想让机械臂末端运动到一个相对的位置。

思路：通过上面的手眼标定结果应用的公式，计算得出aruco标记图在机械臂基底坐标系下的位姿T_target2base，假设我现在想让机械臂末端运动到aruco标记图的正上方500mm处，只需要将T_target2base中的平移向量（X、Y、Z）中的Z轴方向加上500mm得到新的（X、Y、Z’）。

由于识别到的结果是aruco标志图相对于机械臂基底的位置和姿态，而我现在输入到机械臂中的是将机械臂末端移动到aruco标志图上500mm的指定位置，所以需要的是机械臂末端相对于基底的坐标，于是直接在示教器输入上面中的指定点相对于基底的位置（X、Y、Z’），姿势（欧拉角RX、RY、RZ）还使用原来机械臂末端到机械臂基底的姿势就行。

输入（X、Y、Z’、RX、RY、RZ），获取当前机械臂相对于基底的6个关节角值（J1、J2、J3、J4、J5、J6，通过SDK中的 impl->getRobotState()->getJointPositions()接口获得），再经过运动学逆解，得出移动到指定点所需要的6个关节角值，最后输入到机械臂中，即可将机械臂末端运动到目标上方的固定距离500mm处。（此时只是将机械臂末端移动到了目标上方的指定高度位置，位姿和目标却不同步。）

3.2 上小节中是相机识别到了目标，将机械臂移动到指定目标上方的固定高度位置。当前场景是不断更换目标位姿，但仍要求机械臂移动到指定目标上方的固定距离位置，且位置和姿态都要和目标同步

3.2.1 公式

在这里插入图片描述
参数含义：

target：表示相机识别到的目标，这里我用的是aruco标记图。
wp：工作点(waypoint)，可以是指定目标上方固定距离的一个点，也可以是机械臂末端加装夹爪或者工具的位置点（上小节中我这里的工作点用的是法兰中心即机械臂末端），这个点根据实际需要自行定义。
T_target2base：目标坐标系到机械臂基底坐标系的变换矩阵，这是手眼标定的应用结果，即三个4×4矩阵相乘的结果。
T_wp2target：工作点坐标系到目标坐标系的变换矩阵，这是你指定要运动到以目标为基准的固定点。
T_wp2base：工作点坐标系到机械臂基底坐标系的变换矩阵，这是输入到机械臂中的数值。

3.2.2 计算过程

在这里插入图片描述

3.2.3 执行步骤

①相机固定在机械臂末端，将目标摆正置于相机的视野内，设置好固定高度值H。
②按照3.1的方法，先将机械臂末端移动到目标上方H高度的位置。
③再通过3.2中的方法，算出T_wp2target，该值是不变的，表示无论目标的位置和姿态如何改变，始终是移动到目标上方高度H的位置，这个点是跟随目标变化而变化的。
④将机械臂恢复到初始位姿，再次改变目标的位姿，然后对目标进行识别，得到T_target2cam_new，根据1.3中的公式重新计算得到T_target2base_new = T_end2base * T_cam2end * T_target2cam_new，再根据3.2.1中的公式得出T_wp2base_new = T_target2base_new * T_wp*target，将这结果转化为六个关节角值输入到示教器中，此时机械臂末端会再次移动到新摆放的目标上方高度H的位置，且姿势也和新摆放的目标一致。