機(jī)器人基于視覺(jué)的控制（機(jī)器人控制方向與機(jī)器視覺(jué)方向）

本文是自己學(xué)習(xí)《機(jī)器人學(xué)、機(jī)器人視覺(jué)與控制》最新版本的筆記，里面涉及到機(jī)器人坐標(biāo)轉(zhuǎn)換非常詳細(xì)的算法，使用matlab工具軟件舉例。

分為二個(gè)部分：

1、思維方式概述

2、二維坐標(biāo)系的位置姿態(tài)轉(zhuǎn)換詳解

按照思維習(xí)慣由淺入深的講解，為了完成我做一個(gè)扔垃圾機(jī)器人的夢(mèng)想，已經(jīng)學(xué)習(xí)這個(gè)基礎(chǔ)知識(shí)好幾遍了；夢(mèng)想還是比較遠(yuǎn)，但是過(guò)程中可以學(xué)到非常多的知識(shí)，過(guò)程比目的地更有趣。

有這方面興趣的朋友，又可以靜心學(xué)習(xí)的可以深入看看！學(xué)習(xí)沒(méi)有捷徑，該學(xué)的知識(shí)點(diǎn)，術(shù)語(yǔ)，符號(hào)都得搞懂，不然進(jìn)入不了下個(gè)階段的學(xué)習(xí)！

第一部分：思維方式概述

概述捋順一下理解位姿的思維方式

空間中的點(diǎn)是數(shù)學(xué)中常見(jiàn)的概念，可以用坐標(biāo)向量來(lái)描述，向量是既有大小又有方向的一種量；

我剛開(kāi)始接觸的機(jī)器人的時(shí)候，有個(gè)疑問(wèn)，為什么要用向量來(lái)表示呢，直接用坐標(biāo)形式x/y/z數(shù)值不是更好理解嗎，后來(lái)多看了幾遍資料，發(fā)現(xiàn)我認(rèn)為的坐標(biāo)數(shù)據(jù)，其實(shí)在內(nèi)部算法里都是用向量矩陣表示的，所以想深入學(xué)習(xí)機(jī)器人，還是要用向量的思維去看點(diǎn)，比如說(shuō)下圖a的點(diǎn)P，直接就把它看成是向量P，兩個(gè)分向量Py和Px相加的和，Py和Px又是有多個(gè)單位向量組成！單位向量就是長(zhǎng)度為1的向量; 這個(gè)時(shí)候思路應(yīng)該轉(zhuǎn)變成向量思路了，比如P坐標(biāo)（2，3），不應(yīng)該理解成，X軸上是數(shù)據(jù)2，Y軸上是數(shù)據(jù)3；而是理解成P點(diǎn)的X軸坐標(biāo)是2個(gè)單位向量組成，Y軸坐標(biāo)是3個(gè)單位向量組成！

坐標(biāo)系里的一個(gè)點(diǎn)好表示，但是大多數(shù)情況下，是坐標(biāo)系里的一個(gè)物體，而這個(gè)物體又包含很多點(diǎn)；

分兩步分析：

第一，確定這個(gè)物體的位置和方向，如圖b，向量B表示小車的位置和方向；

第二步，以這個(gè)B點(diǎn)為原點(diǎn)，設(shè)定小車自己的坐標(biāo)系，這樣就可以知道小車上1、2、3、4這四個(gè)點(diǎn)的位置和方向，如圖b用四個(gè)向量表示！

接下來(lái)要約定一下用符號(hào)表示坐標(biāo)系相互關(guān)系的方法；

以前我都是忽略這些約定符號(hào)，感覺(jué)看著累，不直觀，也不容易理解；但是越深入學(xué)習(xí)越感覺(jué)這些符號(hào)公式的重要性，因?yàn)楹?jiǎn)單的可以直觀理解，或者用語(yǔ)言就可以描述，但是簡(jiǎn)單的是基礎(chǔ)，最終都會(huì)參與到復(fù)雜的運(yùn)算，復(fù)雜的運(yùn)算只能靠符號(hào)和公式解決，不然會(huì)毫無(wú)章法！

利用上面這些約定，畫(huà)個(gè)如下圖所示的坐標(biāo)系，有兩個(gè)坐標(biāo)系{A}，和{B}；

坐標(biāo)系和向量的關(guān)系可以寫(xiě)成下面的公式，表示A坐標(biāo)系下的P向量；

可以分兩步表示，先表示以A坐標(biāo)系作為參考坐標(biāo)系表示B坐標(biāo)系；

然后點(diǎn)乘B作為參考坐標(biāo)系下的P向量；

上面例子表示出一個(gè)重要思路，那就是一個(gè)坐標(biāo)系可以用另一個(gè)坐標(biāo)系表示，這在機(jī)器人學(xué)里面是應(yīng)用非常多的一個(gè)思路，要記住這個(gè)思路，這就說(shuō)明機(jī)器人實(shí)際應(yīng)用中，無(wú)論工件上設(shè)定的點(diǎn)在哪里，只要它有自己的工件坐標(biāo)系，工件坐標(biāo)系可以和機(jī)器人基坐標(biāo)系關(guān)聯(lián)起來(lái)，那么這個(gè)工件上的點(diǎn)也就可以用機(jī)器人基坐標(biāo)系表示！

所以機(jī)器人坐標(biāo)系可以千變?nèi)f化，但是機(jī)器人最終計(jì)算的只有一組相對(duì)于世界坐標(biāo)系的數(shù)值。

更復(fù)雜點(diǎn)的三個(gè)坐標(biāo)系關(guān)聯(lián)；

坐標(biāo)系{A},{B},{C},如下圖所示：

那么向量P的表示可以用下面的公式，A坐標(biāo)系下的向量P可以分三步表示；

首先表示A坐標(biāo)系下的B坐標(biāo)系；

然后再表示B坐標(biāo)系下的C坐標(biāo)系；

最后表示C坐標(biāo)系下的P向量；公式中間那個(gè)加號(hào)加圓圈代表坐標(biāo)系轉(zhuǎn)換關(guān)系。

接下來(lái)再舉個(gè)三維坐標(biāo)系的例子

幫助大家理解坐標(biāo)系的轉(zhuǎn)換關(guān)系；如下圖所示，在世界坐標(biāo)系{O}里有一個(gè)機(jī)器人坐標(biāo)系為{R}，機(jī)器人上安裝一臺(tái)相機(jī)，相機(jī)坐標(biāo)系為{C}，屋頂上有個(gè)固定的相機(jī)，固定相機(jī)坐標(biāo)系為{F}，還有一個(gè)工件，工件坐標(biāo)系為{B}；

如下面兩個(gè)變換等式；

第一個(gè)等式表示，世界坐標(biāo)系下{O}的固定相機(jī)坐標(biāo)系{F}，變換到固定相機(jī){F}坐標(biāo)系下的工件坐標(biāo)系{B}，等于 世界坐標(biāo)系{O}下的機(jī)器人坐標(biāo)系{R}，變換到機(jī)器人{(lán)R}坐標(biāo)系下的機(jī)器人相機(jī)坐標(biāo)系{C}，再變換到機(jī)器人相機(jī){C}坐標(biāo)系下的工件坐標(biāo)系{B};

第二個(gè)等式表示，世界坐標(biāo)系{O}下的固定相機(jī)坐標(biāo)系{F}，變換到固定相機(jī){F}坐標(biāo)系下的機(jī)器人坐標(biāo)系{R}，等于 世界坐標(biāo)系{O}下的機(jī)器人坐標(biāo)系{R};上圖坐標(biāo)系和下面等式中大家看到世界坐標(biāo)系下的坐標(biāo)系{F}，{R}沒(méi)有左上標(biāo)，那是因?yàn)槎x了世界坐標(biāo)系可以不寫(xiě)左上標(biāo)！

把上面的三維坐標(biāo)系關(guān)系簡(jiǎn)化成下圖所示：

上面的變換關(guān)系在實(shí)際應(yīng)用中還是很多地方用到的，比如一個(gè)移動(dòng)機(jī)器人帶著個(gè)相機(jī)的項(xiàng)目，就可以精確地抓取到工件了！

第二部分，詳解二維空間的位姿描述

二維坐標(biāo)系變換概述，需要的三個(gè)參數(shù)

用向量的思維，二維坐標(biāo)系下的一個(gè)點(diǎn)P，應(yīng)該用向量P表示，向量P又可以用X軸和Y軸的單位向量表示；如下圖的公式，因?yàn)閱挝幌蛄块L(zhǎng)度（模）為1，所以向量P是X個(gè)X軸單位向量與Y個(gè)Y軸單位向量的和；

下圖表示了一個(gè)紅色坐標(biāo)系和一個(gè)藍(lán)色坐標(biāo)系的關(guān)系，圖中明顯可以看出，紅色坐標(biāo)系{B}是藍(lán)色坐標(biāo)系{A}，通過(guò)向量t=（x,y）移動(dòng)得到；然后再逆時(shí)針旋轉(zhuǎn)θ角度。

由上可以看出，二維坐標(biāo)系下，以坐標(biāo)系{A}為參考坐標(biāo)系，坐標(biāo)系{B}的位置與姿態(tài)的確定，需要三個(gè)向量，（x,y,θ）,這三個(gè)向量，涉及到三角函數(shù)的計(jì)算，無(wú)法直接用這三個(gè)值轉(zhuǎn)換坐標(biāo)系，所以下面我們用一種方法來(lái)表示旋轉(zhuǎn)；

旋轉(zhuǎn)變換的推導(dǎo)

看上圖，任意點(diǎn)P相對(duì)于坐標(biāo)系{A}和{B}的向量分別是AP和BP，我們要確定AP和BP之間的關(guān)系，要分兩步走，旋轉(zhuǎn)然后平移；

具體如上圖所示，先只考慮旋轉(zhuǎn)的情況，點(diǎn)P在參考坐標(biāo)系{V}中，用單位向量組合可表示為如下等式：

上式最終寫(xiě)成行向量和列向量的點(diǎn)積形式，矩陣的概念馬上要得到應(yīng)用了；假設(shè)上圖中坐標(biāo)系{V}和{B}的軸長(zhǎng)都是1，也就是單位向量，那么坐標(biāo)系{V}經(jīng)過(guò)旋轉(zhuǎn)θ角度后，得到坐標(biāo)系{B},坐標(biāo)系{B}上X軸和Y軸的單位向量可表示為：

寫(xiě)成矩陣的形式如下：

這個(gè)單位向量旋轉(zhuǎn)的關(guān)系就出來(lái)了，

接著點(diǎn)P在參考坐標(biāo)系{B}中，用單位向量組合可表示為如下等式：

代入上式得：

和等式結(jié)合運(yùn)算，最后可以寫(xiě)成

這個(gè)等式就是表示坐標(biāo)系旋轉(zhuǎn)后，參考坐標(biāo)系{B}里的P點(diǎn)，怎么變換成參考坐標(biāo)系{V}里的P點(diǎn)；等式中間的二維矩陣就叫旋轉(zhuǎn)矩陣，用符號(hào)表示為：

上式可以寫(xiě)成符號(hào)表示：

同樣的道理，坐標(biāo)系旋轉(zhuǎn)后，坐標(biāo)系{V}里的點(diǎn)變換為坐標(biāo)系{B}里的點(diǎn)，公式可以表示成：

上式也可以看出逆矩陣就是旋轉(zhuǎn)矩陣上下標(biāo)互換；

MATLAB模擬旋轉(zhuǎn)變換程序

下面用號(hào)稱可以節(jié)省生命的超級(jí)神器MATLAB來(lái)做一下旋轉(zhuǎn)矩陣的模擬：

>> R=rot2(0.2)
R=0.9801 -0.1987
0.1987 0.9801

這段程序里rot2（θ）是2*2的旋轉(zhuǎn)矩陣，θ的單位是弧度！

旋轉(zhuǎn)矩陣有一些性質(zhì)可以用下面這個(gè)函數(shù)實(shí)驗(yàn)下：

>> det(R)
ans=1
>> det(R*R)
ans=1

旋轉(zhuǎn)矩陣的行列式是1，旋轉(zhuǎn)矩陣乘以旋轉(zhuǎn)矩陣的行列式也是1；

這里的det（A）是矩陣行列式，結(jié)果是矩陣A的行列式；

什么是行列式？

分三步來(lái)理解：

第一步、行列式det（A）是針對(duì)n*n的矩陣A而言的，A表示一個(gè)n維空間到n維空間的線性變換，線性變換就是壓縮或拉伸；假設(shè)原來(lái)空間有個(gè)n維的立方體，經(jīng)過(guò)線性變換變成一個(gè)新的n維立方體。

第二步、原來(lái)立方體體積是V1,新的立方體體積是V2！

第三步、V2除以V1就是det(A)的值。

工具箱也支持用符號(hào)代替數(shù)字計(jì)算，如下聲明一個(gè)符號(hào)theta：

>> syms theta
>> R=rot2(theta)
R=[ cos(theta), -sin(theta)]
[ sin(theta), cos(theta)]

矩陣的指數(shù)和對(duì)數(shù)計(jì)算

下面還是把0.3的弧度轉(zhuǎn)換為旋轉(zhuǎn)矩陣；

>> R=rot2(0.3)
ans=0.9553 -0.2955
0.2955 0.9553

我們可以用logm函數(shù)計(jì)算矩陣的對(duì)數(shù)

>> S=logm(R)
S=0.0000 -0.3000
0.3000 0.0000

然后用工具箱函數(shù)vex來(lái)解析S矩陣；

> > vex(S)
ans=0.3000

得出的結(jié)果是又算出了旋轉(zhuǎn)角度的弧度值0.3；

矩陣對(duì)數(shù)logm的倒數(shù)（逆）是矩陣的指數(shù)函數(shù)expm；

>> expm(S)
ans=0.9553 -0.2955
0.2955 0.9553

結(jié)果得到了原來(lái)的旋轉(zhuǎn)矩陣，還原了logm計(jì)算之前的數(shù)值；

矩陣的對(duì)數(shù)也就是矩陣指數(shù)的倒數(shù)（逆）！矩陣的指數(shù)或?qū)?shù)的計(jì)算還是比較復(fù)雜的，我覺(jué)得我會(huì)用這個(gè)函數(shù)就行了，想深入了解的要去看看《李群論》！

進(jìn)而推導(dǎo)出二維的齊次變換矩陣

上面講了旋轉(zhuǎn)矩陣，這里旋轉(zhuǎn)后再加上平移講一下坐標(biāo)系原點(diǎn)之間的平移，二維坐標(biāo)系平移，就是簡(jiǎn)單的向量加法結(jié)合下面兩圖得出：

進(jìn)而可以簡(jiǎn)寫(xiě)成：

上公式中t=（x,y）,方向是{A}到{B}的變換；

再寫(xiě)成P點(diǎn)的變換：

其中的T就是齊次變換矩陣：

MATLAB函數(shù)計(jì)算齊次變換 矩陣

下面用MATLAB工具箱里面的函數(shù)具體計(jì)算一下;

程序是表示平移（1，2），再旋轉(zhuǎn)30度，得到其次變換矩陣T1;

>> T1=transl2(1, 2) * trot2(30, 'deg')
T1=0.8660 -0.5000 1.0000
0.5000 0.8660 2.0000
0 0 1.0000

transl2函數(shù)表示二維坐標(biāo)系下的平移，trot2函數(shù)表示二維坐標(biāo)系下的旋轉(zhuǎn)；

MATLAB圖形化輸出程序：

Plotvol(0 5 0 5)表示創(chuàng)建一個(gè)X軸長(zhǎng)度為5，Y軸長(zhǎng)度也為5的二維坐標(biāo)系；

trplot2(T1, ‘frame’, ‘1’, ‘color’, ‘b’) 表示建立一個(gè)坐標(biāo)系T1，顯示坐標(biāo)系名稱為1，顏色是b，藍(lán)色；

>> plotvol([0 5 0 5]);
>> trplot2(T1, 'frame', '1', 'color', 'b')

如下圖所示的藍(lán)色坐標(biāo)系：

接著再創(chuàng)建一個(gè)T2坐標(biāo)系，只從原點(diǎn)平移（2，1）；輸出圖形為上圖的紅色坐標(biāo)系：

>> T2=transl2(2, 1)
T2=1 0 2
0 1 1
0 0 1
>> trplot2(T2, 'frame', '2', 'color', 'r');

再創(chuàng)建一個(gè)綠色的坐標(biāo)系T3，它的其次變換是T1*T2；也就是先平移（1，2），再旋轉(zhuǎn)30度，再在坐標(biāo)系{1}的基礎(chǔ)上平移（2，1），上圖中可以看出最終綠色坐標(biāo)系{3}的位置并不是（3，3），那是因?yàn)榈诙纹揭剖窃谧鴺?biāo)系{1}的基礎(chǔ)上平移的！程序如下：

>> T3=T1*T2
T3=0.8660 -0.5000 2.2321
0.5000 0.8660 3.8660
0 0 1.0000
>> trplot2(T3, 'frame', '3', 'color', 'g');

在創(chuàng)建一個(gè)T4，和T3對(duì)比下，T4的變換是T2*T1，變換順序就相反了，它是先平移(2,1),再平移（1，2），再旋轉(zhuǎn)30度；如上圖中的坐標(biāo)系{4}所示，程序如下：

>> T4=T2*T1;
>> trplot2(T4, 'frame', '4', 'color', 'c');

圖中可見(jiàn)，其次變換矩陣左乘和右乘的結(jié)果是完全不同的;

我們?cè)賱?chuàng)建一個(gè)點(diǎn)P（3，2）；

如上圖所示；程序如下：

>> P=[3 ; 2 ];
>> plot_point(P, 'label', 'P', 'solid', 'ko');

下面再來(lái)研究這個(gè)點(diǎn)P相對(duì)與各個(gè)坐標(biāo)系的坐標(biāo)

看看P點(diǎn)在坐標(biāo)系{1}里的表達(dá)，前面學(xué)到的術(shù)語(yǔ)在這里用到了，在坐標(biāo)系{0}里的P點(diǎn)，等于坐標(biāo)系{0}變換到坐標(biāo)系{1}，再表示坐標(biāo)系{1}下的P點(diǎn)，等式如下：

然后轉(zhuǎn)換一下，變成坐標(biāo)系{1}下面的P點(diǎn)坐標(biāo)的表達(dá)，也就是坐標(biāo)系{1}變換為坐標(biāo)系{0}，再點(diǎn)乘坐標(biāo)系{0}下的P點(diǎn)坐標(biāo)向量！

用程序表示為下，得出P點(diǎn)的其次變換形式，P點(diǎn)的矩陣?yán)锩婕觽€(gè)1就是為了和其次變換格式相同，函數(shù)inv是矩陣求逆；

>> P1=inv(T1) * [P; 1]
P1=1.7321
-1.0000
1.0000

也可以試試下面的形式，函數(shù)e2h是將歐幾里得坐標(biāo)系轉(zhuǎn)換成其次變換坐標(biāo)形式，h2e再逆轉(zhuǎn)換，把其次變換形式轉(zhuǎn)換成普通坐標(biāo)形式；

>> h2e( inv(T1) * e2h(P) )
ans=1.7321
-1.0000

詳細(xì)探究純旋轉(zhuǎn)的旋轉(zhuǎn)中心例子

首先創(chuàng)建一個(gè)X坐標(biāo)軸是-5到4，Y坐標(biāo)軸是-1到5的坐標(biāo)系{0}；再創(chuàng)建一個(gè)目標(biāo)坐標(biāo)系{X};函數(shù)eye是創(chuàng)建單位矩陣，下面程序中說(shuō)明T0是3*3的單位矩陣；

>> plotvol([-5 4 -1 5]);
>> T0=eye(3,3);
>> trplot2(T0, 'frame', '0');
>> X=transl2(2, 3);
>> trplot2(X, 'frame', 'X');

再創(chuàng)建一個(gè)2弧度的旋轉(zhuǎn)；

>> R=trot2(2);

輸出如下兩種組合的圖形，如下圖所示，坐標(biāo)系{RX}是圍繞坐標(biāo)系{O}原點(diǎn)旋轉(zhuǎn)2弧度（115度），坐標(biāo)系{XR}是圍繞坐標(biāo)系{X}旋轉(zhuǎn)2弧度，可以看出兩種不同的組合的輸出結(jié)果不同了吧！

>> trplot2(R*X, 'framelabel', 'RX', 'color', 'r');
>> trplot2(X*R, 'framelabel', 'XR', 'color', 'r');

深入點(diǎn)應(yīng)用，如果我們想繞任何一點(diǎn)旋轉(zhuǎn)該怎么做呢？

我們先創(chuàng)建任意一點(diǎn)C，如上圖所示，程序如下：

>> C=[1 2]';
>> plot_point(C, 'label', ' C', 'solid', 'ko')

然后寫(xiě)一下繞C點(diǎn)旋轉(zhuǎn)的程序，理解為先平移到C點(diǎn)，再按照旋轉(zhuǎn)矩陣R旋轉(zhuǎn)，再把矩陣?yán)锩娴拿總€(gè)元素變成負(fù)數(shù)，再平移,得到了{(lán)X}繞C點(diǎn)旋轉(zhuǎn)的坐標(biāo)系{XC},如上圖所示；這個(gè)在實(shí)際應(yīng)用中很有用，比如想讓機(jī)器人繞抓手上任意一點(diǎn)旋轉(zhuǎn)，TCP的理論基礎(chǔ)！

>> RC=transl2(C) * R * transl2(-C)
RC=-0.4161 -0.9093 3.2347
0.9093 -0.4161 1.9230
0 0 1.0000

輸出圖形結(jié)果：

>> trplot2(RC*X, 'framelabel', 'XC', 'color', 'r');

扭轉(zhuǎn)函數(shù)Twist

繞任意點(diǎn)旋轉(zhuǎn)在機(jī)器人工具箱里有個(gè)更方便的函數(shù)Twist，例如我們想創(chuàng)建上面的繞C點(diǎn)的旋轉(zhuǎn)2弧度，可以寫(xiě)成，

>> tw=Twist('R', C)
tw=( 2 -1; 1 )

Twist函數(shù)里面包含很多參數(shù)，我們看看它生成的旋轉(zhuǎn)矩陣是啥樣的：

>> tw.T(2)
ans=-0.4161 -0.9093 3.2347
0.9093 -0.4161 1.9230
0 0 1.0000

是不是和上面一堆公式推導(dǎo)出的RC旋轉(zhuǎn)矩陣是一樣的，這就是MATLAB工具箱的牛逼地方，節(jié)省生命的工具！我覺(jué)得會(huì)用這些函數(shù)就好，如果應(yīng)用到實(shí)際機(jī)器上，MATLAB可以轉(zhuǎn)換成很多種語(yǔ)言，包括PLC的ST語(yǔ)言！

二維坐標(biāo)系的姿態(tài)描述就這么多了，連翻譯帶學(xué)習(xí)了皮特的《機(jī)器人學(xué)、機(jī)器人視覺(jué)與控制》，受益匪淺，二維的適合移動(dòng)小車機(jī)器人的算法，三維也會(huì)用到，對(duì)于自己開(kāi)發(fā)一款運(yùn)動(dòng)機(jī)器人，是很重要的基本功；實(shí)際工業(yè)機(jī)器人使用中也很有用，我想寫(xiě)個(gè)坐標(biāo)系轉(zhuǎn)換的算法，就到這里來(lái)調(diào)用函數(shù)，再生成需要的語(yǔ)言，稍微改動(dòng)下就可以使用，可以增加效率！