最近找小魚問手眼標(biāo)定的非常多，但是小魚時間有限，拉了個小群大家討論，二維碼失效可以關(guān)注公眾號魚香ROS下方加小魚好友拉你進群。

手眼標(biāo)定原理及常用算法Tsai-Lenz介紹

大家好，我是小魚。

上幾節(jié)主要介紹了手眼標(biāo)定程序的使用，但沒有對手眼標(biāo)定原理進行介紹，所以本文主要介紹手眼標(biāo)定的原理，并介紹手眼標(biāo)定算法Tsai-Lenz(實際是作者的名字)的計算過程，以及AX=XB的推導(dǎo)過程。

• 下一篇文章將會使用Python實現(xiàn)該算法
• 該算法適用于將相機裝在手抓上和將相機裝在外部兩種情況
• 論文已經(jīng)傳到git上，地址：https://gitee.com/ohhuo/handeye-tsai
• 本文中所說的夾爪=機器人末端執(zhí)行器=機器人末端

如果你要進行手眼標(biāo)定，可以參考我的其他文章：

• 手眼標(biāo)定-基礎(chǔ)使用
• 手眼標(biāo)定-JAKA機械臂
• 手眼標(biāo)定-AUBO機械臂
• 手眼標(biāo)定-Aruco使用與相機標(biāo)定
• 手眼標(biāo)定-注意事項

如果上述程序使用過程中遇到問題，可以參考：

• 手眼標(biāo)定-常見問題排查

如果你對手眼標(biāo)定原理感興趣，可以參考以下文章：

• 機器人手眼標(biāo)定原理介紹（含詳細(xì)推導(dǎo)過程）使用Tsai-Lenz算法
• 手眼標(biāo)定算法TSAI_LENZ，眼在手外python代碼實現(xiàn)
• 手眼標(biāo)定算法Tsai-Lenz代碼實現(xiàn)（Python、C++、Matlab）

為什么需要手眼標(biāo)定？手眼標(biāo)定標(biāo)什么？

當(dāng)我們要使用機械臂結(jié)合視覺進行抓取時，通過相機獲取了物體在空間中的位姿信息，但此時的位姿信息是基于相機坐標(biāo)系的，并不能直接使用。
我們想讓機器人末端執(zhí)行器到達(dá)目標(biāo)位置，那就要知道目標(biāo)位置在機器人坐標(biāo)系下（一般指機器人底座）的位姿。

我們可以這樣推導(dǎo)出來：

目標(biāo)在機器人坐標(biāo)系下的位姿=目標(biāo)在相機坐標(biāo)系下的位姿—>相機在夾爪（末端）坐標(biāo)系下的位姿—>夾爪（末端）在機器人基坐標(biāo)系下的位姿

等式右邊的三個位姿我們怎么獲取呢？

• 目標(biāo)在相機中的位姿我們可以通過視覺識別程序獲得（可以參考手眼標(biāo)定-Aruco使用與相機標(biāo)定）
• 夾爪（末端）在機器人基坐標(biāo)系的位姿我們可以通過機械臂示教器或者配套的SDK獲得（關(guān)于JAKA和AUBO機械臂的姿態(tài)獲取程序可以參考手眼標(biāo)定-JAKA機械臂， 手眼標(biāo)定-AUBO機械臂）
• 相機在夾爪（末端）坐標(biāo)系下的位姿關(guān)系就是我們要通過手眼標(biāo)定程序計算的

所以為了讓機械臂能夠到達(dá)視覺所識別出來的空間位姿，就必須要知道相機和機械臂末端執(zhí)行器之間的位姿關(guān)系，手眼標(biāo)定就是標(biāo)定出機械臂末端和相機之間的位姿關(guān)系。

AX=XB是什么?有什么用？

繼續(xù)上圖：

上圖表示的是機械臂與相機坐標(biāo)系的變換關(guān)系，i代表i時刻的機器人末端、相機等之間的位姿關(guān)系，j表示j時刻它們的位姿關(guān)系。

注意：i、j時刻標(biāo)定板與機器人位置保持不變。

我們用H表示坐標(biāo)變換（H指homogeneous matrices 齊次變換矩陣）。
例如表示i時刻下夾爪（gripper）的坐標(biāo)變換：$$H_{gi}^{}$$

我們已知多組：

夾爪在機器人坐標(biāo)系中的坐標(biāo) $$H_g^{}$$

標(biāo)定板在相機坐標(biāo)系中的坐標(biāo) $$H_c^{}$$

求

夾爪和相機之間的位姿關(guān)系：$$H_{gc}^{}$$

并且我們知道標(biāo)定過程中，機器人基坐標(biāo)和標(biāo)定板之間的相對位置關(guān)系保持不變，所以有：
$$H_{rc}^i=H_g^i{H}_{gc}^i{H}_c^i\text{\hspace{1em}(1)}$$$$H_{rc}^j=H_g^j{H}_{gc}^j{H}_c^j\text{\hspace{1em}(2)}$$$$H_{rc}^{}=H_{rc}^j=H_{rc}^j{H}_{rc}^{}機器人基座到標(biāo)定板之間的變換關(guān)系\text{\hspace{1em}(3)}$$$$H_{gc}^i=H_{gc}^j=H_{gc}^{}兩次變換中夾爪與相機位姿關(guān)系不變\text{\hspace{1em}(4)}$$

可以得到：
$$H_{gi}^{}{H}_{gc}^{}{H}_{ci}^{}=H_{gj}^{}{H}_{gc}^{}{H}_{cj}^{}將ij寫到下面來$$
$$H_{gj}^{?1}{H}_{gi}^{}{H}_{gc}^{}=H_{gc}^{}{H}_{cj}^{}{H}_{ci}^{?1}先右乘H_{ci}^{?1}再左乘H_{gj}^{?1}\text{\hspace{1em}(6)}$$

我們通過已知條件可以得到：

Ci到Cj之間的轉(zhuǎn)換關(guān)系：$$H_{cij}^{}=H_{cj}^{}{H}_{ci}^{?1}\text{\hspace{1em}(6-1)}$$

Gi到Gj之間的轉(zhuǎn)換關(guān)系：$$H_{gij}^{}=H_{gj}^{?1}{H}_{gi}^{}\text{\hspace{1em}(6-2)}$$

Hgi這里使用的是相機中標(biāo)定板的位姿

通過上面兩個公式可以得到：
$$H_{gij}^{}{H}_{gc}^{}=H_{gc}^{}{H}_{cij}^{}\text{\hspace{1em}(7)}$$
設(shè)：
$$A=H_{gij}^{}B=H_{cij}^{}X=H_{gc}^{}$$
即可以得到：$$AX=XB\text{\hspace{1em}(8)}$$
我們的目標(biāo)是求出相機和夾爪之間的位姿關(guān)系即求出AX=XB中的X
下面講的Tsai-Lenz算法就是用來求解X的一種算法。

Tsai-Lenz算法求解步驟是什么？

問答

要求解$$H_{gij}^{}{H}_{gc}^{}=H_{gc}^{}{H}_{cij}^{}$$中的Hgc，首先要知道H是什么？

H 為齊次變換矩陣，它由3x3的旋轉(zhuǎn)矩陣和3x1的平移矩陣組成。
$$H_{cg}^{}=\left[\begin{array}{cc}{R}_{cg}^{}& T_{cg}^{}\\ 000& 1\end{array}\right]\text{\hspace{1em}(9)}$$

什么是兩步法？

所謂兩步法就是先求Hgc（相機和夾爪之間的齊次矩陣）的旋轉(zhuǎn)部分，再使用旋轉(zhuǎn)部分求出平移部分。
我們將$$H_{gij}^{}{H}_{gc}^{}=H_{gc}^{}{H}_{cij}^{}$$按{9)展開計算得
$$\{\begin{array}{ll}{R}_{gij}^{}{R}_{cg}^{}=R_{cg}^{}{R}_{cij}^{}& \\ (R_{gij}^{}?I)T_{cg}^{}=R_{cg}^{}{T}_{cij}^{}?T_{gij}^{}& \end{array}\text{\hspace{1em}(10)}$$

步驟

求解旋轉(zhuǎn)矩陣R:
在Tsai-Lenz論文中使用旋轉(zhuǎn)軸+旋轉(zhuǎn)角的方式來表示旋轉(zhuǎn)。作者使用了修正的羅德里格斯參數(shù)表示旋轉(zhuǎn)變換。
在齊次矩陣中的R表示一個旋轉(zhuǎn)矩陣,R的特征向量和特征值一定是它的旋轉(zhuǎn)軸和1。
我們可以定義R的旋轉(zhuǎn)軸為Pr(旋轉(zhuǎn)向量)，則有：$$R{P}_r^{}=P_r^{}$$有了旋轉(zhuǎn)軸和旋轉(zhuǎn)角我們就可以確定一個旋轉(zhuǎn)。
使用修正的羅德里格斯變換重新定義Pr:$$P_r^{}=2sin\frac{\theta }{2}[n1,n2,n3{]}^{T}0?\theta ?180\text{\hspace{1em}(11)}$$
接下來上公式，然后證明：
$$Skew(P_{gij}^{}+P_{cij}^{})P_{cg}^{{}^{\prime }}=P_{cij}^{}?P_{gij}^{}\text{\hspace{1em}(12)}$$$$P_{cg}^{}=\frac{2P_{cg}^{{}^{\prime }}}{\sqrt{1+|P_{cj}^{{}^{\prime }}{|}^2}}\text{\hspace{1em}(13)}$$
上面兩個公式告訴了我們$P_{cg}^{}$和$P_{cij}^{},P_{gij}^{}$之間的關(guān)系。注意我們需要兩組以上的數(shù)據(jù)才能求出$P_{cg}^{}$
現(xiàn)在我們證明上面的公式：

這張圖描述了各個向量之間的關(guān)系。從圖中，我們根據(jù)向量之間的關(guān)系，可以得到：
$$P_{cg}^{}\perp (P_{gij}^{}?P_{cij}^{})\text{\hspace{1em}(14)}$$
也可以通過代數(shù)方式證明：
$(P_{gij}^{}?P_{cij}^{}{)}^T{P}_{cg}^{}$
$=(P_{gij}^{}?P_{cij}^{}{)}^T{R}_{cg}^T{R}_{cg}^{}{P}_{cg}^{}$
$=(P_{gij}^T{R}_{cg}^T?P_{cij}^T{R}_{cg}^T)R_{cg}^{}{P}_{cg}^{}$
$=(R_{cg}^{}{P}_{gij}^{}?P_{gij}^{}{)}^T{P}_{cg}^{}$
$=[(R_{cg}^{}?I)P_{gij}^{}{]}^T{P}_{cg}^{}$
$=P_{gij}^T(R_{cg}^T?I{)}^T{P}_{cg}^{}$
$=0$

下次直接截圖補充，用markdown寫證明過程是真的累~
根據(jù)等式[14]可以得到：
(15)
同時我們可以得到：
$P_{gij}^{}?P_{cij}^{}$與$（P_{gij}^{}+P_{cij}^{})\times P_{cg}^{}$共線。
即：
$$P_{gij}^{}?P_{cij}^{}=s(P_{gij}^{}+P_{cij}^{})\times P_{cg}^{}\text{\hspace{1em}(16)}$$
這個特性我們從圖中就可以看出，也可以從等式[15]中得到。

接下來是非常核心的等式：
$$|P_{gij}^{}?P_{cij}^{}|=|（P_{gij}^{}+P_{cij}^{})\times P_{cg}^{{}^{\prime }}|\text{\hspace{1em}(17)}$$
$$P_{cg}^{{}^{\prime }}=\frac{P_{cg}^{}}{\sqrt{4?|P_{cg}^{}{|}^2}}\text{\hspace{1em}(18)}$$

證明過程：
（19）
通過公式[13],[16],[19],我們可以得到：
$$（P_{gij}^{}+P_{cij}^{})\times P_{cg}^{{}^{\prime }}=P_{gij}^{}?P_{cij}^{}\text{\hspace{1em}(20)}$$
使用skew來計算X乘，就可以得到等式[12]：
$$Skew(P_{gij}^{}+P_{cij}^{})P_{cg}^{{}^{\prime }}=P_{cij}^{}?P_{gij}^{}\text{\hspace{1em}(12)}$$

通過等式[12]和等式10我們就能實現(xiàn)手眼矩陣的計算，下一講我們將使用Python、C++、MATLAB來實現(xiàn)Tsai-Lenz手眼標(biāo)定算法。

作者介紹：

我是小魚，機器人領(lǐng)域資深玩家，現(xiàn)深圳某獨腳獸機器人算法工程師一枚

初中學(xué)習(xí)編程，高中開始學(xué)習(xí)機器人，大學(xué)期間打機器人相關(guān)比賽實現(xiàn)月入2W+（比賽獎金）

目前在輸出機器人學(xué)習(xí)指南、論文注解、工作經(jīng)驗，歡迎大家關(guān)注小智，一起交流技術(shù)，學(xué)習(xí)機器人

總結(jié)

以上是生活随笔為你收集整理的机器人手眼标定原理介绍（含详细推导过程）使用Tsai-Lenz算法的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。