驱控一体7DOF机械臂位置控制技术研究*
时间：2023-03-09 22:55:02 来源：千叶帆 本文已影响人
陈 罡,吴 菁,金贵阳,严 楠
(1.浙江机电职业技术学院 自动化学院,浙江 杭州 310053;2.宁波职业技术学院 机电工程学院,浙江 宁波 315800)
7DOF机械臂具有冗余自由度机构特性及良好的运动灵活性,能够克服关节限位,完成复杂避障任务,因此,7DOF机械臂被广泛应用于医疗、工厂作业、救援、核电站辐射探测等多种复杂多变场景中。
7DOF机械臂是一个多输入多输出、非线性、强耦合的复杂系统,在其运动过程中,存在诸如摩擦力、重力矩及惯性矩等外部扰动的影响,因此,系统存在参数摄动、外界干扰及未建模动态等不确定性[1-3]。
常规比例积分微分(PID)、模型预测控制、线性二次调节器等线性算法过于依赖精确模型,不能满足高非线性情况下,机械臂系统快速、高精度的轨迹跟踪性能要求。神经网络、模糊逻辑和自适应等非线性控制算法都有非线性函数的拟合能力,可以用来处理不确定性。
DIETRICH A[4]、YANG Chen-guang[5]、HE Wei[6]、眭东亮[7]采用模糊逻辑、神经网络、滑模变结构控制和自适应等智能控制算法,对机械臂多电机伺服系统进行了控制,解决了实际过程中的机械臂参数不确定问题。
这些非线性方法虽然可解决外部扰动未知、参数不确定和未建模动态等问题,但机械臂在控制过程中干扰信号的强度是未知的,仍会引起系统的抖振现象,故采用观测器的方法对机械臂外部扰动进行估计,并做出补偿,将有利于降低控制系统的增益,获得更好的轨迹跟踪性能。
杨柳[8]提出了一种基于干扰观测器的轨迹跟踪鲁棒控制算法,提升了四旋翼无人机控制过程中的抗干扰性能。鲜斌等人[9]利用扰动观测器,估计和补偿了地面效应带来的扰动,实现了对降落过程中的无人机的精确控制。
此外,扰动观测器还被广泛地用于永磁同步电机控制[10,11]、自主船舶协同路径跟踪控制[12]、机器人控制[13]中。但上述文献中观测器的观测误差是渐进收敛的,存在速度较慢的问题。
为此,有必要引入一种非线性扰动观测器(nonlinear disturbance observer-based, NDOB),以提高对机械臂外部未知扰动及建模不确定性的快速精确估计能力;将其与滑模变结构控制结合,可以提高机械臂的轨迹跟踪性能[14，15]。
但传统滑模变结构控制中存在符号函数,容易引起抖振,在一定程度上降低了机械臂的轨迹跟踪性能。冒建亮等人[16]设计了一种快速连续非奇异终端滑膜控制策略(fast-continuous nonsingular terminal sliding mode control, FNTSMC),采用连续可导滑模面,可有效削弱系统抖振现象,其性能优于传统滑模变结构控制策略。
基于上述研究与分析,笔者首先采用ARM+DSP+FPGA硬件架构,设计驱控一体式控制器控制方案,保证控制器有足够的计算性能;然后,针对冗余机械臂的不确定因素问题,将NDOB与FNTSMC相结合,提出一种NDOB-FNTSMC复合控制策略(其中,外界扰动中的可观测部分采用非线性扰动观测器来消除,不可观测及系统建模不确定部分采用快速连续非奇异终端滑膜控制器进行补偿,以提高机械臂位置控制系统的收敛速度,削弱抖振现象);最后,通过实验,验证驱控一体式控制方案和复合控制策略的有效性,以满足7DOF机械臂高速高精伺服控制及响应要求。
为提升7DOF工业机械臂控制器的性能,结合ARM、DSP和FPGA各自的优点,笔者设计了新型7DOF机械臂驱控一体控制方案[17],如图1所示。
图1 新型7DOF机械臂驱控一体控制方案
由图1可知:ARM负责高级运动路径规划和更高级的算法集成,实现与PC机、示教器、视频和触摸屏等外设的连接扩展;
DSP接受来自ARM的控制指令,完成7路电机的位置、速度环控制,同时承担伺服控制高级算法的实现;
FPGA负责7路电机的电流环控制和外围接口信号的采集,实现传感器采集数据之间的通讯、DSP之间的通讯和外围扩展信号间的通讯,7路电机空间矢量脉宽调制(SVPWM)的生成,并根据指令控制对应电机的运动。
控制器对许多信号的检测处理都是在FPGA模块中实现的,信号监测后进行相应的运算处理,自动检测更新,以达到实时控制的效果。外部通讯通过中断执行,DSP与FPGA模块进行数据交换,直接读取相应寄存器中的处理完成的数据,参与控制,减少通讯等待时间,有效减少了DSP的处理时间,大大提高了控制系统伺服带宽[18]。
因此,采用双核微处理器(ARM+DSP)和FPGA模块结合,实现7路电机的实时控制是一个有效的解决手段。
基于ARM+DSP+FPGA多电机协调伺服控制采用电流环、速度环和位置环3级反馈控制,其架构如图2所示。
图2 电机3环伺服控制架构
从图2中可以看出:当机械臂示教器或控制器发出运动指令后,由ARM模块通过运动学正解逆解、直线轨迹规划、圆弧轨迹规划等方法,生成运动轨迹的参考位置指令信号θr;DSP模块承担伺服系统位置环和速度环的控制算法实现,生成参考电流ir;电流环控制在FPGA内部硬件电路中完成,通过产生的SVPWM驱动信号,完成对功率模块的开关控制,实现对多电机的运动控制。
电机实际位置信号通过电机位置编码器读取,速度反馈信号通过单位时间内位置值的变化量求取,电机实际电流值通过电流采样芯片采集。
FPGA模块读取伺服电机信息、电压和电流等信息,检测、监控伺服电机,通过片外设备接口(external peripheral interface, EPI)总线送给DSP模块,进行数据运算处理。
基于ARM+DSP+FPGA的机械臂多电机协同控制策略,笔者根据采样周期和控制环时间的长短,对多电机控制的顺序进行排序和分类,并合理规划多电机控制时序。
在整个多电机运动控制周期中,笔者将7个电机控制划分为4个状态,即根据计数器的递增,设置如下4种状态:
Count=(Count++)%4;//每次中断计数加1,0～3循环
Switch (Count)
{
Case 0: process State0;break;
Case 1: process State1;break;
Case 2: process State2;break;
Case 3: process State3;break;
}
每种控制状态对应不同序列组合的电机控制和操作,完成对应状态下各电机的任务,满足7DOF机械臂伺服系统的要求。
7DOF机械臂电机控制时序如图3所示。
图3 机械臂7个电机控制时序图
从图3可以看出:在第1个采样周期内,电机1、3、5、7进入工作状态,电机2、4和6处于空闲状态;电机1和5在DSP模块中进行位置环和速度环的闭环控制运算;电机3和7在DSP模块中只进行速度环的闭环控制运算。
电机各信号监测正常后,进入空闲等待,完成状态0周期内任务,等待下一个中断到来。
根据状态0的任务,笔者设计机械臂7个电机控制时序的转换表,如表1所示。
表1 多电机控制时序转换
注:P.V.—在DSP模块位置环和速度环的运算;I.—在FPGA模块中的电流环运算。
从表1中可知:在每个运动周期(state0～state3),机械臂7路电机完成4次电流采样、2次速度采样和1次位置采样,因此,控制系统速度环的频率是电流环的1/2,位置环的频率是电流环的1/4。
由此可见,采用上述控制时序比顺序控制有更多的带宽,控制系统具有更高的控制性能。
4.1 机械臂动力学模型
对于n自由度的串联性机械臂来说,其动力学模型可由拉格朗日动力学方程推导而来:
(1)
为便于后续控制律的运用,式(1)可以被表示为另一种非线性的标准系统方程:
(2)
4.2 非线性扰动观测器
扰动观测器是机器人控制中很重要的一种手段。
(3)
(4)
(5)
基于以上趋近律,代入式(1),可得:
(6)
为了观察机械臂系统中的未知扰动,结合式(2),非线性扰动观测器将被设计为:
(7)
式中:o(x,z)—增益向量,也是给定的非线性函数。
4.3 滑模控制律
针对7DOF机械臂每个关节控制系统,笔者给出了误差定义:
e=x-xd
(8)
式中:xd—机械臂给定的期望关节角参考信号。
机械臂连续快速非奇异终端滑模面函数为:
(9)
式中:β>0,1<γ<2,用sat饱和函数替代常用的sign分段函数可以有效削弱控制输入的抖振现象。
笔者将式(9)的一阶导数代入式(1),可获得系统的等效控制律:
(10)
为了使系统快速收敛,此处引入趋近律:
(11)
式中:0 由此,可得基于趋近律的控制输入为: ur=h(x)-1(k1s-k2sign(s)p) (12) 结合式(10,12),可得最终的控制律为: u=ue+ur (13) 综合上述研究内容,可得到机械臂位置的控制策略,如图4所示。 图4 机械臂位置控制策略 图4中,采用非线性扰动观测器,对模型不确定性及未知外部扰动进行估计,并采用快速连续非奇异终端滑膜控制器,实现对机械臂系统的轨迹跟踪控制。 7DOF机械臂实验测试平台如图5所示。 图5中,实验平台采用驱控一体式控制器,通过多控制端口控制机械臂驱动电机;采用高性能ARM+DSP+FPGA芯片设计的控制平台和接口电路,对7路不同的电机统一控制;并通过智能功率模块(intelligent power module, IPM),实现对7DOF机械臂多路伺服电机一体式驱动,完成多电机的一体控制和驱动,并实时检测电机的动态跟踪特性。 图5 实验测试平台 机械臂7路伺服电机的参数如表2所示。 表2 电机参数 表2中,列出了功率、极对数、转动惯量、定子电阻、定子电感、摩擦系统、额定转速等参数。 为了验证机械臂驱控一体控制器的性能,笔者对机械臂关节2电机进行了电机位置控制,如图6所示。其中,图6(a)为机械臂关节2电机位置控制响应曲线、速度响应和电机电流Iq的变化曲线,图6(b)为位置控制达到稳态后的误差曲线。 图6 关节2电机位置控制 从图6中可以看出:在设定值为8 000状态下,稳态误差不超过0.35,说明机械臂电机位置控制性能良好。 在机械臂末端空载状态下,笔者对关节2电机进行速度控制实验,如图7所示。其中,图7(a)为机械臂关节2电机的速度响应曲线、对应控制电流的响应曲线,图7(b)为速度响应误差曲线。 从图7中曲线变化情况可以得出:在速度控制模式下,电机速度能快速地达到稳定状态,达到稳态后,速度误差在0附近变化,波动在1%以内,速度变化较小,具有很好的控制精度。 图7 关节2电机速度控制 在机械臂末端空载状态下,笔者对关节6电机进行恒力矩控制实验,如图8所示。其中,图8(a)为机械臂关节6电机恒力矩控制实验对应的电机电流、速度响应曲线,图8(b)为关节6电机恒转矩控制误差变化曲线。 图8 关节6电机恒转矩控制 据图8可以看出:电机电流能在瞬间达到设定值,达到稳态后,电机电流误差较小,基本上都在-0.005 A～0.005 A之间,具有很好的恒力矩控制性能。 针对机械臂在运动中存在摩擦力、重力矩等外扰动的影响,笔者分别进行机械臂PID位置控制、NDOB-FNTSMC位置控制实验,以研究机械臂的动态响应性能。 5.3.1 机械臂PID位置控制实验 笔者在机械臂末端抓取5kg载荷状态下,进行PID位置控制实验,得到第4轴和第6轴电机负载状态下PID补偿控制的位置响应实验曲线,如图9所示。 图9 带负载关节4、6 PID位置控制 图9(a)为关节4PID位置控制响应曲线,图9(b)为关节6PID位置控制响应曲线。机械臂关节4、6位置动态响应基本一致,两者超调量相近;关节6做俯仰动作,关节6电机速度和电流波动比关节4明显。 5.3.2 机械臂NDOB-FNTSMC位置控制实验 笔者在机械臂末端抓取5kg载荷状态下,进行NDOB-FNTSMC位置控制实验,如图10所示。其中,图10(a)为关节4NDOB-FNTSMC位置控制响应曲线,图10(b)为关节6NDOB-FNTSMC位置控制响应曲线。 图10 关节4、6 NDOB-FNTSMC位置控制 笔者得到了第4轴和第6轴电机负载状态下NDOB-FNTSMC位置控制响应实验曲线。 对比图(9，10)的实验曲线可以看出:采用NDOB-FNTSMC,起始电流较大,系统响应更快,能提前0.25 s到达控制位置,稳态时超调量减少,检测到的关节电机速度波动较小,电流波动也较小,性能相比图9的常规PID控制提升了10%～15%,一体式控制器伺服驱动性能良好。 采用基于ARM+DSP+FPGA的硬件架构,笔者开发了7DOF机械臂驱控一体式控制器,实现了对机械臂的多电机协同控制;设计了基于非线性扰动观测器的快速连续非奇异终端滑模复合控制策略;通过实验,对驱控一体控制器方案和复合控制策略的性能进行了验证。 研究结论如下: (1)基于ARM+DSP+FPGA硬件架构的7DOF机械臂驱控一体控制方案具有较高的计算性能,从计算分析可知,控制系统速度环的频率是电流环的1/2,位置环的频率是电流环的1/4,相比于顺序控制获得了更多的带宽; (2)根据机械臂空载实验,机械臂关节的位置控制、速度控制、恒力矩控制最终的误差都在1%以内,获得了较好的控制性能; (3)基于非线性扰动观测器的快速连续非奇异终端滑模控制策略,解决了模型不确定和未知扰动的干扰问题,削弱控制系统的抖振现象,确保机械臂轨迹跟踪误差能在限定时间内收敛至特定区域,相比PID控制策略,超调量、系统响应速度等性能提高了10%～15%,具有更好的位置动态响应性能,能较好的满足机械臂运动控制要求。 7DOF机械臂轨迹跟踪控制系统的设计是工业控制领域一个实用的研究课题,笔者主要解决模型不确定和未知扰动下的有限时间位置跟踪问题。 在未来的研究过程中,笔者还将对机械臂轨迹跟踪控制系统的时延、执行器饱和等问题展开进一步的研究。
5.1 实验平台
5.2 控制系统性能测试
5.3 机械臂动态性能实验
