неоднозначность при разложении шага (вращения по оси Y) из матрицы вращения

Мне дана поза робота 4x4, где ось z направлена ​​вперед, ось x обращена на восток (справа), а ось y направлена ​​вниз.

Теперь, чтобы извлечь заголовок робота, я использовал следующую функцию

void inline mat2xyh(Matrix4f& pose, float &x, float &y, float &heading){
    heading = atan2(-pose(2, 0),  sqrt(pose(2, 1) * pose(2, 1) +  pose(2,2) * pose(2,2)) );
    x = pose(0, 3);
    y = pose(2, 3); 
};

Для проверки я устанавливаю код ниже. Когда я исследовал угол разложенной матрицы и угол, который я использовал для построения матрицы вращения, они не совпадают!

Это было, когда я узнал, что есть 2 уникальных решения для высоты тона или что диапазон для высоты тона равен -PI/2 < pitch < PI/2.

Теперь я понятия не имею, что робот смотрит под углом 45 или 135 градусов. Есть ли способ обойти это?

    Vector4f v(0, 0, 1, 0);

    MatrixXf rotation = AngleAxisf(135 * M_PI/180., Vector3f::UnitY()).toRotationMatrix();
    float x,y,h;
    Matrix4f pose1 = Matrix4f::Identity();
    pose1.topLeftCorner<3,3>() = rotation;

    mat2xyh(pose1, x,y,h);

    cout << pose1 << endl;
    cout << rad2deg(h) << endl;
    cout << "Pose: \n" << pose1 * v << endl;

    cout << "====================" << endl;

    MatrixXf rotation2 = AngleAxisf(45 * M_PI/180., Vector3f::UnitY()).toRotationMatrix();
    Matrix4f pose2 = Matrix4f::Identity();
    pose2.topLeftCorner<3,3>() = rotation2;

    mat2xyh(pose2, x,y,h);

    cout << pose2 << endl;
    cout << rad2deg(h) << endl;
    cout << pose2 * v << endl;

Результаты

-0.707107         0  0.707107         0
        0         1         0         0
-0.707107         0 -0.707107         0
        0         0         0         1
heading: 45
Pose: 
 0.707107
        0
-0.707107
        0
====================
 0.707107         0  0.707107         0
        0         1         0         0
-0.707107         0  0.707107         0
        0         0         0         1
heading:45
Pose: 
0.707107
       0
0.707107
       0

eigen rotational-matrices linear-algebra
person avp    schedule 25.01.2019    source источник
comment
Можете ли вы предоставить более подробную информацию о вашей системе координат? В частности, это зафиксировано в теле робота или в мире? Вы используете непоследовательную терминологию, поэтому трудно сказать, соответствует ли XYZ EDN (Восток, Вниз, Север) или RDF (Вправо, Вниз, Вперед). Разница важна, когда робот вращается.   -  person Neil Forrester    schedule 25.01.2019
comment
Также вращение вокруг оси вниз (Y) — это рыскание, а не тангаж. Это может способствовать вашему замешательству.   -  person Neil Forrester    schedule 25.01.2019
comment
Привет, Нил, спасибо за ответ. Да, я действительно имел в виду рыскание. Я не знаком с терминологией, но позвольте мне уточнить. Набор данных, который я использую, взят от Китти, а система координат карты определяется как первая поза робота в момент времени t=0. Это помогает?   -  person avp    schedule 25.01.2019

Ответы (1)


arrow_upward
1
arrow_downward

Если второй аргумент atan2 всегда неотрицательный, вы получите только углы между -pi/2 и pi/2. Если вам нужны углы между -pi и pi, вы можете вычислить, например,

heading = atan2(pose(0, 2), pose(2,2));

Кстати: как @Neil уже сказал в комментариях, «заголовок» также известен как «рыскание». «Шаг» и «крен» также называются «отношением» и «креном».

Кроме того: нередко ось z указывает вверх (или вниз), а ось x вперед, особенно в областях, которые обычно перемещаются только по поверхности. А в компьютерном зрении ось Z направлена ​​вперед, а ось X — вправо. То есть действительно убедитесь, что вы знаете, какое соглашение использует ваш набор данных!

Чтобы узнать больше об углах Эйлера, я рекомендую прочитать их:

person chtz    schedule 25.01.2019
comment
чтц... пытаюсь уложить в голове то, что вы упомянули. Для заголовка = atan2 (поза (0, 2), поза (2,2)); работает ли это для всех сценариев, например, крен, тангаж и рыскание не равны нулю? - person avp; 25.01.2019
comment
Это всегда даст вам какой-то более или менее значимый результат. Если ваш крен или тангаж приблизится к 90°, вы, скорее всего, получите сингулярности. В этих случаях углы Эйлера - действительно плохое представление для вращений. хоть. По сути, вам нужно задать себе вопрос: чего вы ожидаете от «рысканья»? - person chtz; 25.01.2019
comment
Что вы подразумеваете под «значимым результатом»? Робот представляет собой автомобиль, движущийся по несколько плоской поверхности с небольшими качками и качками, поэтому маловероятно, что он столкнется с карданным замком? Не уверен, что вы имели в виду под тем, что я ожидаю от рыскания. По сути, мне дается поза робота 4x4 относительно карты, поэтому мне просто нужно получить довольно точную оценку того, куда движется робот. Это дает вам лучшее представление? - person avp; 25.01.2019
comment
Я не могу вам больше помочь, если вы не знаете, какой именно угол рыскания вы имеете в виду. Если ваш тангаж и крен составляют около 10°, вы можете получить ошибку около 1,5% в зависимости от того, какое соглашение вы используете, поэтому, вероятно, вам не следует слишком беспокоиться об этом. - person chtz; 25.01.2019
comment
Спасибо! Но могу ли я понять, какую информацию вы требуете, когда вы сказали, что я не знаю, о каком угле рыскания я имею в виду. Я знаю, что рыскание (это опечатка, когда я написал шаг, извините) Мне нужно вращение вокруг оси Y, и я понял, что если я использую другие соглашения, я могу получить другие результаты) - person avp; 25.01.2019