專利名稱：陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤pid控制系統(tǒng)及控制方法
技術(shù)領(lǐng)域：
本發(fā)明涉及陀螺經(jīng)緯儀定位與定向領(lǐng)域，特別涉及一種陀螺經(jīng)緯儀粗尋北的自動(dòng) 跟蹤PID控制系統(tǒng)及控制方法。
背景技術(shù)：
陀螺經(jīng)緯儀可以在地理南北緯75°范圍內(nèi)的地面、礦井或隧道任意測點(diǎn)指示出真 北方向，在工程施工測量、艦船制造、軍事瞄準(zhǔn)中具有廣泛的應(yīng)用。陀螺經(jīng)緯儀的定向過程一般包括粗定向(或粗尋北)和精定向(或精尋北)兩 步，即首先進(jìn)行粗尋北，目的是將陀螺經(jīng)緯儀照準(zhǔn)部視準(zhǔn)軸置于偏離真北士30'之內(nèi)，然后 再利用中天法進(jìn)行精尋北，從而找到真北方向。傳統(tǒng)的粗尋北一般采用人工手動(dòng)跟蹤逆轉(zhuǎn) 點(diǎn)法來實(shí)現(xiàn)，經(jīng)測定，手動(dòng)跟蹤逆轉(zhuǎn)點(diǎn)法可以使視準(zhǔn)軸最終指向偏離真北士 10'以內(nèi)，這一 精度已經(jīng)能夠滿足精尋北的要求(士30')，觀測時(shí)間不超過10分鐘。發(fā)明人在實(shí)現(xiàn)本發(fā)明的過程中發(fā)現(xiàn)，現(xiàn)有技術(shù)中至少存在以下的缺點(diǎn)傳統(tǒng)的粗尋北的缺點(diǎn)是操作人員勞動(dòng)強(qiáng)度大，手動(dòng)跟蹤只能依靠經(jīng)驗(yàn)和手感，這 對于新手而言很難操作，輕則手動(dòng)跟蹤不均勻，產(chǎn)生脈動(dòng)式的扭力影響，重則不能及時(shí)跟蹤 上陀螺經(jīng)緯儀運(yùn)動(dòng)，損毀陀螺經(jīng)緯儀，實(shí)踐證明測量誤差50%是由手動(dòng)操作引起的，且在 測量開始時(shí)需要朝向近似真北。

發(fā)明內(nèi)容
為了解決上述問題，減少人為因素對陀螺經(jīng)緯儀尋北測量的影響，提高陀螺經(jīng)緯 儀尋北的自動(dòng)化程度，本發(fā)明提供了一種陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系統(tǒng)及控 制方法，一種陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系統(tǒng)，包括電荷耦合元件、復(fù)雜可編 程邏輯器件、陀螺經(jīng)緯儀、微控制單元、鍵盤、顯示模塊、PID控制模塊和轉(zhuǎn)臺(tái)電機(jī)，所述微控制單元通過所述復(fù)雜可編程邏輯器件驅(qū)動(dòng)所述電荷耦合元件工作，反映 陀螺房運(yùn)動(dòng)信息的光標(biāo)信息通過陀螺經(jīng)緯儀的光路系統(tǒng)成像，光標(biāo)像由所述電荷耦合元件 接收，通過所述電荷耦合元件的光電轉(zhuǎn)換，將光信號(hào)轉(zhuǎn)變?yōu)榉从彻鈴?qiáng)的電壓模擬信號(hào)，所述 電壓模擬信號(hào)，經(jīng)模擬/數(shù)字轉(zhuǎn)換芯片轉(zhuǎn)換為數(shù)字信號(hào)，所述電荷耦合元件采集陀螺光標(biāo) 位置，將所述陀螺光標(biāo)位置傳送給所述微控制單元，所述微控制單元檢測陀螺光標(biāo)位置值 與陀螺零位值之間的陀螺偏差值，根據(jù)所述陀螺偏差值向所述PID控制模塊發(fā)送控制命 令，所述PID控制模塊接收所述微控制單元發(fā)送的控制命令，根據(jù)所述控制命令驅(qū)動(dòng)所述 轉(zhuǎn)臺(tái)電機(jī)實(shí)時(shí)跟蹤陀螺經(jīng)緯儀，直到所述微控制單元找到兩個(gè)逆轉(zhuǎn)點(diǎn)，記錄兩個(gè)逆轉(zhuǎn)點(diǎn)并 計(jì)算粗北方向值，托起陀螺靈敏部，將陀螺照準(zhǔn)部轉(zhuǎn)向測得的粗北方向；所述鍵盤和所述顯 示模塊分別與所述微控制單元相連。所述PID控制模塊具體包括位置控制調(diào)節(jié)單元、速度控制調(diào)節(jié)單元，相應(yīng)地，所 述PID控制模塊接收所述微控制單元發(fā)送的控制命令，根據(jù)所述控制命令驅(qū)動(dòng)所述轉(zhuǎn)臺(tái)電機(jī)實(shí)時(shí)跟蹤陀螺經(jīng)緯儀具體為接收所述微控制單元發(fā)送的控制命令，通過所述位置控制調(diào)節(jié)單元對所述轉(zhuǎn)臺(tái)電 機(jī)的位置進(jìn)行調(diào)節(jié)，通過所述速度控制調(diào)節(jié)單元對所述轉(zhuǎn)臺(tái)電機(jī)的速度進(jìn)行調(diào)節(jié)，進(jìn)而驅(qū) 動(dòng)所述轉(zhuǎn)臺(tái)電機(jī)實(shí)時(shí)跟蹤陀螺經(jīng)緯儀。一種陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系統(tǒng)的控制方法，所述方法包括以下 步驟(1)自動(dòng)跟蹤PID控制系統(tǒng)上電，判斷所述自動(dòng)跟蹤PID控制系統(tǒng)是否輸出跟蹤啟 動(dòng)信號(hào)，如果是，執(zhí)行步驟(2)，如果否，所述自動(dòng)跟蹤PID控制系統(tǒng)重新上電；(2)電荷耦合元件第η次采集陀螺光標(biāo)位置，將第η次陀螺光標(biāo)位置傳送給微控制 單元，其中η為采集的次數(shù)，η為大于1的正整數(shù)；(3)所述微控制單元根據(jù)第η次陀螺光標(biāo)位置值、陀螺零位值，獲取第η次陀螺偏
差值；(4)判斷當(dāng)?shù)讦谴瓮勇萜钪档男盘?hào)方向與第η-1次陀螺偏差值的信號(hào)方向相反， 且至少第η+1次陀螺偏差值的信號(hào)方向、第η+2次陀螺偏差值的信號(hào)方向與所述第η次陀 螺偏差值的信號(hào)方向相同，執(zhí)行步驟(5)；否則，執(zhí)行步驟(8)；(5)所述第η次陀螺光標(biāo)位置值所對應(yīng)的陀螺光標(biāo)位置為第一逆轉(zhuǎn)點(diǎn)；(6)重新執(zhí)行步驟(2)至步驟(5)，所述微控制單元判斷是否找到第二逆轉(zhuǎn)點(diǎn)，如 果是，執(zhí)行步驟⑵；否則，執(zhí)行步驟⑶；(7)所述微控制單元根據(jù)所述第一逆轉(zhuǎn)點(diǎn)和所述第二逆轉(zhuǎn)點(diǎn)計(jì)算陀螺北方向值， 獲取陀螺粗北方向，將陀螺轉(zhuǎn)到粗北方向，流程結(jié)束；(8)所述微控制單元判斷步驟(3)中的陀螺偏差值的絕對值是否大于分離閾值， 如果是，執(zhí)行步驟(9)，如果否，執(zhí)行步驟(10)；(9)所述微控制單元對所述陀螺偏差值進(jìn)行比例計(jì)算和微分計(jì)算；(10)所述微控制單元對所述陀螺偏差值進(jìn)行比例計(jì)算、微分計(jì)算和積分計(jì)算；(11)所述微控制單元根據(jù)步驟(9)或步驟(10)的計(jì)算結(jié)果，計(jì)算對陀螺的控制 量；(12)所述微控制單元根據(jù)所述控制量確定轉(zhuǎn)臺(tái)電機(jī)旋轉(zhuǎn)的角度，對所述轉(zhuǎn)臺(tái)電機(jī) 進(jìn)行旋轉(zhuǎn)，實(shí)現(xiàn)所述轉(zhuǎn)臺(tái)電機(jī)實(shí)時(shí)跟蹤陀螺經(jīng)緯儀，執(zhí)行步驟(3)。本發(fā)明提供的技術(shù)方案的有益效果是本發(fā)明通過利用無扭矩跟蹤逆轉(zhuǎn)點(diǎn)法進(jìn)行自動(dòng)粗尋北，通過本發(fā)明提供的陀螺經(jīng) 緯儀粗尋北的自動(dòng)跟蹤PID控制系統(tǒng)及控制方法，可以實(shí)現(xiàn)陀螺經(jīng)緯儀滿量程360°范圍 內(nèi)粗尋北，減少了人為因素對陀螺經(jīng)緯儀尋北測量的影響，提高了陀螺經(jīng)緯儀尋北的自動(dòng)
化程度。


圖1為本發(fā)明提供的陀螺光標(biāo)在CXD焦平面上的運(yùn)動(dòng)軌跡示意圖；圖2為本發(fā)明提供的陀螺光標(biāo)的運(yùn)動(dòng)軌跡示意圖；圖3為本發(fā)明提供的陀螺光標(biāo)像反映在CXD上的往復(fù)運(yùn)動(dòng)示意圖；圖4為本發(fā)明提供的陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系統(tǒng)；
圖5為本發(fā)明提供的控制方法的流程圖。
具體實(shí)施例方式為使本發(fā)明的目的、技術(shù)方案和優(yōu)點(diǎn)更加清楚，下面將結(jié)合附圖對本發(fā)明實(shí)施方 式作進(jìn)一步地詳細(xì)描述。為了解決上述問題，減少人為因素對陀螺經(jīng)緯儀尋北測量的影響，提高陀螺經(jīng)緯 儀尋北的自動(dòng)化程度，本發(fā)明實(shí)施例提供了一種陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系 統(tǒng)及控制方法，詳見下文描述圖1為陀螺光標(biāo)在CCD (Charge-coupled Device,電荷耦合元件)焦平面上的運(yùn) 動(dòng)軌跡，有左右和上下的兩種位移，只要光標(biāo)足夠長，就能保證掃過C⑶受光面。將圖1展 開，得到圖2中的陀螺光標(biāo)的運(yùn)動(dòng)軌跡，該運(yùn)動(dòng)軌跡是一個(gè)逆時(shí)針的橢圓運(yùn)動(dòng)，由于短軸上 的陀螺光標(biāo)的運(yùn)動(dòng)軌跡很短，基本可以忽略，即經(jīng)過化簡得到圖3中的陀螺光標(biāo)的往復(fù)直 線運(yùn)動(dòng)。轉(zhuǎn)臺(tái)電機(jī)實(shí)時(shí)跟蹤陀螺光標(biāo)運(yùn)動(dòng)，直到在一個(gè)旋轉(zhuǎn)方向上，到達(dá)最遠(yuǎn)點(diǎn)即此旋轉(zhuǎn)方 向上的逆轉(zhuǎn)點(diǎn)，同理跟蹤到另一旋轉(zhuǎn)方向上的逆轉(zhuǎn)點(diǎn)，記錄兩個(gè)逆轉(zhuǎn)點(diǎn)，并根據(jù)兩個(gè)逆轉(zhuǎn)點(diǎn) 計(jì)算北方向值，及時(shí)托起陀螺靈敏部，將陀螺照準(zhǔn)部轉(zhuǎn)向測得的北方向。由陀螺運(yùn)動(dòng)特點(diǎn)可知，在不同初始方位下啟動(dòng)陀螺進(jìn)行一次尋北測量，陀螺擺動(dòng) 的幅值、相位以及陀螺運(yùn)動(dòng)可能出現(xiàn)的最大速度，由于哥氏力的影響很難確定，所以要想使 陀螺光標(biāo)位置值、陀螺零位置值之間存在的陀螺偏差值始終保持為0，實(shí)時(shí)無偏差跟上陀螺 光標(biāo)運(yùn)動(dòng)不現(xiàn)實(shí)。通過本發(fā)明實(shí)施例提供的陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系統(tǒng)和 控制方法，來實(shí)時(shí)采集陀螺光標(biāo)位置，并在上一狀態(tài)基礎(chǔ)上，預(yù)測陀螺運(yùn)動(dòng)的下一狀態(tài)，積 累這些信息可以調(diào)節(jié)比例計(jì)算P、積分計(jì)算I、微分運(yùn)算D，從而控制轉(zhuǎn)臺(tái)電機(jī)運(yùn)動(dòng)，使陀螺 偏差值保持在一定范圍之內(nèi)，即可實(shí)現(xiàn)實(shí)時(shí)跟蹤陀螺經(jīng)緯儀及無扭矩跟蹤陀螺經(jīng)緯儀。在 數(shù)字PID控制系統(tǒng)中，積分計(jì)算I產(chǎn)生積分積累，會(huì)產(chǎn)生過大的超調(diào)量，甚至在陀螺零位值 上下振蕩，這在轉(zhuǎn)臺(tái)電機(jī)的運(yùn)行過程中是不允許的。為了減少在轉(zhuǎn)臺(tái)電機(jī)運(yùn)行過程中積分 計(jì)算I對數(shù)字PID控制系統(tǒng)動(dòng)態(tài)性能的影響，需要在轉(zhuǎn)臺(tái)電機(jī)的啟、停階段或大幅值進(jìn)給 時(shí)，采用PID預(yù)限削弱積分控制方法。即，當(dāng)陀螺偏差值的絕對值大于分離閾值時(shí)，只采用 比例計(jì)算P、微分運(yùn)算D，取消積分計(jì)算I ；當(dāng)轉(zhuǎn)臺(tái)電機(jī)的陀螺偏差值的絕對值小于等于分離 閾值時(shí)，則采用比例計(jì)算P、微分運(yùn)算D和積分計(jì)算I，以消除自動(dòng)跟蹤PID控制系統(tǒng)的穩(wěn)態(tài) 誤差?；谏鲜鲈O(shè)計(jì)原理，設(shè)計(jì)了陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系統(tǒng)，參見圖 4，該陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系統(tǒng)包括CCD、CPLD(CompIexProgrammabIe Logic Device，復(fù)雜可編程邏輯器件)、陀螺經(jīng)緯儀、MCU(MicroControl Unit，微控制單 元)、鍵盤、顯示模塊、PID控制模塊和轉(zhuǎn)臺(tái)電機(jī)，MCU通過CPLD驅(qū)動(dòng)CXD工作，反映陀螺房運(yùn)動(dòng)信息的光標(biāo)信息通過陀螺經(jīng)緯儀的 光路系統(tǒng)成像，光標(biāo)像由CCD接收，通過CCD的光電轉(zhuǎn)換，將光信號(hào)轉(zhuǎn)變?yōu)榉从彻鈴?qiáng)的電壓 模擬信號(hào)，該電壓模擬信號(hào)，經(jīng)A/D (Analog Digital，模擬/數(shù)字)轉(zhuǎn)換芯片轉(zhuǎn)換為數(shù)字信 號(hào)，CCD采集陀螺光標(biāo)位置，將陀螺光標(biāo)位置傳送給MCU，MCU檢測陀螺光標(biāo)位置值與陀螺零 位置值之間的陀螺偏差值，根據(jù)陀螺偏差值向PID控制模塊發(fā)送控制命令，PID控制模塊接 收MCU發(fā)送的控制命令，根據(jù)控制命令驅(qū)動(dòng)轉(zhuǎn)臺(tái)電機(jī)實(shí)時(shí)跟蹤陀螺經(jīng)緯儀，直到MCU找到兩
6個(gè)逆轉(zhuǎn)點(diǎn)，記錄兩個(gè)逆轉(zhuǎn)點(diǎn)并計(jì)算粗北方向值，托起陀螺靈敏部，將陀螺照準(zhǔn)部轉(zhuǎn)向測得的 粗北方向；鍵盤和顯示模塊分別與MCU相連。其中，PID控制模塊包括位置控制調(diào)節(jié)單元、速度控制調(diào)節(jié)單元，相應(yīng)地，PID控 制模塊接收MCU發(fā)送的控制命令，根據(jù)控制命令驅(qū)動(dòng)轉(zhuǎn)臺(tái)電機(jī)實(shí)時(shí)跟蹤陀螺經(jīng)緯儀具體 為接收MCU發(fā)送的控制命令，通過位置控制調(diào)節(jié)單元對轉(zhuǎn)臺(tái)電機(jī)的位置進(jìn)行調(diào)節(jié)，通過速 度控制調(diào)節(jié)單元對轉(zhuǎn)臺(tái)電機(jī)的速度進(jìn)行調(diào)節(jié)，進(jìn)而驅(qū)動(dòng)轉(zhuǎn)臺(tái)電機(jī)實(shí)時(shí)跟蹤陀螺經(jīng)緯儀。其中，鍵盤輸入為人機(jī)接口模塊，通過對鍵盤的控制可以實(shí)現(xiàn)各種操作，顯示模塊 用于對MCU的運(yùn)算結(jié)果進(jìn)行輸出。綜上所述，本發(fā)明實(shí)施例提供了一種陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系 統(tǒng)，通過本發(fā)明實(shí)施例提供的系統(tǒng)可以實(shí)現(xiàn)陀螺經(jīng)緯儀滿量程360°范圍內(nèi)自動(dòng)粗尋北，減 少了人為因素對陀螺經(jīng)緯儀尋北測量的影響，提高了陀螺經(jīng)緯儀尋北的自動(dòng)化程度，提高 了測量精度。一種陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系統(tǒng)的控制方法，參見圖5，詳見下文 描述101 自動(dòng)跟蹤PID控制系統(tǒng)上電，判斷自動(dòng)跟蹤PID控制系統(tǒng)是否輸出跟蹤啟動(dòng) 信號(hào)，如果是，執(zhí)行步驟102，如果否，自動(dòng)跟蹤PID控制系統(tǒng)重新上電；102 :(XD第η次采集陀螺光標(biāo)位置，將第η次陀螺光標(biāo)位置傳送給MCU，其中η為 采集的次數(shù)，η為大于1的正整數(shù)；103 =MCU根據(jù)第η次陀螺光標(biāo)位置值、陀螺零位置值，獲取第η次陀螺偏差值；其中，在實(shí)際應(yīng)用中，在陀螺下放以前，根據(jù)CCD的測量結(jié)果，MCU就可以得到陀 螺零位置，進(jìn)而得到陀螺零位置值。在采用自動(dòng)跟蹤逆轉(zhuǎn)點(diǎn)法的尋北原理關(guān)鍵是能否實(shí)時(shí) 跟蹤上陀螺靈敏部光標(biāo)運(yùn)動(dòng)，保證無扭矩的跟蹤狀態(tài)，此時(shí)需要取一陀螺偏差值ε，即陀螺 光標(biāo)位置值與陀螺零位置值的偏差值，通過控制調(diào)節(jié)，使陀螺偏差值ε保持在一定范圍之 內(nèi)，其中，ε的取值范圍根據(jù)具體的應(yīng)用情況進(jìn)行設(shè)定，本發(fā)明實(shí)施例優(yōu)選為2'，具體實(shí) 現(xiàn)時(shí)，本發(fā)明實(shí)施例對此不做限制。104 判斷當(dāng)?shù)讦谴瓮勇萜钪档男盘?hào)方向與第η_1次陀螺偏差值的信號(hào)方向相 反，且至少第n+1次陀螺偏差值的信號(hào)方向、第n+2次陀螺偏差值的信號(hào)方向與第η次陀螺 偏差值的信號(hào)方向相同，執(zhí)行步驟105 ；否則，執(zhí)行步驟108 ；105 第η次陀螺光標(biāo)位置值所對應(yīng)的陀螺光標(biāo)位置為第一逆轉(zhuǎn)點(diǎn)；其中，例如η等于3，即當(dāng)?shù)?次陀螺偏差值的信號(hào)方向與第2次陀螺偏差值的信 號(hào)方向相反，且至少第4次陀螺偏差值的信號(hào)方向、第5次陀螺偏差值的信號(hào)方向與第3次 陀螺偏差值的信號(hào)方向相同，則第3次陀螺光標(biāo)值所對應(yīng)的陀螺光標(biāo)位置為第一逆轉(zhuǎn)點(diǎn)；106 重新執(zhí)行步驟102-105，MCU判斷是否找到第二逆轉(zhuǎn)點(diǎn)，如果是，執(zhí)行步驟 107;否則，執(zhí)行步驟108;107 :MCU根據(jù)第一逆轉(zhuǎn)點(diǎn)和第二逆轉(zhuǎn)點(diǎn)計(jì)算陀螺北方向值，獲取陀螺粗北方向， 將陀螺照準(zhǔn)部轉(zhuǎn)到獲取到的粗北方向，流程結(jié)束；108 :MCU判斷步驟103中的陀螺偏差值的絕對值是否大于分離閾值ε，如果是，執(zhí) 行步驟109，如果否，執(zhí)行步驟110 ；其中，根據(jù)實(shí)際應(yīng)用情況，來設(shè)定分離閾值ε (零位偏差值的極限值)，當(dāng)|e(k)
7> ε時(shí)，即偏差值較大時(shí)，僅采用PD控制，使自動(dòng)跟蹤PID控制系統(tǒng)有較快的響應(yīng)；當(dāng) e(k) I ^ ε時(shí)，即偏差值較小時(shí)，采用PID控制，以消除自動(dòng)跟蹤PID控制系統(tǒng)的穩(wěn)態(tài)誤 差。通常在實(shí)際的應(yīng)用中，如果陀螺初始方位角處于0° -30°之內(nèi)，下放陀螺馬達(dá)后，陀螺 擺動(dòng)速度不高，此時(shí)通常|e(k) I ( ε，適宜進(jìn)行PID控制，隨著陀螺房向著子午面靠近，速 度偏擺加快，控制量偏差到達(dá)上限，需進(jìn)行積分分離PD控制。如果初始陀螺方位角度偏大， 下放陀螺馬達(dá)后，因其擺幅偏大，陀螺會(huì)快速擺動(dòng)起來，此時(shí)很容易超過分離閾值，即此時(shí) 通常|e(k) I > ε，需要進(jìn)行PD控制。本發(fā)明實(shí)施例利用陀螺偏差值的絕對值和分離閾值之間的邏輯運(yùn)算，可以很方便 地確定預(yù)限削弱積分PID控制的進(jìn)程，實(shí)現(xiàn)轉(zhuǎn)臺(tái)電機(jī)的預(yù)限削弱積分PID控制，彌補(bǔ)模擬 PID調(diào)節(jié)控制的不足。109 =MCU對陀螺偏差值進(jìn)行比例計(jì)算和微分計(jì)算；110 =MCU對陀螺偏差值進(jìn)行比例計(jì)算、微分計(jì)算和積分計(jì)算；其中，通過步驟109中的比例計(jì)算和微分計(jì)算或步驟110中的比例計(jì)算、微分計(jì)算 和積分計(jì)算可以對轉(zhuǎn)臺(tái)電機(jī)的位置、速度進(jìn)行調(diào)節(jié)。111 :MCU根據(jù)步驟109或110的計(jì)算結(jié)果，計(jì)算對陀螺的控制量；112 =MCU根據(jù)控制量確定轉(zhuǎn)臺(tái)電機(jī)的旋轉(zhuǎn)角度，對轉(zhuǎn)臺(tái)電機(jī)進(jìn)行旋轉(zhuǎn)，實(shí)現(xiàn)轉(zhuǎn)臺(tái) 電機(jī)實(shí)時(shí)跟蹤陀螺經(jīng)緯儀，執(zhí)行步驟103。綜上所述，本發(fā)明實(shí)施例提供了一種陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系統(tǒng) 的控制方法，通過本發(fā)明實(shí)施例提供的方法可以實(shí)現(xiàn)陀螺經(jīng)緯儀滿量程360°范圍內(nèi)自動(dòng) 粗尋北，減少了人為因素對陀螺經(jīng)緯儀尋北測量的影響，提高了陀螺經(jīng)緯儀尋北的自動(dòng)化 程度，提高了測量精度。本領(lǐng)域技術(shù)人員可以理解附圖只是一個(gè)優(yōu)選實(shí)施例的示意圖，上述本發(fā)明實(shí)施例 序號(hào)僅僅為了描述，不代表實(shí)施例的優(yōu)劣。以上所述僅為本發(fā)明的較佳實(shí)施例，并不用以限制本發(fā)明，凡在本發(fā)明的精神和 原則之內(nèi)，所作的任何修改、等同替換、改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。
權(quán)利要求
一種陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系統(tǒng)，其特征在于，包括電荷耦合元件、復(fù)雜可編程邏輯器件、陀螺經(jīng)緯儀、微控制單元、鍵盤、顯示模塊、PID控制模塊和轉(zhuǎn)臺(tái)電機(jī)，所述微控制單元通過所述復(fù)雜可編程邏輯器件驅(qū)動(dòng)所述電荷耦合元件工作，反映陀螺房運(yùn)動(dòng)信息的光標(biāo)信息通過陀螺經(jīng)緯儀的光路系統(tǒng)成像，光標(biāo)像由所述電荷耦合元件接收，通過所述電荷耦合元件的光電轉(zhuǎn)換，將光信號(hào)轉(zhuǎn)變?yōu)榉从彻鈴?qiáng)的電壓模擬信號(hào)，所述電壓模擬信號(hào)，經(jīng)模擬/數(shù)字轉(zhuǎn)換芯片轉(zhuǎn)換為數(shù)字信號(hào)，所述電荷耦合元件采集陀螺光標(biāo)位置，將所述陀螺光標(biāo)位置傳送給所述微控制單元，所述微控制單元檢測陀螺光標(biāo)位置值與陀螺零位置值之間的陀螺偏差值，根據(jù)所述陀螺偏差值向所述PID控制模塊發(fā)送控制命令，所述PID控制模塊接收所述微控制單元發(fā)送的控制命令，根據(jù)所述控制命令驅(qū)動(dòng)所述轉(zhuǎn)臺(tái)電機(jī)實(shí)時(shí)跟蹤陀螺經(jīng)緯儀，直到所述微控制單元找到兩個(gè)逆轉(zhuǎn)點(diǎn)，記錄兩個(gè)逆轉(zhuǎn)點(diǎn)并計(jì)算粗北方向值，托起陀螺靈敏部，將陀螺照準(zhǔn)部轉(zhuǎn)向測得的粗北方向；所述鍵盤和所述顯示模塊分別與所述微控制單元相連。
2.根據(jù)權(quán)利要求1所述的陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系統(tǒng)，其特征在于，所 述PID控制模塊具體包括位置控制調(diào)節(jié)單元、速度控制調(diào)節(jié)單元，相應(yīng)地，所述PID控制模 塊接收所述微控制單元發(fā)送的控制命令，根據(jù)所述控制命令驅(qū)動(dòng)所述轉(zhuǎn)臺(tái)電機(jī)實(shí)時(shí)跟蹤陀 螺經(jīng)緯儀具體為接收所述微控制單元發(fā)送的控制命令，通過所述位置控制調(diào)節(jié)單元對所述轉(zhuǎn)臺(tái)電機(jī)的 位置進(jìn)行調(diào)節(jié)，通過所述速度控制調(diào)節(jié)單元對所述轉(zhuǎn)臺(tái)電機(jī)的速度進(jìn)行調(diào)節(jié)，進(jìn)而驅(qū)動(dòng)所 述轉(zhuǎn)臺(tái)電機(jī)實(shí)時(shí)跟蹤陀螺經(jīng)緯儀。
3.一種用于權(quán)利要求1所述的陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系統(tǒng)的控制方 法，其特征在于，所述方法包括以下步驟(1)自動(dòng)跟蹤PID控制系統(tǒng)上電，判斷所述自動(dòng)跟蹤PID控制系統(tǒng)是否輸出跟蹤啟動(dòng)信 號(hào)，如果是，執(zhí)行步驟(2)，如果否，所述自動(dòng)跟蹤PID控制系統(tǒng)重新上電；(2)電荷耦合元件第η次采集陀螺光標(biāo)位置，將第η次陀螺光標(biāo)位置傳送給微控制單 元，其中η為采集的次數(shù)，η為大于1的正整數(shù)；(3)所述微控制單元根據(jù)第η次陀螺光標(biāo)位置值、陀螺零位值，獲取第η次陀螺偏差值；(4)判斷當(dāng)?shù)讦谴瓮勇萜钪档男盘?hào)方向與第η-1次陀螺偏差值的信號(hào)方向相反，且至 少第η+1次陀螺偏差值的信號(hào)方向、第η+2次陀螺偏差值的信號(hào)方向與所述第η次陀螺偏 差值的信號(hào)方向相同，執(zhí)行步驟(5)；否則，執(zhí)行步驟⑶；(5)所述第η次陀螺光標(biāo)位置值所對應(yīng)的陀螺光標(biāo)位置為第一逆轉(zhuǎn)點(diǎn)；(6)重新執(zhí)行步驟(2)至步驟(5)，所述微控制單元判斷是否找到第二逆轉(zhuǎn)點(diǎn)，如果是， 執(zhí)行步驟(7)；否則，執(zhí)行步驟⑶；(7)所述微控制單元根據(jù)所述第一逆轉(zhuǎn)點(diǎn)和所述第二逆轉(zhuǎn)點(diǎn)計(jì)算陀螺北方向值，獲取 陀螺粗北方向，將陀螺轉(zhuǎn)到粗北方向，流程結(jié)束；(8)所述微控制單元判斷步驟(3)中的陀螺偏差值的絕對值是否大于分離閾值，如果 是，執(zhí)行步驟(9)，如果否，執(zhí)行步驟(10)；(9)所述微控制單元對所述陀螺偏差值進(jìn)行比例計(jì)算和微分計(jì)算；(10)所述微控制單元對所述陀螺偏差值進(jìn)行比例計(jì)算、微分計(jì)算和積分計(jì)算；(11)所述微控制單元根據(jù)步驟(9)或步驟(10)的計(jì)算結(jié)果，計(jì)算對陀螺的控制量；(12)所述微控制單元根據(jù)所述控制量確定轉(zhuǎn)臺(tái)電機(jī)旋轉(zhuǎn)的角度，對所述轉(zhuǎn)臺(tái)電機(jī)進(jìn)行 旋轉(zhuǎn)，實(shí)現(xiàn)所述轉(zhuǎn)臺(tái)電機(jī)實(shí)時(shí)跟蹤陀螺經(jīng)緯儀，執(zhí)行步驟(3)。
全文摘要
本發(fā)明公開了一種陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系統(tǒng)及控制方法，屬于陀螺經(jīng)緯儀定位與定向領(lǐng)域，本發(fā)明通過利用無扭矩跟蹤逆轉(zhuǎn)點(diǎn)法進(jìn)行自動(dòng)粗尋北，通過本發(fā)明提供的陀螺經(jīng)緯儀粗尋北的自動(dòng)跟蹤PID控制系統(tǒng)及控制方法，可以實(shí)現(xiàn)陀螺經(jīng)緯儀滿量程360°范圍內(nèi)粗尋北，減少了人為因素對陀螺經(jīng)緯儀尋北測量的影響，提高了陀螺經(jīng)緯儀尋北的自動(dòng)化程度。
文檔編號(hào)G01C19/00GK101980095SQ201010286648
公開日2011年2月23日 申請日期2010年9月19日 優(yōu)先權(quán)日2010年9月19日
發(fā)明者馮莉, 宋樂, 林玉池, 董桂梅, 黃銀國, 齊永岳 申請人:天津大學(xué)