一、三維空間定位原理

假設我們知道自身到定位基站的距離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我們可以寫成矩陣的形式如下

這裡我們令為矩陣A。為矩陣B。既可得到座標為

四、驗證

這裡筆者先使用了CAD繪製了一個座標，再使用MATLAB進行計算和驗證。

clear
clc
A = [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


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

XY =

   46.9941
   51.4751

五、使用C語言實現

採用C語言實現主要是需要用C語言做矩陣的逆和矩陣乘法，具體程式碼如下

//================計算矩陣的逆================//
#include<stdio.h>
#define N 10
double Det(double arcs[N][N],int n)//按第一行展開計算|A|
{
    double ans = 0;
    double temp[N][N];
    int i,j,k;
	double t;
    if(n==1)
    {
        return arcs[0][0];
    }
    for(i=0;i<n;i++)
    {
        for(j=0;j<n-1;j++)
        {
            for(k=0;k<n-1;k++)
            {
                temp[j][k] = arcs[j+1][(k>=i)?k+1:k];

            }
        }
        t = Det(temp,n-1);
        if(i%2==0)
        {
            ans += arcs[0][i]*t;
        }
        else
        {
            ans -=  arcs[0][i]*t;
        }
    }
    return ans;
}
void Minor(double arcs[N][N],double ans[N][N],int n)//計算每一行每一列的每個元素所對應的餘子式，組成A*
{
	int i,j,k,t;
    double temp[N][N];
    if(n==1)
    {
        ans[0][0] = 1;
        return;
    }
    for(i=0;i<n;i++)
    {
        for(j=0;j<n;j++)
        {
            for(k=0;k<n-1;k++)
            {
                for(t=0;t<n-1;t++)
                {
                    temp[k][t] = arcs[k>=i?k+1:k][t>=j?t+1:t];
                }
            }


            ans[j][i]  =  Det(temp,n-1);
            if((i+j)%2 == 1)
            {
                ans[j][i] = - ans[j][i];
            }
        }
    }
}
int main()
{
    double A[N][N] = {{-200,0},{0,-200}};
    double iA[N][N];  
    int i,j;
	double dA;
    dA = Det(A,2);
    Minor(A,iA,2);
    for(i=0;i<2;++i)
    {
        for(j=0;j<2;++j)
        {
            printf("%f ", A[i][j]);
        }
        printf("\n");
    }
    printf("\n");
    for(i=0;i<2;++i)
    {
        for(j=0;j<2;++j)
        {
            printf("%f ",(iA[i][j]/dA));
        }
        printf("\n");
    }
    return 0;
}

//=======================矩陣乘法===================//
#include "stdio.h"

void main()
{
	double Max1[2][2]={{7,8},{1,5}};
	double Max2[2][1]={{0.5},{0.3}};
	double Max3[2][1];
	unsigned char i,j;
	double ans=0;
	for(i=0;i<2;i++)
	{
		for(j=0;j<2;j++)
		{
			ans += Max1[i][j]*Max2[j][0];
		}
		Max3[i][0] = ans;
		ans = 0;
	}
	for(i=0;i<2;i++)
	{
		for(j=0;j<1;j++)
		{
			printf("%f ",Max3[i][j]);
		}
		printf("\n");
	}

}

六、總結

該方法只是筆者的一個想法並沒有具體使用，不知道實際具有誤差的情況下該方法精度如何，待筆者後續測試。筆者水平有限如有紕漏盡請指正。