PLC案例：写工业机器人轨迹自动校正算法实现

摩擦焊刀头

工业机器人项目经常会有工具发生偏移，例如焊接机器人的焊枪，涂胶机器人的胶头，激光切割机器人的激光头，机械加工机器人的刀具等！不能因为小小的偏差就重新编写轨迹！下面给大家介绍一个自动修正枪头偏移数据的案例程序,给需要的朋友提供个思路！

以下分三部分详细解析：

1、项目整体思路；

2、算法分析；

3、ST程序举例；

焊接机器人

项目整体思路介绍

此次举例为焊接机器人，焊枪头自动修正的案例；

主要逻辑，机器人焊枪有小的碰撞，或者重新换了焊枪头，焊枪头相对于机器人法兰盘，有了一定的偏移！我需要把偏移的数据计算出来，补偿到轨迹里，保证轨迹不会跑偏！

用一个X型对射传感器，枪头通过触碰激光线，发出触碰信号的同时，记录机器人的坐标，让机器人枪头走直线，触碰激光线四次，可以得出二元一次线性方程组！计算这个方程组得到一个坐标，即可枪头求出偏差值！

下面就是算法分析详细讲解！

X型校正工具

具体算法分析部分

如下图所示，机器人走直线经过两条激光线，通过刚触碰激光线和从激光线离开，可以得到

P21/P22/P11/P12/P31/P32/P41/P42这八个坐标点！想通过方程组计算，我只需要触碰激光和离开激光的中间点，即图中的P100/P200/P300/P400这四个点！

运动点位图示

如下图所示，P100/P200/P300/P400四个点可以得到两条直线方程，Y=K1X+B1;

Y=K2X+B2;通过解这个方程组的到交叉点P1000的坐标；（初中的二元一次方程组，挺简单吧！）；

具体使用逻辑是：

1、机器人安装枪头后，先按照下图的两条枪头轨迹走一遍，得出的值为标定值；这个值校定后不可更改；

2、后面如果有枪头偏移的情况，就再走一遍下图轨迹，得出新的值，新的值减去标定值，即可得出修正值！一般都是周期性自动调用校正程序，保证轨迹不偏移！

方程组

程序举例

我用ST大概讲解下写法，首先定义变量，如下两个图，为机器人坐标系结构体和程序变量声明部分！

机器人坐标的结构体

程序变量声明

下面就是解二元一次方程组的步骤；可以手动在草稿纸上写下这个方程，下面程序只是把草稿纸上的步骤写成程序表达！如下图所示：

解方程组的算法

结束语：

上面是PLC计算，相同的方法可以机器人自己内部写程序计算，根据实际需求来，都可以！

程序只是举例核心算法部分，实际使用需要一些变量和数据的逻辑判断！核心算法很简单，思路正确了，程序很好写，实际应用的判断逻辑反而更加复杂！

如果想要精度更高的数据，还要反方向再运行一次，取中间值；

如果想精确到两个小数点以内，首先机器人精度要保证，其次要用到时间戳，时间戳可以把信号触发时间存储在本地模块上，然后把时间分割成比如一亿份，可以做到纳秒级别的精度！当然大部分工况是用不到这么高的精度，机械精度就很难达到，得上千万，上亿的设备才能用到！

（涉及领域:电子、电气/电工（考证题库）、通信、维修、自动化、程序等）

点个「赞+在看+分享+收藏」4连