一种六自由度机械臂的控制系统设计

龙 腾

(长沙理工大学 电气与信息工程学院，湖南 长沙 410005)

0 引言

机械臂作为工业自动化的一种设备，在整个工业领域有着非常广泛的应用。实现对机械臂的精准调控是当下研究比较多的课题，也是提高生产效率，改善产品品质的一种方式[1]。针对机械臂的精准调控，人们进行了大量的研究，当下研究比较多的是滕冠[2]等人提出的基于模糊PID控制算法进行机械臂的运动控制，采用角度测量传感器作为反馈单元，然后将输入参数和反馈参数进行比较，再传递给模糊PID核心算法，实现对机械臂的运动控制。李进生[3]等人提出的机械臂运动轨迹的算法设计，通过分析机械臂的正运动学和逆运动学，然后建立对应的仿真模型进行轨迹设计，实现机械臂的轨迹更优、避障等功能。陈杨洋[4]等人实现了基于总线控制的机械臂控制系统，该系统采用PROFIBUS总线实现对机械臂运动轨迹的在线实时监测。上述方法都能较好地实现对机械臂的运动控制，但是运动精度和响应效率不是那么理想。

针对以上分析，本文选用意法半导体公司的STM32F4作为主控制器，关节姿态信息检测器选用陀螺仪传感器，关节的运动控制采用舵机驱动模块，以上三部分构成了机械臂运动单元的硬件控制系统。机械臂运动单元的软件部分采用扩展卡尔曼滤波算法，将陀螺仪采集的关节状态参数作为反馈参数，传递给扩展卡尔曼滤波算法模块，进行预估机械臂的运动状态，实现对机械臂运动过程中的偏差调控[5-6]，这样从硬件和软件两个方面保证了机械臂控制系统的闭环控制。

1 系统整体设计

图1所示是整个机械臂控制系统的架构图，从图中可以看出，整个控制系统分为4层，分别为用户层、估计规划层、关节控制层以及关节状态检测层。用户层主要是用户输入想要机械臂实现的运动轨迹；轨迹规划层主要是根据机械臂的逆运动模型和用户层的输入，进行运动轨迹规划，同时还具有预估机械臂的碰撞状态；关节控制层主要是根据轨迹规划层和关节状态检测层进行轨迹调控。关节状态检测层主要是作为反馈环节，给轨迹规划层和关节控制层提供关节的当前状态信息。而关节控制器层的调控，本文选用扩展卡尔曼滤波算法进行估算和调节。

图1 机械臂控制系统架构图

2 系统详细设计

机械臂的控制系统主要包括硬件部分和软件部分，根据机械臂的设计需求，下面分别从硬件和软件两个方面进行阐述。

2.1 硬件设计

图2所示为机械臂的控制系统硬件构成。用于和用户交互的液晶显示部分选用正点原子设计10寸的触摸屏，主控制器选用ST公司的STM32F4，STM32的时钟可以达到168 MHz，能够处理各种复杂的指令，响应速度非常快，舵机驱动模块主要是实现对机械的运动控制，主控制器通过脉宽调制波(PWM)进行对舵机的调控[7]。陀螺仪主要是作为反馈单元，采集机械臂的运动状态，给轨迹规划层和关节控制层提供关节进行决策使用。陀螺仪传感器和主控制器之间采用I2C接口进行通信，I2C总线的通信主要是通过时钟线和数据线两根线进行的，在空闲的时候，时钟线和数据线都是高电平，当需要进行数据通信时，都是在时钟的边沿触发的。舵机驱动器和主控制器之间采用PWM进行通信，PWM主要是通过调节主控制器输出的脉冲宽度进行对舵机驱动器的控制。

图2 机械臂控制系统硬件框图

2.2 软件设计

机械臂控制系统的软件设计主要包括硬件各个模块(液晶显示模块、舵机驱动模块、陀螺仪数据采集模块等)的驱动程序设计和EKF算法的设计。软件设计的主流程图如图3所示。

图3 机械臂控制系统软件主流程图

系统主流程首先实现主控制器硬件的初始化，然后等待用户输入对应的轨迹坐标，接着陀螺仪开始采集关节的状态，将采集的参数传递给EKF算法，主控制器根据EKF算法输出的结果判断是否需要进行舵机调控位姿，最后系统接着检测是否有新的用户任务进来，这样机械臂控制系统进行循环运行，直到到达设定的坐标位置。

EKF算法是在卡尔曼滤波算法的基础上扩展出来的，该算法是一个随着时间变化的离散的动态系统，它当前的系统状态可以由系统的过程方程和描述系统的观测方程进行表述，如式(1)所示：

y(n)=C(n)x(n)+v2(n)

(1)

式(1)中，y代表一个NX1的向量，表示系统在n时刻的观测向量矩阵;C代表的是系统的观测矩阵;v2代表的是扰动信号向量，它的矩阵维数大小和观测向量的维数一致。

状态方程为：

x(n+1)=F(n+1,n)x(n)+v1

(2)

式中，x代表一个MX1的向量，表示系统在n时刻的系统状态向量矩阵；F是一个MXM的常量矩阵，定义为状态转移矩阵；v1表示带入系统的扰动信号向量。

式(3)是基于条件概率模型，也是贝叶斯先验模型[8]。已知x、y事件发生的概率，并且知道在x事件发生的情况下y事件发生的概率，如果已知上面的三种概率事件，就可以通过贝叶斯先验模型推导出在事件y发生的情况下x发生的概率是多少。EKF就是基于以上概率模型进行条件预测的。

(3)

3 系统验证与分析

该系统采用扩展卡尔曼滤波算法，将陀螺仪采集的关节状态参数传递给EKF核心算法进行预估机械臂的运动状态，实现对机械臂运动过程中的偏差调控。本文分析和仿真了滕冠[2]等人提出的基于模糊PID控制算法进行机械臂的运动控制，通过和本系统提出的EKF算法进行对比分析，发现该系统在控制精度上优于模糊PID的控制策略。图4为滕冠[2]等人提出的基于模糊PID控制算法仿真结果，从图中可以看出，模糊PID在整个轨迹调节过程中波动相对较大，实际轨迹和调整后的轨迹偏差达到25 mm，偏差的中心值在10 mm。图5为本文提出的基于EKF算法的仿真结果，相比模糊PID算法，本文提出的EKF算法波动相对较少，实际轨迹和调整后的轨迹偏差最大达到18 mm,偏差的中心值为6 mm。

综上所述，相比于模糊PID控制算法，本文采用的EKF算法在机械臂精度调节、轨迹平滑度等方面具有一定的优势，对于机械臂的性能优化具有一定的实际意义。

图4 模糊PID控制算法仿真图

图5 EKF算法仿真图