基于实验数据的轮胎模型

参考CarSim的轮胎公式，本篇介绍一种基于实验数据的轮胎模型。输入为垂直力，侧偏角，外倾角，滑移率，输出为牵引力，侧向力和回正力矩。模型对轮胎进行了简化，不考虑路面摩擦，车速及轮胎Mx，My转矩。程序代码用Simulink S-Function完成，计算结果与CarSim自带的轮胎模型进行了对比。

1. 坐标系

轮胎坐标系的坐标原点位于轮胎与地面的接触点，方向设置如下图。

2. 模型公式

Gamma角对轮胎侧向力和回正力矩的影响，通过修正侧偏角Alpha来实现的。公式如下：

其中τ会Alpha角度的斜率，为Camber Trust系数，为Linear Conering Stiffness。

实验数据是针对纯滑移率或纯侧偏情况下采集的。而实际上轮胎力为侧向力和牵引力的合力，彼此是有影响的。因此该模型采用Pacejka的Combine Slip Theory来对两个分力进行椭圆化。

求解公式如下，具体解释可参考CarSim的轮胎帮助文档。

对上面公式进行正交化，max为相应的力最大时取得的delta值。

从而可以求出新的滑移率和侧偏角：

将它们带入实验数据，可得对应的牵引力和侧向力。

对这两个力进行椭圆化：

当时，时。

从而求出最终的牵引力和侧向力：

其中：

上面这些变量之间的关系，参见下图：

回转力矩的公式由下式求出：

3. 轮胎S-Function实现

S-Function的用法，参见博文： MATLAB中的S-Function的用法（C语言）。

线性插值方法，参见博文：双线性插值(Bilinear Interpolation) 。

#define S_FUNCTION_NAME  CarSimTire
#define S_FUNCTION_LEVEL 2
#include "simstruc.h"
#include <math.h>
#define TIRE_FX_KAPPA(S)  ssGetSFcnParam(S,0)
#define TIRE_FX_LOAD(S)  ssGetSFcnParam(S,1)
#define TIRE_FX(S)  ssGetSFcnParam(S,2)
#define TIRE_FY_ALPHA(S)  ssGetSFcnParam(S,3)
#define TIRE_FY_LOAD(S)  ssGetSFcnParam(S,4)
#define TIRE_FY(S)  ssGetSFcnParam(S,5)
#define TIRE_MZ_ALPHA(S)  ssGetSFcnParam(S,6)
#define TIRE_MZ_LOAD(S)  ssGetSFcnParam(S,7)
#define TIRE_MZ(S)  ssGetSFcnParam(S,8)
#define TIRE_CAM_LOAD(S)  ssGetSFcnParam(S,9)
#define TIRE_CAM_COEF(S)  ssGetSFcnParam(S,10)
#define ROWS  300
#define COLS  10
#define PI  3.1416
#define ABS(val) val>=0?val:-val
#define SIGN(val) (val==0)?0:(val>0?1:-1)
#define ZERO 0.00001
static real_T TireFxKappaPeak[COLS];
static real_T TireFyAlphaPeak[COLS];
static int_T TireFxSize[2];
static int_T TireFySize[2];
static int_T TireMzSize[2];
static int_T TireCamSize;

static void mdlInitializeSizes(SimStruct *S)
{
    ssSetNumSFcnParams(S, 11);  /* Number of expected parameters */
    if (ssGetNumSFcnParams(S) != ssGetSFcnParamsCount(S)) return;

    ssSetNumContStates(S, 0);
    ssSetNumDiscStates(S, 0);

    if (!ssSetNumInputPorts(S, 4)) return;
    
    ssSetInputPortWidth(S, 0, 1);
    ssSetInputPortRequiredContiguous(S, 0, true);
    ssSetInputPortDirectFeedThrough(S, 0, 1);
    
    ssSetInputPortWidth(S, 1, 1);
    ssSetInputPortRequiredContiguous(S, 1, true);
    ssSetInputPortDirectFeedThrough(S, 1, 1);
    
    ssSetInputPortWidth(S, 2, 1);
    ssSetInputPortRequiredContiguous(S, 2, true);
    ssSetInputPortDirectFeedThrough(S, 2, 1);
    
    ssSetInputPortWidth(S, 3, 1);
    ssSetInputPortRequiredContiguous(S, 3, true);
    ssSetInputPortDirectFeedThrough(S, 3, 1);

    if (!ssSetNumOutputPorts(S, 3)) return;
    ssSetOutputPortWidth(S, 0, 1);
    ssSetOutputPortWidth(S, 1, 1);
    ssSetOutputPortWidth(S, 2, 1);

    ssSetNumSampleTimes(S, 1);
    ssSetNumRWork(S, 0);
    ssSetNumIWork(S, 0);
    ssSetNumPWork(S, 0);
    ssSetNumModes(S, 0);
    ssSetNumNonsampledZCs(S, 0);

    /* Specify the sim state compliance to be same as a built-in block */
    ssSetSimStateCompliance(S, USE_DEFAULT_SIM_STATE);

    ssSetOptions(S, 0);
}



/* Function: mdlInitializeSampleTimes =========================================
 * Abstract:
 *    This function is used to specify the sample time(s) for your
 *    S-function. You must register the same number of sample times as
 *    specified in ssSetNumSampleTimes.
 */
static void mdlInitializeSampleTimes(SimStruct *S)
{
    ssSetSampleTime(S, 0, CONTINUOUS_SAMPLE_TIME);
    ssSetOffsetTime(S, 0, 0.0);
}



#define MDL_INITIALIZE_CONDITIONS   /* Change to #undef to remove function */
#if defined(MDL_INITIALIZE_CONDITIONS)
  /* Function: mdlInitializeConditions ========================================
   * Abstract:
   *    In this function, you should initialize the continuous and discrete
   *    states for your S-function block.  The initial states are placed
   *    in the state vector, ssGetContStates(S) or ssGetRealDiscStates(S).
   *    You can also perform any other initialization activities that your
   *    S-function may require. Note, this routine will be called at the
   *    start of simulation and if it is present in an enabled subsystem
   *    configured to reset states, it will be call when the enabled subsystem
   *    restarts execution to reset the states.
   */
  static void mdlInitializeConditions(SimStruct *S)
  {
  }
#endif /* MDL_INITIALIZE_CONDITIONS */



#define MDL_START  /* Change to #undef to remove function */
#if defined(MDL_START) 
  /* Function: mdlStart =======================================================
   * Abstract:
   *    This function is called once at start of model execution. If you
   *    have states that should be initialized once, this is the place
   *    to do it.
   */
static void mdlStart(SimStruct *S)
{
    real_T *pFxKappa = mxGetPr(TIRE_FX_KAPPA(S));
    real_T *pFxLoad = mxGetPr(TIRE_FX_LOAD(S));
    real_T *pFx = mxGetPr(TIRE_FX(S));
    real_T *pFyAlpha = mxGetPr(TIRE_FY_ALPHA(S));
    real_T *pFyLoad = mxGetPr(TIRE_FY_LOAD(S));
    real_T *pFy = mxGetPr(TIRE_FY(S));
    int_T *dFx = mxGetDimensions(TIRE_FX(S));
    int_T *dFy = mxGetDimensions(TIRE_FY(S));
    int_T *dMz = mxGetDimensions(TIRE_MZ(S));
    
    TireFxSize[0] = dFx[0];
    TireFxSize[1] = dFx[1];
    TireFySize[0] = dFy[0];
    TireFySize[1] = dFy[1];
    TireMzSize[0] = dMz[0];
    TireMzSize[1] = dMz[1];
    
    PeakValues(TireFxKappaPeak, pFxKappa, pFxLoad, pFx, TireFxSize);
    PeakValues(TireFyAlphaPeak, pFyAlpha, pFyLoad, pFy, TireFySize); 
}
#endif /*  MDL_START */



/* Function: mdlOutputs =======================================================
 * Abstract:
 *    In this function, you compute the outputs of your S-function
 *    block.
 */
static void mdlOutputs(SimStruct *S, int_T tid)
{
    //Paramters
    real_T *pFxKappa = mxGetPr(TIRE_FX_KAPPA(S));
    real_T *pFxLoad = mxGetPr(TIRE_FX_LOAD(S));
    real_T *pFx = mxGetPr(TIRE_FX(S));
    real_T *pFyAlpha = mxGetPr(TIRE_FY_ALPHA(S));
    real_T *pFyLoad = mxGetPr(TIRE_FY_LOAD(S));
    real_T *pFy = mxGetPr(TIRE_FY(S));
    real_T *pMzAlpha = mxGetPr(TIRE_MZ_ALPHA(S));
    real_T *pMzLoad = mxGetPr(TIRE_MZ_LOAD(S));
    real_T *pMz = mxGetPr(TIRE_MZ(S));
    real_T *pCamLoad = mxGetPr(TIRE_CAM_LOAD(S));
    real_T *pCamCoef = mxGetPr(TIRE_CAM_COEF(S));
    
    //Inputs
    real_T *uTireFz = (const real_T*) ssGetInputPortSignal(S,0);  //Vertical Load
    real_T *uAlpha = (const real_T*) ssGetInputPortSignal(S,1);  //Side Slip Angle
    real_T *uGamma = (const real_T*) ssGetInputPortSignal(S,2);  //Inclination Angle(Camber angle)
    real_T *uKappa = (const real_T*) ssGetInputPortSignal(S,3);  //Longitudinal slip ratio
    
    //Outputs
    real_T       *yTireFx = ssGetOutputPortSignal(S,0);
    real_T       *yTireFy = ssGetOutputPortSignal(S,1);
    real_T       *yTireMz = ssGetOutputPortSignal(S,2);

    //Variables
    real_T deltaX, deltaY;
    real_T deltaXMax, deltaYMax;
    real_T deltaXStar, deltaYStar, deltaStar;
    real_T alphaMax, kappaMax;
    real_T alpha2, kappa2;
    real_T tireFx0, tireFy0, tireMz0;
    real_T tireFxNorm0, tireFyNorm0;
    real_T theta,lambda;
    real_T tireFx, tireFy, tireFz, tireMz;
    real_T alpha, kappa, gamma;
    real_T alphaK, gammaK;
    real_T tmp, tmp2, sgn;
    real_T epsilon;

    tireFz = uTireFz[0];
    alpha = uAlpha[0];
    kappa = uKappa[0];
    gamma = uGamma[0];
    
    //Camber Effect (Simple)
    LinearSpline(&gammaK, pCamLoad, pCamCoef, TireCamSize, tireFz);
    BiLinearSpline(&tmp, pFyAlpha, pFyLoad, pFy, TireFySize, pFyAlpha[2], tireFz);
    alphaK = tmp / pFyAlpha[2];
    tmp = ABS(gammaK/alphaK);
    alpha = alpha + gamma * tmp;
    
    //Combined Slip Theory
    tmp = 1.0 + kappa;
    tmp = (tmp < ZERO) ? ZERO : tmp;
    deltaX = -kappa / tmp;
    deltaY = (real_T)tan(alpha * PI / 180.0) / tmp;
    
    LinearSpline(&kappaMax, pFxLoad, TireFxKappaPeak, TireFxSize[1], tireFz); //Slip ratio for the Max Fx
    LinearSpline(&alphaMax, pFyLoad, TireFyAlphaPeak, TireFySize[1], tireFz); //Slip angle for the Max Fy
    deltaXMax = -kappaMax / (1 + kappaMax);
    deltaYMax = (real_T)tan(alphaMax * PI / 180.0) / (1 + kappaMax);
    
    deltaXStar = deltaX / deltaXMax;
    deltaYStar = deltaY / deltaYMax;
    deltaStar = (real_T)sqrt(deltaXStar * deltaXStar + deltaYStar * deltaYStar);
    
    sgn = SIGN(deltaX);
    kappa2 = -deltaStar * deltaXMax / (1 - deltaStar * deltaXMax * sgn);
    BiLinearSpline(&tireFx0, pFxKappa, pFxLoad, pFx, TireFxSize, kappa2, tireFz);
        
    alpha2 = (real_T)atan(deltaStar * deltaYMax)  * 180 / PI;
    BiLinearSpline(&tireFy0, pFyAlpha, pFyLoad, pFy, TireFySize, alpha2, tireFz);

    epsilon = (deltaStar > 1) ? 1 : deltaStar;
    tmp = (deltaStar < ZERO) ? ZERO : deltaStar;
    tireFxNorm0 = tireFx0 - epsilon * (tireFx0 - tireFy0) * (deltaYStar / tmp) * (deltaYStar / tmp);
    tireFyNorm0 = tireFy0 - epsilon * (tireFy0 - tireFx0) * (deltaXStar / tmp) * (deltaXStar / tmp);
    
    
    tmp = ABS(deltaX);
    tmp = (tmp < ZERO) ? ZERO : tmp;
    theta = (real_T)atan(deltaY / tmp);
    lambda = theta;
    
    sgn = SIGN(deltaXStar);
    tireFx = tireFxNorm0 * cos(lambda) * sgn;
    tireFy = -tireFyNorm0 * sin(lambda);
    
    BiLinearSpline(&tireMz0, pMzAlpha, pMzLoad, pMz, TireMzSize, alpha2, tireFz);
    tmp = ABS(tireFy);
    sgn = SIGN(alpha);
    tmp2 = (tireFyNorm0 < ZERO) ? ZERO : tireFyNorm0;
    tireMz = tireMz0 * tmp / tmp2 * sgn;
    
    yTireFx[0] = tireFx;
    yTireFy[0] = tireFy;
    yTireMz[0] = tireMz;
}


static void LinearSpline(real_T* outY, const real_T* dataX, const real_T* dataY, const int_T length, const real_T inX)
{
    int_T i;
    int_T cx1, cx2;
    real_T x1, x2, y1, y2;
    real_T tmp;
    
    //x position
    if(inX <= dataX[0])
    {
        cx1 = 0;
        cx2 = 1;
    }
    else if(inX >= dataX[length-1])
    {
        cx1 = length - 2;
        cx2 = length - 1;
    }
    else
    {
        for(i=0; i<length-1; i++)
        {
            if (inX >= dataX[i] && inX < dataX[i + 1])
            {
                cx1 = i;
                cx2 = i + 1;
                break;
            }
        }
    }
    
    //range
    x1 = dataX[cx1];
    x2 = dataX[cx2];
    y1 = dataY[cx1];
    y2 = dataY[cx2];
    
    //Linear Spline Equation
    tmp = x2 - x1;
    *outY = y1 * (x2 - inX) / tmp + y2 * (inX - x1) / tmp;
}


static void BiLinearSpline(real_T* outZ, const real_T* dataX, const real_T* dataY, const real_T* dataZ, const int_T* size, const real_T inX, const real_T inY)
{
    int_T i;
    int_T cx1, cx2, cy1, cy2;
    real_T x1, x2, y1, y2, z11, z12, z21, z22;
    real_T tmp;
    
    //x position
    if(inX <= dataX[0])
    {
        cx1 = 0;
        cx2 = 1;
    }
    else if(inX >= dataX[size[0]-1])
    {
        cx1 = size[0] - 2;
        cx2 = size[0] - 1;
    }
    else
    {
        for(i=0; i<size[0]-1; i++)
        {
            if (inX >= dataX[i] && inX < dataX[i + 1])
            {
                cx1 = i;
                cx2 = i + 1;
                break;
            }
        }
    }
    
    //y position
    if(inY <= dataY[0])
    {
        cy1 = 0;
        cy2 = 1;
    }
    else if(inY >= dataY[size[1]-1])
    {
        cy1 = size[1] - 2;
        cy2 = size[1] - 1;
    }
    else
    {
        for(i=0; i<size[1]-1; i++)
        {
            if (inY >= dataY[i] && inY < dataY[i + 1])
            {
                cy1 = i;
                cy2 = i + 1;
                break;
            }
        }
    }
    
    //range
    x1 = dataX[cx1];
    x2 = dataX[cx2];
    y1 = dataY[cy1];
    y2 = dataY[cy2];
    z11 = dataZ[cy1 * size[0] + cx1];
    z12 = dataZ[cy2 * size[0] + cx1];
    z21 = dataZ[cy1 * size[0] + cx2];
    z22 = dataZ[cy2 * size[0] + cx2];
    
    //BiLinear Spline Equation
    tmp = (x2 - x1) * (y2 - y1);
    *outZ = z11 * (x2 - inX) * (y2 - inY) / tmp + z21 * (inX - x1) * (y2 - inY) / tmp
            + z12 * (x2 - inX) * (inY - y1) / tmp + z22 * (inX - x1) * (inY - y1) / tmp;
    
}


static void PeakValues(real_T* outX, const real_T* dataX, const real_T* dataY, const real_T* dataZ, const int_T* size)
{
    int_T i, j;
    real_T x, z;
    int_T step;
    real_T stepSize;
    real_T tmpX, tmpY, tmpZ;
    
    step = 500;
    stepSize = (dataX[size[0] - 1] - dataX[0]) / (real_T)step;
   
    for(j=0; j<size[1]; j++)
    {
        tmpY = dataY[j];
        x = dataX[0];
        z = dataZ[j*size[0]];
        
        for(i=0; i<step; i++)
        {
            tmpX = (i + 1) * stepSize + dataX[0];
            BiLinearSpline(&tmpZ, dataX, dataY, dataZ, size, tmpX, tmpY);

            if(tmpZ > z)
            {
                x = tmpX;
                z = tmpZ;
            }
        }
        
        outX[j] = x;
    }

}


#define MDL_UPDATE  /* Change to #undef to remove function */
#if defined(MDL_UPDATE)
  /* Function: mdlUpdate ======================================================
   * Abstract:
   *    This function is called once for every major integration time step.
   *    Discrete states are typically updated here, but this function is useful
   *    for performing any tasks that should only take place once per
   *    integration step.
   */
  static void mdlUpdate(SimStruct *S, int_T tid)
  {
  }
#endif /* MDL_UPDATE */



#define MDL_DERIVATIVES  /* Change to #undef to remove function */
#if defined(MDL_DERIVATIVES)
  /* Function: mdlDerivatives =================================================
   * Abstract:
   *    In this function, you compute the S-function block's derivatives.
   *    The derivatives are placed in the derivative vector, ssGetdX(S).
   */
  static void mdlDerivatives(SimStruct *S)
  {
  }
#endif /* MDL_DERIVATIVES */



/* Function: mdlTerminate =====================================================
 * Abstract:
 *    In this function, you should perform any actions that are necessary
 *    at the termination of a simulation.  For example, if memory was
 *    allocated in mdlStart, this is the place to free it.
 */
static void mdlTerminate(SimStruct *S)
{
}


/*======================================================*
 * See sfuntmpl_doc.c for the optional S-function methods *
 *======================================================*/

/*=============================*
 * Required S-function trailer *
 *=============================*/

#ifdef  MATLAB_MEX_FILE    /* Is this file being compiled as a MEX-file? */
#include "simulink.c"      /* MEX-file interface mechanism */
#else
#include "cg_sfun.h"       /* Code generation registration function */
#endif