УСТРОЙСТВО И СПОСОБ КАЛИБРОВКИ ГИРОДАТЧИКОВ
DEVICE AND METHOD OF CALIBRATING GYRO SENSORS

Родственные заявки
[1] Данная заявка испрашивает приоритет предварительной заявки США № 61/456467, поданной 8 ноября 2010 г., содержание которой, таким образом, включено посредством ссылки в полном объеме.
Область техники
[2] Настоящее раскрытие относится к устройствам, включающим в себя магнитные датчики, и, в частности, к системам, включающим в себя инерциальные гироскопические датчики.
Уровень техники
[3] Недорогие инерциальные и другие датчики получили широкое распространение в многочисленных бытовых электронных устройствах, например смартфонах, игровых контроллерах и пр. Это предъявляет очень жесткие требования к разработчикам. Чем дешевле изделия, тем ниже предполагаемый уровень технической подготовленности их потребителя, и изделие должно отличаться повышенной надежностью и не требовать ухода, даже если в нем используются недорогие датчики низкой точности.
[4] При изготовлении устройства принято калибровать все датчики, применяемые в устройстве, во время изготовления устройства до его отгрузки потребителю. Эта, так называемая заводская калибровка, может осуществляться очень точно с использованием специального оборудования, контроля окружающей среды и опытного персонала. Однако при использовании изделия потребителем калибровка датчика сбивается вследствие изменения температуры, другого напряжения батареи и прочих эффектов, которые не поддаются контролю инженеров-конструкторов.
[5] Этот вопрос правильной калибровки особенно важен для датчиков, которые, чтобы их можно было использовать в решении навигации или определения положения, необходимо интегрировать по времени. При интегрировании ошибки калибровки растут со временем, вследствие чего полученное решение очень быстро становится неприемлемым.
[6] Это означает, что, чтобы гарантировать правильную работу устройства, для каждого датчика, используемого в устройстве, требуются надлежащие sw и алгоритмы, допускающие автономную калибровку в режиме реального времени таких эффектов, как дрейф сигнала и изменение эффективного коэффициента усиления, которые случаются в ходе нормальной работы устройства вследствие вышеперечисленных воздействий окружающей среды. Другими словами, практическая конструкция должна включать в себя внутренние алгоритмы, которые самостоятельно отслеживают и регулируют параметры калибровки датчика в реальном времени.
[7] Это также означает, что требуются способы, позволяющие ослабить эффект интегральной ошибки датчика.
[8] Таким образом, задачей настоящего раскрытия является разрешение одной или более из вышеупомянутых трудностей за счет обеспечения способа, который автоматически регулирует параметры калибровки гиродатчиков в реальном времени в ходе нормальной работы устройства.
[9] Задачей этого раскрытия также является обеспечение способа, который позволяет снизить ошибку при интегрировании сигнала гиродатчика.
[10] Типичная система в настоящее время имеет полный набор датчиков, например 3-осный акселерометр, 3-осный магнитный датчик и 3-осный гиродатчик. Все эти датчики нуждаются в калибровке, чтобы их можно было эффективно использовать. Наиболее распространенный способ калибровки акселерометра предусматривает использование моментов времени, когда система покоится, и использование вектора силы тяжести в качестве естественного стандарта для определения смещения и коэффициента усиления акселерометра. Эти состояния покоя могут быть заданы пользователем в ходе пользовательской калибровки или найдены в ходе работы в реальном времени, например, с помощью “детектора нулевого движения”, как в патенте США 5991692, поскольку в состоянии покоя только вектор силы тяжести влияет на показания акселерометра. Полная калибровка акселерометра может осуществляться с использованием множественных измерений при различных ориентациях устройства. В качестве примера см. патент США 6729176.
[11] Магнитные датчики обычно калибруются с использованием естественного магнитного поля Земли. Калибровку можно производить, регистрируя измерения в нескольких заранее определенных ориентациях устройства, или как фоновый процесс с использованием 3D движения операционного или пользовательского калибровочного устройства, например, опубликованная патентная заявка США № 2009/0070056. Такая процедура создает новые параметры калибровки каждый раз, когда осуществляется калибровка.
[12] Поскольку датчик GPS получил широкое распространение в навигационных системах, где применяются магнитные датчики, его часто используют для калибровки магнитных датчиков путем сравнения направления вектора скорости GPS с направлением, полученным от магнитного датчика. Затем используется прямое вычисление процедуры фильтра Калмана для получения необходимых параметров калибровки для магнитных датчиков.
[13] Это значительный объем работы, который позволяет специалисту в данной области техники получить надлежащую процедуру для точной калибровки магнитных и акселерометрических датчиков, используемых в устройстве, с использованием естественных и постоянно действующих сил земного тяготения и магнитного поля Земли, которые хорошо известны и табулировны с высокой точностью в любом месте на Земле, и с использованием таких дополнительных датчиков, как GPS и температура.
[14] Как известно любому специалисту в данной области техники, при получении ориентации устройства из измерений хорошо откалиброванных магнитных и гравитационных датчиков, каждое определение положения является независимым. Таким образом, неизбежные ошибки в определении направлений магнитного и гравитационного полей не накапливаются с течением времени. Напротив, сглаживая результирующую траекторию, можно уменьшить ошибку ориентации в каждой точке траектории.
[15] Однако в ситуациях, когда устройство испытывает неизвестное ускорение, внутренние акселерометры нельзя использовать для определения ориентации устройства относительно вектора гравитационного поля Земли. Одного лишь вектора магнитного поля недостаточно для восстановления ориентации устройства, так же, как одну антенну GPS нельзя использовать для восстановления ориентации тела. В подобных случаях ничего не остается делать, как применять гиродатчики для определения 3D ориентации устройства.
[16] Гиродатчики измеряют частоту вращения вокруг своей локальной координатной оси. При правильном интегрировании это обеспечивает полное 3D вращение тела относительно его начальной ориентации. Однако, поскольку сигнал гиродатчика нужно интегрировать для получения ориентации, любая ошибка в смещении или коэффициенте усиления гиродатчика быстро растет со временем.
[17] Смещение гиродатчика можно определить, наблюдая сигналы гиродатчика в момент, когда вращение отсутствует, предполагая, что такие моменты можно обнаружить. Однако для калибровки коэффициента усиления гиродатчика необходимо осуществлять очень точное и хорошо известное вращение вокруг каждой оси, что трудно осуществить даже в заводских условиях и просто невозможно для калибровки в режиме реального времени в условиях эксплуатации.
[18] Tekawy et al., US 7667645, US 7393422, предусматривает использование разности мощностей от разных спутников GPS на множественных антеннах GPS для калибровки дрейфа гиродатчика.
[19] Drag et al., US 7657183, использует оптические датчики как основной способ ориентирования. В этом патенте упомянуто, что гиродатчик можно калибровать с использованием этих оптических сигналов, но не объясняется, как это сделать.
[20] Achkar et al., US 5562266, также предлагает использовать детекторы северного и южного полюсов для оценивания дрейфа (смещения) гиродатчика посредством фильтра Калмана, что также аналогично способу, предложенному Basuthakur et al., US 5452869.
[21] Таким образом, в настоящее время способы калибровки коэффициента усиления должны опираться на внешнее определение ориентации траектории устройства, обычно GPS для транспортного средства, звездоискателя для спутников или сонара для подводного аппарата. Однако такие способы не работают, когда устройство испытывает вращение, независимое от его траектории, что нередко происходит с портативными устройствами.
[22] Задачей настоящего раскрытия является обеспечение практичного и эффективного способа, который автоматически регулирует параметры калибровки гиродатчиков в реальном времени в ходе нормальной работы устройства. Задачей этого раскрытия также является обеспечение практичного и эффективного способа повышения точности определения ориентации устройства.
Сущность изобретения
[23] Настоящее раскрытие инструктирует специалиста в данной области техники, как осуществлять надлежащие вычисления в реальном времени, которые применяют измерения магнитных датчиков, прежде чем калибровать их любыми способами, которые известны специалистам в данной области техники, и интегрирование гиродатчиков для определения поправки к коэффициенту усиления гиродатчиков.
[24] Это раскрытие предусматривает, что, если гиродатчики правильно откалиброваны, то вращение устройства, вычисленное путем надлежащего интегрирования сигналов гиродатчика, должно поворачивать компоненты вектора магнитного поля из их ориентации в момент начала интегрирования в их наблюдаемую ориентацию в момент окончания интегрирования сигнала гиродатчика. Тогда любую меру рассогласования между наблюдаемым и повернутым векторами магнитного поля можно использовать для улучшения калибровки гиродатчиков.
[25] Когда такая мера близости установлена, настоящее раскрытие утверждает, что, чем больше ошибка калибровки гиродатчиков, тем больше становится эта мера рассогласования, и что минимум этой меры соответствует наилучшей калибровке гиродатчиков. Таким образом, при построении зависимости между применяемыми параметрами гиродатчика и ошибкой между измеренной и ожидаемой ориентацией вектора магнитного поля, верное значение параметров калибровки гиродатчиков будет давать минимум к этой поверхности.
[26] Это раскрытие также указывает специалистам в данной области техники, что ориентацию устройства, вычисленную путем интегрирования сигнала гиродатчика, можно улучшить, убедившись в том, что вычисленное вращение согласуется с наблюдаемой ориентацией вектора магнитного поля.
[27] Согласно одному варианту осуществления настоящего раскрытия устройство содержит микропроцессор, имеющий соответствующую память; по меньшей мере, два набора независимых датчиков в устройстве, способных регистрировать 3D ориентацию и/или 3D движение устройства, где, по меньшей мере, один набор датчиков неудовлетворительно откалиброван в начале движения устройства; и программу микропроцессора, выполняющуюся на микропроцессоре. Эта программа выполняет этапы a) считывания, сохранения и обработки данных датчика от датчиков в последовательные моменты времени t=0, 1,..., k; b) оценивания 3D ориентации устройства в момент времени t=k с использованием данных датчика на момент времени t=0, 1,..., k; c) прогнозирования значения одного набора вышеупомянутых датчиков в следующий момент времени t=(k+1) с использованием измерений датчика в предыдущие моменты времени: t=m,...,k 0<=m<=k; d) вычисления разности между прогнозируемым и наблюдаемым значениями датчика; и e) использования разности для непрерывного улучшения определения оцененной выше 3D ориентации устройства и/или для калибровки набора датчиков, которые были неудовлетворительно откалиброваны в начале движения устройства.
[28] В одном варианте осуществления устройства наборы датчиков представляют собой набор магнитных датчиков и набор гиродатчиков. В одном варианте осуществления набор гиродатчиков неудовлетворительно откалиброван в начале движения устройства.
[29] В одном варианте осуществления способ калибровки набора гиродатчиков содержит: наблюдение сигналов гиродатчика в течение заранее определенного периода времени; корректировку сигналов гиродатчика путем применения одного из первого предположения относительно неизвестных параметров калибровки гиродатчиков или их номинальных значений; определение воспринимаемого изменения ориентации устройства в течение этого периода времени на основании наблюдаемых и таких скорректированных сигналов гиродатчика; сравнение значений магнитного датчика в начале и конце периода интегрирования, надлежащим образом скорректированных выведенным изменением ориентации устройства; и корректировку начальных значений параметров калибровки гиродатчиков путем применения поправки, выведенной из меры разности между наблюдаемым и номинальным сигналами магнитного датчика с поправкой на вращение.
[30] В одном примерном варианте осуществления устройства для всех каналов гиродатчика определяется только один параметр калибровки. Параметром калибровки может быть неизвестная поправка коэффициента усиления гиродатчика к номинальным значениям. В одном варианте осуществления параметром калибровки может быть неизвестная поправка смещения гиродатчика к номинальным значениям.
[31] В другом варианте осуществления можно определять множественные параметры калибровки. В таком варианте осуществления параметрами калибровки могут быть неизвестные поправки коэффициента усиления для каждого из гиродатчиков. Альтернативно параметрами калибровки могут быть неизвестные поправки смещения для каждого из гиродатчиков. В другом варианте осуществления параметры калибровки представляют собой комбинацию неизвестных поправок смещения и неизвестных поправок коэффициента усиления для всех гиродатчиков.
[32] В другом варианте осуществления мерой разности между сигналом магнитного датчика в конце периода времени и скорректированным на вращение значением вектора магнитного поля в начале периода времени является векторная норма векторной разности этих двух векторов. Согласно варианту осуществления в течение одного и того же интервала времени множественные альтернативные вращения можно определять с разными предполагаемыми значениями параметров гиродатчика при калибровке. Альтернативно в разные интервалы времени разные значения параметров калибровки гиродатчиков можно использовать для определения вращения. В подобных случаях значения параметра поправки можно выбирать способом оптимальной экспериментальной конструкции.
[33] В одном варианте осуществления параметры калибровки можно определять путем нахождения такой комбинации параметров, которая обеспечивает минимум вычисленной меры разности между наблюдаемым и скорректированным на вращение сигналами магнитного датчика. Альтернативно все вычисления могут производиться автономно с помощью данных, которые были зарегистрированы для одного набора параметров калибровки и затем повторно вычислены с искусственным изменением в параметрах калибровки.
[34] В одном варианте осуществления способ определения вращения устройства в соответствии с настоящим раскрытием включает в себя наблюдение сигналов гиродатчика в течение заранее определенного периода времени; корректировку сигналов гиродатчика путем применения параметров калибровки гиродатчиков; определение воспринимаемого изменения ориентации устройства в течение этого периода времени на основании наблюдаемых и таких скорректированных сигналов гиродатчика. Способ дополнительно включает в себя наблюдение сигналов магнитного датчика в начале и конце этого периода времени; корректировку сигнала магнитного датчика путем применения параметров калибровки магнитного датчика; определение воспринимаемого изменения ориентации устройства в течение этого времени, которое согласуется с наблюдаемым изменением сигналов магнитного датчика, скорректированных посредством калибровки, и является ближайшим к вращению, определенному из сигналов гиродатчика; и определение вращения устройства в течение этого интервала времени как комбинации вращения, полученного от гиродатчиков, и вращения, которое согласуется с наблюдениями магнитного датчика.
[35] В таких вариантах осуществления мерой близости вращения является норма кватернионной разности между кватернионом, который вычислен путем интегрирования сигнала гиродатчика, и кватернионом, который обеспечивает вращение устройства, согласующееся с наблюдениями магнитного датчика. Вычисление может производиться автономно с использованием значений датчика, зарегистрированных в ходе фактического движения.
Краткое описание чертежей
[36] На фиг.1 показано устройство, которое состоит из микрокомпьютера (100), который включает в себя процессор (101), шину (102) памяти, память (103), (104), (105) и интерфейс (106) ввода/вывода. Микрокомпьютер имеет дисплей (111) и устройство (112) ввода. Интерфейс ввода/вывода подключен к банку датчиков с помощью их А/Ц преобразователей (121): 3-осным гиродатчикам (122), 3-осным магнитным датчикам (123) и 3-осным акселерометрам (124). ЦП дискретизирует все датчики через интерфейс (106) ввода/вывода и сохраняет соответствующие значения в запоминающем устройстве (105) или в основной памяти (103). Надлежащая компьютерная программа, которая обрабатывает такие полученные данные датчиков, хранится в ПЗУ (104) и выполняется ЦП (101).
[37] На фиг.2 показано восстановление траектории путем интегрирования сигнала гиродатчика при использовании номинального коэффициента усиления гиродатчика, обеспеченного производителем. Пунктирная линия показывает истинную траекторию.
[38] На фиг.3 показано такое же восстановление траектории при использовании оптимального коэффициента усиления, найденного согласно одной реализации представленного способа.
[39] На фиг.4 показан график экспериментальных данных, демонстрирующий, как ошибка при восстановлении вектора магнитного поля зависит от отклонения коэффициента усиления гиродатчика от его номинального значения, установленного производителем.
[40] На фиг.5 показана блок-схема операций алгоритма, демонстрирующая вычисление в реальном времени ошибки восстановления вектора магнитного поля.
[41] На фиг.6 показано вычисление оптимального значения коэффициента усиления гиродатчика из множественных значений ошибки, полученных для разных значений коэффициента усиления гиродатчика.
[42] На фиг.7 показано, что множественные значения ошибки можно вычислять параллельно или последовательно в зависимости от возможностей ЦП. Вычисление фактической траектории сначала осуществляется с использованием номинального значения G0 коэффициента усиления, установленного производителем, и затем переключается на оптимальный коэффициент усиления Gopt, как только он оказывается доступным.
[43] Фиг.8 иллюстрирует, как вращение, полученное путем интегрирования сигнала гиродатчика, кватернион Le, можно уточнить путем вычисления вращения Lf, которое согласуется с наблюдаемой результирующей ориентацией вектора M магнитного поля и является наилучшим приближением к вращению Le гиродатчика. Кватернион Lm обеспечивает вращение, которое поворачивает вектор M магнитного поля из его наблюдаемой позиции M(k) в показанную ориентацию M(k+1), которая является его наблюдаемой ориентацией в момент времени t=k+1. Затем кватернион Lf строится из двух вращений: Lm и дополнительного вращения вокруг M.
Подробное описание
[44] В нижеследующем описании многочисленные конкретные детали изложены для обеспечения более полного раскрытия. Однако специалисту в данной области техники должно быть очевидно, что настоящее раскрытие можно осуществлять на практике без этих конкретных деталей. В ряде случаев общеизвестные признаки могут не быть описаны подробно, чтобы не затемнять сущность изобретения.
[45] В предпочтительной реализации мы используем арифметику кватернионов для определения вращения. Однако этот выбор не ограничивает раскрытие и используется только в целях иллюстрации. Специалисту в данной области техники известно, что существуют другие представления вращения, например поворотная матрица, углы Эйлера или другие способы.
[46] Кватернион представляет собой упорядоченный набор четырех действительных чисел q=(a, b, c, d), обладающий некими особыми математическими свойствами. Подробное описание кватерниона можно найти в многочисленных учебниках по предмету. Для нашей цели мы используем единичный кватернион.
norm(q) = sqrt(a*a+b*b+c*c+d*d)=1 (1)
[47] Единичный кватернион представляет вращение вокруг единичного вектора e=(e1, e2, e3) на угол α
q=[cos(α/2), sin(α/2)*e] (2)
[48] Таким образом, если вектор m=[mx, my, mz], представленный в системе координат (x, y, z), вращается вокруг вектора e на угол α, то новые координаты вектора m будут
m'=q*m*q' (3)
[49] В кватернионном представлении вращение твердого тела можно представить следующим дифференциальным уравнением
dq(t)/dt=0,5*ω(t)*q(t) (4)
[50] где ω=[ωx, ωy, ωz] - вектор угловой скорости вращения в системе координат тела, который измеряется правильно откалиброванными гиродатчиками.
[51] Решение этого дифференциального уравнения можно получить численно с использованием таких способов, как метод Рунге-Кутта или любым другим способом, известным в данной области техники. В то же время необходимо учитывать, что, когда твердое тело сначала совершает поворот согласно кватерниону q1 и затем согласно кватерниону q2, то его полное вращение можно представить их упорядоченным произведением
q12=q2*q1 (5)
[52] Предположим, что в момент времени T=t0 измерения вектора магнитного поля в единичной системе координат имеют вид
m0=[mx0, my0, mz0] (6)
и в момент времени T=tk измеренный вектор магнитного поля имеет следующие внутренние компоненты
mk=[mxk, myk, mzk] (7)
[53] Вращение от T=t0 до T=tk можно вычислить путем интегрирования уравнения (4) с ограничениями (5). Предположим, что это интегрирование дало кватернион qk. Затем, с использованием уравнения (3) ожидаемая композиция вектора магнитного поля в повернутой системе координат будет иметь вид
mk_est=qk*m0*qk' (8)
[54] В этот момент можно сравнить наблюдаемое и оцененное значение вектора магнитного поля и создать меру близости. В качестве способа иллюстрации будем использовать векторную норму разности этих двух векторов
em(k)=norm(mk - mk_est) (9)
[55] Эта операция может осуществляться в следующий интервал времени t(k)-t(k+1), и т.д. В результате, можно вычислить накопленную меру ошибки
Em = em(k)+em(k+1),...,em(k+n) (10)
[56] Первичные измерения гиродатчика являются цифровыми значениями напряжения или тока, генерируемого датчиком. Обозначим эти значения как g=[gx, gy, gz]. Эти значения связаны с угловой скоростью вращения ω, которая используется в уравнении (4). Специалисту в данной области техники известно, что векторы ω и g связаны линейным соотношением для каждого из их компонентов, которые выглядят следующим образом
gk=αk*ωk + bk; k=x, y, z (11)
[57] Таким образом, угловую скорость ω, которая используется в уравнении (4), можно вывести из первичных сигналов гиродатчика в виде
ωk=ck*gk-bk; k=x, y, z (12)
[58] В уравнении (12) ck представляет коэффициент усиления гиродатчика, и bk - смещение гиродатчика. Значения ck и bk должны быть точно известны во время работы устройства, чтобы можно было правильно определить ориентацию устройства. Хотя эти параметры часто определяются в ходе заводской калибровки, их фактические значения в ходе эксплуатации немного отличаются. Предположим, что фактическое значение ω отличается от значения, вычисленного согласно уравнению (12), малой поправкой μ коэффициента усиления, которая является неизвестной величиной.
ω'=(1+μ)*ω (13)
[59] Для определения неизвестного параметра μ мы предлагаем провести несколько операций интегрирования (4) сигнала гиродатчика, параллельно или последовательно, с несколькими разными значениями μ. В предпочтительной реализации, в порядке иллюстрации, но не ограничения, параллельно проводятся три операции интегрирования с μ0=0, μ1=+d, μ2=-d, где d - малое положительное число 0<d<<1. Результаты интегрирования с μ=0 используются для работы устройства, тогда как другие операции интегрирования выполняются в фоновом режиме, и их результаты используются для определения истинного значения μ, как описано ниже.
[60] Мера близости Em - уравнение (10), вычисляется для каждой из этих операций интегрирования. В результате набор {Em(k), m(k)} задает кривую зависимости Em от μ, показанную на фиг.4. Затем оптимальное значение μ, при котором эта функция достигает минимума, можно найти с использованием квадратичной аппроксимации или любого другого способа определения минимума функции, известного специалистам в данной области техники. На фиг.4 показана квадратичная аппроксимация и минимум, найденный на данных, собранных в ходе эксплуатации некоторого фактического устройства.
[61] После отыскания оптимального значения μ=μ*, оно используется для номинального интегрирования: μ0=μ*, μ1=μ*+d, μ2=μ*-d, и весь процесс продолжается, что позволяет немедленно обнаружить любое возможное изменение коэффициента усиления гиродатчика в реальном времени.
[62] Представленная иллюстрация определяет только один параметр как минимум функции одной переменной. Однако аналогичная процедура может осуществляться в случае необходимости оценивать большее количество параметров. Действительно, предположим, что нужно найти поправочные параметры μ для каждой из трех осей гиродатчика в отдельности: μx, μy, μz. Тогда вместо того, чтобы повторять один и тот же патент из 3 параллельных операций интегрирования с поправками [0, 0, 0], [d, d, d] и [-d, -d, -d], можно изменить один параметр для каждого прохода, например [0, 0, 0], [d, d, d], [-d, -d, -d] для первого прохода, затем, определив оптимальный общий коэффициент μ*, как описано выше, следующий проход можно провести с параметрами [0, 0, 0], [d, 0, 0], [-d, 0, 0], таким образом, определяя дополнительную поправку μx*, затем μy* и т.д.
[63] Альтернативно можно придумать эксперимент, где все три параметра определяются в каждом проходе, с применением дополнительных параллельных операций интегрирования. Поскольку вычислительная нагрузка для решения уравнения (4) не является значительной, параллельные вычисление множественных потоков выполнения в одно и то же время вполне осуществимо с современными микропроцессорами, обычно используемыми в таких системах.
[64] В другой реализации каждый раз для одного значения μ осуществляется только один поток выполнения интегрирования и затем результаты нескольких операций интегрирования сравниваются, даже если эти операции интегрирования осуществлялись в разное время.
[65] Даже когда все параметры калибровки оптимизированы, сигналы гиродатчика неизбежно содержат шум, интегрирование которого приводит к ошибкам ориентации, которые растут со временем пропорционально квадратному корню времени интегрирования. Эти ошибки можно снизить, но не исключить, с использованием метода мертвой зоны, согласно которому любой сигнал гиродатчика, который меньше некоторого заранее определенного порога обнуляется. Метод мертвой зоны повышает общее SNR (отношение сигнал-шум), но не исключает совокупного эффекта интегрирования шума.
[66] Таким образом, для сглаживания вышеупомянутых трудностей это раскрытие указывает, как улучшить определение ориентации с использованием вектора магнитного поля помимо интегрирования сигнала гиродатчика.
[67] Если магнитные датчики правильно откалиброваны, то можно ожидать, что вращение, вычисленное путем интегрирования сигнала гиродатчика согласно уравнениям (4)-(5), будет согласовано с вращением, которое переводит вектор m(t0) в m(t). Пусть Lg это кватернион, вычисленный из уравнений (4)-(5).
[68] Вращение, которое поворачивает вектор m из m0 в m1, может осуществляться кватернионом Lm:
m0e=m0/norm(m0); m1e=m1/norm(m1);
e=m0e×m1e;
α=acos(m0e dot m1e) (14)
Lm=[cos(α/2), sin(α/2)*e]
[69] Дополнительное вращение вокруг нового вектора m1 возможно без изменения вектора m1. Это дополнительное вращение можно представить кватернионом Lr
Lr=[cos(β/2), sin(β/2)*m1e] (15)
[70] где β - угол дополнительного вращения, не заданный в этот момент. Настоящее раскрытие указывает, что угол β следует выбирать так, чтобы полное вращение, осуществляемое кватернионами Lm и Lr, (обозначим его Le(β)) было как можно ближе к вращению Lg
Le(β)=Lr(β)*Lm (16)
[71] В предпочтительной реализации этого раскрытия, в качестве меры близости кватернионов выбрана норма разности кватернионов. Таким образом, дополнительное вращение β определяется из минимизации norm(Lg-Le(β))
β -> min(norm(Lg - Le(β)) (17)
[72] Уравнение 17 можно решать многими способами, хорошо известными специалистам в данной области техники. Допустим, минимальное значение (17) достигается при β=β*.
[73] Таким образом, в конце каждого этапа интегрирования сигнала гиродатчика существует два кватерниона, потенциально описывающих вращение: Lg и Le(β*). Эти два кватерниона можно использовать по-разному, например, как взвешенную сумму, весовые коэффициенты которой обратно пропорциональны оцененной точности каждого кватерниона. Предпочтительная реализация предусматривает использование среднего этих двух кватернионов. На фиг.8 показано улучшение, достигаемое предпочтительной реализацией этого раскрытия для испытательного вращения в контролируемых условиях.
[74] Специалисту в данной области техники очевидно, что раскрытые варианты осуществления обеспечивают один или более из вышеописанных преимуществ. По прочтении вышеприведенного описания изобретения специалист в данной области техники сможет предложить различные изменения, эквивалентные замены и различные другие варианты осуществления помимо раскрытых здесь. Таким образом, объем защиты изобретения ограничен определением, содержащимся в нижеследующей формуле изобретения и ее эквивалентах.