В поисках лучшего метода дифференциации кватернионов

Question

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

    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 предварительно умножается на существующее кватернионное представление ориентации тела. Затем повторно нормализуйте.

---

Edit - откуда взялась эта формула:

Рассмотрим начальный кватернион 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

Простое преобразование из "экспоненциальной карты" в кватернион.(экспоненциальная карта, равная угловой скорости, умноженная на 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());
}

Answer 3

Существует метод с очень хорошим компромиссом между скоростью и точностью как увеличить кватерниому, представляющему вращательное состояние (то есть интегрировать дифференциальное уравнение вращательного движения) с небольшим векторным приращением угла dphi (который является векторной угловой скоростью omega мультиплиады по временному шагу 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_;

---

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

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

  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 для малых углов и может иногда игнорироваться (в основном это просто перенормировка кватерниона).