復現RoboDK機器人校準功能（以Staubli TX2?90L / TX200機械臂為測試對象，實測精度接近原廠）

本算法復現了 RoboDK 的機器人校準功能：在訓練集的理論校準后精度與 RoboDK 一致，在測試集的實測精度接近 Staubli 原廠。

參考：RoboDK 機器人校準功能（https://robodk.com.cn/cn/robot-calibration）

特性

支持 SDH 參數子集選擇（每關節 α/a/θ/d 可獨立選擇是否參與校準）。
Base→World 與 Tool→Flange 外參與 DH 參數 聯合估計。
殘差為 TCP 三維位置（mm），輸出統計與直方圖對比。
提供共垂線求解與可視化腳本，便于復核過程。

術語說明

訓練集（Training set）：用于本算法與 RoboDK 共同辨識幾何參數，包含 優化后 DH 參數、基座到世界坐標系位姿 T_base^world 與 TCP 到法蘭位姿 T_tcp^flange。由 3 個步驟的數據組成：
1. 基座坐標系構建：26 組關節序列；
2. TCP 構建：27 組關節序列；
3. DH 校準與坐標系優化：≥60 組關節序列。
測試集（Test set）：在將辨識結果寫入控制器后，用 ≥40 組 笛卡爾 TCP 位姿 驗證機械臂的絕對定位精度。
執行方式：訓練集與測試集均被轉換為 Staubli VAL3 離線程序以驅動機器人（本工程不提供相應 VAL3 程序）。
理論數值計算：以預設關節值經正向運動學求得 T_flange^base，再經坐標變換得到 T_tcp^world，與測量值作差得到位置殘差。

給定關節角 $J\mathbf{J}$ 與前向運動學公式：
校準前（缺省 SDH）

$Ttcpworld=Tbaseworld?Tflangebase(J)(默認?SDH)?Ttcpflange\;\mathbf T_{\text{tcp}}^{\text{world}}\;=\;\mathbf T_{\text{base}}^{\text{world}}\;\cdot\;\underset{\text{(默認 SDH)}}{\mathbf T_{\text{flange}}^{\text{base}}(\mathbf J)}\;\cdot\;\mathbf T_{\text{tcp}}^{\text{flange}}\;$

校準后（優化 SDH）
$Ttcpworld′=Tbaseworld′?Tflangebase′(J)(優化?SDH)?Ttcpflange′\;{\mathbf T_{\text{tcp}}^{\text{world}}}'\;=\;{\mathbf T_{\text{base}}^{\text{world}}}'\;\cdot\;\underset{\text{(優化 SDH)}}{{\mathbf T_{\text{flange}}^{\text{base}}}'(\mathbf J)}\;\cdot\;{\mathbf T_{\text{tcp}}^{\text{flange}}}'\;$

校準效果

實驗對象：TX2-90L 機械臂

機械臂簡介與原廠精度

數據來源：robodk.com/robot/Staubli/TX2-90L

在這里插入圖片描述

Staubli 原廠絕對定位精度：全工作空間 mean 0.07 mm、90%max 0.11 mm；在 508×508×508 mm 立方體子域內 mean 0.05 mm、90%max 0.08 mm。
數據來源：代碼的工作路徑/doc/TX2-90L/AbsoluteCalibrationQualityReport_TX2-90L.pdf

校準對比與結論

絕對定位精度圖示（單位：mm）

絕對定位精度數據（單位：mm）
相同顏色為對比項。

算法	數據集	校準狀態	mean	max	90%max	σ	6σ	num of points
本算法	訓練集	校準前（理論數值）	0.466283	0.874976	0.647841	0.152851	0.917108	60
	訓練集	校準后（理論數值）	0.039163	0.098429	0.062382	0.017692	0.106153	60
	測試集	校準后（實際測量）	0.0509466	0.1020001	0.0838984	—	—	40
RoboDK	訓練集	校準前（理論數值）	0.466	0.875	—	0.154	0.929	60
RoboDK	訓練集	校準后（理論數值）	0.039	0.098	—	0.018	0.093	60

原始數據與報表見 代碼的工作路徑/RobotCalib/doc/TX2-90L/ 與 代碼的工作路徑/RobotCalib/results/TX2-90L/。

要點：

與 RoboDK 在同一訓練集上的理論結果一致量級。
寫入控制器后的實測精度位于 Staubli 原廠報告立方體子域水平附近。
測試集位姿分布與訓練集不同，誤差略有上浮，符合預期。

實驗對象：TX200 機械臂

機械臂簡介與原廠精度

數據來源：robodk.com/robot/Staubli/TX200

在這里插入圖片描述

Staubli 原廠絕對定位精度：全工作空間 mean 0.17 mm、90%max 0.26 mm；在 847×847×847 mm 立方體子域內 mean 0.13 mm、90%max 0.18 mm。
數據來源：代碼的工作路徑/doc/TX200/AbsoluteCalibrationQualityReport_TX200.pdf

校準對比與結論

絕對定位精度圖示（單位：mm）

絕對定位精度數據（單位：mm）
相同顏色為對比項。

算法	數據集	校準狀態	mean	max	90%max	σ	6σ	num of points
本算法	訓練集	校準前（理論數值）	1.285361	2.141703	1.673369	0.306290	1.837739	107
	訓練集	校準后（理論數值）	0.142412	0.487590	0.213881	0.071394	0.428367	107
	測試集	校準后（實際測量）	0.143818	0.501294	0.246057	—	—	44
RoboDK	訓練集	校準前（理論數值）	1.269	2.128	—	0.309	2.196	107
RoboDK	訓練集	校準后（理論數值）	0.142	0.488	—	0.072	0.358	107

原始數據與報表見 代碼的工作路徑/RobotCalib/doc/TX200/ 與 代碼的工作路徑/RobotCalib/results/TX200/。

要點：

與 RoboDK 在同一訓練集上的理論結果一致量級。
實測精度位于原廠全域與立方體子域之間；由于訓練/測試子域范圍較原廠報告的立方體子域更大，誤差略有放大屬預期。
測試集位姿分布更復雜，精度略低于訓練集理論值。

總體結論
在 TX2-90L 與 TX200 兩個樣例上，本算法成功復現了 RoboDK 的校準能力；寫入控制器后的實測絕對精度接近 Staubli 原廠校準水平。

環境依賴

C++17 編譯器（GCC 9+/Clang 10+/MSVC 2019+）
CMake 3.16+
Eigen 3
Ceres Solver（含 EigenQuaternionParameterization）
yaml-cpp
Python 3（可視化腳本：numpy、matplotlib）

Ubuntu 示例：

sudo apt update
sudo apt install -y build-essential cmake libeigen3-dev libyaml-cpp-dev libceres-dev \python3 python3-pip
pip3 install -U numpy matplotlib

macOS (Homebrew)：

brew install cmake eigen ceres-solver yaml-cpp
pip3 install -U numpy matplotlib

編譯

終端進入工程目錄

mkdir build && cd build
cmake .. && make -j8

可執行文件輸出：build/RobotCalibration

快速開始（內置樣例）

項目提供兩套樣例數據 TX2-90L / TX200，樣例數據是我在現場使用高精度設備采集的，并提供兩套校準模型選項 simple / complete,推薦使用complete校準模型，本文所有的校準數據均使用complete模型獲得：

# 運行樣例：TX2-90L + complete
./build/RobotCalibration TX2-90L complete# 運行樣例：TX200 + complete
./build/RobotCalibration TX200 complete

命令行參數：

Usage: RobotCalibration <robot_name: TX2-90L|TX200> <calib_mode: simple|complete>

程序會自動讀取：

DH：config/DH/<robot_name>-default.yml
選項：config/option/CalibConfig<Simple|Complete>.yml
測量：config/measured/<robot_name>/機器人校準-Calibration.csv（及 Base/Tool 初始化所需 CSV）

輸出默認寫入：results/<robot_name>/

輸出與可視化

運行結束后，results/<robot_name>/ 下包含：

OptimalReport_<robot_name>.txt：
- Base/Tool 外參（平移 + 四元數）
- 原始與優化后的 DH 表
- 逐樣本三維位置誤差及統計
accuracy_stats_hist_<robot_name>.png：校準前后的定位精度對比直方圖

示例，OptimalReport_TX2-90L.txt如下：

========================== Calibration Report ==========================[1]base2world Transformation( [X,Y,Z]mm|Quaternion[q1-q4] ):331.331991,  349.413195,  429.000644,  -0.000022,  0.002251,  0.001411,  0.999996[2]tool2flange Transformation( [X,Y,Z]mm|Quaternion[q1-q4] ):24.990101,  0.213438,  14.899634,  0.000000,  0.000000,  0.000000,  1.000000[3] Original SDH Parameters:
Joint   Alpha(deg)      a(mm)           theta(deg)      d(mm)           
------------------------------------------------------------------------
1       -90.000000      50.000000       0.000000        0.000000        
2       0.000000        500.000000      -90.000000      0.000000        
3       90.000000       0.000000        90.000000       50.000000       
4       -90.000000      0.000000        0.000000        550.000000      
5       90.000000       0.000000        0.000000        0.000000        
6       0.000000        0.000000        0.000000        100.000000      [4] Optimized SDH Parameters:
Joint   Alpha(deg)      a(mm)           theta(deg)      d(mm)           
------------------------------------------------------------------------
1       -89.976980      50.072944       0.000000        0.000000        
2       0.021435        499.915919      -90.052042      0.000000        
3       90.007793       -0.258274       90.043030       50.250723       
4       -90.017136      0.022018        0.113726        549.979965      
5       90.009449       -0.008709       -0.073612       -0.016915       
6       0.000000        0.000000        0.000000        100.000000      
------------------------------------------------------------------------[5] Measurement Errors (per group):測量1：誤差 = 0.026507 mm / 0.664059 mm（校準/未校準）測量2：誤差 = 0.022484 mm / 0.473525 mm（校準/未校準）測量3：誤差 = 0.048078 mm / 0.593382 mm（校準/未校準）測量4：誤差 = 0.017212 mm / 0.286039 mm（校準/未校準）測量5：誤差 = 0.027732 mm / 0.647841 mm（校準/未校準）測量6：誤差 = 0.032805 mm / 0.217326 mm（校準/未校準）測量7：誤差 = 0.035003 mm / 0.285724 mm（校準/未校準）測量8：誤差 = 0.047712 mm / 0.182410 mm（校準/未校準）測量9：誤差 = 0.079666 mm / 0.328047 mm（校準/未校準）測量10：誤差 = 0.063394 mm / 0.456054 mm（校準/未校準）測量11：誤差 = 0.034709 mm / 0.391084 mm（校準/未校準）測量12：誤差 = 0.022172 mm / 0.455564 mm（校準/未校準）測量13：誤差 = 0.036401 mm / 0.372025 mm（校準/未校準）測量14：誤差 = 0.030719 mm / 0.494504 mm（校準/未校準）測量15：誤差 = 0.054269 mm / 0.580226 mm（校準/未校準）測量16：誤差 = 0.034244 mm / 0.375517 mm（校準/未校準）測量17：誤差 = 0.008812 mm / 0.634352 mm（校準/未校準）測量18：誤差 = 0.035747 mm / 0.420587 mm（校準/未校準）測量19：誤差 = 0.048377 mm / 0.445601 mm（校準/未校準）測量20：誤差 = 0.062382 mm / 0.520417 mm（校準/未校準）測量21：誤差 = 0.043945 mm / 0.516382 mm（校準/未校準）測量22：誤差 = 0.028854 mm / 0.294653 mm（校準/未校準）測量23：誤差 = 0.040496 mm / 0.220989 mm（校準/未校準）測量24：誤差 = 0.012670 mm / 0.412269 mm（校準/未校準）測量25：誤差 = 0.098429 mm / 0.802622 mm（校準/未校準）測量26：誤差 = 0.034564 mm / 0.312927 mm（校準/未校準）測量27：誤差 = 0.057626 mm / 0.691680 mm（校準/未校準）測量28：誤差 = 0.017561 mm / 0.374001 mm（校準/未校準）測量29：誤差 = 0.037320 mm / 0.463712 mm（校準/未校準）測量30：誤差 = 0.050038 mm / 0.525567 mm（校準/未校準）測量31：誤差 = 0.038108 mm / 0.874976 mm（校準/未校準）測量32：誤差 = 0.046431 mm / 0.425734 mm（校準/未校準）測量33：誤差 = 0.027192 mm / 0.575715 mm（校準/未校準）測量34：誤差 = 0.016891 mm / 0.355442 mm（校準/未校準）測量35：誤差 = 0.048557 mm / 0.327223 mm（校準/未校準）測量36：誤差 = 0.056675 mm / 0.637538 mm（校準/未校準）測量37：誤差 = 0.069998 mm / 0.400602 mm（校準/未校準）測量38：誤差 = 0.086267 mm / 0.289902 mm（校準/未校準）測量39：誤差 = 0.021268 mm / 0.472069 mm（校準/未校準）測量40：誤差 = 0.035523 mm / 0.566289 mm（校準/未校準）測量41：誤差 = 0.035594 mm / 0.565018 mm（校準/未校準）測量42：誤差 = 0.036709 mm / 0.637583 mm（校準/未校準）測量43：誤差 = 0.026452 mm / 0.207000 mm（校準/未校準）測量44：誤差 = 0.024210 mm / 0.485523 mm（校準/未校準）測量45：誤差 = 0.051299 mm / 0.372913 mm（校準/未校準）測量46：誤差 = 0.053558 mm / 0.368482 mm（校準/未校準）測量47：誤差 = 0.036068 mm / 0.304884 mm（校準/未校準）測量48：誤差 = 0.022227 mm / 0.426705 mm（校準/未校準）測量49：誤差 = 0.045959 mm / 0.338290 mm（校準/未校準）測量50：誤差 = 0.064172 mm / 0.386015 mm（校準/未校準）測量51：誤差 = 0.026428 mm / 0.523266 mm（校準/未校準）測量52：誤差 = 0.017636 mm / 0.647093 mm（校準/未校準）測量53：誤差 = 0.022520 mm / 0.511791 mm（校準/未校準）測量54：誤差 = 0.032559 mm / 0.265368 mm（校準/未校準）測量55：誤差 = 0.031090 mm / 0.471388 mm（校準/未校準）測量56：誤差 = 0.031606 mm / 0.795395 mm（校準/未校準）測量57：誤差 = 0.036189 mm / 0.473452 mm（校準/未校準）測量58：誤差 = 0.035285 mm / 0.610204 mm（校準/未校準）測量59：誤差 = 0.055424 mm / 0.681401 mm（校準/未校準）測量60：誤差 = 0.027973 mm / 0.540605 mm（校準/未校準）[6]校準結果統計（位置誤差，單位：mm）mean       max        90%max     σ         6σ        number_of_points
------------------------------------------------------------------------------
校準前      0.466283   0.874976   0.647841   0.152851   0.917108         60
校準后      0.039163   0.098429   0.062382   0.017692   0.106153         60

示例，accuracy_stats_hist_TX2-90L.png如下：
在這里插入圖片描述

手動生成直方圖（可自定義輸入報告路徑）：

python3 code/scripts/visualize_optimal_report.py results/TX200/OptimalReport_TX200.txt

演示平面法向與法向可視化（示例腳本）：

python3 code/scripts/visualize_normal_plane.py

Docker（可選）

如需在容器內復現實驗：

# 構建
docker build -t robotcalib -f docker/Dockerfile .# 運行（掛載當前工程，啟用多核）
docker run --rm -it robotcalib \bash -lc "mkdir build && cd build && cmake .. && make -j8 && ./build/RobotCalibration TX2-90L complete"

如需導出圖片到宿主機，確保將 results/ 目錄掛載到宿主機路徑。

技術服務

機械臂絕對精度/外參校準實戰落地：提供線下的校準服務與線上的全量資料包（原理說明＋完整代碼＋實測數據＋軟件操作＋一線經驗）。線上可持續答疑。

商業合作與答疑可加V，

可選的機械臂校準資料包

【機械臂校準資料包鏈接1】
【機械臂校準資料包鏈接2】

線上的全量資料包（原理說明＋完整代碼＋實測數據＋軟件操作＋一線經驗）包括以下所有內容：

$ tree
.
├── 代碼
│   └── RobotCalib
│       ├── CMakeLists.txt
│       ├── README.md
│       ├── code
│       │   ├── include
│       │   │   ├── core
│       │   │   │   ├── BaseCalib.hpp
│       │   │   │   ├── NormalCrossCompute.h
│       │   │   │   ├── RobotCalib.h
│       │   │   │   └── ToolCalib.hpp
│       │   │   └── tools
│       │   │       ├── DataReader.h
│       │   │       ├── DataStas.h
│       │   │       ├── Forward.h
│       │   │       └── MatrixCompute.h
│       │   ├── scripts
│       │   │   ├── visualize_normal_plane.py
│       │   │   └── visualize_optimal_report.py
│       │   └── source
│       │       ├── core
│       │       │   ├── NormalCrossCompute.cpp
│       │       │   └── RobotCalib.cpp
│       │       ├── main.cpp
│       │       └── tools
│       │           ├── DataReader.cpp
│       │           ├── DataStas.cpp
│       │           └── MatrixCompute.cpp
│       ├── config
│       │   ├── DH
│       │   │   ├── TX2-90L-default.yml
│       │   │   └── TX200-default.yml
│       │   ├── measured
│       │   │   ├── TX2-90L
│       │   │   │   ├── TX2-90L_絕對精度驗證結果（測試集）.xlsx
│       │   │   │   ├── 機器人校準-BaseSetup.csv
│       │   │   │   ├── 機器人校準-Calibration.csv
│       │   │   │   └── 機器人校準-ToolSetup.csv
│       │   │   └── TX200
│       │   │       ├── TX200_絕對精度驗證結果（測試集）.xlsx
│       │   │       ├── 機器人校準-BaseSetup.csv
│       │   │       ├── 機器人校準-Calibration.csv
│       │   │       └── 機器人校準-ToolSetup.csv
│       │   └── option
│       │       ├── CalibConfigComplete.yml
│       │       └── CalibConfigSimple.yml
│       ├── doc
│       │   ├── TX2-90L
│       │   │   ├── AbsoluteCalibrationQualityReport_TX2-90L.pdf
│       │   │   ├── RoboDK校準報告-TX2-90L.pdf
│       │   │   ├── RoboD校準位置精度截圖-TX2-90L.png
│       │   │   ├── TX2-90L簡介.png
│       │   │   └── 原廠校準位置精度截圖-TX2-90L.png
│       │   ├── TX200
│       │   │   ├── AbsoluteCalibrationQualityReport_TX200.pdf
│       │   │   ├── RoboDK校準報告-TX200.pdf
│       │   │   ├── RoboDK校準位置精度截圖-TX200.png
│       │   │   ├── TX200簡介.png
│       │   │   └── 原廠校準位置精度截圖-TX200.png
│       │   └── 計算共垂線的算法原理.md
│       ├── docker
│       │   └── Dockerfile
│       └── results
│           ├── TX2-90L
│           │   ├── OptimalReport_TX2-90L.txt
│           │   └── accuracy_stats_hist_TX2-90L.png
│           └── TX200
│               ├── OptimalReport_TX200.txt
│               └── accuracy_stats_hist_TX200.png
└── 文檔├── robodk軟件使用說明│   └── robodk進行機械臂校準的流程.vsdx├── 校準原理說明│   └── 工業六軸機械臂標定校準原理說明.docx└── 高級工程師經驗分享└── staubli機械臂標定校準實施標準操作流程（真實工作經歷吐血總結）.docx28 directories, 49 files
(base)