構像方程如何反求座標系

忙裡偷閒看到神舟12號上天，心裡甚是激動，想想手裡除錯的工業機器人，再看看太空的空間站，心裡咯噔一下，甚是慚愧，啥時候才能攀上高科技呢，不用在這行屍走肉般的搬磚哦。

自此開始發奮圖強，準備研究點深入的知識！誒！！你說巧不巧！今天研究的機器人知識點還真的能和宇宙飛船高科技掛鉤！！！！

今天研究下工業機器人座標系的計算演算法，宇宙的飛船也需要座標系，不然你不知道它在哪，原理估計和工業機器人差不多。好了！！開始正題！！！

天和空間站

機器人座標系，分為世界座標系，工件座標系，基座標系，工具座標系。按照計算方法，分為兩類。

1、 世界座標系，工件座標系，基座標系。為什麼把這些分為一類，因為這幾個座標系都是基於基座標系透過位姿轉換而來。基座標系是基於機器人底座的機械原點點座標系，也就是DH變換的起始點，永不改變。如圖1和圖2兩個機器人圖片，圖一是實體機器人，圖二是框架，看到機器人底座有基座標系。透過這個基座標系偏移旋轉，可以新建世界座標系，工件座標系。

2、 工具座標系，是基座標系加上6個軸位置引數，DH變換（以後詳解）而來，如圖2。

圖1

圖2

今天先從簡單地開始講，如圖3工件座標系Pi是怎麼透過基座標系Pj計算得到的。

圖3

第一步是旋轉，就拿二維座標系開始講解，容易理解，三維座標系同樣的原理。

如圖4，A點在座標系O裡繞原點旋轉β度後，得到A’點，假定OA長度為1。

圖4

得如下推理：

a

=1*

cosα =cosα 1

b

=1

sinα =sinα 2

a’=cos（α+β）

b’=sin（α+β）

a

′=

cosβcosα

−

sinβsinα 3

b

′=

sinβcosα

+

cosβsinα 4

解*1* *2* *3* *4*方程得：

a

′=

a*cosβ

−

b*sinβ

b

′=

a*sinβ

+

b*cosβ

寫成矩陣的形式是：

可理解為座標系O上一點A，在座標系繞原點旋轉度後得到A’。

例如我需要新建機器人工件座標系，位置和座標系原點重合，只是繞Z軸旋轉角度，那麼

旋轉矩陣

就是旋轉矩陣，座標系的X和Y數值乘以這個旋轉矩陣會得到新的座標系。

更復雜的情況，機器人想新建個座標系，不是和基座標系是同一個位置，有位置偏移和旋轉，如圖3，這種情況怎麼辦呢。

分三步處理的：

1、 新座標系和基座標系重合

2、 做個旋轉，用上面的旋轉矩陣

3、 旋轉變換後，進行位移，橫平豎直的位移。

旋轉矩陣有了，位移矩陣怎麼計算出來的，如下圖5，A平移到A’點：

圖5

a’= a+ta ；

b’= b+tb ；

寫成矩陣形式需要用到齊次變換（有），形式如下：

平移矩陣就是

平移矩陣

這個矩陣了。

按照剛才的邏輯，旋轉矩陣有了，平移矩陣有了，想要得到一個有平移有旋轉的座標系，是先旋轉，再平移，也就是基座標系的X和Y值先乘以旋轉矩陣

旋轉矩陣

再乘以平移矩陣

大家可以看出，一個是二階矩陣一個是三階矩陣，怎麼乘呢，好辦，把旋轉矩陣變成

變化後的旋轉矩陣

那麼座標系旋轉加平移地變換矩陣定義為M，則：

到此，機器人建立工件座標系的演算法就講解完成，大家多多關注我，後面給大家繼續詳解DH變換原理！！