Odrive0.5.1-FOC电机控制 arm_cos_f32.cpp arm_sin

Odrive0.5.1-FOC电机控制 arm_cos_f32.cpp arm_sin_f32.cpp代码实现（一）

news2025/4/15 20:18:40

01 查表法

在 our_arm_cos_f32 函数中，查表（Look-Up Table, LUT）的核心是预计算的正弦值表 sinTable_f32，通过巧妙利用余弦与正弦的相位关系实现快速余弦计算。以下是详细解析：

1. 查的是什么表？

(1) 表内容

表名：sinTable_f32（通常是一个全局常量数组）

存储内容：对正弦函数一个周期（[0, 2π]）的离散采样值。

长度：FAST_MATH_TABLE_SIZE（例如256点，即每 2π/256 ≈ 0.0245 弧度一个采样点）。

(2) 表的生成

表通常在编译时预计算，类似以下代码：

#define FAST_MATH_TABLE_SIZE 256
const float32_t sinTable_f32[FAST_MATH_TABLE_SIZE + 1] = {
    0.0f, 0.024541f, 0.049068f, ..., 0.000000f  //多一项：FAST_MATH_TABLE_SIZE + 1 是为了安全访问 index+1。
};

02 离散采样值

在数学和信号处理中， 离散采样值（Discrete Sampled Values）指的是对连续信号（如正弦波、余弦波）在特定时间或位置间隔上采集的 有限个数据点。这些点是对原始连续信号的数字化近似，用于计算机或嵌入式系统处理。以下是详细解释：

1. 离散采样值的本质

连续信号：如理想的余弦函数 cos⁡(x)cos(x) 是无限平滑且连续的（任意 xx 都有对应的值）。
离散采样：在连续信号上按固定间隔（如每 ΔθΔθ 弧度）取点，生成有限个数值，形成一张查找表（LUT）。

示例：对 cos⁡(x)cos( x) 在 [0,2π][0,2 π] 区间内采样256次，得到256个离散值。

2. 在代码中的具体体现

在 our_arm_cos_f32 函数中：

sinTable_f32就是一个离散采样表，存储了正弦函数在一个周期内的256（或其它数量）个等间隔采样值。

例如： FAST_MATH_TABLE_SIZE=256 时， Δ θ≈0.0245 弧度。

3. 为什么需要离散采样？

场景	连续函数	离散采样
计算方式	实时计算（如泰勒展开）	预计算后查表
速度	慢（需多次浮点运算）	快（直接内存访问）
资源占用	低（仅需代码）	高（需存储表）
适用场景	高精度通用计算	实时嵌入式系统（如电机控制）

4. 离散采样的工作原理

(1) 采样过程

假设对 sin⁡(x)采样，生成表：

import numpy as np
FAST_MATH_TABLE_SIZE = 8
x_samples = np.linspace(0, 2*np.pi, FAST_MATH_TABLE_SIZE, endpoint=False)
sin_table = np.sin(x_samples)  # 得到离散采样值：[0.0, 0.707, 1.0, 0.707, 0.0, -0.707, -1.0, -0.707]

(2) 查表示例

计算 cos⁡(0.785)cos(0.785)（即 π/4≈0.785 π/4≈0.785 弧度）：

转换相位：cos⁡(0.785)=sin(0.785+π/2)=sin(2.356)
归一化索引：2.356/(2π)×8≈3.0 → 索引 3（第4个点）
查表：sin_table[3] = 0.707 → 结果 cos⁡(0.785)≈0.707

5. 插值的作用

离散采样会引入 量化误差，通过插值可提高精度：

线性插值：

若索引非整数（如3.5），取相邻两点（索引3和4）的加权平均：

其中 α 是小数部分（如3.5的 α=0.5）。

6. 实际应用中的权衡

表越大

：精度越高，但内存占用增加（如512点占2KB内存）。

表越小

：速度更快，但误差增大（电机控制中典型用256点，误差±0.1%）。

总结

离散采样值是通过 预先存储连续函数的有限个关键点，用查表代替实时计算的一种优化技术。在嵌入式系统（如ODrive的电机控制）中，它实现了：

极速计算：比实时计算快10倍以上
可控精度：通过调整采样密度平衡性能
资源节省：共享正弦/余弦表，减少重复存储

这种方法的代价是 内存占用和 轻微的精度损失，但在实时性要求高的场景（如FOC算法）中至关重要！

03 代码实现 float32_t our_arm_cos_f32(float32_t x)

这段代码实现了一个优化的浮点余弦函数 (our_arm_cos_f32) ，主要用于实时嵌入式系统中的快速三角函数计算。以下是详细解析：

函数功能

输入：弧度值 x（范围无限制，自动处理周期性）
输出：cos(x) 的近似值（基于查表和线性插值）
特点：
- 比标准库 cosf() 更快（适合实时控制）
- 牺牲少量精度换取速度
- 依赖预计算的 sinTable_f32（正弦表）

float32_t our_arm_cos_f32(float32_t x)
{
  float32_t cosVal, fract, in;                   /* Temporary variables for input, output */
  uint16_t index;                                /* Index variable */
  float32_t a, b;                                /* Two nearest output values */
  int32_t n;
  float32_t findex;

 // 转换为正弦计算：cos(x) = sin(x + π/2)
  in = x * 0.159154943092f + 0.25f;  // 等价于 x/(2π) + 0.25

  //周期处理:利用三角函数的周期性，将输入映射到第一个周期 [0,1]。
  n = (int32_t) in;// 取整数部分
  if (in < 0.0f) //处理负数输入
  {
    n--;
  }

  /* Map input value to [0 1] */
  in = in - (float32_t) n; // 取小数部分

  //查表索引计算:
  findex = (float32_t)FAST_MATH_TABLE_SIZE * in; //FAST_MATH_TABLE_SIZE：正弦表长度（如256），决定精度和内存开销。
  index = (uint16_t)findex; //fract：插值权重（距离上一个表项的比例）。
  if (index >= FAST_MATH_TABLE_SIZE) {
    index = 0;
    findex -= (float32_t)FAST_MATH_TABLE_SIZE; // 小数部分用于插值
  }

  /* fractional value calculation */
  fract = findex - (float32_t) index;

  //线性插值:用相邻两个表项的值线性组合，减少查表误差。
  a = sinTable_f32[index]; // 当前表项值
  b = sinTable_f32[index+1]; // 下一表项值

  /* Linear interpolation process */
  cosVal = (1.0f-fract)*a + fract*b; // 加权平均

  /* Return the output value */
  return (cosVal);
}

/**
 * @} end of cos group
 */