线性自抗扰的增稳云台控制系统研究与实现

黄海润，施振华，苏成悦，何家俊，麦伟图

（1.广东工业大学 物理与光电工程学院，广东 广州 510006；2.广州杰超科技有限公司，广东 广州 510006）

0 引 言

无人系统中，云台的控制策略应用较多的仍然是传统的PID 控制算法，但该算法对无人系统运动中产生的随机干扰响应较慢。基于传统PID 控制算法，韩京清提出一种自抗干扰控制算法ADRC[1]。Mehran 等人通过带宽参数化的方法，将ADRC 简化为线性自抗干扰控制LADRC，减少了需要整定的控制参数，以便于将ADRC控制算法更好地推广应用到实际的工程中[2]。

本文基于LADRC 控制算法并结合IMU 传感器，实现无人系统云台自行稳定的功能，并使用FOC 矢量控制电机驱动算法摄像机云台机动结构进行控制。

1 无人系统增稳云台结构与总体设计

1.1 增稳云台结构模型简化

无人系统的增稳云台的简化结构如图1 所示，其由3 个云台电机、内外框骨架以及底座与减震结构[3]组成。为了保证摄像机能稳定地采集图像，分别使用3 个电机对摄像机相互垂直的三轴姿态角（Yaw、Pitch、Roll）进行控制。

图1 无人系统增稳云台机械结构示意图

为保证云台结构的稳定性与Pitch 轴的电机均匀受力，采用双臂外框的机械结构设计。

1.2 无人系统增稳云台总体设计框图

整个无人系统增稳云台包括三个部分：云台姿态控制核心、FOC 关节电机驱动以及与上位机和遥控器的通信，总体设计框图如图2 所示。主控核心MCU 通过SPI通信协议与无人系统增稳云台内框中的MPU6000 六轴运动传感器通信，获取内框中摄像机的三轴角速度和三轴加速度。主控对传感器的原始数据进行处理，使用四元数法进行姿态解算，得到云台的Roll、Pitch、Yaw 的欧拉角数据。主控结合RC 遥控的控制命令与当前的姿态角数据，通过LADRC 算法得到云台电机的速度目标值。

图2 无人系统增稳云台总体设计示意图

当前无人系统云台普遍采用三相无刷电机作为驱动机构，其驱动方式多为开环的SPWM 波控制方式。SPWM 开环控制三相无刷电机具有无需匹配电机参数、硬件成本低、对控制器性能要求低的优点，但SPWM 开环控制存在控制精度差、响应速度慢等问题。而对三相无刷电机采用磁场矢量定向控制策略（Field-Oriented Control, FOC）则可以避免以上问题。

主控通过RS 485 总线协议将电机的速度目标值发送至单轴FOC 云台电机驱动器，驱动器驱动无刷电机转动，调整摄像机的姿态。

2 无人系统增稳云台硬件设计

2.1 核心控制器硬件电路设计

本文设计选用STM32F103T8U6 作为云台的控制核心，设计两个串口分别用于调试或接收镜头球机的控制信号和向电机发送控制信号。

调试/控制信号接口通过USB 转串口模块连接到计算机PC 端，在串口调试助手中打印程序运行的Log 日志以及运行状态，便于调试时使用。同时，可以接收镜头机芯发来的控制信号。

电机控制串口的通信线由于需要通过云台电机中间走线，易受到电机运行时产生的电磁波干扰，因此通过MAX13487 芯片将TTL 电平的串口通信信号转为RS 485 差分通信信号后，通过SH1.0 接线端子引出。无人系统增稳云台主控硬件设计框架图如图3 所示。

图3 无人系统增稳云台主控硬件设计框架图

为了兼容航模，常用遥控RC 信号控制增稳云台角度的功能，通过排针将STM32 带有PWM 捕获功能的引脚引出，期间使用RC 滤波电路对信号进行滤波处理。

系统总电源采用12 V 供电，除了通过接线端子输出给电机供电，还通过HT7850 芯片与RT9013-3.3 V 芯片将12 V 电源稳压成5 V 与3.3 V，给MCU 等设备供电。

2.2 FOC 电机驱动硬件设计

无人系统增稳云台电机驱动采用机电一体化设计，将驱动电路嵌入云台电机中。 电机驱动采用STM32F103C8T6 作为主控MCU，主控实物图如图4 所示。考虑到云台电机后盖空间狭小的情况，电机驱动使用德州仪器TI 公司的DRV8313 三相电机驱动方案。DRV8313 电机驱动在大小只有6 mm×8 mm 的芯片内集成了MOS 管与MOS 驱动，还具有过流保护、短路保护、欠压锁闭和过温保护等功能。通过INA240APWR 芯片对三相电机的驱动下桥臂采样电阻两端的微小电压进行放大，并通过STM32 的ADC 功能对放大后的模拟电压信号进行采样后转换成数字信号，供FOC 电流环控制算法处理。由于矢量控制算法需要知道三相无刷电机的实时转子位置，故本文选用TLE5012B 磁编码器对转子的位置进行追踪。电机驱动硬件设计框图如图5所示。

图4 无人系统增稳云台主控实物图

图5 无人系统增稳云台电机驱动硬件设计框架图

3 无人系统增稳云台控制软件设计

FOC 控制策略通过采样高精度编码器的数据获取电机转子的实时位置，从而驱动电机三相驱动合成的输出力矩方向垂直于转子。这使得电机的转矩脉动更小、响应速度快，并具有更高的控制精度[4]，同时可为摄像头的图像采集带来更加平稳的画面。

3.1 FOC 电机控制算法

本文设计中，执行机构无刷直流电机采用FOC 电机驱动算法。整体采用串级PID 控制结构，外环为速度控制环，内环为电流控制环。FOC 电机控制算法框图如图6 所示。

图6 FOC 电机控制算法框架图

对模拟PID 离散化后的PID 算法数学表达式如下：

式中：KP为比例系数；KI为积分系数；KD为微分系数；e(n)=期望值-实际值。

通过采集三相电机的相电流后，结合从磁编码器得到的电机转子位置信息进行一次Clark 变换和一次Park变换，可以得到在同步旋转坐标系下的q轴电流iq和d轴电流id。电磁转矩的大小与iq的数值成正比，因此可以通过闭环控制iq的值来控制电机的电磁输出转矩[4]。

采用id=0 的控制模式，电流环中iq与id经过内环PID，id期望值为0，iq的期望值由速度环的PID 经过计算后得出。内环PID 计算后得到目标值Vq、Vd，结合转子当前的转子角度对Vq、Vd进行反Park 变换后，得到静止坐标系下的Vα和Vβ。

Vα和Vβ再经过一次反Clark 变换，可以变为三相电机的期望电压Va、Vb、Vc。之后经由SVM 机制[5]更新到各相的PWM 输出，最后由三相逆变桥作用到电机上。

电机驱动的速度环则以串口发来的速度值为目标，结合编码器前馈的角度数据微分作为当前转速，通过PID 控制器控制[6]。

3.2 FreeRTOS 任务调度

无人系统增稳云台控制软件部分在ARM-Cortex-M3 开发平台上设计，使用FreeRTOS 实时操作系统提高CPU 的使用效率，减轻复杂任务的开发难度。

表1 中列出了设计中的6 个线程以及各个线程负责的任务。

表1 线程及其任务

3.3 四元数姿态解算算法

本文设计中使用的MPU6000 六轴姿态传感器内自带的DMP（Digital Motion Processor）功能模块。通过对MPU6000 的DMP 模块进行配置便可以直接读出姿态欧拉角数据。

DMP 模块减少了MCU 的计算资源，从而能够直接获取到IMU 的欧拉角姿态数据。但是DMP 模块输出姿态角的频率为200 Hz，也就是每5 ms 输出一次姿态数据。而本文的LADRC 控制器的处理频率是1 kHz，DMP模块输出的速度无法满足系统闭环处理的速度。因此采取直接获取原始数据，通过Mahoney 互补滤波算法[7]进行姿态解算的方法获取欧拉角数据。

3.3.1 建立坐标系

建立两个坐标系：地理坐标系E与机体坐标系B0。地理坐标系E也称为导航坐标系，其中X轴指向东，Y轴指向北，Z轴指向天。机体坐标系B0是与机体同步变化，其中X轴指向机体正前方、Y轴指向机体正右方、Z轴机体垂直正上方。姿态解算其实就是描述地理坐标系到机体坐标系的变化过程。

根据文献[7]可得到由地理坐标系E到机体坐标系B0的姿态变换矩阵CBE为：

用四元数的形式表示CBE为：

对比式（2）和式（3）可得四元数转欧拉角的公式为：

3.3.2 更新四元数姿态数据

利用加速度对陀螺仪数据进行纠正之后，获取得到的数据是在机体坐标系下的三轴旋转角速度。为了将其更新至四元数中，需要从四元数的导数入手[8]。

四元数机体三角表达形式如下：

对其进行求导得：

式中，wx、wy、wz为机体坐标系下的角速度。用一阶龙格-库塔法对进行求解可得：

到此便获得了四元数与机体三轴角速度的关系，在周期性获取角速度数据并利用重力加速度对其进行纠正后，通过式（7）可以获取四元数的最新状态。最后利用式（4）可以将四元数更新为欧拉角数据，给LADRC 控制算法作为姿态的前馈输入。

3.4 LADRC 控制算法

LADRC 控制算法包含三个主要部分：跟踪微分器（TD）、线性误差补偿控制器（LESF）、线性扩展观测器（LESO）[9]，算法框图如图7 所示。TD 的作用是为控制输入提供一个缓冲的过程，提取出包含有不确定干扰或噪声的输入信号以及它的微分信号。这个环节的主要作用是解决传统的比例积分微分控制器中的超调性和快速性之间的固有矛盾。其数学表达形式[10]如下：

图7 LADRC 控制算法框架图

式中：v为控制信号的输入；v1为TD 输出的控制输入跟踪值；v2为输入信号的微分；r为跟踪速度因子，用于调控v1对输入的跟踪速度；h为滤波因子，用于滤除输入信号的噪声；fhan(·)是最速控制综合函数[11]。

LESF 线性误差补偿控制器在本质上是一个PD 控制器，其根据当前整个系统的运行状态、监控得到的输出与实际输出之间的差距，以及估计得到的外部干扰来对系统的输出进行补偿控制。其数学表达形式如下：

式中：e1为由LESO 估计的系统实际状态与TD 处理过的系统信号之间得到的误差；e2则是系统的微分误差；b1与b2相当于普通PD 控制器中的比例误差增益与微分误差增益；b0为系统系数。根据文献[2]，通过带宽法可以确定b1=w2c，b2=2wc。其中wc为控制器参数。

线性扩展观测器LESO 是整个LADRC 算法的核心部分，相比于传统的PID 控制，其不仅可以观测系统实际输出的状态，还将系统受到的总扰动纳入估计。LESD 线性扩展观测器的数学表达形式如下：

4 实验与结果分析

在STM32 的MCU 平台上，设计一种基于LADRC 控制算法与FOC 电机控制算法驱动的云台系统。为了验证所设计系统的响应速度与稳态误差，制作并搭建了如图8 所示的三轴增稳云台试验平台。

图8 三轴增稳云台试验平台

开机后对电机进行校准，完成后默认云台三轴姿态稳定在零角度。为验证系统的控制效果，以Pitch 轴为例进行实验，通过串口向云台发送命令，使其俯仰角向上抬30°。

通过对LADRC 的TD 组件r参数进行调整，可以控制输入控制器的目标值对控制命令的跟踪速度。为了保证摄像头运动的稳定性，不断调整r参数，最终将数据确定在r=1。

图9 所示为在不同俯仰角下，由控制器LADRC 模块TD 组件输出v1与v2对控制命令输入处理后的结果，其中v1为控制信号跟踪值，v2为控制信号跟踪值的微分。由图9 可知，经过TD 微分控制器处理后，阶跃控制信号变得平缓，减少了系统的超调，并滤除了控制信号的噪声。

图9 Pitch 轴控制输入与控制器TD 组件输出v1与v2

实际工况下，环境中的各种因素都会对云台系统的姿态角产生干扰。

图10 所示为云台Pitch 轴对v1的跟踪效果，可知俯仰角在0.3 s 内达到设定值，基本没有超调，且没有发生明显振荡，基本达到了设计目标。为了验证所设计的云台系统的抗干扰性，将云台放置于实验室的风洞进行测试，云台风洞试验图如图11 所示。

图10 Pitch 轴角度与控制跟踪效果

图11 云台风洞试验图

实验从无风状态开始，在第3 s 设置风洞的风速为7.5 m/s，随后在第6 s 将风速提升至17.5 m/s，模拟八级风的实际环境。实验过程中增稳云台各轴角度变化如图12~图14 所示。

图12 Pitch 轴角度在风场中的角度变化

图13 Yaw 轴角度在风场中的角度变化

由图12~图14 可以看到，在八级风17.5 m/s 的风速下，云台的三轴角度发生了较为明显的震动。由于三轴受风面积影响不同，以及三轴负载不同，导致其在风场下的角度变化情况不同，但基本维持在±0.15°以内，满足设计要求。

5 结 论

本文设计了一种将LADRC 控制算法作为控制器的增稳云台，完成了FOC 电机驱动算法的实现；将其作为云台的机动执行机构，从IMU 传感器获取角速度数据，并通过Mahoney 互补滤波算法进行姿态解算，获得云台姿态欧拉角度。姿态数据通过LADRC 控制算法处理后，将控制命令发送到三个角度控制电机。通过搭建实验平台对控制效果进行实验，验证了该系统的响应时间在0.3 s 以内，且超调在1%以内，具有较好的控制效果。