基于无剑SoC开源平台的二自由度云台控制系统

李伟博，李珈毅，张志明，翁锦煜，许春权

(同济大学电子与信息工程学院，上海 200092)

0 引言

多自由度云台控制器需要能实时调整负载的运动姿态，其设计与开发是一个典型的机电一体化应用问题，涉及到嵌入式系统、计算机控制、运动轨迹规划和功率控制变换等多学科理论知识和综合实践技能，已在监控、安防、测控、测绘、机器视觉等多种实际应用场景下得到应用[1-4]。以二自由度运动控制设计为例，其主要由核心微控制器、控制执行单元、外围辅助电路等组成，实现控制水平和俯仰的角度变化运动的主要功能，并保证其位置和速度的偏差始终在一定精度范围之内。目前云台控制模块大多采用较成熟的ARM架构MCU作为核心处理器[4-6]，但近年来随着开源指令集RISC-V受到重视[7-8]，针对不同应用可灵活修改指令集和芯片架构设计，使得其在对于成本较敏感的AIoT物联网应用方向更具优势。本文基于RISC-V架构下的无剑SoC开源平台，完成相关核心功能模块的软硬件设计和控制算法编程，经过系统联调实现二自由度云台的精确运动控制。

1 无剑SoC开源平台

低功耗微控制器采用无剑100开源SoC，基于开源的RISC-V CPU架构，采用两级极简流水线，适用于对功耗和成本敏感的IoT应用，提供集芯片架构、基础软件、算法与开发工具于一体的整体解决方案[9-10]。无剑通用设计MCU具有32个GIPO口、12个PWM输入输出口、3个可复用的串口/SPI/IIC等的外设资源，基于FPGA通过EDA工具进行前端仿真和片上测试，足够满足项目需求。这使得本研究能够快速开展运动控制算法与软硬件驱动程序等设计工作。

2 系统构建与实现

二自由度云台控制器由硬件系统和软件系统组成。硬件架构框图如图1所示，主要包括核心控制器、人机交互接口、云台运动控制单元、电源模块等。整个项目基于无剑100开源平台[9-10]，在此基础上进行Verilog和嵌入式C语言编程实现软件系统功能。采用57/42步进电机控制水平和俯仰2个方向的转动运动；利用按键和OLED模块进行人机交互操作；选择蓝牙模块用于远程无线通信；IMU模块用于角度自校准。构建与实现集成一体化的云台控制系统，在系统姿态发生偏移时能够及时检测并控制，实现精确快速监视或者移动场景下的增稳控制。

图1 硬件架构系统框图

2.1 基于无剑SoC开源平台的核心控制器设计

云台控制系统中的核心控制器通过EDA工具进行前端仿真通过后在FPGA上进行测试。首先在Xilinx Vivado 2018.3 HLX中编译官方原版的SoC硬件Verilog描述源码，综合生成比特流(bitstream)文件，再下载到XC7A200T系列FPGA中，生成RISC-V架构的MCU[11-12]；之后使用配套的SDK以及图形化开发套件C-Sky Develop Kit(CDK)进行基于此MCU内核的开发[9-12]，提供UART、SPI、I2C、Timer、PWM等多种IP以及驱动；在此基础上完成包括云台控制算法和驱动程序在内的核心模块功能开发。

Xilinx XC7A200T3B开发板的资源消耗实际测试结果如表1所示，总体资源使用率少于25%。与此同时，动态与静态功率消耗估计分别在0.03 W和0.14 W左右，总功率低于0.2 W。由此可见，无剑100作为一款低功耗SoC，展示出优秀的功率特性以及低资源消耗的特性。

表1 无剑100的资源消耗统计

项目选择主动串行(AS)配置模式，比特流文件烧入板载的Flash中，FPGA在每次上电后作为主控制器，从FLASH中读取数据，进行配置；初始化完成后，进入用户模式，开始正常工作。

2.2 功能化外设模块设计

功能化外设模块包括直流电源模块、驱动模块、人机交互模块和角度采集模块，分别用来为核心控制器和电机驱动供电、控制云台水平竖直2个方向的运动、驱动蓝牙等外设进行人机交互和实时进行角度反馈校准，提高系统的抗干扰性。

2.2.1 电源供应

电源模块分为核心电路供电和驱动电路供电两部分。无剑FPGA核心板和外设模块供电需要直流+5 V/+3.3 V电源；云台驱动电路中，步进电机驱动器和步进电机的供电需要直流+12 V电源。步进驱动器和电机在实际运行时，最大功率工作状态下，检测其总电流在2 A以内；直流12 V电源经LM1085后级线性降压稳压电路得到+5 V/+3.3 V供电，其中+5 V最大输出电流可达3 A，满足无剑开发板工作电流需求，+3.3 V电源最大输出电流1 A，满足外设模块的工作电流要求。室内实验时输入电源共用1个AC220V转DC+12 V/8 A的电源适配器，室外实验或者没有交流供电插座情况下由22 400 mA·h/12.6 V的串联锂电池组供电。

2.2.2 云台驱动

云台开环驱动控制框图如图2所示，硬件设计选择一台57步进电机和一台42步进电机分别提供水平和俯仰方向上的360°的自由旋转运动，由无剑MCU提供驱动控制信号。无剑100的I/O口连接到DM542步进驱动器，分别实现2个旋转方向的自由度运动姿态控制，具有易开发、精度高、延迟低、低速带载能力强等优点。XC7A200T3B开发板与步进电机驱动器的连接方式为共阳极连接，使用6 400细分，即步距角为360°/6 400=0.056 25°，再加上防丢步控制算法，空载时的电机角度控制精度在水平和垂直方向实测最好效果可至0.1°以内。

图2 云台步进电机开环控制

2.2.3 外设模块

外设模块包括功能按键、用于显示控制器内部运行状态的OLED显示模块以及用于无线通讯的蓝牙模块。按键使用GPIO进行读取；OLED使用SPI协议进行指令控制和数据读写；蓝牙模块通过串口转换后进行透传模式通信，实现数据的无线发送和接收。无剑100的UART0/UART1通过中断触发接收方式从FIFO中进行读取数据，分别获取蓝牙通信数据和IMU姿态数据。IMU九轴传感器[13]以10 Hz的速率发送包含校验位和欧拉角数据的数据帧。在云台锁定模式下，无剑100实时检测角度的变化，计算欧拉角姿态并做出相应的反应，产生对应的PWM脉冲信号，向电机发送动作指令，以完成经典PID负反馈控制[14]，实现角度偏差校正。

2.3 SoC平台下的控制器软件设计

SoC平台下的控制器软件设计采用Verilog和嵌入式C语言编程实现，完成云台控制算法和驱动程序在内的核心模块功能开发，以及按键、IMU、蓝牙和OLED等主要外设的数据传输和处理程序。

2.3.1 系统程序运行流程

系统程序运行流程如图3所示。首先连接电源，按下RE_PROG键后，FPGA主动读取快闪存储器(FLASH)中的数据进行配置，等待LED D3亮起即为配置成功，再连接USB线通过电脑端的CDK软件环境进行调试。主函数初始化通用输入/输出接口(GPIO)和异步串行传输(UART)后，设置串口UART0和UART1通过中断触发读取先入先出(FIFO)的方式，按键通过外部中断触发读取，OLED通过GPIO模拟串行外设接口(SPI)协议进行控制显示。串口UART0接收到远程蓝牙指令后，首先解析指令翻译成电机片选序号及其旋转方向和脉冲数量，之后在主循环中进行电机控制，使用GPIO产生固定数量的脉冲宽度调制信号(PWM)和方向控制信号输入到步进驱动器中，达到运动控制目的。OLED用于显示电机当前的位置参数，提示用户验证指令解析是否正确和记录显示云台姿态参数。在解析并执行完用户指令后电机保持不动，串口UART1以100 Hz的频率接收来自IMU的欧拉角姿态数据，并计算判断出当前电机是否有位姿的偏移误差，同时进行实时调整，保持云台稳定。

图3 系统程序流程图

2.3.2 电机控制和状态自校准锁定

驱动电机运转的实现过程需要适应硬件组成，软件控制上使用S曲线算法[15-16]防止步进电机运动过程中出现丢步，增大误差，同时在进入角度锁定状态后实现自校准反馈控制算法。

步进电机的驱动信号可以分为脉冲信号和方向控制信号。选用GPIO口电平反转和适当延时产生驱动步进电机需要的PWM脉冲信号波形，其优点在于脉冲数容易控制、频率与占空比可及时调节。无剑100 SoC主频可达20 MHz，GPIO的电平变换可以依赖高速总线实现快速反转的功能以满足步进驱动所需频率，实测最高可达50 kHz。方向控制信号则使用单个GPIO口引脚产生，输出高电平可使得电机向预设正方向旋转，电平反转为低电平后即可反方向旋转。

步进电机按步长方式进行运动，每一步对应一个脉冲。根据实际应用调节频率即可满足不同运行速度的需要，本设计中细分数取6 400，理论精度为0.056 25°，即输入一个脉冲转动的角度。输入的PWM频率在恒速阶段为f=20 kHz，步进电机运行速度约为3.125 rps。

步进电机把脉冲信号转换为角位移，具有控制精度高、控制简单等特点，即使是开环控制也能获得较高的控制精度。但由于固有机电结构原因，步进电机启动时需要的加速电流较大，线性度变差，会出现启动慢、丢步的现象；同理，在停止时也会出现过冲的现象，从而影响到步进电机的控制精度，所以需要对步进电机启动和停止时的加速度和速度作合理的规划[15-16]。综合考虑后选择使用S曲线加减速控制算法，在从启动到停止整个运动过程中电机的加速度可分为5段，在整个运动控制周期内获得较平稳且收敛较快的电机加速度和速度变化，并基于实际使用环境对S曲线的基本公式y=1/(1+e-x)进行合理的改进，得到整个加速、匀速和减速阶段的频率、周期、速度、加速度的计算公式。其中在加速阶段和减速阶段分别使用频率计算式(1)和式(2)：

(1)

(2)

式中：fmin=50 Hz；fmax=20 000 Hz；i为不同时段的频率节点的序号，每一阶段i均从0开始，加速和减速阶段i的范围为0～1 000。

匀速阶段的频率f为定值20 000 Hz，频率节点序号i为0～10。3个阶段中，每个节点所对应的周期和速度的计算式分别见式(3)和式(4)：

ti=1/fi

(3)

式中：fi为频率，Hz；ti为时间，s。

vi=fi×360/6400

(4)

式中vi为速度，(°)/s。

加减速阶段的加速度a的差分计算式如式(5)所示：

ai=(vi-vi-1)/ti

(5)

式中ai为加速度，(°)/s2。

匀速阶段的加速度a的数值等于0。

3 实验测试与结果分析

搭建二自由度云台实验平台，设计的原型系统实物照片如图4所示，系统硬件结构包括无剑100开源RISCV开发板FMX7AR3B、供电模块、双轴步进电机及其驱动器、九轴惯性测量单元(IMU)、OLED显示屏、蓝牙单元通信模块等。系统使用DC+12 V输入，可直接使用电池供电。整个系统实测空载运行电流不超过1 A，总体功率不高，适合移动使用，可以应用到不同负载的应用场景，如负载较小的监控安防云台、视觉定位的底层驱动系统、负载较大的机器人头部传感器模块等，还能达到较高的精确度和响应速度。

图4 原型系统实物照片

3.1 基于步进电机的多自由度运动控制

云台采用步进电机控制各自由度运动，适用于负载需求变化较大的场合。此外，步进电机在步进驱动器的配合下可以达到较高精确度，且其每一执行步的误差不会积累；启动和停止响应快，不需要太复杂的控制算法。S曲线算法控制下的水平轴运动工况-时间曲线如图5所示，其中图5(a)给出角位移变化效果图，图5(b)给出运动速度变化曲线图，图5(c)给出运动加速度变化曲线图。

(a)水平轴角位移变化曲线图

示例中，水平方向步进电机从起始位置逆时针旋转0.31·360°，加减速阶段角位移和速度随时间变化的S理论仿真曲线(虚线表示)和实际测试效果曲线(实线表示)如图5所示，角位移、速度和加速度的实测数据曲线和理论曲线相契合，加减速阶段的对称性也可反映出步进电机累积误差很小，具有较好的控制精度。

3.2 自适应反馈控制锁定

在用户发送完动作指令后，云台系统接收指令并进行转动到目标位置，之后进入锁定状态。在未接收到用户移动指令时，如果系统外界环境发生三维空间内位置上的偏移，导致整个系统的基准面发生改变，云台的角度也会随之改变，从而无法达到目标位置。采用IMU获取系统基准面的三维空间位置信息，据此判断外界环境是否发生改变，当检测到变化后，启动经典PID负反馈控制[14]算法，保持云台的角度维持在用户所设定的目标角度，达到自适应反馈控制锁定功能。

3.3 系统资源的极小化运用

使用尽量少的接口、简洁高效的程序实现最核心实用的功能，在整个设计中仅使用到与步进电机控制相关的3个通用输入/输出接口以及2个定时器中断与1个异步串行传输通讯接口。从而可以极大地节省FPGA的资源，为更多外设应用分配出充足的开发空间，在后续处理过程中加入图像采集和视觉处理功能等，进一步优化控制算法。

4 结束语

使用开源无剑100 SoC作为主控核心，系统快速灵活编程，协同控制双轴运动。原型系统实际场景测试结果表明，可精简系统设计，实现较高精度和较快响应速度，以及基于IMU姿态的闭环反馈控制等。项目实践可进一步深入了解RISC-V架构，快速集成、快速验证，减少基础模块开发成本，实现精确快速监视、物体捕捉或者用于移动场景下的增稳控制，适合在AIoT环境下的实际应用。