电机控制编程的数学运算优化方案

引言

  电机控制应用设计传统上采用微控制器（MCU）或数字信号处理器（DSP）来运行电机控制算法。在研究永磁同步电机（PMSM）矢量控制的时候，坐标变换的三角函数运算、观测器的迭代、锁相环的鉴相环节（用到了三角函数)都比较消耗电机主控芯片的计算能力。在考虑算法实现的时候，都需要针对主控芯片的实际性能进行一定优化，才能确保算法能够顺利运行。这里我总结下电机控制中对程序算法优化的办法。

数据的概念

浮点数

  浮点数是一种小数表示方法。
  IEEE浮点数标准定义了两种基本的格式：以4个字节表示的单精度格式和以8个字节表示的双精度格式。
  单精度格式的4个字节可以分为三个部分：1位的符号位（0代表正数，1代表负数），8位用做指数，最后的23位用做有效数。下面给出了单精度格式的三部分的划分方式，其中有效数的最低位的最右边。

双精度浮点数情况类似，它需要8个字节来表示，结构如下：

  浮点数的特点是小数点位置的计数信息并没有和整个数字一起存储，所以使用小数的程序必须知道小数点的位置。
  浮点数加法中，最重要的一点就是如何对有效数相加，为了能使它们的有效位匹配，需要利用指数来确定对其如何移位。你需要把有效数部分的相加，但并不是简单的相加。两个数的指数部分的必须对齐。两个浮点数的乘法意味着要把有效数当做整数相乘，并且把指数部分相加。为了使结果规范化，一般需要对指数调整一次到两次。浮点数运算的另一层次的复杂性体现在处理一些较为繁杂的函数运算，例如平方根、指数、对数和三角函数。但所有的都可以通过加减乘除来实现。
  因此，浮点数的计算需要FPU才能高效进行。

定点数

  另一种小数表示方法是约定所有数值数据的小数点隐含在某一个固定位置上，称为定点表示法，简称定点数。定点数相对简单，其小数点位置固定。因此，其从选取小数点位置开始，就确定好了数据的大小范围和精度。
TI的IQmath库、ARM的CMSIS_DSP库就是使用这一格式的典型代表。

提高执行效率的方法

提升硬件性能

  这里最直接的办法，尤其研究高性能控制的时候，应当优先提升硬件性能，将算法实现作为首要目的。
  参考计算机的发展，它是从CPU（向量计算）向 GPU（矢量计算）、AI（矩阵计算）和 FPGA（空间计算）发展。对于电机控制，这一情况也是类似的。最初的单片机作为主控芯片，就类似只有单核CPU进行运算，而现在很多单片机与dsp都有多核运算，比如stm32H7、dsp28379。此外，还有思路是协处理器的采用，比如dsp28035中有clzheqa浮点数协处理器，FPGA中加入arm核或者dsp核、dsp28m35中m3内核与c2000内核混合使用，这种情况下的多核是异构多核，有一个核心负责通讯等任务，另一个核负责计算，这种应用也是很广泛的。

优化程序以提升效率

  考虑到成本因素，从程序优化上来提升效率。并且有的硬件性能也需要程序优化才能充分发挥。
  程序效率不够主要有以下原因：
  1. 编译器的优化等级不够，以至生成的代码本身的效率低下；
  2. 对不支持浮点运算的芯片采用过多浮点运算；
  3. 对于有浮点运算能力的芯片，则可能是采用了双精度浮点运算。比如三角函数库的选取不当，导致在计算正弦、余弦过程中引入了大量的双精度浮点运算。目前多数电机控制芯片的FPU只支持单精度浮点数，TI出的TMS320F28384D支持双精度浮点数。
  程序效率提升办法：
  1. 合理选用编译器的优化等级，提高代码的执行效率；
  2. 在计算表达式中，强制常量为单精度浮点数，以避免引入双精度浮点数动算；
  3. 选用芯片厂商公司提供的采用优化的函数库，以避免由普通的数学函数库引入的双精度浮点数运算。这一点尤其重要，很多人在开发时自己去摸索，然而实际上人家已经有了成熟可靠的方案，而我们的核心其实是研究控制算法，不必在此花费过多时间。
  要使硬件上的性能得以充分的发挥，除了对算法本身进行优化以外，对一些相关因素也要仔细斟酌：
  恰当的设置 Flash 缓冲区的参数。在 STM32F4 中，为了匹配 Flash 存贮器与CPU 之间的对数据、指令的吞吐速率，设有指令缓冲区和数据缓冲区。复位后缓冲区是不工作的，需要软件予以开启，并设置恰当的等待周期数。没有缓冲区的参与，运行在 Flash 中的程序，在运效率上会大幅度的降低。
  选择高效的存贮器来存放数据。快速的数据存取是保证 CPU 不间断的执行指令的前提。这一点上，不仅要考查存贮器本身的速率，还要看是否有其它的处理单元与 CPU 分享该存贮器。比如，在 STM32F4 中，将数据放在 CCM 存贮器中，要比放在 SRAM1 中更能保证 CPU 对数据的存取速率，因为 CCM 存贮器是 CPU独享的，而 SRAM1 还可能被 DMA 访问。
  根据 CPU 指令集的特点，合理的选取计算的数据类型。比如，要计算 16 位的DSP 运算，最好把变量和常量定义成 16位数据，这样有利于编译器使用 SIMD指令对代码进行优化。在单精度浮点数能够满足要求的情况下，将变量或常量定义成单精度类型，有利于编译器使用单精度浮点运算指令，对代码优化。
  选择针对 Cortex-M4 进行优化的数学函数库。ARM 公司为 Cortex_M4 的提供了一整套的 DSP、浮点数运算库，其效率远高于编译器自带的函数库。
  编译器的优化等级是一个重要的设置选项，不同的优化等级下，所生成的代码的效率是有很大差别的。
  对于高频次使用的数据，要考虑放在寄存器类型的变量中。通常，CPU 存取操作数的最快捷的方式是寄存器寻址，可以做到零时钟花费。
  合理安排计算次序，考虑是否可以以乘法代替除法。在数学中，乘法和除法是一对逆运算，除以一个数与乘于这个数的倒数可以等同起来。然而在 CPU 的指令实现中，乘法和除法的计算速度上是有很大差别的，通常乘法的计算速度远高于除法的计算速度。所以，有必要在运算中，使用乘法来代替除法。比如： a÷b÷c可以使用a÷(b×c)来代替。
  找出算法中的重复计算，将其合并，只计算一次。对这样的计算，完全可以在第一次计算之后，将结果放在中间变量中，而在后面的计算中直接引用。
  对于复杂的计算，考虑是否能用查表来代替。查表是一种快速得出结果的好方法，它以牺牲存贮器空间来换取速度。在存贮器空间不是很紧张的情况下，用查表代替计算还是很划算的。

嵌入式系统的数学运算解决方案

  目前，低成本的电机控制器，采用主控芯片的首选是stm32f103c8t6、DSP28035这样的定点芯片，本身不支持硬件浮点运算。然而电机控制算法中会大量涉及浮点数，而定点芯片直接进行浮点运算时的效率很低。与此同时，C2000系列DSP中，只含有硬件乘法器和加法器，除法运算也会占用大量耗时。此外三角函数运算、开方乘方等非线性运算也会占用大量运算时间。
  芯片厂商其实在这方面以及提供了许多解决方案，因此，在开发时我们无需做重复工作，直接采用他们的方案即可。

直接转换定点数

  转换定点数的常见方法有直接将数据写成定点数形式，把除法运算改为乘法与移位来运算，同时将三角函数、开方乘方等运算进行标幺化，转为查表法进行，典型的代表是ST的FOC2.0库。在低成本逆变器程序开发时，都采用此类方法，因为这样的程序执行效率最高，可充分利用定点型主控芯片的性能。

定点运算库

  为了方便开发者的程序编写，许多厂商涉及了专门为定点运算的数学运算库。这些库不仅可以进行Q格式的伪浮点运算，而且还包含了三角函数、开方乘方等运算的Q格式函数，以便主控芯片可以进行伪浮点运算。像TI的IQmath库和ARM的CMSIS DSP库是这一方式的典型代表。两者稍有区别。

IQmath

  TI的IQmath库中，所有数统一为32位，所需要选择的仅仅是小数点所在的位置，IQmath库是封装成lib文件给用户调用，针对C2000系列DSP都可以使用。IQmath库有函数指令可以将浮点数化为定点数，各种Q格式运算都有库函数可以调用，和直接浮点运算时除了调用函数不同外，整个运算过程基本相同。因此，IQmath库编写的程序，无论是从浮点运算修改到Q格式运算，还是反之，过程都十分方便，在我们平时试验中，都是先考虑在DSP上实现算法，然后移植去其他芯片。
  一般IQmath库中的三角函数、根号等非线性运算是采用查表法实现的。像28335、28035、2812等dsp的表存入在bootrom中。
从cmd文件中可以看到，使用IQmath的cmd文件需要添加如下语句：

MEMORY
{
     
PAGE 0 :

   IQTABLES   : origin = 0x3FF000, length = 0x000b50
   RAMH0      : origin = 0x3F8002, length = 0x001FFE

PAGE 1 : 
   RAML0L1  : origin = 0x008000, length = 0x002000
}

SECTIONS
{
     

  

   IQmath           : > RAMH0,       PAGE = 0
   IQmathTables     : > IQTABLES,    PAGE = 0, type = NOLOAD
   IQmathTablesRam  : > RAML0L1,     PAGE = 1

}

  从注释中可以发现，IQmath段是IQmath库中汇编程序的映射，IQmathTables段是IQsin, IQcos, IQatan, IQatan2等使用的查找表的映射，对于F2812来讲，这个查找表被固化在了Boot rom中，使用Boot rom中的查找表可以节约Ram空间，但是Rom的访问需要额外的1个等待周期，所以用户可以自己权衡时间(1 wait)和空间(Ram)哪个对自己是最重要的。另外，如果选择直接调用Boot rom中的查找表，重映射时，必须使用NOLOAD关键字。IQmathTablesRam段是IQasin, IQacos, and IQexp等使用的查找表的映射，如果不使用这些功能，可以直接删除该段，节约Ram或Flash资源。
  还有一点需要注意，IQmath中编译代码的大小是动态的，它是根据用户代码中调用的IQmath库函数来条件编译的，所以初始时，可以将IQmath段的大小定义的稍微大一些，最后根据map文件适当增减段的大小，节约宝贵的Ram资源。
  值得一提的是，TI曾经推出过给arm的m3、m4内核系列的单片机的iqmath库，目前在论坛上已经看到有人在stm32f103c8t6上使用过，这款芯片与不使用CLA的dsp28035相比，性能相差不大，因此可以考虑在stm32上也使用iqmath库。

CMSIS DSP

  而ARM不设计单片机，其cortex-M内核授权给各单片机厂商，如ST和英飞凌使用，而这些厂商又在电机控制中采用了自己的方案来优化数据处理，往往是将ARM的CMSIS DSP库与第一种方案混合使用。本质上，CMSIS DSP库的作用是统一ARM内核的主控芯片的数据处理方式，方便不同厂商生产的ARM系列的MCU的程序移植。在库函数中，其Q格式数是分了8位、16位、32位，其小数点固定位于符号位之后，使用时需要先对数据进行标幺化，这一点相比IQmath库复杂。但是，CMSIS DSP库中，不仅包含了Q格式下除法、三角函数、开方乘方等运算，还包含了矩阵变换、FFT变换、复数运算等。过去，TI也曾经为其生产的ARM系列芯片移植过IQmath库，但目前已经停止支持，其旧版本仍然可用，但不再更新，TI的ARM系列芯片也转向使用ARM的CMSIS DSP库。目前，ARM系列的芯片适合使用CMSIS DSP库，不仅方便各种ARM架构芯片程序的移植，而且还能用于编写FFT、神经网络的程序。值得一提的是，CMSIS DSP库是开源的，各个函数的源码是可见。

算法优化

  像除法、三角函数等运算，可以通过分解运算来实现算法优化，典型的代表有牛顿迭代法和CORDIC算法。
  牛顿迭代法（Newton’s method）又称为牛顿-拉夫逊（拉弗森）方法（Newton-Raphson method），它是牛顿在17世纪提出的一种在实数域和复数域上近似求解方程的方法。多数方程不存在求根公式，因此求精确根非常困难，甚至不可能，从而寻找方程的近似根就显得特别重要。方法使用函数f(x）的泰勒级数的前面几项来寻找方程f(x) = 0的根。牛顿迭代法是求方程根的重要方法之一，其最大优点是在方程f(x) = 0的单根附近具有平方收敛，而且该法还可以用来求方程的重根、复根，此时线性收敛，但是可通过一些方法变成超线性收敛。另外该方法广泛用于计算机编程中。根据牛顿迭代法，可以将除法、三角函数、指数运算、开方乘方等运算化为容易执行的线性运算。因此，上述运算都可以通过线性化优化来加速。
  CORDIC（Coordinate Rotation Digital Computer）算法即坐标旋转数字计算方法，是J.D.Volder1于1959年首次提出，主要用于三角函数、双曲线、指数、对数的计算。该算法通过基本的加和移位运算代替乘法运算，使得矢量的旋转和定向的计算不再需要三角函数、乘法、开方、反三角、指数等函数。查表法、多项式展开法或近似法等这些方法在速度、精度、资源方面难以兼顾。而采用CORDIC算法来实现超函数时，则无需使用乘法器，它只需要一个最小的查找表(LUT)，利用简单的移位和相加运算即可产生高精度的正余弦波形，尤其适合于FPGA的实现。
基于前面的算法优化，尤其是CORDIC算法，其与二进制为基础的数字电路配合，诞生了很多硬件加速器。

浮点运算

  目前，主流芯片厂商均生产了带有FPU的DSP和MCU，使得32位浮点运算实现了单周期完成。另外，为了充分发挥FPU的性能，各厂商也针对浮点运算推出了相应的库函数，以便加速算法运行。
  TI的FPU使用时，需要在工程中设置启用FPU模块，以便进行硬件浮点运算。同时考虑实际工程需要使用除法、三角函数等非线性运算，TI提供了rts2800_fpu32_fast_supplement.lib以及rts2800_fpu32.lib来代替标准的c语言库函数来计算。这些浮点运算库里面包含了关于专门优化过的浮点数的函数，并且使用了DSP的FPU，运算速度相比c语言通用的数学计算库计算sin，cos，log之类的更快。在编程时，只需要按照先后顺序包含这两个库函数，即可加速浮点运算。与IQmath库类似，浮点运算的库使用的查找表FPUmathTables在芯片中存在于bootrom中，可以直接调用Boot rom中的查找表，重映射时，必须使用NOLOAD关键字，但是Rom的访问需要额外的1个等待周期。而如果直接采用RAM加载，则可减少这一时间支出。
  两者的差异时间如下表所示

  ARM的FPU使用也类似，需要在工程设置中启用FPU模块。而针对复数、三角等运算时，ARM提供了相应的库函数，来代替math.h原来包含的c语言标准通用运算函数，以便加速性能。

协处理器加速

  针对除法、三角函数等非线性运算，厂商设计了硬件加速器，来直接硬件完成这些运算。典型的代表有TI的TMU、CLA、VCU和英飞凌的运算协处理器。

CLA

  TI的CLA本身是一个加入到28035等DSP的协处理器，可以执行浮点运算，并且有CLAmath库函数，可以灵活执行各种浮点运算和非线性运算。由于CLA为协处理器，可以与CPU同时工作，往往在CLA执行算法，而CPU去执行各种通讯、保护等程序。不足之处是CLA执行逻辑运算的效率较低，但是其对算法运算的执行效率高于CPU。TI提供了CLAmath.lib这一函数库，帮助CLA模块进行运算。需要注意的是，CLA的舍入方式与CPU不同，主要体现在当数值接近于0时的计算可能会舍入方式不同，因此，一方面可以照着网上的设置让MSTF寄存器中的RND32位置1，改变其舍入方式，另一方面可以将运算分步进行，即分成几项相加。在CLA的程序执行时，由于浮点数带来的数据处理精度的提升以及数据范围的扩大，需要同时考虑原有采样噪声也更容易产生影响，因此，采样窗口、滤波器等参数需要为此而重新考虑。
  CLA的运输和转换效率高于C28X+FPU的架构。而根据TI官网，可以对比28335与28035的性能，60Mhz的28035,相比150Mhz的28335，其MIPS能到120，仅比28335少30。

TMU与VCU

  而TMU则是可以直接加速CPU运算的模块，硬件上支持三角函数、除法等非线性运算；TI的VCU 加速器可缩短编码应用中常见的复杂数学运算的时间。这些加速无需调用库函数，直接通过使能编译选项，即可加速CPU的算法执行，无需改写原有的程序，便于程序在没有相应模块的芯片上移植。

英飞凌协处理器

  英飞凌则针对这类运算设计了协处理器。XMC1300拥有一个特别设计的协处理器-MATH协处理器，它包含以下两个子模块：除法器和Cordic协处理器。该协处理器作为除法器启动时可做32位/32位，32位/16位，16位/16位除法，当它作为Cordic协处理器时可进行三角函数、双曲线函数和一次线性函数。此外除法器的输入可以由除法器的结果或Cordic的结果直接输入，这样构成了除法器和Cordic的级联。

宏定义实现函数

  C中表达式形式的宏定义
  表达式形式的宏定义如：

#define ExpressionName(Var1,Var2) ((Var1)+(Var2))*((Var1)-(Var2))

  C中使用define这种形式宏定义的原因是因为，C语言是一个效率很高的语言，这种宏定义在形式及使用上像一个函数，但它使用预处理器实现，没有了参数压栈，代码生成等一系列的操作。因此，效率很高，这是它在C中被使用的一个主要原因。
  在TI的controlsuit中，就提供了很多宏函数模块，利用这些模块作了电机控制等的demo。
  但是这种宏定义在形式上类似于一个函数，但在使用它时，仅仅只是做预处理器符号表中的简单替换，因此它不能进行参数有效性的检测，也就不能享受C++编译器严格类型检查的好处，另外它的返回值也不能被强制转换为可转换的合适的类型。这样，它的使用就存在着一系列的隐患和局限性。

inline关键词

  为了提高效率且不影响模块化，可以加入inline关键词，在函数声明或定义中函数返回类型前加上关键字inline，即可以把函数指定为内联函数，可以提高程序执行效率。
  像英飞凌的示例中就在多处采用了inline关键词。
  但是内联是以代码膨胀复制为代价，仅仅省去了函数调用的开销，从而提高函数的执行效率。如果执行函数体内代码的时间，相比于函数调用的开销较大，那么效率的收获会很少。另一方面，每一处内联函数的调用都要复制代码, 将使程序的总代码量增大，消耗更多的内存空间。以下情况不宜使用内联：
1）如果函数体内的代码比较长，使用内联将导致内存消耗代价较高。
2）如果函数体内出现循环，那么执行函数体内代码的时间要比函数调用的开销大。

将程序从Flash拷贝至RAM运行（DSP为例）

  以28035为例


  根据28035的存储空间图，可以看到程序运行能利用的存储类型有三种：Ram、BootROM、Flash。三者中，Ram速度最快，BootRom有一个等待周期，Flash速度最慢，最高只能到约80%的Ram的执行效率。
BootRom主要用于存储TI预先设计的一些算法库，如IQmath库、FPUmath库等，可以节约程序空间，如果需要提高执行效率，可以选择加载至RAM中而不适用BootRom预置的算法模块。
  一般程序调试都集中于RAM调试，到了验证完以后烧入Flash。两者都可执行程序，实际工程中。但是为了保证程序执行时间不变，往往会通过TI程序，让程序在Flash中加载，而在Ram中执行。具体操作在另一篇博客中写。