基于DSP的Stewart平台控制系统研制

2018-04-25，，，，

计算机测量与控制 2018年4期

，，，，

(北京环境强度研究所，北京 100076)

0 引言

六自由度 Stewart 平台是一种几何外形为八面体结构的六自由度并联系统，平台具有负载能力强、响应速度快、灵活等优势，可应用于飞机、轮船、汽车等可靠性测试实验中[1]。六自由度Stewart并联机构常采用气动、液压和电动三种驱动方式，驱动方式的选择，对平台的造价、体积、承载能力等方面具有较大的影响[2]。其中，气动驱动方式具有实现简单，质量较轻、成本较低的特点，但由于空气的可压缩性，使得平台难以实现高精度控制；液压驱动方式具有较大的输出功率、具有较大的调速范围和响应速度快的特点，可实现高精度定位控制，但液压系统的油源较难获取，价格昂贵，液压系统的体积通常较大，且实现具有较大的难度，出故障较难排查，维护成本高。电动驱动方式的电源获取比较简单、成本较低、实现较容易，不仅如此，电动六自由度运动平台体积较小，运输和后期的维护成本都较低。在Stewart平台中，已知驱动推杆的杆长，求解运动平台相对于静平台的位置和姿态，是并联机构的研究的热点和难点。在求解位置正解分析中，主要采用的方法有解析法、数值分析法、冗余传感器法和智能算法等[2]。解析法注重得到全部正解，计算量大、运算时间长，求解过程比较复杂，不适合实际应用；冗余传感器法通过在铰接处增加传感器，通过传感器测量旋转角方式求出动平台的位置[3]。智能算法的求解过程较复杂、运算时间较长。在实际工作中，平台对控制命令的响应速度，对平台的控制精度和速度具有重要影响，这就要求正解算法在可靠的同时，简洁、运算速度快。本文将目标定为动平面三点的空间坐标,共含9个变量，并将二次方程转化为线性方程，对于产生的奇异解作了方程补偿，在保证计算准确性的同时，减少了运算次数，提高了效率。Stewart平台控制系统采用直流电机作为平台的驱动装置，支链采用的是电动推杆执行机构，采用DSP作为控制系统实现对电动推杆的实时控制，采用3个LN298双H桥路驱动模块来实现6路电动杆电机的正反转操作，为方便对平台的运动位姿的控制，在PC端通过LabView编写用户数据输入软件，用户通过对平台空间坐标系数据的输入，控制六自由度电动平台进行相应轨迹的运动。

1 Stewart平台控制算法

六自由Stewart电动平台由以下5个部分组成：上平台为动平台，用于安装负载，在电动推杆的驱动下完成位姿的运动；下平台为静平台，是六自由度平台的基座，用于固定整个系统[4]。Stewart电动平台采用电动推杆可伸缩支架，通过驱动器对电动推杆的驱动，驱动上平台做各种运动。

六自由度Stewart运动平台的核心算法为给定平台位姿后，求电动推杆的目标长度，并将这一长度作为指令通过控制器驱动电动推杆。对六自由度Stewart运动平台建立坐标系，下平台为惯性坐标系O，上平台为连体坐标系O’，如图1所示。平台坐标系用(x,y,z,α,β,γ)表示，动平台沿x轴进行纵摆运动，沿y轴进行横摆运动，沿z轴进行升降运动。绕x轴运动与上平台平面形成角α，绕y轴运动与上平台平面形成角β，绕z轴与上平台平面形成角γ[5]。

图1 Stewart运动平台坐标系

当上平台仅沿x,y,z三个轴方向进行平移运动时，得到连体坐标系与惯性坐标系之间的平移变换矩阵为：

上平台分别绕x,y,z轴旋转得到旋转角α,β,γ三个坐标转换矩阵的总旋转矩阵为：

则平台惯性坐标系和连体坐标系之间的变换矩阵T=T1*T2。假设六自由度运动平台的上平台半径为R1、下平台半径为R2、上铰和中心的夹角为θ1、下铰和中心的夹角为θ2、初始支杆长L, 静平面铰接点为A1、A2、A3，动平面铰接点为B1、B2及B3。则上、下平台6个铰点的坐标分别为：

B1=[-R2*sin(θ1/2/180*π) -R2*cos(θ1/2/180*π) 0];

B2=[R2*sin(θ1/2/180*π) -R2*cos(θ1/2/180*π) 0];

B3=[R2*cos((30-θ1/2)/180*π)R2*sin((30-θ1/2)/180*π) 0];

B4=[R2*cos((30+θ1/2)/180*π)R2*sin((30+θ1/2)/180*π) 0];

B5=[-R2*cos((30+θ1/2)/180*π)R2*sin((30+θ1/2)/180*π) 0]；

B6=[-R2*cos((30-θ1/2)/180*π)R2*sin((30-θ1/2)/180*π) 0];

A1=[-R1*cos((30+θ2/2)/180*π) -R1*sin((30+θ2/2)/180*π) 0 1];

A2=[R1*cos((30+θ2/2)/180*π) -R1*sin((30+θ2/2)/180*π) 0 1];

A3=[R1*cos((30-θ2/2)/180*π) -R1*sin((30-θ2/2)/180*π) 0 1];

A4=[R1*sin(θ2/2/180*π)R1*cos(θ2/2/180*π) 0 1];

A5=[-R1*sin(θ2/2/180*π)R1*cos(θ2/2/180*π) 0 1];

A6=[-R1*cos((30-θ2/2)/180*π) -R1*sin((30-θ2/2)/180*π) 0 1];

通过上平台6个铰点的坐标及旋转矩阵转换，可计算出六个推杆各自的位移。杆长的计算公式为Ln,式中n为1～6，分别对应L1～L6，则电动推杆的位移为Ln’=Ln-L。

2 系统软硬件构成

系统由六自由度Stewart平台和控制软件两个部分组成。平台采用6个电动推杆作为执行机构，电动推杆选用TGA-DW-150 mm为直流电机驱动推杆，工作电压为24 V，最大推力为900 N，空载速率为29 mm/s，最大行程为150 mm,初始长度为352 mm，重量仅有1.1 kg。电动推杆输出端为10 kΩ电位器，电动推杆行程的变化，使得电位器中心与两端电阻值发生变化，在电位器两段施加3.3 V电压，从而将电动推杆杆长的变化，转换为电压值的变化。电动平台的上平台、下平台铰夹角为20度。上平台半径为180 mm。下平台半径为220 mm。平台最大翻转角可以达到±20°。

系统采用3个LN298双H桥路驱动模块，来实现6路电动杆电机的正反转操作，每个LN298双H桥驱动模块可单独控制2台直流电机，芯片工作电压为DC 5 V，电机驱动电源电压DC 5～35 V，最大输出电流2 A，最大输出功率为25 W。

图2 控制系统结构图

控制软件分为上位机数据输入软件和数控软件。上位机数据输入软件和下位机软件之间采用应答方式，并采用数据包方式进行交互。数据包大小为13 B，用来承载用户在上位机软件中设置的电动推杆的目标位姿数据(x,y,z,α,β,γ)。数据包由包头和位姿数据两部分组成，包头采用255，位姿数据为x小数部分，x整数部分，y小数部分，y整数部分，依此类推。系统结构如图2所示。用户通过上位机软件输入电动推杆的目标位姿坐标(x,y,z,α,β,γ)并对串口参数进行配置。配置完成后，通过串口将数据包下发给TMS320F28335控制器，等待下位机的响应信号。若收到下位机的响应信号0x55，说明数据下发成功，且完成该点的位姿控制。如果此时数控系统处于忙状态，需要等待其空闲时将数据包下发，这样可以保证位姿控制数据不丢失的同时，实现对电动推杆位姿的精准控制。下位机控制软件在空闲时通过串口向上位机发送应答信号，通知上位机发送数据，通过串口收到位姿数据后，对数据包进行解析得到坐标数据，并对坐标数据进行解算得到电动杆的目标位置杆长，将该杆长转换成电压值，与ADC采集到的电压值反馈信息进行实时对比，根据两个电压的差值对电动推杆进行PWM控制。系统选用的TMS320F28335型数字信号处理器是TI公司推出的32位DSP芯片。该DSP具有外部设备集成度高，成本低、功耗小、性能和精度高的特点。该DSP主频高达150 Mhz,具有32位浮点处理单元，多达18路PWM输出,以及16通道12位ADC[6]。

2.1 上位机软件设计

上位机软件通过LabView编写，软件界面如图3所示，用户通过上位机软件对六自由度平台进行位姿控制，可通过上位机控制软件实现对6自由度电动杆的位姿坐标的定点输入，即x,y,z,α,β,γ位姿坐标信息，也可以通过选择按钮选择不同轴向的正弦信号，来实现电动杆不同姿态的运动坐标信息的生成，用户可在上位机软件中，对正弦信号的幅值、频率、采样频率进行设置。上位机软件通过串口和下位机控制器进行通信。在进行位姿控制时，用户首先对串口进行配置，统一上、下位机的串口参数，包括连接串口的识别，串口的波特率、校验位、停止位的设置。串口参数配置完成后，开启读取，启动接收操作，等待下位机的空闲状态控制字0x55，收到该控制字后，状态指示灯亮，表示下位机与上位机串口通信成功，且此时下位机没有进行位姿控制操作，若没收到，则对串口进行循环读取检测，直到收到下位机空闲状态控制字。随后需要对平台进行对中操作，使得电动平台上平台位置位于中心位置，此时x,y,z三轴偏移量为0，α,β,γ旋转角为0度。用户点击对中旋钮，软件将平台中位坐标信息生成数据包，并通过串口将参数下发给控制器，收到控制器对中完成反馈后，状态指示灯亮。在每次实验开始前都需要对平台进行对中操作，在实验过程中，也可以对平台进行对中操作。对中完成后，用户可根据需要，在上位机软件上对平台的目标位姿进行参数设置，点击写入按钮，将平台位姿参数生成数据包，通过串口下发给下位机，完成位姿参数的下发。

图3 上位机软件界面

2.2 数控软件设计

数控软件在Windows环境下通过CCS软件编写完成，采用模块化结构，将软件分为5个模块进行实现，分别为主程序模块，PWM控制模块，ADC模块，串口通信模块以及位置解耦算法模块。其中，主程序模块负责整个系统的程序调度，PWM控制模块实现PWM初始化，PWM脉宽信号的输出，并可以根据电动杆和目标位置的距离进行PWM控制。ADC模块负责控制ADC采集和采集数据的平滑计算。串口通信模块用来实现上下位机的数据通信以及数据包解析。解耦算法模块将平台位姿坐标信息进行解算，得到此时电动推杆的杆长。

主程序模块负责对系统的各个模块进行调度，系统上电后，完成对系统的时钟工作在150 Mhz、配置串口接收中断、进行GPIO的初始化和配置，开启ADC采集模块、PWM模块以及串口模块，同时每隔100 ns，通过串口向上位机发送0x55空闲状态字，直到收到上位机数据包信息。下位机串口收到上位机发送的平台位置信息后，对数据包进行解析得到平台的目标位置坐标信息，通过解耦算法模块对位置信息进行解算，并将长度信息与AD模块采集的信息进行比较，根据比较结果对6路PWM进行相应的控制，直到电动推杆达到设定位置，回到开始状态并向上位机发送0x55来表示下位机处于空闲状态。

图4 软件程序流程图

串口通信模块采用全双工通信模式，通信过程采用F28335处理器提供SCI串行通信接口，每个SCI串行通信接口的接收端和发送端都具有16B的FIFO缓冲，为减小CPU开销，在接收上位机数据包时，采用中断方式。使能SCI串行通信接口接收端FIFO中断，即当FIFO接收13字节数据后，向CPU发出中断请求。系统监测查询FIFO接收中断，判断中断是否为高，当中断为高时，读取FIFO中的数据。检测第一字节数据是否为255，当其值为255时，表示收到了用户设定的运动平台位姿坐标信息。将该信息中相应轴的小数部分和整数部分合并，解析出x,y,z,α,β,γ位姿坐标信息。

位置解耦算法模块将串口解析的位姿坐标数据，通过解耦算法，解算出电动推杆的目标长度Ln’，并将目标长度Ln’转换成目标电压值V，V=Ln’/L*(voh-vol)+vol，其中L为电动推杆长度；voh为电动推杆达到最大行程时对应的电压值；vol为电动推杆位于初始位置时对应的电压值。由于6个电动推杆之间有一定的误差，每一个电动推杆的voh和vol值均需要进行测量。

PWM控制模块负责输出12路PWM信号，通过12路PWM信号对6个电动推杆的行程进行分别控制，每一个电动推杆由PWMa、PWMb2路信号进行控制:PWMa输出高电平，PWMb输出低点平时，电动推杆伸长；PWMa输出低电平，PWMb输出高电平时，电动推杆缩短；PWMa和PWMb电平相同时，电动推杆静止。同时，可根据PWM高、低电平的占空比，对电动推杆进行调速控制。由于LN298双H桥路驱动模块为门控桥路，每一个H桥路根据PWMa和PWMb电平变化，实现两路24 V电动杆驱动电平的输出。由于当PWMa和PWMb同时为高电平时LN298桥路出现死区，因此在进行PWM信号输出时，使得2路PWM电平在进行翻转时，上升沿信号延迟大于下将沿信号延迟，且在一路信号输出PWM波形时，令另外一路信号输出为低电平信号。

ADC模块负责采集6个电动推杆反馈端的输出电压值，并将该电压值和目标电压值进行比较，根据比较的差值对PWM进行控制。为提高采样值的精度，对每个通道ADC数据进行20次采样并将采样值求和再求平均，得到的数值为该点的电压值。将该点的电压值和电动推杆目标位置电压值进行比较，差值在0.01 V之内，认为电动推杆已经达到目标位置，否则继续对电动推杆进行PWM控制。进行PWM控制时，根据采集电压差值和电动推杆的电压差值的大小，对输出的PWM波形高、低电平占空比进行调整，调成范围为100%～25%，以电动推杆进行伸长操作为例，即差值最大时，PWMa输出为高电平，此时占空比为100%，此时电动推杆的行进速度最快。在差值接近0.01 V时，减小PWM占空比，降低电动推杆的行进速度，以保证对位置的精确控制。通过对PWM的CMPA、CMPB参数进行修改，即可实现对PWM占空比的实时设置，CMPA=12000-PwmposC2/200,CMPB=12000,其中，PwmposC为ADC采集电压和推杆目标电压的差值。

3 试验结果分析

将Stewart平台控制系统应用到六自由度Stewart控制台中进行实验，处理器采用150 MHz时钟，ADC采样速率选用12.5 Mbps,上下位机通信速率最高为38400 bps,PWM输出控制信号为1 kHz,当目标设置为Z轴正弦运动、幅值为70，频率为10 Hz时得到的单轴电动杆反馈电压曲线如图5所示，可以看出电动杆具有很好的位置跟踪能力。Stewart平台实物运动如图6所示。

图5 反馈曲线图

4 结束语

本文分析了Stewart六自由度运动台的运动特性，对平台正解算法进行改进，并建立了基于DSP的控制系统。该Stewart平台控制系统采用直流电机作为平台的驱动装置，支链采用的是电动推杆执行机构，采用DSP作为控制系统实现对电动推杆的实时控制，采用3个LN298双H桥路驱动模块来实现6路电动杆电机的正反转操作，为方便对平台的运动位姿的控制，在PC端通过LabView编写了用户数据输入软件。将该系统应用到六自由Stewart运动台实验中，验证了该系统的稳定性和控制能力，试验表明该系统具有较高精度定位控制能力。