定位系统的设计与实现

监控点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