10

それほど難しいことではないと思いますが、しばらくはそれにこだわっています。

両方向に回転できる関節があります。センサーは、-piと+piの範囲の関節の角度を教えてくれます。

-無限大と+無限大の範囲で変換したいと思います。つまり、たとえばジョイントが時計回りに永久に回転する場合、角度は0から始まり、その後無限大に増加します。matlabでは、unwrap関数はそれを非常にうまく行います。

newAngle = unwrap([previousAngle newAngle]);
previousAngle = newAngle;

注：角度が大きくジャンプしないことを前提としています。確かにPIより優れているものはありません。

注：私は尋ねる前に本当に一生懸命に見えました...

ありがとう ！

4

3 に答える 3

4

いくつかの作業の後、これを思いついた。正常に動作しているようです。

//Normalize to [-180,180):
inline double constrainAngle(double x){
    x = fmod(x + M_PI,M_2PI);
    if (x < 0)
        x += M_2PI;
    return x - M_PI;
}
// convert to [-360,360]
inline double angleConv(double angle){
    return fmod(constrainAngle(angle),M_2PI);
}
inline double angleDiff(double a,double b){
    double dif = fmod(b - a + M_PI,M_2PI);
    if (dif < 0)
        dif += M_2PI;
    return dif - M_PI;
}
inline double unwrap(double previousAngle,double newAngle){
    return previousAngle - angleDiff(newAngle,angleConv(previousAngle));
}

私はこの投稿のコードを使用しました： C++コードでのAngleWrapの処理

于 2013-03-27T03:32:18.147 に答える
4

次の関数は、入力角度間の絶対差が2*pi未満であると想定して機能します。

float unwrap(float previous_angle, float new_angle) {
    float d = new_angle - previous_angle;
    d = d > M_PI ? d - 2 * M_PI : (d < -M_PI ? d + 2 * M_PI : d);
    return previous_angle + d;
}

配列のラップを解除する必要がある場合は、次のルーチンを使用できます。

void unwrap_array(float *in, float *out, int len) {
    out[0] = in[0];
    for (int i = 1; i < len; i++) {
        float d = in[i] - in[i-1];
        d = d > M_PI ? d - 2 * M_PI : (d < -M_PI ? d + 2 * M_PI : d);
        out[i] = out[i-1] + d;
    }
}
于 2016-03-18T22:52:26.470 に答える
3 
// wrap to [-pi,pi]
inline double angle_norm(double x)
{
    x = fmod(x + M_PI, M_2PI);
    if (x < 0)
        x += M_2PI;
    return x - M_PI;
}

double phase_unwrap(double prev, double now)
{
    return prev + angle_norm(now - prev);
}

これは機能します。

于 2016-02-29T11:13:08.520 に答える