В поисках лучшего метода дифференциации кватернионов
У меня есть кватернион (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, а затем я обновляю кватернион, просто добавляя этот дифференциальный кватернион.
Подходит ли этот метод для прогнозирования движения жестких объектов или есть лучший метод для прогнозирования кватерниона с угловой скоростью? Это работает, но я не получаю ожидаемых результатов.
3 ответа
Есть пара вещей, которые могут происходить. Вы не упоминаете о нормализации этого кватерниона. Плохие вещи обязательно произойдут, если вы этого не сделаете. Вы также не говорите, что умножаете дельта-кватернионные компоненты на количество времени, которое прошло 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 вращения.
Существует метод с очень хорошим компромиссом между скоростью и точностью, как увеличивать кватерниому, представляющую вращательное состояние (то есть интегрировать дифференциальное уравнение вращательного движения) с небольшим векторным приращением угла. 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 за малые углы и иногда можно пренебречь (в основном это просто перенормировать кватернион).
Простое преобразование из "экспоненциальной карты" в кватернион. (экспоненциальная карта, равная угловой скорости, умноженная на 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());
}