【技術(shù)實現(xiàn)步驟摘要】
本專利技術(shù)屬于水下機器人定位，具體涉及一種用于水下機器人定位的多普勒速度計殘差構(gòu)建方法。
技術(shù)介紹
1、目前水下多源融合slam(同時定位與建圖)算法對dvl(多普勒速度計)測量數(shù)據(jù)的使用可以大致分為兩種方式，一種將測量速度按時間進行積分得到測量數(shù)據(jù)之間的相對位置約束，利用非線性優(yōu)化得到位姿估計。另外一種則將dvl測量與系統(tǒng)狀態(tài)量關(guān)聯(lián)來作為卡爾曼濾波(及其擴展形式)的觀測方程使用。基于非線性優(yōu)化的算法通常較基于濾波的算法有更高的精度，但后者使用的dvl建模方法更加精確，為了結(jié)合兩者的優(yōu)勢，將dvl測量數(shù)據(jù)更加精確地與狀態(tài)量相關(guān)聯(lián)。我們提出了一種新的適用于非線性優(yōu)化融合的dvl殘差因子構(gòu)建方法，實現(xiàn)了對dvl測量數(shù)據(jù)的精確利用。
技術(shù)實現(xiàn)思路
1、針對現(xiàn)有的基于非線性優(yōu)化的slam算法對dvl測量數(shù)據(jù)的使用不充分不精確，本專利技術(shù)提出一種用于水下機器人定位的多普勒速度計殘差構(gòu)建方法。
2、為了達到以上目的，本專利技術(shù)采用的技術(shù)方案為：
3、本方案提供一種用于水下機器人定位的多普勒速度計殘差構(gòu)建方法，其特征在于包括以下步驟：
4、s1、通過dvl(多普勒速度計)測量水下機器人的三維速度；
5、s2、通過dvl與imu(慣性測量單元)之間的外參將dvl測量數(shù)據(jù)的參考坐標系轉(zhuǎn)換到imu坐標系；
6、s3、通過dvl和imu傳感器測量噪聲參數(shù)以及dvl與imu之間的外參，計算dvl因子協(xié)方差矩陣；
7、s4、通過系統(tǒng)狀態(tài)
8、s5、通過系統(tǒng)狀態(tài)估計量和dvl與imu之間的外參計算dvl殘差關(guān)于系統(tǒng)狀態(tài)估計量的雅可比矩陣；
9、進一步地，步驟s1中dvl測量數(shù)據(jù)如下：
10、通過發(fā)射和接收到的聲學(xué)波束信號的頻移計算得到dvl在對應(yīng)波束的方向上的速度，再通過多個波束(通常是4個)的速度測量結(jié)果轉(zhuǎn)換得到dvl在其本體坐標系xyz軸上的速度其中上標d表示dvl本體坐標系(下文簡稱為dvl坐標系)。
11、再進一步地，所述步驟s2包括以下步驟：
12、s201、在水下slam定位系統(tǒng)中我們以imu作為水下機器人的參考坐標系，需要將dvl的速度轉(zhuǎn)換到imu本體坐標系(下文簡稱為imu坐標系)中，設(shè)是dvl坐標系到imu坐標系的旋轉(zhuǎn)矩陣(通常是已知的)，為逆變換，為dvl在imu坐標系中的速度，有如下等式
13、
14、s202、因為dvl在imu坐標系的速度受到旋轉(zhuǎn)和平移兩中運動方式影響，可以分解為旋轉(zhuǎn)速度分量和平移速度分量使得對于某個特定的水下機器人，dvl在imu坐標系中的坐標是固定并且已知的，因此dvl在imu坐標系中的平移速度分量與imu在imu坐標系中的速度相等，即同理dvl與imu在imu坐標系中的角速度也相等。設(shè)在對應(yīng)dvl測量時刻，imu在imu坐標系中旋轉(zhuǎn)角速度為dvl在imu坐標系中的坐標為又因為角速度與線速度的轉(zhuǎn)換關(guān)系為所以可以將改寫為
15、
16、為了方便推導(dǎo)以及后續(xù)實現(xiàn)，我們將上式中的向量叉乘改寫為矩陣相乘的形式
17、
18、其中k為
19、
20、再進一步地，所述步驟s3計算dvl因子協(xié)方差矩陣包括以下步驟：
21、s301、計算dvl傳感器測量噪聲與dvl速度測量值等式關(guān)系。由于dvl的測量值只受零均值高斯白噪聲ndvl?n(0,σdvl)影響，dvl的測量模型為
22、
23、s302、計算imu角速度測量噪聲與dvl在imu坐標系的速度測量值等式關(guān)系。imu角速度測量值會受到零偏bgyr和測量噪聲ngyr(假設(shè)服從零均值高斯分布)的影響，其測量模型為:
24、
25、其中為imu角速度真值，我們可以得到imu角速度測量值與真值的關(guān)系式為
26、
27、將其帶入步驟s2中的矩陣相乘形式的等式：
28、
29、s303、有了前面的等式關(guān)系，計算dvl在imu坐標系中的測量速度與各個變量之間的關(guān)系式：
30、
31、將的等式帶入上式得：
32、
33、設(shè)該等式中的測量噪聲ndvl和測量噪聲ngyr均服從零均值高斯分布，經(jīng)過線性變換后仍為零均值高斯分布，并且兩個測量噪聲通常相互獨立，因此我們可以得到高斯噪聲n的協(xié)方差矩陣為
34、
35、再進一步地，所述步驟s4包括以下步驟：
36、水下機器人的每一個位姿狀態(tài)量包含著世界坐標系下四元數(shù)估計量(對應(yīng)imu坐標系到世界坐標系的旋轉(zhuǎn)矩陣估計)、速度估計量以及imu角速度零偏估計量我們可以依據(jù)這三個狀態(tài)估計量通過等式得到一個dvl在imu坐標系中的速度估計
37、
38、其中是的逆變換。這樣我們就可以通過速度估計和dvl實際測量的速度值構(gòu)建dvl速度殘差
39、
40、再進一步地，所述步驟s5包括以下步驟：
41、通過步驟s4的殘差公式，我們可以得到dvl速度殘差edvl關(guān)于和的雅可比矩陣
42、
43、其中的定義為
44、
45、本專利技術(shù)的有益效果是：
本文檔來自技高網(wǎng)...【技術(shù)保護點】
1.一種用于水下機器人定位的多普勒速度計殘差構(gòu)建方法，其特征在于，包括以下步驟：
2.根據(jù)權(quán)利要求1所述的一種用于水下機器人定位的多普勒速度計殘差構(gòu)建方法，其特征在于，所述步驟S1中DVL的測量數(shù)據(jù)如下：
3.根據(jù)權(quán)利要求2所述的一種用于水下機器人定位的多普勒速度計殘差構(gòu)建方法，其特征在于，所述步驟S2的步驟如下：
4.根據(jù)權(quán)利要求3所述的一種用于水下機器人定位的多普勒速度計殘差構(gòu)建方法，其特征在于，所述步驟S3的步驟如下：
5.根據(jù)權(quán)利要求4所述的一種用于水下機器人定位的多普勒速度計殘差構(gòu)建方法，其特征在于，所述步驟S4的步驟如下：
6.根據(jù)權(quán)利要求5所述的一種用于水下機器人定位的多普勒速度計殘差構(gòu)建方法，其特征在于，所述步驟S5的步驟如下：
【技術(shù)特征摘要】
1.一種用于水下機器人定位的多普勒速度計殘差構(gòu)建方法，其特征在于，包括以下步驟：
2.根據(jù)權(quán)利要求1所述的一種用于水下機器人定位的多普勒速度計殘差構(gòu)建方法，其特征在于，所述步驟s1中dvl的測量數(shù)據(jù)如下：
3.根據(jù)權(quán)利要求2所述的一種用于水下機器人定位的多普勒速度計殘差構(gòu)建方法，其特征在于，所述步驟s2的步驟如下：
4...
【專利技術(shù)屬性】
技術(shù)研發(fā)人員：武暢，宋子為，郭子凡，李其炎，呂澤杰，趙志奇，
申請(專利權(quán))人：電子科技大學(xué)，
類型：發(fā)明
國別省市：
還沒有人留言評論。發(fā)表了對其他瀏覽者有用的留言會獲得科技券。