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

背景技術：

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

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

技術實現(xiàn)思路

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

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

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

4、s2，獲取朝向角ψ；

5、s3，在像素坐標系下獲取所述相機在所述移動機器人前進方向的偏航角α；

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

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

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

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

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

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

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

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

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

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

16、作為本技術的進一步改進，所述在像素坐標系下獲取所述相機在所述移動機器人前進方向的偏航角α，包括

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

18、

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

20、作為本技術的進一步改進，所述通過所述朝向角ψ和所述偏航角α得到目標和所述相機之間的相對角度θ，θ＝ψ-α；

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

22、作為本技術的進一步改進，所述通過兩個以上所述相機測量的相對角度θ獲取目標的位置，具體的：

23、

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

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

26、所述隨機誤差ωa導致質心坐標的跳動幅度為δu，

27、隨機誤差ωa的表達式如下：

28、

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

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

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

32、

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

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

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

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

37、θ＝θ'+ω

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

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

40、

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

42、

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

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

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

46、fim＝jtσ-1j

47、其中

48、最終算出fim為：

49、

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

51、

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

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

54、

55、其中:

56、

57、當相機的分辨率越高，焦距越大時，對目標的定位精度越高。

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

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

60、當目標在所述相機的所在區(qū)域內時：

61、

62、當時

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

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

65、

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

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

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

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

70、

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

72、

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

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

75、

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

77、

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

79、

80、其中當時

81、

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

83、

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

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

86、θ1＝0°

87、

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

89、θ1＝0°

90、

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

92、|θ1|＝θ*

93、

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