CN105511510A - 仓储安全距离的扫描监测系统及其定位控制方法 - Google Patents

Abstract

本发明公开了一种仓储安全距离的扫描监测系统及其定位控制方法。上述系统包括：控制器、带有细分驱动的步进电机驱动器、步进电机、设置为传动装置的蜗轮蜗杆、工作台、以及位置检测机构；其中，控制器的输入端设置于接收预定位置信息，上述步进电机驱动器连接于上述控制器的输出端和上述步进电机的输入端之间，上述涡轮螺杆连接于上述步进电机和上述工作台之间，上述位置检测机构连接于上述工作台与上述控制器之间。根据本发明提供的技术方案，采用扫描监测系统的闭环控制方案结合上述加减速算法，定位精度高，运行稳定，还可以避免步进电机的频繁调整和抖动。

Description

仓储安全距离的扫描监测系统及其定位控制方法

技术领域

本发明涉及通信领域，具体而言，涉及一种仓储安全距离的扫描监测系统及其定位控制方法。

背景技术

安全距离是保证危化品仓库中堆垛安全的一项重要指标，仓库中堆垛过高过密，容易产生反应热的聚集和不同危化品的混存，造成很大的安全隐患。对堆垛安全距离的监测，可以采用了一种网格化扫描监测的方案，将危化品仓库根据安全距离要求划分为若干网格，利用激光测距仪对每个监测面进行扫描，同时获得扫描时的距离值和角度值，根据扫描时的距离值是否和同一角度安全状态下的距离值一致就可判断堆垛的安全状态。

然而，扫描监测的定位精度直接影响到获得的距离值和角度值的准确性，进而影响到堆垛安全状态的判断，因此，提供一套能够精确定位的扫描监测方案，是目前亟待解决的问题。

发明内容

本发明的主要目的在于公开了一种仓储安全距离的扫描监测系统及其定位控制方法，以至少解决相关技术中扫描监测的定位精度直接影响到获得的距离值和角度值的准确性，进而影响到堆垛安全状态判断的问题。

根据本发明的一个方面，公开了一种仓储安全距离的扫描监测系统。根据本发明的仓储安全距离的扫描监测系统包括：控制器、带有细分驱动的步进电机驱动器、步进电机、设置为传动装置的蜗轮蜗杆、工作台、以及位置检测机构；其中，控制器的输入端设置于接收预定位置信息，所述步进电机驱动器连接于所述控制器的输出端和所述步进电机的输入端之间，所述涡轮螺杆连接于所述步进电机和所述工作台之间，所述位置检测机构连接于所述工作台与所述控制器之间。

根据本发明的另一方面，公开了一种仓储安全距离的扫描监测系统的定位控制方法。

根据本发明的仓储安全距离的扫描监测系统的定位控制方法包括：控制器在步进电机启动时控制所述步进电机的驱动脉冲频率达到步进电机的起跳频率；所述控制器控制所述驱动脉冲频率从起跳频率开始，经历加速、匀速和减速阶段；所述控制器在所述减速阶段完成时控制所述驱动脉冲频率降为起跳频率进入反馈调节阶段，将检测到的位置与目标位置进行对比，根据比较结果控制所述步进电机正向运动或反向运动，以至于达到所述目标位置。

本发明根据危化品仓库安全距离扫描监测的精度要求，提供了一种仓储安全距离的扫描监测系统，该系统包括一个带细分的步进电机驱动器，将步距角进行细分，提高了转角精度；系统中设置一个蜗轮蜗杆作为传动装置，可以降低机械结构引起的误差；并且引入位置检测环节和和闭环位置控制器，使系统的位置精度由检测环节的测量精度决定。

附图说明

图1是根据本发明实施例的仓储安全距离的扫描监测系统的结构框图；

图2是根据本发明优选实施例的仓储安全距离的扫描监测系统的结构示意图；

图3是根据本发明实施例的仓储安全距离的扫描监测系统的定位控制方法的流程图；

图4是根据本发明优选实施例的精确定位控制过程的示意图；

图5是根据本发明优选实施例的采用直线型加减速控制方法进行加减速控制的示意图；

图6是根据本发明优选实施例的采用指数型加减速控制方法进行加减速控制的示意图；

图7是根据本发明优选实施例的采用S型加减速控制方法进行加减速控制的示意图；

图8是根据本发明优选实施例的精确定位控制算法主程序的流程图；

图9是根据本发明优选实施例的加速子程序的流程图；

图10是根据本发明优选实施例的匀速子程序的流程图；

图11是根据本发明优选实施例的减速子程序的流程图；以及

图12是根据本发明优选实施例的反馈调节子程序的流程图。

具体实施方式

下面结合说明书附图对本发明的具体实现方式做一详细描述。

图1是根据本发明实施例的仓储安全距离的扫描监测系统的结构框图。如图1所示，该仓储安全距离的扫描监测系统包括：控制器10、带有细分驱动的步进电机驱动器12、步进电机14、设置为传动装置的蜗轮蜗杆16、工作台18、以及位置检测机构20；其中，控制器10的输入端设置于接收预定位置信息，上述步进电机驱动器12连接于上述控制器10的输出端和上述步进电机14的输入端之间，上述涡轮螺杆16连接于上述步进电机14和上述工作台18之间，上述位置检测机构20连接于上述工作台18与上述控制器10之间。

相关技术中，系统传动链中存在的传动误差，如：齿轮副、传动误差；如果系统中的传动部件刚性不足而产生较大摩擦力时，进给脉冲就会被消耗，它使工作台无法跟踪指令位移：控制系统和驱动系统的指令传递、转换误差也可使电机转角偏离指令位置。由于上述各种非线性因素的影响，必然使工作台的实际位移偏离指令值，而开环系统又无法对其进行有效校正与补偿因此很难使机床工作台达到较高的位置控制精度。在图1所示的仓储安全距离的扫描监测系统中，包括一个带细分的步进电机驱动器，将步距角进行细分，提高了转角精度；为了降低机械结构引起的误差，系统中设置一个蜗轮蜗杆作为传动装置；并且引入位置检测环节和和闭环位置控制器，使系统的位置精度由检测环节的测量精度决定。

优选地，上述控制器，设置于在上述步进电机启动时控制上述步进电机的驱动脉冲频率达到起跳频率，经过加速、匀速、减速阶段后，到达目标位置的预定范围内，控制上述步进电机的频率降为上述起跳频率，将检测到的位置与目标位置进行对比，根据比较结果控制上述步进电机正向运动或反向运动，以至于达到上述目标位置。

图2是根据本发明优选实施例的仓储安全距离的扫描监测系统的结构示意图。如图2所示，该仓储安全距离的扫描监测系统包括：控制器、带细分驱动的步进电机驱动器、步进电机、涡轮螺杆、工作台(云台)。

其中，步进电机是一种将电脉冲信号转换为角位移或线位移的控制电机，其在运动时，各相绕组产生反电动势，脉冲频率增加或降低越快，反电动势越大，这导致其相电流的减小，输出力矩的下降。如果不合理地规划步进电机的加减速，易发生启动时振动失步、堵转及停止时过冲的现象。图2所示的闭环控制虽然能很好的消除误差，但在步进电机启动和停止或速度变化过大时，会出现系统频繁调整抖动的情况，造成系统的不稳定。可以在闭环控制的基础上引入了加减速控制的策略，将控制过程分为：加减速控制部分和反馈调节控制部分。步进电机启动时，控制上述步进电机的驱动脉冲频率达到起跳频率f₀，然后经过加减速控制阶段，到达目标位置附近，此时步进电机频率降为f₀，之后进入反馈调节阶段，将检测到的位置与目标位置进行对比，根据比较结果控制上述步进电机正向运动或反向运动，以至于达到上述目标位置。这样既达到了较高的控制精度，同时避免了由于步进电机频率变化过快引起失步和堵转。

图3是根据本发明实施例的仓储安全距离的扫描监测系统的定位控制方法的流程图。如图3所示，该仓储安全距离的扫描监测系统的定位控制方法包括：

步骤S301：控制器在步进电机启动时控制上述步进电机的驱动脉冲频率达到步进电机的起跳频率；

步骤S303：上述控制器控制上述驱动脉冲频率从起跳频率开始，经历加速、匀速和减速阶段；

步骤S305：上述控制器在上述减速阶段完成时控制上述驱动脉冲频率降为起跳频率进入反馈调节阶段，将检测到的位置与目标位置进行对比，根据比较结果控制上述步进电机正向运动或反向运动，以至于达到上述目标位置。

采用上述方案，将控制过程分为：加减速控制部分和反馈调节控制部分，具体可以参见图4。步进电机启动时，控制上述步进电机的驱动脉冲频率f₀，然后经过加减速控制阶段，到达目标位置附近，此时步进电机的驱动脉冲频率降为f₀，之后进入反馈调节阶段，将检测到的位置与目标位置进行对比，根据比较结果控制上述步进电机正向运动或反向运动，以至于达到上述目标位置。这样既达到了较高的控制精度，同时避免了由于步进电机频率变化过快引起失步和堵转。

加减速控制方法主要有三种：直线型加减速速度曲线、指数型加减速速度曲线、S型加减速速度曲线。如图5所示的直线型加减速速度曲线，这种升降速控制方法的主要优点是数学表达简单，节省资源，易于实现，其主要缺点是加速时间较长，电机通过其谐振点加速可能会有困难，且加速过程开始和结束时速度存在跳变，带来的冲击较大；如图6所示的指数型加减速曲线，比较符合电机的转矩特性，数学表达相对简单，加减速终了时加速度突变小，其不足是启动过程仍然存在较大冲击，加减速过程结束时速度变化是渐进线式的，变化过程太大，需要进行一些修正处理，适用于控制系统处理速度快且对升降速过程要求较高的场合；如图7所示的S型加减速曲线，其特点是转矩性能最好，启动和结束都是连续可导的，因此具有平稳精确的加减速性能。其主要不足是数学表达复杂，主要适用于加减速的平稳性要求最高的场合。

在优选实施过程中，比较上述三种加减速控制方法，综合考虑步进电机运行的平稳性和算法编程实现的难易，上述控制器可以采用指数型加速控制方法对上述步进电机的驱动脉冲频率进行加速控制，并采用指数型减速控制方法对上述步进电机的驱动脉冲频率进行减速控制。

在优选实施过程中，上述定位控制方法设计分为两部分：指数型加减速控制部分和反馈调节部分。先执行指数型加减速控制阶段程序使步进电机接近目标位置，再执行反馈调节阶段程序，进行精确定位。以下结合图8进一步描述上述优选实施方式。

图8是根据本发明优选实施例的精确定位控制算法主程序的流程图。如图8所示，该精确定位控制算法主程序包括：

步骤S801：进入中断程序入口，判断步进电机是否进行正向转动，如果是，执行步骤S803,否则，执行步骤S805。

步骤S803：方向指针加1。

步骤S805：方向指针减1。

步骤S807：判断是否处于加速阶段，如果是，执行步骤S809,否则，执行步骤S811。

步骤S809：执行加速子程序。

步骤S811：判断是否处于匀速阶段，如果是，执行步骤S813,否则，执行步骤S815。

步骤S813：执行匀速子程序。

步骤S815：判断是否处于减速阶段，如果是，执行步骤S817,否则，执行步骤S819。

步骤S817：执行减速子程序。

步骤S819:执行反馈调节子程序。

步骤S821:重装定时器，流程结束。

优选地，上述控制器控制上述驱动脉冲频率经历加速阶段可以进一步包括：预先将加速阶段的时长均匀分割为多级分档，将各分档速度运行的时间转换为步数并存储；当上述控制器控制上述驱动脉冲频率进入加速阶段时，对每一档速度对应的步数进行递减操作，当减至零时当前一档速度控制过程完毕，执行下一档速度控制，直至加速阶段的总步数为零，加速阶段完毕，将状态标识位设置为匀速状态。以下结合图9进一步描述上述优选实施方式。

图9是根据本发明优选实施例的加速子程序的流程图。如图9所示，加速子程序包括:

步骤S901：当上述控制器控制上述驱动脉冲频率进入加速阶段时，加速段总步数减1，台阶内(该分档内)步数减1。

步骤S903：判断台阶内步数是否为0，如果是，执行步骤S907,否则执行步骤S905。

步骤S905：台阶内(该分档内)步数减1。

步骤S907：判断加速段总步数是否为0，如果是，执行步骤S909,否则流程结束。

步骤S909：将状态标识位设置为匀速状态。

用控制器(单片机)实现步进电机的变加速度控制，实际上就是控制脉冲的频率，利用定时器中断该边定时常数，在加速时使脉冲频率逐渐增大，减速时使脉冲频率逐渐减小。将电机在每一档速度下运行的时间转换为相应的步数，存储在一个数组中，步进电机升速过程中，对每一档速度对应的步数进行递减操作，当减至零时表示本阶段升降速过程完毕，进入下一档速度控制，当加速阶段总步数为零，则表示加速阶段已经完成，同理，匀速段和减速段的控制与升速段类似；当减速段完成后，整个指数型加减速控制阶段结束，进入反馈调节阶段，此时步进电机驱动频率为起跳频率，将检测到的位置与目标位置比较，若超过目标位置，则驱动步进电机反向运动，若还未到目标位置，则正向运动，若到达目标位置，则停止。

优选地，上述控制器控制上述驱动脉冲频率经历匀速阶段包括：预先将匀速阶段的时长转换为步数并存储；当上述控制器控制上述驱动脉冲频率进入匀速阶段时，对匀速阶段的总步数进行递减操作，直至匀速阶段的总步数为零，匀速阶段完毕，将状态标识位设置为减速状态。以下结合图10进一步描述上述优选实施方式。

图10是根据本发明优选实施例的匀速子程序的流程图。如图10所示，该匀速子程序包括：

步骤S1001：将匀速阶段的总步数减1。

步骤S1003：判断匀速阶段总步数是否为0，如果是，执行步骤S1005,否则流程结束。

步骤S1005：匀速阶段完毕，将状态标识位设置为减速状态。

优选地，上述控制器控制上述驱动脉冲频率经历减速阶段包括：预先将减速阶段的时长均匀分割为多级分档，将各分档速度运行的时间转换为步数并存储；当上述控制器控制上述驱动脉冲频率进入减速阶段时，对每一档速度对应的步数进行递减操作，当减至零时当前一档速度控制过程完毕，执行下一档速度控制，直至减速阶段的总步数为零，减速阶段完毕，将状态标识位设置为反馈调节状态。以下结合图11进一步描述上述优选实施方式。

图11是根据本发明优选实施例的减速子程序的流程图。如图11所示，该减速子程序包括：

步骤S1101：当上述控制器控制上述驱动脉冲频率进入减速阶段时，减速段总步数减1，台阶内(该分档内)步数减1。

步骤S1103：判断台阶内步数是否为0，如果是，执行步骤S1107,否则执行步骤S1105。

步骤S1105：台阶内(该分档内)步数减1。

步骤S1107：判断加速段总步数是否为0，如果是，执行步骤S1109,否则流程结束。

步骤S1109：将状态标识位设置为反馈调节状态。

在优选实施过程中，起跳频率是步进电机不经过加速，能够直接启动工作的最高频率。系统的起跳频率可按以下公式计算：

$f_{s1}=f_s\sqrt{\frac{1-(T_e/T_v)}{1+(J_1/J_r)}}---\left(1\right)$

其中，f_s为空载启动频率，单位Hz；f_s1为步进电机的惯性负载启动频率,单位Hz；J₁为负载惯量，单位kg·cm2；Jr为电机转子惯量，单位kg·cm2；T_e为负载转矩，单位N·m；T_v为步进电机输出转矩，单位N·m。

若单片机(即控制器)使用定时器中断方式来控制步进电机的速度，那么加减速控制实际上是不断改变定时器的装载值的大小。为了便于编制程序，采用阶梯曲线来逼近升速曲线。根据步进电机的转矩-频率特性曲线，可以得到加速过程中驱动脉冲频率与时间的关系为：

f_r＝f_m-f_me^-t/τ(2)

其中，f_r为升速过程中的驱动脉冲频率；f_m为步进电机的最高连续频率；τ是决定升速快慢的时间常数，实际工作中可由实验来确定。

在程序运行时，若运行速度为f_g，则从式(2)可算出升速时间：

t_r＝τ[lnf_m-ln(f_m-f_g)](3)

将升速段均匀地离散为n段，相邻两次速度变化的时间间隔为

Δt_r＝t_r/n(4)

其中，n即为阶梯升速的分档数，则每一档的频率计算公式为：

$f_k=f_m-f_m{e}^{-({\mathrm{kt}}_r/n)/\mathrm{\τ}}---\left(5\right)$

各分档速度对应的步数N_k为

N_k＝f_k·Δt＝f_kt_r/n(6)

则升速总步数N_r为

$N_r={\mathrm{\Σ}}_{k=1}^n{N}_k---\left(7\right)$

程序执行时，记录每一档执行的步数，当运行到N_k时，进入下一档频率的执行，依次类推，直到执行完n段。

上面以步进电机速度在加速阶段的处理，减速阶段的处理方法和加速阶段类似。

优选地，上述控制器在上述减速阶段完成时控制上述驱动脉冲频率降为起跳频率进入反馈调节阶段，将检测到的工作台位置与目标位置进行对比，根据比较结果控制上述步进电机正向运动或反向运动可以包括：判断检测到的工作台位置与上述目标位置是否一致，如果一致，流程结束；当上述工作台位置与上述目标位置不一致时，判断工作台位置与上述目标位置的大小；当工作台位置大于上述目标位置时，控制上述步进电机反向运动；当工作台位置小于上述目标位置时，控制上述步进电机正向运动。以下结合图12进行描述。

图12是根据本发明优选实施例的反馈调节子程序的流程图。如图12所示，该反馈调节子程序包括：

步骤S1201：判断检测到的工作台(云台)位置与上述目标位置是否一致，如果一致，流程结束，否则执行步骤S1203。

步骤S1203：判断工作台位置与上述目标位置的大小，如果工作台位置与大于上述目标位置，则执行步骤S1205,否则执行步骤S1207。

步骤S1205：控制上述步进电机反向运动。

步骤S1207：控制上述步进电机正向运动。

通过实际验证，采用扫描监测系统的闭环控制方案结合上述加减速算法，可将控制精度在±0.022度左右，系统运行稳定，而在实际对危化品仓库堆垛的安全距离扫描监测中，要求精度在±0.095度，满足在危化品仓库安全距离扫描监测中的精度要求。

具体可以参见表1的实验数据。

表1

序号	设定角度(°)	实际的角度值(°)	误差(°)
				1	5	5.021	0.021
2	10	10.000	0
				3	15	14.978	-0.022
4	20	20.031	0.031
				5	25	25.012	0.012
6	40	40.023	0.023
				7	45	44.981	-0.019

综上所述，借助本发明提供的上述实施例，根据危化品仓库安全距离扫描监测的精度要求，提供了一种精确扫描监测系统。该系统采用指数型加减速算法和闭环反馈调节相结合的控制算法，定位精度高，运行稳定，还可以避免步进电机的频繁调整和抖动。实验结果表明，装置及算法定位精度在±0.022度左右，完全满足仓库安全距离扫描监测的精度要求。

以上公开的仅为本发明的几个具体实施例，但是，本发明并非局限于此，任何本领域的技术人员能思之的变化都应落入本发明的保护范围。

Claims (10)

1.一种仓储安全距离的扫描监测系统，其特征在于，包括：控制器、带有细分驱动的步进电机驱动器、步进电机、设置为传动装置的蜗轮蜗杆、工作台、以及位置检测机构；其中，控制器的输入端设置于接收预定位置信息，所述步进电机驱动器连接于所述控制器的输出端和所述步进电机的输入端之间，所述涡轮螺杆连接于所述步进电机和所述工作台之间，所述位置检测机构连接于所述工作台与所述控制器之间。

2.根据权利要求1所述的系统，其特征在于，所述控制器，设置于在所述步进电机启动时控制所述步进电机的驱动脉冲频率达到起跳频率，经过加速、匀速、减速阶段后，到达目标位置的预定范围内，控制所述步进电机的频率降为所述起跳频率，将检测到的位置与目标位置进行对比，根据比较结果控制所述步进电机正向运动或反向运动，以至于达到所述目标位置。

3.一种权利要求1或2所述的仓储安全距离的扫描监测系统的定位控制方法，其特征在于，包括：

控制器在步进电机启动时控制所述步进电机的驱动脉冲频率达到步进电机的起跳频率；

所述控制器控制所述驱动脉冲频率从起跳频率开始，经历加速、匀速和减速阶段；

所述控制器在所述减速阶段完成时控制所述驱动脉冲频率降为起跳频率进入反馈调节阶段，将检测到的位置与目标位置进行对比，根据比较结果控制所述步进电机正向运动或反向运动，以至于达到所述目标位置。

4.根据权利要求3所述的方法，其特征在于，所述控制器控制所述驱动脉冲频率经历加速阶段包括：

预先将加速阶段的时长均匀分割为多级分档，将各分档速度运行的时间转换为步数并存储；

当所述控制器控制所述驱动脉冲频率进入加速阶段时，对每一档速度对应的步数进行递减操作，当减至零时当前一档速度控制过程完毕，执行下一档速度控制，直至加速阶段的总步数为零，加速阶段完毕，将状态标识位设置为匀速状态。

5.根据权利要求3所述的方法，其特征在于，所述控制器控制所述驱动脉冲频率经历匀速阶段包括：

预先将匀速阶段的时长转换为步数并存储；

当所述控制器控制所述驱动脉冲频率进入匀速阶段时，对匀速阶段的总步数进行递减操作，直至匀速阶段的总步数为零，匀速阶段完毕，将状态标识位设置为减速状态。

6.根据权利要求3所述的方法，其特征在于，所述控制器控制所述驱动脉冲频率经历减速阶段包括：

预先将减速阶段的时长均匀分割为多级分档，将各分档速度运行的时间转换为步数并存储；

当所述控制器控制所述驱动脉冲频率进入减速阶段时，对每一档速度对应的步数进行递减操作，当减至零时当前一档速度控制过程完毕，执行下一档速度控制，直至减速阶段的总步数为零，减速阶段完毕，将状态标识位设置为反馈调节状态。

7.根据权利要求4或6所述的方法，其特征在于，采用以下公式将所述将各分档速度运行的时间转换为步数：N_k＝f_kt_r/n；

其中，N_k为各分档速度对应的步数，k为自然数，f_k为各分档对应的驱动脉冲频率，t_r为加速阶段的时长，n为分档数。

8.根据权利要求7所述的方法，其特征在于，所述各分档对应的驱动脉冲频率f_k采用以下公式得到： $f_k=f_m-f_m{e}^{-\left(k{t}_r/n\right)/\mathrm{\τ}};$

其中，f_m为所述步进电机的最高连续频率，τ是决定加速或减速快慢的时间常数。

9.根据权利要求3所述的方法，其特征在于，所述控制器采用指数型加速控制方法对所述步进电机的驱动脉冲频率进行加速控制，采用指数型减速控制方法对所述步进电机的驱动脉冲频率进行减速控制。

10.根据权利要求3所述的方法，其特征在于，所述控制器在所述减速阶段完成时控制所述驱动脉冲频率降为起跳频率进入反馈调节阶段，将检测到的工作台位置与目标位置进行对比，根据比较结果控制所述步进电机正向运动或反向运动包括：

判断检测到的工作台位置与所述目标位置是否一致，如果一致，流程结束；

当所述工作台位置与所述目标位置不一致时，判断工作台位置与所述目标位置的大小；

当工作台位置大于所述目标位置时，控制所述步进电机反向运动；

当工作台位置小于所述目标位置时，控制所述步进电机正向运动。