原文：https://blog.csdn.net/qq_36552550/article/details/79409242

注：感謝固高長江研究院徐工程師的技術講解，以及matlab程式，機器人系統程式的提供。

在工業現場當中，相機拍攝到的影象有一個相機座標系，而機器人自身也有一個機器人自身的座標系，兩者互相獨立；當我們通過相機進行對物體進行拍攝，通過模式識別得到了目標物體在相機座標系下的座標位置，那麼如何通過計算來讓機器人知道目標物體在機器人座標系的位置，以便機器人進行抓取操作呢？

這一章，首先進行標定原理的講解，然後通過程式進行實現；最後，以洛陽理工學院實驗室的固高工業智慧相機及SCARA機械手為硬體基礎，進行實驗驗證。

10.1 標定原理

如圖10-1所示，OXY為機器人座標系，O’X’Y’是相機座標系。

圖10-1 機器人座標系與相機座標系示意圖

當相機獲取目標物體在相機座標系下的座標(imageP3.x,imageP3.y)時，如何進行計算得到對應在機器人座標系下的座標(robotP3.x,robotP3.y)呢？

（為了對應後面程式，所以座標名稱直接使用下文中的程式變數名）

首先，需要通過兩個已知座標點進行標定。P1點在機器人座標系下和相機座標系下的座標為(imageP1.x,imageP1.y)，(robotP1.x,robotP1.y)；P2點在機器人座標系下和相機座標系下的座標為(imageP2.x,imageP2.y)，(robotP2.x,robotP2.y)。

求得機器人座標系下的向量robotP1P2以及相機座標系下的向量imageP1P2:

imageP1P2.x = imageP2.x - imageP1.x

imageP1P2.y = imageP2.y - imageP1.y

robotP1P2.x = robotP2.x - robotP1.x

robotP1P2.y = robotP2.y - robotP1.y

然後，根據這兩個座標點求得現實距離與影象畫素之間的比例係數factor以及兩個座標系之間的偏轉角thetaRI。

factor = sqrt(robotP1P2.x*robotP1P2.x + robotP1P2.y*robotP1P2.y)/ sqrt(imageP1P2.x*imageP1P2.x + imageP1P2.y*imageP1P2.x)

偏轉角thetaRI的求取：如圖10-2所示。

圖10-2 偏轉角求取示意圖

先求得機器人座標系下向量robotP1P2相對於x軸的偏轉角thetaR，然後求得相機座標系下的向量imageP1P2相對於x軸的偏轉角thetaI；最後兩者相減就是兩個座標系的偏轉角thetaR。

thetaR = atan2((robotP2.y - robotP1.y) ,(robotP2.x - robotP1.x))

thetaI = atan2((imageP2.y - imageP1.y) , (imageP2.x - imageP1.x))

thetaRI = thetaR - thetaI

最後，根據比例係數factor以及偏轉角thetaRI就可以對相機座標系內的任意一個座標進行機器人座標系座標轉換。原理圖如圖10-3所示。

圖10-3 座標轉換示意圖

相機座標系下的座標(imageP3.x,imageP3.y)，那麼相機座標系下的向量imageP1P3為

imageP1P3.x = imageP3.x - imageP1.x

imageP1P3.y = imageP3.y - imageP1.y

還需要求得相機座標系下的相對於x軸偏轉角thetaN = atan2(imageP1P3.y, imageP1P3.x)。這樣通過圖10-3中的原理就可以得出機器人座標系下的座標了。

robotP3.x = factor*sqrt(imageP1P3.x*imageP1P3.x + imageP1P3.y*imageP1P3.y)* cos(thetaN + thetaRI) + robotP1.x

robotP3.y = factor*sqrt(imageP1P3.x*imageP1P3.x + imageP1P3.y*imageP1P3.y)* sin(thetaN + thetaRI) + robotP1.y

得到最終的robotP3座標就可以通訊給機器人讓機器人移動到該座標下，也就是目標物體的位置處。

10.2 標定程式實現

完整的程式實現如下所示：

1. /*

2. 機器人座標系與相機座標系轉換程式

3. 第一步：記錄A點的機器人座標系下座標以及影象座標系下座標，記錄B點的機器人座標系下座標以及影象座標系下座標；

4. 第二步：計算得到現實距離與影象畫素之間的比例係數，以及兩座標系的偏轉角；

5. 前兩步完成後，即完成了標定過程，接下來就可以根據一個新的點的影象座標系下的座標，求得該點的機器人座標點；

6. 第三步：輸入一個影象座標系下的點，程式自動得到機器人座標下的點。

7. */

8.  

9. #include<iostream>

10. #include<cmath>

11. using namespace std;

12.  

13. struct position {

14. double x;

15. double y;

16. };

17.  

18. struct vector {

19. double x;

20. double y;

21. };

22.  

23. int main()

24. {

25. //1 input the positions in the camera coodinate and robot coodinate

26. position imageP1, imageP2, robotP1, robotP2;

27. cout << "請輸入相機座標系下的座標" << endl;

28. cin >> imageP1.x >> imageP1.y >> imageP2.x >> imageP2.y;

29. cout << "請輸入機器人座標系下的座標" << endl;

30. cin >> robotP1.x >> robotP1.y >> robotP2.x >> robotP2.y;

31.  

32. //2 caculate the factor and tangle

33. vector imageP1P2, robotP1P2;

34. imageP1P2.x = imageP2.x - imageP1.x;

35. imageP1P2.y = imageP2.y - imageP1.y;

36. robotP1P2.x = robotP2.x - robotP1.x;

37. robotP1P2.y = robotP2.y - robotP1.y;

38.  

39. double factor, thetaI, thetaR, thetaRI;

40. factor = sqrt(robotP1P2.x*robotP1P2.x + robotP1P2.y*robotP1P2.y)/ sqrt(imageP1P2.x*imageP1P2.x + imageP1P2.y*imageP1P2.x);

41. thetaR = atan2((robotP2.y - robotP1.y) ,(robotP2.x - robotP1.x));

42. thetaI = atan2((imageP2.y - imageP1.y) , (imageP2.x - imageP1.x));

43. thetaRI = thetaR - thetaI;

44. cout <<"兩座標系的弧度偏離角為"<< thetaRI << endl;

45.  
46.  

47. //3 input a new postion in the camera coodinate, computer will caculate the postion information in the robot coodinate

48. position imageP3,robotP3;

49. cout << "請輸入一個新的影象座標系點" << endl;

50. cin >> imageP3.x >> imageP3.y;

51. vector imageP1P3;

52. imageP1P3.x = imageP3.x - imageP1.x;

53. imageP1P3.y = imageP3.y - imageP1.y;

54. double thetaN;

55. thetaN = atan2(imageP1P3.y, imageP1P3.x);

56.  

57. robotP3.x = factor*sqrt(imageP1P3.x*imageP1P3.x + imageP1P3.y*imageP1P3.y)* cos(thetaN + thetaRI) + robotP1.x;

58. robotP3.y = factor*sqrt(imageP1P3.x*imageP1P3.x + imageP1P3.y*imageP1P3.y)* sin(thetaN + thetaRI) + robotP1.y;

59.  

60. cout << "機器人座標系下的該點座標為：" << endl;

61. cout << robotP3.x << "," << robotP3.y << endl;

62.  

63. system("pause");

64. return 0;

65. }

 matlab計算程式如下：

1. clear; clc; close all;

2.  

3. Ar = [411.428 16.614]; % point A in robot coordinate

4. Br = [403.211 -109.381]; % point B in robot coordinate

5.  

6. Ai = [947 90]; % point A in image coordinate

7. Bi = [525 70]; % point B in image coordinate

8.  

9. AiBi = Bi - Ai

10. ArBr = Br - Ar

11.  

12. % calculate mm and pixel coefficient

13. MM_PER_PIXEL = norm(ArBr) / norm(AiBi)

14.  

15. % swap image coordinate xi and yi

16. theta_i = atan2d( AiBi(1), AiBi(2) )

17. % theta_i = atan2d( AiBi(2), AiBi(1) )

18. theta_r = atan2d( ArBr(2), ArBr(1) )

19.  

20. THETA_IR = theta_r - theta_i

21.  

22. %%

23. Ni = [627 333]; % new point in image coordinate

24. AiNi = Ni - Ai;

25. theta_ni = atan2d(AiNi(1),AiNi(2));

26.  

27. Nrx = MM_PER_PIXEL*norm(AiNi)*cosd(theta_ni+THETA_IR) + Ar(1);

28. Nry = MM_PER_PIXEL*norm(AiNi)*sind(theta_ni+THETA_IR) + Ar(2);

29.  

30. Nr = [Nrx Nry]

31.  
32.  
33.  

34. % robot = [428.8647 59.8150]

35. % z = 227.8875

36.  

10.3 工業智慧相機與SCARA機械手座標系的標定例項

硬體裝置為固高智慧相機以及固高系統的SCARA機械手，該例項演示整個標定過程並驗證標定效果。

以一枚硬幣作為目標物體，分別從放置P1點與P2點，分別得到兩個座標點在相機座標系以及機器人座標系下的位置，通過路由器將三者連線起來，並通過ping來測試三者是否通訊順暢。如圖10-4所示。

圖10-4 機器人，相機以及PC機連線示意圖

物體在相機座標系的座標資訊從固高智慧相機軟體中找到，如圖10-5a所示；

圖10-5 相機座標系下的座標獲取

物體在機器人座標系的座標資訊需要移動機器人至物體上方，然後從示教器介面中檢視位置資訊，如圖10-6所示；

圖10-6 機器人座標系下的座標獲取

通過兩個點的不同座標系的座標資訊輸入，計算得到了比例係數和偏轉角，最後放置目標物體在一個新的位置，輸入物體在相機座標系的座標，獲得物體在機器人座標系下的座標，如圖10-7所示。注意：相機座標系輸入座標的時候，需要將x與y座標對調，因為機器人座標系的z軸是向上的，根據右手定則相機座標系的z軸向下，所以相機座標系中軟體顯示的x座標實為y座標，y座標實為x座標。

圖10-7 座標求取結果示意圖

根據10-7所示，機器人座標系下該點座標應該為441.261,28.5031；然後，可以通過示教器直接進行點位移動來驗證。當然，更好的方式是通過示教程式設計，因為只有弄懂了示教程式設計，才能進行下一步的相機進行模式識別，然後將座標傳送給計算機，計算機通過軟體進行計算並把機器人座標系下的座標資訊傳送回機器人，機器人進行自動化跟蹤抓取。

示教程式如下：

程式解釋如下：首先將機器人移動至P87點（可隨意選取，主要是為了觀察機器人是否正常和接下來移動方便），使用str7來儲存441.261,28.5031；接下來就要建立通訊連線，str2儲存的字元為pc，pc在通訊設定中設定的是電腦端的IP地址等資訊；如圖10-8所示。

圖10-8 數值型變數字元型介面示意圖

在機器人等待接受資料的10秒間隔期間，將機器人座標系下的位置資訊通過TCP除錯助手傳送給機器人，儲存在str7中，然後將str7中的字元進行分割，分割的標誌為“,”，“,”前面的分割存入str8，“,”後面的分割存入str9，然後將其轉換為實型變數，並將其複製給P88點，然後讓機器人移動至P88點——即(441.261,28.5031)。具體的機器人程式設計請看《工業機器人控制系統使用者手冊.pdf》。

程式編寫完成後，開啟PC機的網路除錯助手，並輸入機器人端的IP地址以及埠號，以及待發送的內容，如圖10-9所示。

圖10-9 網路除錯助手通訊示意圖

觀察發現機器人移動至物體上方，如圖10-10所示，驗證完成。

圖10-10 機器人移動結果示意圖

觀察發現機器人與目標物體的位置間有小的偏差，原因一是在於機器人未進行精準的零位標定（主要原因）；二是因為對物體的座標選取都為目測得到。