基于FOC 的越野汽车系统设计与实现

戴测，袁杨洋，许霄竹，沈世斌

（南京师范大学 电气与自动化工程学院，江苏南京，210046）

0 引言

无人驾驶汽车因其安全性、稳定性和便捷性被视为未来汽车发展的重要方向。但是，室外环境复杂多变、单一传感器信息质量的抗干扰能力弱、多信息融合手段简陋等问题严重影响着车辆在城市中的定位，目前，高精度导航定位已经成为无人驾驶汽车发展道路上的一大挑战与阻碍。本课题以大学生智能汽车竞赛为背景，基于智能小车这一硬件载体，研究多传感器融合架构与算法实现方案，设计具有自主定位导航的无人车系统，并设计以FOC 为主体的无刷电机控制方案来为车模提供强大且稳定的动力。

1 磁场定向控制理论（FOC）

磁场定向控制(FOC) 理论以矢量变换为基础，将定子电流经过数学变换分解为转矩电流分量和磁通电流分量，分别控制转矩和磁通，把复杂的交流电机控制方式转换为直流电机控制。这种控制方式具有输出转矩平稳、调速范围宽、动态性能好、电压利用率高等优点，广泛应用于 PMSM 控制。FOC 控制的思路是选择电机某一旋转磁场轴作为特定的同步旋转坐标轴。磁场定向轴的选择有 3 种：转子磁场定向、气隙磁场定向、定子磁场定向。气隙磁场定向和定子磁场定向在磁链关系中均存在耦合，使得矢量控制结构更加复杂；转子磁场定向是仿照直流电动机的控制方式，利用坐标变换将交流电动机的定子电流分解成磁场分量电流(相当于励磁电流)和转矩分量电流(相当于负载电流) 并分别加以控制，即磁通电流分量和转矩电流分量二者完全解耦，从而获得类似于直流调速系统的动态性能。

2 基于卡尔曼滤波融合的多传感器导航系统

由于GPS 本身10Hz 频率的限制，面对越野车的高速行驶，显然100ms 更新一次的GPS 信息无法满足高速循迹时循迹所需求的稳定性。因此，我们需要利用惯性元器件陀螺仪的高更新速率的特性，对短时间范围内的坐标进行预测更新，来获得这100ms 以内的较为准确的车模自身的姿态。

针对越野车的本身运动情况，我们可以近似将其视为一个CTRV 的运动模型（恒定转弯率和速度幅度模型）。我们可以将GPS 所获得的坐标结合四元数的航向角速度作为一个观测量用来更新误差，利用短时间内所获得的预测量用于更新当前位置。至此，我们引入卡尔曼滤波来估计车辆位置。

常规的卡尔曼滤波，例如智能车直立组中会用它来对加速度计和角速度计进行滤波，用来获得当前的准确角度。但是常规的卡尔曼滤波仅仅适用于线性的模型，因为常规的卡尔曼滤波的状态转移矩阵不论如何相乘，都只能得到线性关系。

图1 Foc 控制框图

因此，面对CTRV 的运动问题，我们可以引入扩展卡尔曼滤波（Extend Kalman Filter），简称EKF。EKF 和通常我们所用的卡尔曼滤波一样，也需要状态转移矩阵，不同的是，由于卡尔曼滤波仅能用来处理线性的问题，EKF 会对非线性的观测方程以及预测方程进行求导，即在均值处进行一阶的泰勒展开。由上可得，只要对状态转移矩阵和观测矩阵求偏导数，即为求两个矩阵的雅各比矩阵，就可以获得当前状态近似的状态转移矩阵或是观测矩阵。

在获得EKF 的理论模型之后，我们队通过实地采集数据，用Python 建模对EKF 的有效性进行了验证。密集的蓝点为EKF 处理后的坐标，间断的红点为GPS 采集到的点。可以直观地看到，在使用相关算法后，大幅提升了位置信息数据更新的速度。

图2 扩展卡尔曼滤波示意图

3 整体系统方案设计

3.1 整体设计

系统框图如图3所示，由传感器系统、动力系统、转向系统、信号采集板、主控制板、电机驱动板以及供电几个模块构成。传感器系统负责环境数据的获取，为车模的环境感知提供数据。

图3 整体系统结构框图

图4 整体代码结构

本次设计中使用卫星定位模块、惯性测量单元与测距模块，构建了多传感器融合的定位系统。

转向系统由舵机与连杆构成，通过主控制板给定舵机信号控制舵机打角方向带动转向连杆从而控制车模运行方向。

信号采集板为自制印刷电路板，负责将各传感器的信号线与供电线汇集在一起，由一个接口统一连接到主控制板上，避免将各传感器线路直接接在主控制板上导致接线混乱。

主控制板负责室外越野车模的总体控制，是该系统的大脑。主控制板上的接口电路将传感器采集的信号送往单片机，单片机对这些数据使用特定算法进行处理，从而实现车辆的环境感知，进而对车模的电机、舵机发出控制信号，完成室外环境下整个系统的无人运行。

电机驱动板负责驱动直流无刷电机，是整个系统的动力控制单元，通过设计无刷电机驱动板上单片机的算法能够高效、合理地对无刷电机的转矩、转速进行闭环控制，并且在板上留有与主板通讯的接口，可以实现获取主控制板设定的目标速度后对无刷电机的转速进行控制，达到该目标速度，也可将当前电机的实际转速实时反馈给主控制板。

3.2 整体代码结构

首先，打开电源开关车模上电，单片机 TC387 上电启动，各模块初始化，依次为：

（1）按键初始化；

（2）屏幕初始化；

（3）舵机初始化；

（4）陀螺仪初始化；

（5）无刷电机初始化；

（6）激光测距初始化；

（7）GNSS 模块初始化；

（8）串口中断初始化；

（9）人机交互界面初始化；

（10）定时器中断初始化。

通过按键选择是否进行经纬度数据采集，如果进行采集则可通过按键将数据存入单片机并在采集完成后进行预处理。如果不需要采集，则直接读取 Flash 数 据并开始导航，计算实时经纬度与下一个目标点 N 的角度 gps_ang[N]和距离，将 IMU 数 据以 gps_ang[N]为基准进行卡尔曼滤波，以 IMU 的数据和 gps_ang[N]作为输入进行转向控制，采用的是位置式 PID 的方向控制；同时运行基于增量式 PID的速度控制。在这一过程中循环判断，是否到达下一个目标点，是则 N+1 切换目标点。

3.3 无刷电机驱动控制方式

无刷电机的三个半桥我们有三种驱动方式。方式一：上桥给PWM 信号下桥通过IO 控制开启与关闭。方式二：上桥通过IO 控制开启与关闭下桥给PWM 信号。方式三：上下桥都给PWM 信号，但是需要注意的是上下桥的PWM信号必须是互补的，因为上下桥不能同时导通，否则出现电源与地短路的情况，由于MOS 开关具有延时，所以互补PWM 信号还必须设置死区，作用是当其中一个桥关闭之后要延时一段时间再打开另外一个桥，避免出现电源对地短路的情况。在我们的控制中则使用上述方式进行驱动。

由于使用CCU6 模块管理无刷驱动信息，需要CPU 频繁地处理无刷驱动的换相控制。所以我们使用英飞凌基于AURIX 系列MCU 的GTM 模块的无刷驱动方案，可以使无刷代码和控制代码合二为一，同时无刷代码几乎不消耗CPU 资源。

3.4 无刷电机驱动板软件设计

传统的无刷直流电机大多应用六步方波法来控制，对于典型的三相BLDC 电机，有 6 个不同的工作区间，每个区间中对特定的两相绕组通电，产生 6 种方向相差 60°的磁场，按规律循环切换六种磁场的产生从而带动无刷电机的旋转，但方波驱动出力大，转矩脉动和噪声也比较大，影响着无刷直流电机在高精度、低噪声环境下的应用。随着电机控制技术的发展，正弦波驱动控制理念被广泛应用到无刷电机的控制中，有效改善六步方波驱动中存在的缺陷。因此本次设计使用正弦波驱动中的磁场定向控制(Field oriented Control：FOC)技术对直流无刷电机进行转动控制。FOC结构框图如图5 所示。

图5 FOC 控制结构框图

无刷驱动控制程序由主函数与中断服务函数两部分组成。主函数中进行各类外设与软件资源的初始化，将电机转至 a 轴并对编码器进行零点校正，确保电机在 a 轴时编码器输出角度为 0º。在主循环中通过串口持续发送电机的转速、转子当前角度及电流值至上位机，以便观察电机运动情况。图6为主函数流程图。

图6 主函数流程图

本次设计中使用了三个中断服务函数，分别为一个定时器比较中断、一个定时中断以及一个外部中断。定时器比较中断是本次设计中的核心中断。其原理是设置比较值，当定时器计数到该比较值时触发此中断。通过设置合理的比较值，在每个 PWM 周期结束时触发该中断，此中断内进行FOC 电流环控制流程从而更新三路互补 PWM 的占空比以驱动三相逆变电路，从而使电机三相绕组产生所需的磁场带动电机旋转。图7 为定时器比较中断流程图。

图7 定时器比较中断流程图

根据 FOC 控制过程的理论基础，对 FOC 算法进行编程实现。首先进行各个功能模块的函数编写，功能模块主要分为转子位置检测模块、坐标变换模块、SVPWM 生成模块、ADC 模块以及 PID 模块。位置获取模块通过获得正交编码器的数据并处理完成角度的计算。英飞凌提供了正交解码的解算库，通过使用 TC212 的 GPT12 模块中的 T5 定时器对ABZ 正交编码器的A、B 相脉冲进行计数，引脚配置完成后通过代码读取相应寄存器内的值即可获取当前计数的脉冲个数。获取的脉冲数为累计脉冲个数，该值对 1024 取余后乘 360 即为当前转子的角度值。

此外，还需要初始化 Z 相采集引脚的外部中断，使用上升沿触发的方式。坐标变换模块的代码实现过程中大量使用了三角函数的运算，若调用自带的三角函数运算函数会涉及大量的浮点数运算，占用 CPU 较长的运算时间，还可能会导致程序的卡顿。因此本次设计中优化了三角函数的计算方式，使用查表的方式对三角函数进行快速运算，优化程序运行速度。SVPWM 模块的程序编写中有大量的条件判断，此处使用了 switch 语句进行各条件的跳转，避免了使用多个 if 语句的嵌套导致程序冗杂与较差的可读性与可维护性。PID 模块通过编程实现了速度-电流串级 PID 算法，电流环作为内环，速度环作为外环，均使用 PI 调节。速度 PI 为串级 PID 的外环，将编码器测量的电机转速作为测量值，将给定的目标转速作为给定值，经过增量式 PI 运算后得到目标电流值 Itar_d、Itar_q，该环可以对电机的转速进行控制。电流 PI 为串级 PID 的内环，将 Park 变换得出的 Id 与 Iq 作为测量值，将增量速度 PI 的输出 Itar_d、Itar_q 作为给定值，经过增量式 PI 运算后得到控制电压 Uα与 Uβ，该环可以对电机的扭矩进行控制。最终实现无刷电机速度与扭矩双闭环控制。