Question

我遇到了问题，我的算法遇到了一些下溢问题。我基本上是从Bezier曲线中划分出一条路径并处理这个问题我必须使用一些矢量乘法（交叉和点积）才能使两个矢量之间的角度和时钟 - 逆时针方向从一个到另一个。问题是当路径是一条直线时，其中一个控制变量存在下溢问题，基本上阻止了执行并导致错误。

以下是代码：

void BezierInterp() {
    NumOfSetpoints = 10;
    float seqTH[11];
    float orient[10];
    float divider;
    math.MatrixMult((float*) BCoeff, (float*) waypointX, 11, 4, 1,
            (float*) setpoint0);
    math.MatrixMult((float*) BCoeff, (float*) waypointY, 11, 4, 1,
            (float*) setpoint1);
    float dx1, dy1, dx2, dy2, dxy1, dxy2, dir;
    dx1 = cos(state[2]);
    dy1 = sin(state[2]);
    dx2 = setpoint0[1] - setpoint0[0];
    dy2 = setpoint1[1] - setpoint1[0];
    dxy2 = sqrt(sq(dx2) + sq(dy2));
    dir = dx1 * dy2 - dx2 * dy1;
    if (dxy2<0.0001 && dxy2>-0.0001) {
        seqTH[0] = 0.0;
    }
    else{
    if (dir >= 0) {
        seqTH[0] = acos((dx1 * dx2 + dy1 * dy2) / (dxy2));
    } else {
        seqTH[0] = -acos((dx1 * dx2 + dy1 * dy2) / (dxy2));
    }}
    for (uint8_t i = 1; i <= 9; i = i + 1) {
        dx2 = setpoint0[i + 1] - setpoint0[i];
        dy2 = setpoint1[i + 1] - setpoint1[i];
        dxy2 = sqrt(sq(dx2) + sq(dy2));
        dx1 = setpoint0[i] - setpoint0[i - 1];
        dy1 = setpoint1[i] - setpoint1[i - 1];
        dxy1 = sqrt(sq(dx1) + sq(dy1));
        dir = dx1 * dy2 - dx2 * dy1;
        divider= dxy1 * dxy2;
        if (divider<0.0001 && divider>-0.0001) {
                seqTH[0] = 0.0;
            }
        else {
        if (dir >= 0) {
            seqTH[i] = acos((dx1 * dx2 + dy1 * dy2) / (divider));
        } else {
            seqTH[i] = -acos((dx1 * dx2 + dy1 * dy2) / (divider));
        }}
    }
    print_array("seqTh", seqTH, 11, 6);
    orient[0] = state[2] + seqTH[0];
    if (orient[0]<0.0001 && orient[0]>-0.0001){orient[0]=0.0001;}
    for (uint8_t i = 1; i <= 9; i = i + 1) {
        orient[i] = orient[i - 1] + seqTH[i];
        if (orient[i]<0.0001 && orient[i]>-0.0001){orient[i]=0.0001;}
    }
    print_array("orient", orient, 10, 6);
    for (uint8_t i = 1; i <= 9; i = i + 1) {
        setpoint2[i] = orient[i - 1];
        setpoint3[i] = Vref * cos(orient[i - 1]);
        setpoint4[i] = Vref * sin(orient[i - 1]);

    }
    setpoint2[10] = orient[9];
    setpoint3[10] = 0;
    setpoint4[10] = 0;
    setpoint5[10] = 0;
    }
}

正如您在尝试避免错误时所看到的那样，我放了几个if条件，但还不够。实际上问题可能来自dir=dx1 * dy2 - dx2 * dy1;。当沿x或y轴移动时太小而不能浮动。一位朋友建议使用布尔值，但我不确定如何。也许定义boolean dir;然后如果值太小将是0否则将被视为1，在这种情况下我可以使用我现在用于检测方向的相同过程。

您有任何建议或可能有其他解决方案吗？提前致谢奈德

Answer 1

我不熟悉你正在使用的方法，但是当我在过去做过这个时，我已经检测到Bezier的退化情况（两个终点和两个控制点落在直线上）线）作为一个特例。

当然，绘制速度也快得多。

在C“if”条件下处理下溢 - 使用布尔值

1 个答案: