定位系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)

1、定位系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)本文實(shí)現(xiàn)的定位系統(tǒng)針對(duì)室外環(huán)境及辦公室環(huán)境的實(shí)現(xiàn)的定位應(yīng)用，通過對(duì)場(chǎng)景中人員、物品進(jìn)行定位，方便用戶對(duì)目標(biāo)的實(shí)時(shí)監(jiān)測(cè)和管理。上位機(jī)開發(fā)環(huán)境為MFC(MicrosoftFoundationClasses),它是一個(gè)微軟公司提供的類庫，以C+類的形式封裝了WindowsAPI,并且包含一個(gè)應(yīng)用程序框架， 以減少應(yīng)用程序開發(fā)人員的工作量。其中包含的類包含大量Windows句柄封裝類和很多Windows的內(nèi)建控件和組件的封裝類。它的詳細(xì)介紹在這里就不贅述了。下面對(duì)本定位系統(tǒng)的具體實(shí)現(xiàn)過程進(jìn)行比較詳細(xì)的描述。1.1定位系統(tǒng)結(jié)構(gòu)定位系統(tǒng)拓?fù)浣Y(jié)構(gòu)圖如圖1-1所示。其實(shí)際場(chǎng)景應(yīng)用可以參考圖

2、1-2。本拓?fù)渲杏腥N功能不同類型的節(jié)點(diǎn)：主錨節(jié)點(diǎn)、從錨節(jié)點(diǎn)、目的節(jié)點(diǎn)。其中主錨節(jié)點(diǎn)和從錨節(jié)點(diǎn)是一類已知自身位置坐標(biāo)信息的固定節(jié)點(diǎn)，它們的任務(wù)是獲取包含距離信息以及信號(hào)強(qiáng)度值的數(shù)據(jù)包。目的節(jié)點(diǎn)是可以自由移動(dòng)的節(jié)點(diǎn)，可以在一定范圍內(nèi)自由移動(dòng)，定位系統(tǒng)的最終目的就是獲取該類節(jié)點(diǎn)位置的坐標(biāo)信息；從錨節(jié)點(diǎn)負(fù)責(zé)發(fā)起測(cè)距請(qǐng)求，將測(cè)距信息發(fā)往目的節(jié)點(diǎn)，得到從錨節(jié)點(diǎn)與目的節(jié)點(diǎn)的距離信息以及信號(hào)強(qiáng)度信息，并將該信息發(fā)往主錨節(jié)點(diǎn)。主錨節(jié)點(diǎn)在整個(gè)系統(tǒng)中起著至關(guān)重要的作用，首先它要響應(yīng)上位機(jī)發(fā)出的命令，確定要定位的目標(biāo)，并發(fā)出指令，命令從錨節(jié)點(diǎn)對(duì)目標(biāo)節(jié)點(diǎn)進(jìn)行測(cè)距；其次主錨節(jié)點(diǎn)不僅要獲取自身與目標(biāo)節(jié)點(diǎn)的距離信息以及信號(hào)

3、強(qiáng)度信息，還需要協(xié)調(diào)控制三個(gè)從錨節(jié)點(diǎn)，接受從錨節(jié)點(diǎn)發(fā)回的測(cè)距信息；最后主錨節(jié)點(diǎn)負(fù)責(zé)并將這些信息送還給上位機(jī)軟件處理。目的節(jié)點(diǎn)實(shí)時(shí)監(jiān)測(cè)是否有定位請(qǐng)求并配合錨節(jié)點(diǎn)測(cè)出響應(yīng)的距離信息以及信號(hào)強(qiáng)度信息；本定位系統(tǒng)主要包括四大塊：硬件節(jié)點(diǎn)、硬件節(jié)點(diǎn)軟件、上位機(jī)監(jiān)測(cè)軟件以及顯示模塊。每一部分設(shè)計(jì)都有其特殊性，硬件節(jié)點(diǎn)為系統(tǒng)提供了定位所需的硬件平臺(tái)，是信息采集的基礎(chǔ)，第三章已經(jīng)詳細(xì)介紹其性能，這里不再贅述。硬件節(jié)點(diǎn)軟件主要完成了節(jié)點(diǎn)間的鏈路建立和數(shù)據(jù)采集與傳輸。上位機(jī)軟件首先通過有線方式（使用串口）接收由定位硬件節(jié)點(diǎn)采集到的相關(guān)信息，然后對(duì)所采集的定位信息進(jìn)行處理，最后選用合適的定位算法，計(jì)算出目標(biāo)節(jié)點(diǎn)在該

4、參考系的坐標(biāo)顯示模塊負(fù)責(zé)動(dòng)態(tài)顯示節(jié)點(diǎn)定位效果，免去人工思考數(shù)字坐標(biāo)所代表的具體意義，是定位軟件人性化設(shè)計(jì)的一個(gè)體現(xiàn)。圖1-3為系統(tǒng)整體程序流程圖；圖1-3系統(tǒng)整體程序流程圖讀取處理后的測(cè)距結(jié)果顯示到界面根據(jù)以上數(shù)據(jù)由軟件建立坐標(biāo)系主錨節(jié)點(diǎn)監(jiān)測(cè)命令，如果是定位指令，發(fā)出定位指令內(nèi)需做在中好框作內(nèi)需做在中好框作前并件置虛工提好前并件置虛工提好軟設(shè)軟設(shè)卜面分別詳細(xì)介紹余下的三個(gè)模塊本定位系統(tǒng)的基本思想是主錨節(jié)點(diǎn)收集所有的從錨節(jié)點(diǎn)到目標(biāo)節(jié)點(diǎn)以及自身到目標(biāo)節(jié)點(diǎn)的距離信息以及信號(hào)強(qiáng)度(RSSI),由主錨節(jié)點(diǎn)將信息傳送到上位機(jī)監(jiān)測(cè)軟件，上位機(jī)軟件調(diào)用相應(yīng)的定位算法，計(jì)算出目標(biāo)節(jié)點(diǎn)的坐標(biāo)位置并實(shí)現(xiàn)必要的顯示。

5、因此，首先需要在硬件平臺(tái)上實(shí)現(xiàn)節(jié)點(diǎn)間的通信，本文采用星形拓?fù)浣Y(jié)構(gòu)，這種網(wǎng)絡(luò)拓?fù)浣M建簡(jiǎn)單，并且便于數(shù)據(jù)采集。主錨節(jié)點(diǎn)擔(dān)當(dāng)中心節(jié)點(diǎn)的角色，以輪詢的方式發(fā)送測(cè)距命令，這樣實(shí)際在同一時(shí)間段內(nèi)只有一對(duì)節(jié)點(diǎn)在工作。避免了因沖突造成的處理時(shí)延單方向增加，導(dǎo)致定位結(jié)果不準(zhǔn)確?？蓞⒖紙D1-1。因?yàn)楸鞠到y(tǒng)采用的nanoPAN5032模塊自身并沒有組網(wǎng)功能。所以需要作者自己重新編寫代碼以實(shí)現(xiàn)組網(wǎng)功能。通信流程如圖1-4:圖1-4通信流程圖其具體實(shí)現(xiàn)過程如下：(1)主錨節(jié)點(diǎn)接受定位命令。主錨節(jié)點(diǎn)要獲取節(jié)點(diǎn)的位置信息，就要循環(huán)給所有從錨節(jié)點(diǎn)發(fā)送測(cè)距請(qǐng)求信號(hào)，通信方式為輪詢方式。保證所有的從錨節(jié)點(diǎn)都可收到該測(cè)距請(qǐng)求。反復(fù)

6、獲取所有錨節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)的距離以及信號(hào)強(qiáng)度信息；(2)從錨節(jié)點(diǎn)收到測(cè)距請(qǐng)求。從錨節(jié)點(diǎn)向目標(biāo)節(jié)點(diǎn)發(fā)送測(cè)距請(qǐng)求，目標(biāo)節(jié)點(diǎn)如果在該區(qū)域內(nèi)，收到該請(qǐng)求就與該錨節(jié)點(diǎn)發(fā)生點(diǎn)到點(diǎn)的通信，經(jīng)過一個(gè)SDS-TWR測(cè)距過程以后，從錨節(jié)點(diǎn)將獲取到的相應(yīng)的距離信息以及信號(hào)強(qiáng)度信息發(fā)送給主錨節(jié)點(diǎn)；(3)主錨節(jié)點(diǎn)一旦受到測(cè)距信息，就將這些信息傳到上位機(jī)監(jiān)測(cè)軟件中，由檢測(cè)軟件的緩存暫時(shí)保存這些數(shù)據(jù)，以便以后的定位計(jì)算；主錨節(jié)點(diǎn)除了要完成從錨節(jié)點(diǎn)的主體功能外，還負(fù)責(zé)整個(gè)通信的協(xié)調(diào)工作以及將數(shù)據(jù)傳送給上位機(jī)，給定位軟件提供所需的定位信息。下面簡(jiǎn)要介紹下主錨節(jié)點(diǎn)的程序設(shè)計(jì)。圖1-5所示為主錨節(jié)點(diǎn)的軟件流程圖。其中，系統(tǒng)初始化主要

7、是系統(tǒng)的一些硬件管腳、串口、時(shí)鐘等進(jìn)行初始化配置。然后進(jìn)入死循環(huán)，監(jiān)測(cè)上位機(jī)命令。監(jiān)測(cè)命令主要通過讀取串口信息來實(shí)現(xiàn)，如果收到信息，首先進(jìn)行命令格式檢測(cè)，如果正確，再判斷是什么命令。假如為定位命令，則自身發(fā)出測(cè)距請(qǐng)求然后命令其余三個(gè)錨節(jié)點(diǎn)依次對(duì)目標(biāo)節(jié)點(diǎn)發(fā)出測(cè)距請(qǐng)求， 這種拓?fù)浣Y(jié)構(gòu)不僅很容易搭建，而且也避免由于處理沖突導(dǎo)致處理時(shí)延增大，導(dǎo)致測(cè)距結(jié)果變大；intmain(void)RCC_Configuration();/*Configurethesystemclocks*/圖1-5主錨節(jié)點(diǎn)的流程圖GPIO_Configuration();/*ConfiguretheGPIOs*/USART_Co

8、nfiguration();/*ConfiguretheUSART1*/SysTick_Config();/*Configurethesystick*/nano_main(NULL,NULL);其中nano_main()部分代碼如下：intnano_main(intac,char*av口)while(1)while(SendTimingDelay0)/限定最大發(fā)送測(cè)距請(qǐng)求次數(shù)receivedate=0;/是否收到數(shù)據(jù)的狀態(tài)PollApplication();Delay(200);NTRXUpdate();if(receivedate=1)break;SendTimingDelay-;SendT

9、imingDelay=3;for(;inodenumber;i+)while(SendTimingDelay!=0)receivedate=0;Appsendmessage(addri,payloadi,10);Delay(200);NTRXReceivecommand();NVIC_Configuration();/*NVICConfiguration*/if(receivedate=1)break;SendTimingDelay-;)SendTimingDelay=3;)1.2.2從錨節(jié)點(diǎn)程序設(shè)計(jì)其中，系統(tǒng)初始化和主錨節(jié)點(diǎn)一樣。進(jìn)入死循環(huán)，監(jiān)測(cè)來自主錨節(jié)點(diǎn)的命令;如果收到信息，首先進(jìn)行命令

10、格式檢測(cè)，如果正確，再判斷是什么命令。假如為定位命令，則對(duì)目標(biāo)節(jié)點(diǎn)發(fā)出測(cè)距請(qǐng)求。將得到的結(jié)果封包發(fā)送給主錨節(jié)點(diǎn)；1.2.3目標(biāo)節(jié)點(diǎn)程序設(shè)計(jì)下面簡(jiǎn)要介紹下從錨節(jié)點(diǎn)的程序設(shè)計(jì)。圖1-7為目標(biāo)節(jié)點(diǎn)的流程圖圖1-7目標(biāo)節(jié)點(diǎn)的流程圖其中，系統(tǒng)初始化和主錨節(jié)點(diǎn)一樣。進(jìn)入死循環(huán)，監(jiān)測(cè)來自錨節(jié)點(diǎn)的測(cè)距命令；如果收到信息。首先進(jìn)行命令格式檢測(cè)，如果正確，就繼續(xù)和錨節(jié)點(diǎn)通信，開始測(cè)距任務(wù)，并由錨節(jié)點(diǎn)收集測(cè)距信息。1.3上位機(jī)軟件設(shè)計(jì)定位系統(tǒng)上位機(jī)監(jiān)控軟件中，主要包括以下幾個(gè)功能：串口通信、數(shù)據(jù)處理、定位計(jì)算等三個(gè)主要模塊。串口通信是上位機(jī)監(jiān)測(cè)與控制硬件節(jié)點(diǎn)的唯一通道。所有的指令發(fā)出以及信息獲取都是通過串口來實(shí)現(xiàn)的

11、；數(shù)據(jù)處理主要負(fù)責(zé)將用口收集到的原始數(shù)據(jù)加以處理，最大程度還原真實(shí)的距離信息，以便定位算法能夠算出準(zhǔn)確的目的節(jié)點(diǎn)坐標(biāo)；定位算法主要負(fù)責(zé)將處理過的數(shù)據(jù)轉(zhuǎn)換為參考坐標(biāo)系的坐標(biāo)。這里的參考坐標(biāo)系是建立在人工測(cè)定的幾個(gè)錨節(jié)點(diǎn)的相對(duì)位置的基礎(chǔ)上的，下面分別介紹這三個(gè)模塊.串口通信模塊串口通信是連接硬件節(jié)點(diǎn)和上位機(jī)軟件的唯一樞紐，用戶的定位指令以及硬件上傳的測(cè)距信息都是通過串口傳輸?shù)缴衔粰C(jī)軟件中來的。為了方便程序的書寫，這里將用口封裝稱為一個(gè)類classCSerialPort；該類能夠完成對(duì)串口的初始化，打開，讀寫，事件等待等功能MFC的一大特色是消息響應(yīng)機(jī)制，在本類中，為了完成通信，作者定義以下消息:#

12、defineWM_COMM_BREAK_DETECTED#defineWM_COMM_CTS_DETECTED#defineWM_COMM_DSR_DETECTED#defineWM_COMM_ERR_DETECTED#defineWM_COMM_RING_DETECTED#defineWM_COMM_RLSD_DETECTED#defineWM_COMM_RXCHAR#defineWM_COMM_RXFLAG_DETECTED#defineWM_COMM_TXEMPTY_DETECTED然后在類ESAP中使用afx_msgLONGOnCommunication(WPARAMLPARAMpor

13、t);獲取用口送上來的消息。因?yàn)榇谑且粋€(gè)一個(gè)字符的形式將消息上傳上來，故這里響應(yīng)的是其WPARAMch,當(dāng)程序遇到“n并且之前接受的數(shù)據(jù)長(zhǎng)度大于10的時(shí)候，認(rèn)為有新的數(shù)據(jù)包過來，否則丟棄掉。數(shù)據(jù)包格式如表1-1,其中第一行為包格式，第二行為樣例包。各個(gè)數(shù)據(jù)以空格符號(hào)分開，以rn結(jié)尾。表1-1主錨節(jié)點(diǎn)上傳數(shù)據(jù)包格式SerialNumberAnchorAddressTagAddressDistanceRSSI171112.3650得到完整數(shù)據(jù)包后將數(shù)據(jù)包封裝成結(jié)構(gòu)體Result_Node,以節(jié)點(diǎn)的形式加入處理緩存隊(duì)列，方便以后數(shù)據(jù)處理操作，其中結(jié)構(gòu)體定義如下：structResult_Nodei

14、ntserial_number;chardest_addr3;charsrc_addr3;floatdistance;charrssi3;WM_USER+1WM_USER+2WM_USER+3WM_USER+4WM_USER+5WM_USER+6WM_USER+7WM_USER+8WM_USER+9ch,為達(dá)到對(duì)節(jié)點(diǎn)的控制的目的,只需在主程序中調(diào)用afx_msgLONGOnCommadToSend(WPARAMwparam,LPARAMcmd);為作者設(shè)計(jì)的一個(gè)指令包， 包格式如表1-2:其中第一行為包格式，第二行為樣例包。表1-2指令包格式HeadCmdTagReservedTail0 x0

15、0010 x00110 x00110 x00000 x1000因?yàn)镻C機(jī)有太多不安全因素，命令包并沒有像數(shù)據(jù)包那樣沒有包頭包尾，Head和Tail固定為0 x0001和0 x1000。 主錨節(jié)點(diǎn)收到數(shù)據(jù)包后首先對(duì)數(shù)據(jù)包解包， 如果格式不正確。就丟棄，以免因?yàn)槠渌僮?，?dǎo)致數(shù)據(jù)傳入主錨節(jié)點(diǎn)，使得系統(tǒng)不穩(wěn)定。Cmd表示是何種命令，暫時(shí)只定義兩種命令，測(cè)距開始命令和測(cè)距停止命令。RANGINGSTART0 x0011RANGINGSTOP0 x1100Tag為要測(cè)定的目標(biāo)節(jié)點(diǎn)， 它可以為0 x00010 x00ff中任意不同于四個(gè)錨節(jié)點(diǎn)的地址的數(shù)。主錨節(jié)點(diǎn)根據(jù)這一位來確定用戶想要定位的目標(biāo)。如果Cm

16、d為0 x1100,即停止測(cè)距指令，則這一位置應(yīng)該置0 x0000;Reserbed為保留位，暫時(shí)定為0 x0000，這方便以后命令的擴(kuò)展等功能。數(shù)據(jù)處理模塊現(xiàn)在緩存中存放的是一堆Result_Node節(jié)點(diǎn)，但是這些數(shù)據(jù)并沒有被處理，不能直接應(yīng)用于定位算法中。數(shù)據(jù)處理模塊負(fù)責(zé)把這些數(shù)據(jù)分類、排錯(cuò)、濾波、還原。為后續(xù)的定位計(jì)算準(zhǔn)備數(shù)據(jù)。其處理流程如圖1-8:圖1-8數(shù)據(jù)處理流程圖首先從Buff中讀取一個(gè)節(jié)點(diǎn)，因?yàn)橛布呀?jīng)對(duì)數(shù)據(jù)進(jìn)行平滑處理，由于剛開始的時(shí)候平滑數(shù)組所有值都為0,前十個(gè)上傳的信息都被乘上一個(gè)平滑系數(shù)。為了還原這些數(shù)據(jù)，只需判定其serialnumber位是否小于10,然后進(jìn)行相應(yīng)的

17、還原處理。 為了區(qū)分不同錨節(jié)點(diǎn)的距離信息，作者為每個(gè)節(jié)點(diǎn)分配一個(gè)緩存隊(duì)列，對(duì)處理完的距離信息分別加入對(duì)應(yīng)的緩存隊(duì)列，并將緩存隊(duì)列狀態(tài)設(shè)置為非空狀態(tài)。根據(jù)第三章分析處理得出的結(jié)論，根據(jù)不同的情況對(duì)進(jìn)入緩存隊(duì)列的數(shù)據(jù)進(jìn)行相應(yīng)的處理校正。首先根據(jù)新進(jìn)的數(shù)據(jù)與上次數(shù)據(jù)相比較，利用物體在室內(nèi)運(yùn)動(dòng)速度有上限這一前提，制作一個(gè)平滑濾波器，濾掉因外界干擾造成的測(cè)距誤差。然后根據(jù)軟件的場(chǎng)景設(shè)置選擇不同的處理方法，軟件給出靜態(tài)目標(biāo)定位和動(dòng)態(tài)目標(biāo)定位、視距可達(dá)定位和視距不可達(dá)定位四種情況，如果定位人員不知道具體場(chǎng)景，可按照默認(rèn)的選項(xiàng)進(jìn)行定位。軟件會(huì)根據(jù)RSSI值，測(cè)距結(jié)果對(duì)進(jìn)入緩存隊(duì)列的數(shù)據(jù)進(jìn)行處理。最后對(duì)處理完的

18、數(shù)據(jù)進(jìn)行平滑濾波，并將結(jié)果保存，以便后續(xù)計(jì)算模塊使用。上述處理過程被封裝進(jìn)一個(gè)classCDataPro中，并給出接口函數(shù)doubledata_processing（doubleranging_distance,doublerssi_in,intobject,intobstacle程序中只需調(diào)用該函數(shù)， 就可以完成對(duì)數(shù)據(jù)的處理。定位計(jì)算模塊上面數(shù)據(jù)處理已經(jīng)完成，當(dāng)定位模塊檢測(cè)到四個(gè)緩存隊(duì)列的狀態(tài)都是非空狀態(tài)，即滿足定位計(jì)算的基本數(shù)據(jù)要求，就讀取其中的數(shù)據(jù)進(jìn)行定位計(jì)算。要實(shí)現(xiàn)定位就需要將這些距離信息轉(zhuǎn)換為相對(duì)于坐標(biāo)系的坐標(biāo)信息，這才能給人一個(gè)比較直觀的感覺。這涉及到三個(gè)工作：定位坐標(biāo)系的建立，目

19、標(biāo)節(jié)點(diǎn)的計(jì)算，算法的程序?qū)崿F(xiàn)；a）坐標(biāo)系的建立任何一個(gè)定位系統(tǒng)都有自己的參考坐標(biāo)系，本系統(tǒng)的參考坐標(biāo)系是根據(jù)實(shí)際場(chǎng)景生成的相對(duì)坐標(biāo)系。為方便描述，這里約定主錨節(jié)點(diǎn)的物理位置為A、其余三個(gè)錨節(jié)點(diǎn)為B、C、D,如圖1-9所示。首先需人工測(cè)量出A、B、C、D四個(gè)點(diǎn)離參考平面高度H_A、H_B、H_C、H_D,如果是采用二維定位，可簡(jiǎn)單將這四個(gè)值賦值為零。然后測(cè)出四個(gè)點(diǎn)兩兩之間的距離AB,AC,AD,BC,BD,CD?？偣彩畟€(gè)數(shù)據(jù)；初始化A,B,C,D四個(gè)點(diǎn)Z坐標(biāo)為Za=0,Zb=H_B-H_A,Zc=H_C-H_A,Zd=H_D-H_A則可設(shè)四點(diǎn)坐標(biāo)為A(0,0,0),B(Xb,0,H_B-H_A)

20、,C(Xc,Yc,H_C-H_A),D(Xd,Yd,H_A-H_C);很容易推出：XbVAB2(H_BH_A)2_2_22Xc(AC2AB2BC22ZcZb)/(2Xb)YcAC2Cx2Zc2(因?yàn)锳C要與Y軸的正向方向夾角為銳角，所以這里Yc取正值。)Xd(AD2AB2BD22ZdZb)/(2Xb)222_Yd(AD2AC2CD22XdXc2ZdZc)/(2Yc)由此可以定出唯一坐標(biāo)系。為后續(xù)計(jì)算提供參考位置。b)目標(biāo)節(jié)點(diǎn)的計(jì)算目前有很多成熟的定位算法，每種定位算法都有各自的優(yōu)缺點(diǎn)，因?yàn)楸鞠到y(tǒng)設(shè)計(jì)的需要：既支持二維定位，也支持三維定位，故本文有兩種定位算法。一種是Chan算法，另一種是由Ch

21、an算法演變而來的三維定位算法。C(Xc,Yc,H_C-H_A)B(Xb,0,HB-HA)A0,0,0)D(Xd,Yd,HD-HA)圖1-9坐標(biāo)系建立示意圖二維定位中，選取三個(gè)參考點(diǎn)A、B、C,三個(gè)點(diǎn)到目標(biāo)點(diǎn)的距離分別為Ra,Rb,Rco最好的定位結(jié)果圖如圖4-100這時(shí)可以簡(jiǎn)單的利用三個(gè)二元二次方程組求解出目標(biāo)點(diǎn)坐標(biāo)。圖1-10Chan定位算法示意圖當(dāng)出現(xiàn)測(cè)距誤差時(shí)，Ra,Rb,Rc會(huì)出現(xiàn)不同程度的變大或者縮小。例如三個(gè)圓的半徑都比實(shí)際值小，那么三個(gè)圓就沒有交點(diǎn)，但是這并不意味著二元二次方程沒有解。此時(shí)求出的坐標(biāo)為任意兩圓的根軸的交點(diǎn)坐標(biāo)，如圖1-11所示：圖i-ii三個(gè)圓沒有交點(diǎn)的解由于本

22、系統(tǒng)有四個(gè)錨節(jié)點(diǎn)，這就意味著有一個(gè)冗余，可以利用這個(gè)冗余數(shù)據(jù)計(jì)算目標(biāo)節(jié)點(diǎn)坐標(biāo)，具體做法為：四個(gè)距離信息中任意取出三個(gè)來定位,這樣可以計(jì)算出4個(gè)目標(biāo)節(jié)點(diǎn)。 然后取四個(gè)目標(biāo)節(jié)點(diǎn)所組成的四邊形的質(zhì)心為最終定位目標(biāo)節(jié)點(diǎn)。三維定位中，類似于二維定位算法?？梢粤谐鲆粋€(gè)三元二次方程組：2(Xa-X)2(YaY)2(ZaZ)2Ra22(Xb-X)2(YbY)2(ZbZ)2Rb2(1-1)(Xc-X)2(YcY)2(ZcZ)2Rc22(Xd-X)2(YdY)2(ZdZ)2Rd2可得出一個(gè)二TIT次方程組:2XbX2YbY2ZbZRa2Rb2AB22XcX2YcY2ZcZRa2Rc2AC2(1-2)2XdX2Yd

23、Y2ZdZRa2Rd2AD2求解上述方程組即可得出目標(biāo)點(diǎn)的三維坐標(biāo)。這里為方便編程，采用矩陣方式求解，上三元一次方程組可化為AB=C,其中A,X,C均為矩陣,同樣，假如Ra,Rb,Rc,Rd因?yàn)闇y(cè)量的誤差而大于實(shí)際值，那么所求的交點(diǎn)為他們根面的交點(diǎn)；c)程序?qū)崿F(xiàn)將坐標(biāo)系封裝為一個(gè)classCoordinate內(nèi)部封裝了坐標(biāo)系的初始化，坐標(biāo)系的建立，二維坐標(biāo)計(jì)算，三維坐標(biāo)計(jì)算等函數(shù)。Public:Pointcalculate_2d(Pointa,Pointb,Pointc,DISTANCEn2a,DISTANCEn2b,DISTANCEn2c);Pointcalculate_2d(DISTANCEn2a,DISTANCEn2b,DISTANCEn2c,DISTANCEn2d);voidinit(DISTANCEab,DISTANCEac,DISTANCEbc,DISTANCEad,DISTANCEbd,DISTANCEcd,HEIGHTha,HEIGHThb,HEIGHThc,HEIGHThd);voidcreatCoordinate();Pointcalculate_3d(DISTANCEn2a,DISTANCEn2b,DISTANCEn2c,DISTANCEn2d);DISTANCEP2P(Pointa,Pointb);2Xb2Yb2