СПОСОБ НАЧАЛЬНОГО ВЫРАВНИВАНИЯ УСТРОЙСТВА ИНЕРЦИАЛЬНОЙ НАВИГАЦИИ Российский патент 2018 года по МПК G01C25/00 

Область техники

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

Уровень техники

Поскольку датчики, технология навигации и управления микроэлектромеханических систем (МЭМС, Micro-Electro-Mechanical-System) развиваются в сочетании с растущей государственной поддержкой сельского хозяйства, это привело к тому, что точное сельское хозяйство все более пользуется спросом. При управлении сельскохозяйственной машиной с машинным приводом информация о перемещении и положении корпуса транспортного средства может отражаться с помощью пространственной ориентации (включая угол наклона, угол бокового наклона и курсовой угол), скорости и информации о положении кузова транспортного средства в реальном времени. Данная информация может обеспечить важные входные данные для комбинированного алгоритма навигации и управления с высокой точностью.

Бесплатформенная инерциальная навигационная система (Strapdown-Inertial-Navigation-System, SINS) характеризуется такими особенностями, как автономная навигация, надежная конфиденциальность, сильная помехозащищенность, наличие множества навигационных параметров, высокая точность для использования в течение короткого периода времени, и т.п., и широкое использование. Однако, поскольку инерционный датчик имеет присущие ему ошибки, с течением времени навигационные ошибки накапливаются и ухудшается точность навигации, так что требуется помощь других навигационных систем (например, высокоточной GPS-RTK, Real Time Kinematic) со стабильной ошибкой. Инерциальная навигационная система получает информацию о скорости и положении, выполняя интегральную операцию на основе измеренного ускорения кузова транспортного средства. Следовательно, необходимо знать начальную скорость и положение. Кроме того, в инерциальной навигационной системе, использующей географическую систему координат в качестве навигационной системы координат, либо физическая платформа, либо математическая платформа является опорой для измерения ускорения. Кроме того, платформа должна точно выравнивать и отслеживать географическую систему координат, чтобы избежать ошибки платформы, вызванной ошибкой в измерении ускорения. Точность начального выравнивания прямо связана с точностью работы навигационной системы. Таким образом, начальное выравнивание также является одной из ключевых технологий.

Существует два широко используемых способа выравнивания: грубое выравнивание и выравнивание. Однако, два указанных способа выравнивания в основном используются с высокоточными гироскопами, которые могут измерять угловую скорость вращения Земли. Поэтому эти два способа выравнивания не подходят для устройства инерциальной навигации с низкой стоимостью и низкой производительностью.

Раскрытие сущности изобретения

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

Техническим решением согласно настоящему изобретению является способ начального выравнивания устройства инерциальной навигации. Способ включает следующие этапы: обеспечение устройства, снабженного датчиком, и предварительную подготовку датчика; выполнение процесса относительного выравнивания для получения угла ошибки установки датчика; и выполнение процесса абсолютного выравнивания для получения угловой ошибки пространственной ориентации установки датчика и также улучшение точности угла ошибки пространственной ориентации, вычисленного на базе относительного выравнивания.

В указанном способе этап предварительной подготовки датчика включает следующие этапы: инициализация датчика; и фильтрация датчика для уменьшения влияния вибрации устройства на датчик.

В указанном способе этап выполнения обработки относительного выравнивания включает следующие этапы: расположение устройства, чтобы оно было неподвижным в определенном направлении, сбор данных ускорения по трем осям, затем поворот устройства на 180 градусов, сбор данных ускорения по трем осям; и вычисление угла ошибки установки датчика для получения углов ошибки установки датчика в различных направлениях.

В указанном способе формула для вычисления углов ошибки установки следующая:

;

где А, В, С - коэффициенты модели; , , - выходные значения акселерометра в датчике; γ, θ, ϕ - углы ошибки установки датчика, g - гравитационное ускорение Земли, α - угол между плоскостью, в которой находится устройство, и горизонтальной плоскостью.

В указанном способе выполняется аппроксимация данных методом наименьших квадратов для получения формулы для вычисления коэффициентов модели А, В, С:

;

где , , - выходные значения акселерометра в датчике; n - общее число проб; i - точка отбора проб, i=1, 2, …, n, где n - положительное целое число.

В указанном способе, способ для вычисления угла α между плоскостью, в которой находится устройство, и горизонтальной плоскостью заключается в следующем.

Предполагая, что курсовой угол устройства во время первой парковки равен β, угол α между плоскостью, в которой находится устройство, и горизонтальной плоскостью вычислен поданным ускорения по трем осям в момент, когда угол парковки равен β, и данным ускорения по трем осям в момент, когда угол парковки равен (β+π), тогда формула для вычисления α следующая:

.

В указанном способе этап выполнения абсолютного выравнивания включает следующие этапы: построение модели фильтрации Калмана для абсолютного выравнивания, в которой вектор состояния фильтрации Калмана: ; где δx - вектор системы обнаружения ошибок; δp, δv, δψ_nb - позиционная ошибка, скоростная ошибка, и угловая ошибка пространственной ориентации, соответственно; и - нулевое смещение ускорения и нулевое смещение гироскопа, соответственно; Т обозначает перемещение. Вектор наблюдений фильтрации Калмана: ; где p - информация о GPS-позиции, v - информация о GPS-скорости.

Обновленное уравнение системы фильтрации Калмана выглядит следующим образом:

;

где , , - производные от δр, δv, δψ_nb, соответственно; - матрица поворота из системы координат устройства в систему координат навигации, f^b и - выходные данные ускорения и угловой скорости датчика, соответственно.

Начальный угол пространственной ориентации α, вычисленный во время относительного выравнивания, используют в качестве начального значения δψ_nb в векторе состояния фильтрации Калмана, затем вектор системы обновляют в соответствии с обновлением уравнением системы фильтрации Калмана, и выполняют фильтрацию Калмана. После того как фильтрация Калмана сходится, вычисляют ошибку δψ_nb угла пространственной ориентации установки датчика.

В указанном способе устройство представляет собой сельскохозяйственную машину, а сельскохозяйственная машина представляет собой трактор.

В настоящем изобретении используют относительное выравнивание и абсолютное выравнивание. Относительное выравнивание используют для выравнивания системы координат датчика на основе MEMS-IMU (инерциальный измерительный блок, inertial measurement unit) с системой координат трактора. Абсолютное выравнивание используют для выравнивания системы координат трактора с системой координат навигации. Относительный угол ошибки пространственной ориентации вычисляют во время относительного выравнивания. Относительный угол ошибки пространственной ориентации используют в качестве начального значения ошибки пространственной ориентации в векторе состояния во время абсолютного выравнивания для ускорения схождения фильтрации Калмана. Процесс абсолютного выравнивания может дополнительно улучшать точность выравнивания.

Краткое описание чертежей

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

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

На фиг. 2 приведена схема последовательности процесса относительного выравнивания согласно настоящему изобретению;

На фиг. 3 приведена схема последовательности процесса абсолютного выравнивания согласно настоящему изобретению;

Осуществление изобретения

Чтобы обеспечить полное понимание настоящего изобретения, в следующем описании изложено множество конкретных деталей. Однако очевидно, что специалисты в данной области могут реализовать настоящее изобретение без одной или более описанных здесь деталей. В других примерах, чтобы не мешать объяснению настоящего изобретения, хорошо известные особенности не описаны.

Чтобы обеспечить полное понимание настоящего изобретения, ниже приведены подробные этапы и подробная структура для пояснения технического решения согласно настоящему изобретению. Ниже подробно описаны предпочтительные варианты реализации настоящего изобретения. Однако кроме описанных, настоящее изобретение может иметь другие варианты реализации.

Настоящее изобретение обеспечивает способ начального выравнивания устройства инерциальной навигации. Со ссылкой на фиг. 1 способ, по существу, включает следующие этапы.

На этапе S1, устройство снабжают предусмотренным датчиком, и датчик предварительно программируют.

В дополнительном варианте реализации настоящего изобретения этап предварительной подготовки датчика включает следующие этапы.

На этапе S1a датчик инициализируют;

На этапе S1b для уменьшения влияния вибрации устройства на датчик, на датчике выполняют процесс фильтрации, а именно, специальный процесс, показанный на чертежах.

На этапе S2 для вычисления ошибки угла установки датчика выполняют процесс относительного выравнивания.

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

В указанном способе формула для вычисления углов ошибки установки такая:

где А, В, С - коэффициенты модели; , , выходные значения акселерометра в датчике; γ, θ, ϕ - углы ошибки установки датчика, g - гравитационное ускорение Земли, α - угол между плоскостью, в которой находится устройство, и горизонтальной плоскостью.

Выполняют аппроксимацию методом наименьших квадратов для получения формулы для расчета коэффициентов модели А, В, С:

где , , - выходные значения акселерометра в датчике; n - общее число проб; i - точка отбора проб, i=1, 2, …, n, n - положительное целое число. Например, n=8, 9, 10, 12 ….

Способ вычисления угла α между плоскостью, в которой расположено устройство, и горизонтальной плоскостью приведен ниже.

Предположим, что курсовой угол устройства во время первой парковки равен β, угол α между плоскостью, в которой находится устройство, и горизонтальной плоскостью вычислен по данным ускорения по трем осям в момент, когда угол парковки равен β, и данным ускорения по трем осям в момент, когда угол парковки равен (β+π), тогда формула для вычисления α следующая:

А, В, С и α могут быть вычислены по формулам (2) и (3). Затем А, В, С, и α подставляют в формулу (1) для вычисления углов ошибки установки γ, θ, ϕ.

На этапе S3 для вычисления угловой ошибки пространственной ориентации установки датчика, для дополнительного увеличения точности угла ошибки пространственной ориентации, вычисленной по относительному выравниванию, выполняют процесс абсолютного выравнивания.

В настоящем изобретении со ссылкой на фиг. 3, этап выполнения абсолютного выравнивания включает следующие этапы.

Во-первых, строят модель фильтрации Калмана для абсолютного выравнивания.

Получают вектор состояния фильтрации Калмана, как

где δх - вектор системы обнаружения ошибок; δp, δv, δψ_nb - позиционная ошибка, скоростная ошибка, и угловая ошибка пространственной ориентации, соответственно; и - нулевое смещение ускорения и нулевое смещение гироскопа, соответственно; Т указывает перемещение.

После этого вектор наблюдений фильтрации Калмана получают как

где p - информация о GPS-позиции, v - информация о GPS-скорости.

Далее, обновленное уравнение системы фильтрации строят следующим образом:

где , , - производные от δр, δv, δψ_nb, соответственно; - матрица поворота из системы координат устройства в систему координат навигации, f^b и - выходные данные ускорения и угловой скорости датчика, соответственно.

Начальный угол ориентации α, вычисленный по формуле (3) относительного выравнивания, используют в качестве начального значения δψ_nb в векторе состояния фильтрации Калмана (или называемого вектором состояния Калмана), затем вектор системы фильтрации Калмана обновляют в соответствии с формулой (6) и выполняют фильтрацию Калмана.

После того как фильтрация Калмана сходится, вычисленный и выходной углы пространственной ориентации являются относительно корректной угловой ошибкой δψ_nb пространственной ориентации установки датчика (т.е. углом выравнивания). Точность угла ошибки пространственной ориентации, вычисленная при относительном выравнивании, может быть дополнительно улучшена с помощью угловой ошибки δψ_nb пространственной ориентации установки.

В дополнительном варианте реализации настоящего изобретения устройство представляет собой сельскохозяйственную машину. Например, сельскохозяйственная машина является сельскохозяйственным трактором.

Реализация недорогого устройства инерциальной навигации для сельскохозяйственных машин (т.е. трехмерного акселерометра и трехмерного гироскопа) в настоящем изобретении включает два аспекта, т.е. относительное выравнивание и абсолютное выравнивание. Относительное выравнивание используют для выравнивания системы координат датчика на основе MEMS-IMU с системой координат трактора. Абсолютное выравнивание используют для выравнивания системы координат трактора с системой координат навигации. Относительный угол ошибки пространственной ориентации вычисляют во время относительного выравнивания. Относительный угол ошибки пространственной ориентации используют в качестве начального значения ошибки пространственной ориентации в векторе состояния во время абсолютного выравнивания для ускорения схождения фильтрации Калмана. Процесс абсолютного выравнивания может дополнительно улучшать точность выравнивания.

Поскольку в настоящем изобретении принято указанное техническое решение, оно имеет следующие преимущества.

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

(2) В настоящем изобретении выравнивание может быть получено без какого-либо другого периферийного оборудования, и настоящее изобретение имеет более высокую точность выравнивания.

(3) Способ начального выравнивания и инерциальной навигации в настоящем изобретении может увеличивать скорость схождения фильтрации Калмана, и может значительно уменьшать угловую ошибку пространственной ориентации, скоростную ошибку и позиционную ошибку, накапливающуюся стечением времени.

Выше описаны предпочтительные варианты реализации настоящего изобретения. Должно быть понятно, что настоящее изобретение не ограничено конкретными вариантами реализации, описанными выше. Устройства и конструкции, не описанные подробно, должны быть понятны и могут быть осуществлены обычным общепринятым способом. Любой специалист в данной области может использовать способ и техническое содержание, раскрытое выше, для выполнения ряда изменений и модификаций или эквивалентных вариантов реализации, изменяемых посредством эквивалентных модификаций в соответствии с техническим решением настоящего изобретения, не отходя от объема технического решения согласно настоящему изобретению. Это не влияет на существенное содержание настоящего изобретения. Таким образом, какая-либо простая модификация, эквивалентные изменения и модификации, применяемые к техническому существенному содержанию настоящего изобретения, не отходя от технического решения настоящего изобретения, все еще подпадают под объем защиты настоящего изобретения.

Настоящее изобретение относится к той проблеме, что обычный алгоритм начального выравнивания не подходит для устройств инерциальной навигации с низкой стоимостью и низкой производительностью в области сельскохозяйственных машин и технологических машин. Способ начального выравнивания устройства инерциальной навигации включает следующие этапы: обеспечение устройства, снабженного датчиком, и предварительную подготовку датчика; выполнение относительного выравнивания для вычисления угла ошибки установки датчика; выполнение абсолютного выравнивания для вычисления угловой ошибки пространственной ориентации установки датчика для увеличения точности угла ошибки пространственной ориентации, вычисленного во время относительного выравнивания. В процессе относительного выравнивания вычисляют относительный угол ошибки пространственной ориентации, при этом относительный угол ошибки пространственной ориентации используют в качестве начального значения для ошибки пространственной ориентации в векторе состояния в процессе абсолютного выравнивания, таким образом, ускоряя схождение фильтра Калмана. Технический результат заявленного изобретения – повышение точности устройств инерциальной навигации, установленных на сельскохозяйственных машинах. 6 з.п. ф-лы, 3 ил.

1. Способ начального выравнивания устройства инерциальной навигации, включающий следующие этапы:

обеспечение устройства, снабженного датчиком, и выполнение предварительной подготовки датчика;

выполнение относительного выравнивания для вычисления угла ошибки установки датчика;

выполнение абсолютного выравнивания для вычисления угловой ошибки пространственной ориентации установки датчика для улучшения точности угла ошибки пространственной ориентации, вычисленного во время относительного выравнивания, при этом

этап выполнения относительного выравнивания включает следующие этапы:

расположение устройства таким образом, чтобы оно было неподвижным в некотором направлении, сбор данных ускорения по трем осям, затем поворот устройства на 180 градусов и сбор данных ускорения по трем осям;

вычисление угла ошибки установки датчика для получения угла ошибки установки датчика в различных направлениях.

2. Способ по п. 1, в котором этап предварительной подготовки датчика включает следующие этапы:

инициализация датчика и

фильтрация датчика для уменьшения влияния вибрации устройства на датчик.

3. Способ по п. 1, в котором формула для вычисления угла ошибки установки имеет вид:

,

где А, В, С - коэффициенты модели; , , - выходные значения акселерометра в датчике; γ, θ, ϕ - углы ошибки установки датчика; g - гравитационное ускорение Земли; α - угол между плоскостью, в которой находится устройство, и горизонтальной плоскостью.

4. Способ по п. 3, в котором выполняют аппроксимацию данных методом наименьших квадратов для получения формулы для расчета коэффициентов модели А, В, С:

где , , - выходные значения акселерометра в датчике; n - общее число проб; i - точка отбора проб, i=1,2,…, n, где n - положительное целое число.

5. Способ по п. 3, в котором угол между плоскостью, в которой находится устройство, и горизонтальной плоскостью вычисляют следующим образом:

предполагают, что курсовой угол устройства во время первой парковки равен β, угол α между плоскостью, в которой находится устройство, и горизонтальной плоскостью вычислен с использованием данных ускорения по трем осям в момент, когда угол парковки равен β, и данных ускорения по трем осям в момент, когда угол парковки равен (β+π), тогда формула для вычисления α следующая:

6. Способ по п. 5, в котором этап выполнения абсолютного выравнивания включает следующие этапы:

построение модели фильтрации Калмана для абсолютного выравнивания, в которой

вектор состояния фильтрации Калмана: , где δх - вектор системы обнаружения ошибок; δр, δν, δψ_nb - позиционная ошибка, скоростная ошибка и угловая ошибка пространственной ориентации соответственно; и - нулевое смещение ускорения и нулевое смещение гироскопа соответственно; Т указывает перемещение;

вектор наблюдений фильтрации Калмана: , где р - информация о GPS-позиции, v - информация о GPS-скорости;

обновленное уравнение системы фильтрации Калмана выглядит следующим образом:

,

,

,

где , , - производные от δр, δν, δψ_nb соответственно; - матрица поворота из системы координат устройства в систему координат навигации; ƒ^b и - выходные данные ускорения и угловой скорости датчика соответственно;

начальный угол пространственного положения α, вычисленный с помощью относительного выравнивания, используют в качестве начального значения δψ_nb в векторе состояния фильтрации Калмана, затем вектор системы обновляют в соответствии с обновленным уравнением системы фильтрации Калмана и выполняют фильтрацию Калмана; после того как фильтрация Калмана сходится, вычисляют угловую ошибку δψ_nb пространственной ориентации установки.

7. Способ по п. 1, в котором устройство представляет собой сельскохозяйственную машину, а сельскохозяйственная машина представляет собой трактор.