新松機器人：【示教編程】+【PC離線編程】+【PLC遠程IO】相結合

項目背景

先說一下項目背景，項目要求潮溼的工業環境，在保證機器人末端跟流水線保持相對靜止的情況下，控制末端執行器沿$XZ$平面內的點進行移動。

功能要求：

1. 機器人末端執行器實現對工廠流水線的隨動，其中流水線沿下圖$Y$軸方向運動；
2. 傳感器實時獲取目標點在$XZ$平面內的信息；
3. 在隨動的同時，使用傳感器獲取的目標點信息指導末端執行器運動。

已知條件：

1. 傳感器獲取的點只有$X$、$Z$二維座標，即$P(x,z)$；
2. 運動起點的座標可以設定，如$P_0(x_0,y_0,z_0)$；
3. 沿Z軸的運動速度可以給定，如$v_z$；

思路

面對這種工況，一般有兩種思路：完全採用離線編程來實現、採用示教編程+離線編程結合實現，接下來分別進行介紹。

思路1：完全採用離線編程

完全採用離線編程來實現上述功能，具體實現流程如下：

• 編碼器獲取流水線的速度$v_y$；
• 傳感器獲取運動點的座標$P(x,z$)；
• 實現隨動：計算從點$P_0$運動到點$P(x,z)$的時間差：$t = (z-z_0)/v_z$，由此計算出該點的$Y$座標：$y=y_0+t*v_y$；
• 工控機通過離線編程，控制機器人末端執行器按照補全後的點$P(x,y,z)$進行運動，完成功能；

思路2：示教編程+離線編程

該思路使用了機器人的示教點偏移功能(SHIFTON)和補償隨動功能(AMENDON)，考慮到待加工的對象結構、尺寸等都差不多，所以示教一套具有普適性的軌跡點作爲標準點，機器人沿這些標準點進行運動；如果傳感器獲取的工件尺寸與標準尺寸的有偏差，則使用示教點偏移功能在原來的軌跡點上進行一定的偏移，補全偏差即可。而補償隨動功能只需要給定隨動的速度，則機器人在運動時，會自動將隨動速度補償到相應的軌跡點上，實現隨動功能。

示教點偏移功能

具體流程如下：

• 依次示教$N$個點的座標，作爲標準點，寫進示教作業裏；
• 編碼器獲取流水線速度$v_y$；
• 傳感器獲取運動點的座標$P(x,z$)；
• 計算傳感器獲取的點與示教的標準點之間的座標差值；
• 工控機通過離線編程將編碼器獲取的速度$v_y$以及$N$個點的$X$、$Z$座標差寫入到示教盒內(通過修改“位置變量”的方式)；
• PLC通過操作系統IO的方式，實現執行主作業，完成功能；

對比

• 思路1：完全依賴軟件進行運動規劃、控制，工控機作爲控制核心、PLC作爲功能模塊，自己“造輪子”，不可控因素較多；
• 思路2：運動控制寫在示教作業裏，提高了運行的穩定性；同時PLC作爲控制核心，工控機則作爲了一個功能模塊，不參與系統的運動控制；採用了工業上現有的、成熟的機器人補償和隨動功能，簡化了系統流程、降低了風險。

實現

示教作業：

1. 子作業1：BACK

/*
功能：回原位
*/
NOP
MOVL P100 VL=200 CP=1 ACC 100 // P100:原位
RET
END

2. 子作業2：PROCESS

/*
功能：設置軌跡點的位置變量Px爲相應的用戶變量Rx；並按位置變量指定的點進行運動
*/
NOP
// 設置軌跡點的x/y/z座標(其實只需要x、z就夠了，y可賦值爲R2)
AMEND SET P99  				//  將位置變量賦值給amend規劃
AMENDON F#1 M=1				// 實時補償，#F1：機器人基座標系M1：絕對補償
MOVL P1 VL=200 CP=1 ACC 100	// 1代表連續，ACC：最大加速度百分比
MOVL P2 VL=200 CP=1 ACC 100
MOVL P3 VL=200 CP=1 ACC 100
MOVL P4 VL=200 CP=1 ACC 100
AMENDOFF
RET
END

3. 作業名：MAIN

/*
功能：主作業，根據條件選擇執行作業
*/
NOP
CALL IN#1=ON	BACK  	 // 回原位
CALL IN#3=ON	PROCESS  // 運動	
DELAY T=0.2		// 延時0.2s（0.1-999.0s）
END

離線編程:

#include <iostream>    
#include "stdafx.h"   
#include "RobotSDK.h"  

#define VARIABLESIZE 6

using namespace std;

int main()
{
	// 接口初始化操作：操作成功返回0，操作失敗返回-1.
	int ErrorCode = RobotInit();
	if (ErrorCode == 0)
	{
		std::cout << "Robot Initilize success" << std::endl;
	}
	else
	{
		std::cout << "Robot Initilize failed, return code is " << ErrorCode << std::endl;
	}

	//機器人UDP連接，IP爲192.168.3.150，端口號爲9999.
	char* cIP = "192.168.3.150";
	short port = 9999;
	int connectID = ConnectUDP(cIP, port);
	if (connectID>0)
	{
		std::cout << "Robot connect success" << std::endl;
	}
	else
	{
		std::cout << "Robot connect failed, return code is " << connectID << std::endl;
	}
/*
	// 關閉安全門（必需操作）。
	ErrorCode = Send_SafeDoorOFF(connectID);
	if (ErrorCode == 0)
	{
		std::cout << "close safe door success" << std::endl;
	}
	else
	{
		std::cout << "failed" << connectID << std::endl;
	}	
	// 打開作業
	char *jobName = "VARIABLE";
	ErrorCode = OpenJob(jobName, connectID);
	if (ErrorCode == 0)
	{
		cout << "成功打開作業" << endl;
	}
	else
	{
		cout << "打開作業失敗" << endl;
	}
	
	// 報警復位
	ErrorCode = Send_Reset(connectID);
	if (!ErrorCode)
	{
		cout << "報警復位成功" << endl;
	}
	else {
		cout << "報警復位失敗" << endl;
	}

	//清除緩存
	ErrorCode = ClearBuffer(connectID);  //ClearBuffer()函數用於清空機器人控制器中的緩存。
	if (ErrorCode == 0)
	{
		std::cout << "Clear buffer successful!" << std::endl;
	}
	else
	{
		std::cout << "Clear buffer failed, return code is " << ErrorCode << std::endl;
	}

	//伺服上電
	ErrorCode = Send_PowerOn(connectID);
	Sleep(2000);	// 延遲2s，等待上電完成

	if (ErrorCode == 0)
	{
		std::cout << "Servo on power" << std::endl;
	}
	else
	{
		std::cout << "Servo power failed, return code is " << ErrorCode << std::endl;
	}
*/
	// 設置位置變量
	int poseID[VARIABLESIZE] = { 100,99,1,2,3,4 };	// 待修改位置變量的ID
	RPY poseRPY[VARIABLESIZE];	// 位置變量數組，存儲相應點的位姿

	float exAxis[3] = { 0,0,0 }; //外部軸關節值
	int coordinate = 1; //座標系選擇，1：世界座標系；2：用戶座標系；3：工具座標系
	int TOOLNUM = 1; //工件座標系選擇
	int USERNUM = 1; //用戶座標系選擇

	/*
		此處對poseRPY[i]進行操作，修改各點的位姿
	*/
	for (int i = 0; i < VARIABLESIZE; i++)
	{
		ErrorCode  = WriteUserVarRPYS(poseID[i], poseRPY[i], exAxis, coordinate, TOOLNUM,USERNUM, connectID);
	}

	if (ErrorCode == 0)
	{
		std::cout << "成功設置浮點型用戶變量" << std::endl;
	}
	else
	{
		std::cout << "設置浮點型用戶變量失敗" << std::endl;
	}
/*
	// 外部啓動
	ErrorCode = Send_Start(connectID);
	if (ErrorCode == 0)
	{
		std::cout << "Main operation is opened" << std::endl;
	}
	else
	{
		std::cout << "Main operation failed, return code is " << ErrorCode << std::endl;
	}
*/
	// 斷開與機器人的連接
	ErrorCode = Disconnect(connectID);
	if (ErrorCode == 0)
	{
		std::cout << "Disconnect success" << std::endl;
	}
	else
	{
		std::cout << "Disconnect failed, return code is " << ErrorCode << std::endl;
	}

	system("pause");
	return 0;
}

備註

1. 上述代碼實現，並沒有使用SHIFTON進行示教點偏移，因爲SHIFTON一般只能把所有點作爲一個整體進行偏移，對於每個點的每個座標偏移量都不相同的場景不太適用。這裏是直接把目標點的座標直接賦值到位置變量裏，並沒有進行偏移。
2. 離線編程代碼部分不用打開作業也可修改用戶變量，而且當機器人在作業MAIN中循環等待時，也無法對作業進行操作，否則會報錯：“執行中無法操作作業”；
3. AMENDON功能默認沒有被添加進機器人RC系統中，需要聯繫新松工作人員添加。
4. 注意時序問題，需要在離線編程修改完位置變量以後，才能執行PROCESS子作業；否則PROCESS子作業中的P點是上一次修改後的P點，而不是本次的P點。