两自由度茶园综合作业平台研制

杨拥军，潘 峰，吴文亮，罗妙辉

（1.湖南省农业科学院茶叶研究所，湖南 长沙 410125；2.上海赢浩机电设备有限公司，上海 201708；3.株洲市锦瑞机械有限公司，412000）

该茶园综合作业平台采用电动缸两自由度调控设计，能够隔离载体角运动在载体机动状态下建立稳定基准面，使安装在平台上的茶园作业裝备不会因茶园地表起伏产生的抖动和摆动而丢失目标，确保准确作业姿态，在茶园轻简化综合作业过程中得到很好的利用。

平台结构如图1所示，平台主要由下平台、上平台、电动缸、辅助支轴和铰链组成。调平平台外形尺寸约为400mm×400mm×388mm。根据茶园立地条件的要求，通过对调平平台的优化设计，可以得出运动范围满足要求、刚度大、可实现性好的调平平台的几何参数。即运动平台上连接电动缸的两个铰链中心与平台中心呈等腰直角形布置，下平台铰链中心与上平台铰链中心垂直对称布置。

使用Solidwoks建立调平平台三维模型并进行运动空间模拟，当电动缸行程为85mm时，可保证调平平台横滚、俯仰角度达到±26°。

下平台固定于自行式工作机架，是整个设备的基座。

分析结果表明，该系统结构简单，稳定性好，实时性强，具有良好的稳态和动态性能，能够满足茶园工作平台系统的性能要求。

图1 平台模型图

1 工作原理

两自由度茶园作业平台控制系统采用CAN0PEN总线通讯和数字输入输出功能的运动控制器、全数字电机控制器、倾角传感器、控制软件等组成。

平台的工作原理如图2所示。平台为一台两自由度并联机构，该并联机构的两个支腿为两个电动缸。两自由度运动平台在控制系统监测和控制下运行。控制系统为分层控制结构，监控单元负责人机界面交互，接受简述工作过程如下：用户操作指令，并将控制信号下达给实时控制计算机。

图2 系统原理框图

平台控制系统采用全数字伺服控制系统，姿态测量设备测量上平台实时姿态，通过串口通讯将实际姿态传输至微型控制器；控制器接收平台位置姿态后，经过空间运动模型变换，由反解运算解出补偿平台姿态所需两只电动缸的伸长量，通过总线传递给驱动器，由驱动器内部PC得到信息并驱动电动机转动，电动缸按照指令伸缩，平台亦运行到指定水平姿态。而安装在电动机上的编码器实时检测出电动机的力矩、速度、位置信息并发送到驱动器，构成闭环控制系统，以实时精确地控制各电动缸的伸长量，同时信息传输给微型控制器，微型控制器通过反解计算，得出上平台当前姿态，作为解算参考，以确保两只电动缸的协调动作以及控制精度。

两自由度茶园作业平台控制流程图如图3所示，主控制回路：倾角传感器测量平台实际姿态传输给微型控制器，微型控制器通过位姿转换和解算，计算出两只电动缸的运动参数（电动缸伸缩量），并通过CAN通讯把相应的位移量传递给相应的伺服驱动器，伺服驱动器控制电机的转动，进而控制电动缸的长度，以实现上平台两自由度角度调节。

图3 系统控制流程图

2 性能指标

2.1 运动能力指标

名称主要技术指标两自由度茶园作业平台性能指标（1）有效载荷：2 0 k g；总载荷：4 0 k g（含上平台自重）（2）运动参数：位移 速度俯仰（α）±2 5°±2 0°/s±4 0°/s 2 ≤0.1° ≤0.0 6°姿态 加速度 定位精度重复定位精度滚转（β） ±4 0°/s 2 ≤0.1°±2 5°±2 0°/s ≤0.0 6°其他技术指标（1）系统响应频率：0.0 H Z～2 0 H Z。（2）稳态精度：≤0.3°（静态）（3）装机功率：0.2 k w 输入电压：4 8 V D C（4）防护等级：I P 5 4（5）平台总重：约2 8 k g(不含控制箱重量)

2.2 组装形式

机械组件需具有开放性，可拆卸组装。

2.3 产品寿命

额定载荷下，全行程往复工作寿命不小于1×107次，存储寿命不小于48个月。

2.4 噪声

平台系统以20°/s速度运动时，距平台系统1m处的噪声强度低于65dB。

2.5 安全功能

2.5.1 机械结构安全性设计

所有关键受力部件都依照国标要求，机械设计安全系数不小于3.0，驱动裕度不小于3.0。

2.5.2 安全特性

为了保证系统安全、可靠的运行，防止系统失控时对平台结构产生破坏，系统设有三级保护功能：软件保护、电路保护和硬件保护。

软件保护：软件保护为第一级保护，是最基本保护功能，保护发生在系统控制软件运行周期内；实验运动平台系统控制软件设计时，对电机运行的最大行程、运行速度和加速度都设有阀值，防止电机运动时出现超行程超速运动引起的过冲现象。

电路保护：电路保护为第二级保护，保护发生在系统上电运行周期内；系统电机驱动控制器对电机输出码值、最大速度和加速度，最大输出电流设定有阀值，防止电机超速运动引起的过冲现象。

硬件保护：硬件保护为第三级保护，最高级的保护，保护发生在系统安装、验收完成后的全部周期内硬件保护，保证任何失控情况下实验平台结构不发生破坏，用户试验设备不被损坏，保证实验平台工作和保存的所有工况下平台结构不发生破坏。

2.6 环境条件

a.工作温度：-20℃～40℃

b.储存温度：-40℃～65℃

c.相对湿度：95%±3%（40℃）

2.7 可靠性和可维修性

a.平台系统一次连续工作时间不低于48h。

b.使用寿命不小于22000h。

c.平均无故障时间MTBF≥2200h。

d、平均维修时间 MTBF＜4h。

2.8 供电条件要求

a.直流 48VDC，波动范围：-10%～+10%；

b.容量 0.2kVA。

3 控制系统组成及技术组合

3.1 控制系统的组成

图4 六自由度平台控制器

多自由度运动平台控制器如图4所示，控制系统由运动控制计算机、全数字电机控制器、数字输入输出模块、CAN总线通讯模嵌入式块、操作系统、姿态测量系统和控制软件组成。

3.2 运动控制计算机

运动控制计算机设计方案需要满足如下几点要求：

（1）必须选择数学运算能力较强的控制器或处理器，必须采用工业级性能稳定可靠的主控芯片，且有足够的片内存储器。

（2）主控制器必须有丰富的外设，可以方便快捷地搭建硬件平台，简化硬件设计提高硬件系统稳定性。必须具有以太网、CAN通讯、串口通讯等常用的通讯外设接口。

（3）采用模块化设计，开放式硬件架构，各部分功能相互独立，必须具有良好的EMC特性。

（4）采集模块与主控制模块相分离，采集模块必须有较好的滤波特性和抗干扰能力，采集模块和主控模块之间的通讯必须安全可靠。

（5）充分考虑到硬件设计可靠性及抗干扰设计。

考虑到以上问题，采用基于ARM的嵌入式控制方案。嵌入式系统被描述为：“以应用为中心、软件硬件可裁剪的、适应应用系统对功能、可靠性、成本、体积、功耗等严格综合性要求的专用计算机系统”。系统的采集、处理、传输采用相对独立的模块化设计，具有良好的协调能力。图5为系统硬件方案的示意图。

图5 系统方案的示意图

3.2.1 基于Cortex-M4运动控制卡设计[1]

ST公司的STM32F4系列是高性能的基于ARM Cortex-M4的32位微控制器,工作频率高达168MHz。Cortex M4具有浮点单元(FPU)，支持所有的ARM单精度数据处理指令和数据类型，具有DSP处理指令和自适应的实时加速器(ART)。STM32 F4改进了一些外设，如全双工I2S、小于1 uA的RTC以及2.44 MHz/s采样率的ADC,STM32 F4的通信接口高达15个。STM32 F4还包含512KB～1MB的片内内存存储器，多达192KB的SRAM。系统采用STM32F4作为主控芯片。

3.2.2 主控制器总体设计

硬件电路由STM32F407微控制器和外围电路组成，主要包括：STM32F407基本电路、电源电路、时钟电路、复位电路、串口通信电路、以太网通信电路、JTAG调试接口电路、CAN总线通信电路、AD/DA电路、滤波放大电路、数字量输入输出电路等。图6所示为主控制器总体设计框图。

图6 主控制器总体设计框图

电源模块：主要是为整个硬件平台系统正常工作提供所有必须的电源，包括为主控芯片的工作提供稳定的3.3V工作电压，为放大电路的运算放大器提供±15电压。STM32F4主控芯片具有独立的模拟电源引脚，模拟部分和数字部分分开供电，这样有效提高了模拟系统的抗噪性。

串口电路：作为软件开发重要的调试手段，异步串行通信(USRT)在调试的时候可以用来查看和输入相关的信息，在本系统中通过一片MAX3232把串口1和2的3.3V电平转换为RS-232电平，它也是一个和外设(GPRS模块)通信的重要渠道。

CAN电路：CAN通信以最小的CPU负荷来高效处理大量收到的报文，同时减少了线束的数量，采用65HVD230驱动芯片连接到CAN总线上。

滤波放大电路：用于模拟量输入输出，将系统的0-3.3V模拟电压线性放大到系统所需的 10V模拟量信号。

数字量输入输出电路：提供8路光耦隔离的数字量输入信号，和8路数字量输出信号。

以太网电路：利用STM32F4处理器自身的互联特性与以太网控制芯片DP83848C实现以太网通信，通过外接的PHY接口，支持10/100Mbit/s的数据传输速率。

复位电路：系统内部具有完善的内部复位电路，外部电路采用简单的阻容复位方式用于手动系统复位。

3.2.3 基于MII以太网电路设计

以太网模块包括一个符合802.3协议的MAC(介质访问控制器)和专用的DMA控制器。该模块支持独立于介质的接口(MII)和精简的独立于介质的接口(RMII)，通过设置AFIO_MAPR寄存器可以选择使用哪种接口。

独立于介质的接口(MII)用于MAC子层和PHY之间的互联，允许10M位/s和100M位/s数据传输。图7为独立于介质的接口(MII)信号线示意图：

图7 独立于介质的接口(MII)信号线示意图

物理层接口芯片DP83848CVV与处理器接口部分硬件原理图如图8所示。

图8 以太网硬件原理图

3.2.4 光耦隔离CAN总线电路设计[2]

为了增强CAN总线节点的抗干扰能力，并不是在CAN控制器与CAN驱动芯片之间直接相连，而是加入了高速光耦隔离器。这样能很好地实现总线上各个CAN节点之间的电气隔离。在本研究中，系统选用高速光耦6N137，它可以很好的兼容TTL和COMS电平，信号宽度为10MHz，完全可满足CAN总线信号1Mb/s通信速率的要求。有效地提高了主控制器和采集模块之间通信的抗强干扰的能力，使得整个系统的可靠性和稳定性得到增强。采用光耦隔离的CAN总线电路示意图如图9所示。

图9 光耦隔离CAN总线电路示意图

3.2.5 数字量信号输入输出电路设计

由于外界环境以及电路、器件本身对系统引入了干扰和噪声，为了提高CAN总线的数据传输性能，保证其可靠性，采用光耦器件进行信号隔离来抑制干扰。光耦器件是将电信号转变为光信号，当光信号传送到接收方再将其转换为电信号。由于光信号的传送不需要共地，所以可以将光耦器件两侧的地加以隔离。因此，加入光耦器件后，在传输信号的过程中能有效地抑制高频脉冲和各种噪声干扰，大大提高通道上的信噪比。

图10为开关量输入信号电路示意图，VDD为工业24V开关电源正极，VDD为系统主控芯片GPIO电压3.3V，通过光耦器件将两侧的地隔离。

图10 开关量输入信号电路示意图

图11为开关量输出信号电路示意图，VDD为系统主控芯片GPIO电压3.3V，R1为上拉电阻，Q1为NPN三极管，通过光耦器件控制继电器通断来开启和关闭外部开关量信号。

图11 开关量输出信号电路示意图

3.3 全数字电机驱动器

电机驱动器选用深圳步科公司推出了新一代伺服控制系统SMC系列。步科SMCX系列主要使用调频调压以及调整相位的方法，通过速度反馈和位置反馈监控，控制电机的高精度运行。该系统在硬件的快速拆装性、控制的精确性和安全性，以及通讯协议的兼容性等方面都有了跨越式的进步，其中向导式操作和图形化界面可以大大方便用户编程和调试。此外，步科SMC伺服系统的模块化设计，也使现场总线协议、存储器容量和安全级别的选择更加灵活。高度的模块化还为将来的升级提供了保证。

步科SMC伺服系统的指令给定方式，除了使用内置于SMC伺服控制器内的IO点和CANopen总线接口之外，还可以使用扩展的通信接口兼容Profibus、Profinet、EtherCAT、DeviceNet、Ethernet、

TCP/IP、Ethernet、Powerlink 等总线系统进行通信。通过Ethernet(以太网)，可以实现远程的调试和诊断，确保高精度和高可靠性的运行。

3.4 调平系统

3.4.1特 点

该平台采用的倾角传感器系统是一款微型双轴倾角传感装置，选用北微生产的一款CAN输出的小体积双轴倾角传感器（BWM425S）来检测调平平台的水平面角度偏差，该倾角传感器采用成熟的工业级MEMS加速度计，测量范围±88°，最高精度0.01°，工作温度-40℃～+85℃，体积小，重量轻，可以满足对空间有限制的应用需求。本品通过把静态重力场的变化转化成倾角变化，以数字方式直接输出水平倾角数值，具有成本低、温漂小、使用简单、抗外界干扰能力强等特点。

3.4.2 主要特性

（1）双轴倾角测量

（2）最高精度0.01°

（3）分辨率：0.001°

（4）量程：±88°

（5）通信方式：CAN

（6）体积：L90*W40.5*H26(mm)

（7）供电电压：9-35V

（8）防护等级：IP67

3.5 控制软件及控制算法

3.5.1 调试软件

调平平台调试软件界面如图12所示，软件各模块定义及功能如下：

图12 监控软件界面

主操作窗口：主操作窗口主要包括平台的开机、关机、回零位、数据处理等操作输入按钮,见图13。

图13 主操作窗口示意图

监控窗口：主要用于监控系统当前状态，包括实时显示平台实际姿态、电机伺服状态、传感器测量值等。

演示窗口：演示输入窗口，可进行单轴正弦运动和多轴复合运动。对于单轴运动，输入幅值和频率后，平台开始做单姿态的正弦运动；对于复合运动，点击复合运动按钮即可，平台开始做任意姿态的组合运动。此功能主要用于平台性能测试之用。

单步运行窗口：包括单步点动输入及单步连续运动两部分。单步点动输入用于平台微动控制，步距及速度均可设置；单步连续运动，需设定目标位置与运行速度，平台自动平滑运行到指令位置。

主操作窗口：主操作窗口主要包括平台的开机/关机、回零位、暂停/恢复等操作输入按钮。

监控窗口：主要用于监控系统当前状态，包括实时显示平台实际姿态、电机伺服状态等，见图14。

图14 姿态曲线显示窗口示意图

单步运行窗口：包括单步点动输入及单步连续运动两部分。单步点动输入用于平台微动控制，步距及速度均可设置；单步连续运动，需设定目标位置与运行速度，平台自动运行到指令位置。

反解算算法

本方案的平台位置可分解为：

a.由电动缸控制的俯仰、横滚角度运动；

b.由回转平台控制的航向角度运动。

在本方案中，俯仰、横滚两个自由度的位置反解是指：在上平台的俯仰、横滚角度和机构的结构参数已知时，求解两个电动缸的伸缩长度。图15所示建立调平平台俯仰、横滚两个自由度运动系统的固定坐标系OXYZ和运动坐标系。

图15 俯仰、横滚机构原理图

O’X’Y’Z’，动平台分别沿 X、Y、Z 轴分别平移xp、yp、zp距离，且按序绕 X、Y 轴旋转 α、β 角度，则有：

式中，x1i、y1i、z1i分别为动平台三个铰点相对于动坐标系原点O’的空间位置坐标，x1i、y1i、z1i分别是动平台上三个铰点在动平台旋转后相对于固定坐标系原点O的空间位置坐标，H为动平台到回转平台的垂直距离，RXY为旋转变换矩阵，即

由于铰点D10在运动过程中空间位置保持不变，所以动平台不会产生附加移动，即xp=yp=zp=0，同时在虎克铰的约束下能完全克服绕X、Y轴旋转所带来的Z轴上的附加转动。

从上面的推到可以看出，已知平台的俯仰、横滚姿态可以唯一确定电动缸的缸长。

正解算算法

同样将平台位置可分解为由电动缸控制的俯仰、横滚角度运动。

沿用求位姿反解中的坐标系、坐标点和结构参数，已知电动缸的缸长l1,l2并得到动平台三个铰点在固定坐标系的位置x'1i、y'1i、z'1i以及在运动坐标系的初始位置 x1i、y1i、z1i，求解分别绕 X、Y 轴旋转的独立转角α、β。在运动坐标系中，已知初始位置D10（0，0，0），设D11（r,r,0）和设D12(-r,r,0)，由式（4-1）～式（4-3）可得到：

通过式（4-1）和式（4-2）可进一步推导出电动缸的缸长：

由式（4-4）即可得α、β的解析解，然后根据平台的结构参数即运动范围即可得到α、β的唯一解。

轨迹规划和插补算法

轨迹规划与插补算法的目的是让平台实现沿给定的位置指令平稳运动。对于给定的指令轨迹，需要选择合适的控制算法和参数，产生输出，同步控制各轴准确跟踪指令轨迹。

在运动控制中，常用的插补算法有逐点比较插补法、数字积分法和时间分割法。本方案使用时间分割法（亦称数据采样插补法）实现两轴轴联动控制，即将给定轨迹的运行时间细分为许多相等的时间间隔，称为单位时间间隔（或插补周期），每经过一个单位时间间隔进行一次插补计算，算出这段时间间隔内各轴的运动距离，直至整个轨迹运行结束。各轴在每个插补周期的启动和停止阶段分别会出现加速和减速的过渡过程。为了保证伺服系统运动的平稳性，避免冲击和振荡，做到启动不失步、停止不超程，必须对过渡过程进行有效的加减速控制。

数据采样插补可分为两个阶段：粗插补和精细插补。粗插补是用微小的直线线段逼近给定的轮廓，通过下位机软件实现；精插补是在上述微小的直线线段上进行“数据点的密化”。

采用数据采样插补算法，首先要解决的问题是选择合适的插补周期，本项目设计的调平平台采用位置控制模式，采样频率为1000Hz，插补周期为1ms，即主控程序运行时，插补程序每1ms执行一次，为下一个周期算出调平平台各轴的转动角度，同时位置反馈程序将实时采集到的编码器码制换算为当前各轴实际角度位置与目标位置进行比较。

3.5.2 通讯模块

上位主机与运动平台控制主机的通讯方式，用户通过运动平台的API接口，可以顺利地与运动平台建立连接关系。

网络通信接口协议如下：

位机固定IP：192.168.1.122

下位机监听端口号：5050

上位机固定IP：192.168.1.155

上位机监听端口号：6060

下位机接收数据结构体如下：

typedefstruct

{

BYTE sMark; //默认(=33)

BYTE sComd; //下位机接收的工作命令

BYTE sRepa[2];//默认值[0,0]

float sAtti[6];//指令姿态[α,β,γ,X,Y,Z]

float sVelo[6];//默认值[0,0,0,0,0,0]

float sAcce[3];//默认值[0,0,0]

}

下位机接收工作命令（sComd）解析如下：

{

sComd=2;//开机命令,平台开始初始化，并上升到工作位（中立位）

sComd=3;//握手命令，平台切换到工作态

sComd=4;//工作命令，平台在工作态，正常发送运动参数[α,β,γ,X,Y,Z]

sComd=5;//回中位命令，平台由任一姿态回到中位（工作零位）

sComd=6;//关机命令，平台运动到最低位，本次工作结束

}

监控计算机接收数据结构体如下：

typedefstruct

{

BYTE rMark;//默认(=33)

BYTE rComd;//上位机接收的工作状态

BYTE rRepa[2];//默认值[0,0]

float rAtti[6];//当前姿态[α,β,γ,X,Y,Z]

float rRese[6];//电机状态

float rBack[6];//电机码值

}

上位机接收工作状态（rComd）解析如下：

{

rComd=1;//待命状态,平台在最低位，等待开机命令

rComd=2;//开机状态，平台正在开机、初始化

rComd=3;//中位状态，平台开机完毕，等待握手命令

rComd=4;//工作状态，平台在工作态，可以执行姿态指令

rComd=5;//回中状态，平台正在回到中位（工作零位）

rComd=6;//关机状态，平台正在关机

rComd=8;//故障状态，关机重启

}

串口通信协议：1位起始位，1位停止位，8位数据位，1位奇校验位，115.2kbps波特率；

通信通道：COM1；

通信周期：10ms。

数据结构：

1.上位机接受串口数据定义

structDataToMain

{

BYTE rMark;//默认值(=33)

floatrAngXYZ[3];//接收姿态：

};

通信总字节数：13

控制器串口通信模块可使用232、485或422通信协议。

3.6 调平平台速度理论设计[3]

XY理论速度设计

图16 调平平台极限位置运动结构示意图

如图16所示，点O为上平台旋转中心，点A为调平平台旋转5°时铰链点位置，点A1为调平平台在5°边界摆动角度α后铰链点位置。调平平台在5°和-5°时，电动缸同样的行程对应的调平平台角度最小

当α角度极小时（假设为0.1mm），AA1=电动缸行程=角度α对应弧长，

电动缸速度0-250mm/s，即电动缸运动0.1mm所需最短时间为0.1/250=0.0004s，则调平平台零位时最大速度为 0.0318/0.0004°/s=79.5°/s。

3.7 精度分析

X、Y轴角度精度分析

图17 调平平台零位运动示意图

平台零位时，同样的电动缸行程对应的调平平台角度最大。如图17所示，零位时铰链点位置为A1，平台摆动角度α后铰链点位置为A，由17位伺服电机编码器，计算得到电动缸运动分辨率为5mm÷131072=3.8×10-5mm，当平台在零位，电动缸运动 3.8×10-5mm，平台对应角度为 3.8×10-5÷6.28÷180×360°＝1.2×10-5度，远小于技术指标要求，因此，平台精度理论设计完全满足使用要求。

3.8 平滑性

控制器接收监控计算机位置指令（指令周期为10ms）后，在控制软件中将位置指令分解为5步之后依次通过CAN总线发送到伺服驱动器（指令周期为2ms）；伺服电机在2ms控制周期内，由当前位置以S曲线运行到指令位置。即伺服系统的实际控制频率为200Hz，且每个控制周期内都由伺服驱动器实时进行路径规划，确保平台的光滑平稳运行。

4 结论

本项目是以稳定平台项目为依托，以实现平台的高精度为目的，对稳定平台控制系统的设计展开了研究。在以下方面有突破：

（1）对稳定平台控制系统实现数字化，克服了以模拟电路实现为主的传统的稳定平台控制方法中体积大、成本高、故障频繁、对微弱信号信噪分离困难，很难将控制精度提高到1‰以上的级别、模拟器件的工作状态极易受温度漂移的影响，使位置控制产生零点漂移误差等缺陷；鉴于国内大部分稳定控制系统的控制芯片为定点型芯片，其浮点运算能力有限，本设计采用浮点DSP以提高算法的计算精度。

（2）控制系统控制方案的选择对高精度的实现也至关重要。本设计采用电流环、速度环、位置环组合实现多闭环的复合控制方案。在位置环增加前馈控制器，对位置给定进行滤波和预测，以超前的控制作用来补偿系统的动态滞后,提高系统的响应速度，解决系统的稳定性和精度之间的矛盾。

（3）平台的环架位置测量采用旋转变压器粗机、精机组合方式提高测量的角度范围和精度。旋转变压器的测量精度达到0.0479mrad。

（4）信号经过一阶RC硬件滤波电路滤除高频干扰后输出，控制板采集的信号再在软件上采用滑动平均滤波、小波变换滤、FIR滤波等多种滤波算法滤除干扰，提高信号的采样精度。

（5）研究了平台最优安装位置。不同安装位置将导致不同的补偿角速度形式，最终影响系统的精度。本系统采用采用测量轴与俯仰框的OX轴平行结构，用于测量横滚角速度；测试装置安装在俯仰框(内框)上，测量轴与俯仰框的OY轴平行，用于测量俯仰角速度。

同时，稳定平台系统所要完成的任务是使得系统要求具有很高的稳态精度和较好的动态品质，同时又要有足够大的稳定裕度和抗干扰能力。因此该高精度稳定平台完全能够满足高刚度、高谐振频率、高稳定性、高强度茶园作业要求。