灰度丢线后小车直冲赛道:都是「保持上次值」惹的祸
本文最后更新于58 天前,其中的信息可能已经过时,如有错误请发送邮件到574414156@qq.com

小车过急弯必冲出去,而且每次都在同一个位置。起初我怀疑是机械问题,轮子打滑或者重心偏了,重新紧固车架、换了两对新轮子,问题照旧。接着去调 PID,Kp 和 Kd 来回试了好几轮,急弯处该怎么冲还是怎么冲。

后来干脆蹲在赛道边反复放车看,才注意到一件之前完全没在意的事:丢线的那一瞬间,程序到底采到了什么数据。

把八路灰度的原始数值和位置值一起打到串口,答案很直接。丢线的时候八路值全部低于 1000,而位置值纹丝不动,还是丢线前算出来的那个数。

static const int8_t weights[8] = {7, 5, 3, 1, -1, -3, -5, -7};

float weighted_sum = 0.0f;
float total_value = 0.0f;

for (uint8_t i = 0; i < 8; i++)
{
    uint16_t val = uart3_values[i];
    if (val > GRAYSCALE_BLACK_THRESHOLD)
    {
        weighted_sum += (float)val * weights[i];
        total_value += (float)val;
    }
}

if (total_value > 0.0f)
{
    current_position = weighted_sum / total_value;
}
/* 无传感器时保持上次值 */

八路全部低于阈值,循环里的 if 一次都进不去,weighted_sum 和 total_value 都停在 0,赋值语句被跳过,current_position 就保留了上一次的值,也就是上一次还压着线时的位置。

当初加这个判断,动机很单纯,就是防除零。total_value 是除数,为 0 的时候做浮点除法会得到 NaN,位置值一变成 NaN 后面全乱,所以顺手加了个判断。防护本身没错,漏掉的是另一件事:这个函数是在控制回路里跑的。

app_loop 里每轮先是 calc_grayscale_position(),紧接着 detect_curve_type() 也在读同一个 current_position:

float abs_error = fabs(current_position);

if (abs_error > CURVE_SHARP_THRESHOLD)
{
    g_curve_type = CURVE_SHARP;
}

弯道等级决定基础速度,急弯 100、缓弯 160、直道 180。所以丢线那一刻,位置是旧的,弯道等级是旧的,速度档位还是旧的。车已经压到赛道外面,PID 照样按旧偏差算输出、继续打方向,偏差越积越大,车就越冲越远。急弯、十字路口和断线位置丢线的概率最高,失控也集中在这几个地方。

后来我琢磨的改法分两步。先把「有没有压线」和「线在哪」拆成两件事,total_value 为 0 的时候置一个 line_lost 标志,位置值标记为无效,下游拿到无效值就不该再按它算:

if (total_value > 0.0f)
{
    current_position = weighted_sum / total_value;
    line_lost = 0;
}
else
{
    line_lost = 1;   /* 丢线:位置无效 */
}

再处理丢线之后该怎么走。十字路口的空隙和地面反光都会造成短暂丢线,一丢就往回找线,反而会让车在直道上左右摇,所以得加个计时器把两种情况分开:丢线时间很短就当成噪声,继续沿用旧位置,超过设定时长才认定是真丢线,这时候朝黑线消失的方向打轮去找,同时把速度降到最低档,留出反应时间。恢复压线以后清掉标志,回到正常循迹。

这套改法我一直没写进代码。app.c 里 calc_grayscale_position() 到现在还是原样,「无传感器时保持上次值」那句注释也还在,也就是说这个坑目前只是被诊断出来了,并没有真正修掉。

现在写类似的代码,我会先问一句:这个函数在没有输入数据的时候,下游会拿它的输出去干什么。那句注释就是这次留下的记录。

文末附加内容
上一篇