СПОСОБ ОБНОВЛЕНИЯ УГЛОВОГО ПОЛОЖЕНИЯ СЕЛЬСКОХОЗЯЙСТВЕННОЙ МАШИНЫ, ОСНОВАННЫЙ НА ДЕВЯТИОСЕВОМ ДАТЧИКЕ НА ОСНОВЕ МЭМС

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

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