監控點KeyAnchorAnchor定位系統的設計與實現監控點KeyAnchorAnchor本文實現的定位系統針對室外環境及辦公室環境的實現的定位應用，通過對場景中人員、物品進行定位，方便用戶對目標的實時監測和管理。上位機開發環境為MFC(MicrosoftFoundationClasses)，它是一個微軟公司提供的類庫，以C++類的形式封裝了WindowsAPI，并且包含一個應用程序框架，以減少應用程序開發人員的工作量。其中包含的類包含大量Windows句柄封裝類和很多Windows的內建控件和組件的封裝類。它的詳細介紹在這里就不贅述了。下面對本定位系統的具體實現過程進行比較詳細的描述。1.1定位系統結構定位系統拓撲結構圖如圖1—1所示。其實際場景應用可以參考圖1—2。ANCHOR串口代理傳輸測距結果圖1—1定位系統拓撲結構圖ANCHOR串口代理傳輸測距結果KEYANCHOR串行控制線測距鏈路定位目標，內置TAG點本地定位終端圖1—2實際應用場景圖本拓撲中有三種功能不同類型的節點：主錨節點、從錨節點、目的節點。其

中主錨節點和從錨節點是一類已知自身位置坐標信息的固定節點，它們的任務是獲取包含距離信息以及信號強度值的數據包。目的節點是可以自由移動的節點，可以在一定范圍內自由移動，定位系統的最終目的就是獲取該類節點位置的坐標信息；從錨節點負責發起測距請求，將測距信息發往目的節點，得到從錨節點與目的節點的距離信息以及信號強度信息，并將該信息發往主錨節點。主錨節點在整個系統中起著至關重要的作用，首先它要響應上位機發出的命令，確定要定位的目標，并發出指令，命令從錨節點對目標節點進行測距；其次主錨節點不僅要獲取自身與目標節點的距離信息以及信號強度信息，還需要協調控制三個從錨節點,接受從錨節點發回的測距信息；最后主錨節點負責并將這些信息送還給上位機軟件處理。目的節點實時監測是否有定位請求并配合錨節點測出響應的距離信息以及信號強度信息；本定位系統主要包括四大塊：硬件節點、硬件節點軟件、上位機監測軟件以及顯示模塊。每一部分設計都有其特殊性，硬件節點為系統提供了定位所需的硬件平臺,是信息采集的基礎,第三章已經詳細介紹其性能，這里不再贅述。硬件節點軟件主要完成了節點間的鏈路建立和數據采集與傳輸.上位機軟件首先通過有線方式（使用串口）接收由定位硬件節點采集到的相關信息，然后對所采集的定位信息進行處理，最后選用合適的定位算法，計算出目標節點在該參考系的坐標。顯示模塊負責動態顯示節點定位效果，免去人工思考數字坐標所代表的具體意義，是定位軟件人性化設計的一個體現。圖1-3為系統整體程序流程圖;Buff人工測出四個ANCHOR點之間的距離根據以上數據由軟件建立坐標系虛框內工作需提前做好并在Buff人工測出四個ANCHOR點之間的距離根據以上數據由軟件建立坐標系虛框內工作需提前做好并在軟件中設置好圖1-3系統整體程序流程圖下面分別詳細介紹余下的三個模塊。本定位系統的基本思想是主錨節點收集所有的從錨節點到目標節點以及自身到目標節點的距離信息以及信號強度(RSSI)，由主錨節點將信息傳送到上位機監測軟件，上位機軟件調用相應的定位算法，計算出目標節點的坐標位置并實現必要的顯示。因此，首先需要在硬件平臺上實現節點間的通信，本文采用星形拓撲結構，這種網絡拓撲組建簡單，并且便于數據采集。主錨節點擔當中心節點的角色，以輪詢的方式發送測距命令，這樣實際在同一時間段內只有一對節點在工作。避免了因沖突造成的處理時延單方向增加，導致定位結果不準確。可參考圖1-1。因為本系統采用的nanoPAN5032模塊自身并沒有組網功能。所以需要作者自己重新編寫代碼以實現組網功能。通信流程如圖1-4：圖1—4通信流程圖其具體實現過程如下：主錨節點接受定位命令.主錨節點要獲取節點的位置信息，就要循環給所有從錨節點發送測距請求信號，通信方式為輪詢方式。保證所有的從錨節點都可收到該測距請求。反復獲取所有錨節點與目標節點的距離以及信號強度信息；從錨節點收到測距請求。從錨節點向目標節點發送測距請求，目標節點如果在該區域內，收到該請求就與該錨節點發生點到點的通信，經過一個SDS-TWR測距過程以后，從錨節點將獲取到的相應的距離信息以及信號強度信息發送給主錨節點；主錨節點一旦受到測距信息，就將這些信息傳到上位機監測軟件中，由檢測軟件的緩存暫時保存這些數據，以便以后的定位計算；

1。2。1主錨節點程序設計主錨節點除了要完成從錨節點的主體功能外，還負責整個通信的協調工作以及將數據傳送給上位機，給定位軟件提供所需的定位信息.下面簡要介紹下主錨節點的程序設計。圖1-5所示為主錨節點的軟件流程圖。等待上位機命令等待上位機命令圖1—5主錨節點的流程圖其中，系統初始化主要是系統的一些硬件管腳、串口、時鐘等進行初始化配置。然后進入死循環，監測上位機命令。監測命令主要通過讀取串口信息來實現，如果收到信息，首先進行命令格式檢測，如果正確，再判斷是什么命令。假如為定位命令，則自身發出測距請求然后命令其余三個錨節點依次對目標節點發出測距請求，這種拓撲結構不僅很容易搭建，而且也避免由于處理沖突導致處理時延增大，導致測距結果變大；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)//限定最大發送測距請求次數{receivedate=0；//是否收到數據的狀態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從錨節點程序設計下面簡要介紹下從錨節點的程序設計。圖1-6為從錨節點的流程圖。

圖1-6從錨節點的流程圖將信息重新封包圖1-6從錨節點的流程圖將信息重新封包發送給主錨節點對目標節點測距其中,系統初始化和主錨節點一樣.進入死循環，監測來自主錨節點的命令；如果收到信息，首先進行命令格式檢測，如果正確，再判斷是什么命令。假如為定位命令，則對目標節點發出測距請求。將得到的結果封包發送給主錨節點;1.2.3目標節點程序設計下面簡要介紹下從錨節點的程序設計。圖1-7為目標節點的流程圖。圖1-7目標節點的流程圖其中，系統初始化和主錨節點一樣。進入死循環，監測來自錨節點的測距命令；如果收到信息.首先進行命令格式檢測，如果正確，就繼續和錨節點通信，開始測距任務,并由錨節點收集測距信息。1。3上位機軟件設計定位系統上位機監控軟件中，主要包括以下幾個功能：串口通信、數據處理、定位計算等三個主要模塊.串口通信是上位機監測與控制硬件節點的唯一通道。所有的指令發出以及信息獲取都是通過串口來實現的；數據處理主要負責將串口收集到的原始數據加以處理，最大程度還原真實的距離信息，以便定位算法能夠算出準確的目的節點坐標;定位算法主要負責將處理過的數據轉換為參考坐標系的坐標。這里的參考坐標系是建立在人工測定的幾個錨節點的相對位置的基礎上的,下面分別介紹這三個模塊。1。3.1串口通信模塊串口通信是連接硬件節點和上位機軟件的唯一樞紐，用戶的定位指令以及硬件上傳的測距信息都是通過串口傳輸到上位機軟件中來的。為了方便程序的書寫，這里將串口封裝稱為一個類classCSerialPort；該類能夠完成對串口的初始化，打開，讀寫，事件等待等功能。MFC的一大特色是消息響應機制，在本類中，為了完成通信，作者定義以下消息：#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)；獲取串口送上來的消息。因為串口是一個一個字符的形式將消息上傳上來，故這里響應的是其WPARAMch，當程序遇到”\r\n”，并且之前接受的數據長度大于10的時候，認為有新的數據包過來，否則丟棄掉。數據包格式如表1-1，其中第一行為包格式，第二行為樣例包。各個數據以空格符號分開，以\r\n結尾。表1-1主錨節點上傳數據包格式SerialNumberAnchorAddressTagAddressDistanceRSSI171112。3650得到完整數據包后將數據包封裝成結構體Result_Node，以節點的形式加入處理緩存隊列，方便以后數據處理操作，其中結構體定義如下：structResult_Node{intserial_number;chardest_addr[3];charsrc_addr[3];floatdistance;charrssi[3]；}；為達到對節點的控制的目的，只需在主程序中調用afx_msgLONGOnCommadToSend(WPARAMwparam,LPARAMcmd)；為作者設計的一個指令包，包格式如表1—2：其中第一行為包格式，第二行為樣例包。表1—2指令包格式HeadCmdTagReservedTail0x00010x00110x00110x00000x1000因為PC機有太多不安全因素，命令包并沒有像數據包那樣沒有包頭包尾，Head和Tail固定為0x0001和0x1000.主錨節點收到數據包后首先對數據包解包，如果格式不正確。就丟棄，以免因為其他操作，導致數據傳入主錨節點，使得系統不穩定.Cmd表示是何種命令，暫時只定義兩種命令，測距開始命令和測距停止命令。RANGING—START0x0011RANGING—STOP0x1100Tag為要測定的目標節點，它可以為0x0001?0x00ff中任意不同于四個錨節點的地址的數。主錨節點根據這一位來確定用戶想要定位的目標。如果Cmd為0x1100，即停止測距指令，則這一位置應該置0x0000；Reserbed為保留位，暫時定為0x0000，這方便以后命令的擴展等功能.1。3。2數據處理模塊現在緩存中存放的是一堆Result_Node節點,但是這些數據并沒有被處理，不能直接應用于定位算法中.數據處理模塊負責把這些數據分類、排錯、濾波、還原。為后續的定位計算準備數據。其處理流程如圖1—8：圖1—8數據處理流程圖首先從Buff中讀取一個節點，因為硬件已經對數據進行平滑處理，由于剛開始的時候平滑數組所有值都為0,前十個上傳的信息都被乘上一個平滑系數。為了還原這些數據，只需判定其serialnumber位是否小于10，然后進行相應的還原處理。為了區分不同錨節點的距離信息，作者為每個節點分配一個緩存隊列，對處理完的距離信息分別加入對應的緩存隊列，并將緩存隊列狀態設置為非空狀態。根據第三章分析處理得出的結論，根據不同的情況對進入緩存隊列的數據進行相應的處理校正。首先根據新進的數據與上次數據相比較，利用物體在室內運動速度有上限這一前提，制作一個平滑濾波器，濾掉因外界干擾造成的測距誤差。然后根據軟件的場景設置選擇不同的處理方法，軟件給出靜態目標定位和動態目標定位、視距可達定位和視距不可達定位四種情況，如果定位人員不知道具體場景，可按照默認的選項進行定位。軟件會根據RSSI值,測距結果對進入緩存隊列的數據進行處理。最后對處理完的數據進行平滑濾波，并將結果保存，以便后續計算模塊使用.上述處理過程被封裝進一個classCDataPro中，并給出接口函數doubledata_processing(doubleranging_distance，doublerssi_in，intobject，intobstacle)，程序中只需調用該函數，就可以完成對數據的處理.1。3。3定位計算模塊上面數據處理已經完成，當定位模塊檢測到四個緩存隊列的狀態都是非空狀態，即滿足定位計算的基本數據要求，就讀取其中的數據進行定位計算。要實現定位就需要將這些距離信息轉換為相對于坐標系的坐標信息，這才能給人一個比較直觀的感覺。這涉及到三個工作：定位坐標系的建立，目標節點的計算，算法的程序實現；a)坐標系的建立任何一個定位系統都有自己的參考坐標系，本系統的參考坐標系是根據實際場景生成的相對坐標系.為方便描述，這里約定主錨節點的物理位置為A、其余三個錨節點為B、C、D，如圖1—9所示。「。*心.s皿A(0,0,0)AB.-4(bA)B(Xb,0,H_B-H_A):圖1-9坐標系建立示意圖首先需人工測量出A、B、C、D四個點離參考平面高度H_A、H_B、H_C、H_D,如果是采用二維定位，可簡單將這四個值賦值為零。然后測出四個點兩兩之間的距離AB，AC,AD,BC，BD，CD。總共十個數據；初始化A，B,C,D四個點Z坐標為Za=0,Zb=H_B-H_A，Zc=H_C—H_A，Zd=H_D-H_A則可設四點坐標為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(因為AC要與Y軸的正向方向夾角為銳角，所以這里Yc取正值。)Xd=(AD2+AB2—BD2—2xZdxZb)/(2xXb)Yd=(AD2+AC2—CD2—2xXdxXc—2xZdxZc)/(2xYc)由此可以定出唯一坐標系。為后續計算提供參考位置.9目標節點的計算目前有很多成熟的定位算法，每種定位算法都有各自的優缺點，因為本系統設計的需要：既支持二維定位，也支持三維定位，故本文有兩種定位算法。一種是Chan算法，另一種是由Chan算法演變而來的三維定位算法。

二維定位中，選取三個參考點A、B、C,三個點到目標點的距離分別為Ra,Rb,Rc.最好的定位結果圖如圖4-10。這時可以簡單的利用三個二元二次方程組求解出目標點坐標。圖1—10Chan定位算法示意圖當出現測距誤差時，Ra，Rb，Rc會出現不同程度的變大或者縮小。例如三個圓的半徑都比實際值小，那么三個圓就沒有交點，但是這并不意味著二元二次方程沒有解。此時求出的坐標為任意兩圓的根軸的交點坐標，如圖1—11所示：圖1—11三個圓沒有交點的解由于本系統有四個錨節點，這就意味著有一個冗余,可以利用這個冗余數據計算目標節點坐標，具體做法為：四個距離信息中任意取出三個來定位，這樣可以計算出4個目標節點。然后取四個目標節點所組成的四邊形的質心為最終定位目標節點。三維定位中，類似于二維定位算法。可以列出一個三元二次方程組：，(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可得出一個三元一次方程組：(1—1)'2XbX+2YbY+2ZbZ=Ra2-Rb2+AB2(1—2)<2XcX+2YcY+2ZcZ=Ra2-Rc2+AC22XdX+2YdY+2ZdZ=Ra2-Rd2+AD2求解上述方程組即可得出目標點的三維坐標。(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因為測量的誤差而大于實際值,那么所求的交點為他們根面的交點；c)程序實現將坐標系封裝為一個classCoordinate，內部封裝了坐標系的初始化，坐標系的建立，二維坐標計算，三維坐標計算等函數。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