定位系统的设计与实现

本文实现的定位系统针对室外环境及办公室环境的实现的定位应用，通过对场景中人员、物品进行定位，方便用户对目标的实时监测和管理。

上位机开发环境为MFC （Microsoft Foundation Classes),它是一个微软公司提

供的类库，以C++类的形式封装了Windows API，并且包含一个应用程序框架，以减少应用程序开发人员的工作量。其中包含的类包含大量Windows 句柄封装类和很多Windows的内建控件和组件的封装类.它的详细介绍在这里就不赘述了。下面对本定位系统的具体实现过程进行比较详细的描述.

1。1 定位系统结构

定位系统拓扑结构图如图1-1所示。其实际场景应用可以参考图1-2。

Anchor测距结果传输方向Tag测距链路监控点Key AnchorAnchor

串口代理图1—1 定位系统拓扑结构图

ANCHOR传输测距结果KEY ANCHOR串行控制线测距链路定位目标，内置TAG点本地定位终端

图1—2 实际应用场景图

本拓扑中有三种功能不同类型的节点:主锚节点、从锚节点、目的节点。其中

主锚节点和从锚节点是一类已知自身位置坐标信息的固定节点，它们的任务是获取包含距离信息以及信号强度值的数据包.目的节点是可以自由移动的节点,可以在一定范围内自由移动，定位系统的最终目的就是获取该类节点位置的坐标信息；从锚节点负责发起测距请求，将测距信息发往目的节点，得到从锚节点与目的节点的距离信息以及信号强度信息,并将该信息发往主锚节点。主锚节点在整个系统中起着至关重要的作用，首先它要响应上位机发出的命令，确定要定位的目标，并发出指令，命令从锚节点对目标节点进行测距；其次主锚节点不仅要获取自身与目标节点的距离信息以及信号强度信息，还需要协调控制三个从锚节点，接受从锚节点发回的测距信息;最后主锚节点负责并将这些信息送还给上位机软件处理。目的节点实时监测是否有定位请求并配合锚节点测出响应的距离信息以及信号强度信息；

本定位系统主要包括四大块：硬件节点、硬件节点软件、上位机监测软件以及显示模块。每一部分设计都有其特殊性,硬件节点为系统提供了定位所需的硬件平台，是信息采集的基础,第三章已经详细介绍其性能，这里不再赘述.硬件节点软件主要完成了节点间的链路建立和数据采集与传输。上位机软件首先通过有线方式(使用串口）接收由定位硬件节点采集到的相关信息,然后对所采集的定位信息进行处理，最后选用合适的定位算法，计算出目标节点在该参考系的坐标。显示模块负责动态显示节点定位效果，免去人工思考数字坐标所代表的具体意义，是定位软件人性化设计的一个体现。图1-3为系统整体程序流程图；

开始Buff上位机发送定位命令上位机线程二：读取buff数据预处理开始人工测出四个ANCHOR点之间的距离虚框内工作需提前做好并在软件中设置好主锚节点监测命令，如果是定位指令，发出定位指令根据以上数据由软件建立坐标系将数据按来源分类主锚节点收集定位所需相关系，并返还给上位机数据预处理整合数据读取处理后的测距结果上位机线程一：读取串口数据并存入buff计算该点坐标显示到界面

图1—3系统整体程序流程图

下面分别详细介绍余下的三个模块。

1。2 硬件节点软件

本定位系统的基本思想是主锚节点收集所有的从锚节点到目标节点以及自身到目标节点的距离信息以及信号强度（RSSI),由主锚节点将信息传送到上位机监测软件，上位机软件调用相应的定位算法，计算出目标节点的坐标位置并实现必要的显示。因此，首先需要在硬件平台上实现节点间的通信，本文采用星形拓扑结构，这种网络拓扑组建简单,并且便于数据采集。主锚节点担当中心节点的角色，以轮询的方式发送测距命令，这样实际在同一时间段内只有一对节点在工作.避免了因冲突造成的处理时延单方向增加，导致定位结果不准确。可参考图1—1。因为本系统采用的nanoPAN5032模块自身并没有组网功能。所以需要作者自己重新编写代码以实现组网功能.通信流程如图1-4：

主锚节点收到测距命令控制从锚节点测距同时自身对锚节点测距从锚节点目标节点上位机发出命令返回距离和RSSI信息获取距离和RSSI信息返回所有信息上位机接受数据图 1-4 通信流程图

其具体实现过程如下：

（1）主锚节点接受定位命令。主锚节点要获取节点的位置信息，就要循环给所有从锚节点发送测距请求信号，通信方式为轮询方式。保证所有的从锚节点都可收到该测距请求。反复获取所有锚节点与目标节点的距离以及信号强度信息；

（2）从锚节点收到测距请求。从锚节点向目标节点发送测距请求，目标节点如果在该区域内，收到该请求就与该锚节点发生点到点的通信，经过一个SDS—TWR测距过程以后,从锚节点将获取到的相应的距离信息以及信号强度信息发送给主锚节点；

(3）主锚节点一旦受到测距信息，就将这些信息传到上位机监测软件中，由检测软件的缓存暂时保存这些数据，以便以后的定位计算；

1.2。1 主锚节点程序设计

主锚节点除了要完成从锚节点的主体功能外，还负责整个通信的协调工作以

及将数据传送给上位机，给定位软件提供所需的定位信息。下面简要介绍下主锚节点的程序设计.图1-5所示为主锚节点的软件流程图.

开始对目标节点测距初始化系统汇集距离和RSSI信息等待上位机命令命令所有从锚节点开始测距N命令正确与否YN结束命令写入串口，传给上位机程序Y结束 图1-5 主锚节点的流程图

其中,系统初始化主要是系统的一些硬件管脚、串口、时钟等进行初始化配置。然后进入死循环，监测上位机命令。监测命令主要通过读取串口信息来实现，如果收到信息,首先进行命令格式检测，如果正确，再判断是什么命令。假如为定位命令，则自身发出测距请求然后命令其余三个锚节点依次对目标节点发出测距请求，这种拓扑结构不仅很容易搭建，而且也避免由于处理冲突导致处理时延增大，导致测距结果变大;

int main(void) ｛ ……………………………………。

RCC_Configuration()； /＊ Configure the system clocks */

NVIC_Configuration()；

GPIO_Configuration(）;

/* NVIC Configuration */

/＊ Configure the GPIOs

*/ USART_Configuration(); /* Configure the USART1 ＊/

SysTick_Config（)；

/＊ Configure the systick */

nano_main(NULL，NULL）； ………………………… ｝

其中nano_main(）部分代码如下： int {

…………………………

while(1) {

……………………….

for(；i〈nodenumber；i++）

while（SendTimingDelay 〉 0)//限定最大发送测距请求次数

{ ｝

SendTimingDelay =3;

receivedate=0；

//是否收到数据的状态

nano_main(int ac， char *av[］）

PollApplication（); Delay(200); NTRXUpdate(）； if（receivedate==1)

break;

SendTimingDelay—-;

｛ }

}

while（SendTimingDelay != 0) ｛

receivedate=0；

Appsendmessage(addr［i],payload[i］， 10)； Delay（200）;

NTRXReceivecommand(）; if(receivedate==1) ｝

SendTimingDelay =3；

break;

SendTimingDelay--；

……………………………………

1。2。2 从锚节点程序设计

下面简要介绍下从锚节点的程序设计。图1-6为从锚节点的流程图。

开始初始化系统等待主锚节点命令N对目标节点测距命令正确与否YN结束命令将信息重新封包发送给主锚节点Y结束 图1—6从锚节点的流程图

其中，系统初始化和主锚节点一样。进入死循环，监测来自主锚节点的命令;如果收到信息，首先进行命令格式检测，如果正确，再判断是什么命令。假如为定位命令，则对目标节点发出测距请求.将得到的结果封包发送给主锚节点；

1.2。3 目标节点程序设计

下面简要介绍下从锚节点的程序设计。图1—7为目标节点的流程图.

开始初始化系统等待测距请求N请求命令正确与否Y响应测距请求，开始测距

图1—7 目标节点的流程图

其中，系统初始化和主锚节点一样。进入死循环,监测来自锚节点的测距命令；如果收到信息。首先进行命令格式检测，如果正确，就继续和锚节点通信,开始测距任务，并由锚节点收集测距信息。

1。3 上位机软件设计

定位系统上位机监控软件中,主要包括以下几个功能：串口通信、数据处理、定位计算等三个主要模块。串口通信是上位机监测与控制硬件节点的唯一通道。所有的指令发出以及信息获取都是通过串口来实现的；数据处理主要负责将串口收集到的原始数据加以处理，最大程度还原真实的距离信息，以便定位算法能够算出准确的目的节点坐标；定位算法主要负责将处理过的数据转换为参考坐标系的坐标。这里的参考坐标系是建立在人工测定的几个锚节点的相对位置的基础上的,下面分别介绍这三个模块。

1.3。1 串口通信模块

串口通信是连接硬件节点和上位机软件的唯一枢纽，用户的定位指令以及硬件上传的测距信息都是通过串口传输到上位机软件中来的。为了方便程序的书写，这里将串口封装称为一个类class CSerialPort;该类能够完成对串口的初始化，

打开，读写,事件等待等功能。

MFC的一大特色是消息响应机制，在本类中，为了完成通信,作者定义以下消息:

#define WM_COMM_BREAK_DETECTED #define WM_COMM_CTS_DETECTED ＃define WM_COMM_DSR_DETECTED #define WM_COMM_ERR_DETECTED

WM_USER+1

WM_USER+2

WM_USER+3 WM_USER+4

＃define WM_COMM_RING_DETECTED ＃define WM_COMM_RLSD_DETECTED ＃define WM_COMM_RXCHAR

WM_USER+5 WM_USER+6

WM_USER+7 WM_USER+8

#define WM_COMM_RXFLAG_DETECTED

＃define WM_COMM_TXEMPTY_DETECTED WM_USER+9

然后在类 ESAP中使用 afx_msg LONG OnCommunication(WPARAM ch， LPARAM port）；获取串口送上来的消息.因为串口是一个一个字符的形式将消息上传上来，故这里响应的是其WPARAM ch,当程序遇到”\\r\\n”，并且之前接受的数据长度大于10的时候，认为有新的数据包过来,否则丢弃掉。数据包格式如表1—1，其中第一行为包格式，第二行为样例包.各个数据以空格符号分开，以\\r\\n结尾.

表1—1 主锚节点上传数据包格式

Serial Number

1

Anchor Address

71

Tag Address

11

Distance 2.36

RSSI 50

得到完整数据包后将数据包封装成结构体Result_Node，以节点的形式加入

处理缓存队列，方便以后数据处理操作，其中结构体定义如下： struct Result_Node｛

int serial_number； char dest_addr[3］； char src_addr［3］; float distance; char rssi[3］;

｝;

为达到对节点的控制的目的，只需在主程序中调用 afx_msg 包，包格式如表1-2：其中第一行为包格式,第二行为样例包。

表1-2 指令包格式

Head 0x0001

Cmd 0x0011

Tag 0x0011

Reserved 0x0000

Tail 0x1000

LONG

OnCommadToSend（WPARAM wparam, LPARAM cmd)；为作者设计的一个指令

因为PC机有太多不安全因素，命令包并没有像数据包那样没有包头包尾，Head和Tail固定为0x0001和0x1000。主锚节点收到数据包后首先对数据包解包，如果格式不正确.就丢弃,以免因为其他操作，导致数据传入主锚节点，使得系统不稳定.

Cmd表示是何种命令，暂时只定义两种命令，测距开始命令和测距停止命令。 RANGING—START RANGING—STOP

0x0011 0x1100

Tag为要测定的目标节点，它可以为0x0001～0x00ff中任意不同于四个锚节点的地址的数。主锚节点根据这一位来确定用户想要定位的目标。如果Cmd为0x1100，即停止测距指令，则这一位置应该置0x0000;

Reserbed为保留位，暂时定为0x0000，这方便以后命令的扩展等功能。

1.3。2 数据处理模块

现在缓存中存放的是一堆Result_Node节点，但是这些数据并没有被处理，不能直接应用于定位算法中.数据处理模块负责把这些数据分类、排错、滤波、还原.为后续的定位计算准备数据.其处理流程如图1-8:

开始处理读取Buff排除错误数据根据serial number序号还原出距离信息映射还原数据根据Anchor Address对数据进行分类平滑滤波将分类后的数据分别进入代表各节点的缓存队列处理完毕 图1-8 数据处理流程图

首先从Buff中读取一个节点，因为硬件已经对数据进行平滑处理，由于刚开始的时候平滑数组所有值都为0，前十个上传的信息都被乘上一个平滑系数。为了还原这些数据，只需判定其serial number位是否小于10，然后进行相应的还原处理。为了区分不同锚节点的距离信息,作者为每个节点分配一个缓存队列,对处理完的距离信息分别加入对应的缓存队列，并将缓存队列状态设置为非空状态.

根据第三章分析处理得出的结论，根据不同的情况对进入缓存队列的数据进行相应的处理校正。首先根据新进的数据与上次数据相比较,利用物体在室内运动速度有上限这一前提，制作一个平滑滤波器，滤掉因外界干扰造成的测距误差。然后根据软件的场景设置选择不同的处理方法，软件给出静态目标定位和动态目标定位、视距可达定位和视距不可达定位四种情况，如果定位人员不知道具体场景,可按照默认的选项进行定位。软件会根据RSSI值，测距结果对进入缓存队列的数据进行处理。最后对处理完的数据进行平滑滤波，并将结果保存，以便后续计算模块使用。

上述处理过程被封装进一个class CDataPro 中,并给出接口函数double data_processing(double ranging_distance,double rssi_in,int object, int obstacle），程序中只需调用该函数，就可以完成对数据的处理。

1。3。3定位计算模块

上面数据处理已经完成，当定位模块检测到四个缓存队列的状态都是非空状

态，即满足定位计算的基本数据要求,就读取其中的数据进行定位计算。要实现定位就需要将这些距离信息转换为相对于坐标系的坐标信息,这才能给人一个比较直观的感觉.这涉及到三个工作：定位坐标系的建立，目标节点的计算，算法的程序实现；

a)坐标系的建立

任何一个定位系统都有自己的参考坐标系，本系统的参考坐标系是根据实际场景生成的相对坐标系。为方便描述，这里约定主锚节点的物理位置为A、其余三个锚节点为B、C、D,如图1—9所示。

ZD(Xd,Yd,H_D-H_A)C(Xc,Yc,H_C-H_A)ACYBCA（0,0,0）ABB(Xb,0,H_B-H_A)X

图 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);

很容易推出:

2XbAB2（H_BH_A）

Xc(AC2AB2BC22ZcZb)/(2Xb)

YcAC2Cx2Zc2以这里Yc取正值。)

（因为AC要与Y轴的正向方向夹角为锐角，所

Xd(AD2AB2BD22ZdZb)/(2Xb)

Yd(AD2AC2CD22XdXc2ZdZc)/(2Yc) 由此可以定出唯一坐标系。为后续计算提供参考位置。 b)目标节点的计算

目前有很多成熟的定位算法,每种定位算法都有各自的优缺点，因为本系统设计的需要：既支持二维定位，也支持三维定位，故本文有两种定位算法。一种是Chan算法，另一种是由Chan算法演变而来的三维定位算法。

二维定位中，选取三个参考点A、B、C,三个点到目标点的距离分别为Ra，Rb，Rc。最好的定位结果图如图4-10.这时可以简单的利用三个二元二次方程组求解出目标点坐标。

ABRaRbRcC 图1-10 Chan定位算法示意图

当出现测距误差时,Ra，Rb，Rc会出现不同程度的变大或者缩小。例如三个圆的半径都比实际值小，那么三个圆就没有交点，但是这并不意味着二元二次方程没有解.此时求出的坐标为任意两圆的根轴的交点坐标，如图 1—11所示：

AAB的根AC的根轴RaRcBRb轴BC的根轴C 图1—11 三个圆没有交点的解

由于本系统有四个锚节点，这就意味着有一个冗余，可以利用这个冗余数据计算目标节点坐标，具体做法为:四个距离信息中任意取出三个来定位，

这样可以计算出4个目标节点。然后取四个目标节点所组成的四边形的质心为最终定位目标节点。

三维定位中，类似于二维定位算法。可以列出一个三元二次方程组：

(Xa-X)2(YaY)2(ZaZ)2Ra22222(Xb-X)(YbY)(ZbZ)Rb(1—1) 2222(Xc-X)(YcY)(ZcZ)Rc2222(Xd-X)(YdY)(ZdZ)Rd

可得出一个三元一次方程组：

2XbX2YbY2ZbZRa2Rb2AB22222XcX2YcY2ZcZRaRcAC2XdX2YdY2ZdZRa2Rd2AD2(1-2）

求解上述方程组即可得出目标点的三维坐标.

这里为方便编程,采用矩阵方式求解，上三元一次方程组可化为 AB=C，其中A，X,C均为矩阵，

Ra2Rb2AB22Xb2Yb2ZbX2BY22A2XC2Yc2Zc，，CRaRcAC， Ra2Rd2AD2Z2Xc2Yd2Zd可以得出B的最小二乘解为

B(ATA)1ATC

（1-3)

同样，假如Ra,Rb，Rc，Rd因为测量的误差而大于实际值，那么所求的交点为他们根面的交点；

c）程序实现

将坐标系封装为一个class Coordinate,内部封装了坐标系的初始化，坐标系的建立，二维坐标计算,三维坐标计算等函数. Public:

Point calculate_2d （Point a， Point b，Point

c,DISTANCE n2a, DISTANCE n2b， DISTANCE n2c）;

Point calculate_2d （DISTANCE n2a， DISTANCE n2b,

DISTANCE n2c, DISTANCE n2d）;

void init（DISTANCE ab，DISTANCE ac，DISTANCE bc，

DISTANCE ad,DISTANCE bd,DISTANCE cd，HEIGHT ha，HEIGHT

hb,HEIGHT hc，HEIGHT hd）;

void creatCoordinate（）；

Point calculate_3d （DISTANCE n2a， DISTANCE n2b,

DISTANCE n2c, DISTANCE n2d）;

DISTANCE P2P (Point a, Point b);

1。4 显示模块的设计

为了能让用户直观看到目标点的位置，本文特意引用三维界面编程理念,采用

OPENGL技术错误!未找到引用源。，将用户从枯燥的数字中解脱出来。显示界面如图1—12所示。

首先用OpenGL的基本图形和坐标变换，来构造界面中可能用到的模型,比如旋转的雷达。然后需要在OpenGL场景中建立有一定真实感的天空、地面.地面的高度由读入的等高线地图图片数据决定，就可以生成与等高线相同高度差的”山”。天是由一个长方体（称为天空盒)，在它的各个面上贴有表示天的图片形成的.用于天空背景的图片有特殊要求.4个侧面图的边与顶面图的边相连，4侧面图前后相连.图片大小为2的N次方（32、64、128.。。).

至此，与现实相对应的虚拟世界已经搭建好了。人们要在里面行走就需要漫游技术，人对世界的综合视觉观察效果，是来源于人的眼睛。眼睛就像一架摄像机，将外部影像反映到大脑。在计算机3D图形处理技术中，也有类似眼睛的东西gluLookAt(…） 观察函数，如果这个观察点在OpenGL场景中的位置发生变化，在计算机屏幕上的图像就发生变化。

在OpenGL中观察虚拟世界的主要函数gluLookAt（…) ，它的主要的作用是可以改变在OpenGL场景的观察点，这个观察点就好像是眼睛，也好像是人们手中的摄像机.人在一个场景中行走时，看到前面的景物越来越近，两边的物体在向后退，这就是观察点在场景中的位置改变的结果。

最后,为了把定位结果直观展示出来，作者将目标抽象为一个红色的球，这样能使人们迅速准确的找到目标,作者提供了两种视角:行走视角和高空俯视视角。上述所有功能被封装在class baiscobj里面，软件截图如图1-12所示;

图1-12 高空俯视视角图