說明：本次内容基于本部落格四篇文章《基于C#的機器人仿真平台和機器人運動學算法實作》、《六軸機器人軌迹規劃（直線軌迹規劃，弧線軌迹規劃）——C#實作+ABB為例（規劃直接下發離線程式運動）》、《機器人仿真搭建(以ABB為例)》和《ABB_2600運動學》進行整合和Code部分重構和改進。
一、内容改進
- 本次内容新添加了控制子產品，控制界面命名為虛拟示教器。
- 通過對應的機器人算法将虛拟環境中的機器人位置姿态真實的反應到現實機器人中
- 通過虛拟示教器對機器人進行關節控制和逆解線性控制
- 添加了對機器人的多段軌迹規劃的功能
- 對虛拟機器人進行多段軌迹規劃并生成實際機器人的Rapid代碼
二、關節、線性控制部分
可視化界面：
仿真環境：
通信子產品：
正向規劃子產品(通過實際的資料規劃機器人不同軌迹)：
虛拟示教器
記錄虛拟機器人點位
參數設定子產品
生成實際機器人代碼
部分代碼：
/// <summary>
/// 初始化線性運動
/// </summary>
public void Init_T()
{
m_Theta[0] = 0.01;
m_Theta[1] = 0;
m_Theta[2] = 0;
m_Theta[3] = 0;
m_Theta[4] = 30;
m_Theta[5] = 0;
for (int i = 0; i < 6; i++)
{
m_Theta[i] = m_Theta[i] * (Math.PI / 180);
}
m_Theta[1] = m_Theta[1] + Math.PI / 2;
m_robotKine_form3.M_Trans_Mult(m_Theta);
T06_IK = m_robotKine_form3.Rob_Fkine();
T06_IK[0, 3] = T06_IK[0, 3] + addx - subx;
T06_IK[1, 3] = T06_IK[1, 3] + addy - suby;
T06_IK[2, 3] = T06_IK[2, 3] + addz - subz;
}
/// <summary>
/// 将關節步進值初始化為0
/// </summary>
public void Init_Zero_Joint()
{
addj1 = 0; subj1 = 0;
addj2 = 0; subj2 = 0;
addj3 = 0; subj3 = 0;
addj4 = 0; subj4 = 0;
addj5 = 0; subj5 = 0;
addj6 = 0; subj6 = 0;
}
/// <summary>
/// 将線性步進值初始化為0
/// </summary>
public void Init_Zero_Liner()
{
addx = 0; subx = 0;
addy = 0; suby = 0;
addz = 0; subz = 0;
addex = 0; subex = 0;
addey = 0; subey = 0;
addez = 0; subez = 0;
}
private void timer1_Tick(object sender, EventArgs e)
{
//lable_X.Text = XYZ_test[0].ToString();
//lable_Y.Text = XYZ_test[1].ToString();
//lable_Z.Text = XYZ_test[2].ToString();
//label_Q1.Text = Math.Round((Quate_test[0] / 2), 5).ToString();
//label_Q2.Text = Math.Round((Quate_test[1] * 2), 5).ToString();
//label_Q3.Text = Math.Round((Quate_test[2] * 2), 5).ToString();
//label_Q4.Text = Math.Round((Quate_test[3] * 2), 5).ToString();
Init(addj1, subj1, addj2, subj2, addj3, subj3, addj4, subj4, addj5, subj5, addj6, subj6);
for (int i = 0; i < 6; i++)
{
m_Theta[i] = m_Theta[i] * (Math.PI / 180);
}
m_Theta[1] = m_Theta[1] + Math.PI / 2;
m_robotKine_form3.M_Trans_Mult(m_Theta);
T06 = m_robotKine_form3.Rob_Fkine();
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
R[i, j] = T06[i, j];
}
}
m_quate = m_robotKine_form3.T2Q(R);
lable_X.Text = Math.Round(T06[0, 3], 3).ToString();
lable_Y.Text = Math.Round(T06[1, 3], 3).ToString();
lable_Z.Text = Math.Round(T06[2, 3], 3).ToString();
label_Q1.Text = Math.Round((m_quate[0] / 2), 4).ToString();
label_Q2.Text = Math.Round((m_quate[1] * 2), 4).ToString();
label_Q3.Text = Math.Round((m_quate[2] * 2), 4).ToString();
label_Q4.Text = Math.Round((m_quate[3] * 2), 4).ToString();
}
三、控制執行個體
- 關節控制
- 逆解線性控制
- 多段軌迹規劃
- 生成Rapid代碼
- Rapid代碼上傳
部分控制代碼(核心算法為軌迹規劃和機器人運動學算法)：
public double[] info1 = new double[6];
private void timer3_Tick(object sender, EventArgs e)
{
Init_T();
info1 = m_robotKine_form3.Rob_Ikine(T06_IK);
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
R_Liner[i, j] = T06_IK[i, j];
}
}
m_quate_Liner = m_robotKine_form3.T2Q(R_Liner);
lable_X.Text = Math.Round(T06_IK[0, 3], 3).ToString();
lable_Y.Text = Math.Round(T06_IK[1, 3], 3).ToString();
lable_Z.Text = Math.Round(T06_IK[2, 3], 3).ToString();
label_Q1.Text = Math.Round((m_quate_Liner[0] / 2), 4).ToString();
label_Q2.Text = Math.Round((m_quate_Liner[1] * 2), 4).ToString();
label_Q3.Text = Math.Round((m_quate_Liner[2] * 2), 4).ToString();
label_Q4.Text = Math.Round((m_quate_Liner[3] * 2), 4).ToString();
label_J1.Text = Math.Round((info1[0]), 2).ToString();
label_J2.Text = Math.Round((info1[1]), 2).ToString();
label_J3.Text = Math.Round((info1[2]), 2).ToString();
label_J4.Text = Math.Round((info1[3]), 2).ToString();
label_J5.Text = Math.Round((info1[4]), 2).ToString();
label_J6.Text = Math.Round((info1[5]), 2).ToString();
}
private void button2_Click(object sender, EventArgs e)
{
Init_Zero_Joint();
flag_joint = 1;
flag_Liner = 0;
timer1.Enabled = true;
timer2.Enabled = true;
timer3.Enabled = false;
MessageBox.Show("控制器狀态切換為關節控制!");
}
private void button3_Click(object sender, EventArgs e)
{
Init_Zero_Liner();
flag_Liner = 1;
flag_joint = 0;
timer1.Enabled = false;
timer2.Enabled = false;
timer3.Enabled = true;
MessageBox.Show("控制器狀态切換為線性控制!");
}
private void button4_Click(object sender, EventArgs e)
{
m_form4.ShowDialog();
}
private void btn_Pos1_Click(object sender, EventArgs e)
{
if (flag_joint == 1)
{
T = T06;
MessageBox.Show("記錄位置1:" + "[" + T[0, 3] + " " + T[1, 3] + " " + T[2, 3] + "]");
}
else
{
T = T06_IK;
MessageBox.Show("記錄位置1:" + "[" + T[0, 3] + " " + T[1, 3] + " " + T[2, 3] + "]");
}
}
private void btn_Pos2_Click(object sender, EventArgs e)
{
if (flag_joint == 1)
{
T1 = T06;
MessageBox.Show("記錄位置2:" + "[" + T1[0, 3] + " " + T1[1, 3] + " " + T1[2, 3] + "]");
}
else
{
T1 = T06_IK;
MessageBox.Show("記錄位置2:" + "[" + T1[0, 3] + " " + T1[1, 3] + " " + T1[2, 3] + "]");
}
}
private void btn_Pos3_Click(object sender, EventArgs e)
{
if (flag_joint == 1)
{
T2 = T06;
MessageBox.Show("記錄位置3:" + "[" + T2[0, 3] + " " + T2[1, 3] + " " + T2[2, 3] + "]");
}
else
{
T2 = T06_IK;
MessageBox.Show("記錄位置3:" + "[" + T2[0, 3] + " " + T2[1, 3] + " " + T2[2, 3] + "]");
}
}
//直線規劃生成的機器人關節角度
public double[] info2 = new double[6];
/// <summary>
/// 兩點間的直線規劃
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void btn_MoveL_Click(object sender, EventArgs e)
{
timer2.Enabled = false;
timer3.Enabled = false;
flag_Liner_Speed = 1;
flag_joint = 0;
flag_Liner = 0;
flag_Liner_Path = 1;
flag_More_Path = 0;
m_linerPath.Plan_path(T[0, 3], T[1, 3], T[2, 3], T1[0, 3], T1[1, 3], T1[2, 3], Convert.ToDouble(m_form4.Num));
for (int i = 0; i < m_linerPath.x_start.Count; i++)
{
double[,] linepos = new double[4, 4];
for (int j = 0; j < 3; j++)
{
for (int k = 0; k < 3; k++)
{
linepos[j, k] = R[j, k];
}
}
linepos[0, 3] = m_linerPath.x_start[i];
linepos[1, 3] = m_linerPath.y_start[i];
linepos[2, 3] = m_linerPath.z_start[i];
m_quate_Pos = m_robotKine_form3.T2Q(R);
info2 = m_robotKine_form3.Rob_Ikine(linepos);
lable_X.Text = Math.Round(linepos[0, 3], 3).ToString();
lable_Y.Text = Math.Round(linepos[1, 3], 3).ToString();
lable_Z.Text = Math.Round(linepos[2, 3], 3).ToString();
label_Q1.Text = Math.Round((m_quate_Pos[0] / 2), 4).ToString();
label_Q2.Text = Math.Round((m_quate_Pos[1] * 2), 4).ToString();
label_Q3.Text = Math.Round((m_quate_Pos[2] * 2), 4).ToString();
label_Q4.Text = Math.Round((m_quate_Pos[3] * 2), 4).ToString();
Delay(10);
}
}
PS:算法和仿真搭模組化塊可以查找本人部落格中的相關内容，本片文章不再做闡述，謝謝大家，後續會繼續推出更有趣的機器人内容！！！