СПОСОБ ОБНОВЛЕНИЯ УГЛОВОГО ПОЛОЖЕНИЯ СЕЛЬСКОХОЗЯЙСТВЕННОЙ МАШИНЫ, ОСНОВАННЫЙ НА ДЕВЯТИОСЕВОМ ДАТЧИКЕ НА ОСНОВЕ МЭМС Российский патент 2018 года по МПК G01C21/00 A01B69/00 

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

[0001] Изобретение относится к области измерительной техники и, в частности, относится к способу обновления углового положения сельскохозяйственной машины, основанному на девятиосевом датчике на основе МЭМС.

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

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

[0003] В настоящее время инерциальная навигационная система (ИНС) включает ПИНС (платформенную инерциальную навигационную систему, PINS) и бесплатформенную ИНС (бесплатформенную инерциальную навигационную систему, SINS). По сравнению с ПИНС, бесплатформенная ИНС использует датчики на основе ИБИ (инерциального измерительного блока) для установки "математической платформы" путем вычисления, таким образом заменяя ПИНС. Бесплатформенная ИНС в основном используется в навигационных системах летающих аппаратов. Однако, в области управления сельскохозяйственных машин исследования и применение бесплатформенной ИНС остаются на ранней стадии развития. Более того, навигационные управляющие системы для летательных аппаратов значительно отличаются от систем управления сельскохозяйственных машин по объектам применения и условиям окружающей среды. Способ использования бесплатформенной ИНС, применяемый в навигационных управляющих системах летательных аппаратов, не может быть применен в управлении сельскохозяйственной машины.

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

[0004] Имея в виду указанные выше недостатки в применении инерциальной навигации в сельскохозяйственной машине, настоящее изобретение обеспечивает способ обновления углового положения сельскохозяйственной машины, основанный на девятиосевом датчике на основе МЭМС, с минимальными ошибками, высокой точностью и надежностью.

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

[0006] Способ обновления углового положения сельскохозяйственной машины, основанный на девятиосевом датчике на основе МЭМС. Способ обновления углового положения сельскохозяйственной машины, основанный на девятиосевом датчике на основе МЭМС, включает:

установку модели ошибок гироскопа, калибровочной модели эллипса электронного компаса и семиразмерной фильтрующей модели на основе расширенного фильтра Калмана (EKF), и задание вектора параметров, соответствующего динамическому положению транспортного средства;

получение данных, включая ускорение и угловую скорость движения транспортного средства, и интенсивности геомагнитного поля в режиме реального времени посредством девятиосевого датчика на основе МЭМС;

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

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

отличающийся тем, что девятиосевой датчик на основе МЭМС составлен из трехосевого гироскопа, трехосевого акселерометра и трехосевого геомагнитного датчика.

[0007] Согласно одному аспекту настоящего изобретения этап, на котором происходит установка модели ошибок гироскопа, калибровочной модели эллипса электронного компаса и семиразмерной фильтрующей модели на основе расширенного фильтра Калмана (EKF), и задание вектора параметров, соответствующего динамическому положению транспортного средства, описан подробно далее:

вычисляют угловую скорость гироскопа в модели ошибок гироскопа через формулу вычисления ошибки; где формула вычисления ошибки гироскопа следующая: ω=ω_ib+b_ωr+b_ωg, где ω - угловая скорость, выдаваемая гироскопом, ω_ib - реальная угловая скорость гироскопа, b_ωr - смещение нуля гироскопа, и b_ωg - белый шум, выдаваемый гироскопом;

устраняют интерференцию магнитного поля посредством калибровочной модели эллипса электронного компаса; причем калибровочная модель эллипса электронного компаса следующая: , где mx, my - интенсивности магнитного поля, Xoffset и Yoffset - жесткие магнитные интерференции Xsf и Ysf - мягкие магнитные интерференции;

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

где матрица состояния следующая: x=[q b_ωr], q - кватернион векторов q₀, q₁, q₂, q₃, a b_ωr - смещение нуля трехосевого XYZ гироскопа, где ω - угловая скорость гироскопа, ω1 - матрица шума процесса, v1 - матрица шума наблюдения, y - это вектор наблюдения, y=[a ψ_mag]^T, где а - это величина трехосевого ускорения, ψ_mag - курсовой угол, вычисленный электронным компасом,

[0008] Согласно одному аспекту настоящего изобретения этап, на котором происходит получение данных, включая ускорение и угловую скорость движения транспортного средства, и интенсивности геомагнитного поля в режиме реального времени посредством девятиосевого датчика на основе МЭМС, описан подробно далее:

получают угловую скорость транспортного средства посредством гироскопа, и компенсируют смещение нуля гироскопа;

получают данные по ускорению транспортного средства посредством датчика ускорения; и

получают интенсивность геомагнитного поля транспортного средства посредством геомагнитного датчика.

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

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

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

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

[0010] Согласно одному аспекту настоящего изобретения этап, на котором происходит совместная обработка данных по углу, скорости, положению и курсовому углу транспортного средства посредством семиразмерной модели расширенного фильтра Калмана (EKF), и обновление в режиме реального времени динамического углового положения транспортного средства, описан подробно далее:

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

Р_k(+)=[I-K_kH_k]Р_k(-)

где k - точка отсчета времени, - оценка состояния системы, (-) предыдущая точка отсчета времени, (+) последующая точка отсчета времени, Ф_k - матрица перехода состояния, Р_k - матрица минимума среднеквадратической ошибки, Q - ковариационная матрица, соответствующая вектору состояния, K_k - выигрыш ошибки, y_k - вектор наблюдения, H_k - переходная матрица для уравнения наблюдения, R_k - ковариационная матрица, соответствующая вектору наблюдения;

где Q - вектор кватерниона, q₀, q₁, q₂, q₃ - скаляры, формирующие вектор кватерниона, i, j, k - единичные вектора в трехмерной системе координат, обновленная матрица положения представлена ниже:

где - матрица ротации для преобразования системы координат носителя в навигационную систему координат,

где γ, θ, ψ - угол крена, угол наклона в продольной плоскости и курсовой угол соответственно.

[0011] Согласно одному аспекту настоящего изобретения после этапа, на котором происходит совместная обработка данных по углу, скорости, положению и курсовому углу транспортного средства посредством семиразмерной модели расширенного фильтра Калмана (EKF), и обновление в режиме реального времени динамического углового положения транспортного средства, выполняют:

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

курсовой угол:

угол наклона в продольной плоскости:

θ=θ_principle

угол крена:

[0012] Преимущества, которые предоставляет изобретение, представлены ниже: ускорение и угловая скорость движущегося объекта получают в режиме реального времени посредством датчика на основе МЭМС. Угловое ускорение, выдаваемое гироскопом, интегрируют для получения угла. Ускорение интегрируют для вычисления скорости, которую далее интегрируют для вычисления информации о положении. Геомагнитное поле получают посредством геомагнитного датчика, и курсовой угол вычисляют посредством компенсационного алгоритма и совместной обработки данных гироскопа. Далее, данные о положениях преобразовывают в переходную матрицу, так что система координат носителя преобразована в навигационную систему координат. Эта переходная матрица выступает в роли "математической платформы". Алгоритм бесплатформенной инерциальной навигационной системы (SINS) применяют к сельскохозяйственной машине, и переходная матрица представляет особую важность. Поскольку сельскохозяйственная машина находится в движении, то ее положение также постоянно изменяется. Таким образом, переходная матрица также должна постоянно пересчитываться и обновляться. Общепринятые алгоритмы обновления положения включают алгоритм углов Эйлера, алгоритм косинуса направления и алгоритм кватерниона. По сравнению с алгоритмом углов Эйлера, алгоритм кватерниона не имеет точки сингулярности. По сравнению с алгоритмом косинуса направления, алгоритм кватерниона имеет малую величину вычислений. Таким образом, алгоритм кватерниона является наиболее подходящим для использования во встроенном продукте. Геомагнитное поле Земли и модель ошибок гироскопа установлены в плоскости сельскохозяйственной машины, и семиразмерный расширенный фильтр Калмана применяют для обновления матрицы положения. Выполняют оценку кватерниона и смещения нуля гироскопа, и затем выполняют наблюдение курсового угла, вычисленного исходя из ускорения и интенсивности магнитного поля, так что может быть получено высокоточное трехмерное угловое положение. Алгоритм компенсации ошибок и корректирующий алгоритм существенно снижают влияние ошибок в алгоритме бесплатформенной ИНС (SINS). Датчик на основе МЭМС и алгоритм бесплатформенной ИНС обеспечивают высокую эффективность данного изобретения. Как показало тестирование на тракторе, ошибка выходного курсового угла меньше чем 0,1°, и ошибки угла наклона в продольной плоскости и угла крена меньше чем 0,01°. Так как кватернион используется как вектор положения фильтра Калмана, точность вычисления целевых параметров может быть дополнительно улучшена.

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

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

[0014] Фиг. 1 показывает блок-схему способа обновления углового положения сельскохозяйственной машины, основанного на девятиосевом датчике на основе МЭМС в соответствии с вариантом реализации 1 настоящего изобретения;

[0015] Фиг. 2 показывает блок-схему способа обновления углового положения сельскохозяйственной машины, основанного на девятиосевом датчике на основе МЭМС в соответствии с вариантом реализации 2 настоящего изобретения.

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

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

[0017] Вариант реализации изобретения 1:

[0018] Как показано на Фиг. 1, способ обновления углового положения сельскохозяйственной машины, основанный на девятиосевом датчике на основе МЭМС, включает:

[0019] Этап S1: установки модели ошибок гироскопа, калибровочной модели эллипса электронного компаса и семиразмерной фильтрующей модели на основе расширенного фильтра Калмана (EKF), и определения векторов параметров, соответствующих динамическим положениям транспортного средства.

[0020] Этап S1, в котором происходит установка модели ошибок гироскопа, калибровочной модели эллипса электронного компаса и семиразмерной фильтрующей модели на основе расширенного фильтра Калмана (EKF), и задание векторов параметров, соответствующих динамическим положениям транспортного средства, описан в подробности далее:

[0021] Угловую скорость гироскопа в модели ошибок гироскопа вычисляют через формулу вычисления ошибки гироскопа; где формула вычисления ошибки гироскопа следующая: ω=ω_ib+b_ωr+b_ωg, где ω - угловая скорость, выдаваемая гироскопом, ω_ib - реальная угловая скорость гироскопа, b_ωr - смещение нуля гироскопа, и b_ωg - белый шум, выдаваемый гироскопом.

[0022] Интерференцию магнитного поля устраняют посредством калибровочной модели эллипса электронного компаса, где калибровочная модель эллипса электронного компаса такова: , где mx, my - интенсивности магнитного поля, Xoffset и Yoffset - жесткие магнитные интерференции Xsf и Ysf - мягкие магнитные интерференции.

[0023] Положение транспортного средства обновляется посредством семиразмерной модели расширенного фильтра Калмана EKF, где семиразмерная фильтрующая модель использует Расширенный Фильтр Калмана для семиразмерного вектора состояния, и Расширенный Фильтр Калмана EKF включает уравнение состояния и уравнение наблюдения:

[0024] Матрица состояния представлена: x=[q b_ωr], q - кватернион векторов q₀, q₁, q₂, q₃, и b_ωr - смещение нуля трехосевого гироскопа XYZ, где ω - угловая скорость гироскопа, w1 - матрица шума процесса, v1 - матрица шума наблюдения, y - вектор наблюдения, y=[a ψ_mag]^T, где а - это величина трехосевого ускорения, ψ_mag - курсовой угол, вычисляемый электронным компасом,

.

[0025] Поскольку геомагнитное поле имеет слабую интенсивность, оно подвержено влиянию окружающих ферромагнитных материалов и электромагнитного поля. Поэтому необходимо откалибровать гироскоп прежде всего. Калибровочная модель эллипса электронного компаса устанавливается для устранения интерференции геомагнитного поля. Полученная интенсивность магнитного поля вводится с помощью метода наименьших квадратов в сам процесс калибровки, таким образом, чтобы обеспечить получение указанных выше параметров.

[0026] Этап S2: получение данных, включая ускорение и угловую скорость движения транспортного средства, и интенсивности геомагнитного поля в режиме реального времени посредством девятиосевого датчика на основе МЭМС.

[0027] Этап S2 получения данных, включая ускорение и угловую скорость движения транспортного средства, и интенсивности геомагнитного поля в режиме реального времени посредством девятиосевого датчика на основе МЭМС, описан подробно далее:

[0028] Угловую скорость транспортного средства получают посредством гироскопа, и компенсации смещения нуля гироскопа.

[0029] Датчик ускорения используется для получения данных об ускорении транспортного средства.

[0030] Информацию по интенсивности геомагнитного поля транспортного средства получают геомагнитным датчиком.

[0031] Этап S3: согласно полученным данным, включая ускорение и угловую скорость транспортного средства, и интенсивность геомагнитного поля, вычисляют угол, скорость, положение и курсовой угол транспортного средства посредством установленных модели ошибок гироскопа и калибровочной модели эллипса электронного компаса.

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

[0033] Данные по угловому положению получают посредством интегрального вычисления угловой скорости с помощью модели ошибок гироскопа.

[0034] Скорость вычисляют путем интегрирования данных по ускорению, и информация по положению вычисляют путем дальнейшего интегрирования скорости.

[0035] Курсовой угол транспортного средства затем вычисляют исходя из данных по интенсивности геомагнитного поля, которая компенсируется параметром калибровки и корректируется углом наклона, и оба показателя, параметр калибровки и угол наклона, вычисляют посредством эллиптической модели.

[0036] Информацию о движении транспортного средства получают посредством датчика на основе МЭМС в режиме реального времени. Угловая скорость транспортного объекта, полученная посредством гироскопа, корректируется оценкой состояния и смещением нуля гироскопа. Угловая скорость транспортного средства интегрируется для вычисления приращения угла. Показания геомагнитного датчика корректируются и компенсируются данными по мягкому и жесткому магнетизму, а также углом наклона для вычисления курсового угла.

[0037] Этап S4: выполняют совместную обработку данных по углу, скорости, положению и курсовому углу транспортного средства посредством семиразмерной модели расширенного фильтра Калмана (EKF), и обновляют в режиме реального времени динамическое угловое положение транспортного средства.

[0038] Этап S4 совместной обработки данных по углу, скорости, положению и курсовому углу транспортного средства посредством семиразмерной модели расширенного фильтра Калмана (EKF), и обновления в режиме реального времени динамическое угловое положение транспортного средства, подробно описан далее.

[0039] Данные о положении транспортного средства вычисляют с помощью семиразмерной модели расширенного фильтра Калмана (EKF), посредством алгоритма обновления кватерниона положения, причем процесс вычисления алгоритма расширенного фильтра Калмана (EKF) описан ниже:

Р_k(+)=[I-K_kH_k]P_k(-)

[0040] В приведенной выше формуле: к - точка отсчета времени, - оценка состояния системы, (-) - предыдущая точка отсчета времени, (+) - последующая точка отсчета времени, Ф_k - матрица перехода состояния, P_k - матрица минимума среднеквадратической ошибки, Q - ковариационная матрица, соответствующая вектору состояния, K_k - выигрыш ошибки, y_k - вектор наблюдения, H_k - переходная матрица для уравнения наблюдения, R_k - ковариационная матрица, соответствующая вектору наблюдения.

[0041] В приведенной выше формуле Q - вектор кватерниона, q₀, q₁, q₂, q₃ - скаляры, формирующие вектор кватерниона, i, j, k - единичные вектора в трехмерной системе координат. Обновленная матрица положения представлена ниже:

[0042] В приведенной выше формуле - матрица ротации для преобразования системы координат носителя в навигационную систему координат.

[0043] В приведенной выше формуле γ, θ, ψ - угол крена, угол наклона в продольной плоскости и курсовой угол соответственно.

[0044] В приведенной выше формуле, девятиосевой датчик на основе МЭМС составлен из трехосевого гироскопа, трехосевого акселерометра и трехосевого геомагнитного датчика.

[0045] Вариант реализации изобретения 2:

[0046] Как показано на Фиг. 2, способ обновления углового положения сельскохозяйственной машины, основанный на девятиосевом датчике на основе МЭМС, включает следующие этапы:

[0047] Этап S1: установки модели ошибок гироскопа, калибровочной модели эллипса электронного компаса и семиразмерной фильтрующей модели на основе расширенного фильтра Калмана (EKF), и определения векторов параметров, соответствующих динамическим положениям транспортного средства.

[0048] Этап S1, на котором происходит установка модели ошибок гироскопа, калибровочной модели эллипса электронного компаса и семиразмерной фильтрующей модели на основе расширенного фильтра Калмана (EKF), и задание векторов параметров, соответствующих динамическим положениям транспортного средства, подробно описан далее:

[0049] Угловую скорость гироскопа в модели ошибок гироскопа вычисляют через формулу вычисления ошибки гироскопа; где формула вычисления ошибки гироскопа следующая: ω=ω_ib+b_ωr+b_ωg, где ω - угловая скорость, выдаваемая гироскопом, ω_ib - реальная угловая скорость гироскопа, b_ωr - смещение нуля гироскопа, и b_ωg - белый шум, выдаваемый гироскопом.

[0050] Интерференцию магнитного поля устраняют посредством калибровочной модели эллипса электронного компаса, где калибровочная модель эллипса электронного компаса такова: где mx, my - интенсивности магнитного поля, Xoffset и Yoffset - жесткие магнитные интерференции, Xsf и Ysf - мягкие магнитные интерференции.

[0051] Положение транспортного средства обновляют посредством семиразмерной модели Расширенного Фильтра Калмана EKF, где семиразмерная фильтрующая модель использует Расширенный Фильтр Калмана для семиразмерного вектора состояния, и Расширенный Фильтр Калмана EKF включает уравнение состояния и уравнение наблюдения:

[0052] Матрица состояния представлена: x=[q b_ωr], где q - кватернион векторов q₀, q₁, q₂, q3, b_ωr - смещение нуля трехосевого XYZ гироскопа. В формуле ω - угловая скорость гироскопа, w1 - матрица шума процесса, v1 - матрица шума наблюдения, y - вектор наблюдения, y=[a ψ_mag]^T, где а - это величина трехосевого ускорения, ψ_mag - курсовой угол, вычисляемый электронным компасом,

.

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

[0054] Этап S2: получение данных, включая ускорение и угловую скорость движения транспортного средства, и интенсивности геомагнитного поля в режиме реального времени посредством девятиосевого датчика на основе МЭМС.

[0055] Этап S2 получения данных, включая ускорение и угловую скорость движения транспортного средства, и интенсивности геомагнитного поля в режиме реального времени посредством девятиосевого датчика на основе МЭМС, описан подробно далее:

[0056] Угловую скорость транспортного средства получают посредством гироскопа, и компенсируют смещение нуля гироскопа.

[0057] Датчик ускорения используют для получения данных об ускорении транспортного средства.

[0058] Информацию по интенсивности геомагнитного поля транспортного средства получают посредством геомагнитного датчика.

[0059] Этап S3: согласно полученным данным, включая ускорение и угловую скорость движения транспортного средства, интенсивность геомагнитного поля, вычисляют угол, скорость, положение и курсовой угол транспортного средства посредством установленных модели ошибок гироскопа и калибровочной модели эллипса электронного компаса.

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

[0061] Данные по угловому положению получают посредством интегрального вычисления угловой скорости с помощью модели ошибок гироскопа.

[0062] Скорость вычисляют путем интегрирования данных по ускорению, и информация по положению вычисляют путем дальнейшего интегрирования скорости.

[0063] Курсовой угол транспортного средства затем вычисляют исходя из данных по интенсивности геомагнитного поля, которая компенсируется параметром калибровки и корректируется углом наклона, и оба показателя, параметр калибровки и угол наклона, вычисляют посредством эллиптической модели.

[0064] Информацию по движению транспортного средства получают датчиком на основе МЭМС в режиме реального времени. Угловая скорость транспортного средства, полученная посредством гироскопа, корректируется оценкой состояния и смещением нуля гироскопа. Угловая скорость транспортного средства интегрируется для вычисления приращения угла. Показания геомагнитного датчика корректируются и компенсируются данными по мягкому и жесткому магнетизму, а также углом наклона для вычисления курсового угла.

[0065] Этап S4: выполняют совместную обработку данных по углу, скорости, положению и курсовому углу транспортного средства посредством семиразмерной модели расширенного фильтра Калмана (EKF), и обновляют в режиме реального времени динамическое угловое положение транспортного средства.

[0066] Этап S4 совместной обработки данных по углу, скорости, положению и курсовому углу транспортного средства посредством семиразмерной модели расширенного фильтра Калмана (EKF), и обновления в режиме реального времени динамического углового положения транспортного средства, подробно описан далее.

[0067] Данные о положении транспортного средства вычисляют с помощью семиразмерной модели расширенного фильтра Калмана (EKF), посредством алгоритма обновления кватерниона положения, где процесс вычисления алгоритма расширенного фильтра Калмана (EKF) описан ниже:

Р_k(+)=[I-K_kН_k]Р_k(-)

[0068] В приведенной выше формуле, k - точка отсчета времени, - оценка состояния системы, (-) - предыдущая точка отсчета времени, (+) - последующая точка отсчета времени, Ф_k - матрица перехода состояния, Р_k - матрица минимума среднеквадратической ошибки, Q - ковариационная матрица, соответствующая вектору состояния, K_k - выигрыш ошибки, y_k - вектор наблюдения, H_k - переходная матрица для уравнения наблюдения, Rk - ковариационная матрица, соответствующая вектору наблюдения.

[0069] В приведенной выше формуле Q - вектор кватерниона, q₀, q₁, q₂, q₃ - скаляры, формирующие вектор кватерниона, i, j, k - единичные вектора в трехмерной системе координат.

[0070] В приведенной выше формуле - матрица ротации для преобразования системы координат носителя в навигационную систему координат.

[0071] В приведенной выше формуле γ, θ, ψ - угол крена, угол наклона в продольной плоскости и курсовой угол соответственно.

[0072] В приведенной выше формуле, девятиосевой датчик на основе МЭМС составлен из трехосевого гироскопа, трехосевого акселерометра и трехосевого геомагнитного датчика.

[0073] Этап S5: извлечение данных об угловом положении транспортного средства из обновленных данных о положении транспортного средства, для определения данных о величине угла положения. Угловое положение транспортного средства включает в себя угол наклона в продольной плоскости, угол крена и курсовой угол,

Курсовой угол:

Угол наклона в продольной плоскости:

θ=θ_principle

Угол крена:

[0074] Угловое положение транспортного средства может быть извлечено из обновленной и рассчитанной матрицы положения , включая угол наклона в продольной плоскости, угол крена и курсовой угол. Поскольку угол наклона в продольной плоскости θ определен в интервале [-90°, +90°], что согласуется с основным значением обратной функции синуса, то не возникает проблемы многозначности. Угол крена γ определен в интервале [180°, 180°]. Курсовой угол ψ определен в интервале [0°, 360°]. Следовательно, есть проблемы с многозначностью для обоих γ и ψ. После того как основное значение рассчитано, специальный квадрант может быть определен элементами из .

[0075] Преимущества, которые предоставляет изобретение, представлены ниже: ускорение и угловая скорость движущегося объекта получаются в режиме реального времени посредством датчика на основе МЭМС. Угловое ускорение, выдаваемое гироскопом, интегрируется для получения в конечном итоге значения угла. Ускорение интегрируется для вычисления скорости, которая далее интегрируется для вычисления информации о положении. Геомагнитное поле оценивается посредством геомагнитного датчика, и курсовой угол вычисляется посредством компенсационного алгоритма и совместной обработки данных гироскопа. Далее, данные о положениях преобразовывают в переходную матрицу для преобразования системы координат носителя в навигационную систему координат. Эта переходная матрица выступает в роли "математической платформы". Алгоритм бесплатформенной инерциальной навигационной системы SINS применяется к сельскохозяйственной машине, и переходная матрица представляет особую важность. Поскольку сельскохозяйственная машина находится в движении, то ее положение также постоянно изменяется. Таким образом, переходная матрица также должна постоянно пересчитываться и обновляться. Общепринятые алгоритмы обновления положения включают алгоритм углов Эйлера, алгоритм косинуса направления и алгоритм кватерниона. По сравнению с алгоритмом углов Эйлера, алгоритм кватерниона не имеет точки сингулярности. По сравнению с алгоритмом косинуса направления, алгоритм кватерниона имеет меньшую величину вычислений. Таким образом, алгоритм кватерниона является наиболее подходящим для использования во встроенном продукте. Геомагнитное поле Земли и модель ошибок гироскопа определяются в несущей поверхности сельскохозяйственной машины, и семиразмерный расширенный фильтр Калмана EKF применяется для обновления матрицы положения. Производится оценка кватерниона и нулевого смещения гироскопа. Затем производится наблюдение курсового угла, вычисляемого исходя из ускорения и интенсивности магнитного поля, таким образом давая возможность получить высокоточное трехмерное угловое положение. Алгоритм компенсации ошибок и корректирующий алгоритм существенно снижают влияние ошибок в алгоритме бесплатформенной ИНС (SINS). Датчик на основе МЭМС и алгоритм бесплатформенной ИНС обеспечивают высокую эффективность данного изобретения. Как показало тестирование на тракторе, ошибка курсового угла меньше чем 0,1°, и ошибки угла наклона в продольной плоскости и угла крена меньше чем 0,01°. Так как кватернион используется как вектор положения фильтра Калмана, точность вычисления целевых параметров может быть далее улучшена.

[0076] Приведенное выше описание является лишь иллюстрацией конкретных воплощений настоящего изобретения, но объем защиты настоящего изобретения этим не ограничивается. Модификации или замены, легко понимаемые специалистом в данной области с обычной квалификацией, в пределах объема раскрытия настоящего изобретения, все должны попадать в объем защиты настоящего изобретения. Таким образом, объем защиты настоящего изобретения должен определяться прилагаемой формулой изобретения.

Изобретение относится к области измерительной техники и, в частности, относится к способу обновления углового положения сельскохозяйственной машины, основанному на девятиосевом датчике на основе МЭМС. Способ обновления углового положения сельскохозяйственной машины, основанный на девятиосевом датчике на основе МЭМС, включает: установку модели ошибок гироскопа, калибровочной модели эллипса электронного компаса и семиразмерной фильтрующей модели на основе расширенного фильтра Калмана (EKF), и задание вектора параметров, соответствующего динамическому положению транспортного средства (S1); получение данных, включая ускорение и угловую скорость движения транспортного средства, и интенсивности геомагнитного поля в режиме реального времени (S2); вычисление угла, скорости, информации о положении и курсового угла транспортного средства посредством установленной модели ошибок гироскопа, калибровочной модели эллипса электронного компаса (S3); совместную обработку данных по углу, скорости, положению и курсовому углу транспортного средства посредством семиразмерной модели расширенного фильтра Калмана (EKF) (S4), и обновление в режиме реального времени динамического углового положения транспортного средства (S5). Технический результат заявленного изобретения - высокая точность и надежность определения положения сельскохозяйственной машины на основе девятиосевого МЭМС датчика. 5 з.п. ф-лы, 2 ил.

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

установку модели ошибок гироскопа, калибровочной модели эллипса электронного компаса и семиразмерной фильтрующей модели на основе расширенного фильтра Калмана (EKF) и задание вектора параметров, соответствующего динамическому положению транспортного средства;

получение данных, включая ускорение и угловую скорость движения транспортного средства, и интенсивности геомагнитного поля в режиме реального времени посредством девятиосевого датчика на основе МЭМС;

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

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

причем девятиосевой датчик на основе МЭМС составлен из трехосевого гироскопа, трехосевого датчика ускорения и трехосевого геомагнитного датчика.

2. Способ обновления углового положения сельскохозяйственной машины, основанный на девятиосевом датчике на основе МЭМС по п. 1, отличающийся тем, что установка модели ошибок гироскопа, калибровочной модели эллипса электронного компаса и семиразмерной фильтрующей модели на основе расширенного фильтра Калмана (EKF) и задание вектора параметров, соответствующего динамическому положению транспортного средства, также включают:

вычисление угловой скорости гироскопа в модели ошибок гироскопа через формулу вычисления ошибки гироскопа, где формула вычисления ошибки гироскопа следующая: ω=ω_ib+b_ωr+b_ωg, где ω - угловая скорость, выдаваемая гироскопом, ω_ib - реальная угловая скорость гироскопа, b_ωr - смещение нуля гироскопа и b_ωg - белый шум, выдаваемый гироскопом;

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

, где mх, mу - интенсивности магнитного поля, Xoffset и Yoffset - жесткие магнитные интерференции, Xsf и Ysf - мягкие магнитные интерференции;

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

где матрица состояния следующая: , q - кватернион векторов q₀, q₁, q₂, q₃, и b_ωr - смещение нуля XYZ трехосевого гироскопа, где ω - угловая скорость, выдаваемая гироскопом, w1 - матрица шума процесса, v1 - матрица шума наблюдения, у - вектор наблюдения, , где а - это величина трехосевого ускорения, ψ_mag - курсовой угол, вычисляемый электронным компасом,

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

получение угловой скорости транспортного средства посредством гироскопа и компенсацию смещения нуля гироскопа;

получение ускорения транспортного средства посредством датчика ускорения и

получение интенсивности геомагнитного поля транспортного средства посредством геомагнитного датчика.

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

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

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

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

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

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

,

где k - точка отсчета времени, - оценка состояния системы, (-) - предыдущая точка отсчета времени, (+) - последующая точка отсчета времени, Ф_k - матрица перехода состояния, Р_k - матрица минимума среднеквадратической ошибки, Q - ковариационная матрица, соответствующая вектору состояния, K_k - выигрыш ошибки, у_k - вектор наблюдения, Н_k - переходная матрица для уравнения наблюдения, R_k - ковариационная матрица, соответствующая вектору наблюдения;

где Q - вектор кватерниона, q₀, q₁, q₂, q₃ - скаляры, формирующие вектор кватерниона, i, j, k - единичные вектора в трехмерной системе координат, обновленная матрица положения следующая:

где - матрица ротации для преобразования системы координат носителя в навигационную систему координат,

где γ, θ, ψ - угол крена, угол наклона в продольной плоскости и курсовой угол соответственно.

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

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

курсовой угол:

угол наклона в продольной плоскости:

угол крена: