手眼标定法的原理与实现
R. Y. Tsai and R. K. Lenz, “A new technique for fully autonomous and efficient 3D robotics hand/eye calibration,” in IEEE Transactions on Robotics and Automation, vol. 5, no. 3, pp. 345-358, June 1989
·
一、问题的来源
手眼标定问题的产生，源于机器人需要利用视觉传感器进行感知并操作目标物体。这个问题的核心之一在于坐标变换。
一个典型的机器人视觉系统通常涉及多个坐标系：
- 机器人基座坐标系 (Robot Base Frame)：机器人的全局参考系。该坐标系，例如焊接机器人的基座所在的坐标系，通常是固定的。
- 机器人的末端执行器坐标系 (Robot Hand Frame)：安装在机械臂末端的工具（如夹爪等）的坐标系。
- 机器人的相机坐标系 (Camera Frame)。
- 目标物体的坐标系 (Object Frame)：目标物体所在的坐标系。如果使用标定板，则该坐标系也可称为世界坐标系。
机器人可以通过自身的关节编码器计算末端执行器相对于基座的位姿，采用四阶增广矩阵表示该位姿，即Th−bT_{h-b}Th−b。
相机可以通过计算机视觉方法估计目标物体（例如标定板或工件等）相对于相机的位姿，采用四阶增广矩阵表示该位姿，即Tw−cT_{w-c}Tw−c。
然而，机器人只有获取到目标物体在基座坐标系中的位置，才能驱动末端执行器准确地移动到该位置执行任务。
为了获取目标物体在基座坐标系中的位置，必要的前提条件是已知相机相对于末端执行器的位姿，即Tc−hT_{c-h}Tc−h。这个变换矩阵是固定的，由传感器的安装位置决定。
二、问题的定义
在手眼标定问题中，
- 手 (Hand)，指的是机器人的末端执行器，其位姿变换矩阵Th−bT_{h-b}Th−b是已知的；
- 眼 (Eye)，指的是视觉传感器，通常是相机，其位姿变换矩阵Tw−cT_{w-c}Tw−c是已知的；
- 标定 (Calibration)：就是通过已知量Th−bT_{h-b}Th−b和Tw−cT_{w-c}Tw−c，计算或估计未知变换矩阵Tc−hT_{c-h}Tc−h的过程。
根据相机的安装位置，机器人视觉系统主要可分为两种模式：
- Eye-in-Hand (眼在手上)：相机安装在机器人的末端执行器上，随末端执行器一起运动。这种模式常用于精密的抓取等。
- Eye-to-Hand (眼在手外)：相机固定安装在机器人末端执行器之外的一个静止位置。这种模式可用于监控整个工作单元、跟踪移动物体等。
本文主要阐述第一种模式。
三、数学模型
手眼标定的核心原理是基于链式法则的坐标系变换，通过机器人的运动和相机的观测，构建方程，并计算或估计未知的变换矩阵。
建立数学模型的方法如下：
- 定义固定坐标系
假设机器人基座坐标系和目标物体的坐标系，即标定板上的世界坐标系，是固定的。
- 选择任意固定点
标定过程中，将标定板固定，并使其保持静止状态。构建数学模型时，选取固定标定板上的一个固定角点PwP_wPw。
- 寻找已知的变量
末端执行器相对于基座的变换矩阵Th−bT_{h-b}Th−b，直接从机器人控制器中读取。
标定板相对于相机的变换矩阵Tw−cT_{w-c}Tw−c，通过图像识别和PnP算法可估计该矩阵。
- 寻找未知的不变量
相机相对于末端执行器的固定变换Tc−hT_{c-h}Tc−h。
- 设计多条不同的变换路径
路径i： 固定点➡世界坐标系➡相机坐标系（位姿i）➡机器人末端执行器坐标系（位姿i）➡基座坐标系。
沿着路径i，将世界坐标系中的固定点PwP_wPw变换到基座坐标系，得到固定点Pb(i)P_b(i)Pb(i):
Pb(i)=Th−b(i)Tc−hTw−c(i)Pw(3.1) P_b(i)=T_{h-b}(i)T_{c-h}T_{w-c}(i)P_w \tag{3.1} Pb(i)=Th−b(i)Tc−hTw−c(i)Pw(3.1)
路径j： 固定点➡世界坐标系➡相机坐标系（位姿j）➡机器人末端执行器坐标系（位姿j）➡基座坐标系。
沿着路径j，将世界坐标系中的固定点PwP_wPw变换到基座坐标系，得到固定点Pb(j)P_b(j)Pb(j):
Pb(j)=Th−b(j)Tc−hTw−c(j)Pw(3.2) P_b(j)=T_{h-b}(j)T_{c-h}T_{w-c}(j)P_w \tag{3.2} Pb(j)=Th−b(j)Tc−hTw−c(j)Pw(3.2)
由于在该问题中，基座坐标系是固定的且唯一的，点PwP_wPw是固定的且唯一的，所以可建立方程
Pb(i)=Pb(j)(3.3) P_b(i)=P_b(j) \tag{3.3} Pb(i)=Pb(j)(3.3)
约去PwP_wPw，将方程(3.3)变换为
Th−b(i)Tc−hTw−c(i)=Th−b(j)Tc−hTw−c(j) T_{h-b}(i)T_{c-h}T_{w-c}(i)= T_{h-b}(j)T_{c-h}T_{w-c}(j) Th−b(i)Tc−hTw−c(i)=Th−b(j)Tc−hTw−c(j)
继续对上述方程进行矩阵变换:
Th−b−1(j)Th−b(i)Tc−h=Tc−hTw−c(j)Tw−c−1(i) T_{h-b}^{-1}(j)T_{h-b}(i)T_{c-h}=T_{c-h}T_{w-c}(j)T_{w-c}^{-1}(i) Th−b−1(j)Th−b(i)Tc−h=Tc−hTw−c(j)Tw−c−1(i)
最终将其简化为：
AX=XB(3.4) AX=XB \tag{3.4} AX=XB(3.4)
这就是经典的手眼标定方程。
其中，
A=Th−b−1(j)Th−b(i)(3.5) A=T_{h-b}^{-1}(j)T_{h-b}(i) \tag{3.5} A=Th−b−1(j)Th−b(i)(3.5)
是已知的，
B=Tw−c(j)Tw−c−1(i)(3.6) B=T_{w-c}(j)T_{w-c}^{-1}(i) \tag{3.6} B=Tw−c(j)Tw−c−1(i)(3.6)
是已知的，而
X=Tc−hX=T_{c-h}X=Tc−h
是未知的。标定就是获取矩阵X的过程。
关于解算AX=XB的方法，[1, 2]等文献中均有涉及，本文不再赘述。
四、算法实现
使用Grok Code大模型，生成C++代码：
hand_eye_calibration.hpp
#ifndef HAND_EYE_CALIBRATION_HPP
#define HAND_EYE_CALIBRATION_HPP
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <vector>
class HandEyeCalibration
{
public:
struct PosePair {
cv::Mat hand_pose; // 4x4 transformation matrix from hand
cv::Mat camera_pose; // 4x4 transformation matrix from camera
};
/**
* @brief Calibrate hand-eye transformation
* @param pose_pairs Vector of pose pairs (hand and camera poses)
* @return 4x4 transformation matrix from camera to hand
*/
static cv::Mat calibrateHandEye(const std::vector<PosePair>& pose_pairs);
/**
* @brief Convert rotation matrix and translation vector to 4x4 transformation matrix
*/
static cv::Mat createTransformationMatrix(const cv::Mat& R, const cv::Mat& t);
/**
* @brief Extract rotation and translation from 4x4 transformation matrix
*/
static void decomposeTransformationMatrix(const cv::Mat& T, cv::Mat& R, cv::Mat& t);
/**
* @brief Convert OpenCV matrix to Eigen matrix
*/
static Eigen::Matrix4d cvMatToEigen(const cv::Mat& cv_mat);
/**
* @brief Convert Eigen matrix to OpenCV matrix
*/
static cv::Mat eigenToCvMat(const Eigen::Matrix4d& eigen_mat);
/**
* @brief Validate calibration result
*/
static double validateCalibration(const cv::Mat& X, const std::vector<PosePair>& pose_pairs);
private:
/**
* @brief Solve AX = XB using Tsai's method
*/
static cv::Mat solveTsai(const std::vector<cv::Mat>& A_matrices,
const std::vector<cv::Mat>& B_matrices);
/**
* @brief Solve AX = XB using Park's method
*/
static cv::Mat solvePark(const std::vector<cv::Mat>& A_matrices,
const std::vector<cv::Mat>& B_matrices);
/**
* @brief Compute relative transformations between consecutive poses
*/
static void computeRelativeTransformations(const std::vector<cv::Mat>& poses,
std::vector<cv::Mat>& relative_transforms);
};
#endif // HAND_EYE_CALIBRATION_HPP
hand_eye_calibration.cpp
#include "hand_eye_calibration.hpp"
#include <iostream>
#include <cmath>
cv::Mat HandEyeCalibration::calibrateHandEye(const std::vector<PosePair>& pose_pairs) {
if (pose_pairs.size() < 3) {
throw std::runtime_error("At least 3 pose pairs are required for hand-eye calibration");
}
std::vector<cv::Mat> hand_poses, camera_poses;
for (const auto& pair : pose_pairs) {
hand_poses.push_back(pair.hand_pose);
camera_poses.push_back(pair.camera_pose);
}
std::vector<cv::Mat> A_matrices, B_matrices;
computeRelativeTransformations(hand_poses, A_matrices);
computeRelativeTransformations(camera_poses, B_matrices);
// Use Tsai's method as default (more robust for most cases)
return solveTsai(A_matrices, B_matrices);
}
cv::Mat HandEyeCalibration::createTransformationMatrix(const cv::Mat& R, const cv::Mat& t) {
cv::Mat T = cv::Mat::eye(4, 4, CV_64F);
R.copyTo(T(cv::Rect(0, 0, 3, 3)));
t.copyTo(T(cv::Rect(3, 0, 1, 3)));
return T;
}
void HandEyeCalibration::decomposeTransformationMatrix(const cv::Mat& T, cv::Mat& R, cv::Mat& t) {
CV_Assert(T.rows == 4 && T.cols == 4);
R = T(cv::Rect(0, 0, 3, 3)).clone();
t = T(cv::Rect(3, 0, 1, 3)).clone();
}
Eigen::Matrix4d HandEyeCalibration::cvMatToEigen(const cv::Mat& cv_mat) {
Eigen::Matrix4d eigen_mat;
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
eigen_mat(i, j) = cv_mat.at<double>(i, j);
}
}
return eigen_mat;
}
cv::Mat HandEyeCalibration::eigenToCvMat(const Eigen::Matrix4d& eigen_mat) {
cv::Mat cv_mat(4, 4, CV_64F);
for (int i = 0; i < 4; ++i) {
for (int j = 0; j < 4; ++j) {
cv_mat.at<double>(i, j) = eigen_mat(i, j);
}
}
return cv_mat;
}
cv::Mat HandEyeCalibration::solveTsai(const std::vector<cv::Mat>& A_matrices,
const std::vector<cv::Mat>& B_matrices) {
// Implementation of Tsai's hand-eye calibration method
// This solves AX = XB using quaternion-based approach
size_t n = A_matrices.size();
std::vector<Eigen::Matrix4d> A_eigen, B_eigen;
for (size_t i = 0; i < n; ++i) {
A_eigen.push_back(cvMatToEigen(A_matrices[i]));
B_eigen.push_back(cvMatToEigen(B_matrices[i]));
}
// Build the system of equations for rotation part
Eigen::MatrixXd M(3 * n, 3);
Eigen::VectorXd b(3 * n);
for (size_t i = 0; i < n; ++i) {
Eigen::Matrix3d Ra = A_eigen[i].block<3, 3>(0, 0);
Eigen::Matrix3d Rb = B_eigen[i].block<3, 3>(0, 0);
Eigen::Vector3d ta = A_eigen[i].block<3, 1>(0, 3);
Eigen::Vector3d tb = B_eigen[i].block<3, 1>(0, 3);
// Extract rotation matrices
Eigen::Matrix3d Ra_minus_I = Ra - Eigen::Matrix3d::Identity();
Eigen::Matrix3d I_minus_Rb = Eigen::Matrix3d::Identity() - Rb;
// Fill M and b for this sample
M.block<3, 3>(3 * i, 0) = Ra_minus_I;
b.segment<3>(3 * i) = tb - ta;
}
// Solve for translation
Eigen::Vector3d translation = M.colPivHouseholderQr().solve(b);
// Now solve for rotation using the quaternion method
Eigen::MatrixXd K(4 * n, 4);
for (size_t i = 0; i < n; ++i) {
Eigen::Matrix3d Ra = A_eigen[i].block<3, 3>(0, 0);
Eigen::Matrix3d Rb = B_eigen[i].block<3, 3>(0, 0);
// Convert to quaternions and build the system
Eigen::Quaterniond qa(Ra);
Eigen::Quaterniond qb(Rb);
// For quaternion constraint: qa * qx = qx * qb
// This leads to: qa * qx - qx * qb = 0
Eigen::Matrix4d left_mult_qa, right_mult_qb;
left_mult_qa << qa.w(), -qa.x(), -qa.y(), -qa.z(),
qa.x(), qa.w(), -qa.z(), qa.y(),
qa.y(), qa.z(), qa.w(), -qa.x(),
qa.z(), -qa.y(), qa.x(), qa.w();
right_mult_qb << qb.w(), -qb.x(), -qb.y(), -qb.z(),
qb.x(), qb.w(), qb.z(), -qb.y(),
qb.y(), -qb.z(), qb.w(), qb.x(),
qb.z(), qb.y(), -qb.x(), qb.w();
K.block<4, 4>(4 * i, 0) = left_mult_qa - right_mult_qb;
}
// Find the null space of K (smallest singular value)
Eigen::JacobiSVD<Eigen::MatrixXd> svd(K, Eigen::ComputeThinV);
Eigen::Vector4d qx = svd.matrixV().col(3); // Last column for null space
// Normalize quaternion
qx.normalize();
Eigen::Quaterniond qx_quat(qx(0), qx(1), qx(2), qx(3));
// Build final transformation matrix
Eigen::Matrix4d X = Eigen::Matrix4d::Identity();
X.block<3, 3>(0, 0) = qx_quat.toRotationMatrix();
X.block<3, 1>(0, 3) = translation;
return eigenToCvMat(X);
}
cv::Mat HandEyeCalibration::solvePark(const std::vector<cv::Mat>& A_matrices,
const std::vector<cv::Mat>& B_matrices) {
// Implementation of Park's method as alternative
// This method is more numerically stable for some cases
size_t n = A_matrices.size();
Eigen::MatrixXd T(9 * n, 9);
Eigen::VectorXd b(9 * n);
for (size_t i = 0; i < n; ++i) {
Eigen::Matrix4d A = cvMatToEigen(A_matrices[i]);
Eigen::Matrix4d B = cvMatToEigen(B_matrices[i]);
Eigen::Matrix3d Ra = A.block<3, 3>(0, 0);
Eigen::Matrix3d Rb = B.block<3, 3>(0, 0);
Eigen::Vector3d ta = A.block<3, 1>(0, 3);
Eigen::Vector3d tb = B.block<3, 1>(0, 3);
// Build the Kronecker product constraint
Eigen::Matrix3d Ra_minus_I = Ra - Eigen::Matrix3d::Identity();
Eigen::Matrix3d I_minus_Rb = Eigen::Matrix3d::Identity() - Rb;
// Fill the system matrix
T.block<9, 9>(9 * i, 0).setZero();
// This is a simplified version - full Park method is more complex
// For production use, consider using OpenCV's built-in implementation
}
// For simplicity, fall back to Tsai's method
return solveTsai(A_matrices, B_matrices);
}
void HandEyeCalibration::computeRelativeTransformations(const std::vector<cv::Mat>& poses,
std::vector<cv::Mat>& relative_transforms) {
for (size_t i = 1; i < poses.size(); ++i) {
cv::Mat T_prev_inv = poses[i-1].inv();
cv::Mat relative = T_prev_inv * poses[i];
relative_transforms.push_back(relative);
}
}
double HandEyeCalibration::validateCalibration(const cv::Mat& X,
const std::vector<PosePair>& pose_pairs) {
double total_error = 0.0;
size_t num_pairs = pose_pairs.size();
for (size_t i = 0; i < num_pairs; ++i) {
// Check if A_i * X ≈ X * B_i
cv::Mat A_X = pose_pairs[i].hand_pose * X;
cv::Mat X_B = X * pose_pairs[i].camera_pose;
cv::Mat diff = A_X - X_B;
double error = cv::norm(diff);
total_error += error;
std::cout << "Pose pair " << i << " error: " << error << std::endl;
}
return total_error / num_pairs;
}
main.cpp
#include "hand_eye_calibration.hpp"
#include <iostream>
#include <vector>
#include <cmath>
int main(int argc, char* argv[])
{
try {
// Example: Generate synthetic calibration data
// In real application, you would collect this data from your robot and camera
std::vector<HandEyeCalibration::PosePair> pose_pairs;
// Generate example poses (replace with real data collection)
for (int i = 0; i < 10; ++i) { // Need at least 3 pose pairs
HandEyeCalibration::PosePair pair;
// Generate synthetic hand pose (rotation around Z axis)
double angle_hand = i * 36.0 * CV_PI / 180.0; // 36 degrees apart
cv::Mat R_hand = cv::Mat::eye(3, 3, CV_64F);
R_hand.at<double>(0, 0) = cos(angle_hand);
R_hand.at<double>(0, 1) = -sin(angle_hand);
R_hand.at<double>(1, 0) = sin(angle_hand);
R_hand.at<double>(1, 1) = cos(angle_hand);
cv::Mat t_hand = (cv::Mat_<double>(3, 1) << 0.1 * i, 0.05 * i, 0.0);
pair.hand_pose = HandEyeCalibration::createTransformationMatrix(R_hand, t_hand);
// Generate synthetic camera pose (different rotation)
double angle_camera = i * 30.0 * CV_PI / 180.0; // 30 degrees apart
cv::Mat R_camera = cv::Mat::eye(3, 3, CV_64F);
R_camera.at<double>(0, 0) = cos(angle_camera);
R_camera.at<double>(0, 1) = -sin(angle_camera);
R_camera.at<double>(1, 0) = sin(angle_camera);
R_camera.at<double>(1, 1) = cos(angle_camera);
cv::Mat t_camera = (cv::Mat_<double>(3, 1) << 0.08 * i, 0.03 * i, 0.0);
pair.camera_pose = HandEyeCalibration::createTransformationMatrix(R_camera, t_camera);
pose_pairs.push_back(pair);
}
// Perform hand-eye calibration
std::cout << "Performing hand-eye calibration..." << std::endl;
cv::Mat X = HandEyeCalibration::calibrateHandEye(pose_pairs);
std::cout << "Hand-eye transformation matrix X (camera to hand):" << std::endl;
std::cout << X << std::endl;
// Validate the calibration
double avg_error = HandEyeCalibration::validateCalibration(X, pose_pairs);
std::cout << "Average validation error: " << avg_error << std::endl;
// Extract rotation and translation components
cv::Mat R, t;
HandEyeCalibration::decomposeTransformationMatrix(X, R, t);
std::cout << "\nRotation matrix R:" << std::endl;
std::cout << R << std::endl;
std::cout << "\nTranslation vector t:" << std::endl;
std::cout << t << std::endl;
} catch (const std::exception& e) {
std::cerr << "Error: " << e.what() << std::endl;
return 1;
}
return 0;
}
以上三个文件中的C++代码，实现了手眼标定法。此外，OpenCV中有解算手眼标定法的函数，可直接调用，例如
#include <opencv2/calib3d.hpp>
#include <utility>
std::pair<cv::Mat, cv::Mat> calibrateHandEyeOpenCV(const std::vector<cv::Mat>& R_hand,
const std::vector<cv::Mat>& t_hand,
const std::vector<cv::Mat>& R_camera,
const std::vector<cv::Mat>& t_camera)
{
cv::Mat R_cam2hand, t_cam2hand;
cv::calibrateHandEye(R_hand, t_hand, R_camera, t_camera,
R_cam2hand, t_cam2hand,
cv::CALIB_HAND_EYE_TSAI);
return std::make_pair(R_cam2hand, t_cam2hand);
}
开源项目[3]^{[3]}[3]也提供了解算AX=XBAX=XBAX=XB的算法，也可作为参考。
参考文献
[1] R. Y. Tsai and R. K. Lenz, “A new technique for fully autonomous and efficient 3D robotics hand/eye calibration,” in IEEE Transactions on Robotics and Automation, vol. 5, no. 3, pp. 345-358, June 1989, doi: 10.1109/70.34770.
[2] Radu Horaud, Fadi Dornaika. Hand-Eye Calibration. IJRR 1995.
[3] https://github.com/zhixy/SolveAXXB
有“AI”的1024 = 2048，欢迎大家加入2048 AI社区
更多推荐
12
0- 0
已为社区贡献1条内容
所有评论(0)