本發(fā)明屬于非合作航天器導(dǎo)航設(shè)計(jì)領(lǐng)域，尤其涉及一種基于激光雷達(dá)的非合作航天器姿態(tài)估計(jì)方法及系統(tǒng)。

背景技術(shù)：

1、許多當(dāng)前和未來(lái)的星載應(yīng)用涉及與動(dòng)態(tài)自由浮動(dòng)空間目標(biāo)的自主交會(huì)對(duì)接（ar&d），例如在軌衛(wèi)星維護(hù)、捕獲故障或失控的航天器，以及大型空間平臺(tái)的機(jī)器人組裝，這些操作需要對(duì)空間物體進(jìn)行精確和穩(wěn)健的姿態(tài)估計(jì)。然而，估計(jì)缺乏主動(dòng)通信鏈路系統(tǒng)或補(bǔ)充指示器（如發(fā)光二極管或角反射器）的非合作航天器的相對(duì)姿態(tài)和位置仍然是一項(xiàng)艱巨的挑戰(zhàn)。

2、為了應(yīng)對(duì)上述挑戰(zhàn)，最近的任務(wù)概念正在研究機(jī)載傳感器在非合作航天器lichter&dubowsky（2004）姿態(tài)估計(jì)中的應(yīng)用。被動(dòng)傳感器，例如單目和立體相機(jī)，由于其硬件復(fù)雜性低，通常部署在當(dāng)前的星載任務(wù)中。ventura（2016）。然而，無(wú)源傳感器的成像質(zhì)量容易受到光照條件變化的影響。相反，主動(dòng)傳感器，如光探測(cè)和測(cè)距傳感器（lidar）和飛行時(shí)間（tof）相機(jī)，會(huì)產(chǎn)生電磁輻射并接收目標(biāo)物體的回波，使其在復(fù)雜的空間環(huán)境中更具彈性。激光雷達(dá)或tof相機(jī)生成的掃描點(diǎn)提供了目標(biāo)航天器幾何形狀的全面表示，即使在沒(méi)有明顯特征的情況下也能進(jìn)行姿態(tài)估計(jì)。盡管如此，大多數(shù)基于激光雷達(dá)的方法都依賴(lài)于模板匹配進(jìn)行姿態(tài)初始化，這帶來(lái)了一定的局限性，模板匹配需要一個(gè)大型數(shù)據(jù)庫(kù)來(lái)提高姿態(tài)初始化的準(zhǔn)確性，但這會(huì)降低在數(shù)據(jù)庫(kù)中搜索相關(guān)模板時(shí)的計(jì)算速度。因此，亟需提供一種準(zhǔn)確和穩(wěn)健的非合作航天器姿態(tài)估計(jì)方法。

技術(shù)實(shí)現(xiàn)思路

1、有鑒于此，本發(fā)明提供了一種可以克服模板匹配和點(diǎn)云配準(zhǔn)的姿態(tài)初始化的局限性、準(zhǔn)確估計(jì)非合作航天器的相對(duì)姿態(tài)的基于激光雷達(dá)的非合作航天器姿態(tài)估計(jì)方法及系統(tǒng)，具體采用以下技術(shù)方案來(lái)實(shí)現(xiàn)。

2、第一方面，本發(fā)明提供了一種基于激光雷達(dá)的非合作航天器姿態(tài)估計(jì)方法，包括以下步驟：

3、獲取激光雷達(dá)觀測(cè)到非合作航天器的掃描點(diǎn)，并對(duì)所述掃描點(diǎn)進(jìn)行姿態(tài)初始化得到掃描點(diǎn)云；

4、構(gòu)建所述非合作航天器的參考模型，并將所述掃描點(diǎn)云與所述參考模型進(jìn)行匹配得到匹配結(jié)果，其中，所述參考模型包括以所述掃描點(diǎn)云為輸入，并從所述掃描點(diǎn)云提取全局點(diǎn)特征對(duì)所述非合作航天器的相對(duì)姿態(tài)進(jìn)行回歸，采用單位四元數(shù)表示相對(duì)姿態(tài)；

5、基于迭代最近點(diǎn)算法和卡爾曼濾波器對(duì)所述匹配結(jié)果進(jìn)行姿態(tài)跟蹤以完成非合作航天器姿態(tài)估計(jì)。

6、作為上述技術(shù)方案的優(yōu)選，獲取激光雷達(dá)觀測(cè)到非目標(biāo)航天器的掃描點(diǎn)，包括：

7、根據(jù)tait-bryan角按照x-y-z序列從目標(biāo)參考系到激光雷達(dá)參考系推導(dǎo)出固有旋轉(zhuǎn)矩陣，采用單位四元數(shù)表示3d旋轉(zhuǎn)矩陣，其中，、是四元數(shù)的標(biāo)量部分和矢量部分，表示單位為三個(gè)球體，預(yù)設(shè)激光雷達(dá)傳感器在激光雷達(dá)參考系觀察到掃描點(diǎn)，設(shè)u是目標(biāo)參考系與掃描點(diǎn)v對(duì)應(yīng)的目標(biāo)點(diǎn)，將u從目標(biāo)參考系映射到激光雷達(dá)參考系的表達(dá)式為：

8、?（1）

9、和?（2）

10、?（3）

11、其中，表示由的分量形成的斜對(duì)稱(chēng)叉積矩陣，表示目標(biāo)點(diǎn)與激光雷達(dá)傳感器之間的平移向量，使用表示kronecker積。

12、作為上述技術(shù)方案的優(yōu)選，對(duì)所述掃描點(diǎn)進(jìn)行姿態(tài)初始化得到掃描點(diǎn)云，包括：

13、獲取第k時(shí)刻的掃描點(diǎn)云并進(jìn)行歸一化處理；

14、將歸一化處理后的掃描點(diǎn)云輸入已訓(xùn)練的神經(jīng)網(wǎng)絡(luò)中訓(xùn)練得到相對(duì)姿態(tài)；

15、采用迭代最近點(diǎn)算法對(duì)所述相對(duì)姿態(tài)進(jìn)行姿態(tài)跟蹤，并采用卡爾曼濾波器對(duì)姿態(tài)跟蹤對(duì)應(yīng)的誤差狀態(tài)進(jìn)行修正得到非合作航天器的六自由度姿態(tài)的實(shí)時(shí)跟蹤結(jié)果。

16、作為上述技術(shù)方案的優(yōu)選，采用ppam模型進(jìn)行回歸分析，ppam模型從到進(jìn)行回歸，其中，，設(shè)和表示兩個(gè)隨機(jī)變量，給定輸入和模型參數(shù)，目標(biāo)是預(yù)測(cè)相應(yīng)的，ppam可建模為局部仿射變換，對(duì)應(yīng)的表達(dá)式為：

17、?（4）

18、其中，矩陣和矩陣表示仿射變換的參數(shù)；是一個(gè)誤差向量，用于解釋高維觀測(cè)噪聲和重建誤差，對(duì)應(yīng)的潛在變量表示當(dāng)且僅當(dāng)從映射到第j個(gè)仿射變換；

19、若遵循具有對(duì)角協(xié)方差矩陣的零均值高速分布，則和的聯(lián)合概率分布可分解為：

20、?（5）

21、和?（6）

22、其中，表示模型參數(shù)的向量，約束為，若遵循由公式（7）的j高斯分布的混合定義：

23、?（7）

24、通過(guò)考慮觀測(cè)變量x、y和潛在變量的聯(lián)合概率分布，在模型參數(shù)的控制下，利用期望最大化算法來(lái)最大化期望完整數(shù)據(jù)對(duì)數(shù)似然函數(shù)，并獲得模型參數(shù)，對(duì)應(yīng)的表達(dá)式為：

25、?（8）

26、設(shè)是潛在變量的期望，即，若去定了模型參數(shù)，可按照公式（9）推導(dǎo)出回歸的條件密度：

27、?（9）

28、?（10）

29、輸出向量預(yù)設(shè)為公式（9）的期望值：

30、?（11）。

31、作為上述技術(shù)方案的優(yōu)選，構(gòu)建所述非合作航天器的參考模型，包括：

32、將掃描點(diǎn)云的質(zhì)心轉(zhuǎn)換為原點(diǎn)，掃描點(diǎn)根據(jù)模型主軸的長(zhǎng)度進(jìn)行歸一化，設(shè)定每組由n個(gè)點(diǎn)組成的掃描點(diǎn)，表示為，使用一個(gè)共享的多層感知器作為非線性函數(shù)執(zhí)行，將嵌入到高維的空間中；

33、將得到的輸出通過(guò)最大池化來(lái)生成高維全局特征向量g；

34、在j個(gè)不同的全連接層集合中的每一層都利用權(quán)重矩陣和偏差向量對(duì)輸入向量執(zhí)行線性變換，根據(jù)公式（4）的局部仿射變換，第j個(gè)全連接層的輸出記為，對(duì)應(yīng)的表達(dá)式為：

35、?（12）

36、其中，和分別表示第j個(gè)完全連接層的權(quán)重和偏差；

37、神經(jīng)網(wǎng)絡(luò)在計(jì)算相應(yīng)潛在變量的期望值時(shí)取代了公式（9）中的，利用共享mlp對(duì)全局特征向量進(jìn)行變換，并通過(guò)應(yīng)用softmax函數(shù)對(duì)進(jìn)行歸一化，其中，所有共享mlp采用relu和批規(guī)范化，估計(jì)的四元數(shù)是一組j相對(duì)姿態(tài)估計(jì)的平均值具有相應(yīng)權(quán)重的四元數(shù)形式，對(duì)應(yīng)的表達(dá)式為：

38、?（13）

39、給定的本征分解，矩陣對(duì)角線上的元素是矩陣m的特征值，正交矩陣u的列向量為矩陣m的特征向量，估計(jì)的四元數(shù)是與矩陣m最大特征值對(duì)應(yīng)的特征向量。

40、作為上述技術(shù)方案的優(yōu)選，采用均方誤差損失優(yōu)化神經(jīng)網(wǎng)絡(luò)：

41、?（14）

42、??（15）

43、其中，表示地面真值四元數(shù)標(biāo)簽的共軛，是恒等四元數(shù)，在計(jì)算網(wǎng)絡(luò)損耗時(shí)確保四元數(shù)誤差的標(biāo)量部分為正，損失函數(shù)的最后一項(xiàng)表示網(wǎng)絡(luò)參數(shù)的tikhonov正則化，正則化參數(shù)由超參數(shù)控制。

44、作為上述技術(shù)方案的優(yōu)選，基于迭代最近點(diǎn)算法和卡爾曼濾波器對(duì)所述匹配結(jié)果進(jìn)行姿態(tài)跟蹤以完成非合作航天器姿態(tài)估計(jì)，包括：

45、標(biāo)稱(chēng)向量定義為：

46、?（16）

47、其中，q表示相對(duì)姿態(tài)矢量，w表示相對(duì)加速度，r表示相對(duì)位置，v表示相對(duì)線速度，下標(biāo)k表示時(shí)間步長(zhǎng)；

48、姿態(tài)的外推利用了先前對(duì)速度的估計(jì)作為時(shí)間間隔內(nèi)的常數(shù)，計(jì)算處標(biāo)稱(chēng)狀態(tài)矢量的期望值從之前調(diào)整后的值開(kāi)始：

49、?（17）

50、其中，上標(biāo)f表示先驗(yàn)估計(jì)，上標(biāo)表示后驗(yàn)估計(jì)，誤差狀態(tài)向量為：

51、?（18）

52、誤差狀態(tài)的運(yùn)動(dòng)學(xué)可推導(dǎo)為：

53、?（19）

54、，

55、?（20）

56、其中，i是恒等矩陣，表示誤差協(xié)變量，i表示擾動(dòng)脈沖向量，和是關(guān)于誤差狀態(tài)和擾動(dòng)向量的預(yù)測(cè)雅可比矩陣，施加到高斯白噪聲的誤差狀態(tài)的隨機(jī)脈沖由、、和表示，的協(xié)方差可通過(guò)以下公式預(yù)測(cè)：

57、?（21）

58、其中，是擾動(dòng)脈沖的協(xié)方差矩陣，對(duì)的測(cè)量依賴(lài)于迭代最近點(diǎn)算法的姿態(tài)估計(jì)。

59、作為上述技術(shù)方案的優(yōu)選，觀測(cè)模型為：

60、?（22）

61、?（23）

62、其中，是相對(duì)于誤差狀態(tài)向量的測(cè)量雅可比矩陣，表示均值為零、協(xié)方差為的觀測(cè)誤差，若估計(jì)的姿態(tài)被卡爾曼濾波器校正，角速度和線速度的后驗(yàn)估計(jì)通過(guò)公式（24）得出：

63、?（24）。

64、作為上述技術(shù)方案的優(yōu)選，將姿態(tài)誤差定義為估計(jì)的四元數(shù)和地面真值q之間的測(cè)底線距離，平移誤差定義為平移和地表真值r之間差的2范數(shù)以評(píng)估估計(jì)的姿態(tài)，對(duì)應(yīng)的表達(dá)式為：

65、?（25）

66、?（26）。

67、第二方面，本發(fā)明還提供了一種激光雷達(dá)的非合作航天器姿態(tài)估計(jì)系統(tǒng)，應(yīng)用于上述的基于激光雷達(dá)的非合作航天器姿態(tài)估計(jì)方法，包括：

68、數(shù)據(jù)獲取模塊，用于獲取激光雷達(dá)觀測(cè)到非合作航天器的掃描點(diǎn)，并對(duì)所述掃描點(diǎn)進(jìn)行姿態(tài)初始化得到掃描點(diǎn)云；

69、模型構(gòu)建模塊，用于構(gòu)建所述非合作航天器的參考模型，并將所述掃描點(diǎn)云與所述參考模型進(jìn)行匹配得到匹配結(jié)果，其中，所述參考模型包括以所述掃描點(diǎn)云為輸入，并從所述掃描點(diǎn)云提取全局點(diǎn)特征對(duì)所述非合作航天器的相對(duì)姿態(tài)進(jìn)行回歸，采用單位四元數(shù)表示相對(duì)姿態(tài)；

70、姿態(tài)估計(jì)模塊，用于基于迭代最近點(diǎn)算法和卡爾曼濾波器對(duì)所述匹配結(jié)果進(jìn)行姿態(tài)跟蹤以完成非合作航天器姿態(tài)估計(jì)。

71、本發(fā)明提供了一種基于激光雷達(dá)的非合作航天器姿態(tài)估計(jì)方法及系統(tǒng)，通過(guò)獲取激光雷達(dá)觀測(cè)到非合作航天器的掃描點(diǎn)，并對(duì)所述掃描點(diǎn)進(jìn)行姿態(tài)初始化得到掃描點(diǎn)云，構(gòu)建所述非合作航天器的參考模型，并將所述掃描點(diǎn)云與所述參考模型進(jìn)行匹配得到匹配結(jié)果，基于迭代最近點(diǎn)算法和卡爾曼濾波器對(duì)所述匹配結(jié)果進(jìn)行姿態(tài)跟蹤以完成非合作航天器姿態(tài)估計(jì)，解決機(jī)載lidar系統(tǒng)的非合作航天器姿態(tài)跟蹤問(wèn)題，克服了基于模板匹配和點(diǎn)云配準(zhǔn)的姿態(tài)初始化方法的局限性，可以準(zhǔn)確地估計(jì)非合作航天器的相對(duì)狀態(tài)。