優化三維空間定位法及C語言快捷實現

2021-10-13 22:59:20 字數 3063 閱讀 7306

假設我們知道自身到定位基站的距離di (i=1 2 3 4…)並且知道定位基站的座標,那麼我們就可以在空間上建立n個以基站座標為圓心,距離為半徑的空間球體,球體方程如下。其中a,b,c為基站座標,r為被定位者距離基站的絕對距離。

假定,我們擁有三個定位基站,那麼我們就可以得到乙個方程組。該方程組最終可以解得兩組實數資料。此時高度無法準確的確定,如果要確定準確的高度,我們就要在引入乙個基站,構成乙個擁有四個方程的方程組。解這個方程組就可以得到我們想要的座標了。

但是這裡就會產生乙個問題:使用微控制器解乙個含有三個引數的二元二次方程組顯然這樣做不太方面,而且如果被定位者過多或基站過多就會導致座標更新延遲等問題。所以筆者基於這一問題對計算進行優化處理。

首先我們先檢測被定位者的相對高度。這裡設定兩個基站a和b,它們的座標只有z軸不同,即一上一下兩個基站如圖所示,此時空間兩個球體相交與空間乙個乙個空間平面,並且該平面平行於絕對平面(地面)。故我們只需要考慮該平面的高度即可

此時我們得到簡化後的方程組為

化簡後聯立得

此時高度為

在第二節我們已經使用了兩個基站,並得到了被定位者的高度。這裡我們在設定兩個基站c和d,並保證c和d與a或b在同一平面。然後我們將被定位者的座標,利用第二節得到的高度投影到acd所在的平面,這樣我們就可以將空間座標轉換到平面來進行計算,即不在考慮z軸的問題。此時我們列出兩個方程

化簡聯立方程組,消去xy的平方項得到方程a

該方程的物理意義及為兩圓在平面中相交所得到的平面直線。同理化簡併聯立1 3 得到方程 b

綜上方程a和方程b我們可以寫成矩陣的形式如下

這裡我們令

這裡筆者先使用了cad繪製了乙個座標,再使用matlab進行計算和驗證。

clear

clca = [0 0];

b = [100 0];

c = [0 100];%定義座標

la = 70;

lb = 74.17;

lc = 67.86;%定義基站距定位點距離

d = [2*a(1)-2*b(1) 2*a(2)-2*b(2);

2*a(1)-2*c(1) 2*a(2)-2*c(2)]; %定義引數矩陣

xy = [0;

0];z = [(lb^2 - la^2)+(a(1)^2 - b(1)^2)+(a(2)^2 - b(2)^2);

(lc^2 - la^2)+(a(1)^2 - c(1)^2)+(a(2)^2 - c(2)^2)];

xy = d^-1 * z

%%**********=執行結果**********====%%

採用c語言實現主要是需要用c語言做矩陣的逆和矩陣乘法,具體**如下

//***************=計算矩陣的逆***************=//

#include#define n 10

double det(double arcs[n][n],int n)//按第一行展開計算|a|

for(i=0;i=i)?k+1:k];}}

t = det(temp,n-1);

if(i%2==0)

else

}return ans;

}void minor(double arcs[n][n],double ans[n][n],int n)//計算每一行每一列的每個元素所對應的余子式,組成a*

for(i=0;i=i?k+1:k][t>=j?t+1:t];}}

ans[j][i] = det(temp,n-1);

if((i+j)%2 == 1)}}

}int main()

,}; double ia[n][n];

int i,j;

double da;

da = det(a,2);

minor(a,ia,2);

for(i=0;i<2;++i)

printf("\n");

}printf("\n");

for(i=0;i<2;++i)

printf("\n");

}return 0;

}

//********************===矩陣乘法***************====//

#include "stdio.h"

void main()

,}; double max2[2][1]=,};

double max3[2][1];

unsigned char i,j;

double ans=0;

for(i=0;i<2;i++)

max3[i][0] = ans;

ans = 0;

} for(i=0;i<2;i++)

printf("\n");

}}

三維空間剛體旋轉

剛體 運動過程中不會產生形變的物體,運動過程中同乙個向量的長度和夾角都不會發生變化。剛體變換也稱為歐式變換。旋轉矩陣 四元數旋轉向量 尤拉角安裝方式 eigen庫只有標頭檔案,沒有.so和.a二進位制檔案,所以在cmakelists.txt中只需要新增標頭檔案路徑,並不需要使用target link...

BFS 三維空間迷宮

題面 你被困在乙個三維的空間中,現在要尋找最短路徑逃生!空間由立方體單位構成。每次向上下前後左右移動乙個單位需要一分鐘,且不能對角線移動。空間的四周封閉。你的目標是走到空間的出口。問是否存在逃出生天的可能性?如果存在,則需要多少時間?輸入第一行是乙個數表示空間的數量。每個空間的描述的第一行為l,r和...

三維空間和四維空間

從螞蟻走路可以看出。由於螞蟻受限於自己的身體沒有翅膀,就跟人沒有翅膀一樣,碰到吊在自己頭頂的東西就只能繞遠路通過二維的方法到達三維的地方,所以只要有工具,可以是翅膀,可以是深深固定地上的梯子,也可以是飛機,都可以從受限的二維空間達到三維空間。而且這些手段全都是為了用來克服重力的,所以重力跟第三維有不...