关节型机械臂驱控一体化控制系统及控制方法与流程

本发明属于机器人控制器相关领域，尤其涉及一种关节型机械臂驱控一体化控制系统及控制方法。

背景技术：

在基于运动学控制方法的关节型机械臂设计领域中主要有（1）cpac机器人控制系统、（2）crp-s80工业机器人控制系统以及（3）kemotion机器人控制解决方案；在驱控一体化控制系统设计领域中主要有（4）驱控一体化控制器和控制系统、（5）开放式驱控一体化的工业机器人控制器研发以及（6）众为兴qc驱控一体系统。

对于（1）中方案所述的机器人控制系统，关键技术在于逆运动学路径规划解算技术的实现、奇异点避免方法和操作系统实时性设计，该方案逆运动学路径规划结算主要采用几何方法求解，奇异点避免方法的实现通过绕奇异点转动机制实现，实时性设计主要通过将wince操作系统进行内核任务调度机制进行优化与改造进行实现。对于（2）中方案所述的机器人控制系统，关键技术在于不同构型机器人运动学求解过程的归一化求解，该方案针对不同构型的机器人分别设计运动学控制算法，然后将设计的运动学算法进行分析、比较，对相同模块进行归一化处理，最后通过优化技术进行总体整合，使用时再通过特殊设置进行构型配置。对于（3）中方案所属的机器人控制系统，关键技术在于将机器人运动学控制技术与应用工艺进行了深度整合和优化，在传统机器人运动学技术的基础上，结合搬运、码垛、焊接等应用的特殊要求，进行专门的工艺特殊设计，并以此为优化机制进行运动学控制设计。对于（4）中方案所属的机器人控制系统，关键技术主要包括控制器接口设计、应用层操作系统设计、应用程序和人机交互设计、运动控制卡设计及用于io控制的可编程逻辑控制器。对于（5）中方案所述的机器人控制系统，关键技术在于将常规运动控制器、常规驱动器的控制模块、工控机、安全控制卡、示教盒的cpu处理模块有机集成，形成一个一体化的运动控制器，并将其中运动控制接口进行封装开放，使用户可以进行二次开发。对于（6）中所述的机器人控制系统，关键技术在于通过分片式嵌入式设计实现了机器人控制系统逻辑部分功能、人机交互功能、电机实时运动控制功能、机器人安全控制功能和io控制功能。

对于（1）~（6）所述的方案，皆存在成本过高、实时性解决不彻底、使用时需要二次集成不能直接使用的缺点；有的还有奇异点问题解决耗时太长，驱控一体设计方案不完整、封装后的二次开发接口复杂使用技术门槛高，系统硬件过于复杂、稳定性有待提高、负载功率有限的缺点。

导致（1）所述方案在于系统主控制器使用intel架构工控机成本较高，操作系统wince内核改造只能保证软实时，该操作系统架构设计时根本不存在硬实时控制机制，奇异点解决算法过于单一，所设计方案产品为机器人控制系统主控制器，没有将驱动部分和配件设计入内，因此需要二次集成到控制柜中才能在产业中进行实际应用。导致（2）和（3）所述方案缺点的原因在于主控制器使用intel架构工控机成本较高，操作系统直接使用windows系统没有考虑实时性设计，所设计方案产品为机器人控制系统主控制器，没有将驱动部分和配件设计入内，因此需要二次集成到控制柜中才能在产业中进行实际应用。导致（4）所述方案缺点的原因在于主控制器使用intel架构工控机成本较高，操作系统存在版权成本，驱控一体化方案中只设计了传统伺服驱动器的控制部分而未设计电源模块和放大模块，所设计方案产品为机器人控制系统主控制器，没有将驱动部分和配件设计入内，因此需要二次集成到控制柜中才能在产业中进行实际应用。导致（5）所属方案缺点的原因在于主控制器使用intel架构工控机成本较高，操作系统存在版权成本，驱控一体化方案中只设计了传统伺服驱动器的控制部分而未设计电源模块和放大模块，所涉及的封装后开放接口属于高级软件开发范畴，对设计人员技术要求较高，所设计方案产品为机器人控制系统主控制器，没有将驱动部分和配件设计入内，因此需要二次集成到控制柜中才能在产业中进行实际应用。导致（6）所属方案缺点的原因在于使用了多片嵌入式控制芯片，据不完全分析，该系统至少使用了1片arm芯片、2片fpga芯片、9片dsp芯片，造成系统过于复杂，稳定性问题也随之浮现，主控系统复杂导致功耗过大，进而使得电源和伺服放大模块功率受到限制，该控制系统的封装采用盒式封装，现场抗干扰能力有限，扩展后的外部配件不好固定安装，因此需要二次集成为控制柜才能在实际产线中使用。

技术实现要素：

为了解决上述问题，本发明提出了一种关节型机械臂驱控一体化控制系统，其特征在于，包括主控板、信号集成模块、ipm驱动放大模块以及强电管理电路；主控板和信号集成板之间通过spi总线进行通信连接，信号集成模块通过其ipm接口与ipm驱动放大模块连接，信号集成模块通过其强电控制接口与强电管理电路连接；

主控板包括master-fpga、arm-dsp双核处理器、扩展dram、sd卡、隔离io、网络总线模块；主控板的master-fpga的扩展集成板接口、隔离io、sd卡、扩展dram分别采集相关信息后，交由arm-dsp双核处理器进行处理，产生控制信息，控制信息通过master-fpga扩展的集成板接口发送到信号集成板，并写入sd卡和扩展dram，同时操作主控板控制io动作，发送网络总线指令至网络总线模块；

信号集成板上包括slave-fpga接口管理、ipm接口和隔离模块，所述隔离模块包括编码器信号隔离、抱闸电路隔离、强电模块监控隔离、通信隔离、pc调试串行口隔离，各隔离模块分别与slave-fpga连接；pc调试串行口隔离连接示教器或pc端；ipm接口连接六路电机控制信号线，抱闸电路隔离连接电机抱闸线，编码器信号隔离连接电机编码器反馈信号线。

所述强电管理电路主要完成电源整流、过压过流保护、大电容充放电管理、再生电阻温度管理。

所述master-fpga还用于为运动控制多路电流环dq电流分解使用、编码器信号接收后的crc校验和旨在速度检测平滑的快速移动窗口平均、生成4路扩展pwm发生器、电流采样检测三阶降噪滤波器；

所述arm-dsp双核处理器中的dsp模块主要完成系统的路径规划和电机实时pid控制算法以及相应的快速和平滑处理，arm-dsp双核处理器中的arm模块主要完成系统的状态监测、信号管理和任务调度；

所述信号集成板用于在主控板统一时钟下各接口的信号采集、隔离和管理，具体包括ipm接口的电流采集、异常信号采集、电机控制信号发送，抱闸电路接口的信号控制管理，强电控制接口的信息采集和控制管理，扩展io的隔离、信号读取、控制管理，rs485通信接口的隔离、信号读取、信号发送，编码器接口的隔离、信号读取、信号发送等。

一种关节型机械臂驱控一体化控制系统的控制方法，所述方法包括任务示教、路径提取、奇异点处理、运动学路径规划、路径平滑，完成运动学路径规划和路径平滑后将大规模路径规划数据转为实时运动控制部分参考数据，传递至各个轴的位置环、速度环、电流环伺服电机实时运动控制模块。

所述路径提取首先要做的是任务路径描述，任务路径描述采用标准最小指令集格式进行；然后是任务描述解析，任务描述解析分为两部分：一部分为将提取的任务采用二进制编码发送到arm-dsp双核处理器，arm-dsp双核处理器会周期性的将机器人运行状态发送至任务示教端，按照二进制译码后将该信息赋于最小指令集格式电机位置上进行显示；另一部分为将提取的任务以及当前机器人状态在示教器上进行显示，该部分采用ascii编码，按照标准最小指令集格式进行显示；提取后的任务文件直接以二进制格式保存于sd卡中，sd卡信息能复制到其他机器人中直接使用。

所述标准最小指令集格式为从左向右依次为指令id、指令类型、插补类型、加速度、平滑类型、速度单位、精度类型、断点类型、工具id以及各电机位置。

所述奇异点处理采用人机交互式，其过程为奇异点检测、奇异点报警及操作者交互和奇异点躲避；

所述奇异点检测首先检测路径的位置奇异信息，然后基于位置奇异信息检测速度奇异信息，最后基于速度奇异信息检测加速度奇异信息，

所述奇异点报警及操作者交互即检测到奇异信息后，控制相关io发出报警灯信号和报警声音信号，并在示教装置上进行显示，由操作者根据任务特点选择奇异点躲避方法。

所述奇异点躲避方法分为三种，分别为奇异路径重新制定、有限步奇异点躲避和实时监督奇异点躲避，奇异路径重新制定是指放弃当前遇到奇异点的路径重新制定路径避免奇异点，有限步奇异点躲避是指控制机器人每个轴转过特定距离的步长使当前位置偏离奇异点，实时监督奇异点躲避是指各个轴每次转非常小的步长并实时反馈各轴位置信息自动判断是否已经偏离了奇异点，如果已经偏离，则躲避完成。

所述运动学路径规划包括首先提取任务点的起点和终点位置、速度这些运动约束，然后进行路径规划插补方式选择；

所述插补方式分为轴插补、直线插补和圆弧插补，轴插补轨迹生成流程包括多轴同步s曲线路径生成和7段变速时间划分机制，7段变速包括加加速、匀加速、减加速、匀速、加减速、匀减速和减减速；直线插补轨迹生成流程包括正运动学位资求解、变步长路径离散、逆运动学求解、加权最小范数优化；圆弧插补需要由任务起点、终点和中间点三个点构成圆弧曲线，然后进行变步长路径离散、逆运动学求解和加权最小范数优化步骤。

所述大规模路径规划数据转为实时运动控制部分参考数据的过程为arm-dsp双核处理器中的dsp模块完成大规模路径规划数据后首先以较慢速度写入eeprom中，然后在arm-dsp双核处理器中的arm模块控制下eeprom当前数据交替快速写入提前分成两页的专用ram中，eeprom数据读取首地址由arm模块给出，eeprom数据读取量由单页ram存储量决定，与此同时，dsp模块转入实时控制流程，错时访问已经完成写入的ram页数据作为参考位置进行单轴电机实时控制。

有益效果

本发明利用一片集成式arm-dsp芯片完成机器人控制系统主控和电机运动控制，利用两片fpga实现外部接口通信、电源控制、电流环设计，利用一片dsp芯片分时控制最多7路伺服电机；在本发明中arm和dsp芯片间通信数据交互采用内存访问的方式，使得数据交互速度达到最快，采用无操作系统方案和中断硬实时方案，使得系统的实时性得到最大程度的提高，在关节空间内对笛卡尔空间运动学计算结果进行二次优化，保证电机运动平滑性，采用软硬件并行处理机制，设计不间断数据“交互-使用-重新填充”流程，保证系统数据交互与使用互不干扰。

附图说明

图1为关节型机械臂驱控一体化控制系统架构框图。

图2为关节型机械臂驱控一体化控制系统硬件结构图。

图3为图3关节型机械臂驱控一体化控制系统软件架构图。

图4为多路电机串并行混合控制机制图。

图5为多路电机同步控制时序图。

图6为关节型机械臂驱控一体化控制方法流程图。

图7为任务描述标准最小指令集格式图。

图8为人机交互式奇异点处理流程图。

图9为运动学路径规划流程图。

图10为eeprom填充页1、dsp访问页2示意图。

图11为eeprom填充页2、dsp访问页1示意图。

具体实施方式

本发明提出了一种关节型机械臂驱控一体化控制系统及控制方法。下面结合附图，对本发明作详细说明。

本发明利用一片集成式arm-dsp芯片、两片fpga芯片完成机器人控制系统主控和电机运动控制。图1为控制系统架构图，如图所示，在物理模块上系统核心部分为由多核arm+dsp、fpga组成的系统主控板、io扩展模块、电机伺服驱动模块以及ethercat或rs485或can或profilebus网络总线通信接口。

图2为系统硬件结构图，如图所示，系统硬件主要由主控板、信号集成模块、ipm驱动放大模块、系统强电电路接触器模块等构成。其中，主控板上主要有master-fpga和arm-dsp双核处理器、扩展dram、sd卡接口、隔离io、网络总线模块等。信号集成板上主要包括slave-fpga接口管理和隔离模块，包括编码器信号隔离、抱闸电路隔离、强电模块监控隔离、rs485通信隔离、pc调试串行口隔离等。主控板和信号集成板之间通过两块fpga间的spi总线进行通信，通信内容主要包括adc采集数据、pwm信号、编码器信号及时钟统一管理信号、任务指令信息、控制数据信息等。

主控板工作流程为master-fpga扩展的集成板接口、隔离io接口、sd卡接口、dram接口采集相关信息后，交由arm-dsp双核处理器进行处理，产生控制信息，控制信息再通过fpga扩展的集成板接口、隔离io接口、sd卡接口、dram接口发送到信号集成板、sd卡、控制io动作、发送网络总线指令、生成4路基本pwm发生器等。信号集成板主要用于在主控板统一时钟下的信号采集、隔离和管理，具体包括ipm驱动放大电路的电流采集、异常信号采集、电机控制信号发送，抱闸接口信号控制管理，强电电路异常接口信息采集和控制管理，扩展io隔离、信号读取、控制管理，rs485通信接口隔离、信号读取、信号发送，绝对值编码器接口隔离、信号读取、信号发送等。

强电管理电路主要完成电源整流、过压过流保护、大电容充放电管理、再生电阻温度管理等，master-fpga另一主要作用为运动控制多路电流环dq电流分解使用、编码器信号接收后的crc校验和旨在速度检测平滑的快速移动窗口平均、生成4路扩展pwm发生器、电流采样检测三阶降噪滤波器等。arm-dsp双核处理器中的dsp模块主要完成系统的路径规划和电机实时pid控制算法以及相应的快速和平滑处理，主控板arm模块主要完成系统的状态监测、信号管理和任务调度。

系统软件程序采用模块化设计方法，如图3所示，主要模块包括m3-arm处理模块、c28-dsp软件模块、mster-fpga软件模块、slave-fpga模块、示教器通信管理模块和pc端gui通信管理模块等。这里的各个模块为软件模块，逻辑关系为m3-arm处理模块和c28-dsp软件模块为顺序先后串联连接关系，这两个模块与这里其他所述的模块关系为同步并联运行关系。

其中，m3-arm处理模块主要包括系统管理、示教器通信管理、dsp通信、集成板slave-fpga管理、外部io管理以及任务调度等；

c28-dsp模块主要完成dsp子系统管理、m3通信处理、master-fpga通信管理、多路电机实时位置控制和速度控制以及路径规划算法实现等；

mster-fpga软件模块主要处理电流采样、电流环解耦和控制、以及位置速度检测滤波等流程；

集成板slave-fpga主要完成电源管理程序、抱闸控制程序和外部io管理程序等；

示教器管理模块主要完成示教器按键管理、示教器窗口管理、示教器通信管理和示教器文本解析管理等；

pc端gui管理主要完成gui调试、参数配置、示教器模拟、数据采集分析和虚拟示教等；

整个系统位于统一时钟管理之下，除pc端gui管理模块外其他均采用硬件实时中断处理机制。

统一时钟管理下的多路电机串并行混合控制机制，如图4所示为多路电机串并行混合控制机制图，在master-fpga通过并行设计实现7路伺服电机控制的clark-park变换、park逆变换以及与电流解耦密切相关的电流环控制，在dsp上通过串行设计7路电机实时控制的位置环和速度环。clark变换的作用为将基于三轴、两维的定子静止坐标系电流矢量转换为基于两轴的定子静止坐标系电流矢量，park变换的作用为将两轴正交的定子静止坐标系矢量电流转换为等效到d、q轴的转动系统矢量电流，其中clark变换如（1）所示，park变换如（2）所示,pi控制基本描述如（3）所示。

图5统一时钟管理下的dsp串行位置环、速度环计算任务与fpga并行clark-park变换、park逆变换的任务管理机制。采用这种机制的优势在于一方面合理处理了clark-park变换、park逆变换计算过于复杂、耗时远大于位置环、速度环pi计算而导致的两者之间任务等待问题，利于减少控制时间、提高控制频率，另一方面代替传统方案中多路电机需要多路dsp控制造成的成本上升问题。

quote(1)

如图6所示为关节型机械臂驱控一体化控制方法流程图包括任务示教、路径提取、奇异点处理、运动学路径规划、路径平滑以及各个轴的位置环、速度环、电流环伺服电机实时运动控制等模块。

任务路径提取首先要做的是任务路径描述，任务路径描述采用标准最小指令集格式，如图7所示为任务描述标准最小指令集格式图，任务路径描述均按照该指令集进行，路径描述按照图7指令集格式进行编码解码即可。任务路径提取其次要做的是任务描述解析，任务描述解析分为两部分，一部分为将提取的任务发送到arm-dsp主控系统，该部分采用二进制编码，以保证传输和运行实时性最高，与此同时，arm-dsp主控系统会周期性的将机器人运行状态发送至任务示教端，由路径提取模块按照二进制译码后将该信息赋于图7最小指令集格式电机位置上进行显示。另一部分为将提取的任务以及当前机器人状态在示教器上进行显示，该部分采用ascii编码，按照图7指令集格式进行显示方便操作者直接了解机器人状态。提取后的任务文件直接以二进制格式保存于系统sd卡中，将sd卡信息复制到其他机器人中，其他机器人可以直接使用。该关键技术本发明优先在基于单独系统的示教装置上实现，但不是唯一限定。

如图8所示，奇异点处理一共分为三个子模块，分别为奇异点检测、奇异点报警及操作者交互、奇异点躲避。其中，奇异点检测首先检测路径的位置奇异信息，然后基于位置奇异信息检测速度奇异信息，最后基于速度奇异信息检测加速度奇异信息。奇异点检测模块检测到奇异信息后会交给奇异点报警模块处理，控制相关io发出报警灯信号和报警声音信号，并在试教装置上进行显示，由操作者根据任务特点选择奇异点躲避方法。奇异点躲避方法分为三种，分别为奇异路径重新制定、有限步奇异点躲避和实时监督奇异点躲避，奇异路径重新制定是指放弃当前遇到奇异点的路径重新制定路径避免奇异点，有限步奇异点躲避是指控制机器人每个轴转过特定距离的步长使当前位置偏离奇异点，实时监督奇异点躲避是指各个轴每次转非常小的步长并实时反馈各轴位置信息自动判断是否已经偏离了奇异点，如果已经偏离，则躲避完成。该关键技术本发明优先在主控arm芯片上实现，但不是唯一限定。

运动学路径规划技术基本流程如图9所示，首先将提取任务点的起点和终点位置、速度等运动约束。然后进行路径规划插补方式选择，插补方式分为轴插补、直线插补和圆弧插补。轴插补轨迹生成流程包括多轴同步s曲线路径生成和7段变速时间划分机制（加加速、匀加速、减加速、匀速、加减速、匀减速、减减速）。直线插补轨迹生成流程包括正运动学位资求解、变步长路径离散、逆运动学求解、加权最小范数优化等。圆弧插补与直线插补类似，但是圆弧插补除了需要任务起点和终点外，还需要一个中间点，由三个点构成圆弧曲线，然后再进行变步长路径离散、逆运动学求解和加权最小范数优化步骤。该关键技术本发明优先在主控dsp芯片上实现，但不是唯一限定。

大规模路径规划数据转为实时运动控制部分参考数据的传递流程为dsp完成大规模路径规划数据后首先以较慢速度写入eeprom中，然后在arm芯片控制下eeprom当前数据交替快速写入提前分成两页的专用ram中，eeprom数据读取首地址由arm芯片给出，eeprom数据读取量由单页ram存储量决定。与此同时，dsp转入实时控制流程，错时访问已经完成写入的ram页数据作为参考位置进行单轴电机实时控制。图10表示在arm控制下数据由eerpom填充到ram页1的过程，同时在这个时间段也是dsp实时控制所需数据由ram页2进行提供的阶段。图11表示在arm控制下数据由eerpom填充到ram页2的过程，同时在这个时间段也是dsp实时控制所需数据由ram页1进行提供的阶段。需要特别指出的是，本发明dsp路径规划和dsp实时控制优先在一片dsp芯片上通过错时交替运算进行实施。