十八届智能车负压电磁组（二）：舵机方向控制篇

文章目录

前言

一、差比和

二、差比和差

三、舵机方向控制

1、舵机初始化

2、舵机PD控制

---

前言

本篇将介绍如何利用电感值计算偏差来控制舵机。

一、差比和

所谓差比和，就是利用左右两对称电感的值的和除以它们的差。

 

typedef struct 
{
	float L_sp_err;//水平电感当前偏差
	float L_sz_err;//竖直电感当前偏差
	float L_x_err;//斜电感当前偏差
	float L_sp_sz_err;//水平和竖直电感偏差加权
	
	float L_sp_err_last;//水平电感上一次偏差
	float L_sz_err_last;//竖直电感上一次偏差
	float L_x_err_last;//斜电感上一次偏差
	float L_sp_sz_err_last;//水平和竖直电感偏差加权上一次值
	
	float L_sp_err_diff;//水平电感当前偏差与上一次偏差的差值
	float L_sz_err_diff;//竖直电感当前偏差与上一次偏差的差值
	float L_x_err_diff;//斜电感当前偏差与上一次偏差的差值
	float L_sp_sz_err_diff;//水平和竖直电感偏差加权与上一次偏差的差值
	
}DG_err;

计算左右两水平电感的偏差

dg_err.L_sp_err = (dg_state.L_lsp_twice - dg_state.L_rsp_twice) * 100 / (dg_state.L_lsp_twice + dg_state.L_rsp_twice + 1);//(左水平-右水平)/(左水平+右水平+1)
dg_err.L_sp_err_diff = dg_err.L_sp_err - dg_err.L_sp_err_last;//计算误差增量
dg_err.L_sp_err_last = dg_err.L_sp_err;//更新误差

在计算水平电感的偏差时，分母(dg_state.L_lsp_twice + dg_state.L_rsp_twice + 1)是为了避免出现为0的情况。

偏差乘以100是将偏差放大100倍，便于处理。

同理可计算出竖直电感以及斜电感的偏差。

对于车模在循迹时，直道上利用水平电感循迹更为合适，而弯道上可以利用竖直电感或斜电感循迹，在实际测试时，斜电感对于弯道的敏感性会强于竖直电感。所以，在车模实际循迹时，可以将水平电感偏差与斜电感偏差融合起来，使得车模可以走直道以及弯道。

而我是参考了卓大发的一篇文章：《智能车电感差比和差加权算法研究》，采用水平电感与竖直电感的偏差融合进行循迹。

二、差比和差

该算法是将水平电感与竖直电感的差比和结合起来，构造一个新的差比和差公式，使得车模在直道上接近一条直线行驶，而接近弯道时又能很快的做出反应。

公式为：

其中系数A为水平电感权重，B为竖直电感差权重，C为竖直电感和权重，P为比例系数

在实际运用时，需要人为调整A,B,C,P四个参数，使得偏差线性化。

dg_err.L_sp_sz_err_last = dg_err.L_sp_sz_err;//更新上一次的值
fengzi = adc_A*(dg_state.L_lsp_once-dg_state.L_rsp_once) + adc_B*(dg_state.L_lsz_once-dg_state.L_rsz_once);
fengmu = adc_A*(dg_state.L_lsp_once+dg_state.L_rsp_once) + adc_C*myAbs(dg_state.L_lsz_once-dg_state.L_rsz_once);
dg_err.L_sp_sz_err = fengzi * adc_P / (fengmu + 1);//分母加1：防止分母为0
dg_err.L_sp_sz_err_diff = dg_err.L_sp_sz_err - dg_err.L_sp_sz_err_last;//当前-上一次

至此，我们将电感值处理成偏差，该偏差可以用来控制舵机以及后轮差速。

三、舵机方向控制

1、舵机初始化

在刚开始使用舵机时，都需要对舵机进行校准。

（1）找到前轮打正时，舵机占空比输出，该值为归中值，记为Servo_Center；

（2）在Servo_Center基础上增加一定步长占空比，观察舵机向左打角还是右打角，然后一点一点增加占空比，直到前轮与车地盘相触，在影响前轮转动情况下，记下当前舵机占空比与Servo_Center的差值，记为Servo_Delta；

（3）根据左右对称，找到舵机占空比上下限，记为Servo_Left_Max=(Servo_Center + Servo_Delta)，Servo_Right_Min=(Servo_Center - Servo_Delta)（我的是占空比增大向左打角，占空比减小向右打角）。

2、舵机PD控制

对于舵机控制需要其响应具有快速性，故一般采用PD控制。

typedef struct
{
    float                kp;         //P
    float                ki;         //I
    float                kd;         //D
    float                imax;       //积分限幅

    float                out_p;  //KP输出
    float                out_i;  //KI输出
    float                out_d;  //KD输出
    float                out;    //pid输出
	float				 last_out;//pid上一次输出
	float				 out_diff;//pid当前输出与上一次输出的差值

    float                integrator; //< 积分值
    float                last_error; //< 上次误差
    float                last_derivative;//< 上次误差与上上次误差之差

    float                target;     //<  设置的期望值
	
}pid_param_t;

舵机PD控制程序 

//位置式
float ServoPidLocCtrl(pid_param_t * pid,float error)
{	
    pid->out_p = pid->kp * error;
    pid->out_d = pid->kd * (error - pid->last_error);

    pid->last_error = error;

    pid->out = pid->out_p + pid->out_d;

    return pid->out;
}

通过将舵机PD控制程序输出结果作为舵机占空比输出（舵机duty=Servo_Center+Pid_Servo.out），即可控制车模在直道上直线行驶，过弯道时，快速打角。

下一篇将介绍后轮PID控制以及差速控制。

十八届智能车负压电磁组（三）：后轮PID控制及差速控制篇