专利名称：陀螺经纬仪粗寻北的自动跟踪pid控制系统及控制方法
技术领域：
本发明涉及陀螺经纬仪定位与定向领域，特别涉及一种陀螺经纬仪粗寻北的自动 跟踪PID控制系统及控制方法。
背景技术：
陀螺经纬仪可以在地理南北纬75°范围内的地面、矿井或隧道任意测点指示出真 北方向，在工程施工测量、舰船制造、军事瞄准中具有广泛的应用。陀螺经纬仪的定向过程一般包括粗定向(或粗寻北)和精定向(或精寻北)两 步，即首先进行粗寻北，目的是将陀螺经纬仪照准部视准轴置于偏离真北士30'之内，然后 再利用中天法进行精寻北，从而找到真北方向。传统的粗寻北一般采用人工手动跟踪逆转 点法来实现，经测定，手动跟踪逆转点法可以使视准轴最终指向偏离真北士 10'以内，这一 精度已经能够满足精寻北的要求(士30')，观测时间不超过10分钟。发明人在实现本发明的过程中发现，现有技术中至少存在以下的缺点传统的粗寻北的缺点是操作人员劳动强度大，手动跟踪只能依靠经验和手感，这 对于新手而言很难操作，轻则手动跟踪不均勻，产生脉动式的扭力影响，重则不能及时跟踪 上陀螺经纬仪运动，损毁陀螺经纬仪，实践证明测量误差50%是由手动操作引起的，且在 测量开始时需要朝向近似真北。

发明内容
为了解决上述问题，减少人为因素对陀螺经纬仪寻北测量的影响，提高陀螺经纬 仪寻北的自动化程度，本发明提供了一种陀螺经纬仪粗寻北的自动跟踪PID控制系统及控 制方法，一种陀螺经纬仪粗寻北的自动跟踪PID控制系统，包括电荷耦合元件、复杂可编 程逻辑器件、陀螺经纬仪、微控制单元、键盘、显示模块、PID控制模块和转台电机，所述微控制单元通过所述复杂可编程逻辑器件驱动所述电荷耦合元件工作，反映 陀螺房运动信息的光标信息通过陀螺经纬仪的光路系统成像，光标像由所述电荷耦合元件 接收，通过所述电荷耦合元件的光电转换，将光信号转变为反映光强的电压模拟信号，所述 电压模拟信号，经模拟/数字转换芯片转换为数字信号，所述电荷耦合元件采集陀螺光标 位置，将所述陀螺光标位置传送给所述微控制单元，所述微控制单元检测陀螺光标位置值 与陀螺零位值之间的陀螺偏差值，根据所述陀螺偏差值向所述PID控制模块发送控制命 令，所述PID控制模块接收所述微控制单元发送的控制命令，根据所述控制命令驱动所述 转台电机实时跟踪陀螺经纬仪，直到所述微控制单元找到两个逆转点，记录两个逆转点并 计算粗北方向值，托起陀螺灵敏部，将陀螺照准部转向测得的粗北方向；所述键盘和所述显 示模块分别与所述微控制单元相连。所述PID控制模块具体包括位置控制调节单元、速度控制调节单元，相应地，所 述PID控制模块接收所述微控制单元发送的控制命令，根据所述控制命令驱动所述转台电机实时跟踪陀螺经纬仪具体为接收所述微控制单元发送的控制命令，通过所述位置控制调节单元对所述转台电 机的位置进行调节，通过所述速度控制调节单元对所述转台电机的速度进行调节，进而驱 动所述转台电机实时跟踪陀螺经纬仪。一种陀螺经纬仪粗寻北的自动跟踪PID控制系统的控制方法，所述方法包括以下 步骤(1)自动跟踪PID控制系统上电，判断所述自动跟踪PID控制系统是否输出跟踪启 动信号，如果是，执行步骤(2)，如果否，所述自动跟踪PID控制系统重新上电；(2)电荷耦合元件第η次采集陀螺光标位置，将第η次陀螺光标位置传送给微控制 单元，其中η为采集的次数，η为大于1的正整数；(3)所述微控制单元根据第η次陀螺光标位置值、陀螺零位值，获取第η次陀螺偏
差值；(4)判断当第η次陀螺偏差值的信号方向与第η-1次陀螺偏差值的信号方向相反， 且至少第η+1次陀螺偏差值的信号方向、第η+2次陀螺偏差值的信号方向与所述第η次陀 螺偏差值的信号方向相同，执行步骤(5)；否则，执行步骤(8)；(5)所述第η次陀螺光标位置值所对应的陀螺光标位置为第一逆转点；(6)重新执行步骤(2)至步骤(5)，所述微控制单元判断是否找到第二逆转点，如 果是，执行步骤⑵；否则，执行步骤⑶；(7)所述微控制单元根据所述第一逆转点和所述第二逆转点计算陀螺北方向值， 获取陀螺粗北方向，将陀螺转到粗北方向，流程结束；(8)所述微控制单元判断步骤(3)中的陀螺偏差值的绝对值是否大于分离阈值， 如果是，执行步骤(9)，如果否，执行步骤(10)；(9)所述微控制单元对所述陀螺偏差值进行比例计算和微分计算；(10)所述微控制单元对所述陀螺偏差值进行比例计算、微分计算和积分计算；(11)所述微控制单元根据步骤(9)或步骤(10)的计算结果，计算对陀螺的控制 量；(12)所述微控制单元根据所述控制量确定转台电机旋转的角度，对所述转台电机 进行旋转，实现所述转台电机实时跟踪陀螺经纬仪，执行步骤(3)。本发明提供的技术方案的有益效果是本发明通过利用无扭矩跟踪逆转点法进行自动粗寻北，通过本发明提供的陀螺经 纬仪粗寻北的自动跟踪PID控制系统及控制方法，可以实现陀螺经纬仪满量程360°范围 内粗寻北，减少了人为因素对陀螺经纬仪寻北测量的影响，提高了陀螺经纬仪寻北的自动
化程度。


图1为本发明提供的陀螺光标在CXD焦平面上的运动轨迹示意图；图2为本发明提供的陀螺光标的运动轨迹示意图；图3为本发明提供的陀螺光标像反映在CXD上的往复运动示意图；图4为本发明提供的陀螺经纬仪粗寻北的自动跟踪PID控制系统；
图5为本发明提供的控制方法的流程图。
具体实施例方式为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方 式作进一步地详细描述。为了解决上述问题，减少人为因素对陀螺经纬仪寻北测量的影响，提高陀螺经纬 仪寻北的自动化程度，本发明实施例提供了一种陀螺经纬仪粗寻北的自动跟踪PID控制系 统及控制方法，详见下文描述图1为陀螺光标在CCD (Charge-coupled Device,电荷耦合元件)焦平面上的运 动轨迹，有左右和上下的两种位移，只要光标足够长，就能保证扫过C⑶受光面。将图1展 开，得到图2中的陀螺光标的运动轨迹，该运动轨迹是一个逆时针的椭圆运动，由于短轴上 的陀螺光标的运动轨迹很短，基本可以忽略，即经过化简得到图3中的陀螺光标的往复直 线运动。转台电机实时跟踪陀螺光标运动，直到在一个旋转方向上，到达最远点即此旋转方 向上的逆转点，同理跟踪到另一旋转方向上的逆转点，记录两个逆转点，并根据两个逆转点 计算北方向值，及时托起陀螺灵敏部，将陀螺照准部转向测得的北方向。由陀螺运动特点可知，在不同初始方位下启动陀螺进行一次寻北测量，陀螺摆动 的幅值、相位以及陀螺运动可能出现的最大速度，由于哥氏力的影响很难确定，所以要想使 陀螺光标位置值、陀螺零位置值之间存在的陀螺偏差值始终保持为0，实时无偏差跟上陀螺 光标运动不现实。通过本发明实施例提供的陀螺经纬仪粗寻北的自动跟踪PID控制系统和 控制方法，来实时采集陀螺光标位置，并在上一状态基础上，预测陀螺运动的下一状态，积 累这些信息可以调节比例计算P、积分计算I、微分运算D，从而控制转台电机运动，使陀螺 偏差值保持在一定范围之内，即可实现实时跟踪陀螺经纬仪及无扭矩跟踪陀螺经纬仪。在 数字PID控制系统中，积分计算I产生积分积累，会产生过大的超调量，甚至在陀螺零位值 上下振荡，这在转台电机的运行过程中是不允许的。为了减少在转台电机运行过程中积分 计算I对数字PID控制系统动态性能的影响，需要在转台电机的启、停阶段或大幅值进给 时，采用PID预限削弱积分控制方法。即，当陀螺偏差值的绝对值大于分离阈值时，只采用 比例计算P、微分运算D，取消积分计算I ；当转台电机的陀螺偏差值的绝对值小于等于分离 阈值时，则采用比例计算P、微分运算D和积分计算I，以消除自动跟踪PID控制系统的稳态 误差。基于上述设计原理，设计了陀螺经纬仪粗寻北的自动跟踪PID控制系统，参见图 4，该陀螺经纬仪粗寻北的自动跟踪PID控制系统包括CCD、CPLD(CompIexProgrammabIe Logic Device，复杂可编程逻辑器件)、陀螺经纬仪、MCU(MicroControl Unit，微控制单 元)、键盘、显示模块、PID控制模块和转台电机，MCU通过CPLD驱动CXD工作，反映陀螺房运动信息的光标信息通过陀螺经纬仪的 光路系统成像，光标像由CCD接收，通过CCD的光电转换，将光信号转变为反映光强的电压 模拟信号，该电压模拟信号，经A/D (Analog Digital，模拟/数字)转换芯片转换为数字信 号，CCD采集陀螺光标位置，将陀螺光标位置传送给MCU，MCU检测陀螺光标位置值与陀螺零 位置值之间的陀螺偏差值，根据陀螺偏差值向PID控制模块发送控制命令，PID控制模块接 收MCU发送的控制命令，根据控制命令驱动转台电机实时跟踪陀螺经纬仪，直到MCU找到两
6个逆转点，记录两个逆转点并计算粗北方向值，托起陀螺灵敏部，将陀螺照准部转向测得的 粗北方向；键盘和显示模块分别与MCU相连。其中，PID控制模块包括位置控制调节单元、速度控制调节单元，相应地，PID控 制模块接收MCU发送的控制命令，根据控制命令驱动转台电机实时跟踪陀螺经纬仪具体 为接收MCU发送的控制命令，通过位置控制调节单元对转台电机的位置进行调节，通过速 度控制调节单元对转台电机的速度进行调节，进而驱动转台电机实时跟踪陀螺经纬仪。其中，键盘输入为人机接口模块，通过对键盘的控制可以实现各种操作，显示模块 用于对MCU的运算结果进行输出。综上所述，本发明实施例提供了一种陀螺经纬仪粗寻北的自动跟踪PID控制系 统，通过本发明实施例提供的系统可以实现陀螺经纬仪满量程360°范围内自动粗寻北，减 少了人为因素对陀螺经纬仪寻北测量的影响，提高了陀螺经纬仪寻北的自动化程度，提高 了测量精度。一种陀螺经纬仪粗寻北的自动跟踪PID控制系统的控制方法，参见图5，详见下文 描述101 自动跟踪PID控制系统上电，判断自动跟踪PID控制系统是否输出跟踪启动 信号，如果是，执行步骤102，如果否，自动跟踪PID控制系统重新上电；102 :(XD第η次采集陀螺光标位置，将第η次陀螺光标位置传送给MCU，其中η为 采集的次数，η为大于1的正整数；103 =MCU根据第η次陀螺光标位置值、陀螺零位置值，获取第η次陀螺偏差值；其中，在实际应用中，在陀螺下放以前，根据CCD的测量结果，MCU就可以得到陀 螺零位置，进而得到陀螺零位置值。在采用自动跟踪逆转点法的寻北原理关键是能否实时 跟踪上陀螺灵敏部光标运动，保证无扭矩的跟踪状态，此时需要取一陀螺偏差值ε，即陀螺 光标位置值与陀螺零位置值的偏差值，通过控制调节，使陀螺偏差值ε保持在一定范围之 内，其中，ε的取值范围根据具体的应用情况进行设定，本发明实施例优选为2'，具体实 现时，本发明实施例对此不做限制。104 判断当第η次陀螺偏差值的信号方向与第η_1次陀螺偏差值的信号方向相 反，且至少第n+1次陀螺偏差值的信号方向、第n+2次陀螺偏差值的信号方向与第η次陀螺 偏差值的信号方向相同，执行步骤105 ；否则，执行步骤108 ；105 第η次陀螺光标位置值所对应的陀螺光标位置为第一逆转点；其中，例如η等于3，即当第3次陀螺偏差值的信号方向与第2次陀螺偏差值的信 号方向相反，且至少第4次陀螺偏差值的信号方向、第5次陀螺偏差值的信号方向与第3次 陀螺偏差值的信号方向相同，则第3次陀螺光标值所对应的陀螺光标位置为第一逆转点；106 重新执行步骤102-105，MCU判断是否找到第二逆转点，如果是，执行步骤 107;否则，执行步骤108;107 :MCU根据第一逆转点和第二逆转点计算陀螺北方向值，获取陀螺粗北方向， 将陀螺照准部转到获取到的粗北方向，流程结束；108 :MCU判断步骤103中的陀螺偏差值的绝对值是否大于分离阈值ε，如果是，执 行步骤109，如果否，执行步骤110 ；其中，根据实际应用情况，来设定分离阈值ε (零位偏差值的极限值)，当|e(k)
7> ε时，即偏差值较大时，仅采用PD控制，使自动跟踪PID控制系统有较快的响应；当 e(k) I ^ ε时，即偏差值较小时，采用PID控制，以消除自动跟踪PID控制系统的稳态误 差。通常在实际的应用中，如果陀螺初始方位角处于0° -30°之内，下放陀螺马达后，陀螺 摆动速度不高，此时通常|e(k) I ( ε，适宜进行PID控制，随着陀螺房向着子午面靠近，速 度偏摆加快，控制量偏差到达上限，需进行积分分离PD控制。如果初始陀螺方位角度偏大， 下放陀螺马达后，因其摆幅偏大，陀螺会快速摆动起来，此时很容易超过分离阈值，即此时 通常|e(k) I > ε，需要进行PD控制。本发明实施例利用陀螺偏差值的绝对值和分离阈值之间的逻辑运算，可以很方便 地确定预限削弱积分PID控制的进程，实现转台电机的预限削弱积分PID控制，弥补模拟 PID调节控制的不足。109 =MCU对陀螺偏差值进行比例计算和微分计算；110 =MCU对陀螺偏差值进行比例计算、微分计算和积分计算；其中，通过步骤109中的比例计算和微分计算或步骤110中的比例计算、微分计算 和积分计算可以对转台电机的位置、速度进行调节。111 :MCU根据步骤109或110的计算结果，计算对陀螺的控制量；112 =MCU根据控制量确定转台电机的旋转角度，对转台电机进行旋转，实现转台 电机实时跟踪陀螺经纬仪，执行步骤103。综上所述，本发明实施例提供了一种陀螺经纬仪粗寻北的自动跟踪PID控制系统 的控制方法，通过本发明实施例提供的方法可以实现陀螺经纬仪满量程360°范围内自动 粗寻北，减少了人为因素对陀螺经纬仪寻北测量的影响，提高了陀螺经纬仪寻北的自动化 程度，提高了测量精度。本领域技术人员可以理解附图只是一个优选实施例的示意图，上述本发明实施例 序号仅仅为了描述，不代表实施例的优劣。以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和 原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
权利要求
一种陀螺经纬仪粗寻北的自动跟踪PID控制系统，其特征在于，包括电荷耦合元件、复杂可编程逻辑器件、陀螺经纬仪、微控制单元、键盘、显示模块、PID控制模块和转台电机，所述微控制单元通过所述复杂可编程逻辑器件驱动所述电荷耦合元件工作，反映陀螺房运动信息的光标信息通过陀螺经纬仪的光路系统成像，光标像由所述电荷耦合元件接收，通过所述电荷耦合元件的光电转换，将光信号转变为反映光强的电压模拟信号，所述电压模拟信号，经模拟/数字转换芯片转换为数字信号，所述电荷耦合元件采集陀螺光标位置，将所述陀螺光标位置传送给所述微控制单元，所述微控制单元检测陀螺光标位置值与陀螺零位置值之间的陀螺偏差值，根据所述陀螺偏差值向所述PID控制模块发送控制命令，所述PID控制模块接收所述微控制单元发送的控制命令，根据所述控制命令驱动所述转台电机实时跟踪陀螺经纬仪，直到所述微控制单元找到两个逆转点，记录两个逆转点并计算粗北方向值，托起陀螺灵敏部，将陀螺照准部转向测得的粗北方向；所述键盘和所述显示模块分别与所述微控制单元相连。
2.根据权利要求1所述的陀螺经纬仪粗寻北的自动跟踪PID控制系统，其特征在于，所 述PID控制模块具体包括位置控制调节单元、速度控制调节单元，相应地，所述PID控制模 块接收所述微控制单元发送的控制命令，根据所述控制命令驱动所述转台电机实时跟踪陀 螺经纬仪具体为接收所述微控制单元发送的控制命令，通过所述位置控制调节单元对所述转台电机的 位置进行调节，通过所述速度控制调节单元对所述转台电机的速度进行调节，进而驱动所 述转台电机实时跟踪陀螺经纬仪。
3.一种用于权利要求1所述的陀螺经纬仪粗寻北的自动跟踪PID控制系统的控制方 法，其特征在于，所述方法包括以下步骤(1)自动跟踪PID控制系统上电，判断所述自动跟踪PID控制系统是否输出跟踪启动信 号，如果是，执行步骤(2)，如果否，所述自动跟踪PID控制系统重新上电；(2)电荷耦合元件第η次采集陀螺光标位置，将第η次陀螺光标位置传送给微控制单 元，其中η为采集的次数，η为大于1的正整数；(3)所述微控制单元根据第η次陀螺光标位置值、陀螺零位值，获取第η次陀螺偏差值；(4)判断当第η次陀螺偏差值的信号方向与第η-1次陀螺偏差值的信号方向相反，且至 少第η+1次陀螺偏差值的信号方向、第η+2次陀螺偏差值的信号方向与所述第η次陀螺偏 差值的信号方向相同，执行步骤(5)；否则，执行步骤⑶；(5)所述第η次陀螺光标位置值所对应的陀螺光标位置为第一逆转点；(6)重新执行步骤(2)至步骤(5)，所述微控制单元判断是否找到第二逆转点，如果是， 执行步骤(7)；否则，执行步骤⑶；(7)所述微控制单元根据所述第一逆转点和所述第二逆转点计算陀螺北方向值，获取 陀螺粗北方向，将陀螺转到粗北方向，流程结束；(8)所述微控制单元判断步骤(3)中的陀螺偏差值的绝对值是否大于分离阈值，如果 是，执行步骤(9)，如果否，执行步骤(10)；(9)所述微控制单元对所述陀螺偏差值进行比例计算和微分计算；(10)所述微控制单元对所述陀螺偏差值进行比例计算、微分计算和积分计算；(11)所述微控制单元根据步骤(9)或步骤(10)的计算结果，计算对陀螺的控制量；(12)所述微控制单元根据所述控制量确定转台电机旋转的角度，对所述转台电机进行 旋转，实现所述转台电机实时跟踪陀螺经纬仪，执行步骤(3)。
全文摘要
本发明公开了一种陀螺经纬仪粗寻北的自动跟踪PID控制系统及控制方法，属于陀螺经纬仪定位与定向领域，本发明通过利用无扭矩跟踪逆转点法进行自动粗寻北，通过本发明提供的陀螺经纬仪粗寻北的自动跟踪PID控制系统及控制方法，可以实现陀螺经纬仪满量程360°范围内粗寻北，减少了人为因素对陀螺经纬仪寻北测量的影响，提高了陀螺经纬仪寻北的自动化程度。
文档编号G01C19/00GK101980095SQ201010286648
公开日2011年2月23日 申请日期2010年9月19日 优先权日2010年9月19日
发明者冯莉, 宋乐, 林玉池, 董桂梅, 黄银国, 齐永岳 申请人:天津大学