简易后轮差速导出与代码实现

导语

今天写了一部分C车的控制代码，电机部分使用串级PID（尚未完成），转向环为外环，速度环为内环，两个电机具有独立的速度环。而将内外环联系起来的，则是转向差速部分。

由于C车车模体积较小，且使用的STC16单片机性能不算特别优秀，故没有使用完全的转向差速算法，而是在几何上进行了一些简化，在保证控制精度较为合理的情况下减少运算。

几何模型

图一

一般来说前轮的转向角如图一所示，由于前轮左右两轮并非严格平行，而是向外略扩张，其从转轴延长线才能够汇交与远处一点。而这一点被认为是转向轨迹的曲率圆中心。也正是这样的设计，保证了车辆转向的灵活性，亦减小了对轮胎的磨损。

因为轨迹曲率半径与前轮两主销中心距比值较大，所以在一定程度上忽略阿克曼转向几何大概可以获得近似的效果。

图二

在简化的转向控制中，将前轮转向角取做 两前轮中心点与曲率圆圆形的连线 和 后轮轮轴延长线 所成的夹角 θ。

而后轮差速的目的，是配合前轮，使小车做以两前轮中心点以速度V1运动。

所以很容易的：

图三

可以得到这六个公式（注意序号1和5中实际只有两个公式）

其使用黑色圈出的是未知量，六个公式六个未知量，可解。

解得：

图四

代码实现

typedef struct
{
    uint16 Car_Length;  //前后轮距
    uint16 Car_Width;   //后轮距

    float Car_Angle;    //转向角 弧度 左正右负
    float Car_Speed;    //车速 米/秒

    int32 Speed_Motor_L;
    int32 Speed_Motor_R;
}SteeringInfo;

其中, Car_Length 对应图二中 Lc，Car_Width 对应 Lw，Car_Angle 对应 θ，Car_Speed 对应 V2

//简易后轮差速
void Simple_Steering(SteeringInfo *Steering, MotorInfo Motor_L, MotorInfo Motor_R, int32 Servo_PWM)
{
    //期望速度计算
    int32 W;
    Steering->Car_Angle = Servo_PWM * SERVO_ANGLE_EVERY_PWM;
    W = (int32)((Steering->Car_Width / (Steering->Car_Length * 2)) * tan(Steering->Car_Angle));
    Steering->Speed_Motor_L = Steering->Car_Speed * (1 - W);
    Steering->Speed_Motor_R = Steering->Car_Speed * (1 + W);

    Motor_L.Motor_Aim = Steering->Speed_Motor_L;
    Motor_R.Motor_Aim = Steering->Speed_Motor_R;
}

结语

写啥结语，睡大觉！