sift计算描述子代码详解_代码详解——如何计算横向误差？

在路径跟踪控制的论文中，我们常会看到判断精确性的指标，即横向误差和航向误差，那么横向误差和航向误差如何获得？

在前几期代码详解中，参考路径和实际轨迹均由To Workspace模块导出，如图所示：

那么怎么利用这些量计算误差？

计算横向误差最简单的方案，当然是sqrt((x1-x0)^2+(y1-y0)^2)，但是这样计算出来的值是行驶轨迹上的点与参考路径上的点之间的距离，并非车辆与参考路径上距离车辆最近的点之间的距离，也就不符合横向误差的定义。而且如果参考路径和行驶轨迹的采样间隔不同，这样的计算方式就更加不可靠。比较不幸的是前几期代码中，To Workspace模块采用的都是默认采样间隔，即圈中的项为-1，这种情况下参考路径和行驶轨迹的采样间隔就是不同的。

(所有To Workspace的采样间隔都建议改成控制系统的采样间隔，在前几期代码中，该值为0.05，我之前忘记改了)

横向误差的定义可见：

Bai G, Liu L, Meng Y, et al. Path tracking of wheeled mobile robots based on dynamic prediction model[J]. IEEE Access, 2019, 7: 39690-39701.

在此给出计算横向误差的代码：

for i=1:1:15507 %该数值为行驶轨迹点的数量，可在workspace查看

    xr0=x1(i,1);

    yr0=y1(i,1);

    xrdd=10000;

    xrdk=0;

    for ir=1:1:986 %该数值为参考路径点的数量，可在workspace查看

        xrd(ir)=abs(sqrt((x0(ir,1)-xr0)^2+(y0(ir,1)-yr0)^2));

        if xrd(ir)

            xrdd=xrd(ir);

            xrdk=ir;

            xr1=x0(ir,1);

            yr1=y0(ir,1);

        end

    end

    if xrdk>1&&xrdk<986

        xprd21=xrd(xrdk-1);

        xprd22=xrd(xrdk+1);

        if xprd21

            xr2=x0(xrdk-1,1);

            yr2=y0(xrdk-1,1);

        else

            xr2=x0(xrdk+1,1);

            yr2=y0(xrdk+1,1);

        end

    elseif xrdk==1

        xr2=x0(xrdk+1,1);

        yr2=y0(xrdk+1,1);

    else

        xr2=x0(xrdk-1,1);

        yr2=y0(xrdk-1,1);

    end

    an=(yr2-yr1);

    bn=(xr2-xr1);

    cn=bn*yr1-an*xr1;

    e(i,1)=abs(an*xr0-bn*yr0+cn)/sqrt(an^2+bn^2);

    if xrdk<986

        Tmp=(y0(xrdk,1)-y0(xrdk+1,1))*xr0+(x0(xrdk+1,1)-x0(xrdk,1))*yr0+x0(xrdk,1)*y0(xrdk+1,1)-x0(xrdk+1,1)*y0(xrdk,1);

    else

        Tmp=(y0(xrdk-1,1)-y0(xrdk,1))*xr0+(x0(xrdk,1)-x0(xrdk-1,1))*yr0+x0(xrdk-1,1)*y0(xrdk,1)-x0(xrdk,1)*y0(xrdk-1,1);

    end

    if Tmp<=0

        e(i,1)=-e(i,1);

    end

end

源码下载

计算结果：

对于航向误差，则需要对仿真系统进行修改，且待下期分解。