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

監(jiān)控點(diǎn)KeyAnchorAnchor定位系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)監(jiān)控點(diǎn)KeyAnchorAnchor本文實(shí)現(xiàn)的定位系統(tǒng)針對(duì)室外環(huán)境及辦公室環(huán)境的實(shí)現(xiàn)的定位應(yīng)用，通過(guò)對(duì)場(chǎng)景中人員、物品進(jìn)行定位，方便用戶對(duì)目標(biāo)的實(shí)時(shí)監(jiān)測(cè)和管理。上位機(jī)開發(fā)環(huán)境為MFC(MicrosoftFoundationClasses)，它是一個(gè)微軟公司提供的類庫(kù)，以C++類的形式封裝了WindowsAPI，并且包含一個(gè)應(yīng)用程序框架，以減少應(yīng)用程序開發(fā)人員的工作量。其中包含的類包含大量Windows句柄封裝類和很多Windows的內(nèi)建控件和組件的封裝類。它的詳細(xì)介紹在這里就不贅述了。下面對(duì)本定位系統(tǒng)的具體實(shí)現(xiàn)過(guò)程進(jìn)行比較詳細(xì)的描述。1.1定位系統(tǒng)結(jié)構(gòu)定位系統(tǒng)拓?fù)浣Y(jié)構(gòu)圖如圖1—1所示。其實(shí)際場(chǎng)景應(yīng)用可以參考圖1—2。ANCHOR串口代理傳輸測(cè)距結(jié)果圖1—1定位系統(tǒng)拓?fù)浣Y(jié)構(gòu)圖ANCHOR串口代理傳輸測(cè)距結(jié)果KEYANCHOR串行控制線測(cè)距鏈路定位目標(biāo)，內(nèi)置TAG點(diǎn)本地定位終端圖1—2實(shí)際應(yīng)用場(chǎng)景圖本拓?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)強(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ī)軟件首先通過(guò)有線方式（使用串口）接收由定位硬件節(jié)點(diǎn)采集到的相關(guān)信息，然后對(duì)所采集的定位信息進(jìn)行處理，最后選用合適的定位算法，計(jì)算出目標(biāo)節(jié)點(diǎn)在該參考系的坐標(biāo)。顯示模塊負(fù)責(zé)動(dòng)態(tài)顯示節(jié)點(diǎn)定位效果，免去人工思考數(shù)字坐標(biāo)所代表的具體意義，是定位軟件人性化設(shè)計(jì)的一個(gè)體現(xiàn)。圖1-3為系統(tǒng)整體程序流程圖;Buff人工測(cè)出四個(gè)ANCHOR點(diǎn)之間的距離根據(jù)以上數(shù)據(jù)由軟件建立坐標(biāo)系虛框內(nèi)工作需提前做好并在Buff人工測(cè)出四個(gè)ANCHOR點(diǎn)之間的距離根據(jù)以上數(shù)據(jù)由軟件建立坐標(biāo)系虛框內(nèi)工作需提前做好并在軟件中設(shè)置好圖1-3系統(tǒng)整體程序流程圖下面分別詳細(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)必要的顯示。因此，首先需要在硬件平臺(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)確。可參考圖1-1。因?yàn)楸鞠到y(tǒng)采用的nanoPAN5032模塊自身并沒(méi)有組網(wǎng)功能。所以需要作者自己重新編寫代碼以實(shí)現(xiàn)組網(wǎng)功能。通信流程如圖1-4：圖1—4通信流程圖其具體實(shí)現(xiàn)過(guò)程如下：主錨節(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ù)獲取所有錨節(jié)點(diǎn)與目標(biāo)節(jié)點(diǎn)的距離以及信號(hào)強(qiáng)度信息；從錨節(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)過(guò)一個(gè)SDS-TWR測(cè)距過(guò)程以后，從錨節(jié)點(diǎn)將獲取到的相應(yīng)的距離信息以及信號(hào)強(qiáng)度信息發(fā)送給主錨節(jié)點(diǎn)；主錨節(jié)點(diǎn)一旦受到測(cè)距信息，就將這些信息傳到上位機(jī)監(jiān)測(cè)軟件中，由檢測(cè)軟件的緩存暫時(shí)保存這些數(shù)據(jù)，以便以后的定位計(jì)算；

1。2。1主錨節(jié)點(diǎn)程序設(shè)計(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)的軟件流程圖。等待上位機(jī)命令等待上位機(jī)命令圖1—5主錨節(jié)點(diǎn)的流程圖其中，系統(tǒng)初始化主要是系統(tǒng)的一些硬件管腳、串口、時(shí)鐘等進(jìn)行初始化配置。然后進(jìn)入死循環(huán)，監(jiān)測(cè)上位機(jī)命令。監(jiān)測(cè)命令主要通過(guò)讀取串口信息來(lái)實(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()；RCC_Configuration()；AConfigurethesystemclocks大/NVIC_Configuration()NVIC_Configuration();GPIO_Configuration()；大/USART_Configuration();/SysTick_Config();//*NVICConfiguration大/AConfiguretheGPIOs/ConfiguretheUSART1A/＊Configurethesystick＊nanomain(NULLNULL)；}其中nano_main()部分代碼如下：intnano_main(intac,charAav[]){while(1){while(SendTimingDelay〉0)//限定最大發(fā)送測(cè)距請(qǐng)求次數(shù){receivedate=0；//是否收到數(shù)據(jù)的狀態(tài)PollApplication();Delay(200);NTRXUpdate();if(receivedate==1)break;SendTimingDelay—;}SendTimingDelay=3;for(；i〈nodenumber;i++){while(SendTimingDelay!=0){receivedate=0；Appsendmessage(addr[i],payload[i],10);Delay(200)；NTRXReceivecommand();if(receivedate==1)break；SendTimingDelay--；}SendTimingDelay=3；}}1。2。2從錨節(jié)點(diǎn)程序設(shè)計(jì)下面簡(jiǎn)要介紹下從錨節(jié)點(diǎn)的程序設(shè)計(jì)。圖1-6為從錨節(jié)點(diǎn)的流程圖。

圖1-6從錨節(jié)點(diǎn)的流程圖將信息重新封包圖1-6從錨節(jié)點(diǎn)的流程圖將信息重新封包發(fā)送給主錨節(jié)點(diǎn)對(duì)目標(biāo)節(jié)點(diǎn)測(cè)距其中,系統(tǒng)初始化和主錨節(jié)點(diǎn)一樣.進(jìn)入死循環(huán)，監(jiān)測(cè)來(lái)自主錨節(jié)點(diǎn)的命令；如果收到信息，首先進(jìn)行命令格式檢測(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è)來(lái)自錨節(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ā)出以及信息獲取都是通過(guò)串口來(lái)實(shí)現(xiàn)的；數(shù)據(jù)處理主要負(fù)責(zé)將串口收集到的原始數(shù)據(jù)加以處理，最大程度還原真實(shí)的距離信息，以便定位算法能夠算出準(zhǔn)確的目的節(jié)點(diǎn)坐標(biāo);定位算法主要負(fù)責(zé)將處理過(guò)的數(shù)據(jù)轉(zhuǎn)換為參考坐標(biāo)系的坐標(biāo)。這里的參考坐標(biāo)系是建立在人工測(cè)定的幾個(gè)錨節(jié)點(diǎn)的相對(duì)位置的基礎(chǔ)上的,下面分別介紹這三個(gè)模塊。1。3.1串口通信模塊串口通信是連接硬件節(jié)點(diǎn)和上位機(jī)軟件的唯一樞紐，用戶的定位指令以及硬件上傳的測(cè)距信息都是通過(guò)串口傳輸?shù)缴衔粰C(jī)軟件中來(lái)的。為了方便程序的書寫，這里將串口封裝稱為一個(gè)類classCSerialPort；該類能夠完成對(duì)串口的初始化，打開，讀寫，事件等待等功能。MFC的一大特色是消息響應(yīng)機(jī)制，在本類中，為了完成通信，作者定義以下消息：#defineWM_COMM_BREAK_DETECTEDWM_USER+1#defineWM_COMM_CTS_DETECTEDWM_USER+2#defineWM_COMM_DSR_DETECTEDWM_USER+3#defineWM_COMM_ERR_DETECTEDWM_USER+4#defineWM_COMM_RING_DETECTEDWM_USER+5#defineWM_COMM_RLSD_DETECTEDWM_USER+6#defineWM_COMM_RXCHARWM_USER+7#defineWM_COMM_RXFLAG_DETECTEDWM_USER+8#defineWMCOMMTXEMPTYDETECTEDWMUSER+9然后在類ESAP中使用afx_msgLONGOnCommunication(WPARAMch,LPARAMport)；獲取串口送上來(lái)的消息。因?yàn)榇谑且粋€(gè)一個(gè)字符的形式將消息上傳上來(lái)，故這里響應(yīng)的是其WPARAMch，當(dāng)程序遇到”\r\n”，并且之前接受的數(shù)據(jù)長(zhǎng)度大于10的時(shí)候，認(rèn)為有新的數(shù)據(jù)包過(guò)來(lái)，否則丟棄掉。數(shù)據(jù)包格式如表1-1，其中第一行為包格式，第二行為樣例包。各個(gè)數(shù)據(jù)以空格符號(hào)分開，以\r\n結(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_Node{intserial_number;chardest_addr[3];charsrc_addr[3];floatdistance;charrssi[3]；}；為達(dá)到對(duì)節(jié)點(diǎn)的控制的目的，只需在主程序中調(diào)用afx_msgLONGOnCommadToSend(WPARAMwparam,LPARAMcmd)；為作者設(shè)計(jì)的一個(gè)指令包，包格式如表1—2：其中第一行為包格式，第二行為樣例包。表1—2指令包格式HeadCmdTagReservedTail0x00010x00110x00110x00000x1000因?yàn)镻C機(jī)有太多不安全因素，命令包并沒(méi)有像數(shù)據(jù)包那樣沒(méi)有包頭包尾，Head和Tail固定為0x0001和0x1000.主錨節(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è)距停止命令。RANGING—START0x0011RANGING—STOP0x1100Tag為要測(cè)定的目標(biāo)節(jié)點(diǎn)，它可以為0x0001?0x00ff中任意不同于四個(gè)錨節(jié)點(diǎn)的地址的數(shù)。主錨節(jié)點(diǎn)根據(jù)這一位來(lái)確定用戶想要定位的目標(biāo)。如果Cmd為0x1100，即停止測(cè)距指令，則這一位置應(yīng)該置0x0000；Reserbed為保留位，暫時(shí)定為0x0000，這方便以后命令的擴(kuò)展等功能.1。3。2數(shù)據(jù)處理模塊現(xiàn)在緩存中存放的是一堆Result_Node節(jié)點(diǎn),但是這些數(shù)據(jù)并沒(méi)有被處理，不能直接應(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)的還原處理。為了區(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ì)處理完的數(shù)據(jù)進(jìn)行平滑濾波，并將結(jié)果保存，以便后續(xù)計(jì)算模塊使用.上述處理過(guò)程被封裝進(jìn)一個(gè)classCDataPro中，并給出接口函數(shù)doubledata_processing(doubleranging_distance，doublerssi_in，intobject，intobstacle)，程序中只需調(diào)用該函數(shù)，就可以完成對(duì)數(shù)據(jù)的處理.1。3。3定位計(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è)比較直觀的感覺(jué)。這涉及到三個(gè)工作：定位坐標(biāo)系的建立，目標(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所示。「。*心.s皿A(0,0,0)AB.-4(bA)B(Xb,0,H_B-H_A):圖1-9坐標(biāo)系建立示意圖首先需人工測(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)，C(Xc,Yc，H_C—H_A)，D(Xd，Yd，H_A—H_C)；很容易推出：Xb=1.AB2—(H_B-H_4)2Xc=(AC2+AB2—BC2—2xZcxZb)/(2xXb)YcAC2—Cx2—Zc2(因?yàn)锳C要與Y軸的正向方向夾角為銳角，所以這里Yc取正值。)Xd=(AD2+AB2—BD2—2xZdxZb)/(2xXb)Yd=(AD2+AC2—CD2—2xXdxXc—2xZdxZc)/(2xYc)由此可以定出唯一坐標(biāo)系。為后續(xù)計(jì)算提供參考位置.9目標(biāo)節(jié)點(diǎn)的計(jì)算目前有很多成熟的定位算法，每種定位算法都有各自的優(yōu)缺點(diǎn)，因?yàn)楸鞠到y(tǒng)設(shè)計(jì)的需要：既支持二維定位，也支持三維定位，故本文有兩種定位算法。一種是Chan算法，另一種是由Chan算法演變而來(lái)的三維定位算法。

二維定位中，選取三個(gè)參考點(diǎn)A、B、C,三個(gè)點(diǎn)到目標(biāo)點(diǎn)的距離分別為Ra,Rb,Rc.最好的定位結(jié)果圖如圖4-10。這時(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è)圓就沒(méi)有交點(diǎn)，但是這并不意味著二元二次方程沒(méi)有解。此時(shí)求出的坐標(biāo)為任意兩圓的根軸的交點(diǎn)坐標(biāo)，如圖1—11所示：圖1—11三個(gè)圓沒(méi)有交點(diǎn)的解由于本系統(tǒng)有四個(gè)錨節(jié)點(diǎn)，這就意味著有一個(gè)冗余,可以利用這個(gè)冗余數(shù)據(jù)計(jì)算目標(biāo)節(jié)點(diǎn)坐標(biāo)，具體做法為：四個(gè)距離信息中任意取出三個(gè)來(lái)定位，這樣可以計(jì)算出4個(gè)目標(biāo)節(jié)點(diǎn)。然后取四個(gè)目標(biāo)節(jié)點(diǎn)所組成的四邊形的質(zhì)心為最終定位目標(biāo)節(jié)點(diǎn)。三維定位中，類似于二維定位算法?？梢粤谐鲆粋€(gè)三元二次方程組：，(Xa-X)2+(Ya-Y)2+(Za-Z)2=Ra2(Xb-X)2+(Yb-Y)2+(Zb-Z)2=Rb2(1—1)〈(Xc-X)2+(Yc-Y)2+(Zc-Z)2=Rc2、(Xd-X)2+(Yd-Y)2+(Zd-Z)2=Rd2可得出一個(gè)三元一次方程組：(1—1)'2XbX+2YbY+2ZbZ=Ra2-Rb2+AB2(1—2)<2XcX+2YcY+2ZcZ=Ra2-Rc2+AC22XdX+2YdY+2ZdZ=Ra2-Rd2+AD2求解上述方程組即可得出目標(biāo)點(diǎn)的三維坐標(biāo)。(1—2)這里為方便編程，采用矩陣方式求解，上三元一次方程組可化為AB=C，其中A，X,C均為矩陣，-2Xb2Yb2Zb「-x--Ra2-Rb2+AB2-A=2XC2Yc2Zc，B=Y，C=Ra2-Rc2+AC2,2Xc2Yd2ZdZRa2-Rd2+AD2可以得出B的最小二乘解為B=(AM)-iAtC(1-3)同樣，假如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）；vo