本發(fā)明涉及協(xié)同定位選擇，尤其涉及一種基于多相機協(xié)同定位的相機優(yōu)化部署和選擇方法。

背景技術(shù)：

1、協(xié)同定位正逐漸成為全球?qū)Ш叫l(wèi)星系統(tǒng)(gnss)受限環(huán)境、監(jiān)視與救援任務(wù)中定位與導(dǎo)航任務(wù)的一種十分有研究意義的方法。在眾多發(fā)展成熟的協(xié)同定位系統(tǒng)中，利用被動光學(xué)成像技術(shù)的相機因其低成本、低功耗和豐富的紋理信息，在嵌入式平臺上相較于基于無線電的超寬帶、激光雷達(dá)和超聲波設(shè)備表現(xiàn)更為出色。因此，近年來利用相機獲取待測物體與攝像頭光心之間相對方位的協(xié)同定位方法受到了越來越多的關(guān)注。

2、觀測幾何和傳感器的內(nèi)部參數(shù)對測量目標(biāo)的定位精度有著顯著影響。當(dāng)前關(guān)于傳感器部署與選擇優(yōu)化的研究大多面向通用或基于無線電傳播的測量模型，在目標(biāo)定位問題下，使用通用模型對傳感器進(jìn)行最優(yōu)布放，對于角度測量中相機傳感原理在協(xié)同定位優(yōu)化問題上的應(yīng)用仍然被忽視。

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

1、有鑒于此，本發(fā)明的目的在于提出一種基于多相機協(xié)同定位的相機優(yōu)化部署和選擇方法，以解決現(xiàn)有對于角度測量中相機傳感原理在協(xié)同定位優(yōu)化問題上的應(yīng)用的問題。

2、基于上述目的，本發(fā)明提供了一種基于多相機協(xié)同定位的相機優(yōu)化部署和選擇方法，包括

3、s1，在目標(biāo)周圍部署多個移動機器人，每個所述移動機器人上攜帶結(jié)構(gòu)相同的相機；所述相機的光學(xué)中心的高度與目標(biāo)高度相同，所述相機成像平面垂直于水平面；

4、s2，獲取朝向角ψ；

5、s3，在像素坐標(biāo)系下獲取所述相機在所述移動機器人前進(jìn)方向的偏航角α；

6、s4，通過所述朝向角ψ和所述偏航角α得到目標(biāo)和所述相機之間的相對角度θ，θ＝ψ-α；

7、s5，通過兩個以上所述相機的相對角度θ獲取目標(biāo)的位置；

8、s6，將獲取所述朝向角ψ過程中產(chǎn)生的外部誤差ω'誤差和由于相機內(nèi)部機理測量誤差導(dǎo)致測角的內(nèi)部誤差ω”中合得到相機測角總誤差ω；

9、s7，所述內(nèi)部誤差ω”分為由于相機內(nèi)部噪聲和成像平面成像點的波動而導(dǎo)致我們提取到的質(zhì)心坐標(biāo)在u軸方向上跳動而產(chǎn)生的隨機誤差ωa和因為所述相機內(nèi)部光心偏移和幾何畸變產(chǎn)生的幾何誤差ωb

10、外部誤差ω'、隨機誤差ωa和幾何誤差ωb均相互獨立；

11、s8，通過測角模型轉(zhuǎn)變的方法用正態(tài)分布對隨機誤差ωa建模；

12、s9，通過分析相機內(nèi)部機理用瑞利分布對幾何誤差ωb建模；

13、s10，將所述外部誤差ω'、所述隨機誤差ωa和所述幾何誤差ωb通過泰勒展開忽略高階項，近似為正態(tài)誤差，并利用正態(tài)誤差建立多相機協(xié)同定位系統(tǒng)的高斯模型；

14、s11，通過fim和d優(yōu)化準(zhǔn)則提出精度指標(biāo)，分析所述相機測角的定位精度，fim的行列式值越大時，相機測角的定位精度越高；

15、s12，獲取所述相機不同部署位置時的所述正態(tài)誤差的值，選取所述正態(tài)誤差的值最小時的相機部署位置為實際部署位置，完成所述相機的實際優(yōu)化部署和選擇。

16、作為本技術(shù)的進(jìn)一步改進(jìn)，所述在像素坐標(biāo)系下獲取所述相機在所述移動機器人前進(jìn)方向的偏航角α，包括

17、第i個相機光心的坐標(biāo)是(x0i,y0i,z0i)，提取到的目標(biāo)在像素坐標(biāo)下成像的質(zhì)心坐標(biāo)為(xdi,ydi,zdi)，相機的焦距為f，則得到第i個相機測得的偏航角αi的絕對值

18、

19、若偏航角的旋轉(zhuǎn)方向是逆時針方向，則αi的取值為正，否則αi取值為負(fù)，第i個相機測得與目標(biāo)之間的相對角度為：θi＝θi'+ω；θi'為利用第i個相機與目標(biāo)之間的真實相對角度，ω表示每個相機測角時的總誤差。

20、作為本技術(shù)的進(jìn)一步改進(jìn)，所述通過所述朝向角ψ和所述偏航角α得到目標(biāo)和所述相機之間的相對角度θ，θ＝ψ-α；

21、其中，所述朝向角ψ為所述相機裝配在所述移動機器人前進(jìn)方向與俯視世界坐標(biāo)系x軸的夾角，所述相機的前進(jìn)方向垂直于所述相機的成像平面，所述朝向角ψ從移動機器人的里程計中獲得；所述偏航角α為移動機器人前進(jìn)方向與相機與目標(biāo)連線的夾角。

22、作為本技術(shù)的進(jìn)一步改進(jìn)，所述通過兩個以上所述相機測量的相對角度θ獲取目標(biāo)的位置，具體的：

23、

24、其中，yc1為第一個相機的在所述移動機器人前進(jìn)方向與俯視世界坐標(biāo)系中的y軸坐標(biāo)，xc1為第一個相機的在所述移動機器人前進(jìn)方向與俯視世界坐標(biāo)系中的x軸坐標(biāo)；xt和yt分別為目標(biāo)在所述移動機器人前進(jìn)方向與俯視世界坐標(biāo)系中的x軸坐標(biāo)和y軸坐標(biāo)。

25、作為本技術(shù)的進(jìn)一步改進(jìn)，所述外部誤差ω'為由于移動機器人里程計計算產(chǎn)生的誤差噪聲導(dǎo)致朝向角產(chǎn)生的誤差ω'；外部誤差ω'為一個符合均值為0方差為的正態(tài)分布即

26、所述隨機誤差ωa導(dǎo)致質(zhì)心坐標(biāo)的跳動幅度為δu，

27、隨機誤差ωa的表達(dá)式如下：

28、

29、此時，隨機誤差ωa并不服從于正態(tài)分布，令所述相機的光心與目標(biāo)的連線與朝向線重合即使朝向角為0，此時，隨機誤差ωa表達(dá)為：

30、由于的實際值很小，根據(jù)泰勒展開式原理并且忽略高階項，則隨機誤差ωa可視為均值為0，方差為的正態(tài)分布，即

31、引入兩個向量分別表示光心偏移和成像平面畸變對質(zhì)心坐標(biāo)的影響，兩個向量相互獨立并且均滿足均值為0且方差為的正態(tài)分布，合成的向量的大小l滿足瑞利分布,l的概率密度滿足如下的關(guān)系：

32、

33、所述幾何誤差ωb表示為：

34、通過計算可知ε(l)＝1.25σg，var(*)代表計算方差，同理，利用之前分析隨機誤差ωa的方法，由于合成的向量的大小l本身值很小，所以根據(jù)大數(shù)定律和中心極限定理l可以近似為均值為ε(l)，方差為var(l)的正態(tài)分布，因此所述幾何誤差ωb表示為：

35、作為本技術(shù)的進(jìn)一步改進(jìn)，所述利用正態(tài)誤差建立多相機協(xié)同定位系統(tǒng)的高斯模型，包括

36、整個相機測角所有的角度信息用向量來表示：

37、θ＝θ'+ω

38、其中θ＝[θ1,θ2,...,θn]t，ω＝[ω,ω,...,ω]t，θ'表示每個相機與目標(biāo)之間的真實值矩陣，ω表示每個相機測角時的總誤差矩陣；

39、相機測角的誤差的協(xié)方差矩陣用∑＝e(ωωt)表示，通過計算：

40、

41、得到關(guān)于相機測角協(xié)同定位的高斯模型，關(guān)于相對角度θ的條件概率密度為：

42、

43、其中p＝[xt,yt]表示在俯視世界坐標(biāo)系下實際目標(biāo)的位置，θ'(p)為真實各個相機到目標(biāo)之間的相對角度矩陣，θ表示相機測得的與目標(biāo)之間的相對角度矩陣，n表示使用相機的個數(shù)，σ是相機測角的誤差矩陣。

44、作為本技術(shù)的進(jìn)一步改進(jìn)，所述通過fim和d優(yōu)化準(zhǔn)則提出精度指標(biāo)，分析所述相機測角的定位精度，包括

45、當(dāng)所述相機測角中所有誤差均滿足正態(tài)分布時，fim可以表示為：

46、fim＝j(luò)tσ-1j

47、其中

48、最終算出fim為：

49、

50、所述相機的像素點越大會帶來拍攝的圖像誤差越大，用下面公式來描述這一關(guān)系：

51、

52、其中f為相機的分辨率，k為一個常數(shù)；

53、得到對目標(biāo)定位精度影響的函數(shù)：

54、

55、其中:

56、

57、當(dāng)相機的分辨率越高，焦距越大時，對目標(biāo)的定位精度越高。

58、作為本技術(shù)的進(jìn)一步改進(jìn)，所述獲取所述相機不同部署位置時的所述正態(tài)誤差的值，選取所述正態(tài)誤差的值最小時的相機部署位置為實際部署位置，完成所述相機的實際優(yōu)化部署和選擇，包括

59、設(shè)定所述相機的所在區(qū)域為一個半徑為r的圓中，在俯視的二維世界坐標(biāo)系中，圓心為坐標(biāo)原點；

60、當(dāng)目標(biāo)在所述相機的所在區(qū)域內(nèi)時：

61、

62、當(dāng)時

63、此時det(fim)取得最大值；相機對目標(biāo)協(xié)同定位精度最高；

64、若所有的相機到目標(biāo)的距離均相等(即當(dāng)所有的di均相等時)相機的擺放滿足：

65、

66、其中，θ0為任意角，即所有的角滿足均勻陣列分布時，相機測角協(xié)同定位精度最高。

67、作為本技術(shù)的進(jìn)一步改進(jìn)，所述設(shè)定所述相機的所在區(qū)域為一個半徑為r的圓中，在俯視的二維世界坐標(biāo)系中，圓心為坐標(biāo)原點；

68、當(dāng)目標(biāo)在所述相機的所在區(qū)域外時：

69、目標(biāo)定位精度影響的函數(shù)簡化為：

70、

71、當(dāng)n為偶數(shù)，目標(biāo)的位置在俯視世界坐標(biāo)系的x軸正半軸，此時相機的測得的角度不是任意的，相機最大測得角度θ*的絕對值小于90°，即|θi|≤θ*＜90°,i＝1,...,n此時只有下式成立：

72、

73、為保證det(fim)取最大值，如果θ*≥45°上式是可實現(xiàn)的，若θ*＜45°，最大值為即時det(fim)最大；

74、①若θ*≥45°，所有的相機與目標(biāo)的相對角度θ＝[θ1,θ2,...,θn]t應(yīng)滿足

75、

76、②若θ*＜45°，所有的相機與目標(biāo)的相對角度θ＝[θ1,θ2,...,θn]t應(yīng)滿足

77、

78、當(dāng)n為奇數(shù)時，det(fim)寫成：

79、

80、其中當(dāng)時

81、

82、為保證det(fim)取最大值，如果上式是可實現(xiàn)的，反之將det(fim)改寫為

83、

84、其中若θ*≥45°，當(dāng)時，det(fim)最大，因為1-2sin2θ*＜0，因此θ1＝0°，若θ*＜45°，當(dāng)時，det(fim)最大，但是1-2sin2θ*＞0，因此|θ1|＝θ*；因此可以得到n為偶數(shù)時的優(yōu)化布局結(jié)果：

85、①若所有的相機測量角度θ＝[θ1,θ2,...,θn]t滿足：

86、θ1＝0°

87、

88、②若且θ*≥45°，所有的相機與目標(biāo)的相對角度θ＝[θ1,θ2,...,θn]t滿足：

89、θ1＝0°

90、

91、③若θ*＜45°所有的相機與目標(biāo)的相對角度θ＝[θ1,θ2,...,θn]t滿足：

92、|θ1|＝θ*

93、

94、本發(fā)明的有益效果：本技術(shù)通過fim和d優(yōu)化準(zhǔn)則提出精度指標(biāo)；獲取所述相機不同部署位置時的所述正態(tài)誤差的值，選取實際部署位置，完成所述相機的實際優(yōu)化部署和選擇。解決了現(xiàn)有對于角度測量中相機傳感原理在協(xié)同定位優(yōu)化問題上的應(yīng)用的問題。