概况
由于 VEX 中 PWM 频率偏低,因此马达的响应并不是线性的,例如马达指令在较小时存在死区,在较大时对转速的影响可以忽略不计等等。以 269 马达的数据为例(393 也是类似):
那么如何让马达的响应变得线性呢?从数学角度出发,我们需要试图找到,或者逼近以上马达曲线 \(f\) 的反函数 \(g = f^{-1}\),对于输出我们应用反函数再应用到马达上,由于马达曲线是单调的,因此在理想状况下,这波操作以后马达的响应就是线性的了,这一过程也称作马达线性化。
出于简单起见,我们规定这个 \(g\) 的定义域和值域都是 \([-127, 127]\),这样的话,理论上大部分原来没有使用线性化的 PID 算法可以较为简单地加上线性化(不然的话就需要从零开始调参)。显然由马达曲线的单调性可得 \(g(127) = 127\),以及 \(g(-127) = -127\)。
目前机器人社实践过的线性化算法有:
- 去除死区的简单线性化:只比没有线性化好一点,简单来说就是对输出的指令加一个 bias,这样就不会有死区的问题。
- 三段线性插值线性化:就是被 WSJ 吹爆的所谓模型,本质是把马达曲线分为三段并分别用线性函数近似,效果很好,缺点是调参较为复杂。
- 二次函数线性化:我们发现马达曲线的形状有点像 \(f(x) = \sqrt x\) 的图像,因此我们使用二次函数为反函数进行线性化,效果一般。
- 反双曲正切线性化:本篇主题,效果较好。
- 真・模型线性化:使用马达模型直接计算马达曲线进行线性化,效果最好,可以针对不同的电量,场地状况,扭矩实时给出不同的曲线,缺点是计算慢。
数学推导
直觉上,有没有觉得马达曲线的图像很像 \(\tanh\) 函数的图像?
因此我们选择使用双曲正切函数的反函数来进行线性化: \[ \tanh^{-1}(x) = \frac{1}{2}\ln\left(\frac{1 + x}{1 - x}\right) \] 为了适应死区,以及满足上文中定义域和值域的要求,我们魔改拓展一下上式: \[ g(x) = a\operatorname{sgn}(x) + \frac{127 - a}{2b}\ln\left(\frac{c + x}{c - x}\right) \] 解释一下各参数的意义:
- \(a\) 是死区,一般来说 \(5 \le a \le 20\)。
- \(b\) 是我叫做 “缩放因子” 的参数,其图像意义是把原双曲正切函数自变量 \([-b, b]\) 这一段的图像看做马达函数图像形状上的近似。从图像上可以看到,一般 \(2 \le b \le 3\)。
- \(c\) 是一个依赖于 \(a, b\) 的参数,由 \(g(127) = 127\),我们反解得 \(c = 127\frac{e^{2b} + 1}{e^{2b} - 1}\)。
就完了,不难吧?
代码
注意变量名可能和推导中用到的符号并不一致:
typedef struct {
float a, b;
float alpha;
} TanhLinearizer;
void initTanhLinearizer(TanhLinearizer *t, float deadband, float scale = 2.333333) {
->b = scale * 2;
t->a = 127 - deadband;
tfloat x = exp(2 * scale);
->alpha = 127 * (x + 1) / (x - 1);
t}
int getTanhLinearizedOutput(TanhLinearizer *t, int cmd) {
if (cmd > 127) cmd = 127;
if (cmd < -127) cmd = -127;
int s = sgn(cmd);
= fabs(cmd);
cmd return round(t->a / t->b * log((t->alpha + cmd) / (t->alpha - cmd)) + s * (127 - t->a));
}
效果
马达曲线的反函数应该原本是这个样子的(就是原图像转一转):
然后在 \(a = 8, b = 3\) 时我们的线性化算法给出的反函数是这个样子的:
是不是很像呢?实测的效果确实也是极好的。
更多
其实马达函数的图像不只是像 \(\tanh\) 啦,其实观察一下你也可以说它像 \(\arctan\) 之类的,自然也可以发展出一套线性化的方法,从的来说看图像猜函数只能说是一种近似,最好自然是找出马达曲线精确的计算方法,也就是建模。但是这类线性化方法的一大好处就是计算量很小,很快,因此往往更多地得到应用。