1

問題があります。アルゴリズムのアンダーフローの問題で立ち往生しています。私は基本的にベジエ曲線からパスを設計していますが、これに対処するには、2 つのベクトル間の角度と時計と反時計の方向を 1 から別のベクトルに取得するために、いくつかのベクトル乗算 (クロス積と内積) を使用する必要がありました。 . 問題は、パスが直線の場合、制御変数の 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

私はあなたが使用している方法に慣れていませんが、過去にこれを行ったときに、ベジエの縮退ケース (2 つの終点と 2 つの制御点が直線上にある) を次のように検出しました。特殊なケース。

もちろん、これは描画するのもはるかに高速です。

于 2012-06-22T07:40:00.633 に答える