基于倾角传感器的液压比例驱动机械臂位姿控制

高波，李法康

(山东科技大学电气与自动化工程学院，山东青岛 266590)

0 前言

20世纪60年代左右，使用机械手喷浆在国外开始兴起[1]。针对矿井下混凝土喷射具有回弹率高、粉尘大等弊端，1996年，李贻斌等[2]设计了中国第一台煤矿机器人——PJR-2喷浆机器人，为喷浆机器人在我国的发展奠定了基础。

在长期的喷浆作业和研究过程中，总结出了一套完整的最佳喷浆工艺要求。其中针对受喷面喷料不均匀的问题，要求喷枪水平运动(调整喷枪相对待喷面距离)或垂直运动(调整喷枪相对地面高度)的同时不断做圆周运动，并且作业面喷枪顺序应由下而上，先侧帮后拱顶。为了满足该要求，传统的喷浆机械手将结构设计为大臂升高与伸缩臂调平，通过人工操作多关节共同运动，完成喷枪在水平方向或竖直方向的直线运动[3-4]。但是机械臂各关节运动与喷头位姿之间的关系存在耦合，并且操作困难，控制精度低，使得在调整喷枪相对待喷面距离时，其相对地面高度也会随之改变，反之亦然。

电液比例控制相对伺服控制具有抗干扰能力强、成本低的特点，在对机械手的响应速度要求不高、满足基本的控制精度就算达标的情况下，多采用电液比例控制系统[5]。通过在电液比例控制系统的油缸中安装位移传感器，将油缸活塞位移反馈至控制器[6-8]，形成位置闭环，进而实现对关节的控制，但是该方法会使得系统复杂化，可靠性和安全性也会降低[9]。

倾角传感器是一款用于测量运动载体与其初始位置所成夹角的惯性测量设备，获得运动载体的动态参数(横滚和俯仰)信息，由成本较低的倾角传感器构成的闭环控制系统同样具有较好的控制性能[10-13]，性价比较高。

为了摆脱这些局限性，本文作者根据喷枪的位姿，运用几何法求解得到关节角度，基于阀控液压马达系统以及倾角传感器，采用传统PID控制，构成关节位置闭环控制系统，控制相应关节运动实现喷枪的水平运动和竖直运动。通过结构改变，使得各关节运动最大程度实现解耦，使得关节操作简单，且可通过倾角传感器实现控制，利用倾角传感器测量无法解耦的大臂与小臂角度，实现位姿解算和关节角度闭环控制，避免了因安装传感器对系统造成的影响。

1 关节角度的求解

t时刻大、小臂当前位置相对其初始状态的位置关系如图1所示，其中关节1为伸缩关节，关节2、3、4为回转关节，倾角传感器1、2分别用于测量大臂、小臂与各自初始状态的夹角，j1(t)、j2(t)分别为t时刻大臂、小臂与其初始位置之间的夹角。

Zt(t)=Z0+vzt

(1)

Yt(t)=Y0+vyt

(2)

式中：Z0、Y0为喷枪相对关节2在巷道坐标系中的初始位置；vz表示喷枪在Z轴的运动速度，即垂直运动速度；vy表示喷枪在y轴的运动速度，即水平运动速度；t表示时间；Zt(t)表示t时刻喷枪在OXYZ坐标系中Z轴方向上的位置；Yt(t)表示t时刻喷枪在OXYZ坐标系中在Y轴方向上位置。

当需要喷枪在水平方向上运动时，vz=0，vy≠0；当需要喷枪在竖直方向上运动时，vz≠0，vy=0。

由几何法可以得到：

d1sinα(t)-d2sinβ(t)=Zt(t)

(3)

d1cosα(t)+d2cosβ(t)=Yt(t)

(4)

式中：α(t)表示大臂与水平方向的夹角；β(t)表示小臂与水平方向的夹角；d1、d2分别表示大臂、小臂的长度。

联立式(3)(4)得：

当Zt(t)≥0时：

(5)

(6)

当Zt(t)<0时：

(7)

(8)

t时刻α(t)、β(t)与j1(t)、j2(t)之间的位置关系如图1所示。

图1 机械手运动示意

2 关节位置闭环控制系统设计

EPEC(Extreme Performance Engine Controller)作为主控制器接收遥控器发送的速度指令，即上述喷枪在水平和竖直方向的运动速度vy、vz，根据式(3)(4)解算得到α(t)、β(t)，并根据图1中的位置关系可以分别计算得到大臂、小臂在t时刻与其初始状态之间的夹角j1(t)、j2(t)，其作为关节位置闭环控制系统的输入。

关节位置闭环控制系统如图2所示，由PID控制器、EPEC控制器的PWM输出模块、功率放大器、负载敏感比例多路换向阀、液压马达以及倾角传感器组成。j1(t)、j2(t)与倾角传感器实时测量值r1(t)、r2(t)的差值经过PID控制器得到相应的控制量，该控制量作为PWM输出模块的占空比，然后该PWM信号经功率放大器放大后控制负载敏感比例多路换向阀的阀芯运动，从而控制液压马达驱动回转支承，进而驱动机械臂带动喷枪在巷道坐标系中运动直至到达目标位置，最终实现通过遥控器控制喷枪在巷道坐标系Y轴方向和Z轴方向上的平稳运动。

图2 关节位置闭环控制系统框图

3 关节位置闭环控制系统的数学模型

对图2所示的关节位置闭环控制系统的每个组成部分建立数学模型，为下面仿真验证及分析优化提供理论依据。

3.1 关节位置反馈传递函数

倾角传感器用来实时测量机械臂的角度信息r1(t)、r2(t)，在控制系统中作为单位负反馈，与上述解算得到的j1(t)、j2(t)形成关节位置闭环，控制关节运动，驱动机械臂到达期望位置。

以大臂为例，建立位置闭环的数学模型：

e(t)=j1(t)-r1(t)

(9)

式中：e(t)为目标值与测量值之间的差值；j1(t)为t时刻的目标角度；r1(t)为t时刻大臂上倾角传感器的实时测量值。

对式(9)进行拉普拉斯变换得：

E(s)=J1(s)-R1(s)

(10)

式中：E(s)为e(t)的复频域表达式；J1(s)为j1(t)的复频域表达式；R1(s)为r1(t)的复频域表达式；s为复变数。

对于以流量为控制对象的PID控制器，Kd一般取零[14]，输入为E(s)，所以PID控制器的传递函数为

(11)

式中：C(s)为PID控制器输出量；Kp为PID控制器的比例系数；Ki为PID控制器的积分系数。

3.2 PWM控制器的传递函数

PWM控制器采用EPEC控制器的PWM输出模块，其最大输出电压为24 V。PID控制器输出量C(s)作为该模块的占空比，控制该模块电压的输出。当e(t)=0时，即j1=r1时，输出电压为0，关节停止运动，此时机械臂到达目标位置，喷枪停止运动；当e(t)≠0时，某时刻电压的大小在复频域的表达式为

U(s)=[24C(s)]/100

(12)

式中：U(s)为u(t)的复频域表达式。

3.3 纯滞后环节的传递函数

在实际的工程实践中，当控制器的控制量已经改变，但是EPEC控制器的PWM输出模块不能立即响应，会有一定的输出延时，此处取其为0.1 s，再加上总线传输延迟以及比例阀本身的响应速度较慢，使得延时时间在0.1 s附近波动。在控制系统中该环节相当于一个纯滞后环节[15]。

其表达式为

G2(s)=e-τs

(13)

式中：τ为滞后时间，取τ=0.1。

3.4 功率放大器的传递函数

功率放大器按输入电压的大小成比例地输出电流[16]，用于驱动负载敏感比例多路换向阀。该处的输入电压为EPEC的PWM输出模块的电压信号。该环节近似于比例环节，其传递函数为

I(s)=KaU(s)

(14)

式中：I(s)为功率放大器的输出电流；Ka为比例放大系数。

3.5 负载敏感比例多路换向阀的传递函数

比例多路换向阀主要用于工程机械电液控制领域，控制液压油缸或马达的运动方向和速度。比例多路换向阀采用负载敏感技术，使泵的输出压力随负载变化而变化，以及执行元件的速度与负载变化无关，并且能够通过计算机进行控制[17-18]。

负载敏感比例多路换向阀中的电-机信号转化器是比例电磁铁，比例电磁铁线圈起主导作用的是二阶振荡环节[19]。负载敏感比例多路换向阀输入电流I(比例放大器的输出电流)与阀芯位移的传递函数为

(15)

式中：Kv为负载敏感比例多路换向阀流量增益；ωv为负载敏感比例多路换向阀固有频率；εv为负载敏感比例多路换向阀阻尼比；Xv(s)为阀芯位移。

3.6 液压马达的传递函数

阀控液压马达的转动角度与负载敏感比例多路换向阀阀芯位移之间的关系[20]为

(16)

式中：ωh为液压系统的无阻尼固有频率；Kq为电磁阀的流量增益；εh为液压系统的阻尼比；Dm为液压马达的排量；θ(s)为液压马达转角。

液压马达转角与机械臂回转关节转角之间存在一个比例关系，即

R1(s)=Kθθ(s)

(17)

式中：Kθ为液压马达转角与关节角度之间的比例系数。

3.7 关节位置闭环控制系统传递函数方框图

综上，图2所对应的系统传递函数框图如图3所示。

图3 系统传递函数框图

4 关节位置闭环控制系统仿真验证及系统优化

在工程试验过程中，发现所用负载敏感比例多路换向阀在零位附近存在位置死区，除此之外，PWM控制器输出电压时，存在的时间滞后对整个控制系统有着很大的影响。如何解决这2个非线性因素对整个控制系统的响应速度及控制精度至关重要，因此基于上述所确定的数学模型，对整个控制系统进行仿真研究，选用合适的方法对控制系统进行优化。

4.1 负载敏感比例多路换向阀的零位死区的影响及优化

所用负载敏感比例多路换向阀的相关参数为：Ka=0.2 V/A；ωv=108 rad/s；Kv=3.65×10-3m3/s·A；εv=0.6。

为了解决该阀的零位死区对整个控制系统的影响，提高比例阀的响应速度，以负载敏感比例多路换向阀的阀芯位置控制为例，其控制框图如图4所示。

图4 负载敏感比例多路换向阀阀芯位置控制框图

当输入信号为正弦信号时，其仿真结果如图5所示。

图5 负载敏感比例多路换向阀阀芯位置的正弦响应

由图5可知：阀芯零位附近存在的位置死区，使其响应存在一定的时间延迟。

在工程试验过程中，通过电压阶跃响应的方式实现对比例阀位置死区的补偿。即通过测试得到阀芯开始移动时控制器输出的电压，该电压作为比例阀在零位附近的补偿电压Ub，经比例放大器成比例地输出相应的电流，以此来改善比例阀的动态响应。

电压阶跃表达式为

(18)

式中：Uv为电压阶跃模块的输出电压。

加入电压阶跃响应后，负载敏感比例多路换向阀对正弦信号的响应如图6所示。

图6 加入电压阶跃信号后阀芯的正弦稳态响应

如图6所示，加入阶跃电压后，在零位附近，阀芯停滞的时间明显减少，响应更加迅速，负载敏感比例多路换向阀的动态性能得到明显改善。

4.2 控制系统中纯滞后环节的影响及优化

控制系统相关元器件参数分别为：ωh=230 rad/s；εh=11.6；Kq=0.009 m2/s；Dm=3.9×10-6m3/rad；取Kp=2.0，Ki=0.02，Kθ=3.0。

对负载敏感比例多路换向阀的控制进行优化后，关节位置闭环控制系统对单位阶跃信号的响应如图7所示。

发出速度信号给喷枪后，希望喷枪能快速平稳地到达指定位置，且不应出现较大超调避免出现危险，但是从图7可以看出：由于控制系统存在纯滞后环节，使得控制系统的单位阶跃响应存在明显超调，且响应速度较慢。故需要采取必要的措施，优化该系统控制性能，补偿纯滞后对系统的影响。

图7 关节位置闭环控制系统的单位阶跃响应

针对纯滞后系统的传统控制策略包括最优PID控制、Smith预估控制、大林算法等。Smith预估控制的优点在于采用过程模型，使被控过程的延迟输出超前时间τ反映到控制器，从而抵消滞后环节对控制过程的影响[21-23]。传统Smith预估器结构框图如图8所示。

图8 传统Smith预估器结构框图

其中：G(s)为阀控液压马达系统的开环控制函数；e-τs为纯滞后环节；τm为预估器的延迟系数。

在未加入Smith预估器之前，整个阀控液压马达系统的闭环传递函数为

(19)

在加入传统Smith预估器之后，当τm=τ时，整个控制系统的闭环传递函数为

(20)

对比上述两式可得，加入传统Smith预估器之后，排除了控制系统的分母多项式的滞后项，因此，将具有滞后的控制系统模型转换为具有纯滞后项的无滞后系统模型，消去了纯滞后环节对控制系统的控制品质的影响。

在加入传统Smith预估器后，控制系统的传递函数框图如图9所示。

图9 加入传统Smith预估器后控制系统的传递函数框图

整个控制系统的单位阶跃仿真结果如图10所示。

图10 加入传统Smith预估器后控制系统的单位阶跃响应

由图10可以看出：加入传统Smith预估器之后，系统基本没有超调量，误差减小，系统的响应速度明显加快，消除了纯滞后环节的影响。

传统的Smith预估器的良好控制性能是基于受控装置的精确模型，但是对于整个控制系统，τ是一个可变量，会有一些偏差。当τm≠τ时，控制系统的控制性能会有一定的减弱。例如，当τ=0.1，τm=0.08时，在加入传统Smith预估器之后，控制系统的单位阶跃仿真结果如图11所示。

图11 控制系统加入非精确传统Smith预估器时的单位阶跃响应

如图11所示，当τm≠τ时，控制系统的单位阶跃响应出现了超调现象。为了很好地克服传统Smith预估器对模型误差敏感的缺点，文献[24]介绍了一种改进型的Smith预估器。改进型Smith预估器和传统型Smith预估器的区别在于：在系统的主反馈回路中引入了作为低通滤波器的惯性项，误差e经过一阶低通滤波器处理后反馈给控制器，减少了Smith预估模型中误差的影响。改进型Smith预估器结构框图如图12所示。

图12 改进型Smith预估器结构框图

其中：e为R1(s)和Rm(s)的差值；Kf为低通滤波器增益，取Kf=1；Tf为低通滤波器时间常数，取Tf=0.01。

当τ=0.1、τm=0.08时，控制系统在分别加入传统型与改进型Smith预估器非精确模型时的单位阶跃响应如图13所示。

图13 分别加入传统型与改进型Smith预估器非精确模型时的单位阶跃响应(τ=0.1，τm=0.08)

显然，相比传统Smith预估器，控制系统引入改进型Smith预估器之后，超调现象消失。当τ=0.08、τm=0.1时，其单位阶跃响应如图14所示。

图14 分别加入传统型与改进型Smith预估器非精确模型时的单位阶跃响应(τ=0.08，τm=0.1)

从图14可以看出：相比传统Smith预估器，改进型Smith预估器提高了系统的响应速度，降低了系统对延时常数的敏感性。

综上，控制系统在引入改进型Smith预估器之后，喷枪可以平稳快速地到达目标位置。

5 结论

针对在调整喷枪相对待喷面和地面距离时机械臂操作困难等问题，运用倾角传感器和液压比例驱动装置实现了对机械臂的位姿控制。根据喷枪位姿，通过几何法解算得到关节位置；在负载敏感比例多路换向阀的应用中，通过增加电压阶跃信号，解决了比例阀零位死区对比例阀响应速度的影响；采用改进型Smith预估器，降低了纯滞后环节对控制系统的干扰以及控制系统对延时常数的敏感性。最终通过角度解算及闭环控制，在理论方面证实了通过遥控器控制喷枪在水平方向和竖直方向上平稳运动的可行性。

在实际的工程实验当中，该方法已经运用到有关科研项目中，并验证了该方法对关节位置控制有很好的实际控制效果，驱动机械臂带动喷枪可以在巷道坐标系的2个坐标轴方向上平稳运动，控制效果符合理论预期，满足喷浆工艺对机械臂的工作要求。相对传统的控制方法，该方法在保证控制精度的前提下，可以更方便地调整喷枪相对待喷面的距离和喷枪离地高度，具有很好的实用性。