CASL 根据轮速计算行驶轨迹的曲率

98 阅读 0 评论 65 点赞

我是靠谱客的博主端庄小笼包，最近开发中收集的这篇文章主要介绍CASL 根据轮速计算行驶轨迹的曲率，觉得挺不错的，现在分享给大家，希望可以做个参考。

概述

function curveFromWhlSpd (var sigWhlSpdReRiSts,var sigWhlSpdReRiMovDir,var sigWhlSpdReRi,var sigWhlSpdReLeSts,var sigWhlSpdReLeMovDir,var sigWhlSpdReLe)
{
double WhlSpdReRi_mps;
double WhlSpdReLe_mps;
double whlBase=1.672;//m
if (sigWhlSpdReLeSts==0)
{
if (sigWhlSpdReLeMovDir ==1)
{
WhlSpdReLe_mps=sigWhlSpdReLe/3.6;
}
else if(sigWhlSpdReLeMovDir==2)
{
WhlSpdReLe_mps=-sigWhlSpdReLe/3.6;
}
else
{
WhlSpdReLe_mps=0;
}
}
else
{
WhlSpdReLe_mps=0;
}
if (sigWhlSpdReRiSts==0)
{
if (sigWhlSpdReRiMovDir ==1)
{
WhlSpdReRi_mps=sigWhlSpdReRi/3.6;
}
else if(sigWhlSpdReRiMovDir==2)
{
WhlSpdReRi_mps=-sigWhlSpdReRi/3.6;
}
else
{
WhlSpdReRi_mps=0;
}
}
else
{
WhlSpdReRi_mps=0;
}
if ((WhlSpdReLe_mps+WhlSpdReRi_mps)*0.5*3.6 <10)
{
return 0;//车速小于10kph，不计算曲率
}
if (abs(WhlSpdReLe_mps+WhlSpdReRi_mps) < 0.0001)
{
return 0;
}
else
{
//return WhlSpdReRi_mps+WhlSpdReLe_mps;
return 2*(WhlSpdReRi_mps-WhlSpdReLe_mps)/(WhlSpdReLe_mps+WhlSpdReRi_mps)/whlBase*0.98;
}
}

计算结果可以使用以下函数进行滤波处理：

function LowPassFilterNew_DrvR (var sigDrvRFromWhlSpd)
{
double LastValue = 0;
double result = 0;
double pi = 3.141592653589;
double deltaT = 0;
//!
double fc = 0.2; //cutoff frequency (Grenzfrequenz)
double alpha = 0;
Long CallCounter = 0;
double sigDrvRFromWhlSpd_absValue;
double lftCnt;
double rgtCnt;
sigDrvRFromWhlSpd_absValue = abs(sigDrvRFromWhlSpd);
CallCounter++;
if (CallCounter >= 2)
{
deltaT = time(sigDrvRFromWhlSpd) - time(sigDrvRFromWhlSpd[-1]);
alpha = 1/(1/2/pi/deltaT/fc + 1);
result = alpha * sigDrvRFromWhlSpd_absValue + (1 - alpha) * LastValue;
}
else
{
cancel;
//No return value
}
LastValue = result;
if (result > 1000)
{
if (sigDrvRFromWhlSpd<0)
{
lftCnt --;
rgtCnt ++;
}
else
{
lftCnt --;
rgtCnt ++;
}
}
else
{
lftCnt = 0;
rgtCnt = 0;
}
lftCnt = min(max(0,lftCnt),15);
rgtCnt = min(max(0,rgtCnt),15);
if (result > 750)
{
if (lftCnt > rgtCnt)
{
return result;
}
else
{
return -result;
}
}
else
{
if (sigDrvRFromWhlSpd < 0)
{
return -result;
}
else
{
return result;
}
}
}