液压机械臂集中式驱控系统研究与应用

Research and application of the centralized drive and control system for a hydraulic manipulator

摘要

针对大负载作业需求，设计了一种基于液压驱动的六自由度机械臂实时控制系统。采用单一工控机构建液压机械臂实时控制系统，即上层任务调度和底层电液伺服驱动通过单一控制器完成。设计了基于抛物线的改进梯形速度曲线优化策略；提出了融合Levenberg-marquardt （LM）与拟牛顿（Quasi-Newton methods）的逆运动学迭代求解方法；引入了具有抗积分饱和PI控制器，抑制积分饱和影响；通过仿真和物理样机完成了机械臂在笛卡尔空间中的连续轨迹跟踪性能测试和控制系统综合性能测试。实验结果表明：该控制系统稳定可靠，实时性强，满足工程控制要求。

Abstract

A real-time control system for a six-degree-of-freedom robotic arm based on a hydraulic drive is designed to meet the needs of heavy-duty operations. A single IPC is used to construct the real-time control system architecture of the hydraulic robotic arm， i.e.， the upper-level task scheduling and the lower-level electro-hydraulic servo drive are completed by a single controller. An improved parabolic-based trapezoidal velocity profile optimization strategy is designed； an inverse kinematic iterative solution method incorporating levenberg-marquardt （LM） and Quasi-Newton methods is proposed； an anti-integration saturation PI controller is introduced to eliminate the effect of integration saturation. The continuous trajectory tracking performance of the robot arm in Cartesian space and the overall performance of the control system are tested by simulation and physical prototype. The experimental results show that the control system is stable and reliable， with strong real-time performance， and meets the engineering control requirements.

Graphical abstract

关键词

控制工程 / 液压机械臂 / 实时控制 / 迭代 / 电液伺服

Key words

control engineering / hydraulic manipulator / real-time control / iteration / electro-hydraulic servo

0 引 言

机械臂控制系统中常采用电气、气压和液压等驱动方式。其中，电气驱动一般使用旋转电机，但由于其负载能力较弱，难以满足采矿、林业或工程施工中的重载作业需求。相比之下，液压系统具有输出力/扭矩大、高刚度、高功率体积/质量比、高负载效率、快速平稳响应等独特的优点^［1］，在重载作业中受到青睐，如挖掘机^［2］、海洋探测^［3，4］、起重机^［5］、机械臂^［6，7］等。目前多自由度液压机械臂应用广泛，美国、法国、日本和俄罗斯等国家在水下作业型液压驱动机械臂开展了广泛的研究与应用^［8］，如美国Kraft TeleRobotics公司的Kraft Raptor液压机械臂；美国Schilling公司的水下液压机械臂Orion、Conan、Titan 3、Titan 4；加拿大ISE公司的Magnum系列机械臂等^［9］技术相对比较成熟。国内起步较晚，但也有较好产品推出，如“海马号”无人潜水器配备有七功能和五功能的液压机械手^［10］。“奋斗者号”万米载人潜水器搭载了中国科学院沈阳自动化研究所研发的七关节伺服压夜机械臂^［11］。

液压机械臂是一种多体动力学系统，其控制方式分为分散控制和集中控制^［12］。分散控制方式是将机械臂分解成多个独立的机械系统，将关节轴视为单输入/单输出（SISO）系统进行单独控制，而关节之间的耦合可以看作不确定的干扰。相对于分散控制方式，集中控制方式需要考虑关节之间的动态相互影响。分散控制相较于集中控制具有理论性差和实现简单等优点。在运动控制中常采用基于运动控制卡的控制策略，即通过运动控制卡处理实时任务，但是，这种控制方式具有开放性差、二次开发比较复杂等问题，限制了其发展。因此，研究和开发开放式控制系统具有重要的实用和科研价值。

为了提高液压系统的控制精度、改善动力学性能和鲁棒性，国内外学者提出了众多非线性控制算法来解决不同执行机构的问题，从而实现了稳定的位置跟踪。文献［13］采用基于反馈线性化方法实现了电液执行器的同步控制。文献［14］针对液压执行器控制震动和控制精度问题，提出了一种基于反馈线性化和滑模的鲁棒控制策略。文献［15］考虑电液执行器系统中的非线性摩擦问题，设计了鲁棒离散滑模控制器实现对不确定摩擦的自适应补偿。文献［16］针对液压伺服系统中存在的参数不确定和非线性问题，设计了自抗扰自适应控制策略。此外，为了解决系统存在的干扰和不确定性问题，众多学者也研究了基于递推思想的反演控制策略^［17-21］。以上控制方法虽然可以有效提高液压系统的跟踪性能，但前提是需要全部已知或部分了解系统模型及参数，并且这些方法不可避免地需要安装额外的压力传感器、流量传感器等设备。这些要求增加了研发成本并且受到安装空间限制，从而在实际应用中受到了一定程度的局限。并且，大多数研究是在理论层面上进行仿真验证，缺乏对实际物理样机的测试。其中一个原因是商用机械臂通常集成了运动控制器和有限的访问控制权限，因此，验证算法时受到限制或需要进行大量硬件/软件修改。

本文以本实验室（中心）研制的电液伺服驱动六自由度防爆型机械臂为研究对象，开展了液压机械臂实时控制系统研究与应用。基于液压机械臂样机系统，设计了开放式分散式架构-集中控制的液压机械臂实时控制系统；提出了基于抛物线的梯形速度曲线优化轨迹，采用融合LM与拟牛顿的逆运动学迭代求解策略，并设计了基于反馈抑制（Back-Calculation）的抗饱和PI控制器，实现了液压机械臂高性能、稳定的系统控制。以下将从系统构建和运动控制两方面来阐述控制系统的组成与实现。最后，通过物理样机对液压机械臂控制系统进行了评测。实验结果表明：系统运行稳定、可靠，性能满足预期要求。

1 液压驱动机械臂系统

依托国家重点研发计划“石化爆燃环境防爆型机器人研制与应用”项目，研制了大负载履带式防爆型紧凑液压驱动六自由度机械臂样机，如图1所示。该机械臂系统由履带式移动底盘和车载液压驱动机械臂组成，其中移动底盘承载机械臂系统及控制系统，该系统满足负重比大、灵活性高等应用需求，同时实现了工程化和产品化。

主要的技术指标如下：自由度数为六；负重不小于40 kg；臂展为1.5 m。

目前机械臂控制系统一般采用分布式控制系统，其中控制部分和驱动部分是两个独立的单元，即机械臂规划与驱动部分相对独立，从而导致了系统具有一定的封闭性。针对此问题，设计了分散式架构-集中式驱控-体控制系统，如图2所示。该系统由单一的控制器实现所有的控制功能，包括数据传输、上层的运动规划、伺服控制以及参数调节等。具体功能为按模块包括主控制单元、液压机械臂、交互单元、车载液压站和履带底盘。所有设备通过交换机实现与主控制器之间的信息交互，减少了系统中需要的硬件设备数量、系统的复杂度和成本，并提高整个系统的稳定性和实时性，使用基于TCP/IP以太网的通信方式，有利于实现物联。

该液压驱动机械臂系统采用串联构型（见图3），共6个关节，关节描述如表1所示。其中第1、4、6个关节采用旋转液压缸作为驱动，适用于相对结构较小的运动；第2、3、5个关节需要支撑较大的负载，因此，采用了单伸出活塞式液压缸配合相应的连杆机构实现关节转动；每个关节轴端安装有绝对值编码器，用来测量关节角度反馈。车载集成液压站为机械臂系统提供恒定的油液压力，以确保机械臂在工作过程中具有足够的动力和稳定性。末端处置工具用于完成各种操作任务，例如阀门关闭等处置任务。

2 机械臂运动控制

本节以液压机械臂为研究对象完成控制系统的设计与应用。基于EtherCAT总线的实时液压伺服系统如图4所示，在该系统中，工控机（IPC）负责与上位机进行数据传输，并实现机械臂的运动规划、控制指令下达和反馈指令处理等功能。

2.1　机械臂运动学

采用DH方法对液压机械臂系统进行运动学建模，并通过齐次坐标变换矩阵（1）完成机械臂的正运动学求解。

{}_i{}^{i-\mathrm{1}}\mathit{T}=\left[\begin{array}{cccc}c{\theta }_i& -s{\theta }_i& \mathrm{0}& a_{i-\mathrm{1}}\\ s{\theta }_{i}c{\alpha }_{i-\mathrm{1}}& c{\theta }_{i}c{\alpha }_{i-\mathrm{1}}& -s{\alpha }_{i-\mathrm{1}}& -d_{i}s{\alpha }_{i-\mathrm{1}}\\ s{\theta }_{i}s{\alpha }_{i-\mathrm{1}}& c{\theta }_{i}s{\alpha }_{i-\mathrm{1}}& c{\alpha }_{i-\mathrm{1}}& d_{i}c{\alpha }_{i-\mathrm{1}}\\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{1}\end{array}\right]（1）

式中：a_{i-\mathrm{1}}、{\alpha }_{i-\mathrm{1}}，d_i和 {\theta }_i 为DH参数，将各连杆的变换矩阵{}_i{}^{i-\mathrm{1}}\mathit{T}(i=\mathrm{1,2},\cdots ,n-\mathrm{1})顺序相乘即可到末端连杆坐标系｛n｝相对于基坐标系｛0｝的变换矩阵：

{}_n{}^{\mathrm{0}}\mathit{T}={}_{\mathrm{1}}{}^{\mathrm{0}}\mathit{T}{}_{\mathrm{2}}{}^{\mathrm{1}}\mathit{T}\cdots {}_n{}^{n-\mathrm{1}}\mathit{T}={}_{\mathrm{1}}{}^{\mathrm{0}}\mathit{T}(q_{\mathrm{1}}){}_{\mathrm{2}}{}^{\mathrm{1}}\mathit{T}(q_{\mathrm{2}})\cdots {}_n{}^{n-\mathrm{1}}\mathit{T}(q_n)=

\left[\begin{array}{cccc}& & & \\ & {}_n{}^{\mathrm{0}}\mathit{R}& & {}_{}{}^{\mathrm{0}}{P}_n\\ & & & \\ \mathrm{0}& \mathrm{0}& \mathrm{0}& \mathrm{1}\end{array}\right]（2）

式中：q_i为第i个关节的角度值；\mathit{R}为旋转矩阵；P为当前的末端位置。

根据以上运动学模型，利用机械臂反馈的状态数据计算当前机械臂的位姿状态（x，y，z， R，P，Y），其中x、y和z分别为机械臂末端在基坐标系下的笛卡尔位置信息；\mathit{R}、P和Y为机械臂末端在基坐标系下的欧拉角姿态信息。

2.2　改进梯形速度曲线的位置曲线规划

梯形速度曲线是机器人运动规划中常用的一种速度曲线，如图5中红色曲线所示。它基于初始/终止速度、最大加/减速度、最大速度以及起始/终止的位移来计算加速段（T_{\mathrm{a}}）、匀速段（T_{\mathrm{v}}）以及减速段（T_{\mathrm{d}}）所需的时间，并根据位移、速度以及加速度公式计算期望的轨迹。然而，梯形速度曲线存在不连续的加速度，这可能会在快速的进给运动中对机械系统上产生冲击和压力，甚至导致机械臂损伤或不良的振动效应。为解决这个问题，本文提出基于抛物线的梯形速度曲线优化策略，如图5中蓝色虚线所示，该方法通过使用抛物线的光滑性改善梯形速度曲线的不连续性。

改进后的梯形速度曲线仍采用三段式：加速段、匀速段和减速段，在加速和减速段中采用抛物线来改善不连续性。改进后的梯形速度曲线表达式为：

a=\left\{\begin{array}{l}A{\tau }_{\mathrm{1}}+a_m,{\tau }_{\mathrm{1}}\in [\mathrm{0},T_a],{\tau }_{\mathrm{1}}=T_a\mathrm{时},a=a_m\\ \mathrm{0},{\tau }_{\mathrm{2}}\in [T_a,T_a+T_v]\\ -A{\tau }_{\mathrm{3}},{\tau }_{\mathrm{3}}\in [T_a+T_v,T_a+T_v+T_d]\end{array}\right.（3）

v=\left\{\begin{array}{l}{v}_{\mathrm{0}}+\frac{A}{\mathrm{2}}{\tau }_{\mathrm{1}}^{\mathrm{2}}+a_m{\tau }_{\mathrm{1}},{\tau }_{\mathrm{1}}\in [\mathrm{0},T_a],v_{\mathrm{1}}=\frac{A}{\mathrm{2}}{T}_a^{\mathrm{2}}+a_m{T}_a\\ v_m,{\tau }_{\mathrm{2}}\in [T_a,T_a+T_v],v_{\mathrm{2}}=v_m\\ \frac{A}{\mathrm{2}}{\tau }_{\mathrm{3}}^{\mathrm{2}}+v_m,{\tau }_{\mathrm{3}}\in [T_a+T_v,T_a+T_v+T_d],v_{\mathrm{3}}=\frac{A}{\mathrm{2}}{T}_d^{\mathrm{2}}+v_m\end{array}\right.（4）

s=\left\{\begin{array}{l}{q}_{\mathrm{0}}+\frac{A}{\mathrm{6}}{\tau }_{\mathrm{1}}^{\mathrm{3}}+\frac{a_{\mathrm{m}}}{\mathrm{2}}{\tau }_{\mathrm{1}}^{\mathrm{2}},{\tau }_{\mathrm{1}}\in [\mathrm{0},T_a],s_{\mathrm{1}}=q_{\mathrm{0}}+\frac{A}{\mathrm{6}}{T}_a^{\mathrm{3}}+\frac{a_{\mathrm{m}}}{\mathrm{2}}{T}_a^{\mathrm{2}}\\ s_{\mathrm{1}}+v_m{\tau }_{\mathrm{2}},{\tau }_{\mathrm{2}}\in [T_a,T_a+T_v],s_{\mathrm{2}}=s_{\mathrm{1}}+v_m{T}_v\\ s_{\mathrm{2}}+\frac{A}{\mathrm{6}}{\tau }_{\mathrm{3}}^{\mathrm{3}}+v_m{\tau }_{\mathrm{3}},{\tau }_{\mathrm{3}}\in [T_a+T_v,T_a+T_v+T_d],s_{\mathrm{3}}=s_{\mathrm{2}}+\frac{A}{\mathrm{6}}{T}_d^{\mathrm{3}}+v_m{T}_d\end{array}\right.（5）

式中：a_{\mathrm{m}}为最大加速度；v_{\mathrm{m}}为最大速度；{\tau }_{\mathrm{1}}\mathrm{、}{\tau }_{\mathrm{2}}\mathrm{、}{\tau }_{\mathrm{3}}为各阶段运行时间；A为待求参数，其大小决定了曲线的形状与大小。

2.2.1 参数A求解

假设最大速度可以达到，根据式（4）最大速度曲线表达式v_{\mathrm{m}}=\frac{A}{\mathrm{2}}{T}_{\mathrm{a}}^{\mathrm{2}}+a_{\mathrm{m}}{T}_{\mathrm{a}}可得：

A{T}_{\mathrm{a}}^{\mathrm{2}}+\mathrm{2}{a}_{\mathrm{m}}{T}_{\mathrm{a}}-\mathrm{2}{v}_{\mathrm{m}}=\mathrm{0}（6）

求解上述方程可得：

T_{\mathrm{a}}=\frac{-\mathrm{2}{a}_{\mathrm{m}}\pm \sqrt[]{\mathrm{4}{a}_{\mathrm{m}}^{\mathrm{2}}+\mathrm{8}A{v}_{\mathrm{m}}}}{\mathrm{2}A}（7）

假设T_{\mathrm{a}}有解，由方程（7）可得：

A\ge -\frac{a_{\mathrm{m}}^{\mathrm{2}}}{\mathrm{2}{v}_{\mathrm{m}}}（8）

取A=-\frac{a_{\mathrm{m}}^{\mathrm{2}}}{\mathrm{2}{v}_{\mathrm{m}}}，得：

T_a=-\frac{a_{\mathrm{m}}}{A}（9）

2.2.2 最大临界速度

根据式（5）可得最终的轨迹表达式为：

q_{\mathrm{1}}=s_{\mathrm{3}}=q_{\mathrm{0}}+\frac{A}{\mathrm{6}}{T}_{\mathrm{a}}^{\mathrm{3}}+\frac{a_{\mathrm{m}}}{\mathrm{2}}{T}_{\mathrm{a}}^{\mathrm{2}}+v_{\mathrm{m}}{T}_{\mathrm{v}}+\frac{A}{\mathrm{6}}{T}_{\mathrm{d}}^{\mathrm{3}}+v_{\mathrm{m}}{T}_{\mathrm{d}}（10）

根据梯形曲线对称原理，假设加速度段和减速段的时间相同，考虑临界状态（无匀速段），即T_{\mathrm{v}}=\mathrm{0}，轨迹可以简化为：

q_{\mathrm{1}}=q_{\mathrm{0}}+\frac{A}{\mathrm{6}}{T}_{\mathrm{a}}^{\mathrm{3}}+\frac{a_{\mathrm{m}}}{\mathrm{2}}{T}_{\mathrm{a}}^{\mathrm{2}}+\frac{A}{\mathrm{6}}{T}_{\mathrm{a}}^{\mathrm{3}}+v_{\mathrm{m}}{T}_a（11）

将A=-\frac{a_{\mathrm{m}}^{\mathrm{2}}}{\mathrm{2}{v}_{\mathrm{m}}}和T_a=-\frac{a_{\mathrm{m}}}{A}代入上式可得：

\begin{array}{l}{q}_{\mathrm{1}}=q_{\mathrm{0}}+\frac{A}{\mathrm{6}}{T}_{\mathrm{a}}^{\mathrm{3}}+\frac{a_{\mathrm{m}}}{\mathrm{2}}{T}_{\mathrm{a}}^{\mathrm{2}}+\frac{A}{\mathrm{6}}{T}_{\mathrm{a}}^{\mathrm{3}}+v_{\mathrm{m}}{T}_{\mathrm{a}}=\\ q_{\mathrm{0}}+\frac{A}{\mathrm{3}}{\left(-\frac{a_{\mathrm{m}}}{A}\right)}^{\mathrm{3}}+\frac{a_{\mathrm{m}}}{\mathrm{2}}{\left(-\frac{a_{\mathrm{m}}}{A}\right)}^{\mathrm{2}}+v_{\mathrm{m}}\left(-\frac{a_{\mathrm{m}}}{A}\right)=\\ q_{\mathrm{0}}+\left(-\frac{a_{\mathrm{m}}^{\mathrm{3}}}{\mathrm{3}{A}^{\mathrm{2}}}\right)+\frac{a_{\mathrm{m}}^{\mathrm{3}}}{\mathrm{2}{A}^{\mathrm{2}}}-\frac{a_{\mathrm{m}}{v}_{\mathrm{m}}}{A}=\\ q_{\mathrm{0}}+\frac{a_{\mathrm{m}}^{\mathrm{3}}}{\mathrm{6}{A}^{\mathrm{2}}}-\frac{a_{\mathrm{m}}{v}_{\mathrm{m}}}{A}=q_{\mathrm{0}}+\mathrm{4}\frac{a_{\mathrm{m}}^{\mathrm{3}}{v}_{\mathrm{m}}^{\mathrm{2}}}{\mathrm{6}{a}_{\mathrm{m}}^{\mathrm{4}}}+\frac{\mathrm{2}{a}_{\mathrm{m}}{v}_{\mathrm{m}}^{\mathrm{2}}}{a_{\mathrm{m}}^{\mathrm{2}}}\end{array}（12）

整理可得临界速度为：

v_{\mathrm{t}\mathrm{e}\mathrm{m}\mathrm{p}}=\sqrt[]{\frac{\mathrm{3}{a}_{\mathrm{m}}^{\mathrm{2}}(q_{\mathrm{1}}-q_{\mathrm{0}})}{\mathrm{8}}}（13）

式中：v_{\mathrm{t}\mathrm{e}\mathrm{m}\mathrm{p}}为临界速度。

假设存在均速阶段，令匀速段时间为T_{\mathrm{V}}，此时由式 （12） 可得：

q_{\mathrm{1}}-q_{\mathrm{0}}=\frac{\mathrm{8}{v}_{\mathrm{m}}^{\mathrm{2}}}{\mathrm{3}{a}_{\mathrm{m}}}+v_{\mathrm{m}}{T}_{\mathrm{V}}（14）

求解上述方程可得匀速段的时间T_{\mathrm{V}}：

T_{\mathrm{V}}=\left(q_{\mathrm{1}}-q_{\mathrm{0}}-\frac{\mathrm{8}{v}_{\mathrm{m}}^{\mathrm{2}}}{\mathrm{3}{a}_{\mathrm{m}}}\right)/V_{\mathrm{m}}（15）

系统能否达到最大速度判别式为：

（16）

对给定的期望位置需进行如下两种情况的讨论：

Case 1： q_{\mathrm{1}}\ge q_{\mathrm{0}}

对于q_{\mathrm{1}}\ge q_{\mathrm{0}}的情况，根据公式即可求出相应的曲线轨迹、速度和加速度曲线。

Case 2：

针对，即初始位置大于终点位置时，上述求解过程会出现复数求开平方的情况而导致计算失败，引入系数\sigma =\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}(q_{\mathrm{1}}-q_{\mathrm{0}})，其中\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}为符号函数，

（17）

对预先给定的起始和终点位置、速度和加速度，通过系数进行取反操作，即，{\widehat{q}}_{\mathrm{0}}=\sigma q_{\mathrm{0}}，{\widehat{q}}_{\mathrm{1}}=\sigma q_{\mathrm{1}}，{\widehat{v}}_{\mathrm{0}}=\sigma v_{\mathrm{0}}，{\widehat{v}}_{\mathrm{m}}=\sigma v_{\mathrm{m}}，然后将其作为已知参数代入式（3） （4） 和 （5） 中即可求解出相应的位置、速度和加速度轨迹，再对曲线再次取反，最终完成轨迹的输出。

2.3　基于迭代的连续轨迹跟踪控制

机械臂逆运动学的求解方法包括迭代法、解析法和几何法。采用解析法和几何法求解多自由度机械臂逆解较为复杂，而基于迭代的求解方法也有众多的研究成果，其中LM（Levenberg-marquardt）迭代方法在偏差较大的情况下可以实现快速收敛，拟牛顿可以在偏差较小的情况下实现全局的二次收敛，基于以上原理提出了一种融合LM与拟牛顿的逆运动学迭代求解方法。逆运动学算法的原理框图如图6所示。图中，k为迭代次数，k_{\mathrm{m}\mathrm{a}\mathrm{x}}为最大迭代次数。

算法1: 迭代逆运动学方法

首先采用LM算法完成大范围的搜索，当满足判断条件时，由LM算法转为拟牛顿方法。最后，由当前值x_k与搜索方向h通过迭代公式x_{k+\mathrm{1}}=x_k+\alpha h即可求解下一迭代时刻的最优值x_{k+\mathrm{1}}。终止条件为，其中\epsilon是常数。

LM算法是在高斯牛顿的基础上增加了可信区间，属于一种信赖域法。其通过迭代步公式({\mathit{J}}^{\mathrm{T}}\mathit{J}+\lambda I)h={\mathit{J}}^{\mathrm{T}}f,\lambda >\mathrm{0}，求搜索方向h。其中({\mathit{J}}^{\mathrm{T}}\mathit{J}+\lambda I)为海森矩阵的近似值，\mathit{J}是雅可比矩阵，f是残差，即观测值与估计值的差值，\lambda是增加的惩罚因子，采用Nielsen的改进更新策略：

\begin{array}{l}\mathrm{i}\mathrm{f}\begin{array}{c}\end{array}\sigma >\mathrm{0}\\ \begin{array}{cc}& \end{array}\lambda =\lambda ·\mathrm{m}\mathrm{a}\mathrm{x}(\frac{\mathrm{1}}{\mathrm{3}},\mathrm{1}-{(\mathrm{2}\sigma -\mathrm{1})}^{\mathrm{3}}\},\upsilon =\mathrm{2};\\ \mathrm{e}\mathrm{l}\mathrm{s}\mathrm{e}\\ \begin{array}{cc}& \end{array}\lambda =\lambda ·\upsilon ,\upsilon =\mathrm{2}·\upsilon \end{array}

式中：\sigma =\frac{f(x)-f(x+h)}{q(\mathrm{0})-q(h)}为增益比例，分母为近似模型的下降量，分子为实际模型的下降量，f(x)-f(x+h)为期望最小化函数的相邻步的差值，q(h)为f(x+h)的估计值，q(\mathrm{0})为估计值的初始值，\upsilon为因子，初值取为{\upsilon }_{\mathrm{0}}=\mathrm{2}。

拟牛顿法采用基于BFGS算法对海森矩阵进行近似，近似更新率为^［22］：

\begin{array}{l}\varsigma =x-x_{\mathrm{d}},y=g_{k+\mathrm{1}}-g_k,v=B\varsigma \\ B=B+\frac{y{y}^{\mathrm{T}}}{{\varsigma }^{\mathrm{T}}y}-\frac{v{v}^{\mathrm{T}}}{{\varsigma }^{\mathrm{T}}v},h=-B·g_{k+\mathrm{1}}\end{array}

式中：x_{\mathrm{d}}为用户期望位姿值；x为系统的实际位姿值；\varsigma为期望值与实际值的差值；y为相邻步之间梯度差；g_{k+\mathrm{1}}和g_k分别为相邻步的梯度；B为BFGS算法的迭代公式；h为搜索方向。

实现流程如算法 1 所示。

2.4　抗积分饱和PI控制

在液压驱动机械臂的实际控制过程中，不可避免地启动后终止再次启动系统的情况，此时会导致控制系统长时间存在某一方向的偏差，从而使带有积分作用的PI控制器输出控制量不断累积，甚至可能超出执行器允许的最大动作范围，从而进入饱和区，当再次启动时引起较大的超调，反应迟钝等现象。为解决此问题，引入了抗积分饱和的速度前馈PI控制器，如图7所示。基于反馈抑制（Back-calculation）原理的抗饱和速度前馈PI控制器表达式为：

u(t)=K_{\mathrm{p}}e(t)+K_{\mathrm{i}}\int (e(t)+K_c(u(t)-u_l(t)))\mathrm{d}t+k_{\mathrm{v}}{\dot{\theta }}_{\mathrm{d}}（18）

式中：u_{\mathrm{l}}为执行器控制量的上限值。

该控制器的实现过程较为简单，只需要根据当前误差和速度信息计算出速度前馈项，从而抵消偏差累积引起的积分作用，并减小超调和反应迟钝等现象。

为了防止给予系统过大的冲击和获得速度前馈值，先设计一个合适的过度过程，离散化的微分跟踪器（TD）的结构如下：

\left\{\begin{array}{l}{x}_{\mathrm{1}}(k+\mathrm{1})=x_{\mathrm{1}}(k)+T{x}_{\mathrm{2}}(k)\\ x_{\mathrm{2}}(k+\mathrm{1})=x_{\mathrm{2}}(k)+Tfst(x_{\mathrm{1}}-z_{\mathrm{d}}(k),x_{\mathrm{2}},r,h)\end{array}\right.（19）

式中： fst(·)定义为

fst(·)=

\left\{\begin{array}{l}d=r{h}^{\mathrm{2}},a_{\mathrm{0}}=h{x}_{\mathrm{2}},y=x_{\mathrm{1}}+a_{\mathrm{0}}\\ fsg(y,d)=(\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}(y+d)-\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}(y-d))/\mathrm{2}\\ a_{\mathrm{1}}=\sqrt[]{d(d+\mathrm{8}\left|y\right|)}\\ \begin{array}{l}{a}_{\mathrm{2}}=a_{\mathrm{0}}+\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}(y)(a_{\mathrm{1}}-d)/\mathrm{2}\\ a=(a_{\mathrm{0}}+y-a_{\mathrm{2}})fsg(y,d)+a_{\mathrm{2}}\end{array}\\ fsg(a,d)=(\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}(a+d)-\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}(a-d))/\mathrm{2}\\ \begin{array}{l}fst(·)=-r(a/d-\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}(a))fsg(a,d)-\\ r\mathrm{s}\mathrm{i}\mathrm{g}\mathrm{n}(a)\end{array}\end{array}\right.（20）

式中：r为加速度系数；h为滤波系数，x_{\mathrm{1}}和x_{\mathrm{2}}分别为期望的跟踪轨迹和速度；T为采样周期。

3 仿真与实验验证

为了验证本文提出的算法的有效性，分别进行了仿真实验和物理样机实验。仿真实验使用MATLAB Simulink完成，计算机配置为Intel（R） Core（TM） i5-8400 CPU@2.80 GHz，16 GB RAM和120 GB固态硬盘且装有 MATLAB R2021B。根据控制系统原理图4搭建了实时控制系统，如图8所示。物理样机如图1和图9所示。

3.1　改进梯形曲线验证

对改进梯形速度曲线算法进行了测试，假设最大速度v_{\mathrm{m}}=\mathrm{2}\mathrm{m}\mathrm{m}/\mathrm{s}，最大加速度a_{\mathrm{m}}=\mathrm{10}{\mathrm{m}\mathrm{m}/\mathrm{s}}^{\mathrm{2}}，初始速度v_{\mathrm{0}}=\mathrm{0}，终点速度v_{\mathrm{1}}=\mathrm{0}，通过改变起始位置q_{\mathrm{0}}和终点位置q_{\mathrm{1}}对算法进行测试，即分别考虑q_{\mathrm{0}}和q_{\mathrm{1}}的相对大小。实验设置6种状态分别为：

基于2.2节的改进算法，针对上述不同情况的位置、速度和加速度曲线规划如图10所示。

由图10可以看出，经过改进的梯形速度曲线算法可以实现不同情况下机械臂关节运动的位置曲线、速度曲线和加速度曲线规划，并表现出良好的性能，实现了平滑的曲线轨迹规划和较短的运动时间。结果表明：本文提出的优化曲线算法可以有效提高机械臂关节的控制性能，具有实际应用价值。

3.2　连续轨迹跟踪仿真测试

3.2.1　逆解算法性能测试

采用MATLAB机器人工具箱（MATLAB Robotics Toolbox）对所设计的运动学逆解算法性能进行测试，目的是测试基于机器人当前状态求解下一个状态的能力。测试实验流程为：

（1） 选取机械臂任一初始关节角度{\theta }_{\mathrm{0}}（在关节范围内），并求得当前的姿态矩阵为{\mathit{T}}_{\mathrm{0}}。

（2）在初始角度的基础上设置随机角度增量为{\theta }_{\mathrm{e}}=k\times \mathrm{r}\mathrm{a}\mathrm{n}\mathrm{d}(\mathrm{1,6})，其中k=\mathrm{1}是比例因子，其中k取值一个较小的数，以模拟是连续轨迹的下一个点。期望的关节角度为\theta ={\theta }_{\mathrm{0}}+{\theta }_{\mathrm{e}}。

（3）根据期望的关节角度，通过函数fkine(\theta )求得期望的位姿矩阵\mathit{T}。

（4）根据初始姿态矩阵{\mathit{T}}_{\mathrm{0}}和期望位姿矩阵\mathit{T}可计算迭代的最终结果。

每组实验进行了多次，选取了测试结果较好的一组列于表2中，包括迭代次数和计算时间。由表2可看出，本文所提混合算法相较于单一的算法具有更少的收敛次数和计算时间。

注1：其解算时间与计算机性能有关，并且每次结果不同。由于MATLAB中矩阵的计算复杂度大，因此，迭代时间不代表实际应用中的计算时间,经过算法迁移后，在实际的工控机上运行可以在控制系统采样周期内完成计算，满足实时性要求。

注2：迭代算法是基于前一时刻位姿完成，因此，两个相差较大的位姿之间会导致迭代算法失效。

3.2.2　连续轨迹跟踪测试

基于MATLAB/Simulink和机器人工具箱搭建了逆运动学仿真测试平台，见图11（b）。轨迹采用三维空间双纽线作为期望跟踪轨迹，见图11（a）图，假设末端的姿态不变，其轨迹方程为：

\left\{\begin{array}{l}{x}_{\mathrm{d}}=\mathrm{0.1}\mathrm{s}\mathrm{i}\mathrm{n}(\mathrm{\pi }t)+x_{\mathrm{0}}\\ y_{\mathrm{d}}=\mathrm{0.1}\mathrm{c}\mathrm{o}\mathrm{s}(\mathrm{\pi }t)+y_{\mathrm{0}}\\ z_{\mathrm{d}}=\mathrm{0.1}\mathrm{s}\mathrm{i}\mathrm{n}(\mathrm{0.5}\mathrm{\pi }t)+z_{\mathrm{0}}\\ R=\mathrm{0},P=\mathrm{0},Y=\mathrm{\pi }\end{array}\right.（21）

式中：(x_{\mathrm{d}},y_{\mathrm{d}},z_{\mathrm{d}},R,P,Y)为机械臂末端待跟踪的位姿坐标值；t为系统运行时间。

图12和图13分别为机械臂末端在笛卡尔空间中的连续轨迹跟踪误差曲线和各关节的运动曲线，由图可以看出，机械臂末端可以实现轨迹位置的几乎零误差实时跟踪，并且轨迹光滑。

3.3　控制器抗饱和测试

为测试控制系统的抗饱和能力，分别采用仿真和物理实验进行验证。

3.3.1　抗饱和仿真测试

仿真测试实验流程为：假设首先开启控制器，此时机械臂液压部分处于停机状态，经过一段时间启动液压机器人，最后的测试结果如图14所示。

由图14可以看出，在控制器启动时（A点），此时由于液压设备未启动，实际输出量与期望的控制量之间的持续偏差，会导致控制器输出出现朝某一方向的持续累积，如图14中A\to B所示。若采用无饱和抑制的PI控制器，其输出会在积分作用下持续累积，即图中B\to C段。假设在D点时启动机械臂液压设备，由于积分累积的较大输出偏置会增加调整时间，即图14中的C\to E段。假设给PI设置一个饱和函数限制其最大/小输出，输出结果如图中的点线所示。因为仅限制了最大/小输出量，而没有解决饱和的现象，同样出现延迟阶段，即图中的D\to E段。采用抗积分饱和的PI控制可以实现如图14中虚线A\to B\to D\to F段所示调整效果。

3.3.2　抗饱和实验

本文以受饱和影响最大的第二个关节作为实验对象。在首先启动控制器，控制器启动100 s后启动液压系统，监测第二个关节的运动情况和控制器的输出值，测试结果如图15和图16所示。

无饱和抑制的控制器测试结果如图15所示，由图可以看出，不带有抗饱和的PI控制具有大约0.6 s的反应延迟，关节角度的抖动误差为9°。

图16为具有饱和抑制的控制器测试结果，由图可以看出，相对于无饱和抑制的控制器具有更小的关节抖动（大约2°），并且响应基本无延迟。实验结果表明：本文设计的控制器具有较好的抗饱和效果、较快的系统响应和更小的机械臂抖动。

注3: 积分饱和现象无法完全消除，并且随着时间的推移影响越来越大，只能通过算法抑制，尽量减小对系统的影响。

3.4　机器人末端跟踪实验

为验证整体控制系统的性能和可靠性，以本中心设计的六自由度液压驱动机械臂物理样机为实验对象。液压动力源为恒压车载集成液压站，供油压力16 MPa；采用CODESYS作为开发环境；控制器采用研华UNO-2372G紧凑型工控机；伺服放大器采用自研模拟量输出模块；采用SSI接口12位编码器（最小0.08°）作为角度反馈，如图8所示。采样频率设置为250 Hz （4 ms）。

实验目的是测试机械臂末端跟踪连续轨迹的能力。任务设计为通过控制器发送连续的轨迹信号，采用机械臂末端安装的记号笔（见图17）绘制轨迹，实时观测末端轨迹跟踪情况并记录的各关节角度状态。待跟踪的期望连续轨迹采用二维平面双纽线轨迹，即保持z轴和姿态不变。待跟踪的曲线表达式为：

\left\{\begin{array}{l}{x}_{\mathrm{d}}=\mathrm{0.1}\mathrm{s}\mathrm{i}\mathrm{n}(\mathrm{0.5}\mathrm{\pi }t)+x_{\mathrm{0}}\\ y_{\mathrm{d}}=\mathrm{0.1}\mathrm{c}\mathrm{o}\mathrm{s}(\mathrm{0.5}\mathrm{\pi }t)+y_{\mathrm{0}}\\ z_{\mathrm{d}}=\mathrm{0.1}\mathrm{s}\mathrm{i}\mathrm{n}(\mathrm{0.5}\mathrm{\pi }t)·\mathrm{0}+z_{\mathrm{0}}\\ R=\mathrm{0},P=\mathrm{0},Y=\mathrm{\pi }\end{array}\right.（22）

式中：(x_{\mathrm{0}},y_{\mathrm{0}},z_{\mathrm{0}})为机械臂末端的初始位置坐标；(x_{\mathrm{d}},y_{\mathrm{d}},z_{\mathrm{d}},R,P,Y)为机械臂末端待跟踪的位姿坐标。

表3为各关节PI控制参数。图18为末端跟踪连续轨迹时的分帧截图，从整体的运行状态来看，整体系统运行稳定，没有卡顿等现象发生。

图19为各关节的跟踪曲线迹；图20为各关节的跟踪误差曲线。由结果可以看出，在跟踪连续轨迹时，各关节跟踪轨迹平滑，误差<1°。末端的跟踪轨迹和跟踪误差分别如图21和图22所示。由结果可以看出，机械臂可以实时跟踪期望轨迹，轨迹的最大误差<0.01 m。

整体实验结果表明:本文设计的集中式驱控一体式液压驱动六自由度机械臂控制系统具有较好的实时性和稳定的轨迹跟踪能力。

4 结束语

本文针对大负载作业稳定控制需求构建了分散式架构-集中式驱控一体液压驱动机械臂控制系统，基于EtherCAT总线传输数据的实时高效的优点和CODESYS方便的编程特点，实现了控制系统更紧密的集成和更高效的通信，并提高了整个系统的稳定性和实时性。针对快速进给运动中的机械臂振动效应，提出基于抛物线的梯形速度曲线优化轨迹；针对机器人运动学逆解求解复杂的问题，提出了LM-BFGS联合的机械臂逆运动学迭代求解算法；针对积分饱和的影响，引入了基于反馈抑制（Back-Calculation）的抗饱和速度前馈的PI控制器；最后，通过仿真和物理实验验证了控制系统稳定性和电液伺服驱动机械臂连续轨迹的跟踪控制能力。实验结果表明：本文设计的集中驱控一体式液压控制系统具有良好的性能和较高的应用价值。后续工作将在以上基础之上继续改进，并进一步开展特殊行业机械臂处置作业等方向的研究。同时，考虑安装更多的传感器，采用更先进的算法进一步提升性能。

参考文献

[1] Yao J, Deng W. Active disturbance rejection adaptive control of hydraulic servo systems[J]. IEEE Transactions on Industrial Electronics,2017, 64(10): 8023-8032.

[2] Zhang L, Zhao J, Long P, et al. An autonomous excavator system for material loading tasks[J]. Sci Robot, 2021, 6(55): eabc3164.

[3] Zhang J, Li W, Yu J, et al. Development of a virtual platform for telepresence control of an underwater manipulator mounted on a submersible vehicle[J]. IEEE Transactions on Industrial Electronics, 2017, 64(2): 1716-1727.

[4] Li Z, Ma X, Li Y, et al. ADRC-ESMPC active heave compensation control strategy for offshore cranes[J]. Ships and Offshore Structures, 2020, 15(10): 1098-1106.

[5] 张旭飞, 邵焱, 付玉琴, 起重机变幅系统Simcenter 3D机液联合仿真分析[J].机械工程学报, 2022, 58(24): 334-341.

[6] Zhang Xu-fei, Shao Yan, Fu Yu-qin, et al. Mechanical and hydraulic co-simulation analysis for crane luffing system based on simcenter 3D[J]. Journal of Mechanical Engineering, 2022, 58(24): 334-341.

[7] Koivumaki J, Mattila J. Stability-guaranteed impedance control of hydraulic robotic manipulators[J]. IEEE/ASME Transactions on Mechatronics, 2017, 22(2): 601-612.

[8] Pi Y, Wang X. Trajectory tracking control of a 6-DOF hydraulic parallel robot manipulator with uncertain load disturbances[J]. Control Engineering Practice, 2011, 19(2): 185-193.

[9] 陈鹰. 海底热液科学考察中的机电装备技术[J]. 机械工程学报, 2002(): 207-211.

[10] Chen Ying. Mechatronic devices in the scientific expedition of deep-ser vents[J]. Journal of Mechanical Engineering, 2002(Sup.1): 207-211.

[11] Sivčev S, Coleman J, Omerdić E, et al. Underwater manipulators: a review[J]. Ocean Engineering, 2018, 163: 431-450.

[12] 陶军. 大洋深处强国梦——我国首台4 500米级无人遥控潜水器“海马号”[J]. 国土资源科普与文化, 2016, 2016(1): 13-19.

[13] Tao Jun. Deep ocean power dream— China's first 4 500 meters unmanned remote control submersible "Haima"[J]. Scientific and Cultural Popularization of Natural Resources, 2016, 2016(1): 13-19.

[14] 王清梅, 王秀莲, 孙斌, 水下主从伺服液压机械手控制系统设计[J]. 液压与气动, 2013, 2013(11): 33-37.

[15] Wang Qing-mei, Wang Xiu-lian, Sun Bin, et al. Design of underwater master-slave hydraulic servo manipulator control system[J]. Chinese Hydraulics & Pneumatics, 2013, 2013(11): 33-37.

[16] Cortés J, Egerstedt M. Coordinated control of multi-robot systems: a survey[J]. SICE Journal of Control, Measurement, and System Integration, 2017, 10(6): 495-503.

[17] Guo Q, Chen Z, Shi Y, et al. Synchronous control for multiple electrohydraulic actuators with feed-back linearization[J]. Mechanical Systems and Signal Processing, 2022, 178: No.109280.

[18] Hoang Q D, Park J, Lee S G. Combined feedback linearization and sliding mode control for vibration suppression of a robotic excavator on an elastic foundation[J]. Journal of Vibration and Control, 2021, 27(3/4): 251-263.

[19] Lin Y, Shi Y, Burton R. Modeling and robust discrete-time sliding-mode control design for a fluid power electrohydraulic actuator (EHA) system[J]. IEEE/ASME Transactions on Mechatronics, 2013, 18(1): 1-10.

[20] Yao J, Deng W. Active disturbance rejection adaptive control of hydraulic servo systems[J]. IEEE Transactions on Industrial Electronics, 2017, 64(10): 8023-8032.

[21] 李元春, 王蒙, 盛立辉, 液压机械臂基于反演的自适应二阶滑模控制[J]. 吉林大学学报: 工学版, 2015, 45(1): 193-201.

[22] Li Yuan-chun, Wang Meng, Sheng Li-hui, et al. Adaptive second order sliding mode control for hydraulic manipulator based on backstepping[J]. Journal of Jilin University (Engineering and Technology Edition), 2015, 45(1): 193-201.

[23] Won D, Kim W, Shin D, et al. High-gain disturbance observer-based backstepping control with output tracking error constraint for electro-hydraulic systems[J]. IEEE Transactions on Control Systems Technology, 2015, 23(2): 787-795.

[24] Guo K, Wei J, Tian Q. Nonlinear adaptive position tracking of an electro-hydraulic actuator[J]. Proceedings of the Institution of Mechanical Engineers, Part C: Journal of Mechanical Engineering Science, 2015, 229(17): 3252-3265.

[25] Guo K, Wei J, Fang J, et al. Position tracking control of electro-hydraulic single-rod actuator based on an extended disturbance observer[J]. Mechatronics, 2015, 27: 47-56.

[26] 方一鸣,焦宗夏,王文宾, 轧机液压伺服位置系统的自适应反步滑模控制[J]. 电机与控制学报, 2011, 15(10): 95-100.

[27] Fang Yi-ming, Jiao Zong-xia, Wang Wen-bin, et al. Adaptive backstepping sliding mode control for rolling mill hydraulic servo position system[J]. Electrical Machine and Control, 2011, 15(10): 95-100.

[28] Powel M J D. Updating conjugate directions by the BFGS formula[J]. Mathematical Programming, 1987, 38(1): 29-46.

基金资助

国家重点研发计划项目(2022YFC2604004)

国家自然科学基金项目(U20A20201)

河北省重点研发计划项目(20311803D)