C++ 中的连续角度（matlab 中的 eq unwrap 函数）

Question

我想这并不难，但我已经坚持了一段时间了。

我有一个可以双向旋转的关节。传感器为我提供 -pi 和 +pi 范围内的关节角度。

我想在-无穷大和+无穷大范围内转换它。这意味着，如果关节永远顺时针旋转，角度将从 0 开始，然后增加到无穷大。 在 matlab 中，unwrap 函数可以很好地做到这一点：

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

注意：假设角度不会发生大的跳跃，肯定不会优于 PI。

注：我问之前真的很认真的看...

谢谢！

Answer 1

以下函数可以完成这项工作，假设输入角度之间的绝对差小于 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;
    }
}

Answer 2

经过一些工作，想出了这个。看起来工作正常。

//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++ 代码中处理角度包裹

Answer 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);
}

这有效。

Answer 4

没有一个起作用。必须实施它。

#include <iostream>
#include <iomanip>
#include <numbers>
#include <vector>
#include <cmath>

// Normalize to [0,2PI):
double phaseNorm(double x)
{
    x = fmod(x, 2*std::numbers::pi);
    if (x < 0)
        x += 2*std::numbers::pi;
    return x;
};

// unwrap phase [-PI,PI]
std::vector<double> phaseUnwrap(std::vector<double> in)
{
    // Normalize to [0,2PI):
    std::transform(in.begin(),in.end(),in.begin(),[&](double d){ return phaseNorm(d); });

    // unwrap iteration
    for(size_t i = 0; i < in.size()-1; ++i)
    {
        int n2PiJump = in[i] / (2*std::numbers::pi);
        in[i+1] += n2PiJump * 2*std::numbers::pi;
        if(in[i]-in[i+1] > std::numbers::pi)
        {
            in[i+1] += 2*std::numbers::pi;
        }
    }
    return in;
}

int main() {

    // create phase vector
    int n = 3;
    std::vector<double> phase;
    for(int h = 0; h < 3; ++h)
    {
        for(int i = n; i > -n; --i)
        {
            phase.push_back(-i*std::numbers::pi/n);
        }
    }

    // print phase vector
    std::cout << std::setw(25) << "Input vector: ";
    for(auto& p : phase)
    {
        std::cout << std::setw(8) << p << " ";
    }
    std::cout << std::endl;

    //  normalize phase vector
    std::cout << std::setw(25) << "Normalized vector: ";
    for(auto& p : phase)
    {
        p = phaseNorm(p);
        std::cout << std::setw(8) << p << " ";
    }
    std::cout << std::endl;

    // unwrap phase vector
    std::cout << std::setw(25) << "Unwraped norm. vector: ";
    std::vector<double> phaseUnwraped = phaseUnwrap(phase);
    for(auto& p : phaseUnwraped)
    {
        std::cout << std::setw(8) << p << " ";
    }    
    std::cout << std::endl;

    return 0;
}

           Input vector: -3.14159  -2.0944  -1.0472        0   1.0472   2.0944 -3.14159  -2.0944  -1.0472        0   1.0472   2.0944 -3.14159  -2.0944  -1.0472        0   1.0472   2.0944 
      Normalized vector:  3.14159  4.18879  5.23599        0   1.0472   2.0944  3.14159  4.18879  5.23599        0   1.0472   2.0944  3.14159  4.18879  5.23599        0   1.0472   2.0944 
  Unwraped norm. vector:  3.14159  4.18879  5.23599  6.28319  7.33038  8.37758  9.42478   10.472  11.5192  12.5664  13.6136  14.6608   15.708  16.7552  17.8024  18.8496  19.8968   20.944

要编译，请不要忘记 c++20 标志

-std=c++20

，因为

std::numbers::pi

或使用

M_PI

中包含的

<math.h>

。