【智能车-极速越野】3-软件程序设计

在此次开发中，由于引入了全新的赛道元素和传感器模块（如 GPS模块等），如何在编写程序时能够突破传统组别的思维束缚，合理地融合 GPS、IMU等模块的数据、在基础方案上进行深度优化和提高稳定性便成为了我们需要解决的核心问题，也是本次软件设计的重点所在。

本文将从软件系统整体设计入手系统分析各个模块的特性和数据处理方法，通过数据解算、滤波、各模块间数据融合的方式，最终实现对于车辆的精准控制，并实现车辆在高速运行状态下的稳定度和性能。

一、经纬度数据处理

我们想让小车按照规定的路径自主导航，那么第一点就是让小车做定位，将GPS与单片机进行串口通信传回来的数据如图所示。

实际上，我们需要进行处理的数据是经纬度信息，所以我们提取处理$GNRMC这样格式的数据即可。例如：

($GNRMC,085252.800,A,2840.23442,N,11549.19002,E,3.017,176.03,080322,0.0,0,A*68)

这时就需要把不用的信息滤除，可以用上位机去设置，也可以发送十六进制指令，这个需要看不同型号GPS对应的操作手册。

二、坐标系变换处理

这里需要注意，当把提取出来的经纬度放入百度地图里查看时，竟然发现定位相差甚远，这并不是模块精度问题。之所以会产生“偏差”，这就涉及到一个有关坐标系转换的问题：GCJ-02 火星坐标系统纠偏。模块输出数据的格式为NMEA-0183，使用的坐标系是WGS-84。而国内的地图软件大都是使用的GCJ-02坐标系。所以如果将WGS-84的经纬度填入国内的地图软件，就会产生极大的偏差。在这里，我们可以做一个坐标系变换处理。

把提取出来的经纬度放入百度地图里查看时，竟然发现定位相差甚远，不要着急，这并不是模块坏了。之所以会产生“偏差”，这就涉及到一个有关坐标系转换的问题：GCJ-02 火星坐标系统纠偏。模块输出数据的格式为NMEA-0183，使用的坐标系是WGS-84。而国内的地图软件大都是使用的GCJ-02坐标系。所以开发者如果将WGS-84的经纬度填入国内的地图软件，就会产生极大的偏差。

三、路径规划处理

获取到经纬度信息后，我们在路径上采集点位信息。假设采集7个点位，如图所示，那么按照顺序1—>2—>3—>4—>5—>6—>7—>1逐点前进，就可以完成一圈的导航。这里注意一下，采集点位时要抱着电脑一个点一个点去输入，我们将点位存在Flash里，通过查芯片手册，F3277有128个扇区，每个扇区有4页，一页有1K，存储点位信息不在话下，通过按键来触发存点。

点位采集完毕，下一步就是对点循迹，因为提前采好了每个点的坐标，我们利用两点经纬度来计算出两个点之间的距离以及方向角。解算得到的距离用来判断是否到达目标点（距离小于一定值），方向角来告诉小车该往哪个方向行驶。这时小车虽然知道了该往哪个方向走，但是不知道自己有没有朝向这个方向，这个时候就用上了IMU，使用九轴陀螺仪，利用其中的磁力计解算并输出角度值（东北天坐标系下），将GPS解算出的角度值和IMU输出的角度值做闭环pid，控制舵机打角。

四、航向角数据处理

1、IMU数据处理

由于JY901S模块带有自动解算功能，故我们直接读取其解算出的绝对角度值，但是其使用的是顺时针 0-360度坐标系，而 GPS使用的是 0-180坐标系，故为了方便后期计算，需要先将 IMU数据对应到 GPS所用坐标系上后在进行处理。

同时，正如上文所说，由于采用的是九轴模式，故需要进行对北，但在实际使用过程中，由于手动对北或多或少会有一点误差，且起始误差会对整个寻迹过程产生较大的影响，故需要在发车后对于 IMU的整体零偏进行修正。而由于 GPS解算出的绝对角度偏差不受对北影响，此处采用平均值解算修正的方法将发车初始阶段的 IMU角度数值对齐 GPS数值，使得对北误差显著降低。

而在实际运行中，为了解决高速运行中的 IMU误差问题，则对其数据和 GPS结算数据融合并进行卡尔曼滤波，使得误差不会过度发散，修正前后角度误差对比如下图所示：
（1）未滤波

（2）滤波后

其中蓝色为GPS解算值，红色为IMU解算值，可以看出在经过滤波修正之后IMU的误差大大缩小，足以满足实际需求。

我们可以选择使用六轴或九轴角度传感器来获取航向角，利用其中的磁力计解算并输出角度值（东北天坐标系下），将GPS解算出的角度值和IMU输出的角度值做闭环pid，控制舵机打角。我们使用的是维特智能的九轴角度传感器，上电自动校准。

维特智能的JY901模块是一款高精度的姿态测量模块，能够测量被测物体的姿态角度，以欧拉角或者四元素的方式输出。

2、航向角换算

模块的数据采用16进制的方式进行传输，这种方式的优点是效率高，可以用很少的字节传输需要的数据，比如一个数据1.523，如果用16进制方式传送的话，2个字节就够了，而用ascii码的话，需要5个字节，在需要大量数据传输的时候就比较浪费有限的带宽了。而16进制数据的缺点呢，就是可读性不强，不像Ascii码那样，直接就能读出数据了。

16进制数据传输的原理是这样的，先要确定数据的表示范围，然后是每个数据可用的字节数。比如角度的数据，每个角度的数据范围是±180度，而2个字节16进制数的表示范围是-32767~32768之间，那么我们就把±180的数据映射到±32768之间。假设原理的数据是x，变换以后的数据是y，那么y=x/18032768。2个字节能表示的最小精度是1/32768180=0.0055°，这对于角度的精度来说也够用了。

3、数据融合算法

若想让车跑的稳又快，以上是远远不够的，算法优化才是关键。我们的系统采用了松组合导航组合方式，如图3所示，在该方式下，GPS与IMU各自独立工作，最终将两者数据融合，用于修正IMU系统的相关参数，最终给出较好的导航估计结果。

对于GPS/IMU 的数据融合，给出以下四种方法：

(1) 加权平均法
利用过去若干个按照时间顺序排列起来的同一变量的观测值并以时间顺序变量出现的次数为权数，计算出观测值的加权算术平均数，以这一数字作为预测未来期间该变量预测值的一种趋势预测法。该方法从传感器提取数据，并将数据中的冗余信息作加权平均算法处理，以其结果作为融合值进行后续导航工作，简单好用。

(2) 卡尔曼滤波法
卡尔曼滤波方法通过系统输入输出观测数据，进行数据的递推，对系统状态进行最优估计。卡尔曼滤波方法因其稳定性、容错性良好，在飞行器追踪、惯性导航初始对准等方面得到广泛运用，是目前最广泛的滤波算法之一。

(3) 多贝叶斯估计法
贝叶斯估计法将传感器数据进行概率统计并组合，不确定性通过概率条件表达，当获取的传感器数据观测坐标一致时可直接对数据进行融合。多贝叶斯估计法将导航系统中的多个传感器输出数据分别制定一个贝叶斯估计，将各个关联概率分布合成后验概率分布，并使联合分布函数的似然数最小，以此得到融合数据。

(4)人工神经网络法
它从信息处理角度对人脑神经元网络进行抽象， 建立某种简单模型，按不同的连接方式组成不同的网络。神经网络以强大的学习性、自适应性及容错性著称，能够进行大量非线性映射的计算。在数据融合中，各信息源所提供的数据都具有一定的不确定性，神经网络根据样本的相似性特征进行分类处理，同时根据人为设定的学习算法进行学习，得到数据分类的推理机制，实现信息融合。

五、各主要功能电路图系统软件设计与流程图

1、小车控制主程序流程

本次智能车系统开发将以 GPS和 IMU两个传感器模块作为主要赛道数据来源，通过对二者数据的融合得出车辆姿态偏差值，再通过滤波及 PID等算法最终解算出输出值从而控制车辆的运动。
其中，由于 GPS和 IMU回传速率的不同，在实际运行中为了保证控制的实时性，将以 IMU数据回传速率（ 200HZ）作为数据处理和输出更新的频率，实测后发现以 5ms为周期的数据解算和控制能满足实际应用的需要。
该系统的总体设计框架和运行逻辑如下图所示：

2、无刷电机驱动板程序流程