一种基于linux+QT的开放性控制系统

技术领域

本发明涉及开放性控制系统技术领域，具体为一种基于linux+QT的开放性控制系统。

背景技术

当前绝大部分的工业机器人控制器都是嵌入式的，基于无图形界面的，主要有下面几种主流开发方案:

方案1基于CODESYS为基础的PLC控制器开发，如倍福，汇川，禾川等，CODESYS是德国传统的运动控制厂商，有非常深厚的技术积累(基于IEC6113-3的标准PLC编程语言，以及各种通讯协议的主从站)，而且只做软件卖授权不做硬件，吸引了国内外的大量合作伙伴，但是一方面他不是开源系统构建的，另一方面他支持高级语言，C++的难度较大，工程师想要深入集成开源的类库，或者自主编写复杂算法的难度很大。

方案2基于LINUX或ARM，ZYNQ的嵌入式开发板位基础的RTOS实时系统开发，如越疆，节卡，傲博等机器人控制系统，这类实现方案，确实绕过了CODESYS的卡脖子问题，而且批量生产的软硬件成本很低，但是往往开发难度大，只适合大型公司有成熟研发团队，一般从立项到完成一两年起步，且没有图形化的编程系统，以及被定制的硬件系统，使得难以适应小型的机器人和总线开发案例，基本只适合自己公司内部使用，无法被定义成通用的机器人控制器让客户根据实际应用项目自己使用。

方案3基于VXWORKS等实时系统构建的机器人操作系统，如ABB，KUKA等国外品牌的机器人控制系统，这种方案虽然国内也有人机器人公司尝试，实际上比前面两种都更加受限，因为VXWORKS的系统虽然有很强的实时性，但是缺乏开放性，即便是例如新增一个串口驱动也非常困难，因此只适合极少数研发能力很强的公司，而且VXWORKS对通讯协议栈的集成难度也很大，并不像LINUX那样有非常丰富的开源或商用的组件可以灵活裁剪和替换，应该说生态是日益匮乏的，懂这方面的工程师也很难找到。

除了上述几种主流机器人控制器开发方案，也有一些其他方案，例如定制ETHERCAT主站芯片的方案，做机器人控制确实能达到很好的实时性，但是这种方案对于支持的主板有要求，非纯软件方案，从开发的效率和可移植性来看缺乏吸引力，成本问题也是始终无法跟开源方案相比的。

发明内容

针对现有技术的不足，本发明提供了一种基于linux+QT的开放性控制系统，具备开放性好，有图形截面，可二次开发特性好的优点，解决了目前普遍工业机器人控制器的开放性差，无图形界面，可二次开发特性差的问题。

为实现上述目的，本发明提供如下技术方案：一种基于linux+QT的开放性控制系统，包括底层架构、中间层架构和顶层架构；

其中，底层架构为丰富的开源、商用的算法包和各种类库，包括Eigen，ROS，OPENCV，OPENGL以及各种通讯协议栈；

中间层架构为行业的应用包，包括主从跟随，码垛，装配，视觉标定，力位控制；

顶层架构为图形化界面，支持第三方的界面集成，用QT实现。

进一步，所述底层架构是总线配置，使用两个全局变量TEST_ROBOT_TYPE和TEST_IOBOARD_TYPE来区分总线配置的硬件类型，组态配置好之后，需要实际获取从站PDO数据，可以直接复制IGH主站的ethercatcstruct-a0的终端输出，来实例化每个pdo的注册数据。

进一步，所述底层架构最关键的算法中，ETHERCAT配合PREEMPT实时补丁的DC同步机制是关键，每个伺服通信周期消耗剩余时间和计算下一周期时间的配合实现函数如下：

//send process data

ecrt_domain_queue(domainServoOutput)；

ecrt_domain_queue(domainServoInput)；

wait_send(master,PERIOD_NS)；

sync_dc_time(master)；

ecrt_master_send(master)；

calc_system_diff_time()；

wait_recv(&sleep_time)；

clock_gettime(CLOCK_TO_USE,&wakeupTime)；

wakeupTime＝timespec_add(wakeupTime,cycletime)；

clock_nanosleep(CLOCK_TO_USE,TIMER_ABSTIME,&wakeupTime,NULL)。

进一步，所述中间层架构中的算法层部分，除了常规的DH方程组的解法，还包括七轴机器人的旋量解法和奇异值分解SDV算法。

进一步，所述机器人逆解本质是解多元非线性方程组，综合牛顿-拉普森方法和雅克比，提出一种有效的迭代求方程组解的方法，基本逻辑如下：

1)先求AA

2)再将A

3)将矩阵A分解为A＝U∑V

4)最后将矩阵A写为A＝σ1u1v1T+σ2u2v2T+…σrurvrT。

进一步，所述迭代为梯度下降，雅可比反映了一种变化趋势，多元非线性方程组如果雅克比存在，能够找到极值点，所述迭代的每一步，产生一个更接近最终目标的结果，并且放入下次迭代初始值。

进一步，所述中间层架构中的算法层部分同样实现了牛顿欧拉的经典迭代方法，两种方法结合，用冗余手段保障计算结果充分可靠，首先外推DH点的角速度，得到的角速度公式为：

然后外推DH点的线加速度，线加速度的公式为：

然后外推质心处的线加速度、合力和合力矩，质心处的线加速度、合力和合力矩的公式分别为：

最后内推关节的合力和合力矩，内推关节的合力和合力矩的公式分别为：

进一步，所述得到的最终结果公式为：

进一步，所述顶层架构中的界面层是针对机器人开发的3D仿真+树形图编程，界面跟后台的实时线程和核心的函数放在一起，直接调用。

进一步，所述界面层一键切换脚本二次开发，适应高级开发人员，如果对图形界面编程不满意，或者部分函数需要手写，在执行之前可以手动修改。

与现有技术相比，本申请的技术方案具备以下有益效果：

该基于linux+QT的开放性控制系统，集成并开放的机器人算法和工艺包，在一套控制器上完成了典型四轴，六轴，七轴机器人的运动学，动力学，轨迹规划算法，兼容标准通信协议的工业和协作机器人，实现拖动示教，碰撞检测等功能，支持多机器人，多任务协同作业，在大批量部署的时候具备显著成本优势，支持典型应用包如码垛，涂胶，焊接，皮带线跟随，力传感器混合控制，2D，3D视觉标定，让机器人控制器操作更简单，应用部署和实现更快捷，适用场景更广泛，集成了AI的深度和强化学习训练集，可以完成AI常见任务如YOLO检测，轮廓识别，轨迹跟踪，动态补偿等。

跟底层算法一体化的图形化用户界面，图形编程环境和脚本交互支持，基于十多年的实际现场经验，深刻理解工程师需要的图形编程系统的理想效果，因此上位机编程系统基于QT的图形化拖拽方式，尽可能保留了用户使用习惯的基础上，增加了额外的创新点，例如在整体列表存放位置，变量列表，将同类功能指令合并来简化学习过程，为每一种工艺包和指令提供丰富的说明和范例辅助理解和掌握，此外为了适应高级开发，允许一键从图形界面切换到脚本编程，提高开放程度和灵活性，为了适应更多层次的开发，客户可以从他喜欢的高级语言如C#，C++等接口调用指令完成项目。

ETHERCAT和PROFINET通信协议栈在LINUX系统集成了开源的实时补丁和ETHERCAT协议栈并优化到了工业级的稳定性，可靠性，结合算法积累，解决了控制器被人卡脖子的三个核心问题，区别于市面上大部分使用基于国外codesys内核的解决方案，我们具备独立自主知识产权，为了适配具有普遍工程师基础的西门子PLC，集成了PROFINET协议让产品适用性更广泛。

针对非ETHERCAT通信类型机器人品牌，集成了其他品牌各家的SDK和ROS接口，已经测试通过的包括aubo，franka，ur，denso，elfin，jaka等，这为我们控制器兼容更多品牌提供了可能，同时也满足了客户不同开发层面的要求。

附图说明

图1为本发明系统架构示意图；

图2为本发明中总线配置示意图；

图3为本发明中PDO数据配置示意图；

图4为本发明中旋量方法概念和原理示意图；

图5为本发明中旋量方法的参数定义示意图；

图6为本发明中旋量方法求解逻辑示意图；

图7为本发明中拉格朗日方法的方程组示意图；

图8为本发明系统动力学路程图；

图9为本发明中支持3D仿真和图形化编程一体化示意图；

图10为本发明中一键切换脚本编程示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参阅图1-10，本实施例中的一种基于linux+QT的开放性控制系统，包括底层架构、中间层架构和顶层架构；

其中，底层架构为丰富的开源、商用的算法包和各种类库，包括Eigen，ROS，OPENCV，OPENGL以及各种通讯协议栈；

中间层架构为行业的应用包，包括主从跟随，码垛，装配，视觉标定，力位控制；

顶层架构为图形化界面，支持第三方的界面集成，用QT实现。

底层架构是总线配置，使用两个全局变量TEST_ROBOT_TYPE和TEST_IOBOARD_TYPE来区分总线配置的硬件类型，组态配置好之后，需要实际获取从站PDO数据，可以直接复制IGH主站的ethercat cstruct-a0的终端输出，来实例化每个pdo的注册数据。

底层架构最关键的算法中，ETHERCAT配合PREEMPT实时补丁的DC同步机制是关键，每个伺服通信周期消耗剩余时间和计算下一周期时间的配合实现函数如下：

//send process data

ecrt_domain_queue(domainServoOutput)；

ecrt_domain_queue(domainServoInput)；

wait_send(master,PERIOD_NS)；

sync_dc_time(master)；

ecrt_master_send(master)；

calc_system_diff_time()；

wait_recv(&sleep_time)；

clock_gettime(CLOCK_TO_USE,&wakeupTime)；

wakeupTime＝timespec_add(wakeupTime,cycletime)；

clock_nanosleep(CLOCK_TO_USE,TIMER_ABSTIME,&wakeupTime,NULL)。

中间层架构中的算法层部分，除了常规的DH方程组的解法，还包括七轴机器人的旋量解法和奇异值分解SDV算法。

机器人逆解本质是解多元非线性方程组，综合牛顿-拉普森方法和雅克比，提出一种有效的迭代求方程组解的方法，基本逻辑如下：

1)先求AA

2)再将A

3)将矩阵A分解为A＝U∑V

4)最后将矩阵A写为A＝σ1u1v1T+σ2u2v2T+…σrurvrT。

以典型六轴机器人解法为例，定义每个旋量的轴和参考位置矩阵如图5所示，使用我们这种求解方法，可以处理不满足piper法则的特殊构型

迭代为梯度下降，雅可比反映了一种变化趋势，多元非线性方程组如果雅克比存在，能够找到极值点，迭代的每一步，产生一个更接近最终目标的结果，并且放入下次迭代初始值。

在具体机器人算法实现步骤中，基本可以通过调用下面两个核心算法以及相关的衍生算法，实现机器人的逆解不断迭代产生最终符合精度要求的解。

IKinBodyNR(JointNum,(double*)Blist,M,T,thetalist0,eomg,ev,maxiter,thetalist)；

IKinSpaceNR(JointNum,(double*)Slist,M,T,thetalist0,eomg,ev,maxiter,thetalist)；

int IKinBodyNR(intJointNum,double*Blist,doubleM[4][4],doubleT[4][4],double*thetalist0,double eomg,double ev,int maxiter,double*thetalist)

{

inti,j；

double Tsb[4][4]；

double iTsb[4][4]；

double Tbd[4][4]；

double se3Mat[4][4]；

double Vb[6]；

int ErrFlag；

double*Jb＝(double*)malloc(JointNum*6*sizeof(double))；

if(Jb＝＝NULL)

{

return 2；

}

double*pJb＝(double*)malloc(JointNum*6*sizeof(double))；

if(pJb＝＝NULL)

{

free(Jb)；

return 2；

}

double*dtheta＝(double*)malloc(JointNum*sizeof(double))；

if(dtheta＝＝NULL)

{

free(Jb)；

free(pJb)；

return 2；

}

MatrixCopy(thetalist0,JointNum,1,thetalist)；

FKinBody(M,JointNum,Blist,thetalist,Tsb)；

TransInv(Tsb,iTsb)；

Matrix4Mult(iTsb,T,Tbd)；

MatrixLog6(Tbd,se3Mat)；

se3ToVec(se3Mat,Vb)；

ErrFlag＝VecNorm2(3,&Vb[0])>eomg||VecNorm2(3,&Vb[3])>ev；

i＝0；

#if(DEBUGMODE)

///Debug///

printf("iteration:％4d thetalist:",i)；

int k；

for(k＝0；k

{

printf("％4.6lf ",thetalist[k])；

}

printf(" ")；

#endif

while(ErrFlag&&i

{

JacobianBody(JointNum,Blist,thetalist,Jb)；

MatrixPinv(Jb,6,JointNum,2.2E-15,pJb)；

MatrixMult(pJb,JointNum,6,Vb,1,dtheta)；

for(j＝0；j

{

thetalist[j]＝thetalist[j]+dtheta[j]；

}

i++；

FKinBody(M,JointNum,Blist,thetalist,Tsb)；

TransInv(Tsb,iTsb)；

Matrix4Mult(iTsb,T,Tbd)；

MatrixLog6(Tbd,se3Mat)；

se3ToVec(se3Mat,Vb)；

ErrFlag＝VecNorm2(3,&Vb[0])>eomg||VecNorm2(3,&Vb[3])>ev；

#if(DEBUGMODE)

///DEBUG///

printf("iteration:％4d thetalist:",i)；

for(k＝0；k

{

printf("％4.6lf ",thetalist[k])；

}

printf(" ")；

//

#endif

}

free(Jb)；

free(pJb)；

free(dtheta)；

return ErrFlag；

}

int IKinSpaceNR(int JointNum,double*Slist,double M[4][4],double T[4][4],double*thetalist0,double eomg,double ev,int maxiter,double*thetalist)

{

int i,j；

double Tsb[4][4]；

double iTsb[4][4]；

double Tbd[4][4]；

double AdT[6][6]；

double se3Mat[4][4]；

double Vb[6]；

double Vs[6]；

int ErrFlag；

double*Js＝(double*)malloc(JointNum*6*sizeof(double))；

if(Js＝＝NULL)

{

return 2；

}

double*pJs＝(double*)malloc(JointNum*6*sizeof(double))；

if(pJs＝＝NULL)

{

free(Js)；

return 2；

}

double*dtheta＝(double*)malloc(JointNum*sizeof(double))；

if(dtheta＝＝NULL)

{

free(Js)；

free(pJs)；

return 2；

}

MatrixCopy(thetalist0,JointNum,1,thetalist)；

FKinSpace(M,JointNum,Slist,thetalist,Tsb)；

TransInv(Tsb,iTsb)；

Matrix4Mult(iTsb,T,Tbd)；

MatrixLog6(Tbd,se3Mat)；

se3ToVec(se3Mat,Vb)；

Adjoint(Tsb,AdT)；

MatrixMult((double*)AdT,6,6,Vb,1,Vs)；

ErrFlag＝VecNorm2(3,&Vs[0])>eomg||VecNorm2(3,&Vs[3])>ev；

i＝0；

#if(DEBUGMODE)

///Debug///

printf("iteration:％4d thetalist:",i)；

int k；

for(k＝0；k

{

printf("％4.6lf ",thetalist[k])；

}

printf(" ")；

#endif

while(ErrFlag&&i

{

JacobianSpace(JointNum,Slist,thetalist,Js)；

MatrixPinv(Js,6,JointNum,2.2E-15,pJs)；

MatrixMult(pJs,JointNum,6,Vs,1,dtheta)；

for(j＝0；j

{

thetalist[j]＝thetalist[j]+dtheta[j]；

}

i++；

FKinSpace(M,JointNum,Slist,thetalist,Tsb)；

TransInv(Tsb,iTsb)；

Matrix4Mult(iTsb,T,Tbd)；

MatrixLog6(Tbd,se3Mat)；

se3ToVec(se3Mat,Vb)；

Adjoint(Tsb,AdT)；

MatrixMult((double*)AdT,6,6,Vb,1,Vs)；

ErrFlag＝VecNorm2(3,&Vs[0])>eomg||VecNorm2(3,&Vs[3])>ev；

#if(DEBUGMODE)

///DEBUG///

printf("iteration:％4d thetalist:",i)；

for(k＝0；k

{

printf("％4.6lf ",thetalist[k])；

}

printf(" ")；

//

#endif

}

free(Js)；

free(pJs)；

free(dtheta)；

return ErrFlag；

}

在动力学算法部分，融合了牛顿欧拉和拉格朗日的两种方法，针对拉格朗日的动力学方程组。

通过对不同项求偏微分，并将结果汇总，舍弃最小影响影子，提高计算速度，最终得到完整六轴机器人的动力学方程，如图7所示。

部署到机器人的这部分相关代码

％％动能

％外推0->1

％MASS1质心处的角速度

W101＝R10*W000+dq1*Z111；

％MASS1质心处的线速度

V101＝R10*(V000+cross(W000,P001))；

％连杆1质心处速度

Vc1＝R10*(V101+cross(W101,Pc111))；

％动能1

k1＝m1*(Vc1(1)^2+Vc1(2)^2+Vc1(3)^2)/2+((transpose(W101))*Ic1*W101)/2；

％外推1->2

W202＝R21*W101+dq2*Z222；

V202＝R21*(V101+cross(W101,P112))；

Vc2＝R21*(V202+cross(W202,Pc222))；％连杆2质心处速度

k2＝m2*(Vc2(1)^2+Vc2(2)^2+Vc2(3)^2)/2+((transpose(W202))*Ic2*W202)/2；

％外推2->3

W303＝R32*W202+dq3*Z333；

V303＝R32*(V202+cross(W202,P223))；

Vc3＝R32*(V303+cross(W303,Pc333))；％连杆3质心处速度

k3＝m3*(Vc3(1)^2+Vc3(2)^2+Vc3(3)^2)/2+((transpose(W303))*Ic3*W303)/2；

％外推3->4

W404＝R43*W303+dq4*Z444；

V404＝R43*(V303+cross(W303,P334))；

Vc4＝R43*(V404+cross(W404,Pc444))；％连杆4质心处速度

k4＝m4*(Vc4(1)^2+Vc4(2)^2+Vc4(3)^2)/2+((transpose(W404))*Ic4*W404)/2；

％外推4->5

W505＝R54*W404+dq5*Z555；

V505＝R54*(V404+cross(W404,P445))；

Vc5＝R54*(V505+cross(W505,Pc555))；％连杆5质心处速度

k5＝m5*(Vc5(1)^2+Vc5(2)^2+Vc5(3)^2)/2+((transpose(W505))*Ic5*W505)/2；

％外推5->6

W606＝R65*W505+dq6*Z666

V606＝R65*(V505+cross(W505,P556))；

Vc6＝R65*(V606+cross(W606,Pc666))；％连杆6质心处速度

k6＝m6*(Vc6(1)^2+Vc6(2)^2+Vc6(3)^2)/2+((transpose(W606))*Ic6*W606)/2；

％％势能

G＝[0；0；-g]；

％link 1

u1＝-m1*G'*(R01*Pc111+P001)；

％link 2替换Pc111->(R12*Pc222+P112)

u2＝-m2*G'*(R01*(R12*Pc222+P112)+P001)；

％link 3替换Pc222->(R23*Pc333+P223)

u3＝-m3*G'*(R01*(R12*(R23*Pc333+P223)+P112)+P001)；

％link 4替换Pc333->(R34*Pc444+P334)

u4＝-m4*G'*(R01*(R12*(R23*(R34*Pc444+P334)+P223)+P112)+P001)；

％link 5替换Pc444->(R45*Pc555+P445)

u5＝-m5*G'*(R01*(R12*(R23*(R34*(R45*Pc555+P445)+P334)+P223)+P112)+P001)；

％link 6替换Pc555->(R56*Pc666+P556)

u6＝-m6*G'*(R01*(R12*(R23*(R34*(R45*(R56*Pc666+P556)+P445)+P334)+P223)+P112)+P001)；

％％最终结果

k＝k1+k2+k3+k4+k5+k6；％总动能；

u＝u1+u2+u3+u4+u5+u6；％总势能。

中间层架构中的算法层部分同样实现了牛顿欧拉的经典迭代方法，两种方法结合，用冗余手段保障计算结果充分可靠，首先外推DH点的角速度，得到的角速度公式为：

/>

然后外推DH点的线加速度，线加速度的公式为：

然后外推质心处的线加速度、合力和合力矩，质心处的线加速度、合力和合力矩的公式分别为：

最后内推关节的合力和合力矩，内推关节的合力和合力矩的公式分别为：

得到的最终结果公式为：

部署到机器人的这部分相关代码流程

％外推0->1

W101＝R10*W000+dq1*Z111,

dW101＝R10*dW000+cross(R10*W000,dq1*Z111)+ddq1*Z111,

dV101＝R10*(dV000+cross(dW000,P001)+cross(W000,cross(W000,P001))),

dVc101＝dV101+cross(dW101,Pc111)+cross(W101,cross(W101,Pc111)),

Fc111＝m1*dVc101,

Nc111＝Ic1*dW101+cross(W101,Ic1*W101)；

％外推1->2

W202＝R21*W101+dq2*Z222,

dW202＝R21*dW101+cross(R21*W101,dq2*Z222)+ddq2*Z222,

dV202＝R21*(dV101+cross(dW101,P112)+cross(W101,cross(W101,P112))),

dVc202＝dV202+cross(dW202,Pc222)+cross(W202,cross(W202,Pc222)),

Fc222＝m2*dVc202,

Nc222＝Ic2*dW202+cross(W202,Ic2*W202)；

％外推2->3

W303＝R32*W202+dq3*Z333,

dW303＝R32*dW202+cross(R32*W202,dq3*Z333)+ddq3*Z333,

dV303＝R32*(dV202+cross(dW202,P223)+cross(W202,cross(W202,P223))),

dVc303＝dV303+cross(dW303,Pc333)+cross(W303,cross(W303,Pc333)),

Fc333＝m3*dVc303,

Nc333＝Ic3*dW303+cross(W303,Ic3*W303)；

％外推3->4

W404＝R43*W303+dq4*Z444,

dW404＝R43*dW303+cross(R43*W303,dq4*Z444)+ddq4*Z444,

dV404＝R43*(dV303+cross(dW303,P334)+cross(W303,cross(W303,P334))),

dVc404＝dV404+cross(dW404,Pc444)+cross(W404,cross(W404,Pc444)),

Fc444＝m4*dVc404,

Nc444＝Ic4*dW404+cross(W404,Ic4*W404)；

％外推4->5

W505＝R54*W404+dq5*Z555,

dW505＝R54*dW404+cross(R54*W404,dq5*Z555)+ddq5*Z555,

dV505＝R54*(dV404+cross(dW404,P445)+cross(W404,cross(W404,P445))),

dVc505＝dV505+cross(dW505,Pc555)+cross(W505,cross(W505,Pc555)),

Fc555＝m5*dVc505,

Nc555＝Ic5*dW505+cross(W505,Ic5*W505)；

％外推5->6

W606＝R65*W505+dq6*Z666,

dW606＝R65*dW505+cross(R65*W505,dq6*Z666)+ddq6*Z666,

dV606＝R65*(dV505+cross(dW505,P556)+cross(W505,cross(W505,P556))),

dVc606＝dV606+cross(dW606,Pc666)+cross(W606,cross(W606,Pc666)),

Fc666＝m6*dVc606,

Nc666＝Ic6*dW606+cross(W606,Ic6*W606),

R67 ＝[1 0 0

0 1 0

0 0 1]；

f777＝[0；0；0],

n777＝[0；0；0]；

％内推7->6

f666＝R67*f777+Fc666,

n666＝Nc666+R67*n777+cross(Pc666,Fc666)+cross(P667,R67*f777)；

％内推6->5

f555＝R56*f666+Fc555,

n555＝Nc555+R56*n666+cross(Pc555,Fc555)+cross(P556,R56*f666)；

％内推5->4

f444＝R45*f555+Fc444,

n444＝Nc444+R45*n555+cross(Pc444,Fc444)+cross(P445,R45*f555)；

％内推4->3

f333＝R34*f444+Fc333,

n333＝Nc333+R34*n444+cross(Pc333,Fc333)+cross(P334,R34*f444)；

％内推3->2

f222＝R23*f333+Fc222,

n222＝Nc222+R23*n333+cross(Pc222,Fc222)+cross(P223,R23*f333)；

％内推2->1

f111＝R12*f222+Fc111,

n111＝Nc111+R12*n222+cross(Pc111,Fc111)+cross(P112,R12*f222)；

此外，除了计算得到关节理论力矩，我们通过正雅克比，从关节力递推到末端力，可以更好的捕捉机器人实际末端受力情况，并在matlab+simulink仿真下，可以给予任意方向和大小的外部力，测试机器人针对受力情况的反应。

顶层架构中的界面层是针对机器人开发的3D仿真+树形图编程，界面跟后台的实时线程和核心的函数放在一起，直接调用。

界面层一键切换脚本二次开发，适应高级开发人员，如果对图形界面编程不满意，或者部分函数需要手写，在执行之前可以手动修改。

动态轨迹记录和采集，同样在开放性系统中，支持可视化显示点位之后，可以让调试效率得到很大的提升，否则只能靠猜测或试错，走过去了，撞击了，拍急停，才知道哪里数据不对，现在可以在跑过去之前先仿真验证一下。

与现有技术相比，本申请的技术方案具备以下有益效果：

该基于linux+QT的开放性控制系统，集成并开放的机器人算法和工艺包，在一套控制器上完成了典型四轴，六轴，七轴机器人的运动学，动力学，轨迹规划算法，兼容标准通信协议的工业和协作机器人，实现拖动示教，碰撞检测等功能，支持多机器人，多任务协同作业，在大批量部署的时候具备显著成本优势，支持典型应用包如码垛，涂胶，焊接，皮带线跟随，力传感器混合控制，2D，3D视觉标定，让机器人控制器操作更简单，应用部署和实现更快捷，适用场景更广泛，集成了AI的深度和强化学习训练集，可以完成AI常见任务如YOLO检测，轮廓识别，轨迹跟踪，动态补偿等。

跟底层算法一体化的图形化用户界面，图形编程环境和脚本交互支持，基于十多年的实际现场经验，深刻理解工程师需要的图形编程系统的理想效果，因此上位机编程系统基于QT的图形化拖拽方式，尽可能保留了用户使用习惯的基础上，增加了额外的创新点，例如在整体列表存放位置，变量列表，将同类功能指令合并来简化学习过程，为每一种工艺包和指令提供丰富的说明和范例辅助理解和掌握，此外为了适应高级开发，允许一键从图形界面切换到脚本编程，提高开放程度和灵活性，为了适应更多层次的开发，客户可以从他喜欢的高级语言如C#，C++等接口调用指令完成项目。

ETHERCAT和PROFINET通信协议栈在LINUX系统集成了开源的实时补丁和ETHERCAT协议栈并优化到了工业级的稳定性，可靠性，结合算法积累，解决了控制器被人卡脖子的三个核心问题，区别于市面上大部分使用基于国外codesys内核的解决方案，我们具备独立自主知识产权，为了适配具有普遍工程师基础的西门子PLC，集成了PROFINET协议让产品适用性更广泛。

针对非ETHERCAT通信类型机器人品牌，集成了其他品牌各家的SDK和ROS接口，已经测试通过的包括aubo，franka，ur，denso，elfin，jaka等，这为我们控制器兼容更多品牌提供了可能，同时也满足了客户不同开发层面的要求。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。

尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型。