此文系第16届智能车智能视觉组-上海交通大学AuTop战队开源算法讲解第九部分--速度控制方案，专栏及开源方案链接：

llo：第16届智能车智能视觉组-上海交通大学AuTop战队开源汇总

上文（木烨：第16届智能车智能视觉组-上海交通大学AuTop战队开源算法讲解(八)系统辨识与速度控制算法）介绍了AuTop在速度控制方面的算法及认识，下面将简单介绍一下，我们的速度控制方案。

一. 速度控制方案

  1. 1. 常规速度控制

常规速度控制指非特殊元素下的速度控制，或者说直道与弯道的速度控制。长直道的速度没有什么需要强调的，我们设定了NORMAL_SPEED为基础速度，弯道速度在此基础上进行加减得到。

我们采用的是根据轨迹斜率的速度控制。由于我们之前已经得到中线的轨迹，因而可以很方便的取中线上一定距离的某点计算车中心到该点的斜率，并乘以一个相关系数，NORMAL_SPEED减去该值即得到弯道目标速度。

在此，纯粹根据斜率成比例进行速度的增减，在长直道突入弯道的情况下，可能会有刹不住车的感觉，因而可以由比例控制换为PD控制。即建立弯道速度增减PID，突入弯道时，由于微分项的作用，会得到一个较低的target_speed，从而获得更快的减速。

实际运行中，直道入弯一般都是初入弯速度降不下来，产生打滑漂移。若给较小的目标速度又过于浪费，因而也可以考虑之前提到的模糊PID的思想，在长直道突入弯道时，给一个较大的PD参数，速度降下来后恢复小的参数，形成一种踩一脚刹车之后在弯道继续维持高速的感觉。

关于速度的控制，若建立简单的数学模型，即考虑过弯的极限速度，在小车的运行中，假定最大摩擦固定，那我们可以得到极限速度与过弯半径成根号关系。

[公式]

因而，我们也可以考虑这一因素，给小车一个更加安全的速度上限，即由曲率半径和标定参数计算得到过弯的速度上限。

三点曲率半径的计算代码如下，标定参数则需要在实践中得到了。由于时间原因，这一部分我们并没有时间进行实践，但理论上是可行的。

float radius_3pts(float pt0[2], float pt1[2], float pt2[2]){
    float a, b, c, d, e, f, r, x, y;
    a = 2*(pt1[0]-pt0[0]);
    b = 2*(pt1[1]-pt0[1]);
    c = pt1[0]*pt1[0]+pt1[1]*pt1[1]-pt0[0]*pt0[0]-pt0[1]*pt0[1];
    d = 2*(pt2[0]-pt1[0]);
    e = 2*(pt2[1]-pt1[1]);
    f = pt2[0]*pt2[0]+pt2[1]*pt2[1]-pt1[0]*pt1[0]-pt1[1]*pt1[1];
    x = (b*f-e*c)/(b*d-e*a);
    y = (d*c-a*f)/(b*d-e*a);
    r = sqrt((x-pt0[0])*(x-pt0[0])+(y-pt0[1])*(y-pt0[1]));
    return r;
}

另外，关于弯道的控制再写一些东西。弯道比较理想的方向环控制方案是在入弯时，贴弯道的内弯，出弯时，贴外弯，此时车辆转弯半径较大，保证车辆速度可以较大。我们在实践中发现纯跟踪算法本身具有着这样的过弯特性，过弯路径效果提升很明显。通过调整纯跟踪的预瞄距离，可以有效提高路径规划的最优性，之前也有看过建议将预瞄距离与速度耦合成二次函数的控制方式，即低速短预瞄防止撞路肩，高速长预瞄保证良好的过弯轨迹。弯道的速度控制方案应为入弯减速避免打滑，出弯加速节约时间。

网上看到别的队伍报告中提到一种十字交叉法，也是很有意思且简介的一种方法，判断中线与道路有无交点决定基础速度，对于小S弯道也具有不减速的特性。

  1. 2. 特殊元素速度控制

这一部分其实需要说的部分并不多，根据实际运行的情况调节即可。

十字一般不需降速，圆环入环(CROSS_BEGIN)时建议微微降速保证入环的姿态，三叉口微微降速保证openart的识别结果。

  1. 3.多模态PID切换

多模态PID的切换即设定多组速度控制PID，在前文提到，我们通过MATLAB的系统辨识与调参工具箱得到不同硬度的PID参数。

此外，为防止起步时速度过高冲出赛道，我们还使用了变积分PID（增量式）起到缓加速的作用，同时增量式PID在克服打滑、减少猛加速上也有着不错的效果。

在Apriltag的停车上，我们使用了位置环的停车方式，保证小车不会冲出过多看不到二维码。这里其实最好采用，固定小车与Apriltag位置（如刻意控制差25cm），不过我们未能来得及尝试。

最后，通过对大转角PWM限幅的方式，减少打滑，例如将舵机转角与PWM进行一定的耦合，防止大转角猛加速带来打滑失控冲出赛道。

二．差速标定方法

常见的差速方式有两种，机械差速与软件差速。前者依靠机械差速器实现，例如B车模，后者指双电机驱动在过弯时软件设定不同的目标速度以起到差速的效果。

下面将简要介绍一下我们采用差速方案。良好的差速会使得小车过弯十分丝滑，而不好的差速则会有严重拖尾或者甩尾的感觉。

C车模采用的是阿克曼结构，特点是，对于车模来说，小车在转向时四轮的垂线交于一点。依据阿克曼转向几何设计的车辆，沿着弯道转弯时，利用四连杆的相等曲柄使内侧轮的转向角比外侧轮大大约2-4度，使四个轮子路径的圆心大致上交会于后轴的延长线上瞬时转向中心，让车辆可以顺畅的转弯

阿克曼建立简单的数学关系模型如下，可以看出，差分速度与tanα成比例关系，考虑机械等因素，我们可以直接标定一个比例参数k。标定方法为，设定小车舵机转角为固定值（例如12°），在平地上手推或者低速让其原地画圆一段距离，记录左右编码器的具体数值，即可计算得到k。当然，也可以不采用标定的方法，直接测量小车实际尺寸进行计算

另外，请注意，这里的α角是车轮的实际转角，我们一般方向环得到的是舵机控制的角度，这两者的角度关系同样需要标定，比例关系标定即可。

三. 机械机构与轮胎

机械结构在较高的速度下对小车的性能影响也很大，一般而言2.3m/s以下的车速不需要过多关注机械，更高速度的提高可能就需要将一部分注意放在机械上了。我们两个人其实对于机械只是略有了解，因而后面部分仅做抛砖引玉用，欢迎大佬指正。

1.前束与外倾

车轮的前束角有内束与外束两种，外束有利于转弯性能，但在直道上容易抖动；内束稳定可以消去较小抖动，保证直线上的稳定动态性能。

车轮外倾，作用是为了保证接触面积，过弯时由于摩擦作用车轮会发生向弯道内侧偏移，为保证接触面积和摩擦力，一般采用外倾的方式。

字丑轻喷:(

考虑前束与外倾两个因素，一般小车前轮呈现一种内八状态，内束稳定抗抖，外倾保证过弯性能

2.轮胎

高速状态下，轮胎抗打滑的作用就体现出来了，比赛跑车时听到咯吱咯吱的声音，那便可以肯定这支队伍一定精心打磨过轮胎，还有许多队伍甚至有祖传轮胎之说。

轮胎的打磨一般需要：软化剂，砂纸（砂轮机），硅橡胶等等。软化剂我们使用的是逐飞店铺的，黄色小瓶，效果显著。一般过程是，涂上软化剂，包上保鲜膜（有钱用软化剂泡轮胎的另当别论），放置一晚上，用砂纸或砂轮机打磨一段时间，跑一天，晚上重复上软化剂，以此类推。

用砂纸打磨应该是挺费劲的一个事情，因为用砂轮机完整打磨一个轮胎也要半小时左右。硅橡胶是为了防止打磨将轮胎磨破，可使用南大硅橡胶进行补胎。另外打磨时，轮毂与橡胶也很容易脱开，需要合适的胶处理，我们也没有找到合适的工具解决这一问题。

我们对于轮胎的处理还是比较粗浅，有的学校会对轮子充气和增加海绵填充物等方式，可以更好的提高轮胎性能。

最后，轮胎以及赛道的干净很重要，发车之前擦轮胎和擦赛道是一个智能车人应有的习惯。建议轮胎使用微湿的抹布，赛道使用较干的抹布进行，擦拭完毕等二者都干掉再发车。也有推荐购买WD-40除锈剂，实践上对于轮胎的清洁确实有作用（就是味道上有些略有难闻）。

如果你更改了你的算法而性能没有提升！如果你提升了速度过不去弯！如果你隔夜的代码不能正常跑车了！请先不要怀疑自己，先擦一遍赛道和轮子吧，你会有意外收获的!

SJTU-AuTop完整开源方案链接，如果觉得我们的方案对您有帮助，请在github上帮忙点个star吧:)

llo：第16届智能车智能视觉组-上海交通大学AuTop战队开源汇总