实验3高精地图和车辆定位







第3章学习素材

3.1实验目的
自动驾驶高精地图与无人驾驶系统的安全性、稳定性、舒适性紧密关联，是无人驾驶的核心关键技术之一; 车辆定位技术是自动驾驶环境感知、规划决策以及车辆控制等技术实现的重要支撑。为加深高精地图与车辆定位的理解及知识掌握，本实验将基于清华大学校园高精地图数据及不同定位源数据，结合高精地图与车辆定位理论知识进行地图与定位相关实验操作，实现具体功能实验的同时，加深对基础理论知识的理解。
本实验的学习目标： 
(1) 了解自动驾驶高精地图处理的常用软件。
(2) 理解自动驾驶高精地图的构成及建图方法。
(3) 理解自动驾驶不同定位源数据及其使用方法。
(4) 理解UWB定位的基本原理与位置计算方法。
3.2实验过程简介
共安排两个实验，实验一，基于高精地图的室外定位实验(Mapinfo与ArcGIS软件为例)，以高精地图为基准，基于Mapinfo 软件实现对地图要素的构建与更新，掌握如何在软件中实现地图的编辑与操作，并基于室外采集的GPS数据，同步叠加到地图数据中； 实验二，基于UWB的室内定位实验，通过了解UWB定位原理与求解方法，实现基于电子沙盘的UWB室内定位，并加强对理论知识的理解。

3.3前导知识
3.3.1常用地图软件介绍及操作
1. MapInfo软件简介

MapInfo是美国MapInfo公司开发的桌面地理信息系统软件，是一种数据可视化、信息地图化的桌面解决方案。MapInfo采用分层的数据组织形式，通过图层来管理和显示空间数据，并且功能十分丰富，包括地图的数据存储、绘制、信息分析、数据挖掘等。MapInfo的数据表达包括地图表达(Mapper)、数据表浏览(Browser)、直观图表达(Grapher)，支持包括Oracle、Microsoft SQL server在内的数据库。
MapInfo的含义是“Mapping + Information”,即： 地图对象+属性数据，在MapInfo中，地图由空间数据和属性数据组成，主要协调两大关系： 地图界面与其背后的表，每层数据对应一张表，表的管理需要四个文件： TAB文件、DAT文件、ID文件、MAP文件，TAB文件定义每层中的要素属性(如定义道路、水域、信号灯等)，一种要素定义一张表，DAT文件存放该要素所有元素的属性(如ID、名称、位置、类型等)，ID文件存储图形对象索引，是属性与空间图形化界面的连接，MAP文件存放空间数据描述(如连接点、边界、颜色等)。四种文件的关系示意图如图31所示。



图31四种文件的关系


MapInfo数据存在于两种文件中： 图形数据存储在.MIF文件、文本数据存储在.MID文件。
MIF是内存初始化文件，是MapInfo的通用数据交换格式，这种格式是ASCII码，可以编辑，容易生成，且可以工作在MapInfo支持的所有平台上。MIF文件的数据包括两部分——文件头和数据区，如图32所示，文件头存放如何创建MapInfo表格的信息； 数据区存放图形对象的解释，包括每个点对象的符号样式、点位坐标，每个线对象的线样式、节点数据、节点坐标，区域对象的填充模式、每个区域包含的子区域个数及每个区域的节点数等。



图32MIF文件的文件头和数据区


MID文件则按记录顺序保存了每个空间对象的所有属性信息，如图33所示。



图33MID文件内容示意图


MapInfo工作界面如图34所示，主要包括菜单栏、主工具条、绘图工具条、常用工具条、地图窗口等。主工具条包括选择对象、改变地图窗口的视图、取得对象的信息和显示对象间距的工具，还包括一条命令按钮，用于改变图层的属性和打开图例或同级窗口。绘图工具条包括用于创建和编辑地图对象的工具和命令。常用工具条包含文件、编辑和窗口菜单中常用的菜单功能工具，以及快速访问新建窗口和联机帮助等。



图34MapInfo工作界面


2. MapInfo软件操作
1) MapInfo软件安装
在安装包文件夹中双击setup.exe，启动安装程序。在如图35所示的对话框中单击Next按钮，在如图36所示的License Information对话框中勾选I accept the term in the license agreement单选按钮，然后单击Next按钮，在如图37所示的对话框中勾选Check here if you are installing an Evaluation copy单选按钮，并自定义填写User Name和Organization信息，填写完毕后单击Next按钮，在如图38所示的Setup Type对话框中选择Typical并单击Next按钮，在如图39所示的对话框中选择安装路径并单击Next按钮，在图310所示的对话框中单击Install按钮开始安装，等待安装完成，最后，在图311所示的窗口中单击Finish按钮，完成安装。


图35安装界面




图36License Information对话框




图37Customer Information对话框



安装完成后需要进行语言汉化和软件激活。进入安装包文件夹中的“破解和汉化补丁”文件夹，将里面的4个文件MAPINFOW.MNU、micore.dll、mires.dll、msres.dll复制到图39中选择的安装路径内，如D:＼Softwares＼MapInfo，复制过程对同名文件选择“全部替换”。


2) 数据导入软件
打开MapInfo软件，在“菜单栏”中单击“导入”选项，进入清华大学艺术博物馆路段地图数据路径，选择文件类型为.mif，如图312所示，选择nav_point.MIF，单击“打开”按钮，弹出图313所示对话框，选择nav_point.TAB的存储位置，单击“保存”按钮，完成TAB文件的存储。



图38Setup Type对话框




图39Destination Folder对话框




图310Ready to Install the Program对话框




图311安装完成





图312导入.mif文件




图313转入到表，存储为TAB文件




在“菜单栏”→“文件”中单击“打开”按钮，在弹出的对话框中选择“文件类型”为.tab，选择刚刚保存的TAB文件，单击“打开”按钮，如图314所示。成功打开TAB文件后，显示图315所示窗口，地图窗口出现地图数据，图层控制区可看到导入的nav_point图层(若没有出现图层控制区，则在主工具条中单击“图层控制”即可打开)。


图314选择打开TAB文件




图315成功打开nav_point.TAB


对清华大学艺术博物馆路段地图数据中的green_area.MIF、nav_link.MIF、node_point.MIF、restrict_table.MIF、water_area.MIF重复上述步骤，导入和打开所有地图图层数据。



3) 基本操作步骤
(1) 在nav_point图层增加一个点。
该部分的目标是在nav_point图层增加一个点，该点的信息包括： 
经度: 116.322404； 纬度: 40.008168； MAPID： 595612； ID： 10000106； TYPE： 81506044； NAME： 天猫超市； PREFIX： 10。
要求： 上述给定的属性定义要精确，其他属性可空缺或自定义。
具体的操作步骤如下。
如图316所示，在主工具条里打开“图层控制”，右击nav_point图层，选择“查看属性表”。打开属性表后，右击属性表第一列空白列，选择“新建行”，如图317所示，将需要添加点的信息对应输入进属性表。


图316查看属性表




图317属性表新建行


填写完信息后，选择“菜单栏”→“表”→“创建点”命令，在图318所示窗口里选择“为表创建点”为nav_point，自定义使用符号，设置完毕后单击“确定”按钮。在图层控制区中开启nav_point图层的标签，则可在地图中找到增加的点“天猫超市”，如图319所示。




图318创建点




图319在图层控制中打开nav_point图层


(2) 在nav_link图层增加一个路段。
该部分的目标是在nav_link图层增加一个路段，路段的信息包括： 

线型： 直线； 起点经度： 116.319431； 起点纬度： 40.017938； 终点经度： 116.348997； 终点纬度： 40.017445； MAPID： 595612； ID： 10249296； NAME： MapInfo实习。
要求上述给定属性定义要精确，其他信息可空缺或自定义。
具体的操作步骤如下。
按照上述在地图中增加点的操作，首先在nav_point图层中增加起点和终点(属性信息可只填写NAME、经度、纬度)。完成后可在地图中找到“起点”和“终点”，如图320所示。


图320地图中的起点与终点


如图321所示，在属性控制区将nav_link图层的编辑功能打开，在绘图工具条中选择“线”，并按S键打开SNA模式，单击“起点”处星号，按住鼠标不放拖动直线到“终点”处星号，即完成一条从起点到终点的直线路段。


图321手动绘制直线路段


在图层控制区中右击nav_link图层，选择“查看属性表”，如图322所示，在表中最后一行可以看到刚刚手动添加的直线路段，将Name改为“MapInfo实习”，保存并退出； 如图323所示，在图层控制区打开nav_link图层的标签，在地图区可以看到刚刚添加的路段的名称。



图322在属性表中添加路段名称




图323地图区显示添加的路段名称


(3) GPS数据处理及导入MapInfo软件操作。
该部分首先使用MATLAB对GPS数据进行处理，导出为xls格式，再将xls文件导入MapInfo，以GPS数据为例，具体操作步骤如下。
该GPS闭环轨迹数据为“2017_0506_5_闭环.txt”。在MATLAB中打开数据处理脚本GPS_data_get.m，将文件名定义语句修改为“filename = '2017_0506_5_闭环';”，然后按F5键运行脚本，运行完毕后可以在文件夹中看到输出文件“2017_0506_5_闭环.xls”。
在MapInfo中，选择“菜单栏”→“文件”→“打开”命令，文件类型选择.xls，选择“2017_0506_5_闭环.xls”并打开，在图324所示弹窗中选择“命名范围”为“全部工作表Sheet 1”，不需要勾选“以选中范围的上一行作为列标题”，单击“确定”按钮； 在弹出的“设置字段属性”弹窗里，设置字段从上到下依次为Latitude和Longitude，类型均为浮点型，如图325所示，单击“确定”按钮，完成后可以在文件夹内找到生成的“2017_0506_5_闭环.TAB”文件。


图324打开“2017_0506_5_闭环.xls”时的弹窗




图325设置字段属性


在MapInfo中，选择“菜单栏”→“文件”→“打开”命令，文件类型选择.tab，选择“2017_0506_5_闭环.TAB”并打开； 再选择“菜单栏”→“表”→“创建点”命令，如图326所示，在弹出的弹窗中“为表创建点”选择“_2017_0506_5_闭环”，“使用符号”处可以设置不同符号和颜色，在“取得X坐标的列”处选择Longitude，在“取得Y坐标的列”处选择Latitude，设置完毕后单击“确定”按钮； 如图327所示，在图层控制区单击加号，在弹窗中选择“_2017_0506_5_闭环”，单击OK按钮，可以看到如图328所示，图层控制区里出现了“_2017_0506_5_闭环”图层，地图中也出现了红色的GPS闭环轨迹。


图326为GPS闭环轨迹数据创建点




图327打开“_2017_0506_5_闭环”图层




图328打开“_2017_0506_5_闭环”GPS数据


重复上述步骤，将第2组GPS单点定位数据(没有RTK)“2017_0506_6_没有RTK.txt”导入MapInfo； 另外，在名称为“20170506_*.txt”的10个GPS轨迹数据中选取*等于自己学号最后一位的数据作为第3组GPS数据，导入MapInfo中，最终效果如图329所示。






图329成功打开3组GPS数据


3. ArcGIS软件简介
ArcGIS是美国ESRI公司研发的构建于工业标准之上的无缝扩展的GIS产品家族。它整合了数据库、软件工程、人工智能、网络技术、云计算等主流IT技术，为用户提供一套完整的、开放的企业级GIS解决方案。无论是在桌面端、服务器端、浏览器端、移动端乃至云端，ArcGIS都有与之对应的组件，并且可由用户自由定制，以满足不同层次的应用需求。ArcGIS 是一个全面的系统，用户可用来收集、组织、管理、分析、交流和发布地理信息，作为世界领先的地理信息系统构建和应用平台，可供全世界的人们将地理知识应用到政府、企业、科技、教育和媒体领域。ArcGIS桌面产品是一系列整合的应用程序的总称，包含ArcMap、ArcCatalog、ArcToolbox以及ArcGlobal等在内的用户界面组件。这里主要使用的是ArcMap。 
首先介绍ArcGIS软件的安装步骤，这里的安装步骤与MapInfo很类似。在安装文件夹中双击Setup.exe启动安装程序； 在弹出的欢迎界面单击Next按钮； 勾选I accept the master agreement单选按钮后单击Next按钮，在Selection Installation Type中选择Complete，安装完整功能； 在弹出的Destination Folder中可以自定义选择软件安装路径，这个安装路径需要记住，后续需要在安装路径下进行破解和汉化，选择完毕后单击Next按钮； 在弹出的Python Destination Folder中设置Python文件夹，可以按照默认设置，单击Next按钮； 在弹出的Ready to Install the Program中，可以勾选或者不勾选用户体验提升项目，然后单击Install按钮开始安装； 等待几分钟后安装完成，单击Finish按钮； 在弹出的ArcGIS Administrator Wizard中选择Advanced(ArcInfo)Concurrent Use并单击OK按钮，再在弹出的安装完成的弹窗中也单击OK按钮即可。
然后，安装完成后需要进行破解和汉化。具体的破解步骤为，将安装包文件内Crack文件夹下的Afcore.dll文件复制到刚刚安装软件时选择的安装路径内的bin文件夹内，如C:＼Program Files (x86)＼ArcGIS＼Desktop10.7＼bin，注意复制时对同名文件选择“替换”； 具体的汉化步骤为，进入安装包文件夹内的ZHCN文件夹，将ZHCN文件夹内的所有文件复制到软件安装路径下，如C:＼Program Files (x86)＼ArcGIS＼Desktop10.7＼，注意复制时对同名文件选择“合并”(注意这里和上面不一样，不能选择“替换”)。
4. ArcGIS软件操作
以清华大学艺术博物馆附近的路段为例，介绍ArcMap中的一些基本操作，具体任务与MapInfo软件执行内容相同，介绍如何导入清华大学艺术博物馆路段的地图数据，如何在nav_point图层增加一个点，如何在nav_link图层增加一个路段以及如何进行GPS数据的处理及导入操作。
1) 导入清华大学艺术博物馆附近路段的数据
ArcMap软件中的数据格式区别于MapInfo中的TAB文件，在ArcMap中应该用SHP文件。
以管理员身份运行打开ArcMap，在弹出的启动对话框中选择“空白地图”并单击“确定”按钮，如图330所示。接下来导入SHP文件数据，如图331所示，有两种方式，第一种方式是右击“内容列表”中的“图层”，再单击弹出的“添加数据”； 第二种方式是在工具条中找到“添加数据”图标，直接单击图标即可。在弹出的文件选择框里把6个SHP文件选上，单击“添加”按钮即可。导入完成后的地图如图332所示。



图330启动界面




图331选择添加数据




图332添加地图


2) 在nav_point图层新增加一个点
该部分的目标与MapInfo中相同，是在nav_point图层增加一个点，该点的信息包括： 
经度： 116.322404； 纬度： 40.008168； MAPID： 595612； ID： 10000106； TYPE： 81506044； NAME： 天猫超市； PREFIX： 10。
要求： 上述给定的属性定义要精确，其他属性可空缺或自定义。
如图333所示，右击nav_point图层，在弹出的菜单中选择“编辑要素”→“开始编辑”选项。如图334所示，在“图层”中右击nav_point图层，单击“打开属性表”选项，与MapInfo中的操作相同，在属性表最后添加上述“天猫超市”点的信息。保存后可在地图中看到“天猫超市”的信息，如图335所示。



图333打开编辑




图334打开属性表




图335添加完成天猫超市


3) 在nav_link图层增加一个路段
该部分的目标与MapInfo相同，在nav_link图层增加一个路段，路段的信息包括： 
线型： 直线； 起点经度： 116.319431； 起点纬度： 40.017938； 终点经度： 116.348997； 终点纬度： 40.017445； MAPID： 595612； ID： 10249296； NAME： ArcGIS实习。
要求上述给定属性定义要精确，其他信息可空缺或自定义。
具体的操作步骤如下。
按照上述步骤2)中在nav_point图层先添加起点和终点，然后右击nav_link图层,在弹出的菜单中选择“打开编辑”选项，按如图336所示打开“捕捉工具条”并将工具条内的捕捉选项全选，使线条绘制时能够自动捕捉到起点和终点。接下来绘制直线路段，如图337所示，首先选择绘图工具条中的直线(如果无法单击直线，则执行“编辑器”→“编辑窗口”→“创建要素”命令)，在弹出的窗口内单击nav_link，然后捕捉单击起点，再捕捉单击终点，再右击“完成草图”，即完成直线路段的绘制。接下来对新画的路段补充元素，打开nav_link图层属性表，在最后一行可以看到自动生成的属性信息，在名称字段部分添加“ArcGIS实习”，保存即可。添加直线路段完成后如图338所示。



图336打开捕捉工具条并将工具条内选项全选




图337选择直线，连接起点和终点




图338添加直线路段属性


4) GPS数据导入软件操作
该部分对MapInfo中使用的3组GPS数据进行导入操作，目前已将GPS数据的TAB文件格式转换成SHP文件格式，只需要在菜单栏中单击“添加数据”选项，选择“2017_0506_5_闭环.shp”“2017_0506_6_没有RTK.shp”“20170506_*.shp”三个文件导入即可，添加后如图339所示(图中选择的是20170506_3.shp文件)。


图339添加GPS数据


3.3.2定位源介绍
多数场景下，汽车定位需要依赖全球导航卫星系统（global navigation satellite system,GNSS），因此，开展基于卫星的车辆定位实验，需要了解GNSS常用的标准协议。GNSS测量中常用的标准数据格式包括RINEX、SP3、NMEA0813和RTCM等。RINEX(receiver independent exchange format,与接收机无关的交换格式)是一种在 GPS测量应用中普遍采用的标准数据格式。该数据记录格式与接收机的制造厂商和具体型号无关，主要包含观测值文件、导航电文文件、气象数据文件、GLONASS导航电文文件、GEO导航电文文件及卫星和接收机钟文件。SP3精密星历数据格式的全称是标准产品第3号(standard product #3)，它是一种在卫星大地测量中广泛采用的数据格式，由美国国家大地测量局(national geodetic survey,NGS)提出，专门用于存储 GPS卫星的精密轨道数据。SP3格式文件是文本文件，其基本内容是卫星位置和卫星钟记录，另外，还可以包含卫星的运行速度和钟的变率。RTCM（radio technical commission for martime services）是由国际海运事业无线电技术委员会设立的差分定位数据标准。RTCM差分协议共定义了 64种电文，每种电文帧长为(N+2)个字，其中电文头两个字称为通用电文。电文信息包含在N个字中，N随电文类型不同而不同，同类电文可能由于卫星的个数不同也不相同。
车辆定位中，应用最广泛的是NMEA0813数据格式。NMEA0183协议是 GNSS 接收机应当遵守的标准协议，也是目前 GNSS 接收机上使用最广泛的协议，大多数常见的 GNSS 接收机、GNSS 数据处理软件、导航软件都遵守或至少兼容这个协议。GNSS接收机根据NMEA0183协议的标准规范，将位置、速度等信息通过串口传送到PC机、PDA等设备。所有遵循NMEA0183协议的GPS语句可采用ASCII码格式，以“＄”开头，后面是语句头。语句头分两部分: 前两个字母表示“系统ID”，表示该语句属于何种系统或设备； 后3个字母表示“语句ID”，表示该语句是关于哪个方面的数据。语句头后面是数据体，包含不同的数据字段。NMEA0813常用命令如表31所示。


表31NMEA0813 常用命令



序号命令说明最大帧长


1$GNGGAGPS/北斗定位信息72
2$GNGSA当前卫星信息65
3$GPGSV可见GPS卫星信息210
4$BDGSV可见北斗卫星信息210
5$GNRMC推荐定位信息70
6$GNVTG地面速度信息34
7$GNGLL大地坐标信息—
8$GNZDA当前时间(UTC1)信息—
 
以GNGGA为例，其语句的基本格式如下所示。其中M指单位米; hh指校验和; CR和LF代表回车符和换行符。



$GNGGA,(1),(2),(3),(4),(5),(6),(7),(8),(9),M,(10),M,(11),(12)*hh(CR)(LF)




(1) UTC 时间，格式为 hhmmss.ss。 
(2) 纬度，格式为 ddmm.mmmmm(度分格式)。 
(3) 纬度半球，N 或 S(北纬或南纬)。 
(4) 经度，格式为 dddmm.mmmmm(度分格式)。 
(5) 经度半球，E 或 W(东经或西经)。 
(6) 卫星状态，0=未定位，1=非差分定位，2=差分定位。 
(7) 正在使用的用于定位的卫星数量(00~12)。
(8) HDOP 水平精确度因子(0.5~99.9)。
(9) 海拔高度(-9999.9~9999.9m)。
(10) 大地水准面高度(-9999.9~9999.9m)。
(11) 差分时间(从最近一次接收到差分信号开始的秒数，非差分定位，此项为空)。
(12) 差分参考基站标号(0000~1023，首位 0 也将传送，非差分定位，此项为空)。
举例如下： 



$GNGGA, 095528.000, 2318.1133, N, 11319.7210, E, 1, 06, 3.7, 55.1, M, -5.4, M, 0000*69 





3.3.3UWB定位方法
本实验基于UWB设备实现室内定位。在实验室的沙盘边缘，可以找到四个黑色的UWB设备，这四个就是固定不动的基站，下文称之为Anchor； 在每台实验小车的尾部，都有一个UWB设备，这就是用于定位的标签，下文称之为Tag。车上的Tag能够测量每个Anchor的距离，本次实验的目的就是根据小车上的Tag到四个Anchor的距离计算出小车在基站坐标系中的位置坐标，并按照指定格式发布出来。实验沙盘示意图如图340所示。通过到多个Anchor距离与参考Anchor的坐标，可以列出多组球面方程，进而由数学方法可以求解出标签的坐标，图341所示为常见的三边定位原理示意图。注意，当Anchor与Tag同属一个平面时，定位问题退化为二维定位问题，球面方程也就变成了圆方程。在沙盘实验中，车辆并不会垂直地面运动，本次实验求解的坐标值是二维坐标。


图340实验沙盘示意图




图341求解小车坐标示意图


求解出二维坐标后，在小车上通过指定的cyber消息发布出来，可以在上位机看到定位的效果，并看出小车在地图中对应的位置。与此同时，也可以用键盘的W、A、S、D键控制小车在沙盘运动，同时观察上位机上定位结果的变化。


理论上，当Anchor(固定点)坐标固定且已知，各Anchor与Tag求解距离精确无误差时，设Tag坐标为x、y，可以列写方程如下： 

(x－A0x)2+(y－A0y)2=d20

(x－A1x)2+(y－A1y)2=d21

(x－A2x)2+(y－A2y)2=d22

根据此方程，在基站坐标已知、距离可测的情况下就能通过联立求解的方式求出x和y的值，即Tag的坐标。在本实验场景中，基站有4个，且基站坐标已知，如果两两联立求解方程式，则问题就变得有些复杂了。需要联立6组方程，从12个解中排除8个解，再对最终4个解求取平均。但这个方式显然缺少扩展性，且从编程实现的角度讲，逻辑有些复杂。假如将来做三维定位，使用8个基站，那么用这种方式显然会变得越来越复杂。


本实验将学习使用梯度下降法求解定位。在求解本实验的方程组时，也可以将求解的迭代过程看作下山的过程。在本实验中，存在4个Anchor，因此求解Tag的位置需要列方程组如下： 

(x－A0x)2+(y－A0y)2=d20

(x－A1x)2+(y－A1y)2=d21
(x－A2x)2+(y－A2y)2=d22

(x－A3x)2+(y－A3y)2=d23

在现实情况下，上述方程无解。在上式基础上引入一个误差的概念，不存在任何一组x、y可使得误差error为0。

c1=(x－A0x)2+(y－A0y)2－d20

c2=(x－A1x)2+(y－A1y)2－d21

c3=(x－A2x)2+(y－A2y)2－d22

c4=(x－A3x)2+(y－A3y)2－d23

error=(c21+c22+c23+c24)

已知Anchor坐标与测量距离时，error只于x、y的取值有关。只要找到一组(x,y)，使得error的值比其他所有组(x,y)所对应的error都要小，该组(x,y)就是此方程的最优解。而error就是图342中“山”的高度(纵轴)，x、y的取值就是图342的横轴。在求解的过程中，总是存在一个初始点(x,y)，如果想要误差最小，就要找到当前能使error减小的方向，使x、y向此方向变化。想要判断error减小的方向，就要通过求微分的方式进行判断。本实验中的问题是二维的，因此梯度包含两个分量。两个分量分别是error对两个自变量x与y的偏导数。



图342二维梯度下降法示意图


了解了以上内容，就可以套用梯度下降法的公式直接解决方程的求解问题。

θi=θi－αθiJ(θ)

其中，θ是待求解的参数集合，在本问题中就是(x,y)； θi则代表其中一个参数x或y； J代表error, α为迭代的步长。将公示套在本实验的问题中，可得： 

x=x－αxJ(x,y)

y=y－αyJ(x,y)

其中，xJ(x,y)表示J对x求偏导，其含义为J(x,y)处x方向的导数。此项为正，代表x增大J增大； 为负则代表x增大J减小。当此项为正时，x要减小才能使J减小，因此x=x－αxJ(x,y)。α的含义是步长，也是整个算法中需要调校或凭经验选取的数值。如果步长太小，则需要迭代很久才能使J最小； 如果步长太大，可能无法收敛到J的最小值。
还剩下最后一个问题，如何求解xJ(x,y)和yJ(x,y)?求解偏导数有两种方法： 第一种方法是将J的式子列写出来，手动分析求出或用计算机辅助求出偏导的表达式，将其写入程序由计算机运算迭代； 另一种方法是在对精度要求并不极高的情况下，直接使用数值方法进行计算，直接求取xJ(x,y)的数值。

xJx,y≈Jx+d,y－Jx－d,y2

上式中d取比较小的数，如0.01甚至更小。这种方法在求取数值微分时非常常见，请熟练掌握，活学活用。如果最终J稳定在一个数值，则说明最终迭代计算得出的x、y就是最优解，即Tag的坐标。 
3.4实验步骤
3.4.1基于高精地图的室外定位实验

实验以清华大学艺术博物馆附近的高精地图数据为基础，学习基于MapInfo软件编辑操作(点、线、面要素的编辑)以实现地图更新，并将外业采集的GPS定位数据导入MapInfo中，获取在高精地图中所处位置与周围环境信息。具体任务： 在MapInfo软件中打开待更新的高精地图，根据道路变化更新情况，在高精地图中对点、线、面等几何要素进行添加和删除，实现地图更新，并将GPS数据导入，获取当前所处位置及周边环境，显示高精地图中的定位效果。
操作步骤如下。
1. 在MapInfo软件中导入待更新的高精地图数据
本实验使用的高精地图为清华大学艺术博物馆附近区域，该高精地图有待更新，首先介绍如何在MapInfo软件中打开数据。
在MapInfo中，执行“菜单栏”→“文件”→“打开”命令，文件类型选择.tab，将100101bxline.tab(标线)、100109BXDP_JT.TAB(标线箭头)、200101dm.tab(地面)、200102HLDP_LG.TAB(护栏)、300101diangan.tab(电杆)全部打开，打开后如图343所示，该高精地图包括标线、标线箭头、地面、护栏、电杆等图层信息，读者可以在图层控制区通过勾选或取消勾选图层，查看单独每个图层的图像信息。



图343打开高精地图


2. 基于点、线、面等几何要素基本操作实现在高精地图中进行更新
高精地图数据中的一些路段、电杆、地面等点、线、面元素，需要进行删除和更新元素的补充。下面学习如何在高精地图中对点、线、面等几何要素进行删除和添加的操作，以及如何对新添加的元素进行属性信息补充，在掌握高精地图的编辑同时，实现地图更新。
1) 点元素更新(删除与添加)
首先是对点元素进行删除与添加的操作。电杆图层_300101_diangan属于点元素图层，在这个图层里记录了艺术博物馆附近的电杆地理位置信息，但是该图层缺漏了部分电杆或不慎将一些其他物品误认成电杆，这时就需要对电杆进行增加和删除操作。下面介绍如何实现电杆等点元素的增加和删除。
首先在图层控制区找到电杆图层_300101_diangan，将“编辑模式”打开，如图344所示，使该图层处于可以编辑的状态。然后，在绘图工具条中找到“符号”并单击，如图345所示，此时便可以在高精地图中添加电杆。在想要添加电杆的位置单击鼠标左键，即可完成电杆点元素的添加，如图346所示，在方框处添加了3个电杆。


图344打开电杆图层编辑模式




图345单击绘图工具条中的“符号”




图346添加点元素


在地图上手动添加完点后，需要对添加的3个点的属性信息进行补充，在图层控制区里右击电杆图层_300101_diangan，在弹出的菜单中选择“查看属性表”，如图347所示，在属性表最后可以看到新增加的3行，分别对应刚刚手动添加的3个点，FID属性信息默认为0，自行修改FID属性为19、20、21，即完成所添加点的属性信息的补充，如图348所示。


图347电杆图层属性表




图348更新新添加点的属性信息


如果是想要删除地图中冗余或错误的电杆点元素，或者在绘制过程中需要修改删除，则选中想要删除的电杆，并在键盘按下Delete键可完成电杆的删除，读者可以自行尝试删除一些电杆，如果想撤销对电杆点元素的删除，可按Ctrl+Z组合键。





2) 线元素更新(删除与添加)
该部分实验学习对地图中线元素的添加和删除操作，以实现地图要素中线元素特征物的更新。
导入标线图层_100101_bxline属于线元素图层，记录了清华大学艺术博物馆附近的道路等线元素，其中可能缺漏了部分道路边线或者有一些误加入的线条，这时就需要对相应的线元素进行增加和删除操作，从而实现道路边界线的更新。下面介绍如何实现对线元素的增加和删除。
在图层控制区找到标线图层_100101_bxline，将“编辑模式”打开。在绘图工具条中可以找到“线”“折线”“弧线”等线元素绘制工具，如图349所示，分别可以绘制直线、折线、弧线等线元素。为了方便看清线条，可在图层控制区将其他图层取消勾选，如图350所示。以图351所示的图层右下侧道路为例，该部分道路明显有残缺，需要手动将其补充完整。该部分道路属于直线道路，因此在绘图工具条中单击“线”，并按S键打开SNA模式，此时可以捕捉到点元素，手动将直线道路补充完整，补充后如图352所示。在补充完成后，可以对添加的线条属性信息进行补充，打开标线图层属性表，如图353所示，可看到属性表的最后增加了若干空行，这些空行即对应刚刚手动添加的线条，在属性表里填充这些信息，即完成所添加线条的属性信息的补充，如图354所示。




图349线元素绘制工具




图350只显示标线图层




图351图层右下侧路段




图352添加线元素更新道路信息




图353标线图层属性表




图354更新线元素属性信息


删除线元素的操作与删除点元素相同，如果发现在标线图层中存在冗余或者错误道路线条，或者在地图编辑过程中不慎错误绘制了线条，选中该线条后按Delete键即可，也可自行选择线元素尝试删除，如果想撤销对线元素的删除，可按Ctrl+Z组合键。
3) 面元素更新(删除与添加)
该部分实验学习对面元素的添加和删除操作，以实现地图面状区域的更新操作。
在导入的图层中，地面图层_200101_dm属于面元素图层，在这个图层里记录了清华大学艺术博物馆附近的地面等面元素，如果有部分地面被遗漏或者出现错误，需要进行删除和添加面元素的操作。
如图355所示，在图层控制区只勾选地面图层_200101_dm，使地图区只显示地面元素，便于分辨，然后将地面图层的“编辑模式”打开。在绘图工具条中可以找到“多边形”“椭圆”“矩形”“圆角矩形”等面元素绘制工具，如图355箭头处所示。如图356所示，在清华大学艺术博物馆西侧(图中左侧)空缺部分增加一个地面元素，以“多边形”为例，通过连续确定多边形的多个顶点，为地面图层补充了一个多边形地面。然后需要对新增加的地面元素补充属性信息，右击地面图层打开属性表，在最后一行补充其属性信息，如图357所示。




图355打开地面图层编辑模式




图356添加多边形地面




图357补充更新面元素的属性信息


如果遇到冗余或错误的面元素，或者在绘制面元素的过程中需要修改及重新绘制，则需要删除面元素，删除面元素的操作与删除点、线元素相同，选中后按Delete键即可，读者可自行选择面元素尝试删除，如果想撤销对面元素的删除，可按Ctrl+Z组合键。
4) GPS数据导入获取所在位置与周围环境信息
该部分实验主要实现在MapInfo中导入室外采集并处理好的三组GPS数据，分别是GPS闭环轨迹数据、GPS在没有RTK情况下的轨迹数据以及与另外一条GPS轨迹数据，将三组GPS数据与高精地图叠加显示，以下是具体操作步骤。

首先选择“菜单栏”→“文件”→“打开”命令，在弹出的文件选择对话框中，找到并打开预习环节中处理好的三组GPS数据对应的TAB文件(“2017_0506_5_闭环.TAB”“2017_0506_6_没有RTK.TAB”“20170506_*.TAB”，*等于自己学号最后一位)，如图358所示，打开三个TAB文件后即完成GPS数据在高精地图中的叠加显示，其中红色、黄色、蓝色线条分别对应三组GPS轨迹数据，可以看到采集的GPS数据与道路元素基本重叠，表示真实车辆围绕清华大学艺术博物馆附近停车场行驶的轨迹。GPS轨迹在地图中的颜色和点型可以自定义改变，例如，在图层控制区右击_20170506_3图层，单击图层属性，在图359所示的弹窗中的Layer Display选项卡中勾选Style Override复选框，并单击下方方框，即可修改符号样式和颜色。



图358打开GPS数据




图359自定义设置线条颜色和点型


5) 基于高精地图定位显示与功能展示 
高精地图拥有精确位置信息、丰富道路元素的数据，通过精细化描述道路、车道线、路沿、交通标志等静态与动态信息，为自动驾驶定位、规划、决策、控制等应用提供安全保障，可以帮助汽车定位、预先感知路面复杂信息，结合智能路径规划，让汽车做出正确决策，是自动驾驶领域的核心与基础。
为更好地了解高精地图，通过在MapInfo中分别添加GPS原始定位数据与融合惯导之后的定位数据，观察二者的区别，也进一步理解高精地图中的定位效果。
首先，选择“菜单栏”→“文件”→“打开”命令，在图360所示弹出的对话框中找到并选择novatel.TAB与ublox.TAB两个文件，单击打开。其中，novatel.TAB是GPS融合惯导之后的定位数据，ublox.TAB是原始的GPS定位数据。打开两段定位数据后，可以按照之前的方法修改融合定位数据图层novatel的颜色属性为红色，修改后如图361所示。


图360打开两组数据




 图361成功打开并修改颜色属性


接下来观察两组数据，如图362所示，取消勾选融合数据图层novatel，使融合数据曲线暂时不可见，可以观察到，原始GPS定位数据能够将车辆运行轨迹较准确地记录下来，但是这种定位精度局限于道路级别，精度为米级，在左下位置GPS轨迹出现了明显偏离车道的弯曲部分。再单独观察融合定位数据，如图363所示，取消勾选原始GPS数据图层ublox，并勾选融合数据图层novatel，只显示融合数据曲线，可以观察到，融合后的数据能够精确记录车辆在车道内的轨迹信息，定位精度可以达到车道级别。放大左下部分进行对比，如图364所示，两组数据在定位精度上的区别更加明显。



图362单独显示原始GPS定位数据




图363单独显示融合定位数据




图364放大显示在高精地图中的定位效果


高精地图的精度达到厘米级，能够协同支持GPS组合导航定位，同时在自动驾驶智能感知、自主决策与规划等诸多方面也发挥着重要作用。
3.4.2基于UWB的室内定位实验
通过了解UWB定位原理与求解方法，进行基于UWB的室内定位实验，实现基于电子沙盘的UWB室内定位。实验任务包括定位信息显示、UWB定位实验以及将定位结果转换到WGS84坐标系中。
操作步骤如下。
1. 定位信息显示
这一部分实验仅对录制好的定位数据进行显示，不涉及实车实验。
(1) 在Docker环境下，使用命令bash docker/scripts/into_geek.sh进入容器环境。Docker容器的安装方法以及登录方法请参照实验1相关介绍，代码如下： 



root@in_dev_docker:/apolLo# bash apollo.sh butldsystem check passed. Build continue

Running butld under CPU mode. No GPU is required to run the build.「INFo］ Start building, please wait ....

INFO: Reading 'startup' options from /apollo/tools/bazel.rc: batchcpuscheduling host_jvm_args=xx:useParallelGC

………………………………………….

Loading package: modules/perceptior

Loading package: cyber/service discovery

Loading package: cyber/class loader/test

Loading package: cyber/python/cyber_py/examples

Loading package: cyber/sysmo

Loading package: cyber/examples/common component example

Loading package: cyber/message「INFol Butlding on x86 64..

「INFO］ Building withjobs8ram utilization factor 80 for x86 64

INFO: Reading 'startup' options from /apollo/tools/bazel.rc: batch_cpu_scheduling host_jvm_args=xx:UseParallelGC

INFO: (0113 07:44:59.513) Found 696 targets...

INFO: (0113 07:46:31.862) Elapsed time: 95.303s, Critical Path: 14.21s.

=====================

［ OK ］ Build passed!

［INFo］ Took 103 seconds

=====================





在启动的容器里，下载测试的定位数据包，地址是https://github.com/forgeeks/playground/releases /download/0.2/localization.record.00000，运行wget命令即可下载，代码如下所示。如果显示localization.record.00000 saved则表示数据下载成功。



root@in_dev_docker:/apollo/data/bag# wget 

https://github.com/forgeeks/playground/releases/download/0.2/Localization.record.00000

20210113 15:51:59 https://github.com/forgeeks/plavaround/releases/download/0.2/localization.record.00000

Resolving github.com (github.com)... 52.74.223.119

Connecting to github.com (github.com)|152.74.223.119|:443... connected

HTTP request sent, awaiting response... 302 Found

Length: 2118400 (2.0M) ［application/octetstream］

Saving to: 'localization.record.00000'

20210113 15:52:20 (112 KB/s).'localization.record.00000' saved ［2118400/2118400］



root@in_dev_docker:/apollo/data/bag#





(2) 通过data_record命令回放localization.record.00000数据(cyber_record playf localization.record.00000)。运行如下cyber命令可能出现command not found错误。




root@in dev docker:/apollo# cyber_node list

bash: cybernode: command not found




在/apollo文件夹运行如下source cyber/setup.bash命令即可。



root@in dev_ docker:/apollo# source cyber/setup.bash




在正常播放的情况下会显示数据记录时间以及数据播放进度，代码如下： 



root@in_dev_docker:/apollo# cyber recorder play f data/bag/localization.record.00000

file: data/bag/localization.record.00000, chunk_number: 2, begin_tine: 1577388561236920243 (20191227 03:29:21), end time: 1577388595030675751 (20191227 03:29:55), message number: 6533

earliest_begin_time: 1577388561236920243, latest_end_time: 1577388595030675751, total_msg_num: 6533



Please wait 3 second(s) for loading....

Hit ctrl+C to stop, Space to pause, or 's' to step.



［RUNNING］ Record Time: 1577388574.550Progress: 13.313/ 33.7941





(3) 打开上位机软件(上位机软件打开方法参见实验1相关介绍)，单击定位实验按钮,应该能够观察到绿色轨迹，如图365所示。这就是UWB自带的定位信息。



图365上位机上实时显示的定位结果


测试过程中可能出现定位结果无法实时在上位机上显示的情况，此时需要将cyber/setup.bash里的CYBER_IP改为172.17.0.1，上位机build/apollo_edu_terminal.sh里的IP地址为127.0.0.1。代码如下： 



export CYBER_PATH=s(cd "s(dirname "S{BASH SOURCE［O］)")" && pwd)

binary_path="/apollo/bazelbin/cvber"

cyber_tool_path="/apollo/bazelbin/cyber/tools"

apollotoolpath="/apolLo/bazelbin/modules/tools"

recorder_path="sfcyber toolpath}/cyber_recorder"

monitor_path="${cyber_toolpath}/cyber monitor"

visualizer_path="$fapolLo_tool path}/visualizer"

PYTHON_LD_PATH="/apolLo/bazelbin/cyber/py wrapper"

Launch_path="$ECYBER PATH)/tools/cyber launch"

channel_path="SfCYBER PATH}/tools/cyber channel"

node_path="S CYBER PATH/tools/cvber node

service_path="SfCYBER PATH}/tools/cyber service"qt_path=/usr/local/Qt5.5.1/5.5/gcc, 64

rosbaq_to_record_path="/apolLo/bazelbin/modules/data/tools/rosbag_to_record"



export_LD_LIBRARY_PATH=Sfat path/lib:SLD_LIBRARY_PATH

export_QT_QPA_PLATFORM_PLUGIN_PATH=Sfatpath}/plugins

export_PATH=s{binarypath}: s{recorderpath) : s{monitorpath} :s{Launchpath} : s{channelpath) : ${nodepath):s{servicepath}:$fqtpath}/bin:s{visualizer_path}:sfrosbag_ to_record path}: SPATH

export_PYTHONPATH=S PYTHON LD PATH}:SfCYBER PATH}/python: SPYTHONPATH



export CYBER_DOMAIN_ID=80

# export CYBER_IP=127.0.0.1

export CYBER_IP=172.17.0.1



export GLOG_Log_dir=/apollo/data/log

export GLOG_alsologtostderr=0

export GLoG_colorlogtostderr=1

export GLOG_minloqlevel=0



export sysmo_start=0



# for DEBUG Log

#export GLOGminloqlevel=1#export GLOG v=4



source S{CYBER_PATH}/tools/cyber_tools_auto_complete.bash



"cyber/setup.bash" 36L, 1418C





(4) 另起第二个终端进入Docker，输入cyber_monitor命令，可看到如下所示界面，如果查看到与UWB相关的信息被发送，则说明启动成功(绿色信息表示cyber_monitor正在接收的信息)。



ChanneLs FrameRatio

/coefficient 0.00

/chassis 16.66








/control 20.92

/control_reference0.00

/geek/acc50.06

/geek/gyro 49.99

/geek/uwb/localization0.00

/geek/uwb/poselocalization 50.00

/localization/kalman_filter0.00

/monitor 0.00

/perception/control trajectory 0.00

/perception/get_point 0.00

/perception/lane Line 0.00

/perception/obstacles 0.00

/perception/road_left_potnt 0.00

/perception/road_mean_point 0.00

/perception/road_right_point0.00

/perception/translation_point 0.00

/perception/vertical_view 0.00

/planning/dwa_trajiectory 0.00

/planning/global_traiectory 0.00

/planning/mission_point 0.00





2. UWB定位
这部分是针对小车的UWB定位实验，其主要实验步骤如下。
(1) 将UWB设备放置在沙盘上，通过5V移动电源对其供电。四个UWB设备的位置如图366所示，如果其相对位置发生改变，需要测量它们之间的相对距离，并在设置文档中重新填写距离参数。



图366UWB设备在沙盘上的分布


(2) 为小车通电，并确认已经启动系统。登录上位机系统(参照实验1相关介绍)，连接小车启动的无线网络(如Apollo_Edu_005, 无密码)。
(3) 在上位机打开一个终端，ssh登录到小车上(ssh +X geekcar@10.42.0.1, 密码123456)，代码如下： 



edu003gedu003ThinkPadT490: ~$ ssh X geekcar@10.42.0.1

geekcar@10.42.0.1's password:

Welcome to ubuntu 18.04.2 LTS (GNU/Linux 4.9.14etegra aarch64)



* Documentation: https://help.ubuntu.com

* Management: https://landscape.canonical.com

* support: https: //ubuntu.com/advantage



This system has been minimized by removing packages and content that are not required on a system that users do not log into.



To restore this content, you can run the 'unmintmize' command.



368 packages can be updated.

0 updates are security updates.



Last login: Fri Jan 10 20:35:05 2020 from 10.42.0.98

geekcar@geekcardesktop:~$





切换到代码目录/apollo/modules/exercises/exercise5localization/，编辑样例代码localization_todo_3.py。操作过程如下:



geekcar@geekcardesktop:~ $ cd geek lite/modules/exercises/exercise5localizaticn/

geekcar@qeekcardesktop:~/geek Lite/modules/exercises/exercise5localization$ ls



example localization todo.py

geekcar@geekcardesktop: ~/qeek lite/modules/exercises/exercisesLocalizations$ gedit localization_todo.py





补全实验样例代码中的problem,slope_fx, loss_fun函数，代码如下所示，反复执行、调试至正常运行，直至静态下输入结果与UWB自身定位结果相近。



# 定义函数f(x)

def problem(self, x, data):

return 0



# 定义损失函数

def loss_fun(self, x, datas):

sum_err = 0;

return sum_err



# 计算损失函数的斜率

def slope_fx(self, x, datas):

J1 = 0

J2 = 0

return ［J1, J2］





其中，problem（）函数用于计算一次取值下的误差； loss_fun（）函数用于计算所有数据的总误差； slope_fx（）函数用于计算梯度下降的梯度。参考本实验中的公式以及example代码完成三个函数后，执行python localization_todo.py。
(4) 在cyber_monitor中查看是否存在/uwb/localization 消息，代码如下所示。如果有，则说明执行成功。



Channels FrameRatto

/Coefficient 0.00

/chasis 16.78 

/control20.06 

/control_reference0.00 

/geek/acc 50.19

/geek/gyro50.00 

/geek/uwb/localization24.61 

/geek/uwb/pose50.05





详细定位消息如下所示。 



channelName: /geek/uwb/localization

MessageType: apollo.localization.pos

FrameRatto: 29.54

RawMessage size: 36 Bytes

x: 1.123999953

y: 2.575999975

z: 3.062905177

yaw: 0.799187662





(5) 在上位机界面中可以看到红色定位轨迹和绿色定位轨迹。其中红色定位轨迹为用户完成的定位结果，绿色定位轨迹是UWB自身定位结果，后者可以用作定位结果的参考值。如果两者的位置比较接近，说明读者较好地完成了定位实验； 如果两者的位置相距较远，则可以对定位源码继续进行优化，直至达到较好的定位效果。可以看出，绿色轨迹和实车位置较为接近(见图367和图368)。



图367小车实际位置




图368小车定位结果


3. 将定位结果转换到WGS84坐标系
坐标转换的步骤如下。
(1) 切换到代码目录/apollo/modules/exercises/exercise5localization/，编辑样例代码coodinate.py。
(2) 补全坐标转换代码中xyz2llh（）函数和llh2xyz（）函数，使用样例数据反复执行、调试直至程序正常运行。代码如下： 



# 定义局部坐标系向WGS84坐标系转换函数xyz2llh()

def xyz2llh(self):

alpha = self.xyz［0］/self.L1

beta = self.xyz［0］/self.L2



# 定义WGS84坐标系向局部坐标系转换函数xyz2llh()

def llh2xyz(self):





(3) 设置四个Anchor在WGS84坐标系以及局部坐标系下的坐标，通过坐标转换函数将小车在局部坐标系下的定位结果投影到WGS84坐标系。
思考题 
1. UWB定位实验中，小车的静态定位精度和运动过程中的定位精度是否有差异？如果有差异，造成这种差异的原因可能是什么？
2. 针对UWB定位，除了梯度下降法，还可以通过什么方式求解小车位置？
3. 小车定位过程中，除UWB外还可以用到轮速计和IMU等传感器。请问可以怎样实现UWB+IMU+轮速计的融合定位？
4. 本实验由局部坐标系转换为WGS84坐标系用到的是简化版的坐标转换模型。请思考实际开展坐标转换时还需要考虑哪些方面的因素。
5. 自动驾驶高精地图与GPS定位数据叠加显示过程中，坐标系统对叠加结果会有哪些影响？ 
参考文献

［1］费立凡.MapInfo基础教程［M］.北京： 测绘出版社,2005.

［2］张明明,于沧海.ArcGIS 10.1超级学习手册［M］.北京： 人民邮电出版社,2015.

［3］曹冲.北斗与GNSS系统概论［M］.北京: 电子工业出版社，2016.

［4］李晓欢,杨晴虹,宋适宇,等.自动驾驶汽车定位技术［M］.北京： 清华大学出版社,2019.

［5］牟乃夏,刘文宝,王海银,等.ArcGIS 10 地理信息系统教程： 从初学到精通［M］.北京：  测绘出版社，2012.

［6］WILLIAMS R J.Simple statistical gradientfollowing algorithms for connectionist reinforcement learning［J］.Machine Learning,1992,8(34):229256.

［7］GROVES, PAUL D .Principles of GNSS,inertial,and multisensor integrated navigation systems［J］.Industrial Robot,2013,67(3):191192.

［8］ZHANG J, ORLIK P V,SAHINOGLU Z,et al.UWB systems for wireless sensor networks［J］.Proceedings of the IEEE.2009,97(2):31331.