вращение кватерниона и матрица вращения имеют небольшую разницу - PullRequest
Новая
       34

вращение кватерниона и матрица вращения имеют небольшую разницу

0 голосов
/ 05 сентября 2018

У меня есть трехмерное вращение во времени, представленное как мгновенное вращение вокруг каждой оси (крен, наклон, рыскание).

Я пытаюсь накапливать это вращение с течением времени (всего около 50 тыс. Измерений). Я пытался сделать это двумя разными способами. Использование матриц вращения и расчет кватернионов. Реализация матриц вращения, кажется, дает правильные результаты, но я знаю, что это менее рекомендуется для накопления многих вращений.

2 результата кажутся довольно схожими, но он накапливает небольшую разницу между 2 результатами с течением времени (около 1 градуса на каждые 250 измерений). Я не уверен, откуда эта разница. Вне зависимости от того, вызвано ли это точностью с плавающей запятой при вычислении умножения многих матриц, или использованием неверных параметров для инициализации кватерниона.

Это код, который я использую: 

# Last known rotation. As quaternion and as rotation matrix
last_rotation_matrix = ....
last_quaternion_rotation = .....

# time_diff_seconds is approximately 4/1000 
# the momentary rotation speed is around 0-1 radian per second. so [roll,pitch,yaw] are around 0-0.004)
roll = rotation_x_speed * time_diff_seconds
pitch = rotation_y_speed * time_diff_seconds
yaw = rotation_z_speed * time_diff_seconds
total_rotation = np.sqrt(roll**2 + pitch**2 + yaw**2)

# This function creates a rotation matrix based on the given parameters
diff_rotation_matrix = rotation_matrix(roll, pitch, yaw)
# THIS IS THE LINE THAT I SUSPECT:
diff_quaternion_rotation = Quaternion(axis=[rotation_x_speed, rotation_y_speed, rotation_z_speed], radians=total_rotation)

new_rotation_matrix = diff_quaternion_rotation.dot(last_rotation_matrix)
new_quaternion_rotation = diff_quaternion_rotation * last_rotation_matrix

Подозреваемая строка - это строка, инициализирующая переменную diff_quaternion_rotation.

1 Ответ

0 голосов
/ 05 сентября 2018

total_rotation = np.sqrt(roll**2 + pitch**2 + yaw**2)

Это неправильно - углы Эйлера не могут быть добавлены таким образом. Точно так же как и ваш осевой расчет.

Вместо этого существует явный алгоритм для преобразования углов Эйлера в кватернионы:

[1]

(если ваша пользовательская библиотека не имеет этой функции): 

cy, sy = cos(yaw * 0.5), sin(yaw * 0.5)
cr, sr = cos(roll * 0.5), sin(roll * 0.5)
cp, sp = cos(pitch * 0.5), sin(pitch * 0.5)

diff_quaternion_rotation = Quaternion(w = cy * cr * cp + sy * sr * sp,
                                      x = cy * sr * cp - sy * cr * sp,
                                      y = cy * cr * sp + sy * sr * cp,
                                      z = sy * cr * cp - cy * sr * sp)
...