基于DSP的欠驱动体操机器人的摇起控制设计

1 引言
    欠驱动机器人是一种部分关节为被动关节，能用较少的驱动装置完成复杂任务的机械系统。欠驱动机器人是近年来开始出现的一个较新的研究领域，针对欠驱动机器人系统的研究目前得到不少学者的关注。欠驱动机器人系统在很多的机器人系统例如多指机器人手、轮式移动机器人、太空机器人等非完整约束系统中都存在。因此近几年来，欠驱动机器人的运动控制问题引起国内外广泛的研究兴趣。同时，随着集成芯片技术的飞速发展，人们对欠驱动机器人控制的实时性和精度提出了越来越高的要求。
    本文设计了一种基于DSP的机器人控制系统。该控制系统采用两级控制结构。以通用PC作为上位机，完成目标设定、轨迹生成、系统管理和人机接口等功能；以美国Analog Device公司的定点数字信号处理芯片ADSP2181为核心，作为机器人控制的下位机。该控制器充分应用了DSP运算的高速性，提高了系统的实时和稳定性。

2 体操机器人硬件系统设计
2．1 控制系统硬件结构
    控制对象为三关节欠驱动体操机器人，其中手臂关节为被动关节。控制转矩来自两台直流伺服电机，配5l：l减速器伺服电机，经轴线相交的圆锥齿轮传输至驱动关节，驱动关节通过动力耦合作用使被动关节产生运动。欠驱动机器人控制系统的任务就是对驱动关节电机进行控制，电机运行之前从控制界面输入控制目标参数，上位PC机负责发送控制命令与数据采集。DSP处于整个控制系统的底层，主要用来接收来自PC机的控制命令，对驱动关节执行电机控制，同时把底层信息反馈给上位PC机，以实现机器人运动信息的数据分析等功能，用一块DSP运动控制卡对机器人的肩关节或髋关节进行插补计算和伺服控制，采用PCI标准总线进行上、下位机的通讯，实现双速率运行。控制箱面板提供了各关节工作状态显示及伺服报警提示，还可以对关节进行手动控制。三关节欠驱动体操机器人控制系统结构如图l所示，体操机器人本体如图2所示。

2．2 运动控制器结构
    系统采用ADSP218l数字信号处理器为核心，实现高性能的控制运算的伺服运动控制器。如图3所示，运动控制器的控制过程为增量编码器的A、B相信号作为位置反馈输入信号，运动控制器通过四倍频、加减计数器得到实际位置。实际位置的信息保存在位置寄存器中，PC机可通过控制寄存器读取。运动控制器的目标位置由PC机设定，通过内部计算得到位置误差，经过数字伺服滤波器后，送到数模转换(DAC)或脉宽调制器(PWM)硬件处理电路，经过转换最后输出伺服电机的控制信号：+／-lOV模拟信号或PWM信号。

2．3 系统的通讯
    系统采用PCI总线进行通讯。PCI总线的主要优点是性能高(数据传输率可达到132 264Mb／s)，总线通用性强，成本低，使用方便灵活。系统通讯采用PLX9054接口芯片，结合双口RAM，实现了DSP和PCI总线间的双向高速实时数据交换。PCI总线与双口RAM的数据交换，采用了定时传送加握手信号的方式进行。具体实现如下：上位机每隔一个固定的时间T下传一组数据，数据传送完成后，发出一个发送完信号，下位机接受到这个数据后，立即从双口RAM中读取数据。下位机上传数据也采用同样的处理方式。这种方式特别适合于机器人控制系统的通讯。
2．4 驱动元件的选择
    驱动元件选择了直流伺服电机，其参数为200W／7220mm／107mN·m Maxon。其驱动器可实现位置、速度和转矩三种不同的控制方式：具有共振抑制和控制功能，可弥补机械的刚性不足，从而实现高速定位。同时，还采用PID滤波器，外加速度和加速度前馈，即PID+Kvff+Kaff滤波器。通过调节各参数，滤波器能对大多数系统实现精确而稳定的控制。因此非常适合应用于机器人控制系统。

3 摇起控制策略
    由于体操机器人大范围的运动，摇起问题是高度非线性又极具挑战性的问题。摇起过程如同人在单杠一样，先使体操机器人来回摇动几次，体操机器人始终在下，一旦有足够的能量施加到系统上，机器人即进入倒立状态，如图4所示。

    Spong建议一种非线性反馈方法，这种方法利用部分线性反馈定义PD控制器。从直观上讲，当体操机器人从悬挂稳定平衡状态转移到倒立不稳定平衡状态的过程中，其势能是不断增加的，因此需要向系统输入足够的能量。本研究从能量增加的角度出发，采用带有振幅和频率的正弦方式和斜坡函数输入，这样可同时增加摆动。
    根据拉格朗日运动方程式可推导出三关节机器人的动力学模型：

   
τ为系统的广义外力，体操机器人在运动过程中的总能量为：

   
Ki，Pi分别为第f个关节(手臂、躯干、腿)的动能和势能。
    在整个摇起过程，为保证能量不断增加须满足，能量的导数必须满足如下条件：

   
    根据文献的研究结果：利用M(q)一C(q，q)为反对称矩阵，有：

   
    显然，为满足能量不断增加的不等式条件，摇起控制转矩可选择为：

   
式中sgn()为取符号函数，Ni为附加力。
    实验证明采用正弦和斜坡函数，此算法位于基于能量摇起模块void swing_up()中，这些数据分别从运．动控制器的2、1通道输入，需说明的是，在对体操机器人系统进行数学建模时已明确，体操机器人系统是一个力控设备，根据牛顿第二定律，力与加速度成正比，因此给系统施加的控制量就是加速度，而速度则是一个事先设定的较大数值，在控制过程中，一般不会达到这个速度值。这样保证产生的附加力随能量的增加而减小，可摇起体操机器人到倒立平衡位置。

4 控制系统软件设计
    实施控制时，主要是控制软件的编写与控制其参数的调节。由于体操机器人控制系统的实时性要求较高，控制软件必须满足实时性的要求，本系统控制周期为6ms。控制软件必须在准确的控制周期内完成数据采样，处理并且发出控制信号给运动控制器。
    首先由上位PC机设置好控制参数，系统开始运行并完成初始化工作。底层控制器对各转动关节进行位置采样，同时接收来自上位PC机的控制指令，并把两者结合在一起进行分析，通过编写进去的控制算法生成相应的转矩控制信号，经功率放大后送给执行电机，同时把各关节的运动信息上传给PC机，如此反复，完成整个闭环运动控制。
    下位机控制器上电后主程序进行控制器初始化操作、禁止看门狗、设置关键寄存器、设置中断向量和中断寄存器、初始化事件管理器、基于能量摇起、进行实时控制模块等。其中实时控制模块中用到ADSP2181可编程定时器，它能够产生周期性的定时中断，定时间隔是处理器时钟周期的整数倍。当定时器被使能后，一个16位的计数寄存器TCOUNT每隔m个周期就会减1，其初始化程序如下：
void init_interrupt()
{disable0；∥关闭中断(在初始化中断过程中严禁产生中断)
outportb(0x43,0x36)；／／写中断控制字
outportb(0x40,0xf6)；∥写数据寄存器：0x40为数据寄存器内存映射地址
outportb(0x40,0xlb)；∥数据为16位时钟细分比率，先写入低字节0xf6，后写入高字节0xlb
oldhandler=getvect(INTR)；∥保存原有的中断服务程序句柄
setvect(INTR，handler)；／／设置新的中断服务程序句柄
enable()；∥打开中断}
    上述中断初始化代码创建一个6毫秒的定时器中断，即当打开中断后，将每隔6毫秒对体操机器人系统进行一次实时控制。6毫秒的定时时间是通过向数据寄存器写入0x1bf6=7158获得的。

5 结语
    基于高性能ADSP2181的控制器设计能够满足机器人的摇起控制要求，且由于采用PC+控制器分级控制和模块化设计思想，将有利于软、硬件升级，及大大缩短开发周期。系统在实验室进行机器人摇起实验时，各电机轴运转平稳，动作协调轨迹跟踪实时性好，机器人没有出现抖动、喘振等现象。
    今后还需要探讨更多的摇起与平衡控制策略，完善DSP的控制程序。