1

我有一个问题，我的算法遇到了一些下溢问题。我基本上是从贝塞尔曲线中寻找一条路径，为了解决这个问题，我必须使用一些向量乘法（叉积和点积），以使两个向量之间的角度和时钟计数器时钟方向从一个到另一个. 问题是当路径是直线时，其中一个控制变量有下溢问题，基本上阻塞执行并导致错误。

这是代码：

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，在这种情况下，我可以使用我现在使用的相同程序来检测方向。

您有什么建议或不同的解决方案吗？在此先感谢内德

4

1 回答 1

0

我不熟悉您使用的方法，但是当我过去这样做时，我检测到贝塞尔曲线的退化情况（两个端点和两个控制点落在一条直线上）为一个特例。

当然，这也更快地绘制。

于 2012-06-22T07:40:00.633 回答