[發(fā)明專(zhuān)利]一種基于距離測(cè)量的靜止剛體定位方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201810567164.0
• 申請(qǐng)日: 2018-06-05
• 公開(kāi)（公告）號(hào): CN108872935B
• 公開(kāi)（公告）日: 2020-11-10
• 發(fā)明（設(shè)計(jì)）人: 王剛;蔣堅(jiān)
• 申請(qǐng)（專(zhuān)利權(quán)）人: 寧波大學(xué)
• 主分類(lèi)號(hào): G01S5/06
• 分類(lèi)號(hào): G01S5/06
• 代理公司: 寧波奧圣專(zhuān)利代理事務(wù)所(普通合伙) 33226
• 代理人: 周玨

本發(fā)明公開(kāi)了一種基于距離測(cè)量的靜止剛體定位方法，其先確定剛體定位問(wèn)題的約束最小二乘表述形式，其包含有待估計(jì)的旋轉(zhuǎn)矩陣和位置矢量；然后通過(guò)改變約束條件，得到剛體定位問(wèn)題的約束最小二乘表述形式的轉(zhuǎn)化形式，進(jìn)而得到剛體定位問(wèn)題的半正定規(guī)劃形式；再對(duì)剛體定位問(wèn)題的半正定規(guī)劃形式進(jìn)行求解，得到旋轉(zhuǎn)矩陣和位置矢量各自的初步值；最后對(duì)旋轉(zhuǎn)矩陣的初步值進(jìn)行正交化，將正交化后的值作為旋轉(zhuǎn)矩陣的估計(jì)值，且直接將位置矢量的初步值作為其估計(jì)值；優(yōu)點(diǎn)是其在無(wú)線傳感器網(wǎng)絡(luò)中錨節(jié)點(diǎn)分布比較差或無(wú)線傳感器網(wǎng)絡(luò)中噪聲比較大的情況下，也能對(duì)剛體的旋轉(zhuǎn)和位置進(jìn)行精確估計(jì)，且計(jì)算復(fù)雜度低。

技術(shù)領(lǐng)域

本發(fā)明涉及一種目標(biāo)定位方法，尤其是涉及一種無(wú)線傳感器網(wǎng)絡(luò)中基于距離測(cè)量的靜止剛體定位方法，其定位內(nèi)容為估計(jì)剛體的旋轉(zhuǎn)和位置。

背景技術(shù)

近些年來(lái)，無(wú)線傳感器技術(shù)的快速發(fā)展使得無(wú)線傳感器網(wǎng)絡(luò)(WSN)在與定位導(dǎo)航、安防監(jiān)控相關(guān)的不同領(lǐng)域得到了廣泛應(yīng)用。在很多實(shí)際應(yīng)用中，精確估計(jì)剛體(即有固定形狀的物體)的旋轉(zhuǎn)和位置是非常重要的，如機(jī)器人、航天飛船、水下交通工具等等。因此，對(duì)無(wú)線傳感器網(wǎng)絡(luò)中剛體的定位方法的研究十分有必要。

目前，對(duì)于無(wú)線傳感器網(wǎng)絡(luò)中剛體的定位方法，使用較多的是基于距離測(cè)量的方法，其優(yōu)點(diǎn)是測(cè)量系統(tǒng)復(fù)雜度低，可實(shí)現(xiàn)高精度的定位結(jié)果。然而，在無(wú)線傳感器網(wǎng)絡(luò)中錨節(jié)點(diǎn)分布比較差或無(wú)線傳感器網(wǎng)絡(luò)中噪聲比較大的情況下，現(xiàn)有的基于距離測(cè)量的方法的定位精度會(huì)明顯下降；而現(xiàn)有的定位精度較高的定位方法，其計(jì)算復(fù)雜度比較高。

發(fā)明內(nèi)容

本發(fā)明所要解決的技術(shù)問(wèn)題是提供一種基于距離測(cè)量的靜止剛體定位方法，其在無(wú)線傳感器網(wǎng)絡(luò)中錨節(jié)點(diǎn)分布比較差或無(wú)線傳感器網(wǎng)絡(luò)中噪聲比較大的情況下，也能對(duì)剛體的旋轉(zhuǎn)和位置進(jìn)行精確估計(jì)，且計(jì)算復(fù)雜度低。

本發(fā)明解決上述技術(shù)問(wèn)題所采用的技術(shù)方案為：一種基于距離測(cè)量的靜止剛體定位方法，其特征在于包括以下步驟：

步驟一：設(shè)定無(wú)線傳感器網(wǎng)絡(luò)中存在M個(gè)用于接收測(cè)量信號(hào)的錨節(jié)點(diǎn)和一個(gè)剛體，并設(shè)定剛體的內(nèi)部放置有N個(gè)用于發(fā)射測(cè)量信號(hào)的未知節(jié)點(diǎn)；在無(wú)線傳感器網(wǎng)絡(luò)中建立一個(gè)空間坐標(biāo)系作為全局參考坐標(biāo)系，并在剛體的內(nèi)部設(shè)置一個(gè)空間坐標(biāo)系作為局部參考坐標(biāo)系；將M個(gè)錨節(jié)點(diǎn)在全局參考坐標(biāo)系中的坐標(biāo)位置對(duì)應(yīng)記為a₁,...,a_m,...,a_M，將剛體運(yùn)動(dòng)前N個(gè)未知節(jié)點(diǎn)在局部參考坐標(biāo)系中的坐標(biāo)位置對(duì)應(yīng)記為c₁,...,c_i,...,c_N；其中，M和N均為正整數(shù)，M≥4，N≥3，a₁表示第1個(gè)錨節(jié)點(diǎn)在全局參考坐標(biāo)系中的坐標(biāo)位置，m為正整數(shù)，1≤m≤M，a_m表示第m個(gè)錨節(jié)點(diǎn)在全局參考坐標(biāo)系中的坐標(biāo)位置，a_M表示第M個(gè)錨節(jié)點(diǎn)在全局參考坐標(biāo)系中的坐標(biāo)位置，c₁表示剛體運(yùn)動(dòng)前第1個(gè)未知節(jié)點(diǎn)在局部參考坐標(biāo)系中的坐標(biāo)位置，i為正整數(shù)，1≤i≤N，c_i表示剛體運(yùn)動(dòng)前第i個(gè)未知節(jié)點(diǎn)在局部參考坐標(biāo)系中的坐標(biāo)位置，c_N表示剛體運(yùn)動(dòng)前第N個(gè)未知節(jié)點(diǎn)在局部參考坐標(biāo)系中的坐標(biāo)位置；

步驟二：使剛體運(yùn)動(dòng)，將剛體運(yùn)動(dòng)后N個(gè)未知節(jié)點(diǎn)在全局參考坐標(biāo)系中的坐標(biāo)位置對(duì)應(yīng)記為s₁,...,s_i,...,s_N；然后獲取每個(gè)未知節(jié)點(diǎn)到各個(gè)錨節(jié)點(diǎn)的測(cè)量距離，將第i個(gè)未知節(jié)點(diǎn)到第m個(gè)錨節(jié)點(diǎn)的測(cè)量距離記為r_mi；其中，s₁表示剛體運(yùn)動(dòng)后第1個(gè)未知節(jié)點(diǎn)在全局參考坐標(biāo)系中的坐標(biāo)位置，s_i表示剛體運(yùn)動(dòng)后第i個(gè)未知節(jié)點(diǎn)在全局參考坐標(biāo)系中的坐標(biāo)位置，s_N表示剛體運(yùn)動(dòng)后第N個(gè)未知節(jié)點(diǎn)在全局參考坐標(biāo)系中的坐標(biāo)位置；