一種基于sopc的空間繩系機(jī)器人控制系統(tǒng)及方法
【專利摘要】本發(fā)明公開了一種基于SOPC的空間繩系機(jī)器人控制系統(tǒng)及方法，包括FPGA模塊，該FPGA模塊包括處理器，以及與處理器相連的PIO外設(shè)和UART內(nèi)核，所述PIO外設(shè)通過通用I/O連接噴氣推進(jìn)裝置；所述UART內(nèi)核通過CAN通信網(wǎng)絡(luò)與測量系統(tǒng)相連以實(shí)時采集空間繩系機(jī)器人的動力學(xué)和運(yùn)動學(xué)信息，所述UART內(nèi)核通過RS232接口與動力學(xué)仿真系統(tǒng)相連以接收理想的動力學(xué)和運(yùn)動學(xué)信息，所述UART內(nèi)核通過外接無線模塊和空間搭載平臺模擬系統(tǒng)進(jìn)行通信以接收空間搭載平臺的指令。本發(fā)明集成度高，體積小，軟件可移植性好，F(xiàn)PGA內(nèi)嵌的Nios?II系統(tǒng)可定制，開發(fā)周期短，易升級。能夠用于空間繩系機(jī)器人的地面實(shí)驗(yàn)。
【專利說明】—種基于SOPC的空間繩系機(jī)器人控制系統(tǒng)及方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及航天【技術(shù)領(lǐng)域】，具體涉及一種基于SOPC的空間繩系機(jī)器人控制系統(tǒng)及方法。
【背景技術(shù)】
[0002]空間繩系機(jī)器人主要用于抓捕和清理失效衛(wèi)星等空間垃圾和進(jìn)行衛(wèi)星維護(hù)等工作，這種“空間搭載平臺+系繩+抓捕機(jī)器人”的繩系結(jié)構(gòu)，相比一般的“平臺+多自由度機(jī)械臂+機(jī)械手”結(jié)構(gòu)，能夠提供更遠(yuǎn)的工作距離和更高的可靠性。但是，由于空間繩系機(jī)器人特殊的繩系結(jié)構(gòu)，其工作時存在著無法避免的系繩干擾；另外，基于空間環(huán)境、成本、實(shí)用性等方面來考慮，空間繩系機(jī)器人在重量、體積和功耗等方面又有著嚴(yán)格的要求和限制。因此，對于空間繩系機(jī)器人的控制系統(tǒng)提出了以下方面的任務(wù)需求:
[0003](I)空間繩系機(jī)器人的姿態(tài)和軌道控制
[0004]因?yàn)榭臻g繩系機(jī)器人存在一個釋放系繩并向操作目標(biāo)機(jī)動的過程，在這個過程中，存在諸多外界干擾；同時，在機(jī)器人進(jìn)行抓捕操作時，其相對操作目標(biāo)的姿態(tài)和軌道必須保持穩(wěn)定。因此，空間繩系機(jī)器人控制系統(tǒng)的核心任務(wù)就是實(shí)現(xiàn)空間繩系機(jī)器人向操作目標(biāo)機(jī)動過程中的軌道控制與姿態(tài)穩(wěn)定，以及執(zhí)行抓捕任務(wù)時相對操作目標(biāo)的軌道和姿態(tài)穩(wěn)定。
[0005](2)空間繩系機(jī)器人內(nèi)部的數(shù)據(jù)流控制
[0006]空間繩系機(jī)器人控制系統(tǒng)接收到視覺測量系統(tǒng)和慣性測量系統(tǒng)提供的測量數(shù)據(jù)后進(jìn)行位置和姿態(tài)確定，經(jīng)過計算后，形成控制信號來控制噴氣推進(jìn)裝置，進(jìn)而來調(diào)整空間繩系機(jī)器人本體的姿態(tài)，或進(jìn)行軌道機(jī)動；另外，空間繩系機(jī)器人控制系統(tǒng)還要與其他系統(tǒng)協(xié)調(diào)工作來控制抓捕執(zhí)行機(jī)構(gòu)。因此，空間繩系機(jī)器人控制系統(tǒng)與測量系統(tǒng)，噴氣推進(jìn)裝置和其他系統(tǒng)之間的數(shù)據(jù)流控制是空間繩系機(jī)器人控制系統(tǒng)的任務(wù)需求之一。
[0007](3)空間繩系機(jī)器人和空間搭載平臺間的數(shù)據(jù)通信
[0008]因?yàn)榭臻g搭載平臺必須獲得空間繩系機(jī)器人的狀態(tài)信息，才能有效地控制系繩的釋放、回收、拖曳、切斷等動作；另外，平臺也需要向空間繩系機(jī)器人發(fā)送一些測量數(shù)據(jù)和控制指令。因此，空間繩系機(jī)器人與空間搭載平臺間的數(shù)據(jù)通信任務(wù)也必須由空間繩系機(jī)器人控制系統(tǒng)來承擔(dān)。
[0009]綜上所述，空間繩系機(jī)器人控制系統(tǒng)承擔(dān)著空間繩系機(jī)器人的軌道和姿態(tài)控制計算、輸出噴氣閥控制信號和與其他系統(tǒng)和空間搭載平臺的通信等任務(wù)，關(guān)系到空間繩系機(jī)器人系統(tǒng)的任務(wù)實(shí)現(xiàn)和可靠性，是空間繩系機(jī)器人研究中的關(guān)鍵技術(shù)之一。

【發(fā)明內(nèi)容】

[0010]為了完成空間繩系機(jī)器人姿態(tài)軌道控制算法的地面驗(yàn)證和空間繩系機(jī)器人相關(guān)的地面實(shí)驗(yàn)，本發(fā)明提供了一種基于SOPC的空間繩系機(jī)器人控制系統(tǒng)及方法。
[0011]本發(fā)明所采用的技術(shù)方案是:[0012]一種基于SOPC的空間繩系機(jī)器人控制系統(tǒng)，其特征在于:包括FPGA模塊，該FPGA模塊包括Nios II處理器，以及與Nios II處理器相連的PIO外設(shè)和UART內(nèi)核，所述PIO外設(shè)通過通用I/O連接噴氣推進(jìn)裝置；所述UART內(nèi)核通過CAN通信網(wǎng)絡(luò)與測量系統(tǒng)相連以實(shí)時采集空間繩系機(jī)器人的動力學(xué)和運(yùn)動學(xué)信息，所述UART內(nèi)核通過RS232接口與動力學(xué)仿真系統(tǒng)相連以接收理想的動力學(xué)和運(yùn)動學(xué)信息，所述UART內(nèi)核通過外接無線模塊和空間搭載平臺模擬系統(tǒng)進(jìn)行通信以接收空間搭載平臺的指令。
[0013]所述FPGA進(jìn)一步包括有Avalon總線，所述處理器通過指令總線和數(shù)據(jù)總線與Avalon總線連接,所述PIO外設(shè)及UART內(nèi)核分別通過定義的Avalon接口連接到Avalon總線上。
[0014]所述控制系統(tǒng)進(jìn)一步包括有串行配置存儲器EPCS16模塊和SDRAM模塊，所述FPGA進(jìn)一步包括有SDRAM控制器和EPCS控制器，所述SDRAM控制器和EPCS控制器分別通過定義的Avalon接口連接到Avalon總線；所述FPGA內(nèi)的SDRAM控制器通過地址、數(shù)據(jù)和控制信號連接到SDRAM模塊；所述FPGA內(nèi)的EPCS控制器通過FPGA芯片上指定的引腳連接到串行配置存儲器EPCS16。
[0015]所述FPGA進(jìn)一步包括有系統(tǒng)ID和JTAG UART, JTAG UART使用FPGA內(nèi)嵌的JTAG電路，上位機(jī)通過JTAG專用弓丨腳訪問JTAG電路，通過JTAG與FPGA實(shí)現(xiàn)連接；系統(tǒng)ID通過Avalon總線與處理器連接。一種基于以上控制系統(tǒng)的控制方法，F(xiàn)PGA的處理器接收到空間搭載平臺的釋放指令后，將接收的實(shí)時動力學(xué)和運(yùn)動學(xué)信息和理想的動力學(xué)和運(yùn)動學(xué)信息比較后，經(jīng)計算形成噴氣占空比數(shù)據(jù)，通過PIO外設(shè)傳送給噴氣推進(jìn)裝置，以調(diào)整姿態(tài)。
[0016]所述控制方法包括逼近段、調(diào)準(zhǔn)段和捕獲段，當(dāng)空間搭載平臺模擬系統(tǒng)啟動釋放命令后，測試系統(tǒng)即進(jìn)入逼近段仿真，動力學(xué)仿真系統(tǒng)進(jìn)入逼近動力學(xué)計算，根據(jù)權(quán)利要求5的方法調(diào)整位姿，然后進(jìn)入到調(diào)準(zhǔn)段，采用權(quán)利要求5的方法進(jìn)行位姿調(diào)準(zhǔn)，結(jié)束后進(jìn)入到捕獲段，再根據(jù)權(quán)利要求5的方法進(jìn)行更精確的調(diào)整，最后執(zhí)行捕獲操作。
[0017]與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果是:
[0018](I)本發(fā)明基于SOPC技術(shù)，選用Altera公司Cyclone II系列的FPGA作為主處理器，它提供了豐富的內(nèi)部資源，具有很強(qiáng)的性能和極低的功耗，能夠達(dá)到系統(tǒng)功能及性能的要求，與ARM和DSP相比其開放性高，軟件可移植性好。
[0019](2) FPGA內(nèi)嵌的Nios II系統(tǒng)可根據(jù)用戶需求自定制，包含多種方便實(shí)用的周邊設(shè)備，集成度高，開發(fā)周期短，升級容易。
[0020](3)選擇國際上應(yīng)用最廣泛的現(xiàn)場總線之一，CAN作為空間繩系機(jī)器人控制系統(tǒng)與測量系統(tǒng)及其他系統(tǒng)之間的通信網(wǎng)絡(luò)，組網(wǎng)靈活，實(shí)時性好，軟件開銷小，可靠性高。
【專利附圖】

【附圖說明】
[0021]圖1為空間繩系機(jī)器人控制系統(tǒng)任務(wù)需求示意圖。
[0022]圖2為空間繩系機(jī)器人控制系統(tǒng)整體結(jié)構(gòu)框圖。
[0023]圖3為空間繩系機(jī)器人控制系統(tǒng)地面測試試驗(yàn)系統(tǒng)框圖。
[0024]圖4為空間繩系機(jī)器人逼近段、調(diào)準(zhǔn)段仿真時序。
【具體實(shí)施方式】[0025]下面結(jié)合附圖對本發(fā)明作進(jìn)一步說明。
[0026]如圖1所示，空間繩系機(jī)器人控制系統(tǒng)承擔(dān)著空間繩系機(jī)器人的軌道和姿態(tài)控制計算、輸出噴氣推進(jìn)執(zhí)行機(jī)構(gòu)控制信號和與其他系統(tǒng)和空間搭載平臺間的通信等任務(wù)，關(guān)系到空間繩系機(jī)器人系統(tǒng)的任務(wù)實(shí)現(xiàn)和可靠性，【具體實(shí)施方式】如下:
[0027]如圖2所示，本發(fā)明控制系統(tǒng)基于SOPC技術(shù)，包括FPGA模塊，SDRAM存儲器模塊，串行配置存儲器EPCS16模塊，RS232模塊，CAN網(wǎng)絡(luò)模塊和無線模塊等。
[0028]FPGA選用Altera公司推出的Cyclone II系列中的一種，型號為EP2C8Q208C8，內(nèi)部自定制的Nios II軟核包括:Nios II軟核處理器I個、Avalon總線I個、JTAG UARTl個、系統(tǒng)IDl個、SDRAM控制器I個、EPCS控制器I個、PIO外設(shè)為12位寬、UART內(nèi)核4個。NiosII軟核處理器選用全功能型，通過指令總線和數(shù)據(jù)總線與Avalon總線連接JTAG UART使用FPGA內(nèi)嵌的JTAG電路，上位機(jī)可以通過FPGA上的JTAG專用引腳來訪問JTAG電路，通過JTAG與FPGA實(shí)現(xiàn)連接，可進(jìn)行FPGA配置程序下載、應(yīng)用程序下載和在線調(diào)試；系統(tǒng)ID通過Avalon總線與Nios II軟核處理器相連接，它為每個Nios II處理器提供一個唯一的身份標(biāo)識符，可防止程序下載錯誤；SDRAM控制器、EPCS控制器、PIO外設(shè)、UART內(nèi)核分別通過定義的Avalon接口連接到Avalon總線上,進(jìn)而與Nios II軟核處理器進(jìn)行信息交互；P10外設(shè)通過12位寬的通用I/O 口與噴氣閥控制信號連接，進(jìn)而輸出噴氣占空比來控制噴氣推進(jìn)執(zhí)行機(jī)構(gòu)，進(jìn)而調(diào)整軌道或姿態(tài)；UART內(nèi)核通過配置好的通用I/O 口與單片機(jī)串口連接，并外加CAN收發(fā)器即構(gòu)成CAN網(wǎng)絡(luò)模塊，在測量系統(tǒng)作為CAN節(jié)點(diǎn)接入CAN網(wǎng)絡(luò)后，即可進(jìn)行通信，這里單片機(jī)選用Silicon Laboratory公司集成CAN控制器的C8051F502芯片，CAN收發(fā)器型號為CTM1050T ；UART內(nèi)核通過配置好的通用I/O 口經(jīng)電平轉(zhuǎn)換后構(gòu)成RS232模塊，不經(jīng)過電平轉(zhuǎn)換而直接外接串口轉(zhuǎn)WIFI模塊后構(gòu)成無線模塊，進(jìn)而和空間搭載平臺進(jìn)行通信。
[0029]FPGA內(nèi)的SDRAM控制器通過地址、數(shù)據(jù)和控制信號連接到SDRAM存儲芯片HY57V641620HG，完成SDRAM存儲芯片的所有邏輯，使得設(shè)計者可以方便地將片外的SDRAM芯片連接到自定制的Nios II系統(tǒng)中來，SDRAM存儲芯片主要用于存儲應(yīng)用程序和所需的變量。
[0030]FPGA內(nèi)的EPCS控制器通過FPGA芯片上指定的引腳連接到串行配置存儲器EPCS16，EPCS16芯片主要用于存儲FPGA的配置信息。
[0031]本發(fā)明控制系統(tǒng)采用5V直流電源供電，因?yàn)镕PGA芯片需要提供3.3V，1.2V兩種不同的電壓，用來作為CAN控制器的單片機(jī)還需要提供2.5V電壓，所以需要將5V電壓轉(zhuǎn)換為這三種不同的電壓。選用的將5V轉(zhuǎn)換為3.3V的芯片為Texas Instruments公司的TPS54316，將3.3V轉(zhuǎn)換為1.2V的芯片為SIPEX公司的SPX3819M5-L-1-2，將5V轉(zhuǎn)換為2.5V的芯片為Advanced Monolithic Systems公司的AMS1117系列芯片。
[0032]下面通過一個地面測試試驗(yàn)來具體介紹本發(fā)明的工作過程。
[0033]測試系統(tǒng)框圖如圖3所示。測試系統(tǒng)中的空間繩系機(jī)器人由空間繩系機(jī)器人控制系統(tǒng)、視覺測量系統(tǒng)、慣性測量系統(tǒng)、噴氣推進(jìn)執(zhí)行機(jī)構(gòu)和無線模塊組成。視覺測量系統(tǒng)和慣性測量系統(tǒng)提供實(shí)時相對動力學(xué)和運(yùn)動學(xué)信息，通過CAN網(wǎng)絡(luò)傳輸?shù)絅1s II處理器中。動力學(xué)仿真系統(tǒng)、空間搭載平臺模擬系統(tǒng)和人機(jī)交互系統(tǒng)是三臺PC機(jī)，它們之間通過以太網(wǎng)連接，進(jìn)行通信。動力學(xué)仿真系統(tǒng)仿真空間繩系機(jī)器人和目標(biāo)的動力學(xué)和運(yùn)動學(xué)，實(shí)時生成理想相對動力學(xué)和運(yùn)動學(xué)信息，通過RS232接口傳輸?shù)絅ios II處理器中；空間搭載平臺模擬系統(tǒng)模擬空間機(jī)動平臺，通過無線模塊接收空間繩系機(jī)器人的狀態(tài)信息并發(fā)出指令；人機(jī)交互系統(tǒng)作為主控系統(tǒng)參與整個任務(wù)流程，顯示各個任務(wù)段的相關(guān)信息。
[0034]測試試驗(yàn)中設(shè)計的空間繩系機(jī)器人的工作過程為:空間繩系機(jī)器人接收到空間搭載平臺的釋放指令后，在Nios II處理器中將實(shí)時動力學(xué)和運(yùn)動學(xué)信息和理想動力學(xué)和運(yùn)動學(xué)信息進(jìn)行比較，并根據(jù)控制算法進(jìn)行計算，形成噴氣占空比數(shù)據(jù)，傳送到Avalon總線上，并經(jīng)12位寬的PIO接口傳送給噴氣推進(jìn)執(zhí)行機(jī)構(gòu)，噴氣推進(jìn)執(zhí)行機(jī)構(gòu)隨即開始工作，粗略調(diào)整姿態(tài)，使繩系機(jī)器人逼近目標(biāo)；當(dāng)距離目標(biāo)較近時，采用精度更好的控制算法，計算得到噴氣占空比，進(jìn)一步調(diào)整位姿，對準(zhǔn)目標(biāo)；當(dāng)進(jìn)入捕獲范圍時，進(jìn)行更精確的位姿調(diào)整，然后進(jìn)入捕獲準(zhǔn)備狀態(tài)。根據(jù)空間繩系機(jī)器人的工作過程，可以將測試試驗(yàn)相應(yīng)地分為以下三個階段:逼近段，調(diào)準(zhǔn)段和捕獲段。
[0035]當(dāng)空間搭載平臺模擬系統(tǒng)啟動釋放命令后，測試系統(tǒng)即進(jìn)入逼近段仿真。動力學(xué)仿真系統(tǒng)開始進(jìn)行逼近動力學(xué)計算，主定時器內(nèi)逼近段的計算時序如圖4所示。計算得到位姿數(shù)據(jù)后，通過以太網(wǎng)將噴氣占空比、位姿數(shù)據(jù)等發(fā)送給人機(jī)交互系統(tǒng)，滿足階段完成條件后，本階段仿真結(jié)束，開始進(jìn)入調(diào)準(zhǔn)段仿真。
[0036]在調(diào)準(zhǔn)段中，動力學(xué)仿真系統(tǒng)開始進(jìn)行調(diào)準(zhǔn)動力學(xué)計算，計算時序和逼近段相同，只是姿態(tài)控制算法不同。計算得到位姿數(shù)據(jù)后，將噴氣占空比、位姿數(shù)據(jù)等發(fā)送給人機(jī)交互系統(tǒng)，滿足階段完成條件后，本階段仿真結(jié)束，開始進(jìn)入捕獲段仿真。
[0037]捕獲段中，動力學(xué)仿真系統(tǒng)繼續(xù)進(jìn)行動力學(xué)計算，對空間繩系機(jī)器人的位置和姿態(tài)進(jìn)行更精確的調(diào)整，當(dāng)滿足階段完成條件后，本階段仿真結(jié)束，下面即可執(zhí)行捕獲操作。
[0038]測試試驗(yàn)結(jié)果表明:
[0039](I)空間繩系機(jī)器人控制系統(tǒng)能夠滿足位置姿態(tài)控制算法的性能需求；
[0040](2)空間繩系機(jī)器人控制系統(tǒng)與噴氣推進(jìn)執(zhí)行機(jī)構(gòu)間的I/O接口輸出正常，能夠準(zhǔn)確地完成對噴氣閥的控制；
[0041](3)空間繩系機(jī)器人控制系統(tǒng)與空間搭載平臺間的數(shù)據(jù)通信正常。
[0042]所以，本發(fā)明能夠滿足空間繩系機(jī)器人的任務(wù)需求。
【權(quán)利要求】
1.一種基于SOPC的空間繩系機(jī)器人控制系統(tǒng)，其特征在于:包括FPGA模塊，該FPGA模塊包括Nios II處理器，以及與Nios II處理器相連的PIO外設(shè)和UART內(nèi)核，所述PIO外設(shè)通過通用I/O連接噴氣推進(jìn)裝置；所述UART內(nèi)核通過CAN通信網(wǎng)絡(luò)與測量系統(tǒng)相連以實(shí)時采集空間繩系機(jī)器人的動力學(xué)和運(yùn)動學(xué)信息，所述UART內(nèi)核通過RS232接口與動力學(xué)仿真系統(tǒng)相連以接收理想的動力學(xué)和運(yùn)動學(xué)信息，所述UART內(nèi)核通過外接無線模塊和空間搭載平臺模擬系統(tǒng)進(jìn)行通信以接收空間搭載平臺的指令。
2.根據(jù)權(quán)利要求1所述的一種基于SOPC的空間繩系機(jī)器人控制系統(tǒng)，其特征在于:所述FPGA進(jìn)一步包括有Avalon總線,所述處理器通過指令總線和數(shù)據(jù)總線與Avalon總線連接，所述PIO外設(shè)及UART內(nèi)核分別通過定義的Avalon接口連接到Avalon總線上。
3.根據(jù)權(quán)利要求1所述的一種基于SOPC的空間繩系機(jī)器人控制系統(tǒng)，其特征在于:所述控制系統(tǒng)進(jìn)一步包括有串行配置存儲器EPCS16模塊和SDRAM模塊，所述FPGA進(jìn)一步包括有SDRAM控制器和EPCS控制器，所述SDRAM控制器和EPCS控制器分別通過定義的Avalon接口連接到Avalon總線；所述FPGA內(nèi)的SDRAM控制器通過地址、數(shù)據(jù)和控制信號連接到SDRAM模塊；所述FPGA內(nèi)的EPCS控制器通過FPGA芯片上指定的引腳連接到串行配置存儲器EPCS16。
4.根據(jù)權(quán)利要求1所述的一種基于SOPC的空間繩系機(jī)器人控制系統(tǒng)，其特征在于:所述FPGA進(jìn)一步包括有系統(tǒng)ID和JTAG UART, JTAG UART使用FPGA內(nèi)嵌的JTAG電路，上位機(jī)通過JTAG專用弓I腳訪問JTAG電路，通過JTAG與FPGA實(shí)現(xiàn)連接；系統(tǒng)ID通過Avalon總線與處理器連接。
5.根據(jù)權(quán)利要求1所述的一種基于SOPC的空間繩系機(jī)器人控制系統(tǒng)的控制方法，其特征在于:FPGA的處理器接收到空間搭載平臺的釋放指令后，將接收的實(shí)時動力學(xué)和運(yùn)動學(xué)信息和理想的動力學(xué)和運(yùn)動學(xué)信息比較后，經(jīng)計算形成噴氣占空比數(shù)據(jù)，通過PIO外設(shè)傳送給噴氣推進(jìn)裝置，以調(diào)整姿態(tài)。
6.根據(jù)權(quán)利要求5所述的控制方法，其特征在于:所述控制方法包括逼近段、調(diào)準(zhǔn)段和捕獲段，當(dāng)空間搭載平臺模擬系統(tǒng)啟動釋放命令后，測試系統(tǒng)即進(jìn)入逼近段仿真，動力學(xué)仿真系統(tǒng)進(jìn)入逼近動力學(xué)計算，根據(jù)權(quán)利要求5的方法調(diào)整位姿，然后進(jìn)入到調(diào)準(zhǔn)段，采用權(quán)利要求5的方法進(jìn)行位姿調(diào)準(zhǔn)，結(jié)束后進(jìn)入到捕獲段，再根據(jù)權(quán)利要求5的方法進(jìn)行更精確的調(diào)整，最后執(zhí)行捕獲操作。
【文檔編號】G05D1/08GK103984349SQ201410129411
【公開日】2014年8月13日 申請日期:2014年4月1日 優(yōu)先權(quán)日:2014年4月1日
【發(fā)明者】黃攀峰, 劉彬彬, 馬駿, 孟中杰 申請人:西北工業(yè)大學(xué)