Ищу лучший способ сделать Quaternion Differiaton

Question

У меня есть кватернион (4x1) и вектор угловой скорости (3x1), и я называю функцией для вычисления дифференциального кватерниона, как объяснено в этом веб -. Анкет Код выглядит так:

    float wx = w.at<float>(0);
float wy = w.at<float>(1);
float wz = w.at<float>(2);
float qw = q.at<float>(3); //scalar component 
float qx = q.at<float>(0);
float qy = q.at<float>(1);
float qz = q.at<float>(2);

q.at<float>(0) = 0.5f * (wx*qw + wy*qz - wz*qy);    // qdiffx
q.at<float>(1) = 0.5f * (wy*qw + wz*qx - wx*qz);    // qdiffy
q.at<float>(2) = 0.5f * (wz*qw + wx*qy - wy*qx);    // qdiffz
q.at<float>(3) = -0.5f * (wx*qx + wy*qy + wz*qz);   // qdiffw

Итак, теперь у меня есть дифференциальный кватернион, хранящийся в Q, а затем я обновляю кватернион Просто добавление это дифференциальный кватернион.

Подходит ли этот метод для прогнозирования движения жестких объектов или есть лучший метод для прогнозирования кватерниона с угловой скоростью? Это работает, но я не получаю ожидаемых результатов.

Answer 1

Есть пара вещей, которые могут происходить. Вы не упоминаете, что повторно нормализует этот кватернион. Плохие вещи определенно произойдут, если вы этого не делаете. Вы также не говорите, что размножаете компоненты дельта-кварталов на количество времени, которое прошло dt Прежде чем добавить их в оригинальный кватернион. Если ваша угловая скорость находится в радианах в секунду, но вы выходите вперед только на долю секунды, вы зайдете слишком далеко. Тем не менее, даже так, поскольку вы переходите через дискретное количество времени и пытаетесь притворяться, что это бесконечно максимально, странные вещи произойдут, особенно если ваш таймп или угловая скорость велики.

Физический двигатель, ODE, предоставляет возможность обновить вращение тела с его угловой скорости, как будто он сделал бесконечно малый шаг или обновление, используя шаг конечного размера. Конечный шаг гораздо более точен, но включает в себя немного тригера. функции, как и немного медленнее. Соответствующий исходный код ODE можно увидеть Здесь строки 300-321, с кодом поиска дельта-квадрата Здесь строка 310.

float wMag = sqrt(wx*wx + wy*wy + wz*wz);
float theta = 0.5f*wMag*dt;
q[0] = cos(theta);  // Scalar component
float s = sinc(theta)*0.5f*dt;
q[1] = wx * s; 
q[2] = wy * s;
q[3] = wz * s;

Где sinc(x) является:

if (fabs(x) < 1.0e-4) return (1.0) - x*x*(0.166666666666666666667);
else return sin(x)/x;

Это позволяет вам избежать проблемы разрыва за нулем и все еще очень точное.

Кватернион q затем предварительно подходит к существующему кватернионному представлению ориентации организма. Затем повторно нормализуйте.

---

Редактировать-где эта формула происходит:

Рассмотрим начальный кватернион q0 и окончательный кватернион q1 что результат после вращения с угловой скоростью w за dt количество времени. Все, что мы делаем здесь, это превращает вектор угловой скорости в кватернион, а затем вращаю первую ориентацию этим кватернионом. Как кватернины, так и угловые скорости являются изменениями на представлении оси. Тело, которое вращается от его канонической ориентации theta вокруг единичной оси [x,y,z] будет иметь следующее представление кватерниона его ориентации: q0 = [cos(theta/2) sin(theta/2)x sin(theta/2)y sin(theta/2)z]Анкет Тело, которое вращение theta/s вокруг единичной оси [x,y,z] будет иметь угловую скорость w=[theta*x theta*y theta*z]. Анкет Итак, чтобы решить, сколько вращения произойдет dt Секунды, мы сначала извлекаем величину угловой скорости: theta/s = sqrt(w[0]^2 + w[1]^2 + w[2]^2). Анкет Затем мы находим фактический угол, умножившись на dt (и одновременно разделите на 2 для удобства превращения этого в кватернион). Поскольку нам нужно нормализовать ось [x y z], мы также делимся на theta. Анкет Вот где sinc(theta) Часть приходит от. (поскольку theta имеет дополнительное 0.5*dt В нем - величина, мы умножаем это обратно). А sinc(x) Функция просто использует аппроксимацию серии Тейлор, когда x маленький, потому что это численно стабильно и точнее сделать это. Способность использовать эту удобную функцию - это то, почему мы не просто разделяли на фактическую величину wMag. Анкет Тела, которые не очень быстро вращаются, будут иметь очень маленькие угловые скорости. Поскольку мы ожидаем, что это будет довольно распространено, нам нужно стабильное решение. Мы заканчиваем кватернион, который представляет собой шаг на один шаг dt ротации.

Answer 2

Есть метод с очень хорошим компромиссом между скорость и точность как Увеличить кватерниому, представляющую вращательное состояние (IE интегрирует дифференциальное уравнение вращательного движения) с помощью небольшого приращения угла вектора dphi (которая является векторной угловой скоростью omega Mulptipliad по временному шагу dt ).

А Точный (и медленный) метод вращения кватерниона вектором:

void rotate_quaternion_by_vector_vec ( double [] dphi, double [] q ) {
  double x = dphi[0];
  double y = dphi[1];
  double z = dphi[2];

  double r2    = x*x + y*y + z*z;
  double norm = Math.sqrt( r2 );

  double halfAngle = norm * 0.5d;
  double sa = Math.sin( halfAngle )/norm; // we normalize it here to save multiplications
  double ca = Math.cos( halfAngle );
  x*=sa; y*=sa; z*=sa;  

  double qx = q[0];
  double qy = q[1];
  double qz = q[2];
  double qw = q[3];

  q[0] =  x*qw + y*qz - z*qy + ca*qx;
  q[1] = -x*qz + y*qw + z*qx + ca*qy;
  q[2] =  x*qy - y*qx + z*qw + ca*qz;
  q[3] = -x*qx - y*qy - z*qz + ca*qw;
}

Проблема в том, что вы должны вычислить медленные функции, такие как cos, sin, sqrt. Анкет Вместо этого вы можете получить значительный прирост скорости и разумную точность для небольших углов (что имеет место, если временный шаг вашего моделирования разумный маленький) путем аппроксимирования sin а также cos по расширению Тейлора, выраженное просто с использованием norm^2 вместо norm.

Как это Быстрый метод вращения кватерниона вектором:

void rotate_quaternion_by_vector_Fast ( double [] dphi, double [] q ) {
  double x = dphi[0];
  double y = dphi[1];
  double z = dphi[2];

  double r2    = x*x + y*y + z*z;

  // derived from second order taylor expansion
  // often this is accuracy is sufficient
  final double c3 = 1.0d/(6 * 2*2*2 )      ; // evaulated in compile time
  final double c2 = 1.0d/(2 * 2*2)         ; // evaulated in compile time
  double sa =    0.5d - c3*r2              ; 
  double ca =    1    - c2*r2              ; 

  x*=sa;
  y*=sa;
  z*=sa;

  double qx = q[0];
  double qy = q[1];
  double qz = q[2];
  double qw = q[3];

  q[0] =  x*qw + y*qz - z*qy + ca*qx;
  q[1] = -x*qz + y*qw + z*qx + ca*qy;
  q[2] =  x*qy - y*qx + z*qw + ca*qz;
  q[3] = -x*qx - y*qy - z*qz + ca*qw;

}

Вы можете повысить точность, выполнив половину угла, что еще на 5 размножений:

  final double c3 = 1.0d/( 6.0 *4*4*4  ) ; // evaulated in compile time
  final double c2 = 1.0d/( 2.0 *4*4    ) ; // evaulated in compile time
  double sa_ =    0.25d - c3*r2          ;  
  double ca_ =    1     - c2*r2          ;  
  double ca  = ca_*ca_ - sa_*sa_*r2      ;
  double sa  = 2*ca_*sa_                 ;

Или еще более точное под другим углом расщепления до половины:

  final double c3 = 1.0d/( 6 *8*8*8 ); // evaulated in compile time
  final double c2 = 1.0d/( 2 *8*8   ); // evaulated in compile time
  double sa = (  0.125d - c3*r2 )      ;
  double ca =    1      - c2*r2        ;
  double ca_ = ca*ca - sa*sa*r2;
  double sa_ = 2*ca*sa;
         ca = ca_*ca_ - sa_*sa_*r2;
         sa = 2*ca_*sa_;

---

ПРИМЕЧАНИЕ: Если вы используете более сложную схему интеграции, чем просто Verlet (например, Runge-Kutta), вы вероятно, понадобится дифференциал кватерниона, а не новый (обновленный) кватернион.

Это можно увидеть в коде здесь

  q[0] =  x*qw + y*qz - z*qy + ca*qx;
  q[1] = -x*qz + y*qw + z*qx + ca*qy;
  q[2] =  x*qy - y*qx + z*qw + ca*qz;
  q[3] = -x*qx - y*qy - z*qz + ca*qw;

это можно интерпретировать как умножение старого (не обновленного) кватерниона ca (косинус половины углового), которая является приблизительным ca ~ 1 Для маленьких углов и добавления остальных (некоторые перекрестные взаимодействия). Итак, дифференциал просто:

  dq[0] =  x*qw + y*qz - z*qy + (1-ca)*qx;
  dq[1] = -x*qz + y*qw + z*qx + (1-ca)*qy;
  dq[2] =  x*qy - y*qx + z*qw + (1-ca)*qz;
  dq[3] = -x*qx - y*qy - z*qz + (1-ca)*qw;

где термин ( 1 - ca ) ~ 0 под небольшими углами и иногда их можно пренебрегать (в основном это просто перенормизирует Quternion).

Answer 3

Простое преобразование из «экспоненциальной карты» в кватернион. (Экспоненциальная карта, равная угловой скорости, умноженная на Deltatime). Результат Кватернион - это дельта -вращение для пройденной Deltatime и угловой скорости "W".

Vector3 em = w*deltaTime; // exponential map
{
Quaternion q;
Vector3 ha = em/2.0; // vector of half angle

double l = ha.norm();
if(l > 0)
{
    double ss = sin(l)/l;
    q = Quaternion(cos(l), ha.x()*ss, ha.y()*ss, ha.z()*ss);
}else{
    // if l is too small, its norm can be equal 0 but norm_inf greater 0
    q = Quaternion(1.0, ha.x(), ha.y(), ha.z());
}