Кватернионы и реальная механика, Вопрос синхронизации Опции

[alexPec]

Всем доброго дня. Неожиданно столкнулся с такой проблемой.

Есть механика, на рисунке. Внутренняя рамка С может поворачиваться вокруг осей Х и У относительно рамки А моторами М1 и М2. Вся система располагается в море, на корабле. Рамка А прикреплена жестко к кораблю. Задача - стабилизировать рамку С, так чтобы ее нормаль (g) совпадала с вектором гравитации.
Допустим, устанавливаю на рамку С гироскоп и акселерометр (трехосевые). При старте все хорошо - я точно знаю, как располагается подвижная система координат и точно знаю, что угол поворота вокруг оси Х я могу компенсировать мотором М1, а вокруг У- мотором М2. Гироскопы неизбежно плывут - с этим ничего не поделать. И если уплывания по осям Х и У можно компенсировать, зная вектор гравитации (измеряю акселерометром), то уплывание по оси g пока не вижу способа компенсировать. Да и компенсировать собственно не надо, проблема в том, что из-за этого уплывания со временем, когда уплывание достигнет например, 90 градусов, то мотором М1 я уже буду крутить кватернион вокруг оси Y, а мотором М1 - вокруг Х. Как-то бы привязать кватернион по оси g к рамке А.
Привязывать отдельно рамку А и отдельно кватернион к одной глобальной системе координат (например, по компасу) нет возможности.

Вопрос такой: как-то можно определить, каким мотором крутить, чтобы компенсировать появившийся крен рамки С? Т.е. крен -то я из кватерниона вытащу и разложу на составляющие по осям Х и У, а как определить, как сейчас расположены оси Х и У подвижной системы координат относительно осей моторов?

PS охота именно комплексно, через кватернионы решить задачу, а не отдельно по каждой оси мерить и компенсировать угловые скорости.

Эскизы прикрепленных изображений

 

[Rst7]

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

Еще раз повторюсь, заканчивайте с кватернионами. Рассматривайте физический смысл. Неважно, как Вы интегрируете данные IMU (кватернион, DCM - пофиг), у Вас должны быть две функции - одна - это пересчет вектора из системы координат IMU в звездную систему координат, и вторая - наоборот (если работать с DCM - то там все банально, сама по себе DCM и есть матрица пересчета из одной системы координат в другую, а пересчет в обратную сторону - это умножение на обратную матрицу). Вот, например, Вы измеряете вектор гравитации - этот вектор в системе координат IMU. Если Вы его пропустите через функцию перехода в звездную систему координат, то этот вектор будет представлять из себя вектор, у которого x, y равны 0, а z равен g. Когда x и y ненулевые - это и есть ошибка. Но можно точно так же переводить и не только вектора измерений, но и вектора ошибки. Т.е. если Вы возьмете вектор [cos(a-va),sin(a-va),0], где а - референсный курс, а va - курс по результатам интегрирования, то это будет вектор ошибки по курсу в звездной системе координат. Преобразовав этот вектор в систему координат IMU как раз и получатся ошибки по каждой из осей. Дальше PI-регулятор и добавка к данным с гироскопа. Все.

[alexPec]

Вы возьмете вектор [cos(a-va),sin(a-va),0], где а - референсный курс, а va - курс по результатам интегрирования, то это будет вектор ошибки по курсу в звездной системе координат. Преобразовав этот вектор в систему координат IMU как раз и получатся ошибки по каждой из осей. Дальше PI-регулятор и добавка к данным с гироскопа. Все.

Хорошо, рефренсный курс равен 0, va - курс по результатам интегрирования. Но повторюсь, если бы механика лежала на столе, это бы сработало, но сама механика на корабле тоже вращается, поэтому курс по результатам интегрирования - это сумма: поворот корабля + уплывание гироскопа. Как отсюда выделить уплывание чтобы вычислить собственно разницу между курсом механики и курсом по результатам интегрирования?

энкодеры на оси приделайте

Так они есть, что это дает?