代码详解——如何计算横向误差？

路径跟踪快讯

发布于 2022-01-25 13:59:38

1.3K0

发布于 2022-01-25 13:59:38

文章被收录于专栏：Path Tracking LettersPath Tracking Letters

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

在前几期代码详解中，参考路径和实际轨迹均由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
            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<xprd22
            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

源码下载

计算结果：