2023.5更新
最近幫別人做了個手眼標(biāo)定，然后我標(biāo)定完了大概精度能到1mm左右。所以原文中誤差10mm可能是當(dāng)時那個臂本身的坐標(biāo)系有問題。然后用的代碼改成了基于python的，放在下面。
新來的小伙伴可以只參考前面的代碼就可以完成標(biāo)定了。
有問題的話可以留言，一起交流~
手眼標(biāo)定需要的內(nèi)容：
1.不同姿態(tài)下拍攝棋盤格的照片若干張（我用了10張）
2.記錄下機(jī)械臂的姿態(tài)格式為（x,y,z,rx,ry,rz）其中姿態(tài)為rpy角，單位為deg， 需要保存成excel文件
代碼中需要修改的內(nèi)容：
1.相機(jī)內(nèi)參矩陣self.K 以及畸變參數(shù)
2.棋盤格尺寸
3.存放img和excel的路徑
import os
import cv2
import xlrd2
from math import *
import numpy as np
# p[321.501 242.543] f[606.25 605.65]
class Calibration:
def __init__(self):
self.K = np.array([[606.25, 0.00000000e+00, 321.501],
[0.00000000e+00, 605.65, 242.543],
[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]], dtype=np.float64)
self.distortion = np.array([[0, 0, 0.0, 0.0, 0]])
self.target_x_number = 11
self.target_y_number = 8
self.target_cell_size = 20
def angle2rotation(self, x, y, z):
Rx = np.array([[1, 0, 0], [0, cos(x), -sin(x)], [0, sin(x), cos(x)]])
Ry = np.array([[cos(y), 0, sin(y)], [0, 1, 0], [-sin(y), 0, cos(y)]])
Rz = np.array([[cos(z), -sin(z), 0], [sin(z), cos(z), 0], [0, 0, 1]])
R = Rz @ Ry @ Rx
return R
def gripper2base(self, x, y, z, tx, ty, tz):
thetaX = x / 180 * pi
thetaY = y / 180 * pi
thetaZ = z / 180 * pi
R_gripper2base = self.angle2rotation(thetaX, thetaY, thetaZ)
T_gripper2base = np.array([[tx], [ty], [tz]])
Matrix_gripper2base = np.column_stack([R_gripper2base, T_gripper2base])
Matrix_gripper2base = np.row_stack((Matrix_gripper2base, np.array([0, 0, 0, 1])))
R_gripper2base = Matrix_gripper2base[:3, :3]
T_gripper2base = Matrix_gripper2base[:3, 3].reshape((3, 1))
return R_gripper2base, T_gripper2base
def target2camera(self, img):
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (self.target_x_number, self.target_y_number), None)
corner_points = np.zeros((2, corners.shape[0]), dtype=np.float64)
for i in range(corners.shape[0]):
corner_points[:, i] = corners[i, 0, :]
object_points = np.zeros((3, self.target_x_number * self.target_y_number), dtype=np.float64)
count = 0
for i in range(self.target_y_number):
for j in range(self.target_x_number):
object_points[:2, count] = np.array(
[(self.target_x_number - j - 1) * self.target_cell_size,
(self.target_y_number - i - 1) * self.target_cell_size])
count += 1
retval, rvec, tvec = cv2.solvePnP(object_points.T, corner_points.T, self.K, distCoeffs=self.distortion)
Matrix_target2camera = np.column_stack(((cv2.Rodrigues(rvec))[0], tvec))
Matrix_target2camera = np.row_stack((Matrix_target2camera, np.array([0, 0, 0, 1])))
R_target2camera = Matrix_target2camera[:3, :3]
T_target2camera = Matrix_target2camera[:3, 3].reshape((3, 1))
return R_target2camera, T_target2camera
def process(self, img_path, pose_path):
image_list = []
for root, dirs, files in os.walk(img_path):
if files:
for file in files:
image_name = os.path.join(root, file)
image_list.append(image_name)
R_target2camera_list = []
T_target2camera_list = []
for img_path in image_list:
img = cv2.imread(img_path)
R_target2camera, T_target2camera = self.target2camera(img)
R_target2camera_list.append(R_target2camera)
T_target2camera_list.append(T_target2camera)
R_gripper2base_list = []
T_gripper2base_list = []
data = xlrd2.open_workbook(pose_path)
table = data.sheets()[0]
for row in range(table.nrows):
x = table.cell_value(row, 3)
y = table.cell_value(row, 4)
z = table.cell_value(row, 5)
tx = table.cell_value(row, 0)
ty = table.cell_value(row, 1)
tz = table.cell_value(row, 2)
R_gripper2base, T_gripper2base = self.gripper2base(x, y, z, tx, ty, tz)
R_gripper2base_list.append(R_gripper2base)
T_gripper2base_list.append(T_gripper2base)
R_camera2base, T_camera2base = cv2.calibrateHandEye(R_gripper2base_list, T_gripper2base_list,
R_target2camera_list, T_target2camera_list)
return R_camera2base, T_camera2base, R_gripper2base_list, T_gripper2base_list, R_target2camera_list, T_target2camera_list
def check_result(self, R_cb, T_cb, R_gb, T_gb, R_tc, T_tc):
for i in range(len(R_gb)):
RT_gripper2base = np.column_stack((R_gb[i], T_gb[i]))
RT_gripper2base = np.row_stack((RT_gripper2base, np.array([0, 0, 0, 1])))
# print(RT_gripper2base)
RT_camera_to_gripper = np.column_stack((R_cb, T_cb))
RT_camera_to_gripper = np.row_stack((RT_camera_to_gripper, np.array([0, 0, 0, 1])))
print(RT_camera_to_gripper)#這個就是手眼矩陣
RT_target_to_camera = np.column_stack((R_tc[i], T_tc[i]))
RT_target_to_camera = np.row_stack((RT_target_to_camera, np.array([0, 0, 0, 1])))
# print(RT_target_to_camera)
RT_target_to_base = RT_gripper2base @ RT_camera_to_gripper @ RT_target_to_camera
print("第{}次驗(yàn)證結(jié)果為:".format(i))
print(RT_target_to_base)
print('')
if __name__ == "__main__":
image_path = r"I:\data_handeye\img"
pose_path = r"I:\pose.xlsx"
calibrator = Calibration()
R_cb, T_cb, R_gb, T_gb, R_tc, T_tc = calibrator.process(image_path, pose_path)
calibrator.check_result(R_cb, T_cb, R_gb, T_gb, R_tc, T_tc)
結(jié)果展示：
下面三個矩陣代表的是在不同姿態(tài)下解算得到標(biāo)定板在基坐標(biāo)系下的姿態(tài)，最后一列是位置，單位為mm，可以看出精度差不多在1mm左右。
第1次驗(yàn)證結(jié)果為:
[[ 3.98925017e-03 9.99742174e-01 -2.23533483e-02 -3.90700113e+02]
[ 9.99982733e-01 -4.08467234e-03 -4.22477708e-03 -1.11656116e+02]
[-4.31499393e-03 -2.23361086e-02 -9.99741206e-01 -5.01618116e+00]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
第2次驗(yàn)證結(jié)果為:
[[ 2.05751209e-03 9.99866123e-01 -1.62327447e-02 -3.90254641e+02]
[ 9.99993197e-01 -2.10692535e-03 -3.02753423e-03 -1.13056000e+02]
[-3.06133010e-03 -1.62264051e-02 -9.99863657e-01 -6.26002256e+00]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
第3次驗(yàn)證結(jié)果為:
[[ 4.87875821e-03 9.99819129e-01 -1.83822511e-02 -3.91281959e+02]
[ 9.99986520e-01 -4.91059054e-03 -1.68694875e-03 -1.11955299e+02]
[-1.77691133e-03 -1.83737731e-02 -9.99829609e-01 -6.29677977e+00]
[ 0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]
------------------下面為原文---------------------
一、前期準(zhǔn)備
1.1 手眼標(biāo)定原理
手眼標(biāo)定的原理網(wǎng)上有很多，推薦幾篇博客：
手眼標(biāo)定_全面細(xì)致的推導(dǎo)過程
機(jī)器人手眼標(biāo)定
深入淺出地理解機(jī)器人手眼標(biāo)定
簡單來說需要求解一個AX=XB的矩陣方程，具體求解方法可以看
手眼標(biāo)定AX=XB求解方法（文獻(xiàn)總結(jié)）
1.2 Aruco返回位姿的原理
ArUco標(biāo)記最初由S.Garrido-Jurado等人在2014年發(fā)表的論文Automatic generation and detection of highly reliable fiducial markers under occlusion中提出。ArUco的全稱是Augmented Reality University of Cordoba。Aruco是一種類似二維碼的定位標(biāo)記輔助工具，通過在環(huán)境中部署Markers，可以輔助機(jī)器人進(jìn)行定位。
OpenCV Aruco 參數(shù)源碼完整解析理解！
ArUco Marker檢測原理
1.3 生成一個Aruco Marker
在線aruco生成網(wǎng)站，注意要選擇original格式，打印出來之后備用。
1.4 安裝aruco_ros包
cd ~/catkin_ws/src
git clone https://github.com/pal-robotics/aruco_ros.git
cd ..
catkin_make
1.5 安裝realsense_ros包
按照GitHub上的步驟安裝即可。
https://github.com/IntelRealSense/realsense-ros
二、實(shí)驗(yàn)環(huán)境
- 機(jī)械臂：rokae xMate3 pro
- 相機(jī)：realsense d435i
- Aruco id：123 size：100mm
- ubuntu 16.04 ROS+ win 10 RobotAssist
- 安裝方式：eye in hand
三、實(shí)驗(yàn)過程
3.1 配置Aruco launch文件
roscd aruco_ros
cd launch
gedit single.launch
修改single.launch文件，需要修改markerId markerSize為你生成的aruco的id和size，以及/camera_info /image指向realsense發(fā)布的話題，camera_frame改為/camera_link。
<launch>
<arg name="markerId" default="123"/>
<arg name="markerSize" default="0.1"/> <!-- in m -->
<arg name="eye" default="left"/>
<arg name="marker_frame" default="aruco_marker_frame"/>
<arg name="ref_frame" default=""/> <!-- leave empty and the pose will be published wrt param parent_name -->
<arg name="corner_refinement" default="LINES" /> <!-- NONE, HARRIS, LINES, SUBPIX -->
<node pkg="aruco_ros" type="single" name="aruco_single">
<remap from="/camera_info" to="/camera/color/camera_info" />
<remap from="/image" to="/camera/color/image_raw" />
<param name="image_is_rectified" value="True"/>
<param name="marker_size" value="$(arg markerSize)"/>
<param name="marker_id" value="$(arg markerId)"/>
<param name="reference_frame" value="$(arg ref_frame)"/>
<param name="camera_frame" value="/camera_link"/>
<param name="marker_frame" value="$(arg marker_frame)" />
<param name="corner_refinement" value="$(arg corner_refinement)" />
</node>
</launch>
3.2 獲取Aruco相對于相機(jī)的位姿
運(yùn)行aruco_ros+realsense節(jié)點(diǎn)
roslaunch realsense2_camera rs_camera.launch
roslaunch aruco_ros single.launch
查看顯示的圖像和返回的位姿
rosrun image_view image_view image:=/aruco_single/result
rostopic echo /aruco_single/pose
3.3 獲取機(jī)械臂末端的位姿：
使用Rokae自帶的上位機(jī)HMI軟件Robot Assist，可以簡單讀取。
其中ABC為“ZYX”型歐拉角，Q1~Q4為（w,x,y,z）四元數(shù)。
3.4 Opencv求解手眼矩陣
代碼參考手眼標(biāo)定（一）：Opencv4實(shí)現(xiàn)手眼標(biāo)定及手眼系統(tǒng)測試 ，加了點(diǎn)修改。
aruco返回的位姿為四元數(shù)形式，因此將attitudeVectorToMatrix函數(shù)的輸入改成了 位置+歐拉角–>旋轉(zhuǎn)矩陣（輸入為nx6），位置+四元數(shù)—>旋轉(zhuǎn)矩陣（輸入為nx7）。
#include <iostream>
#include <string>
#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/calib3d.hpp>
#include <iterator>
#include <algorithm>
using namespace cv;
using namespace std;
template <typename T>
std::ostream& operator<< (std::ostream& out, const std::vector<T>& v) {
if (!v.empty()) {
out << '[';
std::copy(v.begin(), v.end(), std::ostream_iterator<T>(out, ", "));
out << "\b\b]";
}
return out;
}
int num = 16;
//相機(jī)中組標(biāo)定板的位姿，x,y,z,w,x,y,z
Mat_<double> CalPose = (cv::Mat_<double>(num, 7) <<
0.01963879, -0.009811901, 0.205815151, -0.472094888, 0.101775888, -0.345924179, 0.804428087,
0.018775674, -0.008453256, 0.187205523, -0.486293263, 0.154739671, -0.358078652, 0.781891409,
0.012794545, -0.004475131, 0.181938171, 0.50586501, -0.228167491, 0.37478512, -0.742681831,
0.032269519, -0.00216203, 0.173868418, 0.520070883, -0.334713601, 0.409799027, -0.670490746,
0.036921501, -0.00520581, 0.173751414, 0.509143837, -0.389764156, 0.430712138, -0.635093308,
0.028183416, -0.003799309, 0.157465994, -0.493999273, 0.434277098, -0.443106472, 0.609118031,
0.011437201, 0.00248707, 0.160097286, 0.505064681, -0.477579273, 0.433387075, -0.573588135,
0.032602217, 0.021788951, 0.224987134, -0.45692465, 0.54759083, -0.46897897, 0.520982603,
0.012125032, -0.002286719, 0.17284067, -0.469627705, 0.49778925, -0.506302662, 0.524703054,
0.005448693, -0.004084263, 0.196215734, 0.420676917, -0.55355367, 0.57085096, -0.43673613,
-0.001541587, -0.000362108, 0.187365457, 0.386895866, -0.565030889, 0.607491241, -0.402499782,
0.0283868, 0.00129835, 0.21059823, 0.367601287, -0.614671423, 0.616489404, -0.327091961,
0.026122695, -0.006088101, 0.222446054, -0.360286232, 0.636446394, -0.615347513, 0.294070155,
0.016605999, 0.002255499, 0.214847937, -0.322620688, 0.64815307, -0.642728715, 0.250426214,
-0.004833174, -0.00322185, 0.206448808, -0.318973207, 0.693907003, -0.618815043, 0.183894282,
-0.023912154, -0.006630629, 0.213570014, -0.312473203, 0.731833827, -0.595186057, 0.111952241);
//機(jī)械臂末端的位姿，x,y,z,w,x,y,z
Mat_<double> ToolPose = (cv::Mat_<double>(num, 7) <<
0.508533, -0.260803, 0.292825, -0.0002, 0.2843, 0.8712, 0.4003,
0.527713, -0.230407, 0.300856, 0.0012, 0.2461, 0.8979, 0.365,
0.544016, -0.192872, 0.326135, 0.0075, 0.1894, 0.9311, 0.3117,
0.552381, -0.149794, 0.342366, 0.0208, 0.113, 0.9692, 0.2178,
0.564071, -0.118316, 0.356974, 0.0196, 0.0855, 0.9829, 0.1617,
0.574263, -0.073508, 0.354265, 0.0123, 0.0659, 0.991, 0.1162,
0.573574, -0.038738, 0.363898, 0.01, 0.0166, 0.9961, 0.0866,
0.563025, -0.000004, 0.432336, 0, 0, 1, 0,
0.561599, 0.018044, 0.376489, 0.0398, 0.0384, 0.9984, 0.0135,
0.553613, 0.117616, 0.38041, 0.0523, 0.0278, 0.993, -0.102,
0.546303, 0.163889, 0.350932, 0.0606, 0.0419, 0.9854, -0.1537,
0.527343, 0.191386, 0.356761, 0.066, -0.0058, 0.9719, -0.2261,
0.525386, 0.218939, 0.352163, 0.0683, -0.0293, 0.9638, -0.2561,
0.510583, 0.26087, 0.30687, 0.0742, -0.0232, 0.9467, -0.3125,
0.494977, 0.290977, 0.257741, 0.0708, -0.089, 0.9264, -0.359,
0.464551, 0.322355, 0.214358, 0.0712, -0.1525, 0.8982, -0.4062);
//R和T轉(zhuǎn)RT矩陣
Mat R_T2RT(Mat& R, Mat& T)
{
Mat RT;
Mat_<double> R1 = (cv::Mat_<double>(4, 3) << R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2),
R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2),
R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2),
0.0, 0.0, 0.0);
cv::Mat_<double> T1 = (cv::Mat_<double>(4, 1) << T.at<double>(0, 0), T.at<double>(1, 0), T.at<double>(2, 0), 1.0);
cv::hconcat(R1, T1, RT);//C=A+B左右拼接
return RT;
}
//RT轉(zhuǎn)R和T矩陣
void RT2R_T(Mat& RT, Mat& R, Mat& T)
{
cv::Rect R_rect(0, 0, 3, 3);
cv::Rect T_rect(3, 0, 1, 3);
R = RT(R_rect);
T = RT(T_rect);
}
//判斷是否為旋轉(zhuǎn)矩陣
bool isRotationMatrix(const cv::Mat& R)
{
cv::Mat tmp33 = R({ 0,0,3,3 });
cv::Mat shouldBeIdentity;
shouldBeIdentity = tmp33.t() * tmp33;
cv::Mat I = cv::Mat::eye(3, 3, shouldBeIdentity.type());
return cv::norm(I, shouldBeIdentity) < 1e-6;
}
/** @brief 歐拉角 -> 3*3 的R
* @param eulerAngle 角度值
* @param seq 指定歐拉角xyz的排列順序如："xyz" "zyx"
*/
cv::Mat eulerAngleToRotatedMatrix(const cv::Mat& eulerAngle, const std::string& seq)
{
CV_Assert(eulerAngle.rows == 1 && eulerAngle.cols == 3);
eulerAngle /= 180 / CV_PI;
cv::Matx13d m(eulerAngle);
auto rx = m(0, 0), ry = m(0, 1), rz = m(0, 2);
auto xs = std::sin(rx), xc = std::cos(rx);
auto ys = std::sin(ry), yc = std::cos(ry);
auto zs = std::sin(rz), zc = std::cos(rz);
cv::Mat rotX = (cv::Mat_<double>(3, 3) << 1, 0, 0, 0, xc, -xs, 0, xs, xc);
cv::Mat rotY = (cv::Mat_<double>(3, 3) << yc, 0, ys, 0, 1, 0, -ys, 0, yc);
cv::Mat rotZ = (cv::Mat_<double>(3, 3) << zc, -zs, 0, zs, zc, 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 == "xzy") rotMat = rotY * rotZ * rotX;
else if (seq == "yxz") rotMat = rotZ * rotX * rotY;
else if (seq == "xyz") rotMat = rotZ * rotY * rotX;
else {
cv::error(cv::Error::StsAssert, "Euler angle sequence string is wrong.",
__FUNCTION__, __FILE__, __LINE__);
}
if (!isRotationMatrix(rotMat)) {
cv::error(cv::Error::StsAssert, "Euler angle can not convert to rotated matrix",
__FUNCTION__, __FILE__, __LINE__);
}
return rotMat;
//cout << isRotationMatrix(rotMat) << endl;
}
/** @brief 四元數(shù)轉(zhuǎn)旋轉(zhuǎn)矩陣
* @note 數(shù)據(jù)類型double； 四元數(shù)定義 q = w + x*i + y*j + z*k
* @param q 四元數(shù)輸入{w,x,y,z}向量
* @return 返回旋轉(zhuǎn)矩陣3*3
*/
cv::Mat quaternionToRotatedMatrix(const cv::Vec4d& q)
{
double w = q[0], x = q[1], y = q[2], z = q[3];
double x2 = x * x, y2 = y * y, z2 = z * z;
double xy = x * y, xz = x * z, yz = y * z;
double wx = w * x, wy = w * y, wz = w * z;
cv::Matx33d res{
1 - 2 * (y2 + z2), 2 * (xy - wz), 2 * (xz + wy),
2 * (xy + wz), 1 - 2 * (x2 + z2), 2 * (yz - wx),
2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (x2 + y2),
};
return cv::Mat(res);
}
/** @brief ((四元數(shù)||歐拉角||旋轉(zhuǎn)向量) && 轉(zhuǎn)移向量) -> 4*4 的Rt
* @param m 1*6 || 1*7的矩陣 -> 6 {x,y,z, rx,ry,rz} 7 {x,y,z, qw,qx,qy,qz}
* @param useQuaternion 如果是1*7的矩陣，判斷是否使用四元數(shù)計(jì)算旋轉(zhuǎn)矩陣
* @param seq 如果通過歐拉角計(jì)算旋轉(zhuǎn)矩陣，需要指定歐拉角xyz的排列順序如："xyz" "zyx" 為空表示旋轉(zhuǎn)向量
*/
cv::Mat attitudeVectorToMatrix(cv::Mat m, bool useQuaternion, const std::string& seq)
{
CV_Assert(m.total() == 6 || m.total() == 7);
if (m.cols == 1)
m = m.t();
cv::Mat tmp = cv::Mat::eye(4, 4, CV_64FC1);
//如果使用四元數(shù)轉(zhuǎn)換成旋轉(zhuǎn)矩陣則讀取m矩陣的第第四個成員，讀4個數(shù)據(jù)
if (useQuaternion) // normalized vector, its norm should be 1.
{
cv::Vec4d quaternionVec = m({ 3, 0, 4, 1 });
quaternionToRotatedMatrix(quaternionVec).copyTo(tmp({ 0, 0, 3, 3 }));
// cout << norm(quaternionVec) << endl;
}
else
{
cv::Mat rotVec;
if (m.total() == 6)
rotVec = m({ 3, 0, 3, 1 }); //6
else
rotVec = m({ 7, 0, 3, 1 }); //10
//如果seq為空表示傳入的是旋轉(zhuǎn)向量，否則"xyz"的組合表示歐拉角
if (0 == seq.compare(""))
cv::Rodrigues(rotVec, tmp({ 0, 0, 3, 3 }));
else
eulerAngleToRotatedMatrix(rotVec, seq).copyTo(tmp({ 0, 0, 3, 3 }));
}
tmp({ 3, 0, 1, 3 }) = m({ 0, 0, 3, 1 }).t();
//std::swap(m,tmp);
return tmp;
}
int main()
{
//定義手眼標(biāo)定矩陣
std::vector<Mat> R_gripper2base;
std::vector<Mat> t_gripper2base;
std::vector<Mat> R_target2cam;
std::vector<Mat> t_target2cam;
Mat R_cam2gripper = (Mat_<double>(3, 3));
Mat t_cam2gripper = (Mat_<double>(3, 1));
vector<Mat> images;
size_t num_images = num;
// 讀取末端，標(biāo)定板的姿態(tài)矩陣 4*4
std::vector<cv::Mat> vecHb, vecHc;
cv::Mat Hcb;//定義相機(jī)camera到末端grab的位姿矩陣
Mat tempR, tempT;
for (size_t i = 0; i < num_images; i++)//計(jì)算標(biāo)定板位姿
{
cv::Mat tmp = attitudeVectorToMatrix(CalPose.row(i), true, ""); //轉(zhuǎn)移向量轉(zhuǎn)旋轉(zhuǎn)矩陣
vecHc.push_back(tmp);
RT2R_T(tmp, tempR, tempT);
R_target2cam.push_back(tempR);
t_target2cam.push_back(tempT);
}
//cout << R_target2cam << endl;
for (size_t i = 0; i < num_images; i++)//計(jì)算機(jī)械臂位姿
{
//cv::Mat tmp = attitudeVectorToMatrix(ToolPose.row(i), true, "zyx");
cv::Mat tmp = attitudeVectorToMatrix(ToolPose.row(i), true, ""); //機(jī)械臂位姿為歐拉角-旋轉(zhuǎn)矩陣
//tmp = tmp.inv();//機(jī)械臂基座位姿為歐拉角-旋轉(zhuǎn)矩陣
vecHb.push_back(tmp);
RT2R_T(tmp, tempR, tempT);
R_gripper2base.push_back(tempR);
t_gripper2base.push_back(tempT);
}
//cout << t_gripper2base[0] << " " << t_gripper2base[1] << " " << t_gripper2base[2] << " " << endl;
//cout << t_gripper2base << endl;
//手眼標(biāo)定，CALIB_HAND_EYE_TSAI法為11ms，最快
calibrateHandEye(R_gripper2base, t_gripper2base, R_target2cam, t_target2cam, R_cam2gripper, t_cam2gripper, CALIB_HAND_EYE_TSAI);
Hcb = R_T2RT(R_cam2gripper, t_cam2gripper);//矩陣合并
std::cout << "Hcb 矩陣為： " << std::endl;
std::cout << Hcb << std::endl;
cout << "是否為旋轉(zhuǎn)矩陣：" << isRotationMatrix(Hcb) << std::endl << std::endl;//判斷是否為旋轉(zhuǎn)矩陣
//Tool_In_Base*Hcg*Cal_In_Cam，用第一組和第二組進(jìn)行對比驗(yàn)證
cout << "第一組和第二組手眼數(shù)據(jù)驗(yàn)證：" << endl;
cout << vecHb[0] * Hcb * vecHc[0] << endl << vecHb[1] * Hcb * vecHc[1] << endl << endl;//.inv()
cout << "標(biāo)定板在相機(jī)中的位姿：" << endl;
cout << vecHc[1] << endl;
cout << "手眼系統(tǒng)反演的位姿為：" << endl;
//用手眼系統(tǒng)預(yù)測第一組數(shù)據(jù)中標(biāo)定板相對相機(jī)的位姿，是否與vecHc[1]相同
cout << Hcb.inv() * vecHb[1].inv() * vecHb[0] * Hcb * vecHc[0] << endl << endl;
cout << "----手眼系統(tǒng)測試----" << endl;
cout << "機(jī)械臂下標(biāo)定板XYZ為：" << endl;
for (int i = 0; i < vecHc.size(); ++i)
{
cv::Mat cheesePos{ 0.0,0.0,0.0,1.0 };//4*1矩陣，單獨(dú)求機(jī)械臂下，標(biāo)定板的xyz
cv::Mat worldPos = vecHb[i] * Hcb * vecHc[i] * cheesePos;
cout << i << ": " << worldPos.t() << endl;
}
getchar();
}
3.5 實(shí)驗(yàn)結(jié)果
精度還沒有做到極致，目前誤差在1cm以內(nèi)。
四、相關(guān)思考總結(jié)
4.1 多種姿態(tài)的表示方法
代碼中的attitudeVectorToMatrix可以實(shí)現(xiàn)：位置+歐拉角–>旋轉(zhuǎn)矩陣（輸入為nx6）位置+四元數(shù)—>旋轉(zhuǎn)矩陣（輸入為nx7）
-
歐拉角到旋轉(zhuǎn)矩陣的算法不唯一。當(dāng)使用歐拉角計(jì)算時務(wù)必注意歐拉角的順序以及輸入的應(yīng)為角度值。如果不知道歐拉角的順序，可以使用https://quaternions.online/進(jìn)行驗(yàn)證。
-
四元數(shù)到旋轉(zhuǎn)矩陣的算法唯一。但是四元數(shù)會有兩種表示方式
(w,x,y,z)和(x,y,z,w)，attitudeVectorToMatrix函數(shù)中使用(w,x,y,z)，但是aruco返回的姿態(tài)的四元數(shù)為(x,y,z,w)一定要記得修改！
4.2 機(jī)器人的末端坐標(biāo)系
- 機(jī)械臂的末端坐標(biāo)系是以最后一個軸的旋轉(zhuǎn)面作為原始平面建立的，不是末端法蘭的平面，兩者之間至少有20cm的距離。
- 代碼中的數(shù)據(jù)求得的手眼矩陣是相機(jī)相對于機(jī)械臂末端的轉(zhuǎn)換矩陣。但是我的機(jī)械臂末端裝了六維力傳感器以及一個抓手，如果想要得到相對于抓手末端的手眼矩陣是不是單純的對z值進(jìn)行增減即可？當(dāng)時我第一時間想到可以將原始數(shù)據(jù)機(jī)械臂位姿的z都減少一個相同的值，來模擬末端裝了一個tcp，再代入程序進(jìn)行求解得到如下數(shù)據(jù)。發(fā)現(xiàn)手眼矩陣并無變化，而是標(biāo)定板在基坐標(biāo)系下的z值減少了。仔細(xì)一想，發(fā)現(xiàn)問題在于不同姿態(tài)下末端不一定鉛錘于平面，因此這樣的數(shù)據(jù)程序認(rèn)為是你的標(biāo)定板放低了，而不是末端裝了個tcp。
- 回到手眼矩陣的本質(zhì)，是相對于某一個坐標(biāo)系的旋轉(zhuǎn)+平移。理論上可以對手眼矩陣的平移向量z值加以修改，較為簡單的轉(zhuǎn)換到tcp坐標(biāo)系下，但是這個z值好像難以把控。嘗試后還是決定再做一次手眼標(biāo)定，直接將tcp坐標(biāo)系加入到機(jī)械臂末端。
4.3 如何提升精度
AX=XB求解方法為Tsai-Lenz 兩步法，網(wǎng)上有其提升精度的注意點(diǎn)如下。說的很抽象，我看的似懂非懂，也沒人做過對照實(shí)驗(yàn)。希望大家多多討論交流。
(1) 不管采集多少組用于標(biāo)定的運(yùn)動數(shù)據(jù)，每組運(yùn)動使運(yùn)動角度最大。
(2) 使兩組運(yùn)動的旋轉(zhuǎn)軸角度最大。
(3) 每組運(yùn)動中機(jī)械臂末端運(yùn)動距離盡量小，可通路徑規(guī)劃實(shí)現(xiàn)該條件。
(4) 盡量減小相機(jī)中心到標(biāo)定板的距離，可使用適當(dāng)小的標(biāo)定板。
(5) 盡量采集多組用于求解的數(shù)據(jù)。
(6) 使用高精度的相機(jī)標(biāo)定方法。
(7) 盡量提高機(jī)械臂的絕對定位精度，如果該條件達(dá)不到，至少需要提高相對運(yùn)動精度。文章來源：http://www.zghlxwxcb.cn/news/detail-441527.html
五、參考文獻(xiàn)
手眼標(biāo)定_全面細(xì)致的推導(dǎo)過程
機(jī)器人手眼標(biāo)定
深入淺出地理解機(jī)器人手眼標(biāo)定
手眼標(biāo)定AX=XB求解方法（文獻(xiàn)總結(jié))
OpenCV Aruco 參數(shù)源碼完整解析理解
ArUco Marker檢測原理
https://github.com/IntelRealSense/realsense-ros
手眼標(biāo)定（二）：Tsai 求解方法文章來源地址http://www.zghlxwxcb.cn/news/detail-441527.html
到了這里，關(guān)于（已修正精度 1mm左右）Realsense d435i深度相機(jī)+Aruco+棋盤格+OpenCV手眼標(biāo)定全過程記錄的文章就介紹完了。如果您還想了解更多內(nèi)容，請?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
