六轴机器人轨迹规划（直线轨迹规划，弧线轨迹规划）——C#实现+ABB为例（规划直接下发离线程序运动）

机器人直线插补算法，弧线插补算法+离线编程转换（空间直线插补规划、空间弧线插补规划、离线编程、ABB二次开发、六轴机器人控制、C#）

一，通过对空间点的插补，形成空间点轨迹

1.插补算法原理简述：

（1）直线插补时，指定起止坐标与速度。
（2）确定要插直线的周期增量，分解到xyz轴

思路：直线插补采用简单线性插补即可，根据插补次数分别计算各轴步矩然后累加。

（MatLab代码和C#代码）

Matlab代码
% 计算插补点数
stepNum = sqrt((p3(1)-p1(1))^2+(p3(2)-p1(2))^2+(p3(3)-p1(3))^2)/step;
pos = zeros(4,stepNum+1);
    
fprintf("lineNum\n");
dx=(p3(1)-p1(1))/stepNum;
dy=(p3(2)-p1(2))/stepNum;
dz=(p3(3)-p1(3))/stepNum;

for t=0:1:stepNum
	pos(1,t+1)=p1(1)+dx*t;
	pos(2,t+1)=p1(2)+dy*t;
	pos(3,t+1)=p1(3)+dz*t;
end

class LinearPath
    {
        int number = 0;
        double LineLength = 0;
        double dx, dy, dz = 0;
        public List x_start = new List();
        public List y_start = new List();
        public List z_start = new List();
        public void clear()
        {
            x_start.Clear();
            y_start.Clear();
            z_start.Clear();
        }
        public void Plan_path(double xs,double ys,double zs,double xe,double ye,double ze,double speed)
        {
            clear();            
            x_start.Add(xs);
            y_start.Add(ys);
            z_start.Add(zs);
            LineLength = Math.Sqrt((xe - xs) * (xe - xs) + (ye - ys) * (ye - ys) + (ze - zs) * (ze - zs));
            number = Convert.ToInt32(LineLength / speed);
            dx = (xe - xs) / number;
            dy = (ye - ys) / number;
            dz = (ze - zs) / number;
            for(int i = 0; i < number; i++)
            {
                x_start.Add(x_start[i] + dx);
                y_start.Add(y_start[i] + dy);
                z_start.Add(z_start[i] + dz);
            }
        }

    }

2.弧线插补算法简述

1.将三维平面转换到二维平面，将三维问题转化为二维went。

2.三点确定一个圆，通过该三点确定新坐标系的方向（以方向向量的形式显示）

3.通过将该三点转换到新坐标上，然后计算圆心和相关半径。

Matlab和C#代码

v1 = p2 - p1;
v2 = p3 - p1;
if find(norm(v1)==0) | find(norm(v2)==0) %#ok
    fprintf('输入点不能一样\n');rad = -1;return;
end
v1n = v1/norm(v1);%坐标除以模长得到单位向量，该向量与原来方向向量相同
v2n = v2/norm(v2);
% 计算圆平面上的单位法向量
% 检查三点是否共线
nv = cross(v1n,v2n);
 if all(nv==0)
    fprintf('三个点共线\n');rad = -2;return;
 end
if find(sum(abs(nv),2)<1e-5)%查找nv总和是否小于1e-5
    fprintf('三点过于趋近直线\n');rad = -1;return;
end
% 计算新坐标系UVW轴
u = v1n;
w = cross(v2,v1)/norm(cross(v2,v1));%cross() return向量的叉积，即向量行列式[i j k;ax ay az;bx by bz];
v = cross(w,u);
% 计算投影
bx = dot(v1,u);
cx = dot(v2,u);
cy = dot(v2,v);
% 计算圆心
h = ((cx - bx/2)^2 + cy^2 -(bx/2)^2)/(2*cy);
center = zeros(1,3);
center(1,:) = p1(1,:) + bx/2.*u(1,:) + h.*v(1,:);

 public void Circelcenter_or_Rad(double[] p1,double[] p2,double[] p3)//形参输入采用字符串分割方法" ";
        {
             for(int i = 0; i < 3; i++)
            {
                v1[i] = p2[i] - p1[i];
                v2[i] = p3[i] - p1[i];
            }
             for(int i = 0; i < 3; i++)
            {
                v1n[i] = v1[i] / Math.Sqrt(v1[0] * v1[0] + v1[1] * v1[1] + v1[2] * v1[2]);//v1的单位向量
                v2n[i] = v2[i]/ Math.Sqrt(v2[0] * v2[0] + v2[1] * v2[1] + v2[2] * v2[2]);//v2的单位向量
            }
            nv[0] = v1n[1] * v2n[2] - v1n[2] * v2n[1];
            nv[1] = v1n[2] * v2n[0] - v1n[0] * v2n[2];
            nv[2] = v1n[0] * v2n[1] - v1n[1] * v2n[0];
            if (nv[0] == 0 & nv[1] == 0 & nv[2] == 0)
            {
                MessageBox.Show("Three points collinear!");return;
            }
            //else if (Math.Abs(nv[0]+nv[1]+nv[2])< 1.0000e-05)
            //{
            //    MessageBox.Show("Three points too close to straight line!");return;
            //}
            else
            {
                U = v1n;
                //W = cross(v2,v1)/norm((cross(v2,v1)));
                cW[0] = v2[1] * v1[2] - v2[2] * v1[1];
                cW[1] = v2[2] * v1[0] - v2[0] * v1[2];
                cW[2] = v2[0] * v1[1] - v2[1] * v1[0];
                for(int i = 0; i < 3; i++)
                {
                    W[i] = cW[i] / Math.Sqrt(cW[0] * cW[0] + cW[1] * cW[1] + cW[2] * cW[2]);
                }
                //v = cross(w,u);
                V[0] = W[1] * U[2] - W[2] * U[1];
                V[1] = W[2] * U[0] - W[0] * U[2];
                V[2] = W[0] * U[1] - W[1] * U[0];

                //bx = dot(v1,u)
                bx = v1[0] * U[0] + v1[1] * U[1] + v1[2] * U[2];
                //cx = dot(v2,u)
                cx = v2[0] * U[0] + v2[1] * U[1] + v2[2] * U[2];
                //cy = dot(v2,v)
                cy = v2[0] * V[0] + v2[1] * V[1] + v2[2] * V[2];

                //h = ((cx-bx/2)*(cx-bx/2)+cy*cy-(bx/2)*(bx/2))/(2*cy);
                h = ((cx - bx / 2) * (cx - bx / 2) + cy * cy - (bx / 2) * (bx / 2)) / (2 * cy);
                for (int i = 0; i < 3; i++)
                {
                    m_center[i] = p1[i] + ((bx / 2) * U[i]) + (h * V[i]);
                }
                m_rad = Math.Sqrt((m_center[0] - p1[0]) * (m_center[0] - p1[0]) + (m_center[1] - p1[1]) * (m_center[1] - p1[1]) + (m_center[2] - p1[2]) * (m_center[2] - p1[2]));
            }
        }

弧线轨迹效果图

离线编程：机器人离线编程，是指操作者在编程软件里构建整个机器人工作应用场景的三维虚拟环境，然后根据加工工艺等相关需求，进行一系列操作，自动生成机器人的运动轨迹，即控制指令，然后在软件中仿真与调整轨迹，最后生成机器人执行程序传输给机器人。

二，机器人二次开发和离线编程

1.机器人二次开发和离线编程

（1）通过API和ABB机器人通讯

（2）通过API对机器人进行进行基础数据操作（文件传输，启停，上电下电等其他控制）

（3）通过程序进行轨迹规划并基于不同机器人的语言自动产生离线程序

附上操作界面（界面）

 

实际操作过程图（轨迹规划和生成离线程序）

 

离线程序保存

离线程序上传文件保存代码

 try
            {
                if (this.richTextBox1.Text == "")
                    return;
                saveFileDialog1.DefaultExt = "txt";
                saveFileDialog1.Filter = "Text files (*.txt)|*.txt|All files (*.*)|*.*|Rapidmod(*.mod)|*.mod";
                if (this.saveFileDialog1.ShowDialog() == DialogResult.Cancel)
                    return;
                string FileName = this.saveFileDialog1.FileName;

                if (saveFileDialog1.ShowDialog() == DialogResult.OK && FileName.Length > 0)
                {
                    // Save the contents of the RichTextBox into the file.
                    richTextBox1.SaveFile(saveFileDialog1.FileName, RichTextBoxStreamType.PlainText);
                    MessageBox.Show("文件已成功保存");
                }
            }
            catch (System.Exception ex)
            {
                MessageBox.Show("文件保存失败" + ex.ToString());
            }

实际操作视频

欢迎技术咨询同上篇文章！