果园巡检机器人长臂抖动抑制方法

姜海勇，姜文光，邢雅周，李 娜，杨 欣

（1. 燕山大学河北省轻质结构装备设计与制备工艺技术创新中心，秦皇岛，066004；2. 燕山大学机械工程学院，秦皇岛，066004；3. 河北农业大学机电工程学院，保定，071001）

0 引 言

现代果树培育生产体系中，巡检机器人是重要的组成部分之一，主要完成果树图像自动采集和相关环境信息提取与上报等功能[1-2]。矮砧密植苹果园冠层高度约为2.5～4 m，采用桁架结构机器人进行图像采集时，受行间距限制观测范围小且成本高；采用无人机则难以在风雨条件下开展图像采集工作。为了研究诸如落花落果原因及防治措施，特别是在风雨等气候条件下进行监测，迫切需要一款机动性强、成本低廉而稳定性较高的果园巡检机器人。

长臂结构具有轻量化优势，但刚度低带来的不稳定问题显著，采用增加用料提高刚度或者增加阻尼机构进行被动减振的方式，不适应轻量化发展的需求，以合理的控制方法改善臂体的稳定性是重要的发展方向。结构振动抑制方法主要有开环和闭环两类，张美艳等[3]采用输入整形法，通过输入量的规划使二自由度机械臂在运动过程中保持稳定。Li等[4]针对高空作业平台在旋转运动过程中的横向抖动问题，将臂体等效为摆动的单梁，基于拉格朗日动力学模型，在变位启停阶段采用傅里叶级数规划轨迹。反馈控制在不同结构的振动抑制过程中形式多样，如李君等[5]针对果园链索的抖动问题，采用Hamilton方法建立了链锁行进与横向抖动的动力学耦合模型，基于图像反馈链索的抖动，采用步进电机驱动机构，实现了在5个周期内快速抑制链锁抖动的效果。薛涛等[6]针对长臂喷药机臂体抖动，制约设备作业效率和安全性能的问题，采用超声波传感器监测长臂末端到地面的距离，使用PID控制器进行主动抑制，有效抑制了臂体抖动。李扬等[7]通过加速度传感器积分得到振幅的方式反馈控制高空剪枝剪的末端抖动，采用粒子群优化PID控制器参数，取得了良好的稳定效果。Qiu等[8]针对悬臂水平面内抖动问题，采用光学成像方式监测末端抖动，并构建了递归神经网络控制器，以基频抖动抑制为目标取得了抑振的效果。Fareh等[9]采用系统辨识技术为一段长0.3 m的等截面水平摆动悬臂梁建立了动力学模型，采用压电陶瓷作为运动检测传感器和执行器进行振动抑制，该方法在微机电设备中取得了优越的稳定性效果。Zuyev等[10]和Weldegiorgis等[11]采用臂体根部应力反馈的方式进行抖动抑制。以上研究均采用连续体偏微分方程建模，在结构复杂及扰动不确定情况下，此类方法在应用过程中由于建模困难而不便应用。

针对变形体建模复杂的难题，Howell等[12-14]于1992年提出了伪刚体模型（Pseudo Rigid Body Model， PRBM）法，在柔性结构设计与控制领域取得了良好效果。Yu等[15]以末端集中载荷下的变形量相等为约束条件，采用滑动副与旋转副串联的机构形式，建立了精确描述柔性梁在同时存在轴向伸长和侧向挠曲变形情况下的伪刚体模型，而后以基频相等条件确定了各刚性组件的质量，这可以理解为获得了一阶固有频率等效的动力学模型[16]。近年来微分平坦理论在欠驱动装备控制中广泛采用[17-19]，其基本思想是：非线性系统只要有一组输出量，使得状态变量和输入变量能由这组输出量及其微分表示，该系统即为平坦系统。该理论应用表现为对原系统的降维处理，也可以理解为一系统在某一坐标系下的欠驱动特征，在微分平坦空间下却可以表现为全驱动系统。Sira-Ramírez等[18]采用微分平坦方法解决了多个欠驱动系统的控制问题。但同时他也认为微分平坦输出量的寻找经常是非常复杂且巧妙，而无定式可循的一项工作[19]。姜海勇等[20]针对变长度臂体的抖动问题，将悬臂段等效为一个刚性杆，采用加速度传感器检测末端振动，在臂体基频约为3 Hz的情况下取得了较好的效果，此方法在长臂柔性显著情况下，基频低响应复杂而难以胜任。

自抗扰控制（Active Disturbance Rejection Control,ADRC）技术的核心思想是将模型的误差以及外界的扰动作为扩张状态量，进行实时观测与补偿。大量研究表明线性或非线性系统在合适的反馈作用下都能够被转化为积分串联标准型[21-22]。简言之，只要扰动和误差在观测器的带宽范围之内即可被观测器捕获，进而由跟随的控制器补偿，在控制器校正能力充足的条件下即可消除误差[23]，这使得控制系统在无模型信息或模型信息不充分的条件下，仍具有优异的鲁棒性[24]。

综上所述，针对长柔性臂体低频抖动抑制问题的研究中，采用多点反馈进行控制的方法未见报道。本文研究目标是基于二阶模态振型等效方法建立柔性臂的等效多刚体动力学模型；基于3个倾角传感器的仰角信号反馈，按照微分平坦思想获取合成输出量；基于自抗扰思想将高阶模型低阶化处理[25-26]，拟建立巡检机器人臂体的抖动抑制系统，实现臂体快速稳定进而达到高效影像采集的目的。

1 柔性长臂的等效动力学模型

为了对果树冠层进行检测，需采用长臂将摄像头抬升至冠层高度，本研究设计了如图1所示果园巡检机器人，其长臂总长为4.8 m，为了适应果园的不确定地形设计长臂仰角调节范围为25°～70°，在长臂末端安装无线传输型摄像头。工作过程中抑制臂体低频抖动，维持摄像头与水平面之间的角度稳定在某一设定值，以保证高质量图像信息的采集工作。本文将整个臂体等效为三杆两扭簧平面机构，如图2所示，实施过程中以前二阶振型等效为依据，将悬臂段等效为两个铰接的刚性杆和两个扭簧。在传感器选择方面，倾角传感器和加速度传感器同属微机电系统技术产品。在运动信息采集时，基频越低的条件下需要加速度传感器的灵敏度越高；而采样频率相似的倾角传感器检测仰角，抖动频率低时测量精度更高。这便导致基频越低时，加速度传感器的成本比倾角传感器更高。因此采用三个倾角传感器进行臂体运动信息采集是更经济可行的方案。进一步根据传感器输出值的物理意义建立等效模型，再凭借平面二杆二扭簧机构的静平衡条件与二阶振型等效条件，获得各等效杆及扭簧的参数。电推杆变幅推力等效为三杆机构固定铰接点O处的扭矩。在等效模型与真实臂体总长相等的条件下，两个支撑铰链点O与O′之间的臂体看作一段刚性杆。外伸的悬臂段等效为两段刚性杆，传感器2和3的安装位置以检测最大截面转角为目标。

等效模型建立过程如下：首先，基于二阶模态振型及其导数曲线确定外伸段等效二杆杆长及臂体上传感器的安装位置；然后，根据静力平衡条件下的仰角变形等效建立静平衡约束；将固有频率代入到等效刚性杆的仰角振型中，确定各杆等效质量及扭簧等效刚度，构建三杆二扭簧机构与长臂整体等效模型。

1.1 二杆二扭簧机构动力学模型

按照拉格朗日动力学原理，参照图2可得二杆二扭簧机构的拉格朗日函数为

设θi0为臂体保持某一平衡状态时第i杆的仰角，考虑各等效杆的仰角θi均在其平衡位置附近的小范围内变化，基于小变形近似，式（1）可简化为

考虑长臂体低阶模态为主的低频振动特性，结合静平衡约束条件，与模态等效约束条件，可确定两个杆长与两个扭簧刚度系数。设简谐振动频率为ω，则式（2）所表示的无阻尼振动模型的特征方程B可表示为

臂体在重力作用下发生弯曲变形并保持在某一稳定姿态，臂体根部的变幅力矩维持整个臂体的平衡。式（4）是根据图2建立的等效静平衡方程。

采用有限元法计算臂体的前两节模态，将前两阶模态的固有角频率i0ω（i=1，2）分别代入到B的行列式中，获得两个模态等效约束方程为

联立式（4）～（5）可以求解得到悬臂段所等效的两刚性杆的质量（m2，m3）和两个等效扭簧的刚度系数（k1，k2）。

1.2 三杆二扭簧机构动力学模型

臂体根部段看作一刚性杆，其杆长及质量等物理量直接测取，则臂体等效为所示的平面三杆二扭簧模型。各仰角增量均为小量，表示为θδi=θi-θi0，在静平衡条件下悬臂段的根部所受到的支撑力矩与臂体自重产生力矩平衡，而根部力矩在等效两杆机构中将由扭簧1提供，为此存在如下关系：

按照臂体静止状态时长臂根部力矩与臂体自重产生的力矩对应关系，可获得静平衡力矩为

臂体所需力矩τ可表达为力矩增量τδ与静平衡力矩之和，记为τ=τδ+τ0，三杆二扭簧平面机构的拉格朗日动力学模型可写为增量形式如下：

其中

2 基于微分平坦输出的控制系统

由于系统输入只有变幅驱动，从式（8）可以看出，该系统是一个单输入三输出的系统，所以模型将表现为单输入多输出的欠驱动系统结构[27]。3个仰角在不同平衡姿态时变形量的不一致导致控制难度较大。为了得到单入单出结构以简化抖动抑制系统，按照微分平坦原理为臂体的等效平面三杆二扭簧机构寻找平坦量。

2.1 微分平坦输出量

动力学模型式（8）整理为

控制目标在于给出一个角度目标时，臂体稳定时将回复到一个特定平衡姿态，为此设定如下最简线性组合形式，将臂体3个部位的仰角增量组合为一个系统输出量：

式（9）两侧同时左乘(βα1)，结果为

其中

再对式（10）两侧同时取二阶导数可得：

微分平坦思想的核心在于，平坦量及其多阶导数能够作为一个向量基代数表示系统原有的m个状态量和n个输入量[19]，即所寻找的平坦量及其前（m+n-1）阶导数线性无关。由于式（9）中有1个输入和3个仰角及其二阶导数项，这就需要平坦量y及其前6阶导数线性无关。此外，τδ是唯一输入，所有状态量在其作用下应共同响应。据此θδi应该由y及其前4阶导数线性表达，否则某一仰角的二阶状态量将必须由平坦量及其6阶以上导数表达。当式（12）中仅包含y与各仰角增量的二阶导数，此时系统输入显然无法被平坦量解耦，因此增加约束条件如下：

在此条件下式（11）化为

其中

由此待定微分平坦量的两个变量可以由p表示为将式（16）代回到（11）式可得：

其中

式（17）的两侧同时取二阶导数得：

与式（13）的处理方式相同，令该系数为0可得：

将该结果代入到式（16）得到平坦输出F的显式表达，再代入到式（18）可得：

其中

再对该式两端同时取二阶导数得：

将式（10）、式（12）、式（17）～（18）、式（22）～（23）联立求解可得：

将此结果中各表达式代入到式（8）得：

其中

至此，三杆二扭簧等效平面机构模型中三个仰角及其二阶导数项能够被平坦输出量y及其前6阶导数项线性表达，表明在此设定下系统是平坦的。

2.2 基于微分平坦输出量的自抗扰控制系统

将式（25）中平坦输出量的4阶和6阶导数项、与线性化过程中产生的误差及未建模部分、以及外部扰动一并看作综合扰动f，据此公式（25）可改写为

参考线性自抗扰控制理论 ，微分平坦量与输入力矩一同送入扩张状态观测器。将f看作新的扩张状态量，且认为扩张状态量是可微的，即定义扩张状态观测器的状态量为采用式（27）所示的Luenberger观测器对系统状态进行观测。

式中

将观测器的极点布置在oω-处[27-28]，取基于扩张状态观测器并进行如式（28）所示的极点配置，系统将是渐近稳定的，意味着观测器捕获扰动量，并将其送入反馈控制环节中；在控制率合适的条件下，系统将会简化为标准积分串联型，如式（29）所示。

将反馈控制器的特征值设置在cω-处[29]，控制率为

3 抖动抑制试验

为了适应苹果、梨、樱桃等果树的表型信息采集需求，巡检机器人臂体总长为4.9 m，臂体采用铝合金方管，电推杆调节仰角，悬臂部分长为4.2 m。在臂体末端安装无线传输型摄像头其重量为311 g。3处倾角的检测均采用CTS系列倾角传感器，其分辨率为0.01°，动态精度为0.1°，输出频率为100 Hz。以Arm32芯片构建控制器主体，通过三路数字输入通道读取传感器信号，一路输出用于向脉宽调制（Pulse Width Modulation, PWM）直流电机驱动模块输送占空比指令。

3.1 等效模型参数

长臂中电推杆作用点之上的悬臂部分是一段等截面均质梁，当末端安装具有显著质量摄像头时，其质量不可忽略，采用有限元模型进行模态分析。本文采用Ansys软件中的BEAM189单元和Mass21单元，为长臂的悬臂段及末端质量建立300个单元的有限元模型，进行模态分析，其前两阶模态的固有频率为

提取第二阶模态下各节点处截面转角θsec的振型如图3所示。对数据进行拟合得到二阶模态转角振型曲线方程为

距固定端2.39 m处转角为0，以该点作为两个等效刚性杆的铰接点。即确定第二杆长度为2.39 m，第三杆长度为1.81 m；再对振型曲线方程求导，距离固定端1.07和4.2 m的两点处导数为0，该点是二阶振型转角最大点，该两点分别为第2、3倾角传感器的安装位置，即确定lk1=1.07 m，将第3倾角传感器安装在臂体末端。

在臂体仰角工作范围内的随机平衡状态下读取3个倾角传感器的读数（rad）为

将静变形量和前两阶固有频率代入式（4）（5），以及等效前后质量相等条件联立所构成的方程组求解，便可获得两杆质量和两扭簧刚度系数共4个待定量。长臂根部段处于最底端的铰链与电推杆铰链点之间，根部段按刚性杆等效，其质量质心位置及长度在实际模型中直接测取。这样便确定了长柔性臂等效为3个刚性杆的质量、长度和两个扭簧的刚度，如表1所示。

表1 等效机构参数Table 1 Parameters of the equivalent mechanism

3.2 计算微分平坦输出量

将表1中数据代入到公式（21）中得到p=0.014 95，再代入式（16）中得到微分平坦量因子α=3.41，β=1.13。臂体处于不同姿态并保持平衡时，三倾角传感器所在位置的仰角受具体刚度和质量分布影响，其各自平衡仰角是变化的，为此不易直接给出规划。在小变形振动的假设下，以θ0作为臂体仰角的参考目标，三个测点的统一角度参考便于操作，则微分平坦输出量记为

臂体抖动抑制的目标是通过控制力矩，使y趋近于零。试验过程中通过在臂体中下部用手或橡皮锤给以锤击，记录传感器的输出数据。首先记录无控制条件下，臂体在锤击作用下的振动情况如图4所示。在冲击开始时刻臂体末端的摆幅达到10°，由于长臂自身阻尼微弱，经过约30 s时间后末端摆幅降至4°，而后还要再经历大约40 s时间末端摆幅降至2°以内，缓慢恢复稳定状态。

3.3 PID抖动抑制试验

PID控制器作为应用最为广泛的反馈控制方法适应性强，本文将臂体3处的仰角输出值按照微分平坦输出计算方法得到系统单一输出y，采用PID控制器对y进行反馈控制的系统框图如图5所示。

经调谐得到比例系数为10.21，积分系数为0.31，微分系数为3.55。取得的控制效果如图6所示，从图6a中可见与图4相似的锤击作用致使臂体末端在第6 s产生摆幅约为12°的初始振动，在控制作用下，约7 s后摆幅下降至2°以内恢复基本稳定状态。从图6b中可见平坦输出量在振动抑制过程中一直存在小幅抖动情况，系统输入PWM占空比一直处于高速的换向状态，且抑振过程中输出PWM占空比出现了4次饱和现象。

3.4 ADRC抖动抑制试验

在自抗扰控制器中引入模型信息能够有效的降低观测器的负担提高跟踪效率。等效三杆两簧机构中扭簧刚度、杆长、杆质量共8个常值，代入式（26）中得到：

调谐观测器带宽为1.26，控制器带宽为0.93，仍然在臂体的中部与前述实验相同位置施加冲击载荷，试验结果如图7所示。图7a中可看出，初始冲击在第5 s时带来的末端抖动幅值达到11°，在ADRC控制器的作用下，经过7 s时间，末端振幅降至3°以内，冲击作用发生9s后，末端抖动幅值降至2°以内。从图7b中可以看出，采用该方法时，振动抑制过程中输出有两次饱和，抖动抑制过程与PID控制器相比较为平滑。从图7c中可以看出，平坦输出量与综合扰动量观测量z3基本保持一致，观测器对合成输出跟踪较为准确，说明扰动量能够被准确捕获。在大幅冲击发生时由于驱动器的饱和，观测值出现小幅超过被跟踪量的情况，但仍能快速恢复对平坦量的跟踪。此外，对比图6和图7可以看出，ADRC控制方法在小幅振动过程中动作较为柔和，有效抑制了高频的小幅振动。

4 结 论

本文采用有限元方法为带有摄像头的果园巡检机器人长柔性臂提取模态数据，结合静平衡等效与前两阶模态等效的方法，构建了长臂的等效动力学模型。采用线性无关法得到了系统的微分平坦输出量，构建了单入单出的反馈控制系统。在冲击扰动下，通过PID与ADRC控制器试验验证了该方法的有效性。以平坦输出为反馈量的控制作用下，臂体均能在9 s以内迅速抑制大幅抖动，采用PID控制器时，末端摆动幅度达到10°的抖动在3个控制周期内便减弱到2°以内，但后期存在小幅较高频率抖动难以消除的问题；采用ADRC控制器时系统输出较为柔和，力矩输出饱和2次，明显少于PID方法，虽然大幅抖动需要5个周期才能有效抑制，但是后期不易发生高频抖动。农业工程中涉及长臂机构存在冲击扰动，或行进条件下存在臂体抖动的情况，需要进行主动抖动抑制时均可试用该方法。