说明：本次内容基于本博客四篇文章《基于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:算法和仿真搭建模块可以查找本人博客中的相关内容，本片文章不再做阐述，谢谢大家，后续会继续推出更有趣的机器人内容！！！
1664
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
