焊缝跟踪仪的标定（眼在手上）

韭菜钟2026-08-06 17:36

文章目录

由于焊缝跟踪仪是3d线扫相机的一种特殊应用（可以理解为功能定制化的3d线扫相机），所以标定过程不用完全按照3d线扫相机的操作方式来进行，大部分操作都可以简化。这里我们采用其说明书介绍的四点标定法来进行。

方法简介

定好一个能被跟踪仪识别的物理点P

工具采集物理点P的点位P1

同一物理点的线扫观察点P2、P3、P4。

利用采集得到的数据，进行标定计算，得到变换矩阵

此时，标定已经完成。

接下来，当输入跟踪仪的坐标（0，y，z）以及此时的机械臂tcp值（xyzuvw），便可以得到跟踪仪跟踪到的点在基坐标系下的（x，y，z）

下面具体介绍整个操作过程。

数据采集

进行标定前需要先完成机械臂工具标定，在标定时使用标定好的工具+在基坐标系下进行。

注意，此方法得到的变换矩阵是跟踪仪坐标系相对于工具坐标系的，得到此变换矩阵后，假如想要得到相对于法兰盘的变换矩阵，也简单。只要将此矩阵叠加工具变换矩阵即可。

也可以直接将变换矩阵标定为跟踪仪坐标系相对与法兰坐标系的，只需要更改P2-P4点的坐标系为法兰坐标系即可。

在采集前，我们要先准备一个阶梯状的物体。其实也不一定要阶梯状，只要是能够被焊缝跟踪仪识别到的特征都可以，焊缝跟踪仪可以识别到比较多的特征（对接焊缝、内角焊缝等），可以根据自己实际的物体来选择。我们这里就选择了阶梯型（搭接焊缝）来介绍了。

我们在标定块上用mark笔在边缘处定好一个点，将这个点作为我们的物理点P。

采集P1点

采集此点时，机械臂的示教系统需要启用工具。

因为这时候需要采集此点在机械臂基坐标系下的值，所以必须启用工具并用工具尖端（TCP）去对准物理点P，然后从示教器上读取此时TCP的读数P1(x,y,z,u,v,w)

采集P2-P4点及其对应的跟踪仪器点

在采集P2-P4点时，示教系统继续启用和标定P1时的工具。

采集P2点

移动机器人，让跟踪仪的线打在我们标记好的物理点P上，并能够识别出特征点（软件上出现十字星标记）。再调节一下机器人，让十字标记出现在z方向0度正上方（也就是y值为0，z值为正），如下图所示。记录下此时物理点P在机械臂上的读数P2、及跟踪仪的读数S2（0，y，z）

采集P3点

再调节一下机器人（让机器人往上移动后再往右移动），让十字标记出现在梯形视图的左下方（也就是y值为负，z值为负的位置，也就是图中的第3象限），如下图所示。记录下此时物理点P在机械臂上的读数P3、及跟踪仪的读数S3（0，y，z）

采集P4点

再调节一下机器人（让机器人往上移动后再往左移动），让十字标记出现在梯形视图的左下方（也就是y值为正，z值为负的位置，也就是图中的第4象限），如下图所示。记录下此时物理点P在机械臂上的读数P4、及跟踪仪的读数S4（0，y，z）

至此，我们需要的数据都采集完了。

采集的数据如下(这个数据P2-P4点用的是带工具的读数)，可以使用这个数据验算一下。

点 x y z u v w sensor y sensor z
P1 1256.79 223.38 597.58 -171.37 149.85 -6.1
P2 1283.3300 74.8500 607.6800 -171.3700 149.8500 -6.1000 0.0000 40.1600
P3 1293.8500 44.5200 724.5200 -171.3700 149.8500 - 6.1000 -31.8300 -76.2500
P4 1267.2800 117.8700 728.9300 -171.3700 149.8500 -6.1000 45.6600 -81.3900

算法实现

需要用到eigen库来简化代码，或者你也可以让你的AI将其改成纯手搓的。

linesensorcalibrator.h

cpp 复制代码 
#pragma once
#include <string>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>

// 单线（2D 轮廓）焊缝跟踪仪的外参标定。
// 跟踪仪刚性安装在机械臂末端执行器（法兰/TCP）上。
//
// 传感器只上报物理点在自身坐标系下的 (y, z)，x 固定为 0，
// 因为激光面就是传感器的 y-z 平面。本类求解刚性变换 T，
// 把传感器点 s = [0, y, z] 变换到记录的机器人位姿坐标系
// （法兰或 TCP，取决于 xyzuvw 代表什么）：
//
//     p_pose = R * [0, y, z]^T + t
//
// 由于 x 恒为 0，R 的第一列 r1 不会被数据激励；解出 r2、r3、t 后
// 用 r1 = r2 x r3 重构。
class LineSensorCalibrator
{
public:
    struct Pose6D {
        double x{0}, y{0}, z{0};   // mm
        double u{0}, v{0}, w{0};   // 度
        std::string toString() const;
        static Pose6D fromString(const std::string& s, bool* ok = nullptr);
    };

    // 把 (u,v,w) 转换为旋转矩阵所用的欧拉角约定。
    enum class EulerConvention {
        ZYZ,   // Rz(u) * Ry(v) * Rz(w)   （与 3DCamCalibration 一致）
        ZYX,   // Rz(u) * Ry(v) * Rx(w)   （偏航-俯仰-横滚）
        XYZ    // Rx(u) * Ry(v) * Rz(w)   （固定轴/RPY）
    };

    struct Observation {
        Pose6D pose;      // 观测到该点时的机器人位姿
        double y{0};      // 传感器读数，mm
        double z{0};      // 传感器读数，mm
    };

    void setConvention(EulerConvention c) { m_conv = c; }
    EulerConvention convention() const { return m_conv; }

    // P1：TCP 触碰物理标定点时的位姿，其平移分量就是标定点在基坐标系下的坐标。
    void setTouchPose(const Pose6D& p) { m_touch = p; m_hasTouch = true; }
    bool hasTouch() const { return m_hasTouch; }
    const Pose6D& touchPose() const { return m_touch; }

    void addObservation(const Pose6D& pose, double y, double z);
    void removeObservation(int index);
    void clearObservations();
    int  observationCount() const { return static_cast<int>(m_obs.size()); }
    const Observation& observation(int i) const { return m_obs[i]; }

    // 求解外参。需要触碰位姿 P1 和至少 3 个观测。
    // T_out  : 4x4 传感器 -> 位姿坐标系变换。
    // pose_out: 同一变换的 xyzuvw 表示（按所选欧拉角约定）。
    // rmsMm  : 所有观测的重投影 RMS 误差，mm。
    bool calibrate(Eigen::Matrix4d& T_out,
                   Pose6D&          pose_out,
                   double&          rmsMm,
                   std::string*     error = nullptr) const;

    // 把传感器坐标系下的点 (0,y,z) 变换到"记录位姿坐标系"（如 TCP 坐标系）：
    // p_pose = T_es * [0,y,z]^T
    Eigen::Vector3d sensorToPose(const Eigen::Matrix4d& T_es,
                                 double y, double z) const;

    // 把传感器坐标系下的点变换到机器人基坐标系（用于标定后验证）：
    // p_base = T_pose_in_base * T_es * [0,y,z]^T
    Eigen::Vector3d sensorToBase(const Eigen::Matrix4d& T_es,
                                 const Pose6D& pose, double y, double z) const;

    Eigen::Matrix3d toR(double u_deg, double v_deg, double w_deg) const;
    void            fromR(const Eigen::Matrix3d& R,
                          double& u_deg, double& v_deg, double& w_deg) const;
    Eigen::Matrix4d toMatrix(const Pose6D& p) const;
    Pose6D          fromMatrix(const Eigen::Matrix4d& T) const;

private:
    EulerConvention          m_conv{EulerConvention::ZYZ};
    Pose6D                   m_touch;
    bool                     m_hasTouch{false};
    std::vector<Observation> m_obs;
};

linesensorcalibrator.cpp

cpp 复制代码 
#include "linesensorcalibrator.h"

#include <cmath>
#include <sstream>
#include <Eigen/Dense>

namespace {
constexpr double kPi  = 3.14159265358979323846;
constexpr double kDeg = kPi / 180.0;
constexpr double kRad = 180.0 / kPi;
}

Eigen::Matrix3d LineSensorCalibrator::toR(double u_deg, double v_deg, double w_deg) const
{
    const Eigen::AngleAxisd Ru(u_deg * kDeg, Eigen::Vector3d::UnitZ());
    const Eigen::AngleAxisd Rv(v_deg * kDeg, Eigen::Vector3d::UnitY());
    switch (m_conv) {
    case EulerConvention::ZYX: {
        const Eigen::AngleAxisd Rw(w_deg * kDeg, Eigen::Vector3d::UnitX());
        return (Ru * Rv * Rw).toRotationMatrix();
    }
    case EulerConvention::XYZ: {
        const Eigen::AngleAxisd Rx(u_deg * kDeg, Eigen::Vector3d::UnitX());
        const Eigen::AngleAxisd Rz(w_deg * kDeg, Eigen::Vector3d::UnitZ());
        return (Rx * Rv * Rz).toRotationMatrix();
    }
    case EulerConvention::ZYZ:
    default: {
        const Eigen::AngleAxisd Rw(w_deg * kDeg, Eigen::Vector3d::UnitZ());
        return (Ru * Rv * Rw).toRotationMatrix();
    }
    }
}

void LineSensorCalibrator::fromR(const Eigen::Matrix3d& R,
                                 double& u_deg, double& v_deg, double& w_deg) const
{
    switch (m_conv) {
    case EulerConvention::ZYX: {
        v_deg = std::atan2(-R(2,0), std::sqrt(R(0,0)*R(0,0) + R(1,0)*R(1,0))) * kRad;
        const double cv = std::cos(v_deg * kDeg);
        if (std::fabs(cv) > 1e-7) {
            u_deg = std::atan2(R(1,0), R(0,0)) * kRad;
            w_deg = std::atan2(R(2,1), R(2,2)) * kRad;
        } else {
            u_deg = 0.0;
            w_deg = std::atan2(-R(0,1), R(1,1)) * kRad;
        }
        break;
    }
    case EulerConvention::XYZ: {
        v_deg = std::atan2(R(0,2), std::sqrt(R(0,0)*R(0,0) + R(0,1)*R(0,1))) * kRad;
        const double cv = std::cos(v_deg * kDeg);
        if (std::fabs(cv) > 1e-7) {
            u_deg = std::atan2(-R(1,2), R(2,2)) * kRad;
            w_deg = std::atan2(-R(0,1), R(0,0)) * kRad;
        } else {
            u_deg = std::atan2(R(2,1), R(1,1)) * kRad;
            w_deg = 0.0;
        }
        break;
    }
    case EulerConvention::ZYZ:
    default: {
        const double cv = std::max(-1.0, std::min(1.0, R(2,2)));
        v_deg = std::acos(cv) * kRad;
        const double sv = std::sin(v_deg * kDeg);
        if (std::fabs(sv) > 1e-7) {
            u_deg = std::atan2(R(1,2), R(0,2)) * kRad;
            w_deg = std::atan2(R(2,1), -R(2,0)) * kRad;
        } else {
            u_deg = 0.0;
            w_deg = (cv > 0.0) ? std::atan2(-R(0,1), R(0,0)) * kRad
                               : std::atan2( R(0,1),-R(0,0)) * kRad;
        }
        break;
    }
    }
}

Eigen::Matrix4d LineSensorCalibrator::toMatrix(const Pose6D& p) const
{
    Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
    T.block<3,3>(0,0) = toR(p.u, p.v, p.w);
    T(0,3) = p.x; T(1,3) = p.y; T(2,3) = p.z;
    return T;
}

LineSensorCalibrator::Pose6D LineSensorCalibrator::fromMatrix(const Eigen::Matrix4d& T) const
{
    Pose6D p;
    p.x = T(0,3); p.y = T(1,3); p.z = T(2,3);
    fromR(T.block<3,3>(0,0), p.u, p.v, p.w);
    return p;
}

std::string LineSensorCalibrator::Pose6D::toString() const
{
    std::ostringstream os;
    os.precision(4);
    os << std::fixed << x << " " << y << " " << z
       << " " << u << " " << v << " " << w;
    return os.str();
}

LineSensorCalibrator::Pose6D
LineSensorCalibrator::Pose6D::fromString(const std::string& s, bool* ok)
{
    std::istringstream ss(s);
    std::string tok;
    std::vector<double> vals;
    while (ss >> tok) {
        if (!tok.empty() && (tok.back() == ',' || tok.back() == ';')) tok.pop_back();
        try { vals.push_back(std::stod(tok)); }
        catch (...) { if (ok) *ok = false; return {}; }
    }
    Pose6D p;
    if (vals.size() >= 6) {
        p.x=vals[0]; p.y=vals[1]; p.z=vals[2];
        p.u=vals[3]; p.v=vals[4]; p.w=vals[5];
        if (ok) *ok = true;
    } else {
        if (ok) *ok = false;
    }
    return p;
}

void LineSensorCalibrator::addObservation(const Pose6D& pose, double y, double z)
{
    m_obs.push_back({pose, y, z});
}

void LineSensorCalibrator::removeObservation(int index)
{
    if (index >= 0 && index < static_cast<int>(m_obs.size()))
        m_obs.erase(m_obs.begin() + index);
}

void LineSensorCalibrator::clearObservations() { m_obs.clear(); }

bool LineSensorCalibrator::calibrate(Eigen::Matrix4d& T_out,
                                     Pose6D&          pose_out,
                                     double&          rmsMm,
                                     std::string*     error) const
{
    auto fail = [&](const char* msg) { if (error) *error = msg; return false; };

    if (!m_hasTouch)
        return fail("缺少触碰点 P1（TCP 触碰标定点时的位姿）。");
    if (m_obs.size() < 3)
        return fail("至少需要 3 个传感器观测点（P2、P3、P4）。");

    // 标定点在机器人基坐标系下的坐标 = 触碰位姿时的 TCP 平移。
    const Eigen::Vector3d Pw(m_touch.x, m_touch.y, m_touch.z);

    // 对每个观测 i：R*[0,yi,zi]^T + t = q_i，其中
    //   q_i = T_pose(i)^-1 * Pw  （标定点在位姿坐标系下的坐标）。
    // 未知量 X = [r2(3); r3(3); t(3)]，每个观测给出 3 个方程：
    //   yi*r2 + zi*r3 + t = q_i。
    const int N = static_cast<int>(m_obs.size());
    Eigen::MatrixXd A(3 * N, 9);
    Eigen::VectorXd b(3 * N);
    A.setZero();

    std::vector<Eigen::Vector3d> q(N);
    for (int i = 0; i < N; ++i) {
        const Observation& o = m_obs[i];
        const Eigen::Matrix4d T = toMatrix(o.pose);
        const Eigen::Matrix4d Tinv = T.inverse();
        q[i] = (Tinv * Pw.homogeneous()).head<3>();

        const int r = 3 * i;
        A.block<3,3>(r, 0) = o.y * Eigen::Matrix3d::Identity();  // r2 系数
        A.block<3,3>(r, 3) = o.z * Eigen::Matrix3d::Identity();  // r3 系数
        A.block<3,3>(r, 6) = Eigen::Matrix3d::Identity();        // t 系数
        b.segment<3>(r) = q[i];
    }

    // 最小二乘求解。
    const Eigen::VectorXd X =
        A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b);

    Eigen::Vector3d r2 = X.segment<3>(0);
    Eigen::Vector3d r3 = X.segment<3>(3);
    Eigen::Vector3d t  = X.segment<3>(6);

    if (!r2.allFinite() || !r3.allFinite() || !t.allFinite())
        return fail("退化配置（位姿/读数近似共线）。");
    if (r2.norm() < 1e-6 || r3.norm() < 1e-6)
        return fail("退化配置：传感器坐标轴不可观测。");

    // x 轴不会被数据激励（x 恒为 0）：用叉乘重构。
    const Eigen::Vector3d r1 = r2.cross(r3);

    // 把 [r1 r2 r3] 投影到最近的正交旋转矩阵（SO(3)）。
    Eigen::Matrix3d M;
    M.col(0) = r1; M.col(1) = r2; M.col(2) = r3;
    Eigen::JacobiSVD<Eigen::Matrix3d> svd(M, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d R = svd.matrixU() * svd.matrixV().transpose();
    if (R.determinant() < 0) {
        Eigen::Matrix3d V = svd.matrixV();
        V.col(2) *= -1.0;
        R = svd.matrixU() * V.transpose();
    }

    T_out = Eigen::Matrix4d::Identity();
    T_out.block<3,3>(0,0) = R;
    T_out.block<3,1>(0,3) = t;
    pose_out = fromMatrix(T_out);

    // 计算位姿坐标系下的 RMS 重投影误差。
    double sse = 0.0;
    for (int i = 0; i < N; ++i) {
        const Eigen::Vector3d s(0.0, m_obs[i].y, m_obs[i].z);
        const Eigen::Vector3d pred = R * s + t;
        sse += (pred - q[i]).squaredNorm();
    }
    rmsMm = std::sqrt(sse / N);
    return true;
}

Eigen::Vector3d LineSensorCalibrator::sensorToPose(const Eigen::Matrix4d& T_es,
                                                   double y, double z) const
{
    // 传感器点 x 恒为 0。
    const Eigen::Vector3d s(0.0, y, z);
    return (T_es * s.homogeneous()).head<3>();
}

Eigen::Vector3d LineSensorCalibrator::sensorToBase(const Eigen::Matrix4d& T_es,
                                                   const Pose6D& pose, double y, double z) const
{
    // p_base = T_pose_in_base * T_es * [0,y,z]^T
    const Eigen::Matrix4d T_pose = toMatrix(pose);
    const Eigen::Vector3d s(0.0, y, z);
    return (T_pose * T_es * s.homogeneous()).head<3>();
}
相关推荐
暂未成功人士！2 个月前
相机标定---张正友相机标定和手眼标定
数码相机·手眼标定·相机标定
boss-dog4 个月前
3D视觉机器人中手眼标定的精度提升方法记录——ICP算法
算法·3d·机器人·手眼标定·icp
boss-dog5 个月前
关于眼在手外的相对误差计算和分析
手眼标定·误差计算
TTGGGFF8 个月前
具身智能：零基础入门睿尔曼机械臂（五）—— 手眼标定核心原理与数学求解
机械臂·手眼标定·具身智能
这张生成的图像能检测吗8 个月前
（论文速读）一种基于双目视觉的机器人螺纹装配预对准姿态估计方法
人工智能·计算机视觉·机器人·手眼标定·位姿估计·双目视觉·螺纹装配
起个名字费劲死了1 年前
手眼标定之已知同名点对，求解转换RT，备份记录
c++·数码相机·机器人·几何学·手眼标定
boss-dog1 年前
手眼标定：九点标定、十二点标定、OpenCV 手眼标定
opencv·手眼标定
CV工程师小朱1 年前
OpenCV机械臂手眼标定
opencv·机械臂·手眼标定
kuan_li_lyg2 年前
MATLAB - 机械臂手眼标定（眼在手内） - 估计安装在机器人上的移动相机的姿态
开发语言·人工智能·matlab·机器人·ros·机械臂·手眼标定
热门推荐
01VSCode+CodeX扩展，无法启动02GitHub 镜像站点03国内可直接用、免费额度/永久免费的大模型API清单（含 SiliconFlow、火山、阿里、智谱、百度、Kimi、DeepSeek、DMXAPI 等）04如何新建文件夹？ 电脑新建文件夹的4种方法052026 国产 AI 大模型横评：DeepSeek、通义千问、Kimi、文心一言、星火、豆包谁更能打？06微信历史版本含下载地址（ Windows PC | 安卓 | MAC ）及设置微信不更新07历年考研数学一、数学二、数学三真题试卷及答案PDF08Agnes AI 免费 API 接入指南：文本、生图、生视频，一套接口全免费09AI 编程 IDE 全景解析 2026：Agent 全面接管开发链路102026年7月AI圈大地震：GPT-5.6被政府限制、Claude入驻Slack、Anthropic自研芯片