第四十三篇,点线关系

2023-12-15 14:22:25

一、缘起

故事的起源是做AD的插值预瞄发现一个bug,首先放上一维线性插值的代码如下:

template <typename T>
T lerp(const T &x0, const double t0, const T &x1, const double t1,
       const double t) {
  if (std::abs(t1 - t0) <= 1.0e-6) {
    return x0;
  }
  const double r = (t - t0) / (t1 - t0);
  const T x = x0 + r * (x1 - x0);
  return x;
}

比如在两个轨迹点a和b之间做插值预瞄,正常来说插值点应当落在ab之间,一开始没发现什么问题,只是轨迹跟踪误差有点儿大,就找优化的手段。

二、现象与初步分析

找来找去最后还是通过MATLAB plot做了个动画发现了端倪,有的时段插值后的预瞄距离会莫名其妙地逐渐缩短,换句话说,插值预瞄点会逐渐靠近自车甚至飘到自车后头去。

分析后发现,当自车在线段ab之外时正常,如下面左图所示场景;当自车落在线段ab之间时问题就出现了,如下面右图所示场景(P是期望的插值位置):

进一步分析发现,当自车落在线段ab之间时,按照插值原理,自车与后方点即a点的距离应置为负值才能保证插值结果的正确性,而代码中未做处理,执行std::hypot()之后就不管了,所以无论何种场景,与任何点的距离都是正值,这种没头脑的不符合数学物理常识的方法是有隐患的。

三、数学抽象

到这里也就明确了,需要对自车与轨迹点连线ab的相对位置做检查,并根据检查结果对自车与a点的距离做正负处理,抽象为数学问题即点线关系

在本例中,为点在线段区间内还是区间外的问题,如下图所示:

四、方法一

从几何学的角度可知,可以用构造直线方程的方法来做。

首先,我们有ab两点的坐标,很容易构造出它俩所在直线的方程y=kx+m;

然后,经过点a且与线ab垂直的直线Line1、经过点b且与线ab垂直的直线Line2,Line1与Line2所夹形成一个空间,如果自车在此空间外,则与a点的距离为正;如果自车落在此空间内部,则需将与a点的距离置为负;

根据初中数学,很容易可知Line1与Line2的斜率都为-1/k,它们分别经过点a、b,则可以得出完整的直线方程;

关键来了,Line1与Line2的直线方程有了,怎么利用这些信息检查自车是否在区间内?

方法是这样的:将自车坐标的x代入Line1方程,得到位于Line1上的一个y,如果此y大于点a的y则自车在区间外,否则自车在区间内,查看上述示意图可知,代码如下。

double dfDistWith_a = 1; // 假定先前已算出自车与a点的距离绝对值为1m
double dfX, dfY; // 假定自车坐标
double dfX_a, dfY_a; // 假定轨迹点a的坐标
double dfX_b, dfY_b; // 假定轨迹点b的坐标

if (std::abs(dfX_a-dfX_b) > 1E-4)
{
    // 这里的处理直接取了倒数,省去一步检查除数为0的操作
    double k_perpendicular = (dfY_b-dfY_a) / (dfX_b-dfX_a);
    k_perpendicular *= -1.0; // x = k*y + b

    double b_perpendicular_a = dfX_a - k_perpendicular * dfY_a;
    double b_perpendicular_b = dfX_b - k_perpendicular * dfY_b;
    double pseudo_x_a = k_perpendicular * dfY + b_perpendicular_a;
    double pseudo_x_b = k_perpendicular * dfY + b_perpendicular_b;
    if ((dfX>pseudo_x_a && dfX<pseudo_x_b) || 
        (dfX<pseudo_x_a && dfX>pseudo_x_b))
    {
        dfDistWith_a *= -1.0F;
    }
}
else
{
    if ((dfY>dfY_a && dfY<dfY_b) || 
        (dfY<dfY_a && dfY>dfY_b))
    {
        dfDistWith_a *= -1.0F;
    }
}

五、更优解

另外,还有一个更简便代码量也更少的方法,即向量夹角法

重新观察最上面的左右两个图可以发现这样一个规律,当自车在区间外时,自车与a点所成向量,与向量ab的夹角为钝角,否则为直角或锐角。

有了这一信息,可以想到,将上述两个向量构造出来,做向量点乘(也叫内积),判断点乘结果的正负(钝角为负,否则为0或正),即可得知自车与a点的距离该正还是该负,方便快捷!

代码如下,代码量和逻辑都简洁了很多,并且少了除法、ab是横线竖线等特殊情况的判断,优雅!

如果使用二维向量库Vec2d的话代码量会更少。

double dfDistWith_a = 1; // 假定先前已算出自车与a点的距离绝对值为1m
double dfX, dfY; // 假定自车坐标
double dfX_a, dfY_a; // 假定轨迹点a的坐标
double dfX_b, dfY_b; // 假定轨迹点b的坐标

double dfVector_a_x = dfX - dfX_a; // 构造一个向量
double dfVector_a_y = dfY - dfY_a;
double dfVector_b_x = dfX_b - dfX_a; // 构造另一个向量
double dfVector_b_y = dfY_b - dfY_a;

if (dfVector_a_x*dfVector_b_x + dfVector_a_y*dfVector_b_y > 0) // 向量点乘
{
    dfDistWith_a *= -1.0F; // over
}

文章来源:https://blog.csdn.net/qq_42466012/article/details/134996325
本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。