專利名稱：空間機器人視覺測量、路徑規(guī)劃、gnc一體化仿真系統(tǒng)的制作方法
技術領域：
本發(fā)明涉及一種空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，可用于對空間機器人跟蹤、接近、捕獲非合作目標過程中的所有關鍵算法進行閉環(huán)仿真和驗證。

背景技術：
隨著技術的進步，人類的活動在不斷地向太空擴展。據(jù)統(tǒng)計，全球平均每年發(fā)射80-130顆衛(wèi)星，然而有2-3顆衛(wèi)星未能正確入軌，而正確入軌的衛(wèi)星中，又有5-10顆在壽命初期(入軌后前30天)即失效，其中，機械故障導致的衛(wèi)星失效占了相當大的比例。典型的例子如我國的鑫諾二號衛(wèi)星，于2006年10月29日發(fā)射升空后，衛(wèi)星雖然成功定軌，且衛(wèi)星的測控、姿軌控等分系統(tǒng)均處于良好的狀態(tài)，然而，由于太陽帆板二次展開和天線展開未能完成，衛(wèi)星無法正常工作，雖然科研人員采用多種方法進行營救，仍然未能恢復其功能，這顆耗資20億人民幣、設計壽命為15年的衛(wèi)星隨即成了一顆廢星。對于那些完全失效或由于任務結束被放棄的衛(wèi)星，停留在太空將成為太空垃圾，不但占用了寶貴的軌道資源，還可能危機其它航天器的安全。為了盡可能挽回損失或凈化軌道環(huán)境，各國正在研究以空間機器人為手段，以衛(wèi)星維修、生命延長及太空垃圾清除為目的的在軌服務技術。
為確保在軌任務的成功執(zhí)行，需要在地面進行充分的仿真、實驗以對空間機器人跟蹤、接近、捕獲目標衛(wèi)星過程中的關鍵算法——視覺測量、路徑規(guī)劃、協(xié)調控制等進行驗證和評估。對于空間機器人這樣的系統(tǒng)，要同時開展微重力環(huán)境下兩星交會、對接、捕獲等過程的實驗，目前還沒有很好的解決方法(徐文福，梁斌，李成，等.空間機器人微重力模擬實驗系統(tǒng)研究綜述.機器人，2009，31(1)88-96)。如常用的氣浮式試驗系統(tǒng)，僅能開展平面3個自由度——兩個平動、一個轉動的運動試驗；自由落體或失重飛行試驗系統(tǒng)的試驗空間、試驗時間非常有限，吊絲配重式試驗系統(tǒng)的重力補償精度低，且無法實現(xiàn)基座的自由漂??；水浮式地面試驗系統(tǒng)可以進行三維空間的微重力實驗，實驗時間也不受限制，但空間機器人原型樣機不能直接在水浮系統(tǒng)上進行測試，必須進行專門的設計以免受到水下環(huán)境的影響，而且，水的阻力及慣量會改變空間機器人系統(tǒng)的動力學特性。另外，硬件在環(huán)內的半物理仿真實驗系統(tǒng)，采用數(shù)學模型與實物相結合，通過動力學模型計算真實系統(tǒng)在三維空間的運動，然后通過實物來模擬其運動。由于運動范圍有限、所能采用的實物有限，半物理仿真實驗系統(tǒng)僅能進行部分任務的仿真實驗。如文獻(Wenfu Xu，Bin Liang，Yangsheng Xu，Cheng Li，Wenyi Qiang.A Ground Experiment System of Free-floating Space RobotFor Capturing Space Target[J].Journal of Intelligent and Robotic Systems.2007，48(2)187-208)所建的地面實驗系統(tǒng)，僅能進行目標捕獲階段的仿真驗證，且未能對視覺測量的空間環(huán)境及光照條件進行模擬。數(shù)學仿真具有實現(xiàn)簡單、投資少、靈活性高的優(yōu)點，尤其在系統(tǒng)設計、任務分析、算法的理論驗證等階段，顯得十分重要。然而，以往的仿真系統(tǒng)，往往僅考慮某一方面的仿真驗證，而其他相關部分往往做簡化處理，目前為止，還沒有查閱到能同時開展圖像處理與位姿測量、航天器GNC算法、空間機器人路徑規(guī)劃以及控制的動力學閉環(huán)仿真驗證。因此，開發(fā)一套空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)是非常必要和迫切的。


發(fā)明內容
本發(fā)明的目的在于克服現(xiàn)有技術的不足，提供了一種空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)。該系統(tǒng)不需要采用真實的測量設備、空間機器人系統(tǒng)及目標星，在一臺獨立的PC機上即可進行空間機器人跟蹤、接近、捕獲非合作目標過程中的所有關鍵算法的閉環(huán)仿真和驗證。
本發(fā)明的空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，包括圖像處理與位姿測量模塊(1)、空間機器人規(guī)劃與控制模塊(2)、目標星控制器模塊(3)、系統(tǒng)動力學模塊(4)、系統(tǒng)3D幾何模型模塊(7)、雙目相機模擬成像模塊(6)及立體標定模塊(5)。其中 圖像處理與位姿測量模塊(1)，接收由雙目相機模擬成像模塊(6)輸出的左、右相機圖像Img_L[W][H]、Img_R[W][H]，并對圖像進行實時處理，最后計算出非合作目標星相對于空間機器人基座和末端的相對位置和姿態(tài)

該測量結果作為空間機器人規(guī)劃與控制模塊(2)的輸入； 空間機器人規(guī)劃與控制模塊(2)，根據(jù)從圖像處理與位姿測量模塊(1)輸入的相對位姿測量結果

以及系統(tǒng)動力學模塊(4)輸入的系統(tǒng)當前狀態(tài)——基座質心姿態(tài)Ψc、角速度ωc、機械臂關節(jié)角Θ、角速度

自主規(guī)劃跟蹤接近過程中基座進行姿態(tài)、軌道控制的期望位姿軌跡，以及利用機械臂抓捕目標過程中機械臂關節(jié)角的運動軌跡，并調用相應的控制算法跟蹤期望的軌跡，輸出為系統(tǒng)質心控制力、基座姿態(tài)控制力矩以及機械臂各關節(jié)的控制力矩，該輸出是系統(tǒng)動力學模塊(4)的一部分輸入； 目標星控制器模塊(3)，建立目標衛(wèi)星的控制器和執(zhí)行機構模型，對目標進行位置、姿態(tài)的6DOF控制。該模塊根據(jù)設定的目標運動模式，規(guī)劃期望的位置、姿態(tài)軌跡，然后由該模塊中的控制器、執(zhí)行機構產生目標的控制力、力矩，實現(xiàn)目標按給定的規(guī)律運動，如三軸穩(wěn)定、自旋穩(wěn)定或翻滾衛(wèi)星。該模塊的輸出為目標星動力學的作用力和力矩，是系統(tǒng)動力學模塊(4)的另一部分輸入； 系統(tǒng)動力學模塊(4)，將兩星的相對軌道動力學、空間機器人多剛體動力學以及目標星的姿態(tài)動力學集成在同一環(huán)境中，用于模擬整個空間機器人系統(tǒng)——包括基座及機械臂，及目標星的軌道、姿態(tài)、及多剛體運動狀況。該模塊的輸入是空間機器人規(guī)劃與控制模塊(2)、目標星控制器模塊(3)輸出的各自由度(系統(tǒng)狀態(tài))的驅動力/力矩，輸出為空間機器人系統(tǒng)質心位置rc、線速度vc、姿態(tài)Ψc、角速度ωc、機械臂關節(jié)角Θ、角速度

以及目標星的質心位置rt、線速度vt、姿態(tài)Ψt、角速度ωt，作為系統(tǒng)幾何模型模塊(7)以及空間機器人規(guī)劃與控制模塊(2)的輸入。
系統(tǒng)3D幾何模型模塊(7)，接收來源于系統(tǒng)動力學模塊(4)輸出的空間機器人系統(tǒng)質心位置rc、姿態(tài)Ψc、機械臂關節(jié)角Θ，以及目標星的質心位置rt、姿態(tài)Ψt，建立空間機器人系統(tǒng)、目標星的3D幾何模型，以及航天器所在軌道環(huán)境的地球及星空背景模型，各模型由相應的狀態(tài)變量實時驅動，顯示各部分的3D運動情況；同時，該模塊輸出地球參考系、空間機器人系統(tǒng)本體坐標系以及目標衛(wèi)星本體坐標系相對于左、右相機坐標系的位置、姿態(tài)，作為雙目相機模擬成像模塊(6)的輸入； 雙目相機模擬成像模塊(6)，接收系統(tǒng)3D幾何模型模塊(7)輸出的輸出地球參考系、空間機器人系統(tǒng)本體坐標系以及目標衛(wèi)星本體坐標系相對于左、右相機坐標系的位置、姿態(tài)，根據(jù)實際相機的光學參數(shù)——分辨率、視場角、畸變等，生成立體視覺系統(tǒng)的左、右相機各自所成的圖像，并模擬圖像采集過程，將疊加噪聲后的圖像信息存儲在左、右相機圖像矩陣Img_L[W][H]、Img_R[W][H]，其中W×H代表相機CCD的分辨率，典型的為512×512。該模塊的輸出即為Img_L[W][H]、Img_R[W][H]，作為圖像處理與位姿測量模塊(1)的輸入。
立體標定模塊(5)，包括左右相機標定圖像生成、立體標定算法等子模塊。標定圖像可采用棋盤格或圓形光標，通過設定不同的位置、指向，提取21幅圖，然后采用最小二乘法標定相機的參數(shù)。標定數(shù)據(jù)用于非合作目標識別算法中。
所述的圖像處理與位姿測量模塊(1)包括圖像處理、3D重構及位姿測量三個子模塊。圖像處理子模塊接收自雙目相機模擬成像模塊(6)輸入的左、右相機圖像，并采用合適的圖像處理算法分別對左、右相機圖像進行處理，提取出目標特征點在左、右相機圖像中的2D圖像信息；3D重構子模塊利用已由立體標定模塊(5)標定好的相機投影模型——左、右相機投影矩陣分別為CL和CR，對由圖像處理子模塊輸出的特征點的2D圖像信息進行立體匹配，得到特征點在世界坐標系中的3D坐標；位姿測量子模塊根據(jù)特征點的3D坐標構造目標參考坐標系，并計算該參考坐標系相對于世界坐標系的位置

和姿態(tài)
所述的圖像處理算法包括圖像濾波、邊沿檢測、直線提取。平滑濾波采用中值濾波算法，邊沿檢測采用Canny算法，直線提取采用Hough變換算法，中值濾波對于給定的n個數(shù)值{a1，a2，…，an}，將它們按大小順序排列。當n為奇數(shù)時，位于中間位置的那個數(shù)值稱為這n個數(shù)值的中值。當n為偶數(shù)時，位于中間位置的兩個數(shù)值的平均值稱為這n個數(shù)值的中值，記作med{a1，a2，…，an}，圖像經過中值濾波后，某像素的輸出等于該像素領域中的各像素灰度的中值；Canny邊緣檢測算法用高斯濾波器平滑圖像、用一階偏導的有限差分來計算梯度的幅值和方向、對梯度幅值應用非極大值抑制、用雙閾值算法檢測和連接邊緣；Hough變換算法用點-線(點-直線或點-曲線)的對偶性，將圖像空間x-y中的直線，變換成為參數(shù)空間的一點； 所述的空間機器人規(guī)劃與控制模塊(2)包括基座GNC、基座執(zhí)行機構、空間機器人路徑規(guī)劃器、關節(jié)控制器四個子模塊?；鵊NC根據(jù)當前系統(tǒng)狀態(tài)——基座姿態(tài)Ψc、角速度ωc、機械臂關節(jié)角Θ、角速度

以及目標衛(wèi)星的相對位姿測量結果

執(zhí)行空間機器人跟蹤、接近、?？磕繕诵l(wèi)星過程中的GNC算法，產生各執(zhí)行機構的控制指令——反作用飛輪的控制電壓、推力器的推力脈沖、磁力矩器的控制電壓；基座執(zhí)行機構包括反作用飛輪組件、推力器組件以及磁力矩器模型，將基座GNC子模塊的控制指令，轉換為基座質心作用力fb、力矩τb；空間機器人路徑規(guī)劃器根據(jù)當前系統(tǒng)狀態(tài)和目標衛(wèi)星的相對位姿測量結果，規(guī)劃空間機器人捕獲、維修目標衛(wèi)星過程中機械臂各關節(jié)角的軌跡——期望關節(jié)角Θd、角速度

關節(jié)控制器子模塊包括六個關節(jié)的控制器，各控制器接收路徑規(guī)劃器規(guī)劃的期望關節(jié)角Θd和角速度

采用PID控制律，生成機械臂各關節(jié)的驅動力矩τb； 所述的GNC算法包括相對導航、制導律、姿態(tài)與軌道控制等部分。相對導航分別根據(jù)相對位置、姿態(tài)測量值，建立線性平移狀態(tài)觀測器、相對姿態(tài)狀態(tài)觀測器，將得到的平移位置矢量估計值、平移速度矢量估計值，作為平移控制的測量輸入，而將得到的相對姿態(tài)估計值和相對角速度估計值，作為相對姿態(tài)控制的測量輸入。制導律包括VBar直線接近制導律和位置保持制導律，在直線制導律下，空間機器人沿目標衛(wèi)星軌道坐標系的-VBar方向進行直線接近，其余兩個方向參考軌跡為零；相對位置保持制導律用于對停靠點的保持，使空間機器人和目標衛(wèi)星維持在一個穩(wěn)定的相對位置，在位置保持制導律下，設計的相對速度為零，標稱相對位置為某一固定值。姿態(tài)與軌道控制包括基于飛輪的姿態(tài)控制、基于噴氣的姿態(tài)控制，以及姿態(tài)與軌道六自由度控制，其中六自由度的控制通過姿態(tài)確定和相對導航的數(shù)據(jù)加上補償信息，由控制器算法得出作用在星體上的力矩和外力信息，通過推力器的分配得出各個推進閥的工作狀態(tài)。
所述的空間機器人路徑規(guī)劃器實現(xiàn)了一種空間機器人捕獲非合作目標的自主路徑規(guī)劃方法，該方法利用手眼相機的相對位姿測量值，實時規(guī)劃空間機器人的運動，以最終捕獲目標。主要包括如下步驟位姿偏差計算、目標運動的預測、空間機器人末端運動速度規(guī)劃、空間機器人避奇異的路徑規(guī)劃、基座運動的預測等。首先，根據(jù)手眼測量數(shù)據(jù)判斷相對位姿偏差ep和eo是否小于設定的閾值εp和εo，即捕獲區(qū)域，三軸相對位置在10mm以內，三軸姿態(tài)偏差在1°左右。若小于，則閉合手爪、捕獲目標；反之，則根據(jù)相對位姿偏差，實時估計目標的運動狀態(tài)，并將估計的結果反應到機械臂末端速度的規(guī)劃中，以保證機械臂末端時刻朝最近的方向(直線)趨近目標，機械臂末端能自主跟蹤目標的運動，直到最后捕獲目標。規(guī)劃出末端運動速度后，即調用自主奇異回避算法，以解算關節(jié)的期望角速度，并據(jù)此預測機械臂運動對基座的擾動，當擾動超出容許的范圍時，則自動調整機械臂的關節(jié)運動速度，以保證期望的偏轉在許可的范圍內。整個過程一直持續(xù)到機械臂捕獲到目標為止。
所述的空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，其特征在于所述的目標星控制器模塊(3)設定的目標衛(wèi)星姿態(tài)、軌道機動目標，采用6DOF控制規(guī)律，產生目標衛(wèi)星質心位置控制力ft、姿態(tài)控制力矩τt； 所述的系統(tǒng)動力學模塊(4)包括空間機器人系統(tǒng)的軌道動力學模型、多剛體動力學模型、目標衛(wèi)星的姿態(tài)及軌道動力學模型?？臻g機器人系統(tǒng)的軌道動力學模型接收基座執(zhí)行機構輸出的噴氣作用力，計算系統(tǒng)質心的位置、線速度；空間機器人系統(tǒng)多剛體動力學接收基座執(zhí)行機構輸出的姿態(tài)控制力矩、機械臂關節(jié)控制器輸出的關節(jié)驅動力矩，計算由基座和機械臂組成的多剛體系統(tǒng)的狀態(tài)變化——姿態(tài)Ψc、角速度ωc、機械臂關節(jié)角Θ、角速度

目標衛(wèi)星姿態(tài)、軌道動力學接收由目標星控制器模塊(3)輸出的作用力、作用力矩，計算目標衛(wèi)星質心位置rt、線速度vt、姿態(tài)Ψt、角速度ωt； 所述的系統(tǒng)幾何模型模塊(7)包括軌道環(huán)境、空間機器人3D模型、目標星3D模型三個子模塊，各子模塊采用OpenGL進行建模。軌道環(huán)境模型子模塊根據(jù)空間機器人及目標衛(wèi)星所處的軌道位置，繪制地球背景、星空背景；空間機器人3D模型子模塊根據(jù)基座質心位置rc、姿態(tài)Ψc、機械臂關節(jié)角Θ繪制空間機器人系統(tǒng)的3D幾何模型；目標星3D模型子模塊根據(jù)目標衛(wèi)星質心位置rt、姿態(tài)Ψt，繪制目標衛(wèi)星的3D幾何模型。該模塊還輸出地球參考系、空間機器人系統(tǒng)本體坐標系以及目標衛(wèi)星本體坐標系相對于左、右相機坐標系的位置、姿態(tài)； 所述的雙目相機模擬成像模塊(6)由左相機圖像生成、左相機圖像采集、右相機圖像生成、右相機圖像采集等子模塊組成。相機圖像生成子模塊接收3D幾何模型模塊(7)輸出的地球參考系、空間機器人系統(tǒng)本體坐標系以及目標衛(wèi)星本體坐標系相對于左、右相機坐標系的位置、姿態(tài)，根據(jù)相機的分辨率、視場角、畸變系數(shù)等，采用透視投影模型，生成相應相機所觀察到的圖像；圖像采集子模塊模擬圖像采集過程中的噪聲傳遞過程，將噪聲數(shù)據(jù)疊加于所生成的圖像，圖像數(shù)據(jù)存儲在矩陣Img_L及Img_R中，作為圖像處理與位姿測量模塊(1)的輸入； 所述的空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，其特征在于所述的圖像噪聲包括熱噪聲、電子噪聲、放大器噪聲以及量化噪聲，采用加性高斯白噪聲進行模擬。
所述的空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，其特征在于所述的立體標定模塊(5)包括左相機標定圖像生成、右相機標定圖像生成、以及立體標定算法三個子模塊。左、右相機標定圖像生成子模塊分別生成位于世界坐標系下不同位置處的標定版在左、右相機中的圖像，標定版為7×7圓形光標陣列，共生成標定圖像21幅；立體標定算法根據(jù)所生成的21幅標定圖像，采用最小二乘法分別對左、右相機投影模型進行標定，得出左、右相機投影矩陣CL和CR。
本發(fā)明與現(xiàn)有技術相比具有如下優(yōu)點(1)系統(tǒng)成本低、安全性、靈活性好，且易于實現(xiàn)。不需要采用真實的測量設備、空間機器人系統(tǒng)及目標星，在一臺獨立的PC機上即可進行空間機器人跟蹤、接近、捕獲非合作目標過程中的所有關鍵算法的閉環(huán)仿真和驗證；(2)將多剛體動力學、軌道、姿態(tài)動力學集成一體，開展18DOF的聯(lián)合仿真，更真實、準確的反映系統(tǒng)的動力學特性；(3)模擬的相機圖像考慮了航天器飛行軌道上的地球背景、星空背景、及圖像采集噪聲，更準確地反映了在軌視覺測量的惡劣條件；(4)采用的非合作目標自主識別與位姿測量，不要求在目標星上安裝用于輔助測量的發(fā)光標志器或角反射鏡，也無需知道目標的幾何尺寸，直接識別航天器自身的部件，并計算相對位姿；(5)采用的空間機器人目標捕獲自主路徑規(guī)劃方法，能實時預測目標的運動狀態(tài)、自主回避運動學及動力學奇異、實時預測機械臂運動對基座產生的干擾并自主調整規(guī)劃的路徑；(6)采用的協(xié)調控制方法，既考慮了整個空間機器人系統(tǒng)的控制性能，又適應了當前的星載處理器的控制能力。



圖1是典型的空間機器人在軌服務流程； 圖2是空間機器人系統(tǒng)及非合作目標在軌捕獲示意圖； 圖3是空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)組成圖； 圖4是空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)的界面； 圖5是空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)的立體標定界面； 圖6是雙目立體視覺測量原理； 圖7是非合作目標識別與位姿測量算法流程； 圖8是所識別出的非合作目標的帆板支架； 圖9是交會參考坐標系與捕獲點坐標系的定義； 圖10是飛輪姿態(tài)控制框圖； 圖11是噴氣姿態(tài)控制框圖； 圖12是噴氣軌道控制框圖； 圖13是姿態(tài)與軌道六自由度噴氣控制框圖； 圖14是空間機器人目標捕獲自主路徑規(guī)劃流程圖； 圖15是推力器的布局圖； 圖16是單臂空間機器人系統(tǒng)運動學鏈示意圖； 圖17是OpenGL的堆棧原理； 圖18是三維物體模型類的繼承關系； 圖19是3D幾何建模的流程圖； 圖20是基于透視投影的虛擬圖像生成原理； 圖21是模擬生成的虛擬左相機圖像(512×512黑白)； 圖22是模擬生成的虛擬右相機圖像(512×512黑白)；
具體實施例方式 一、系統(tǒng)組成 典型的空間機器人在軌服務思想如圖1所示，即利用空間機器人系統(tǒng)，對故障衛(wèi)星進行跟蹤、接近、捕獲、對接、維修等操作。在軌服務流程大致可分為如下幾個階段 (i)遠距離跟蹤接近空間機器人系統(tǒng)從自身軌道機動到距離目標衛(wèi)星300m左右的位置； (ii)中距離跟蹤接近空間機器人系統(tǒng)從300m機動到距離目標衛(wèi)星15m左右的位置； (iii)近距離交會與?？靠臻g機器人系統(tǒng)從15m左右的位置機動到1m左右的位置，并進行相對?？?，使得目標衛(wèi)星處于機械臂的工作空間內； (iv)在軌抓捕與對接利用空間機械手抓捕目標衛(wèi)星，并將其與空間機器人基座對接在一起； (v)在軌維修或執(zhí)行離軌操作，即將目標帶入地球大氣層或墳墓軌道利用機械臂對目標衛(wèi)星進行在軌維修，或者將目標帶入地球大氣層或墳墓軌道。
假設待服務對象為一帆板未展開的故障衛(wèi)星，其上未預先安裝用于視覺測量的標志器和用于捕獲的手柄，即為非合作目標。所設計的空間機器人在軌服務系統(tǒng)由一飛行基座和機械手組成，如圖2所示。其中，飛行基座上安裝了目標測量系統(tǒng)——基座立體視覺、對接機構、姿軌控系統(tǒng)等，空間機械手由6DOF機械臂、抓捕手爪及末端立體視覺組成。將利用空間機器人系統(tǒng)，對故障衛(wèi)星進行跟蹤、接近、抓捕、維修等操作。
為對整個空間機器人在軌服務過程進行閉環(huán)仿真，開發(fā)了一套空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，其組成如圖3所示，包括六個大的模塊圖像處理與位姿測量模塊、空間機器人規(guī)劃與控制、目標控制器、系統(tǒng)動力學模型、系統(tǒng)幾何模型及雙目相機模型?！皥D像處理與位姿測量模塊”主要是處理雙目立體相機的圖像——包括圖像濾波、邊沿檢測及直線提取等，并提供目標的相對位姿測量值。“空間機器人規(guī)劃與控制”根據(jù)相對位姿測量數(shù)據(jù)，規(guī)劃基座的期望位姿和機械臂關節(jié)角的運動軌跡，并調用相應的控制算法跟蹤期望的軌跡。“目標星控制器”用于產生目標的控制力矩，以實現(xiàn)目標按給定的規(guī)律運動，如三軸穩(wěn)定、自旋穩(wěn)定或翻滾衛(wèi)星?！跋到y(tǒng)動力學模型”包括相對軌道動力學、空間機器人動力學及目標姿態(tài)動力學模型，用于模擬真實對象的運動，其輸入為各自由度的驅動力/力矩，輸出為各自由度的狀態(tài)變化?！跋到y(tǒng)幾何模型”的功能是根據(jù)各部分——空間機械臂、基座、目標星、地球及星空背景的幾何參數(shù)和當前狀態(tài)繪制并顯示各部分的3D運動情況。系統(tǒng)的3D模型通過OpenGL建立。“雙目相機模型”根據(jù)實際相機的光學參數(shù)——分辨率、視場角，以及安裝位置，產生實際相機所成的像。
所開發(fā)的非合作目標自主識別與捕獲全數(shù)學仿真系統(tǒng)如圖4所示，該系統(tǒng)還可進行雙目立體視覺標定，如圖5所示。
二、非合作目標自主識別與位姿測量算法 (1)相機成像及3D重構模型 針孔模型(pin-hole model)是最常用的相機成像模型，可表示為 其中，WP＝[XW，YW，ZW]T為空間某點P在世界坐標系中的坐標，(u，v)是以像素為單位的圖像坐標，C為相機的投影變換矩陣，由其內外參數(shù)確定?；诹Ⅲw視覺的3D重建原理如圖6所示，假定第i個特征點Pi在世界坐標系中的位置為WPi＝[XWi YWi ZWi]T，投影在左右相機的圖像坐標分別為pLi＝[uLi vLi]T和pRi＝[uRi vRi]T。根據(jù)(1)，對于左、右相機，有如下關系 矩陣CL、CR分別為左右相機的變換矩陣，αi、βi為對應的元素。對式(2)和(3)進行化簡，有 寫成矩陣的形式 K·WPi＝U (5) 式(5)有四個方程、三個未知數(shù)，為超越方程，可用最小二乘法進行求解 WPi＝(KTK)-1KTU (6) 上述過程即為基于立體視覺的3D重構過程。
(2)基于立體視覺的相對位姿測量算法 基于立體視覺的相對位姿測量算法流程如圖7所示。主要包括如下步驟 (i)圖像濾波分別對左右相機圖像進行濾波，以消除噪聲干擾，得到平滑的左右相機圖像實際獲得的圖像一般都含有噪聲，本發(fā)明采用中值濾波算法進行圖像平滑。中值濾波是一種抑制噪聲的非線性處理方法。對于給定的n個數(shù)值{a1，a2，…，an}，將它們按大小順序排列。當n為奇數(shù)時，位于中間位置的那個數(shù)值稱為這n個數(shù)值的中值。當n為偶數(shù)時，位于中間位置的兩個數(shù)值的平均值稱為這n個數(shù)值的中值，記作med{a1，a2，…，an}。圖像經過中值濾波后，某像素的輸出等于該像素領域中的各像素灰度的中值。中值濾波的方法運算簡單，易于實現(xiàn)，而且能較好地保護邊界。
(ii)邊沿檢測對兩幅圖像分別進行邊沿檢測，得到邊沿特征信息 邊緣(edge)是指圖像局部強度變化最顯著的部分。邊緣主要存在于目標與目標、目標與背景、區(qū)域與區(qū)域(包括不同色彩)之間，是圖像分割、紋理特征和形狀特征等圖像分析的重要基礎。本文采用Canny算法對圖像進行邊緣檢測。Canny邊緣檢測算法的步驟如下(a)用高斯濾波器平滑圖像；(b)用一階偏導的有限差分來計算梯度的幅值和方向；(c)對梯度幅值應用非極大值抑制；(d)用雙閾值算法檢測和連接邊緣。
(iii)直線提取對進行邊沿檢測后的圖像進行直線提取，得到包含三角形支架各直線在內的各條直線信息 在利用Canny算法檢測出圖像邊沿后，再采用Hough變換提取直線。哈夫變換的基本思想是利用點-線(點-直線或點-曲線)的對偶性，即圖像空間x-y中的直線，經過Hough變換后成為參數(shù)空間的一點。
Hough變換算法的步驟如下(a)適當?shù)亓炕瘏?shù)空間；(b)假定參數(shù)空間的每一個單元都是一個累加器，把累加器初始化為零；(c)對圖像空間的每一點，在其所滿足的參數(shù)方程對應的累加器上加1；(d)累加器陣列的最大值對應模型的參數(shù)。
(iv)三角支架的識別從提取后的所有直線信息中，識別對應于三角形帆板支架的六條直線 采用Hough變換后，可以提取出多條直線，包括目標衛(wèi)星的輪廓、太陽電池陣列及太陽帆板支架。由于帆板支架有明顯的特征，故以帆板支架為識別對象。從眾多直線中識別出屬于帆板支架的六條直線，是本文研究的關鍵。由于除了已知帆板支架是一三角形外，沒有其他任何先驗知識，包括三角形的邊長、內角、各邊的傾斜度等，因此，識別的難度可想而知。在此假定圖像可通過遙測通道下傳到地面，地面操作員根據(jù)該圖像設置兩個初始參考點pr1、pr2，星載程序即可完成對帆板支架的自動識別，隨即轉入跟蹤模式，以后能夠自動根據(jù)圖像，實時跟蹤帆板支架所在的區(qū)域。參考點pr1為三角形內的任意一點，pr2為三角形外且位于太陽電池陣列上的任意一點，pr1、pr2通過鼠標直接在遙測圖像中選取，沒有特別嚴格的要求。識別的原理及結果如圖8所示。
(v)交點特征的提取利用識別出來的三角形支架對應的六條直線，計算出三角形的頂點 (vi)立體匹配、3D重構根據(jù)識別出的三角形支架各頂點分別在左、右相機中的2D信息，進行各特征點的3D重構，得到三角形支架各頂點在世界坐標系中的3D坐標，即WP1～WP6 (vii)目標位置、姿態(tài)的測量根據(jù)3D重構的結果，進一步構建抓捕對象坐標系，并計算出其相對于世界坐標系——追蹤星測量坐標系的位置和姿態(tài)。
根據(jù)已識別出的六個特征點，可建立目標的“交會參考坐標系”∑R2和“捕獲點坐標系”∑Cap，如圖9所示。首先，支架中心的三個點(Q1～Q3)的三維坐標由下面幾式確定 則中點 坐標系∑R2的原點為 坐標系∑R2的X軸為單位矢量 坐標系∑R2的Y軸為單位矢量 坐標系∑R2的Z軸由右手定則確定 aR2＝nR2×oR2 (14) 則坐標系∑R2相對于交會參考系∑R1的姿態(tài)旋轉矩陣為 R1AR2＝[nR2 oR2 aR2] (15) 姿態(tài)歐拉角可通過姿態(tài)旋轉矩陣R1AR2求出，結合式(11)求出的相對位置，至此，目標的相對位置和姿態(tài)均已測出。類似的，“捕獲點坐標系”∑Cap相對于“交會參考系”∑R1的位置、姿態(tài)亦可計算出。
三、空間機器人路徑規(guī)劃與控制 (1)基座GNC 相對導航由于基座立體相機只能提供相對位置和相對姿態(tài)角信息，為得到相對速度和相對姿態(tài)角速度信息并降低復雜性，采用狀態(tài)觀測器方案對上述信息進行估值。由交會相機提供出的相對位置矢量測量值，建立一個線性平移狀態(tài)觀測器，將觀測器得到的平移位置矢量估計值和平移速度矢量估計值，作為平移控制的測量輸入。為減少狀態(tài)觀測器的延遲帶來的影響，采用推力器管理功能模塊提供預測控制力信息；根據(jù)相機給出的相對姿態(tài)角測量值，建立一個線性相對姿態(tài)狀態(tài)觀測器，由該線性觀測器得到的相對姿態(tài)估計值和相對角速度估計值，作為相對姿態(tài)控制的測量輸入。同樣，為減少狀態(tài)觀測器的延遲帶來的影響，采用推力器管理功能模塊或飛輪控制模塊部分提供預測控制力矩信息。
制導律設計(i)VBar直線接近與后撤制導律，適用于在中、近距離的接近與后撤操作。在該制導律下，空間機器人沿目標衛(wèi)星軌道坐標系的-VBar方向進行直線接近與后撤，其余兩個方向參考軌跡為零。分別設置最大加速度限制，根據(jù)初速度、加速度、時間、距離等要求，計算二元方程，得出和時間相關的參考軌跡和速度；(ii)位置保持制導律，適用于各距離段的相對?？奎c保持，使空間機器人和目標衛(wèi)星維持在一個穩(wěn)定的相對位置。在該制導律下，設計的相對速度為零，標稱相對位置為某一固定值，相對位置設置一個不控區(qū)間，在此區(qū)間內不進行位置控制，否則以標稱相對位置為目標進行控制。區(qū)間的選擇根據(jù)位置控制精度指標來選擇，在近距離?？勘３制陂g，不控區(qū)為零或較小，距離遠處設置較大；(iii)遠距離接近與后撤的C-W制導律，適用于空間機器人相對目標衛(wèi)星的遠距離接近與后撤操作，在此制導律下，空間機器人根據(jù)相對運動的末端期望位置和速度，在初始時間點根據(jù)C-W方程計算需要的速度增量和末端需要的速度增量，并在某些固定的時間點計算當前速度偏差來決定是否中途修正。C-W制導律的計算結果與當前位置和速度、末端位置和速度以及飛行時間有關。
控制算法設計空間機器人的控制包括姿態(tài)控制和軌道控制兩類，姿態(tài)控制包括非任務期間的速率阻尼、地球捕獲和定向、對日捕獲和定向、變軌時姿態(tài)穩(wěn)定控制、全姿態(tài)捕獲控制，以及任務期間的對目標定向和對地定向控制，速率阻尼、變軌以及連續(xù)軌道控制時采用噴氣控制模式，其他模式主要采用零動量反作用飛輪進行控制。軌道控制包括相對位置保持、跟蹤接近與后撤以及伴隨飛行等多種，采用噴氣控制來實現(xiàn)。對于姿態(tài)和軌道均采用噴氣控制的情況，設計六自由度控制策略實現(xiàn)姿態(tài)和軌道的同步控制。
(i)基于飛輪姿態(tài)控制 當采用飛輪控制時，采用姿態(tài)四元數(shù)作為姿態(tài)參數(shù)。為滿足系統(tǒng)穩(wěn)態(tài)精度與動態(tài)性能要求，輪控設計基于以下原則減少帆板撓性振動的影響；克服系統(tǒng)測量噪聲及飛輪摩擦力矩的影響；克服外干擾的影響；考慮慣量時變，保證系統(tǒng)有一定的穩(wěn)定裕度。采用PID控制器，結合補償器對星體進行控制，控制框圖如圖10。飛輪卸載采用磁卸載和噴氣卸載兩種，磁卸載公式為 當整星某軸角動量超過啟控閾值時，啟動磁卸載，當角動量低于給定的脫控閾值時，斷開磁卸載控制。噴氣卸載在如下情況時工作當外擾力矩作用使輪子達到飽和時，啟動噴氣卸載。此時姿態(tài)控制轉入運行段噴氣控制模式，反作用飛輪按脫控閾值保持狀態(tài)模式進行工作。當飛輪的轉速達到脫控閾值時自動重新進入輪控模式。
(ii)基于噴氣的姿態(tài)控制方法 采用PD控制器，結合補償器對星體進行控制。推力器采用脈寬調制方式輸出?？刂瓶驁D如圖11所示。
(iii)噴氣軌道控制設計 采用PD控制器，結合補償器對星體進行控制。推力器采用脈寬調制方式輸出?？刂瓶驁D如圖12所示。
(iv)姿態(tài)與軌道六自由度噴氣控制 若姿態(tài)無法做到全力偶控制，這樣，由噴氣實現(xiàn)的姿態(tài)控制必然會導致軌道的變化，對于空間交會的兩航天器來說，在姿態(tài)控制的同時考慮軌道的影響是必須的。六自由度控制的基本思想是通過姿態(tài)確定和相對導航的數(shù)據(jù)加上補償信息，由控制器算法得出作用在星體上的力矩和外力信息，通過推力器的分配得出各個推進閥的工作狀態(tài)?？刂瓶驁D如圖13所示。
(2)機械臂捕獲目標的自主路徑規(guī)劃 上述圖像處理與位姿測量算法給出了“捕獲點坐標系”∑Cap相對于“交會參考系”∑R1的位置、姿態(tài)，自主規(guī)劃算法首先將該測量結果先轉化為“捕獲點坐標系”∑Cap相對于“機械臂末端坐標系”的位置、姿態(tài)，然后根據(jù)此結果，實時規(guī)劃空間機器人的運動，以最終捕獲目標。主要包括如下步驟手眼相機測量、位姿偏差計算、目標運動的預測、空間機器人末端運動速度規(guī)劃、空間機器人避奇異的路徑規(guī)劃、基座運動的預測等。首先，根據(jù)手眼測量數(shù)據(jù)判斷相對位姿偏差ep和eo是否小于設定的閾值εp和εo，若小于，則閉合手爪、捕獲目標；反之，則根據(jù)相對位姿偏差，實時估計目標的運動狀態(tài)，并將估計的結果反應到機械臂末端速度的規(guī)劃中，以保證機械臂末端時刻朝最近的方向趨近目標，機械臂末端能自主跟蹤目標的運動，直到最后捕獲目標。規(guī)劃出末端運動速度后，即調用自主奇異回避算法，以解算關節(jié)的期望角速度，并據(jù)此預測機械臂運動對基座的擾動，當擾動超出容許的范圍時，則自動調整機械臂的關節(jié)運動速度，以保證期望的偏轉在許可的范圍內。整個過程一直持續(xù)到機械臂捕獲到目標為止，其流程如圖14所示。
(3)基座執(zhí)行機構 采用反作用“三正交+一斜裝”的反作用飛輪、“20+1推力器”以及磁力矩器作用執(zhí)行機構。
(i)反作用飛輪 采用4個反作用飛輪，安裝方式為3正交加1斜裝。其中三個飛輪的輸入軸沿星體的X、Y、Z軸正交安裝，第四個斜裝飛輪作為備份，與本體+X、-Y、-Z軸成54.73°安裝。
在力矩模式中，飛輪的輸入是力矩指令。通過調節(jié)電機的電樞電流I產生所需的電磁力矩，克服了軸承摩擦力矩之后，得到加速飛輪的力矩，施于衛(wèi)星的控制力矩是飛輪的反作用力矩。
單個飛輪考慮動摩擦力矩、動不平衡力矩、粘性摩擦力矩，飛輪組考慮飛輪組與星體及飛輪組自身的耦合干擾力矩，其計算模型如下 飛輪動力學模型 ωw＝Hw/Jw(18) 式中 Jw——飛輪的轉動部分的轉動慣性量； Hw——飛輪的角動量； ωw——飛輪的角速率； Td——飛輪軸的驅動合力矩。
飛輪軸的驅動合力矩的計算公式為 Td＝KMVUw-Tf (19) 式中 KMV——飛輪的力矩電壓系數(shù)； Uw——飛輪的控制電壓； Tf——輪的摩擦力矩。
飛輪的摩擦力矩的計算公式為 式中 αw——氣動阻力力矩系數(shù)； Cw——飛輪的粘性摩擦力矩系數(shù)； Te——飛輪的動摩擦力矩； sign——符號函數(shù)。
(ii)推力器 為滿足GNC的控制要求，推進分系統(tǒng)共采用20臺1N推力器做為平移姿態(tài)控制推力器，分為每組10臺的兩組互為備份，分別裝于質心平面的四個三機機組和底部的四個雙機機組上，1臺5N推力器做為軌控推力器，布置于底部平面的幾何中心。推力器總體布局見圖15。
推力器的建模采用脈寬調制的方式，即根據(jù)推力器控制指令——噴氣時間，決定每個推力器的作用脈寬。
(iii)磁力矩器 采用3個磁力矩器，主要用于飛輪卸載。沿星體的X、Y、Z軸正交安裝。磁力矩器的建模如下 已知期望生成的磁矩為U_mt[3]，本體坐標系相對于地心-慣性坐標系的姿態(tài)四元素為Qbi，地磁場強度在慣性坐標系下的矢量為Bi，則其在本體坐標系下的矢量Bb為 Bb＝Cbi×Bb (21) Cbi為根據(jù)姿態(tài)四元素為Qbi計算得到的姿態(tài)變換矩陣。則磁力矩器產生的控制力矩為 Tmag＝(Umt+Nnoierr)×Bb (22) 其中Nnoierr為其隨機誤差。
(4)機械臂關節(jié)角控制器 當期望的機械臂關節(jié)角、角速度規(guī)劃出來后，需要設計相應的控制策略，控制機械臂關節(jié)角跟蹤所規(guī)劃的軌跡。采用如下PD控制律進行控制 上式中，Kp＝diag[kp1，kp2，…，kp6]，Kd＝diag[kd1，kd2，…，kd6]分別為各個關節(jié)的比例微分控制參數(shù)，

為期望的關節(jié)角、角速度，

為當前的關節(jié)角、角速度。
四、空間機器人系統(tǒng)及目標星的動力學建模 (1)航天器軌道動力學模型 記質點在慣性系下的位置為r1，在中心天體體固系下的慣性加速度aE，慣性系到中心天體體固系的坐標旋轉矩陣為CEI，反之為CIE，則有 上式中，求導為在慣性系下的導數(shù)，括號表示aE為CEIr1的函數(shù)。將上式積分即可得到質點在慣性系下的位置速度。
追蹤星和目標星的軌道坐標系分別記為Oo1Xo1Yo1Zo1、Oo2Xo2Uo2Zo2，兩航天器的相對位置(追蹤星質心Oo1在目標星軌道坐標系Oo2Xo2Yo2Zo2中的坐標)rc＝[x，y，z]，相對速度為在兩航天器相距較近，且均運行在近圓軌道的條件下，相對運動可用Hill方程進行簡化描述 其中，(fx，fy，fz)為施加在追蹤星上的控制力在目標星軌道坐標系下的投影。m1為追蹤星的質量，ω為軌道角速度，對圓軌道來說，ω為常值。
(2)航天器的姿態(tài)動力學模型 根據(jù)角動量定律，衛(wèi)星繞其質心轉動的角動量方程為 式中 Ib——星體轉動慣量矩陣，在本體系下的表示，常數(shù)； ωb——星體絕對角速度向量，在本體系下的表示； Tc——作用于星體上的控制力矩，在本體系下的表示； Td——作用于星體上的擾動力矩，在本體系下的表示。
在航天器姿態(tài)動力學建模中，需要考慮的擾動力矩包括氣動力矩、太陽輻射力矩、剩磁力矩和重力梯度力矩。
(3)空間機器人系統(tǒng)的多剛體動力學模型 空間機器人動力學建模和控制方面的重要研究成果參見文獻(Xu Y，Kanade T.Space RoboticsDynamics and Control[M].Kluwer Academic Publishers，November，ISBN 0-7923-9265-5，1992)。單臂空間機器人的模型如圖16所示，由6自由度機械臂和作為基座的航天器組成。
空間機器人的一般運動學方程可表示為 其中，ve、ωe為機械臂末端線速度和角速度；v0、ω0為基座的線速度(質心處)和角速度。Jb為與基座運動相關的雅可比矩陣，而Jm為與機械臂運動相關的雅可比矩陣。
對于自由漂浮模式——基座位置、姿態(tài)均不受控，系統(tǒng)的線動量和角動量守恒，將守恒方程代入化簡，得到自由漂浮模式下空間機器人系統(tǒng)的運動學方程 其中Jg為空間機器人的廣義雅可比矩陣，它是基座姿態(tài)、機械臂關節(jié)角，以及各剛體質量、慣量的函數(shù)。
空間機器人系統(tǒng)的動力學方程(拉格朗日動力學方程)為 上式中 cb、cm∈R6分別為與基座運動和機械臂運動相關的非線性力，包括向心力和哥氏力；Fb∈R6為作用于基座的力和力矩，τm∈R6為機械臂關節(jié)的驅動力矩。
(4)動力學模型的VC實現(xiàn) 上述的動力學計算公式(25)、(26)及(29)均可寫成改寫成如下形式 其中，X為狀態(tài)變量，U為驅動力、力矩，

為相應的方程。以相對軌道動力學方程(25)為例 X＝[x，y，z]T (32) U＝[fx，fy，fz]T(33) 采用四階Runge-Kutta法對方程(31)進行數(shù)值積分，即可計算下一個時刻兩星的相對位置和速度。
五、3D幾何模型 利用OpenGL(Open Graphics Libaray)實現(xiàn)三維建模和實時仿真。OpenGL，是一個3D圖形庫，它被定義為“一個圖形硬件的軟件接口”，是計算機工業(yè)標準應用程序接口，主要用于定義二維和三維圖形。OpenGL是用于開發(fā)簡捷的交互式二維和三維圖形應用程序的最佳工具，任何高性能的圖形應用程序，從3D動畫、CAD輔助設計到可視化仿真，都可以利用OpenGL高質量、高性能的特點，因此成為專業(yè)圖形處理、科學計算等高端應用領域的標準圖形庫。在實際編程中，為了保存幾何變換和投影變換的操作，OpenGL維護兩個棧，即投影變換棧和幾何變換棧。棧中保存的元素即為投影變換和幾何變換的變換矩陣。在調用幾何變換操作時，OpenGL將該幾何變換操作的變換矩陣與當前棧的棧頂元素相乘，得到一個新的矩陣并將其作為幾何變換棧的棧頂。兩個棧之間的關系如圖17所示，壓棧操作的同時會把數(shù)據(jù)復制到新的棧頂。
由于機械臂各臂桿之間的關系建立在其D-H參數(shù)的基礎上，三維建模時也按照這一原理，因此在繪制每個桿臂的模型前要先在前一個桿臂坐標系的基礎上建立當前桿臂坐標系，然后才在新建立的坐標系下繪制桿臂，以此類推一直到機械臂末端的執(zhí)行器。但是，繪制好執(zhí)行器的模型后，當前的坐標系就留在了機械臂的末端執(zhí)行器上；通過出棧操作就能實現(xiàn)幾何變換矩陣的快速恢復，使其對應于初始的軌道坐標系，在此坐標系下進行目標衛(wèi)星的幾何建模。建模時，采用面向對象的編程思想，建立一個基本的物體模型類CBase3dObj，該類有6個位姿屬性——成員變量，三軸平移和三軸旋轉、設置顏色以及建模等方法。并在這個基本類的基礎上，派生出虛擬仿真環(huán)境中所有物體對應的類，如圖18所示。這些物體包括空間機器人基座CChaser、機械臂CArm、目標星CTarget和太空場景CScene。其中，空間機器人、目標星、太空場景與軌道坐標之間的關系由位姿屬性確定，太空場景主要指星星和地球。每個類中都包含了對應物體的幾何尺寸、顏色等屬性，并在其主建模函數(shù)中，包含了一系列基本的幾何體坐標變換和繪制函數(shù)，在太空場景類中還要實現(xiàn)紋理圖像的讀取和映射。
在這些基本物體模型類的基礎上，我們就能建立整個虛擬仿真環(huán)境，具體過程如下初始化完成后，先設置好虛擬相機坐標系相對于軌道坐標系的位姿，并保留當前的坐標系關系——幾何變換陣入棧；接著設置空間機器人基座坐標系相對于軌道坐標系的位姿，然后調用母星模型的繪制函數(shù)；在此當前坐標轉移的基礎上，根據(jù)機械臂的安裝位置，調用機械臂類對象的坐標平移和旋轉函數(shù)實現(xiàn)坐標系的變換，之后就能在母星上繪出機械臂的模型；繪制好母星及機械臂后，恢復當前坐標系到軌道坐標系，然后根據(jù)目標星的漂浮軌跡設置好目標星坐標系相對于軌道坐標系的位姿，再調用目標星模型的繪制函數(shù)?？傮w軟件實現(xiàn)流程如圖19所示。
六、雙目相機成像模型 通過在OpenGL中，設置透視投影來產生模擬的相機圖像，透視投影函數(shù)為 void gluPerspective(GLdouble fovy，GLdouble aspect，GLdouble zNear，GLdouble zFar)； 上述函數(shù)創(chuàng)建一個對稱透視視景體，參數(shù)fovy為視野在X-Z平面的角度，aspect是投影平面寬度與高度的比率；zNear和zFar分別是近、遠裁剪面到視點的距離。透視投影的含義如圖20所示。根據(jù)相機的視場角FOV、安裝位置及分辨率Img_W×Img_H，首先得到相機所能觀察到的圖像范圍，以左相機圖像生成為例，在VC中用下面的程序實現(xiàn) glViewport(0，0，Img_W，Img_H)； glMatrixMode(GL_PROJECTION)； glLoadIdentity()； gluPerspective(FOV，Img_W/Img_H，1，100000)； glMatrixMode(GL_MODELVIEW)； ……… m_GL.SetLighting()； glLoadMatrixd(&m_Chaser.m_MatrixTmp1
)； m_Cam.ConfigerCamera()； glPushMatrix()； m_Target.Draw()； glPopMatrix()； m_Chaser.Draw()； m_Arm.Draw()； m_Scene.Draw()； 執(zhí)行上述程序后，左相機所能觀察到的圖像存在在緩存中，該圖像仍然是24位彩色圖像，需要轉換為8位的黑白圖像，疊加上圖像噪聲后，存儲在圖像矩陣中 glReadBuffer(GL_BACK)； glReadPixels(0，0，Img_W，Img_H，GL_RED，GL_UNSIGNED_BYTE，ImageDataPixels)； for(j＝0；j＜Img_H；j++)/*----單通道的圖----相當于8位圖---*/ { for(i＝0；i＜IMG_WIDTH；i++) { g_Img_L[i][j]＝ImageDataPixels[i*Img_H+j]+ImgNoise[i][j]； } } ImgNoise是采用高斯隨機數(shù)函數(shù)生成的隨機數(shù)矩陣，用于模擬圖像噪聲。由此，實現(xiàn)了左相機的圖像生成與采集，得到含噪聲的圖像矩陣g_Img_L，類似的可得到含噪聲的右相機圖像矩陣g_Img_R。該兩矩陣作為圖像處理與位姿測量模塊的輸入。典型經過圖像生成與采集得到的的左、右相機圖像，分別如圖21、圖22所示。
七、立體相機標定 標定圖像生成與采集的實現(xiàn)與上面介紹的步驟相同，在此介紹標定的算法。針孔成像模型(1)可改寫為 式中，(Xwi，Ywi，Zwi，1)為空間第i點的坐標；(ui，vi，1)為第i點的圖像坐標；mij為投影矩陣M的第i行j列元素。式(35)包含三個方程 將式(36)中的第一式除以第三式，第二式除以第三式分別消去Zci后，可得到2個關于mij的線性方程 上式表示，如果標定塊上有N個已知點，并已知它們的空間坐標(Xwi，Ywi，Zwi)與它們的圖像點坐標(ui，vi)，則有2N個關于C矩陣元素的線性方程，下面用矩陣形式表示這些方程 由(1)式可見，C矩陣乘以任意不為零的常數(shù)并不影響(Xw，Yw，Zw)與(u，v)的關系，因此上式中可以指定m34＝1，從而得到關于M矩陣其他元素的2N個線性方程，這些未知元素的個數(shù)為11個，記為11維向量m，將方程組簡寫成 Km＝U (39) 其中，K為式(38)左邊2N×11矩陣，m為未知的11維向量，U為式(38)右邊的2N維向量，K，U為已知向量，當2N＞11時，我們可以用最小二乘法求出上述線性方程組的解為 m＝(KTK)-1KTU (40) m向量與m34＝1構成了所求解的C矩陣，由上可見，由空間6個或6個以上已知點與它們的圖像點坐標，就可以求出C矩陣。在標定中，標定版上有7×7個已知點，且采集了不同位置、姿態(tài)下的21幅圖，使方程的個數(shù)大大超過未知數(shù)的個數(shù)，從而用最小二乘法求解以降低誤差造成的影響。
對左、右相機，均采用如上的標定算法，分別得到標定矩陣CL、CR。
權利要求
1.空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，其特征在于包括圖像處理與位姿測量模塊(1)、空間機器人規(guī)劃與控制模塊(2)、目標星控制器模塊(3)、系統(tǒng)動力學模塊(4)、系統(tǒng)3D幾何模型模塊(7)、雙目相機模擬成像模塊(6)及立體標定模塊(5)。其中
圖像處理與位姿測量模塊(1)，接收由雙目相機模擬成像模塊(6)輸出的左、右相機圖像Img_L[W][H]、Img_R[W][H]，并對圖像進行實時處理，最后計算出非合作目標星相對于空間機器人基座和末端的相對位置和姿態(tài)
該測量結果作為空間機器人規(guī)劃與控制模塊(2)的輸入；
空間機器人規(guī)劃與控制模塊(2)，根據(jù)從圖像處理與位姿測量模塊(1)輸入的相對位姿測量結果
以及系統(tǒng)動力學模塊(4)輸入的系統(tǒng)當前狀態(tài)——基座質心姿態(tài)Ψc、角速度ωc、機械臂關節(jié)角
角速度
自主規(guī)劃跟蹤接近過程中基座進行姿態(tài)、軌道控制的期望位姿軌跡，以及利用機械臂抓捕目標過程中機械臂關節(jié)角的運動軌跡，并調用相應的控制算法跟蹤期望的軌跡，輸出為系統(tǒng)質心控制力、基座姿態(tài)控制力矩以及機械臂各關節(jié)的控制力矩，該輸出是系統(tǒng)動力學模塊(4)的一部分輸入；
目標星控制器模塊(3)，建立目標衛(wèi)星的控制器和執(zhí)行機構模型，對目標進行位置、姿態(tài)的6DOF控制。該模塊根據(jù)設定的目標運動模式，規(guī)劃期望的位置、姿態(tài)軌跡，然后由該模塊中的控制器、執(zhí)行機構產生目標的控制力、力矩，實現(xiàn)目標按給定的規(guī)律運動，如三軸穩(wěn)定、自旋穩(wěn)定或翻滾衛(wèi)星。該模塊的輸出為目標星動力學的作用力和力矩，是系統(tǒng)動力學模塊(4)的另一部分輸入；
系統(tǒng)動力學模塊(4)，將兩星的相對軌道動力學、空間機器人多剛體動力學以及目標星的姿態(tài)動力學集成在同一環(huán)境中，用于模擬整個空間機器人系統(tǒng)——包括基座及機械臂，及目標星的軌道、姿態(tài)、及多剛體運動狀況。該模塊的輸入是空間機器人規(guī)劃與控制模塊(2)、目標星控制器模塊(3)輸出的各自由度(系統(tǒng)狀態(tài))的驅動力/力矩，輸出為空間機器人系統(tǒng)質心位置rc、線速度vc、姿態(tài)Ψc、角速度ωc、機械臂關節(jié)角
角速度
以及目標星的質心位置rt、線速度vt、姿態(tài)Ψt、角速度ωt，作為系統(tǒng)幾何模型模塊(7)以及空間機器人規(guī)劃與控制模塊(2)的輸入。
系統(tǒng)3D幾何模型模塊(7)，接收來源于系統(tǒng)動力學模塊(4)輸出的空間機器人系統(tǒng)質心位置rc、姿態(tài)Ψc、機械臂關節(jié)角
以及目標星的質心位置rt、姿態(tài)Ψt，建立空間機器人系統(tǒng)、目標星的3D幾何模型，以及航天器所在軌道環(huán)境的地球及星空背景模型，各模型由相應的狀態(tài)變量實時驅動，顯示各部分的3D運動情況；同時，該模塊輸出地球參考系、空間機器人系統(tǒng)本體坐標系以及目標衛(wèi)星本體坐標系相對于左、右相機坐標系的位置、姿態(tài)，作為雙目相機模擬成像模塊(6)的輸入；
雙目相機模擬成像模塊(6)，接收系統(tǒng)3D幾何模型模塊(7)輸出的輸出地球參考系、空間機器人系統(tǒng)本體坐標系以及目標衛(wèi)星本體坐標系相對于左、右相機坐標系的位置、姿態(tài)，根據(jù)實際相機的光學參數(shù)——分辨率、視場角、畸變等，生成立體視覺系統(tǒng)的左、右相機各自所成的圖像，并模擬圖像采集過程，將疊加噪聲后的圖像信息存儲在左、右相機圖像矩陣Img_L[W][H]、Img_R[W][H]，其中W×H代表相機CCD的分辨率，典型的為512×512。該模塊的輸出即為Img_L[W][H]、Img_R[W][H]，作為圖像處理與位姿測量模塊(1)的輸入。
立體標定模塊(5)，包括左右相機標定圖像生成、立體標定算法等子模塊。標定圖像可采用棋盤格或圓形光標，通過設定不同的位置、指向，提取21幅圖，然后采用最小二乘法標定相機的參數(shù)。標定數(shù)據(jù)用于非合作目標識別算法中。
2.根據(jù)權利要求1所述的空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，其特征在于所述的圖像處理與位姿測量模塊(1)包括圖像處理、3D重構及位姿測量三個子模塊。圖像處理子模塊接收自雙目相機模擬成像模塊(6)輸入的左、右相機圖像，并采用合適的圖像處理算法分別對左、右相機圖像進行處理，提取出目標特征點在左、右相機圖像中的2D圖像信息；3D重構子模塊利用已由立體標定模塊(5)標定好的相機投影模型——左、右相機投影矩陣分別為CL和CR，對由圖像處理子模塊輸出的特征點的2D圖像信息進行立體匹配，得到特征點在世界坐標系中的3D坐標；位姿測量子模塊根據(jù)特征點的3D坐標構造目標參考坐標系，并計算該參考坐標系相對于世界坐標系的位置
和姿態(tài)
3.根據(jù)權利要求2所述的空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，其特征在于所述的圖像處理算法包括圖像濾波、邊沿檢測、直線提取。平滑濾波采用中值濾波算法，邊沿檢測采用Canny算法，直線提取采用Hough變換算法，中值濾波對于給定的n個數(shù)值{a1，a2，…，an}，將它們按大小順序排列。當n為奇數(shù)時，位于中間位置的那個數(shù)值稱為這n個數(shù)值的中值。當n為偶數(shù)時，位于中間位置的兩個數(shù)值的平均值稱為這n個數(shù)值的中值，記作med{a1，a2，…，an}，圖像經過中值濾波后，某像素的輸出等于該像素領域中的各像素灰度的中值；Canny邊緣檢測算法用高斯濾波器平滑圖像、用一階偏導的有限差分來計算梯度的幅值和方向、對梯度幅值應用非極大值抑制、用雙閾值算法檢測和連接邊緣；Hough變換算法用點一線的對偶性，將圖像空間x-y中的直線，變換成為參數(shù)空間的一點；
4.根據(jù)權利要求1所述的空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，其特征在于所述的空間機器人規(guī)劃與控制模塊(2)包括基座GNC、基座執(zhí)行機構、空間機器人路徑規(guī)劃器、關節(jié)控制器四個子模塊?；鵊NC根據(jù)當前系統(tǒng)狀態(tài)——基座姿態(tài)Ψc、角速度ωc、機械臂關節(jié)角
角速度
以及目標衛(wèi)星的相對位姿測量結果
執(zhí)行空間機器人跟蹤、接近、?？磕繕诵l(wèi)星過程中的GNC算法，產生各執(zhí)行機構的控制指令——反作用飛輪的控制電壓、推力器的推力脈沖、磁力矩器的控制電壓；基座執(zhí)行機構包括反作用飛輪組件、推力器組件以及磁力矩器模型，將基座GNC子模塊的控制指令，轉換為基座質心作用力fb、力矩τb；空間機器人路徑規(guī)劃器根據(jù)當前系統(tǒng)狀態(tài)和目標衛(wèi)星的相對位姿測量結果，規(guī)劃空間機器人捕獲、維修目標衛(wèi)星過程中機械臂各關節(jié)角的軌跡——期望關節(jié)角
角速度
關節(jié)控制器子模塊包括六個關節(jié)的控制器，各控制器接收路徑規(guī)劃器規(guī)劃的期望關節(jié)角
和角速度
采用PID控制律，生成機械臂各關節(jié)的驅動力矩τb；
5.根據(jù)權利要求4所述的空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，其特征在于所述的GNC算法包括相對導航、制導律、姿態(tài)與軌道控制等部分。相對導航分別根據(jù)相對位置、姿態(tài)測量值，建立線性平移狀態(tài)觀測器、相對姿態(tài)狀態(tài)觀測器，將得到的平移位置矢量估計值、平移速度矢量估計值，作為平移控制的測量輸入，而將得到的相對姿態(tài)估計值和相對角速度估計值，作為相對姿態(tài)控制的測量輸入。制導律包括VBar直線接近制導律和位置保持制導律，在直線制導律下，空間機器人沿目標衛(wèi)星軌道坐標系的-VBar方向進行直線接近，其余兩個方向參考軌跡為零；相對位置保持制導律用于對?？奎c的保持，使空間機器人和目標衛(wèi)星維持在一個穩(wěn)定的相對位置，在位置保持制導律下，設計的相對速度為零，標稱相對位置為某一固定值。姿態(tài)與軌道控制包括基于飛輪的姿態(tài)控制、基于噴氣的姿態(tài)控制，以及姿態(tài)與軌道六自由度控制，其中六自由度的控制通過姿態(tài)確定和相對導航的數(shù)據(jù)加上補償信息，由控制器算法得出作用在星體上的力矩和外力信息，通過推力器的分配得出各個推進閥的工作狀態(tài)。
6.根據(jù)權利要求4所述的空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，其特征在于所述的空間機器人路徑規(guī)劃器實現(xiàn)了一種空間機器人捕獲非合作目標的自主路徑規(guī)劃方法，該方法利用手眼相機的相對位姿測量值，實時規(guī)劃空間機器人的運動，以最終捕獲目標。主要包括如下步驟位姿偏差計算、目標運動的預測、空間機器人末端運動速度規(guī)劃、空間機器人避奇異的路徑規(guī)劃、基座運動的預測等。首先，根據(jù)手眼測量數(shù)據(jù)判斷相對位姿偏差ep和eo是否小于設定的閾值，即捕獲區(qū)域——三軸相對位置在10mm以內，三軸姿態(tài)偏差在1°左右。若小于，則閉合手爪、捕獲目標；反之，則根據(jù)相對位姿偏差，實時估計目標的運動狀態(tài)，并將估計的結果反應到機械臂末端速度的規(guī)劃中，以保證機械臂末端時刻朝最近的方向趨近目標，機械臂末端能自主跟蹤目標的運動，直到最后捕獲目標。規(guī)劃出末端運動速度后，即調用自主奇異回避算法，以解算關節(jié)的期望角速度，并據(jù)此預測機械臂運動對基座的擾動，當擾動超出容許的范圍時，則自動調整機械臂的關節(jié)運動速度，以保證期望的偏轉在許可的范圍內。整個過程一直持續(xù)到機械臂捕獲到目標為止。
7.根據(jù)權利要求1所述的空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，其特征在于所述的系統(tǒng)動力學模塊(4)包括空間機器人系統(tǒng)的軌道動力學模型、多剛體動力學模型、目標衛(wèi)星的姿態(tài)及軌道動力學模型。空間機器人系統(tǒng)的軌道動力學模型接收基座執(zhí)行機構輸出的噴氣作用力，計算系統(tǒng)質心的位置、線速度；空間機器人系統(tǒng)多剛體動力學接收基座執(zhí)行機構輸出的姿態(tài)控制力矩、機械臂關節(jié)控制器輸出的關節(jié)驅動力矩，計算由基座和機械臂組成的多剛體系統(tǒng)的狀態(tài)變化——姿態(tài)Ψc、角速度ωc、機械臂關節(jié)角
角速度
目標衛(wèi)星姿態(tài)、軌道動力學接收由目標星控制器模塊(3)輸出的作用力、作用力矩，計算目標衛(wèi)星質心位置rt、線速度vt、姿態(tài)Ψt、角速度ωt；
8.根據(jù)權利要求1所述的空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，其特征在于所述的系統(tǒng)幾何模型模塊(7)包括軌道環(huán)境、空間機器人3D模型、目標星3D模型三個子模塊，各子模塊采用OpenGL進行建模。軌道環(huán)境模型子模塊根據(jù)空間機器人及目標衛(wèi)星所處的軌道位置，繪制地球背景、星空背景；空間機器人3D模型子模塊根據(jù)基座質心位置rc、姿態(tài)Ψc、機械臂關節(jié)角
繪制空間機器人系統(tǒng)的3D幾何模型；目標星3D模型子模塊根據(jù)目標衛(wèi)星質心位置rt、姿態(tài)Ψt，繪制目標衛(wèi)星的3D幾何模型。該模塊還輸出地球參考系、空間機器人系統(tǒng)本體坐標系以及目標衛(wèi)星本體坐標系相對于左、右相機坐標系的位置、姿態(tài)；
9.根據(jù)權利要求1所述的空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，其特征在于所述的雙目相機模擬成像模塊(6)由左相機圖像生成、左相機圖像采集、右相機圖像生成、右相機圖像采集等子模塊組成。相機圖像生成子模塊接收3D幾何模型模塊(7)輸出的地球參考系、空間機器人系統(tǒng)本體坐標系以及目標衛(wèi)星本體坐標系相對于左、右相機坐標系的位置、姿態(tài)，根據(jù)相機的分辨率、視場角、畸變系數(shù)等，采用透視投影模型，生成相應相機所觀察到的圖像；圖像采集子模塊模擬圖像采集過程中的噪聲傳遞過程，將噪聲數(shù)據(jù)疊加于所生成的圖像，圖像數(shù)據(jù)存儲在矩陣Img_L及Img_R中，作為圖像處理與位姿測量模塊(1)的輸入；
10.根據(jù)權利要求1所述的空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)，其特征在于所述的立體標定模塊(5)包括左相機標定圖像生成、右相機標定圖像生成、以及立體標定算法三個子模塊。左、右相機標定圖像生成子模塊分別生成位于世界坐標系下不同位置處的標定版在左、右相機中的圖像，標定版為7×7圓形光標陣列，共生成標定圖像21幅；立體標定算法根據(jù)所生成的21幅標定圖像，采用最小二乘法分別對左、右相機投影模型進行標定，得出左、右相機投影矩陣CL和CR。
全文摘要
空間機器人視覺測量、路徑規(guī)劃、GNC一體化仿真系統(tǒng)由圖像處理與位姿測量模塊1、空間機器人規(guī)劃與控制模塊2、目標星控制器模塊3、系統(tǒng)動力學模型模塊4、系統(tǒng)3D幾何模型模塊7、雙目相機模擬成像模塊6及立體標定模塊5等組成。該系統(tǒng)將圖像處理與位姿測量、GNC算法、路徑規(guī)劃、控制及動力學等集成一體，在一臺獨立的PC機上即可進行空間機器人跟蹤、接近、捕獲非合作目標過程中的所有關鍵算法的閉環(huán)仿真和驗證。該系統(tǒng)不需要采用真實的測量設備、空間機器人系統(tǒng)及目標星，成本低、安全性、靈活性好，易于實現(xiàn)；該系統(tǒng)可擴展性好，通過修改參數(shù)即可實現(xiàn)具有不同參數(shù)的相機(安裝位置、視場角)、空間機器人(運動學、動力學參數(shù))的閉環(huán)仿真驗證。
文檔編號G01C25/00GK101726296SQ20091007347
公開日2010年6月9日 申請日期2009年12月22日 優(yōu)先權日2009年12月22日
發(fā)明者徐文福, 梁斌, 李成, 王學謙, 齊海萍 申請人:哈爾濱工業(yè)大學