三线式三维运动协调控制系统的设计与研究
李嘉楠+王富东+朱成海+杨歆豪
摘 要： 为改进当前固定轨道式的工业货物搬运方式，实现仓库货物按任意设定的三维轨迹进行精确搬运的目标，提出三线式三维悬挂运动控制系统的模型。以DSPIC30F4011单片机为控制核心，Futaba S3010舵机作为执行机构，张力检测机构作为保护机构，LCD屏和按键作为设定显示机构，搭建系统模型，根据模型建立数学模型，并进行相关算法的设计，最终实现三线式三维运动协调控制。
关键词： 三线式； 运动控制； 舵机； 圆轨迹方程
中图分类号： TN02?34； TP23 文献标识码： A 文章编号： 1004?373X（2017）13?0148?04
Abstract： In order to improve the current industry goods transportation way with fixed track， and realize the target that the warehouse goods can be transported according to the 3D track set arbitrarily， a model of trilinear 3D suspended motion control system is put forward. To construct the system model， the DSPIC30F4011 single chip microcomputer is taken as the control kernel， the Futaba S3010 steering engine is employed as the actuator， the tension detecting mechanism is acted as the protection mechanism， and the LCD screen and key are used as the setup display mechanism. According to the system model， its mathematical model was established. The relevant algorithm was designed to realize the trilinear 3D motion coordination control.
Keywords： three?line type； motion control； steering engine； circular path equation
0 引 言
我国在贵州安装了世界最大的射电望远镜，其直径长达500 m，需要安装4 450块反射板，并且安装精度必须达到5 mm。诸如这样的浩大工程，如果安装固定轨道来完成反射板的安装，其工程量将极为浩大，并且精度难以达到要求。同样，在工业应用中，对货物的搬运方式也是层出不穷。简单的诸如车间的货物有的采用吊车或者起重机等设备[1]，通常这些设备的运动都是靠手工操作，对操作者的劳动强度较大[2]。当然，各行各业自动化的货物搬运方式也在不断发展，例如皮带式装车辅助设备、真空吸盘助力搬运设备[3]、地链搬运系统[4]、堆垛机控制系统[5]等，增加了货物搬运的自动化程度，为人们的生产带来了方便。但是，这些方式都需要固定轨道，固定轨道搬运控制不灵活、成本较大、安装复杂，难以实现精确的任意轨迹的三维搬运。
因此，本文提出一种新的三线式三维运动控制的货物运输搬运控制思路，以三台舵机输出轴旋转角度为直接控制量，通过与舵机输出轴相连的杆作为传动机构，实现对货物运动控制的目的。研究的难点在于协调三台舵机的输出轴的旋转角度关系，使货物按照指定的路径运动到指定位置。根据上述思路，本文进行了示例系统模型的搭建。通过将三台Futaba S3010舵机分别安装在不同位置，其输出轴连接连杆，杆的另一端连接绳，绳通过一个固定滑轮连接到货物，通过连杆的旋转带动货物的运动。通过三台舵机各自旋转相应的角度，达到货物的某一轨迹的运动效果。本文以倾斜的圆轨迹为例实现示例的功能。
1 建立数学模型
1.1 系统整体结构
根据系统的设计需要，搭建如图1所示的系统。在图1系统结构中，是一个等边三角形，点为等边三角形的中心。为避免线直接缠裹在输出轴上导致轴半径变化造成误差，在舵机输出轴上安装连杆。分别从与舵机A、舵机B、舵机C输出轴相连的杆的另一端引出三条线，三条线的另一端相交于待搬运物块。点处安装固定滑轮作为固定点，且安装有3个张力传感器，用于防止张力过大发生绳断的情况。可以通过三个舵机分别旋转不同的角度，牵引物体按照某一特定轨迹到达空间内的任一点。
1.2 数学模型的建立
如图2所示，现以点为原点，为轴，建立三维直角坐标系。为建立规范化数学方程，作如下假设：设点的坐标为点的坐标为，点的坐标为，点坐标为，设三根等长的线的原始长度为点到点的距离分别为。
以舵机A为例，设舵机A连杆的长度为连杆与平面的夹角为舵机旋转中心点到固定点的距离为连杆末端到点的弧线长度为。具体示意图如图2所示。同理，可得舵机与平面夹角为弧线长度为。
将式（2）和式（3）代入式（1），建立舵机旋转角度与待控制点坐标的关系：
根据分析，在本系统中控制量为三个舵机的旋转角度，即。根据给定的任意点坐标，得到相对应的控制量。因此，根据式（4），将写成由表示的形式：
根据分析可知，点的坐标为被控制量，点到点的距离以及舵机旋转角度为可控量，其中为直接控制量，为间接控制量。通过控制进而控制点的坐标。
式（5）所示的系统数学模型方程中，将要求点坐标代入，即可得到所需的控制量。至此，系统的数学模型建立完毕。
2 系统的硬件设计
2.1 控制机构
本系统的控制部分由DSPIC30F4011单片机来完成。PIC单片机由美国Microchip公司推出，其硬件系统设计便捷，指令系统设计精炼，采用精简指令集以及哈佛总线结构，使其拥有速度高、功率低、驱动电流大及控制能力强的优点[6]。它集成了单片机的控制功能和数字信号处理器（DSP）的计算能力[7]，有专门的PWM模块，且具有一定的驱动能力，对于对功率要求不高的电机，无需驱动电路，直接将芯片产生的PWM波送出即可。本设计中PWM波接收机构是舵机，直接通过芯片PWM的发生端口连接舵机端口即可完成相关功能，简单方便。
2.2 交互机构
本系统采用键盘完成对整个系统的预先设定功能，通过金鹏OCMJ2X8C型带中文显示功能的LCD屏完成对相关参数的显示。当系统处于功能设定模式时，LCD屏显示相对应的设置量，当系统处于运行阶段时，LCD屏将显示核心参数的运行变化情况。
2.3 执行机构
针对本设计的具体情况，直接控制量为旋转角度，因此只有步进电机和舵机可以作为本系统的执行机构。步进电机是将电脉冲信号转换成角位移的执行元件，其电机转子的转角与输入的脉冲数成正比[8]。而舵机则是通过输入脉冲的脉冲宽度来控制旋转角度，无论外界转矩如何，其始終保持在某一个角度，直到得到一个新的脉冲宽度的脉冲[9]。
虽然二者均可以完成相应功能，但是从控制的难易程度来看，相比于步进电机控制步数而言，舵机的直接角度控制显得更为方便简单，更为直接。并且舵机具有结构简单，易于控制，可靠性高[10]的优点，因此本文的执行机构为三台由日本Futaba公司生产的S3010舵机。S3010舵机成本低，且具有较大的扭矩，且反应速度快。
该舵机有三根外接线，电源线和地线，以及一根控制信号线，无需驱动电路，直接将芯片端口产生的PWM波信号送入信号控制端即可。S3010舵机的基本使用方法如下：通过给定不同的脉冲宽度，舵机将旋转一定的角度。控制前需要测定时所需的PWM波的脉宽和时的脉宽。
2.4 张力检测机构
在货物吊运过程中，会存在两种极端的情况：一种是某一根吊绳承受的张力过大；另一种情况则是某一根吊绳所承受的张力接近于零。上述两种情况中，前者将会造成绳子断裂，产生严重事故，后者则会造成运动轨迹发生严重偏离。作为模型，考虑上述两种情况是必要的。因此模型中引入了张力检测传感器，预警吊绳所受张力过大或者为零等极端情况。张力传感器根据四片等阻值应变片的阻值变化产生的不同电压输出，经滤波放大后进行张力计算。在本模拟系统中，采用云想JL?SOMT040型张力传感器进行张力检测。将检测的张力值大于事先鉴定的绳子张力极限值的90%，或者接近于零时的情况视为报警信号。
2.5 硬件结构框图
根据系统的设计要求，本文搭建了如图3所示的硬件控制电路示意图。
3 系统的软件设计
本设计中，控制悬挂物的运动轨迹，使悬挂物按照某一方向运动，关键在于三个舵机输出轴的旋转角度的相互配合。通过三个舵机同时旋转不同的特定角度，从而使悬挂物朝某个方向运动。而三维模型前文已经建立，控制时只需输入任何目标运动轨迹的方程即可完成任意轨迹的运动。本文以空间中倾斜的圆轨迹为例进行分析。
3.1 空间圆轨迹方程的建立
要完成空间倾斜圆轨迹的控制，必须要建立三维空间中的圆方程。对于空间倾斜圆并无固定公式，因此，本文作出如下简单推导。规定该空间圆面与平面夹角为30°，与平面垂直，如图4所示。
3.2 控制算法实现
根据3.1节得到的空间倾斜圆的方程，下面就可以通过控制算法实现货物按该圆轨迹进行运动。本设计中，物体按倾斜圆轨迹运动，运动到圆上某个指定位置停止。具体算法如下所示：
（1） 完成系统的初始化，以及舵机的初始0°位置的回归。
（2） 读取设定的圆半径，圆心位置以及目标点信息。
（3） 以为自变量，和为应变量，根据半径要求，确定的起点和终点值，并根据精度的需求，对合理的离散化取点。
（4） 根据每个离散点的值，通过式（5）换算出相应的值。
（5） 从第一个离散点开始，根据对应的值，计算出相应的脉冲宽度，输出对应的三个PWM波分别控制三个舵机。
（6） 读取三个张力传感器电压输出，在数字化转换后进行相关计算，若张力大于极限张力的90%或者张力接近于0，停止所有舵机运行，并且发出报警信号，否则执行下一步。
（7） 取下一个离散点，重复步骤（4），步骤（5），直到运动到目标点为止，等待下一个指令。
4 模拟系统的调试
根据设计思路，搭建了如图6所示的三线式三维运动控制系统的模型，并完成了以倾斜的圆轨迹为示例的运动轨迹控制模拟。为清晰地反映运动轨迹效果，本文在被控物块下加上铅笔，记录运动轨迹，记录的运动轨迹如图7所示。
对于图7，为使轨迹更加清晰，对图片进行了相关图像学的处理。为验证可行性，本文对直线运动、平面圆运动、斜面圆运动进行了多次测试，测试结果如表1所示。
表1中，直线误差的测量是直接通过直尺进行测量。圆则是以同一点作为圆心，多次作半径为5 cm的圆，对其半径进行误差测量。
5 结 语
本文针对目前货物搬运方式的不足，提出一种新的运动控制方式，不仅增加了运动的精确性，也提高了灵活性。在搭建了实物模型后，进行了多次试验，均符合要求，验证了基于DSPIC30F4011的三线式三维悬挂运动控制系统是可行的。
参考文献
[1] 武林.三维悬挂物体运动控制系统[J].浙江师范大学学报，2007，30（2）：171?175.
[2] 乔东凯，杨向宇，李多民，等.基于PLC对三维立体运动系统的电气控制及应用[J].工程设计学报，2012，19（6）：465?468.
[3] 焦振广.箱式货物自动装车系统的设计[D].青岛：山东科技大学，2014.
[4] 宋志兰，王华.地链搬运系统及其应用[J].物流技术与应用，2014（5）：118?121.
[5] 郝岩柱.自动化立体仓库堆垛机控制系统的设计与路径优化[D].沈阳：东北大学，2009.
[6] 王海峰.基于PIC单片机的温湿度监控系统设计[J].现代电子技术，2011，34（23）：201?203.
[7] 陈建生.夹持式架空地线巡线机器人的设计[D].苏州：苏州大学，2015.
[8] 杨海光，刘文怡，朱思敏，等.步进电机对测量角度的定位与控制[J].自动化仪表，2013，34（2）：46?48.
[9] 丁俐，余瑾，吴垠，等.基于PIC单片机的仿生机器鱼的舵机控制[J].微计算机信息，2010，26（14）：88?89.
[10] 崇阳，张科，王靖宇.一种基于模糊ADRC的舵机控制算法设计与实现[J].西北工业大学学报，2011，29（2）：217?222.
摘 要： 为改进当前固定轨道式的工业货物搬运方式，实现仓库货物按任意设定的三维轨迹进行精确搬运的目标，提出三线式三维悬挂运动控制系统的模型。以DSPIC30F4011单片机为控制核心，Futaba S3010舵机作为执行机构，张力检测机构作为保护机构，LCD屏和按键作为设定显示机构，搭建系统模型，根据模型建立数学模型，并进行相关算法的设计，最终实现三线式三维运动协调控制。
关键词： 三线式； 运动控制； 舵机； 圆轨迹方程
中图分类号： TN02?34； TP23 文献标识码： A 文章编号： 1004?373X（2017）13?0148?04
Abstract： In order to improve the current industry goods transportation way with fixed track， and realize the target that the warehouse goods can be transported according to the 3D track set arbitrarily， a model of trilinear 3D suspended motion control system is put forward. To construct the system model， the DSPIC30F4011 single chip microcomputer is taken as the control kernel， the Futaba S3010 steering engine is employed as the actuator， the tension detecting mechanism is acted as the protection mechanism， and the LCD screen and key are used as the setup display mechanism. According to the system model， its mathematical model was established. The relevant algorithm was designed to realize the trilinear 3D motion coordination control.
Keywords： three?line type； motion control； steering engine； circular path equation
0 引 言
我国在贵州安装了世界最大的射电望远镜，其直径长达500 m，需要安装4 450块反射板，并且安装精度必须达到5 mm。诸如这样的浩大工程，如果安装固定轨道来完成反射板的安装，其工程量将极为浩大，并且精度难以达到要求。同样，在工业应用中，对货物的搬运方式也是层出不穷。简单的诸如车间的货物有的采用吊车或者起重机等设备[1]，通常这些设备的运动都是靠手工操作，对操作者的劳动强度较大[2]。当然，各行各业自动化的货物搬运方式也在不断发展，例如皮带式装车辅助设备、真空吸盘助力搬运设备[3]、地链搬运系统[4]、堆垛机控制系统[5]等，增加了货物搬运的自动化程度，为人们的生产带来了方便。但是，这些方式都需要固定轨道，固定轨道搬运控制不灵活、成本较大、安装复杂，难以实现精确的任意轨迹的三维搬运。
因此，本文提出一种新的三线式三维运动控制的货物运输搬运控制思路，以三台舵机输出轴旋转角度为直接控制量，通过与舵机输出轴相连的杆作为传动机构，实现对货物运动控制的目的。研究的难点在于协调三台舵机的输出轴的旋转角度关系，使货物按照指定的路径运动到指定位置。根据上述思路，本文进行了示例系统模型的搭建。通过将三台Futaba S3010舵机分别安装在不同位置，其输出轴连接连杆，杆的另一端连接绳，绳通过一个固定滑轮连接到货物，通过连杆的旋转带动货物的运动。通过三台舵机各自旋转相应的角度，达到货物的某一轨迹的运动效果。本文以倾斜的圆轨迹为例实现示例的功能。
1 建立数学模型
1.1 系统整体结构
根据系统的设计需要，搭建如图1所示的系统。在图1系统结构中，是一个等边三角形，点为等边三角形的中心。为避免线直接缠裹在输出轴上导致轴半径变化造成误差，在舵机输出轴上安装连杆。分别从与舵机A、舵机B、舵机C输出轴相连的杆的另一端引出三条线，三条线的另一端相交于待搬运物块。点处安装固定滑轮作为固定点，且安装有3个张力传感器，用于防止张力过大发生绳断的情况。可以通过三个舵机分别旋转不同的角度，牵引物体按照某一特定轨迹到达空间内的任一点。
1.2 数学模型的建立
如图2所示，现以点为原点，为轴，建立三维直角坐标系。为建立规范化数学方程，作如下假设：设点的坐标为点的坐标为，点的坐标为，点坐标为，设三根等长的线的原始长度为点到点的距离分别为。
以舵机A为例，设舵机A连杆的长度为连杆与平面的夹角为舵机旋转中心点到固定点的距离为连杆末端到点的弧线长度为。具体示意图如图2所示。同理，可得舵机与平面夹角为弧线长度为。
将式（2）和式（3）代入式（1），建立舵机旋转角度与待控制点坐标的关系：
根据分析，在本系统中控制量为三个舵机的旋转角度，即。根据给定的任意点坐标，得到相对应的控制量。因此，根据式（4），将写成由表示的形式：
根据分析可知，点的坐标为被控制量，点到点的距离以及舵机旋转角度为可控量，其中为直接控制量，为间接控制量。通过控制进而控制点的坐标。
式（5）所示的系统数学模型方程中，将要求点坐标代入，即可得到所需的控制量。至此，系统的数学模型建立完毕。
2 系统的硬件设计
2.1 控制机构
本系统的控制部分由DSPIC30F4011单片机来完成。PIC单片机由美国Microchip公司推出，其硬件系统设计便捷，指令系统设计精炼，采用精简指令集以及哈佛总线结构，使其拥有速度高、功率低、驱动电流大及控制能力强的优点[6]。它集成了单片机的控制功能和数字信号处理器（DSP）的计算能力[7]，有专门的PWM模块，且具有一定的驱动能力，对于对功率要求不高的电机，无需驱动电路，直接将芯片产生的PWM波送出即可。本设计中PWM波接收机构是舵机，直接通过芯片PWM的发生端口连接舵机端口即可完成相关功能，简单方便。
2.2 交互机构
本系统采用键盘完成对整个系统的预先设定功能，通过金鹏OCMJ2X8C型带中文显示功能的LCD屏完成对相关参数的显示。当系统处于功能设定模式时，LCD屏显示相对应的设置量，当系统处于运行阶段时，LCD屏将显示核心参数的运行变化情况。
2.3 执行机构
针对本设计的具体情况，直接控制量为旋转角度，因此只有步进电机和舵机可以作为本系统的执行机构。步进电机是将电脉冲信号转换成角位移的执行元件，其电机转子的转角与输入的脉冲数成正比[8]。而舵机则是通过输入脉冲的脉冲宽度来控制旋转角度，无论外界转矩如何，其始終保持在某一个角度，直到得到一个新的脉冲宽度的脉冲[9]。
虽然二者均可以完成相应功能，但是从控制的难易程度来看，相比于步进电机控制步数而言，舵机的直接角度控制显得更为方便简单，更为直接。并且舵机具有结构简单，易于控制，可靠性高[10]的优点，因此本文的执行机构为三台由日本Futaba公司生产的S3010舵机。S3010舵机成本低，且具有较大的扭矩，且反应速度快。
该舵机有三根外接线，电源线和地线，以及一根控制信号线，无需驱动电路，直接将芯片端口产生的PWM波信号送入信号控制端即可。S3010舵机的基本使用方法如下：通过给定不同的脉冲宽度，舵机将旋转一定的角度。控制前需要测定时所需的PWM波的脉宽和时的脉宽。
2.4 张力检测机构
在货物吊运过程中，会存在两种极端的情况：一种是某一根吊绳承受的张力过大；另一种情况则是某一根吊绳所承受的张力接近于零。上述两种情况中，前者将会造成绳子断裂，产生严重事故，后者则会造成运动轨迹发生严重偏离。作为模型，考虑上述两种情况是必要的。因此模型中引入了张力检测传感器，预警吊绳所受张力过大或者为零等极端情况。张力传感器根据四片等阻值应变片的阻值变化产生的不同电压输出，经滤波放大后进行张力计算。在本模拟系统中，采用云想JL?SOMT040型张力传感器进行张力检测。将检测的张力值大于事先鉴定的绳子张力极限值的90%，或者接近于零时的情况视为报警信号。
2.5 硬件结构框图
根据系统的设计要求，本文搭建了如图3所示的硬件控制电路示意图。
3 系统的软件设计
本设计中，控制悬挂物的运动轨迹，使悬挂物按照某一方向运动，关键在于三个舵机输出轴的旋转角度的相互配合。通过三个舵机同时旋转不同的特定角度，从而使悬挂物朝某个方向运动。而三维模型前文已经建立，控制时只需输入任何目标运动轨迹的方程即可完成任意轨迹的运动。本文以空间中倾斜的圆轨迹为例进行分析。
3.1 空间圆轨迹方程的建立
要完成空间倾斜圆轨迹的控制，必须要建立三维空间中的圆方程。对于空间倾斜圆并无固定公式，因此，本文作出如下简单推导。规定该空间圆面与平面夹角为30°，与平面垂直，如图4所示。
3.2 控制算法实现
根据3.1节得到的空间倾斜圆的方程，下面就可以通过控制算法实现货物按该圆轨迹进行运动。本设计中，物体按倾斜圆轨迹运动，运动到圆上某个指定位置停止。具体算法如下所示：
（1） 完成系统的初始化，以及舵机的初始0°位置的回归。
（2） 读取设定的圆半径，圆心位置以及目标点信息。
（3） 以为自变量，和为应变量，根据半径要求，确定的起点和终点值，并根据精度的需求，对合理的离散化取点。
（4） 根据每个离散点的值，通过式（5）换算出相应的值。
（5） 从第一个离散点开始，根据对应的值，计算出相应的脉冲宽度，输出对应的三个PWM波分别控制三个舵机。
（6） 读取三个张力传感器电压输出，在数字化转换后进行相关计算，若张力大于极限张力的90%或者张力接近于0，停止所有舵机运行，并且发出报警信号，否则执行下一步。
（7） 取下一个离散点，重复步骤（4），步骤（5），直到运动到目标点为止，等待下一个指令。
4 模拟系统的调试
根据设计思路，搭建了如图6所示的三线式三维运动控制系统的模型，并完成了以倾斜的圆轨迹为示例的运动轨迹控制模拟。为清晰地反映运动轨迹效果，本文在被控物块下加上铅笔，记录运动轨迹，记录的运动轨迹如图7所示。
对于图7，为使轨迹更加清晰，对图片进行了相关图像学的处理。为验证可行性，本文对直线运动、平面圆运动、斜面圆运动进行了多次测试，测试结果如表1所示。
表1中，直线误差的测量是直接通过直尺进行测量。圆则是以同一点作为圆心，多次作半径为5 cm的圆，对其半径进行误差测量。
5 结 语
本文针对目前货物搬运方式的不足，提出一种新的运动控制方式，不仅增加了运动的精确性，也提高了灵活性。在搭建了实物模型后，进行了多次试验，均符合要求，验证了基于DSPIC30F4011的三线式三维悬挂运动控制系统是可行的。
参考文献
[1] 武林.三维悬挂物体运动控制系统[J].浙江师范大学学报，2007，30（2）：171?175.
[2] 乔东凯，杨向宇，李多民，等.基于PLC对三维立体运动系统的电气控制及应用[J].工程设计学报，2012，19（6）：465?468.
[3] 焦振广.箱式货物自动装车系统的设计[D].青岛：山东科技大学，2014.
[4] 宋志兰，王华.地链搬运系统及其应用[J].物流技术与应用，2014（5）：118?121.
[5] 郝岩柱.自动化立体仓库堆垛机控制系统的设计与路径优化[D].沈阳：东北大学，2009.
[6] 王海峰.基于PIC单片机的温湿度监控系统设计[J].现代电子技术，2011，34（23）：201?203.
[7] 陈建生.夹持式架空地线巡线机器人的设计[D].苏州：苏州大学，2015.
[8] 杨海光，刘文怡，朱思敏，等.步进电机对测量角度的定位与控制[J].自动化仪表，2013，34（2）：46?48.
[9] 丁俐，余瑾，吴垠，等.基于PIC单片机的仿生机器鱼的舵机控制[J].微计算机信息，2010，26（14）：88?89.
[10] 崇阳，张科，王靖宇.一种基于模糊ADRC的舵机控制算法设计与实现[J].西北工业大学学报，2011，29（2）：217?222.
