农机常用跟踪算法----纯追踪算法的具体实现

        最近在做无人驾驶项目的过程中需要用到路径跟踪算法，考虑到无模型的路径跟踪算法有Stanley法和pure pursuit法。相比之下Stanley算法的实现更快捷，在解算出横向偏差和航向偏差之后只需要调整增益系数解算前轮转角即可。而pp算法需要用到路径信息，需要实时的搜寻前视参考点。

        首先介绍实验平台，目前小车的上下位机采用串口通讯，上位机通过QT搭建，下位机Stm32F429做主控

这里只讲参考前视点的动态生成和pure pursuit算法前轮转角决策量计算。

PP算法原理，（以下内容搬运于《implement of the pure pursuit path tracking algorithm》）

                ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​​​​        ​​​​

   通过几何计算得出前轮转角   这里的前轮转角的正负要和下位机的控制逻辑统一起来。

        在整个系统中，导航上位机获取实时定位信息，将经纬度转化为高斯-克吕格直角坐标系便于路径规划。为方便计算，在路径跟踪程序里将参考路径的Y(E)-X(N)轴互换改为直角坐标系X(E)-Y(N)。即：车身的实时位置和规划路径符合X-Y坐标系。

​        首先是前视点的动态选取，根据输入的前视距离进行动态搜寻。在上位机程序中通过QMap容器将参考路径点信息存储在内存块中。通过调用e_ImportPathData函数读取全部参考路径信息。其中参考路径每个索引点路径点存储gauss位置信息，索引点序号，索引点航向。        

        根据当前参考索引点(当前索引点的选取参考论文《基于动态路径搜索的农机自动驾驶软件系统研制》)和设置的前视距离Ld(一般2-3m)，考虑到计算量，采用基于当前定位点和参考索引点后续的n个点依次计算距离(n可根据规划的索引点间隔（dOffsetLimit）去设置，一般0.25m间隔一个索引点，这一系列参考索引点区间定义为“前视区域“)

        依次计算前视区域所有点到当前定位点的距离，找出最接近定义Ld长度的点作为前视参考点。找到该点的键，通过迭代器获取对应的value，至此前视参考点完成索引。

//左转负右转正
double PathTracking::Pure_pursuit_Algorithm(T_GaussPos tgpos, double Ld, double dOffsetLimit
                                            , quint64 nowindex, quint64 nfullindex,QString strpath)
{
    //定义一个长度范围区间减少查找计算量参考点进行初始化
    m_refpoint=tgpos;
    int deflength=10;
    //转化为点的个数，计算取整
    int lengthnum=qRound(deflength/dOffsetLimit);
    e_ImportPathData(strpath);//加载参考文件
    CMapPathFileInfo::iterator iterele;
    quint64 startnum,endnum,findnum;
    startnum=nowindex;
    endnum=nowindex+lengthnum;
    T_NavPlanInfo RefplanInfo,FirstPointInfo;
    //搜寻最近点
    T_Point point[lengthnum];
    if(endnum<=nfullindex)
    {
        double disBetweenpoint=0;
        int i=0;
        //查找区间内的所有点
        for(;startnum<endnum;startnum++)
        {
            //取值
           iterele=m_mapPlanFileLines.find(startnum);
           RefplanInfo=iterele.value();
           disBetweenpoint=qSqrt((qPow((RefplanInfo.dX-tgpos.dX),2)+qPow((RefplanInfo.dY-        
                                                                          tgpos.dY),2)));
           point[i].dis=qAbs(disBetweenpoint-Ld);
           point[i].num=startnum;
           i++;
        }
        double mindis;
        mindis=point[0].dis;
        //查找最短距离点的编号
        for(int j=0;j<20;j++)
        {

            if(mindis>point[j].dis)
            {
                findnum=point[j].num;

            }
        }
        iterele=m_mapPlanFileLines.find(findnum);
        RefplanInfo=iterele.value();
    }
    else
    {
        //终点即为前视点
        iterele=m_mapPlanFileLines.find(nfullindex);
        RefplanInfo=iterele.value();
    }
    iterele=m_mapPlanFileLines.begin();
    FirstPointInfo=iterele.value();
    qDebug()<<"前视索引点修正前"<<RefplanInfo.dX-FirstPointInfo.dX<<RefplanInfo.dY-FirstPointInfo.dY<<"点"<<RefplanInfo.nIndex;
    qDebug()<<"当前定位点"<<tgpos.dX-FirstPointInfo.dX<<tgpos.dY-FirstPointInfo.dY<<"航向"<<tgpos.dHeading;
    //坐标系转换
   turn_body_coordinate(tgpos,RefplanInfo);
   //Refplaninfo已修改为车身坐标系下的位置
    double delta=-atan(2*RefplanInfo.dX*CARLENGTH/qPow(Ld,2));
    qDebug()<<"delta"<<delta;
    return delta;
}

将前视参考点转化到车身坐标系（很重要）

在获取到前视点在车身坐标系的坐标后可代入pp算法的公式

下面为参考点由高斯坐标系转化到车身坐标系的函数。tgpos为车身当前状态(dX，dY,dHeading)

void PathTracking::turn_body_coordinate(T_GaussPos tgpos, T_NavPlanInfo &referencePoint)
{
    int dXtemp=referencePoint.dX-tgpos.dX;
    int dYtemp=referencePoint.dY-tgpos.dY;
    qDebug()<<"XYTemp"<<dXtemp<<dYtemp;
    tgpos.dHeading=tgpos.dHeading/57.3;
    //修正到车身坐标系-（右前上） 的前视点
    referencePoint.dX=dXtemp*qCos(tgpos.dHeading)-dYtemp*qSin(tgpos.dHeading);
    referencePoint.dY=dXtemp*qSin(tgpos.dHeading)+dYtemp*qCos(tgpos.dHeading)
}

 这里例举拖拉机常采用的前视距离配套行驶速度

0.1km/h-1km/h   Ld=1.7-1.8;

2km/h-4km/h      Ld=2.0-2.2;

5km/h-7km/h      Ld=2.2-2.4;

7km/h-9km/h      Ld=2.4-2.6;

10km/h以上         Ld=2.8-3.0;

        规划了一个往复式梭行作业路径路径跟踪效果如下图所示：