《感測器外參標定總結》

0. 前言

無論是在影象測量或者機器視覺應用中，相機引數的標定都是非常關鍵的環節，其標定結果的精度及演算法的穩定性直接影響相機工作產生結果的準確性。因此，做好相機標定是做好後續工作的前提，提高標定精度是工作的重點所在。

外參由旋轉矩陣R和平移向量t組成。

旋轉矩陣的逆矩陣是它的轉置矩陣。

3 維旋轉的跡數等於 1 + 2 cos（θ），這可用來快速的計算任何 3 維旋轉的旋轉角。

旋轉矩陣常用於以下三種場景：

描述一個frame（座標系）相對於另一個frame間的位姿；（如：描述相鄰兩幀間的相機運動）；

描述一個point從一個frame變換到另一個frame；（其實點沒動，只是在兩個frame觀測時，座標不一樣而已；如：路標點點P在不同相機座標系下座標值不同，但在世界座標系下是固定的）；

描述一個point在同一個frame中的運動；（這裡的點是運動了的，在VSLAM中應用較少）。

已知旋轉矩陣，怎麼求解尤拉角呢？

當一個車上裝了多個/多種感測器，而它們之間的座標關係是需要確定的。這個工作可分成兩部分：內參標定和外參標定，內參是決定感測器內部的對映關係，比如攝像頭的焦距，偏心和畫素橫縱比（+畸變係數），而外參是決定感測器和外部某個座標系的轉換關係，比如姿態引數（旋轉和平移6自由度）。內參這一塊可由張正友標定方法來標定，有很多現成工具，比較簡單，這裡也給出一個連結。這一篇文章我們主要來講一下外參的標定。

常用術語

內參矩陣： Intrinsic Matrix 焦距： Focal Length

主點: Principal Point 徑向畸變: Radial Distortion 切向畸變: Tangential Distortion

旋轉矩陣： Rotation Matrices 平移向量： Translation Vectors 平均重投影誤差： Mean Reprojection Error

重投影誤差: Reprojection Errors

重投影點： Reprojected Points

1. 手眼標定

在SLAM領域中，目前不僅僅侷限於單個感測器的建圖，目前正在向GPS/IMU和攝像頭或者鐳射雷達的標定，雷達和攝像頭之間的標定這樣多感測器融合的趨勢發展。而

手眼標定是一個被標定方法普遍研究的

，一定約束條件下的問題：可以廣義的理解，一個“手”（比如GPS/IMU）和一個“眼”（鐳射雷達/攝像頭）都固定在一個機器上，那麼當機器運動之後，“手”和“眼”發生的姿態變化一定滿足一定的約束關係，這樣求解一個方程就可以得到“手”-“眼”之間的座標轉換關係，一般是AX=XB形式的方程。對於手眼標定而言，其系統分為兩種：

eye in hand，眼在手上，相機移動

以及

eye to hand，眼在手外，相機固定

手眼標定的目的就是獲取

機器人座標系

和

相機座標系

的關係，最後將

視覺識別

的結果轉移到

機器人座標系

下。（相機識別的東西座標，機器人要控制，當然是要基於機器人座標）

眼在手外，eye-to-hand：也即攝像頭安裝在手臂之外的部分，與機器人的基座（世界座標系）相對固定，不隨著機械臂的運動而運動；

eye to hand 眼在外

eye in hand 眼在手

眼在手上，eye-in-hand：也即攝像頭安裝在機械臂上，會隨著機械臂的運動而發生運動。

1.1 眼在手外

涉及到

四個座標系

：

{B}：機器人基座標系

【固定不變】

{E}：機器人末端連桿（與標定板固連的連桿）座標系

{K}：標定板座標系，（字母 B 和 C 都被佔用了，所以隨便選個 K）

{C}：相機座標系

【固定不變】

主要就是求解D

它們之間的座標變換：

A：機器人末端在機械臂基座座標系下的位姿，這其實就是機器人運動學正解的問題（已知，後面會補充）；

B：標定板在機器人末端座標系下的位姿，由於是隨便裝的標定板，所以未知；

C：相機在標定板座標系下的位姿，這個其實就是求解相機的外參（已知，後面會補充）；

D：相機在機器人基座標系下的位姿，

這就是我們想求的東西

，而 D=A·B·C

如上圖所示，我們讓機器人走兩個位置，保證這兩個位置都能使得攝像頭看到標定板，於是有：

根據：D1=A1

B

C1 D2=A2

B

C2

因為標定板與機械臂末端連桿是固定的，所以 B 變換也是固定不變的。

稍微變換一下：

這是一個典型的

AX = XB

問題，而且根據定義，其中的 X 是一個 4x4 齊次變換矩陣：

我們有了 A，B 和 C，就能直接求解得到我們想要的手眼標定結果 D 了。

1.2 眼在手上

我們會得到一堆跟前面情況類似的座標系（完全相同，就不贅述了）和座標變換關係（略有不同）：

A：機器人末端在機械臂基座座標系下的位姿，這其實就是機器人運動學正解的問題（已知，後面會補充）；

B：相機在機器人末端座標系下的位姿，這個變換是固定的，只要知道這個變換，我們就可以隨時計算相機的實際位置，所以

這就是我們想求的東西

；

C：相機在標定板座標系下的位姿，這個其實就是求解相機的外參（已知，後面會補充）；

D：相機在機器人基座標系下的位姿，實際使用過程中標定板並不存在，所以這個變換關係我們不關心。

這個跟前面類似，直接讓機械臂運動兩個位置，保證這兩個位置下都可以看到標定板。然後構建空間變換回路：

很好，又成了一個

AX=XB

問題。

相關變換

1）變換 A

：它是機械臂末端在機器人基座座標系下的位姿，其實就是機器人學裡面最基礎的運動學正解。

這個沒什麼好多說的，就是去翻機器人學的教程，然後用 DH 矩陣等方法來計算機器人運動學正解。如果是在 ROS 裡，可以直接透過 KDL 或者 TF 等來直接得到。

簡而言之：只要知道機器人在當前狀態下每個關節的角度，我們就

可以計算得到 A 變換

。

變換 C

：它是相機在標定板座標系下的位姿。其實就是相機的

外參

。

先解釋一下，對於一般相機而言，我們會用

小孔成像

模型來對其進行建模。

所以，

在相機座標系下

，空間中 3D 點 （X， Y， Z， 1）

c 與影象上對應 2D 點 （x y）

c 正好是滿足如下關係：

其中的變換矩陣，就是我們說的

內參

矩陣，包含相機焦距等只與相機內部結構有關的引數。

然後，我們一般描述物體，都是在

世界座標系

下的進行的，也即 （X， Y， Z， 1）_w ，而不是相機座標系（因為我們無法知道相機座標系的位置）。所以，我們需要先把物體座標從世界座標系變換到相機座標系。

於是，我們就得到了完整的相機模型：

相機模型有了，我們要做的就是先確定內參矩陣，然後根據不同時刻拍攝的標定板圖片來求取外參矩陣：

也即相機在世界座標系下的變換關係 C

。

求解AX=XB

這裡我們找到了一個博主使用SVD對矩陣的求解 這裡的A，B和X都是SE（3）中的元素，即一個座標系到另一個座標系的位姿變換。具體來說無論是A，B還是X，都可以寫作\left［ \begin{matrix} R & t\0 & 1\end{matrix} \right］的形式，其中R是旋轉矩陣（SO（3）群中的元素，滿足正交且行列式為1），t是任意一個實數列向量。寫開來就是

也就是等價於

我們很容易發現，如果我們先解方程（2），把 R_X 解出來，那麼方程（3）對 t_X來說就是一個線性方程了，我們可以直接透過最小二乘法求出t_X。實際上在絕大多數情況下我們都是用這種兩部法，先求 R_X，再求 t_X

我們主要看（2）。透過硬體上的測量，我們可以得到許多組 R_A和R_B ，也就是說我們其實要解這樣一個方程組：

其中{ R

{A1}，R

{A2}，…。，R

{An}}，{R

{B1}，R

{B2}，…，R

{Bn}}都是已知的旋轉矩陣， R_X 是一個未知的旋轉矩陣。 之後即可透過奇異值分解來構建線性問題，並透過SVD最下二乘法得到t_X

總結一下總體步驟就是

2. 感測器外參標定

在自動駕駛中，感測器與感測器之間的外參標定要麼是機械結構賦予的粗略值，或者就是透過聯合標定的形式實現外參的確定。這一類外參標定的手段以及策略能夠使開發者更輕易地實現感測器的標定工作。在不同感測器資料融合的過程中，資料在不同座標系下的轉換都需要使用2個座標系的外參，通常為旋轉矩陣R和平移矩陣T。

idar-imu外參標定工具lidar_align

https：//

github。com/miracle629/l

idar_align

這個專案是基於瑞士蘇黎世聯邦理工大學———自動駕駛實驗室再次開發的工作，其實現的步驟是：

首先（RT）^{-1}是imu初始座標系，imu做積分可獲得任一時刻相對初始位姿的位姿變換矩陣C，則（RT）^{-1}C（RT）表示此時刻鐳射雷達原點位姿在世界座標系中的表示，假設某一點在鐳射雷達座標系下的表示為S，則其在世界座標系下的絕對座標為（RT）^{-1}C（RT）S

點雲掃描前後2幀有很大一部分點雲重合，利用點雲配准算法，可以估計前後2幀之間的移動轉動，從而將每2幀之間的變換C_i估計出來，C_iS表示i點在世界絕對座標系下的座標。

我們使用

ICP最近鄰迭代演算法將2種表示方法描述的同一片點雲區域配準

，構建並最佳化最近鄰誤差，這裡不直接對獲得的laser座標系構建最佳化目標，而對座標系內的點雲構建最佳化目標，運用了

統計誤差平均效應，直至收斂

，獲得RT變換矩陣的估計值，從而獲得鐳射雷達與imu的外參變換矩陣。

lidar-camera的PnP標定方法

這點其實很直觀就可以想到，因為lidar提供3D的資料，而camera提供2D資料，在高翔博士的《視覺SLAM十四講》裡，PnP方法是作為視覺里程計裡一種估計相機位姿的方法提出的，其中3D點是在世界座標系下的描述，最終得到每個時刻camera的位姿（當然也是在世界座標系下的描述），

我們只要把世界座標系設定在lidar座標系之上，就變成了這裡的標定問題

，只是這裡我們的camera沒辦法相對於lidar座標系（即世界座標系）運動。而現存的開源方案也提供了合適的解決方法：

https：//

github。com/CPFL/Autowar

e/tree/master/ros/src/sensing/fusion/packages/autoware_camera_lidar_calibrator

這裡使用二維碼給lidar和camera對接完成標定。這裡其實變成了內參的標定選擇。而博主 miracle629也提出了自己的構想。

lidar-gps的標定方法

這部分相關資料較少，我們找到了環宇的相關資料，從資料中我們可以看到本質上仍然是最小二乘的解法。鐳射雷達與 GPS 座標系之間的標定，擬採用鐳射里程計的方法獲得鐳射雷達區域性的軌跡，然後透過鐳射雷達軌跡與 GPS 軌跡之間進行匹配，從而最佳化外參。 下圖反應的就是東風技術中心的前向鐳射雷達所建的區域性地圖以及在建圖過程中輸出的軌跡，藍色是鐳射雷達所輸出的軌跡，紅色是 GPS+IMU 所輸出的軌跡。從圖上可以看出，正因為鐳射雷達與 GNSS 系統存在座標系不統一的問題，使得對於同一個車輛的運動，兩個不同感測器輸出的軌跡形狀相似，但是存在一個 se3 的變換。這裡Apollo給出了一個示例。詳情也可以參考這篇文章，透過兩條軌跡的計算獲得四元數資訊

常用標定工具

OpenCV

OpenCV應該是使用最廣泛的相機內參標定工具了，它包含了Pinhole、Kannala-brandt及Ｍei的實現

對針孔模型的實現在，並有一個使用用例：連結

對Kanala-brandt模型的實現在modules/calib3d/src/fisheye。cpp檔案中，採用８引數，其distortion引數k 1 k 4 k1~k4

k

1

k

4對應於原文的k 2 k 5 k2~k5

k

2

k

5。官方文件

對Ｍei模型的實現在/opencv_contrib/modules/ccalib/src/omnidir。cpp檔案中，可參閱官方使用說明文件

2。

CamOdoCal

CamOdoCal是Lionel Heng博士在ETH博士後期間的工作，其中實現了本文中述及的所有模型，程式碼地址

用該工具箱標定開箱即用，作者已經實現好了標定示例。只是新版本Ubuntu系統上編譯可能有一些小坑，多是opencv_contrib沒找對路徑的鍋，改一改CMakeLists。txt就好。

3。Kalibr

Kaibr在SLAM領域比較出名，它提供多相機系統、相機-IMU系統互標定的功能

該庫也實現了Pinhole、Kannala-brandt及MEI模型（程式碼註釋中註明參考了CamOdoCal庫的實現），同時支援與不同畸變模型的組合，可參照其wiki說明。其中omni對應的是Mei模型，Spherical對應的是Kanala-brandt模型

3. 參考連結

古月居

https：//

guyuehome。com/35393

https：//

robot。czxy。com/docs/pcl

/chapter05/handeye/

https：//

blog。csdn。net/Sandy_WYM

_/article/details/83503861

https：//

blog。csdn。net/w_weixiao

tao/article/details/107786152

https：//

blog。csdn。net/miracle62

9/article/details/87854450