0%

odriver之测速

传感器

绝对磁编码器

绝对磁编码器比较简单,直接读寄存器就可以。实际使用中需要配合径向冲磁的磁铁使用,且最好能远离其他可能造成磁场变化的器件;其次需要注意角度更新频率,动态特性等参数。

线性霍尔传感器

对于线性霍尔传感器一般是需要进行校准的,其使用方法一般是两个传感器呈80度对称分布,同样需要配合径向冲磁的磁铁使用。

当磁铁旋转一周的时候,线性霍尔的传感器的输出应该正好是一个正弦信号,但是正弦的幅值需要校准,让传感器旋转一周分别获取最大值和最小值,这样就可以重构一周的信号与位置的关系。单个传感器就可以获取全部位置角度的信息,但是单个传感器存在一个问题,就是在正弦波的极点处值的变化非常小,而在线性区域变化则比较明显,因此使用两个传感器呈90度摆放则可以解决这个问题。如下图:

如果a传感器取sin值为hall_sin,b传感器取cos值为hall_cos,则最终的角度为atan2f(hall_sin, hall_cos)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
/**
* @brief Hall Sensor read angle
* @param None
* @retval None
*/
uint16_t LinearHallAngleEncoder::LinearHallSensorGetAngle(void)
{
float hall_sin = 0.0f, hall_cos = 0.0f;

LinearHallSensorReadValue();

hall_sin = (hall_raw_a_ - hall_sensor_calibration_.hall_middle_a_) / \
hall_sensor_calibration_.hall_amplitude_a_;
hall_cos = (hall_raw_b_ - hall_sensor_calibration_.hall_middle_b_) / \
hall_sensor_calibration_.hall_amplitude_b_;

std::clamp(hall_sin, -1.0f, 1.0f);
std::clamp(hall_cos, -1.0f, 1.0f);

return atan2f(hall_sin, hall_cos);
}

\[ tan(x) = \frac{sin(a)}{cos(b)} \]

当sin在1或者-1附近的时候cos值接近0,因此虽然sin值变化很小,但是除以cos后就会变化很明显,毕竟cos值这个时候是变化最明显的,接近线性状态。同理在sin值在0附近的时候,cos在1附近,这时sin变化明显,sin值除以一个1附近的值仍然是变化明显的。多么美妙的设计啊,神奇的三角函数!

转子角度和速度的状态估计

状态估计

估计器有两种状态:位置和速度。位置以编码器计数为单位,速度以计数/秒为单位。由于编码器在我们转完一整圈时不断计数,我们可以想象状态是沿着展开的线而不是角度,因此我们可以谈论位置(以计数为单位)而不是角度。

因此,鉴于位置和速度这两种状态,我们可以做的最简单的事情就是使用速度来预测随时间变化的位置。这对于在离散计数之间插入编码器位置很重要。也就是说,我们始终以一阶(直线)跟踪预期位置,即使我们处于编码器脉冲之间。这提供了更平滑的控制和更高的有效精度。 预测预期角度的位置也很重要,这样我们就可以预测下一个编码器脉冲何时到来。如果它晚了或早了,这给了我们关于我们高估或低估了速度这一事实的信息。所以:这是该预测的代码,速度的超级简单一阶积分:

1
2
// Predict current pos
rotor->pll_pos += CURRENT_MEAS_PERIOD * rotor->pll_vel;

好的,现在我们执行检查编码器边缘是早还是晚的部分。这围绕着一个floor函数,该函数以全浮点精度获取预测角度(就像编码器计数有很多小数位),并告诉我们应该对应的编码器计数。就像如果预测角度是一个斜坡,编码器计数将是一个阶梯。我们使用 floor 函数来做这个从坡道到楼梯的映射。 如果我们预测的角度有点落后,我们的预测还没有切换到下一个位置,但编码器已经切换到下一个位置,delta_pos 将是 1.0f 是实际位置与预测位置的差值。

1
2
// discrete phase detector
float delta_pos = (float)(rotor->encoder_state - (int32_t)floorf(rotor->pll_pos));

所以,我们知道我们是早还是晚,现在我们就此采取行动。假设我们迟到了,我们需要加快速度估计,也就是下面第二行。在第二行中,我们加速(以一定速率增加速度)与我们的位置误差成正比:就像质量弹簧系统一样。同样,在没有阻尼的情况下,它会形成谐波振荡器并围绕正确值振荡。所以我们需要添加阻尼。这是通过将位置估计值也更接近测量值来完成的,因此是下面的一行。解释它的另一种方法是我们有一个 PI 循环,其中编码器是我们尝试使用一些有限带宽跟踪的输入信号。

1
2
3
// pll feedback
rotor->pll_pos += CURRENT_MEAS_PERIOD * rotor->pll_kp * delta_pos;
rotor->pll_vel += CURRENT_MEAS_PERIOD * rotor->pll_ki * delta_pos;

边界处理

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
// if we are stopped, make sure we don't randomly drift
if (snap_to_zero_vel || !config_.enable_phase_interpolation) {
interpolation_ = 0.5f;
// reset interpolation if encoder edge comes
// TODO: This isn't correct. At high velocities the first phase in this count mayvery well not be at the edge.
} else if (delta_enc > 0) {
interpolation_ = 0.0f;
} else if (delta_enc < 0) {
interpolation_ = 1.0f;
} else {
// Interpolate (predict) between encoder counts using vel_estimate,
interpolation_ += current_meas_period * vel_estimate_counts_;
// don't allow interpolation indicated position outside of [enc, enc+1)
if (interpolation_ > 1.0f) interpolation_ = 1.0f;
if (interpolation_ < 0.0f) interpolation_ = 0.0f;
}
float interpolated_enc = corrected_enc + interpolation_;

如果速度为0,则将插值强制设置为0.5,仅是取一个中间值,实际速度为0的可能性很小。当预测误差大于0的时候,实际上就是预测值刚好过了传感器测量值的时候,这个时候插值应该很小,当然对于高速转动的时候是不成立的,在低速的时候将插值设置为0是可以的。相反当预测误差小于0的时候,就是传感器刚好切换到下一个位置的时候,这个时候预测值刚好滞后一个count(同样只有在低速的时候才成立)这个时候插值应该设置为1。而当预测误差一个count内的时候说明速度比较低,这个时候插值应该根据预测的速度来计算,对预测速度进行积分。但是积分的上下限就是一个count即[0,1]。最后将传感器读出来的值加上插值就是预估的角度。

参数设计

调节

我们如何选择这些增益?与任何过滤器一样,需要权衡延迟与平滑度。合适的值取决于编码器的分辨率和应用程序。高带宽控制需要高带宽状态估计,但代价是对编码器脉冲的反应更敏锐。 更高分辨率的编码器意味着每个脉冲在“阶梯”中具有更小的幅度,因此您得到的反应不那么尖锐,因此您可以针对系统中相同幅度的噪声/振铃调高带宽。

理论

阻尼比

当弹簧质量系统完全没有损耗,质量会一直摆动,不会结束,每一次的摆动振幅都和之前一样,这种理想情形称为无阻尼。 若系统的损耗很大,例如弹簧质量系统放置在黏滞的液体中,系统会慢慢的回到初始位置,甚至不会过冲,这称为过阻尼。 一般而言,在摆动时会出现过冲,再往另一边摆动,再回来,在摆动过程中,系统消耗了一些能量,而摆动振幅也会越来越小,最后回到初始位置,这称为欠阻尼。 在过阻尼及欠阻尼二个条件之间,有一个特定的情形是系统不会过冲,会在最快时间回到初始位置,这称为临界阻尼。临界阻尼和过阻尼都不会过冲,而临界阻尼是最快回到初始位置的那一个阻尼条件。

阻尼比定义

阻尼比常用ζ表示,是二阶微分方程步阶响应及频率响应的参数之一。在控制理论及谐振子中相当重要。阻尼比表示系统的阻尼相对于临界阻尼的比值。

二阶系统相对于阻尼比 ζ 的阶跃响应

  1. 过阻尼振荡。阻尼比大于1,极点均为负实数。系统在没有振荡的情况下达到稳定状态。随着阻尼比的增加,它达到稳态的速度变慢。
  2. 无阻尼振荡。注意所有的极点都在虚轴上。阻尼比为零,存在无阻尼振荡。
  3. 欠阻尼振荡。阻尼比在0~1之间,极点为负实部的复数。当系统达到稳定状态时,振荡逐渐减小到零。
  4. 临界阻尼振荡。在没有振荡的情况下以最快的方式达到稳定状态。这两个极点具有相同的负值。

更定量地说,越靠左的极点“越快”(更高的带宽)逼近,具有更大虚部的极点以更高的频率振荡。有什么不同?同样,这最好用图片显示:

请注意,在所有这些中,极点位置的单位可以采用弧度/秒。因此,如果极对位于(-2 ± 5i),那么我们将得到一个频率为 5 弧度/秒的正弦响应,它以 2 弧度/秒的速度衰减。

所以假设蓝色轨迹是响应输入位置突然增加的位置,即编码器输入变化。假设我们希望过滤后的输出尽可能快地跟踪它,而不会过冲。当两个极点(红色的 X)恰好在彼此的顶部时会发生这种情况:这称为临界阻尼(请参见第一张图片中右下角的示例)。

极点

。。。

推导

假设我们有一些 P 和 I 增益( \(K_p\)\(K_i\)),极点是什么,因此带宽是多少,阻尼是多少?我选择将其导出为一个连续时间系统(用离散时间来近似)。

所以下面的方程式有两个状态,p是位置,v速度。为了清楚起见,我使用了一个变量来e表示位置状态和编码器读数之间的误差,后者指定 \(p_i\) 为输入位置。因此将两个方程写成矩阵格式。

\[ \dot{p} = v + k_p \cdot e = v + k_p (p_i - p) \tag{1} \]

\[ \dot{v} = k_i \cdot e = k_i (p_i - p) \tag{2} \]

写成矩阵:

\[ \begin{bmatrix} \dot{p} \\ \dot{v} \end{bmatrix} = \begin{pmatrix} -k_p & 1 \\ -k_i & 0 \end{pmatrix} \begin{bmatrix} p \\ v \end{bmatrix} + \begin{bmatrix} k_p \\ k_i \end{bmatrix} p_i \tag{3} \]

这个系统的极点等于左边矩阵的特征值,系统矩阵,通常称为A。

好的,矩阵的特征值是多少?有了现代技术,我们可以让计算机为我们做这些:我们使用 SymPy。在SymPy 现场亲自尝试输入此命令:

1
Matrix([[-x, 1], [-y, 0]]).eigenvals()

假装那x意味着 \(K_p\) 那y意味着 \(K_i\) 。解决方案是:

\[ p_{ok} = \frac{-k_p}{2} \pm \frac{\sqrt{k_p^2 - 4 k_i}}{2} \cdot i \tag{4} \]

我们希望极点的实部是某个特定数字,这是我们的带宽(以弧度/秒为单位),我们希望虚部在两个极点上都为零。

\[ k_p = -2 \cdot p_{ok} \tag{5} \]

\[ k_i = \frac{k_p^2}{4} \tag{6} \]

这正是代码中的内容:

1
2
3
motor->rotor.pll_kp = 2.0f * rotor_pll_bandwidth;
// Critically damped
motor->rotor.pll_ki = 0.25f * (motor->rotor.pll_kp * motor->rotor.pll_kp);

参考文献

https://discourse.odriverobotics.com/t/rotor-encoder-pll-and-velocity/224/4
https://www.ti.com.cn/zh-cn/sensors/magnetic-sensors/linear-hall-effect-sensors/products.html
https://www.ti.com.cn/product/cn/DRV5055
https://figshare.com/articles/figure/Step_response_of_a_second_order_system_with_respect_to_the_damping_ratio_950_the_poles_are_shown_as_X/500243
https://www.wikiwand.com/zh/%E9%98%BB%E5%B0%BC%E6%AF%94