CN110187721A - 一种千分度精密云台运动控制装置及方法 - Google Patents

Abstract

本发明公开了一种千分度精密云台运动控制装置，由壳体和设于壳体内的方位驱动部件、俯仰驱动部件、指北部件、控制部件、网络通信部件及电源管理部件组成；还公开了其控制方法，包括路径规划、运动控制和运动补偿，通过路径规划，将运动路径分段，对于不同的路径分段，采用不同的运动控制算法：在开始，为防止电机失步采用较低的启动速度，通过加载驱动力，以最短时间达到最大的速度；在高速运动段，按最大的运动速度匀速运动以保证最少的运动时间，从而优化千分度云台的运动控制时间，最后对于俯仰运动，通过补偿算法消除云台因负载扭矩影响而产生的空回位置误差，从而提高云台的运动控制精度。

Description

一种千分度精密云台运动控制装置及方法

技术领域

本发明属于云台运动控制技术领域，具体涉及一种千分度云台位置的精确控制装置，以及其控制方法。

背景技术

云台是安防行业的一种常用设备，随着安防行业的发展，不仅需要对目标进行适时的监控，还需要对目标进行精确的定位、测距及快速的跟踪。

传统的安防云台大多数采用重复定位精度只有0.1°，基本上都是采用步进电机驱动，通过分度方式来实现步进精度和重复定位精度，因为齿轮加工误差及装调的误差，该方式无法进一步改善云台的运动精度；而采用力矩电机又会使得千分度云台的价格高昂，同时开机后驱动力一直与阻力平衡的工作方式使其功耗较大，不适用于高性格比要求的安防行业。

随着安防行业的发展，目前应用于边境安防监控，渔业安防监控等领域的可见光摄像机采用500mm或以上焦距的高清镜头，红外热像仪采用100mm或以上焦距的镜头，作用距离大多数达到或远于4km，如果采用重复定位精度0.1°的云台，对于家用小汽车或5m左右的渔船的定位误差约6.8m，如果采用激光测距，根本无法快速瞄准目标，也就无法精度的测距。同时对于4km的目标，6.8m的误差也无法满足对目标的精确定位。

在千分度云台位置控制中，由于加速度或速度的突变，特别是在接近停止时，运动的抖动会影响云台停止的终点位置，从而影响千分度云台的定位精度；此外，由于云台本身负载的影响，在逐渐降低速度的过程中后，随着驱动力下降直到零时，传动系统的齿隙误差，会使最终的位置与当前控制位置有一个偏移，根据齿隙加工和装配精度的大小，该误差比运动抖动的误差值还要大，且该误差值不是一个恒定值。上述原因导致云台的运动精度无法达到0.005度或以下。

发明内容

为了解决上述千分度高精密云台的重复定位问题，本发明的目的之一是提供一种千分度精密云台运动控制装置。

本发明解决其技术问题所采用的技术方案是：一种千分度精密云台运动控制装置，由壳体和设于壳体内的方位驱动部件、俯仰驱动部件、指北部件、控制部件、网络通信部件及电源管理部件组成；其中所述的方位驱动部件包括方位电机、方位反馈模块和方位电机驱动模块，所述的方位反馈模块由方位限位开关和方位编码反馈模块组成；所述的俯仰驱动部件包括俯仰电机、俯仰反馈模块和俯仰电机驱动模块，所述的俯仰反馈模块由俯仰限位开关和俯仰编码反馈模块组成，方位限位开关、方位编码反馈模块、俯仰限位开关和俯仰编码反馈模块完成运动的反馈；所述的控制部件由主控模块、连接方位编码反馈模块的方位四倍频模块和连接俯仰编码反馈模块的俯仰四倍频模块组成；所述的网络通信部件由网络通信部件、用于MCU通信的第一串口控制模块和用于外接光电的第二串口控制模块组成；所述的电源管理部件由反接保护模块、过压/过流保护模块和DC-DC模块依次连接而成。

所述的一种千分度精密云台运动控制装置，其指北开关包括方位光电开关和俯仰光电开光；所述的方位限位开关和俯仰限位开关可以是行程开关或光电开关。

所述的一种千分度精密云台运动控制装置，其方位电机通过方位蜗轮蜗杆连接方位传动轴，所述的方位编码反馈模块同轴连接在方位传动轴上。

所述的一种千分度精密云台运动控制装置，其俯仰电机通过俯仰蜗轮蜗杆连接俯仰传动轴，所述的俯仰编码反馈模块同轴连接在俯仰传动轴上。方位编码反馈模块与方位运动的最后转动轴同轴连接，俯仰编码反馈模块与俯仰运动的最后转动轴同轴连接，用于消除传动机械中的传动齿隙误差。

本发明的目的之二是提供上述控制装置的控制方法，包括如下步骤：

步骤一，给定最大速度、加速度：设定云台的最大工作速度和最大加速度值，然后由主控模块计算出按最大加速度从初始状态加速到最大速度的时间；

步骤二，给定位置：通过网络通信部件设定云台的相对或绝对位置，然后通过第一串口控制模块将设定位置传递给主控模块，主控模块控制方位电机驱动模块和俯仰电机驱动模块同时运动，并以大于8M的高速周期读取方位编码反馈模块和俯仰编码反馈模块的采样值，获得云台的当前位置；

步骤三，云台方向设定：主控模块将接收的设定位置与云台的当前位置比较，计算设定位置与当前位置之间的方位/俯仰差值：方位差值为正时，顺时针转动，反之逆时针转；俯仰差值为正时，向下转动，反之向上转动；

步骤四，云台路径规划：根据上述差值的范围，按最短工作时间要求，来对云台的运动路径分段；如果差值≤1°，则将云台路径分为启始段和结束段；如果差值＞1°，则将云台路径分为启始段、高速段和结束段；快速设定启始段、高速段和结束段的长度，根据分段路径，计算出各个分段的速度和加速度，并选定运动的方式，减小运动中的冲击和震动，其中启始段为云台由静止加速到最大速度的时间所运动的行程，结束段为云台由最大速度减速到停止的时间所运动的行程，高速段为启始段和结束段之间的行程；

步骤五，云台运动控制，根据每一段的运动方式，确定每一段的运算控制算法：在启始段，云台按照最大加速度采用匀加速的运动方式运行到步骤四中路径规划的启始段终点，控制算法采用位置增量式PID控制，通过PWM调节方位电机驱动模块和俯仰电机驱动模块，规避速度和加速度突变，有效消除运动的抖动，使云台的方位/俯仰两个自由度运动的速度达到最大值；在高速段，云台采用位置增量式PI控制，按照最大工作速度以匀速运动方式运动到步骤四中路径规划的高速段终点；

在步骤四中路径规划的结束段，云台分成两步来完成：第一步，采用增量式PID或PI位置控制算法，通过增加用于改变运动加速度的冗余位置点，改变运行路径，可精确达到指定的位置，并根据当前的位置与设定位置差，确定当前的综合齿隙误差，并计算当前位置补偿值；第二步，采用脉动暂停的方式驱动方位电机和俯仰电机，在靠近终点（即步骤二中给定的位置）时，先采用均匀减速，最后段平滑加速度的值，从而减小冲击，提高运动的位置精度。

所述的一种千分度精密云台运动控制方法，将设定位置与当前位置之间的方位/俯仰差值作为云台在该位置的综合齿隙，按综合齿隙的2倍值作为补偿值，按计算的补偿值，向相反的方向运动，反向补偿以消除综合齿隙误差。

所述的一种千分度精密云台运动控制方法，在结束段中运动运动接近终点时，通过脉动暂停、同步采用来检测驱动力卸载后产生的齿隙误差，并根据精度要求作出多次补偿修正。

与现有技术相比，本发明主要具备以下的技术优点：

本发明控制装置和方法可使千分度云台的重复定位精度在0.005～0.01°之间，如果增加编码反馈模块的精度，还可以进一步提高重复定位精度；

当重复定位精度0.01°时，对于4km左右的目标，最大误差0.68m，对于车和小船等目标，能快速定位并测距；

当重复定位精度在0.005°时，对于4km左右的目标，最大误差0.34m，对行人能快速定位并测距；

此外，千分度云台微步进精度可以达到0.002°左右，方便千分度云台的精确瞄准，因此，千分度云台在安防的后续应用中，具有较好的前景。

附图说明

图1是本发明控制装置的结构组成示意图；

图2是本发明控制装置的工作原理示意图；

图3是本发明控制装置俯仰驱动部件的结构示意图；

图4是本发明控制装置方位控制系统的结构示意图；

图5是本发明控制方法的路径三分段规划示意图，其中点划线是计算的常规路径，实线是理想运动控制路径；

图6是本发明控制方法的路径冗余位置点控制示意图，其中点划线是常规路径，空心点是周期采样点，黑点是路路径改进点，实线是改进后的拟合线；

图7是本发明控制方法的路径二分段规划示意图；

图8是本发明控制方法的更小相对路径二分段规划示意图；

图9是本发明控制方法的负载扭矩示意图；

图10是本发明控制方法的传动系统中的齿隙误差示意图；

图11是本发明控制方法的流程图。

各附图标记为：1—千分度云台，2—方位驱动部件，3—俯仰驱动部件，4—指北部件，5—控制部件，6—网络通信部件，7—方位反馈模块，8—方位电机驱动模块，9—俯仰电机驱动模块，10—俯仰反馈模块，11—主控模块，12—方位四倍频模块，13—第一串口控制模块，14—网络通信部件，15—方位光电限位开关，16—方位编码反馈模块，17—俯仰限位开关，18—俯仰编码反馈模块，19—电源管理部件，20—壳体，21—俯仰四倍频模块，22—第二串口控制模块，23—俯仰电机，24—俯仰传动系统，25—俯仰蜗轮蜗杆，26—俯仰传动轴，27—方位电机，28—方位传动系统，29—方位传动轴，30—方位蜗轮蜗杆，31—转轴，32—转轴。

具体实施方式

下面结合附图和实施例对本发明作进一步详细说明。

本发明的目的一是实现精密云台重复性定位，有利于快速瞄准目标，目的二是保证第一条的情况下，快速完成每一次位置精确定位，保证定位的实时性。

参照图1所示，本发明千分度云台1主要由以下部件组成：电源管理部件19，方位驱动部件2、俯仰驱动部件3、指北部件4、控制部件5、网络通信部件6及壳体20。

所述的方位驱动部件2由方位电机27、方位反馈模块7和方位电机驱动模块8组成，所述的方位反馈模块7由方位限位开关15和方位编码反馈模块16（方位相对编码器）组成。如图3所示，所述的方位电机27通过方位蜗轮蜗杆30连接有方位传动轴29，所述的方位编码反馈模块16同轴连接在方位传动轴29上；所述的方位电机27、方位蜗轮蜗杆30、方位传动轴29和方位编码反馈模块16组成方位传动系统28，安装在机架上。所述的俯仰驱动部件3由俯仰电机23、俯仰反馈模块10和俯仰电机驱动模块9组成，所述的俯仰反馈模块10由俯仰（行程）限位开关17和俯仰编码反馈模块18（俯仰相对编码器）组成。方位限位开关15、方位编码反馈模块16、俯仰限位开关17和俯仰编码反馈模块18完成运动的反馈。所述的方位限位开关15和俯仰限位开关17可以是行程开关或光电开关。如图4所示，所述的俯仰电机23通过俯仰蜗轮蜗杆25连接有俯仰传动轴26，所述的俯仰编码反馈模块18同轴连接在俯仰传动轴26上；所述的俯仰电机23、俯仰蜗轮蜗杆25、俯仰传动轴26和俯仰编码反馈模块18组成俯仰传动系统24，安装在机架上。

千分度云台的结构必须要有一定的刚度和强度，方位编码反馈模块16和俯仰编码反馈模块18分别与方位传动轴29和俯仰传动轴26同轴安装并固定，并准确定位。方位编码反馈模块16和俯仰编码反馈模块18固定在传动轴可以将传动齿隙综合成可测量齿隙值。方位编码反馈模块16与方位运动的最后转动轴同轴连接，俯仰编码反馈模块18与俯仰运动的最后转动轴同轴连接，这样可用于消除传动机械中的传动齿隙误差。

此外为了提高云台的刚度，两个方位编码器与两个传动轴必须采用多点固定，不能采用悬臂梁方式固定。如果条件允许，应采用多支点梁，最小要二个支点的安装方式。

所述的控制部件5由主控模块11、方位四倍频模块12和俯仰四倍频模块21组成，为提高控制精度，编码反馈模块需要采用四倍频方式；由于无法采用传动比倍频方式，因此要选用分辨率较大的相对编码器，并采用硬件四倍频（由单芯片搭建或CPLD/小型FPGA完成）减少对采样精度的影响；其中方位四倍频模块12与方位编码反馈模块16连接，而俯仰四倍频模块21与俯仰编码反馈模块18连接，以提高计数的精度；通过上述的结构设计，可以有效地将传动系统的齿隙变成综合的齿隙，然后通过脉动暂停算法，实时补偿当前的综合齿隙误差。主控模块11可采用ARM、DSP及FPGA来完，具有较高的速度和较强的计算能力。

所述的指北部件4是指方位光电开关和俯仰光电开光组成的北光电限位开关，在千分度云台初始化过程中，按同一个转动方向完成云台的初始化。

所述的网络通信部件6由网络通信部件14（网络视频模块）、第一串口控制模块13和第二串口控制模块22组成。网络通信部件14主要是100M网络路由器，用于传送网络监控视频，同时通过网络转串口方式，提供两个串行接口：第一串口控制模块13用于外部网络与云台通信，第二串口控制模块22用于外部网络与云台支撑的光电系统通信。

所述的电源管理部件19由反接保护模块、过压/过流保护模块和C-DC模块依次连接而成；电源管理部件19为整个系统提供供电电源，通过包含的多种电源保护来提高云台的可靠性。

本发明的工作原理如附图2所示：

主控模块11控制方位电机驱动模块8和俯仰电机驱动模块9同时运动，在运动过程中，主控模块11按8M的采样速度读取方位编码器反馈模块16和俯仰编码反馈模块18的采样值，该采样值在读入主控模块11前，分别经过方位四倍频模块12和俯仰四倍频模块21，可以提高采样的分辨率，从而在不增加硬件成本的条件下，提高云台的运动精度。

主控模块11通过第一串口控制模块13接收设定位置后，将设定位置与当前位置比较，确定采用三段式分段还是二段式分段方式，其中二段式分段与三段式分段中的启始段与结束段控制方式相同，只是设定最大速度不相，当相对移动路径太短时，为保证控制精度，减少了启始段（启动段）的长度。

方位限位光电开关15和俯仰行程开关17主要用于云台的极限位置保护，如果采用方位360×n转动方式，可以不关注方位限位开关。

本发明云台的运动控制方法的步骤如附图11所示：包括两个部分，第一部分为千分度云台的运动路径的自动规划，第二部分为千分度位置精确运动算法，运动算法主要分为消除运动冲击和震动及补偿齿隙误差。具体包括如下步骤：

步骤一，给定最大速度、加速度。

设定云台的最大工作速度和最大加速度值，该值与结构设计强度及传动系统传动比相关，然后对于给定的相对（运动）位置与绝对（运动）位置，首先由主控模块11计算出按最大加速度加速到最大速度的时间。该分段是初步分段计算，根据上述的计算，确定每一段的分段点及每一段的运动参数。

步骤二，给定位置。

通过网络通信部件6设定云台的相对（运动）位置或绝对（运动）位置，然后通过第一串口控制模块13将设定位置传递给主控模块11，主控模块11控制方位电机驱动模块8和俯仰电机驱动模块9同时运动，并以大于8M的高速周期读取方位编码反馈模块16和俯仰编码反馈模块18的采样值，获得云台的当前位置。

步骤三，云台方向设定。

主控模块11通过网络通信部件6和第一串口控制模块13将接收的设定位置与云台的当前位置比较，计算设定位置与当前位置之间的方位/俯仰差值，根据差值来确定云台旋转的方向：方位差值为正时，顺时针转动，反之逆时针转；俯仰差值为正时，向下转动，反之向上转动。

步骤四，云台路径规划。

确定运动路径中各个分段点（即开始段、高速段和结束段之间的连接点）的位置，根据分段点分段路径：根据上述差值的范围，按最短工作时间要求，来对云台的运动路径分段；如果差值≤1°，采用二段式分段的路径规划，示意图如附图7所示，将云台路径分为启始段和结束段；如果差值＞1°，则采用三段时分段的路径规划，路径规划如附图5所示，将云台路径分为启始段、高速段和结束段。确定每一段的运动参数，快速设定启始段、高速段和结束段的长度，根据分段路径，计算出各个分段的速度和加速度，并选定运动的方式，减小运动中的冲击和震动，其中启始段为云台由静止加速到最大速度的时间所运动的行程，结束段为云台由最大速度减速到停止的时间所运动的行程，高速段为启始段和结束段之间的行程（长度）。所有的行程尽可能采用最大速度及最大加速度，可以节省工作时间，保证云台的运动实时性。如果差值＜0.5度时，采用二段式分段时，需要根据当前差值下调运动速度，如附图8所示，否则路径无法闭环，同时速度太高也无法精确定位。

步骤五，云台运动控制：根据每一段的运动方式，确定每一段的运算控制算法。

在启始段：云台按匀加速运动方程，按照最大加速度采用均匀加速的运动方式运行到指定的位置点（开始段的终点）来减小运动时间，如附图5所示中虚线路径，如有应用需要，通过在路径中增加位置点（开始段中插入点）的方式调整运动的速度，如附图5中实线所示，如果在该段也要消除冲击和震动，则在启始段增加插入多个位置点，该位置点低于初步设定点，在启始段与高速段接合处也采用同样的方式，可以消除和减小运动的抖动，由于该处冲击、震动不影响位置运动精度，因此为减小运动时间，采用附图6虚线规划路径，该运动采用直线运动方式，不必采用多点拟合，采样点（ADC按节拍来测量当前云台运动的位置的点）直接采用Y=a*t2/2来确定，其中a为千分度云台的最大加速度，t为采样时间间隔。控制算法：采用位置增量式PID控制，通过PWM调节方位电机驱动模块8和俯仰电机驱动模块9，规避速度和加速度突变，有效消除运动的抖动，使云台的方位/俯仰两个自由度运动的速度达到最大值。采用双环控制算法也能快速完成启始段的工作。相对编码器的四倍频计算采用硬件芯片完成，能更好地保证控制的精度。

在高速段（中间段）由于速度突变也会影响运动的抖动，因此采用设定的规划路线，云台采用位置增量式PI控制，按照最大工作速度以匀速运动方式运动到设定点（高速段的终点），采样点直接采用Y=V*t来确定，其中V为最大速度，t最采样时间间隔。在高速段由于是恒高速运行，采用位置增量式PI算法就可以完成，双环控制算法也能满足要求。

在结束段，靠近终点（步骤二指定位置）时，先采用均匀减速，最后段平滑反向加速，通过平滑运动，消除运动冲击、震动，然后由实时检测的齿隙值，计算运动补偿值：云台在结束段必须要改变相关的路径点（图8中按匀减速设定的空心点），从而可以平滑速度和加速度的突变，如附图6所示中冗余的实心黑点所示，通过改变采样点的位置值（实际上是反馈的编码器的值），可以修改规划的路径，这部分都必须在路径规划中选确定，采样率为8M，实际路径为采样间隔路径的分段拟合；特征分段点（路径分段的终点）和路径修改点（图8所示增加的黑点）存储在RAM中，方便调用。

在结束段，云台分成两步来完成：

第一步，采用增量式PID或PI位置控制算法，通过增加用于改变运动加速度的冗余位置点，改变运行路径，可精确达到指定的位置，并根据当前的位置与设定位置差，确定当前的综合齿隙误差，并计算当前位置补偿值，通过主控模块11来设定不同的位置点，按修改后的位置点，采用线性拟合方式，将云台驱动行走设定的位置点，卸载电机的驱动，让电机处于自由运动状态下。如附图5中s2～s3段实线所示，该冗余位置点（路径修改点）可以稍微延长该段的工作时间，并通过平滑速度和加速度将运动的冲击点消除或减缓，使得运动位置精度得到提高。

第二步，采用脉动暂停的方式驱动方位电机27和俯仰电机23：千分度云台工作在机械自锁条件下，云台停止工作后电机不带电，可以有效节省功耗，在负载的作用下，千分度云台会在重力力矩的作用下会转动一个微小的角度，该角度与云台传动系统综合齿隙相关（包括电机、减速器、蜗轮蜗杆等传动单元所有的齿轮副），主控模块11可以根据获得的云台当前位置值与设定位置值相减，从而得到该位置条件下的综合齿隙误差。由于齿轮加工、装配的非均匀性，磨损的非均匀性，每一个齿轮的齿隙是不相同的，且顺时针转动和逆时针转动的齿隙也不相同，但相邻齿轮的齿隙误差较小。

由于传动系统中齿轮、蜗轮蜗杆的齿隙误差不确定，必须通过脉动暂停的检测方式，才能精确测量当前位置的齿隙误差值。此外，由于运动过程中采样存在采样误差，因此为精确测量当前的位置值，共用脉动暂停的方式，在暂停时，通过短延时后，当负载扭矩消除综合齿隙后，能准确测量当前的位置值。该方式需要一个递归循环，可以有效提高千分度云台的位置控制精度。

本发明的千分度云台采用与传统云台不同的驱动方式：传统云台一般基于步进电机驱动，通过分度方式对目标进行测量和定位，采用的是开环控制方式；而本发明采用的是直流电机与编码器联合的闭环驱动方式，随着相对编码器分辨率提高、价格的下降，该方式不仅有效控制千分度云台的制造成本，还可以成倍提高重复定位精度。

为了提高运动精度，必须消除运动中的冲击和震动：由于运动中存在转动惯量，电机运动的加速度和速度控制在不同的运动阶段会有较大的变化，从而导致云台的冲击和震动。为了提高运动的速度，避免电机运动过程中不失步，并减小过冲，本发明将千分度云台的运动过程分成启始段、高速段和结束段三段，在启始段，保证电机不失步，则电机需要低速启动，然后按最大加速度，加速到最大速度，减少启始段的运动时间；在高速度，为了减小运动时间，要保持最大的速度走完该行程；在结束段，需要减小终点（步骤二中设定点）的冲击和震动。

将设定位置与当前位置之间的方位/俯仰差值作为云台在该位置的综合齿隙，按综合齿隙的2倍值作为补偿值，按计算的补偿值，向相反的方向运动，反向补偿以消除综合齿隙误差。由于该补偿值较小，采用双环控制算法保证位置度的同时，需要测算速度环，速度环由采样周期计算（位置差与采样时间比），采用双环控制可以避免因位置相差较小导致的长时间死循环。通过四倍频计数器12计算得到的当前计数值与设定的编码反馈模块的值差即为综合齿隙误差，正常情况下，一次补偿就可以完成齿隙误差的消除，如果齿轮加工精度较差，二次或多次补偿能满足云台精度的要求。

千分度云台的综合齿隙误差如附图10所示，该图中只画出一最后一级传动齿轮副，由于齿轮传动中发热，同时齿轮各个齿还存在加工误差，为了防止齿轮传动中啮合卡死现象，每一个齿轮副都会存在安装间隙；向一个方向转动时，不存在齿隙，但反向转动时，就会有一个较大的空回，该空回就是当前转动位置的综合齿隙，该综合齿隙包含传动系统中所有齿隙副的齿隙，因此综合齿隙不是一个恒定值，在不同的位置，值会有变化。

在结束段中运动运动接近终点时，由于负载及综合齿隙的影响，通过脉动暂停来检测驱动力卸载后产生的齿隙误差。由于无法准确确定整个传动系统的齿隙误差，采用实时测量的方式，通过增量PID控制算法，可以准确驱动千分度云台走到指定的位置，然后暂停电机驱动，此时在云台的负载驱动下，将会导致云台自由转动，消除当前的综合齿隙误差。负载引起的运动，会导致码反馈模块数字的变化，必须计于总的计数值内。无论采样速度多快，对于电机运动都存在迟滞或滞后。为了消除采样误差，周期性脉动暂停，可以保证当前采样值的精度。多重循环可以递归消除不同传动系统不同齿隙啮合位置的综合齿隙误差值。由于运动过程中采样，存在采样误差值，在脉动暂停过程中采样，可以消除采样的误差，通过循环补偿方式，可以消除不同位置齿隙导致的误差。

在千分度云台的负载重力扭矩和运动应力下，云台会转动到一个平衡位，在暂停过程中，一直循环采样，如果采样值不再变化，则表示云台运动结束。云台在负载下运动如附图9所示，特别是云台的俯仰运动，由于支撑的光电系统重量较大，云台俯仰的角度越大，负载的扭矩也越大，虽然蜗轮蜗杆会采生一个自锁力，但传动系统中的齿隙（也可当作空回），是无法自锁住的。

再次设定终点（补偿位置设置点）的位置，并采用双环控制方式，由于有速度环，可以保证循环不会因驱动力微小于阻力而长时间无法跳出循环。完成循环后，再次暂停，由负载自由转动，消除综合齿隙，并采样当前的位置值，如果没有达到指的精密，按上述的方式，再次进行补偿，如果达到指定的精度，则可跳出循环，表明该次运动控制完成。

暂停驱动，让负载带动云台自由转动到自锁死位置，采样当前的位置编码值，并与b1)中设定的位置值比较，如果精度达到或小于0.005度的要求，则可以跳出循环，如果精度不满足要求，则可以比较上一次误值来调整补偿值，然后跳到b6)步进行循，正常控制不应超过三次循环补偿。

如果千分度云台结构设计及控制正常，一次补偿就可以满足位置精度的要求，如果三次循环（一次正常补偿，一次正向修正，一次反向修正），都无法运动到正确的位置，则说明双环控制设定的参数值需要修正。

运动控制实际上是在采样过程中，每段位置与设定位置是否一致，如果不一致时，通过修改PWM占空比来增加或减小电机的驱动扭矩来匹配。根据主控计算能，及设定电路硬件要求，采样率速度越高越好，但硬件设计必须支撑。如采用cortex M4的ARM芯片，可以支撑8M的采样率。

如果上述的路径规划完成后，差将路径中的点自动计算并存储在相应的存储器中后，就可以运动各段的控制算法来完成云台的设定位置

上述行程的改变主要是通过在行程中插入冗余位置点（路径修改点）来调整，云台运动在接近终点（步骤二设置点）时，需要控制速度和加速度的突变，通过插入冗余位置点，平滑速度和加速度运动曲线，合理减小运动的抖动，从而提高位置定位精度。

云台的运动控制中，在第一次运动到设定的终点（步骤二设置点）时，采用位置精确控制方式，保证该位置的精度，再通过负载随机转动，可以精确计算当前位置的综合齿隙误差；在极小位置差的情况下，主要采用速度制方式，辅助位置控制方式，能避免运动死循环；在运动中增加周期性脉动短暂停，既可以消除采样的误差，又可以满足因周期性脉动短暂停而导致负载自动驱动千分度云台的旋转，用于计算当前的综合齿隙；最后采用递归循环，减少千分度云台邻近点（传动最后一级齿轮）齿隙误差的不同。

本发明控制方法中，针对上述两种齿隙误差，千分度云台的位置定位方式采用两步进行，第一步采用路径规划方式，根据选定的最大速度和加速度，按最短工作时间要求，合理确定云台的运动路径，设置两段或三段运动路径，如果设定的位置与当前的位置有较大的差距（相对于编码器的反馈值，最小要大于1000个计数值或1度以上），可以进行三段路径分段控制，并采用不同的控制算法，根据分段路径，计算出各个分段的速度和加速度，并选定运动的方式，减小运动中的冲击和震动。第二步采用运动控制方式，第一阶段采用增量式PID或PI位置控制算法，可精确达到指定的位置，并根据当前的位置与设定位置差，确定当前的综合齿隙误差，并计算当前位置补偿值；第二阶段采用双环控制算法，可以消除较小位置差的运动抖动，同时采用周期性脉动检测、通过暂停驱动力，可以有效计算云台在负载下的齿隙误差，并设定精确的补偿值，此外，暂停驱动还可以消除采样频率引起的误差使得千分度云台的控制精度满足设定要求。

对于微步距运动控制，如果相对距离小于上述的值，只作二段路径分段（启始段和结束段），该二段的运动控制算法与三段路径中相同段的控制方法相同，可保证运动精度，节省运动时间。由于传动系统中齿轮、蜗轮蜗杆的齿隙误差不确定，必须通过脉动暂停的检测方式，才能精确测量当前位置的齿隙误差值。此外，由于运动过程中采样存在采样误差，因此为精确测量当前的位置值，共用脉动暂停的方式，在暂停时，通过短延时后，当负载扭矩消除综合齿隙后，能准确测量当前的位置值。该方式需要一个递归循环，可以有效提高千分度云台的位置控制精度。通过周期性短暂停，消除运动采样误差，脉动递归双环控制循环方法，提高定位精度。

根据千分度云台允许的最大速度和最大加速度，合理分配三段路径的长度，从而能较快地实现千分度云台的位置精确控制。经过上述两步，在相对编码器分辨率达到90000时，千分度云台可以达到0.003°以上的精度值，由于微步进采用双环控制方式，方位可以达到0.001°精度，俯仰能达到0.002°精度。

在附图6中，空心点用来初步计算规划的路径，如按空心点的规划路径，在三段的接合处都将会有较大的冲击和震动，影响云台的运动精度，但在启始段与高速段和高速段与结束段接合处的冲击、震动由于不在最终的停止位，可以通过后续的运动调节补偿，在结束段与停止之间，需要通过插入冗余位置点（路径修改点）来调整规划的路径，从而消除或减小停止处的冲击和震动。

上述实施例仅例示性说明本发明的原理及其功效，以及部分运用的实施例，对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (7)

1.一种千分度精密云台运动控制装置，其特征在于：由壳体（20）和设于壳体（20）内的方位驱动部件（2）、俯仰驱动部件（3）、指北部件（4）、控制部件（5）、网络通信部件（6）及电源管理部件（19）组成；其中

所述的方位驱动部件（2）包括方位电机（27）、方位反馈模块（7）和方位电机驱动模块（8），所述的方位反馈模块（7）由方位限位开关（15）和方位编码反馈模块（16）组成；

所述的俯仰驱动部件（3）包括俯仰电机（23）、俯仰反馈模块（10）和俯仰电机驱动模块（9），所述的俯仰反馈模块（10）由俯仰限位开关（17）和俯仰编码反馈模块（18）组成，方位限位开关（15）、方位编码反馈模块（16）、俯仰限位开关（17）和俯仰编码反馈模块（18）完成运动的反馈；

所述的控制部件（5）由主控模块（11）、连接方位编码反馈模块（16）的方位四倍频模块（12）和连接俯仰编码反馈模块（18）的俯仰四倍频模块（21）组成；

所述的网络通信部件（6）由网络通信部件（14）、第一串口控制模块（13）和第二串口控制模块（22）组成；

所述的电源管理部件（19）由反接保护模块、过压/过流保护模块和DC-DC模块依次连接而成。

2.根据权利要求1所述的一种千分度精密云台运动控制装置，其特征在于，所述的指北开关（4）包括方位光电开关和俯仰光电开光；所述的方位限位开关（15）和俯仰限位开关（17）为行程开关或光电开关。

3.根据权利要求1所述的一种千分度精密云台运动控制装置，其特征在于，所述的方位电机（27）通过方位蜗轮蜗杆（30）连接方位传动轴（29），所述的方位编码反馈模块（16）同轴连接在方位传动轴（29）上。

4.根据权利要求1所述的一种千分度精密云台运动控制装置，其特征在于，所述的俯仰电机（23）通过俯仰蜗轮蜗杆（25）连接俯仰传动轴（26），所述的俯仰编码反馈模块（18）同轴连接在俯仰传动轴（26）上。

5.一种如权利要求1所述千分度精密云台运动控制装置的控制方法，其特征在于，包括如下步骤：

步骤一，设定云台的最大工作速度和最大加速度值，然后由主控模块（11）计算出按最大加速度从初始状态加速到最大速度的时间；

步骤二，通过网络通信部件（6）设定云台的相对或绝对位置，然后通过第一串口控制模块（13）将设定位置传递给主控模块（11），主控模块（11）控制方位电机驱动模块（8）和俯仰电机驱动模块（9）同时运动，并读取方位编码反馈模块（16）和俯仰编码反馈模块（18）的采样值，获得云台的当前位置；

步骤三，主控模块（11）将接收的设定位置与云台的当前位置比较，计算设定位置与当前位置之间的方位/俯仰差值：方位差值为正时，顺时针转动，反之逆时针转；俯仰差值为正时，向下转动，反之向上转动；

步骤四，根据上述差值的范围，按最短工作时间要求，来对云台的运动路径分段；如果差值≤1°，则将云台路径分为启始段和结束段；如果差值＞1°，则将云台路径分为启始段、高速段和结束段；其中启始段为云台由静止加速到最大速度的时间所运动的行程，结束段为云台由最大速度减速到停止的时间所运动的行程，高速段为启始段和结束段之间的行程；

步骤五，云台运动控制：

在启始段，云台按照最大加速度采用匀加速的运动方式运行到启始段终点，采用位置增量式PID控制，通过PWM调节方位电机驱动模块（8）和俯仰电机驱动模块（9），使云台的方位/俯仰两个自由度运动的速度达到最大值；

在高速段，云台采用位置增量式PI控制，按照最大工作速度以匀速运动方式运动到高速段终点；

在结束段，云台分成两步来完成：第一步，采用增量式PID或PI位置控制算法，通过增加用于改变运动加速度的冗余位置点，改变运行路径；第二步，采用脉动暂停的方式驱动方位电机（27）和俯仰电机（23），在靠近终点时，先采用均匀减速，最后平滑向加速直到零，减小加速度突变产生的振动和冲击。

6.根据权利要求5所述的一种千分度精密云台运动控制方法，其特征在于，将设定位置与当前位置之间的方位/俯仰差值作为云台在该位置的综合齿隙，按综合齿隙的2倍值作为补偿值，反向补偿以消除综合齿隙误差，采用双环控制算法保证位置度。

7.根据权利要求6所述的一种千分度精密云台运动控制方法，其特征在于，在结束段中运动运动接近终点时，通过脉动暂停、同步采用来检测驱动力卸载后产生的齿隙误差，并根据精度要求作出多次补偿修正。