СПОСОБ ОБНОВЛЕНИЯ УГЛОВОГО ПОЛОЖЕНИЯ СЕЛЬСКОХОЗЯЙСТВЕННОЙ МАШИНЫ, ОСНОВАННЫЙ НА ДЕВЯТИОСЕВОМ ДАТЧИКЕ НА ОСНОВЕ МЭМС
METHOD OF UPGRADING ANGULAR POSITION OF AGRICULTURAL MACHINE BASED ON THE NINE-AXIS MEMS SENSOR

Область техники
[0001] Изобретение относится к области измерительной техники и, в частности, относится к способу обновления углового положения сельскохозяйственной машины, основанному на девятиосевом датчике на основе МЭМС.
Уровень техники
[0002] Развитие технологии датчиков на основе МЭМС (микроэлектромеханических систем), а также технологии навигации и управления, вместе с другими технологиями поддерживает сельское хозяйство Китая, что делает точное сельское хозяйство растущей популярной тенденцией. Во время машинного управления сельскохозяйственной техникой информация о различных аспектах транспортного средства, включая угол наклона в продольной плоскости, угол крена и курсовой угол, может обеспечить важные исходные данные для высокоточного интегрированного алгоритма навигации и управления.
[0003] В настоящее время инерциальная навигационная система (ИНС) включает ПИНС (платформенную инерциальную навигационную систему, PINS) и бесплатформенную ИНС (бесплатформенную инерциальную навигационную систему, SINS). По сравнению с ПИНС, бесплатформенная ИНС использует датчики на основе ИБИ (инерциального измерительного блока) для установки "математической платформы" путем вычисления, таким образом заменяя ПИНС. Бесплатформенная ИНС в основном используется в навигационных системах летающих аппаратов. Однако, в области управления сельскохозяйственных машин исследования и применение бесплатформенной ИНС остаются на ранней стадии развития. Более того, навигационные управляющие системы для летательных аппаратов значительно отличаются от систем управления сельскохозяйственных машин по объектам применения и условиям окружающей среды. Способ использования бесплатформенной ИНС, применяемый в навигационных управляющих системах летательных аппаратов, не может быть применен в управлении сельскохозяйственной машины.
Раскрытие сущности изобретения.
[0004] Имея в виду указанные выше недостатки в применении инерциальной навигации в сельскохозяйственной машине, настоящее изобретение обеспечивает способ обновления углового положения сельскохозяйственной машины, основанный на девятиосевом датчике на основе МЭМС, с минимальными ошибками, высокой точностью и надежностью.
[0005] Для решения поставленных задач, варианты реализации настоящего изобретения предусматривают следующие технические решения.
[0006] Способ обновления углового положения сельскохозяйственной машины, основанный на девятиосевом датчике на основе МЭМС. Способ обновления углового положения сельскохозяйственной машины, основанный на девятиосевом датчике на основе МЭМС, включает:
установку модели ошибок гироскопа, калибровочной модели эллипса электронного компаса и семиразмерной фильтрующей модели на основе расширенного фильтра Калмана (EKF), и задание вектора параметров, соответствующего динамическому положению транспортного средства;
получение данных, включая ускорение и угловую скорость движения транспортного средства, и интенсивности геомагнитного поля в режиме реального времени посредством девятиосевого датчика на основе МЭМС;
вычисление угла, скорости, положения и курсового угла транспортного средства посредством установленной модели ошибок гироскопа, калибровочной модели эллипса электронного компаса, в соответствии с полученными данными, включая ускорение и угловую скорость движения транспортного средства, и интенсивность геомагнитного поля;
совместная обработка данных по углу, скорости, положению и курсовому углу транспортного средства посредством семиразмерной модели расширенного фильтра Калмана (EKF), и обновление в режиме реального времени динамического углового положения транспортного средства;
отличающийся тем, что девятиосевой датчик на основе МЭМС составлен из трехосевого гироскопа, трехосевого акселерометра и трехосевого геомагнитного датчика.
[0007] Согласно одному аспекту настоящего изобретения этап, на котором происходит установка модели ошибок гироскопа, калибровочной модели эллипса электронного компаса и семиразмерной фильтрующей модели на основе расширенного фильтра Калмана (EKF), и задание вектора параметров, соответствующего динамическому положению транспортного средства, описан подробно далее:
ib
ωr
ωg
ib
ωr
ωg
вычисляют угловую скорость гироскопа в модели ошибок гироскопа через формулу вычисления ошибки; где формула вычисления ошибки гироскопа следующая: ω=ω+b+b, где ω - угловая скорость, выдаваемая гироскопом, ω - реальная угловая скорость гироскопа, b - смещение нуля гироскопа, и b - белый шум, выдаваемый гироскопом;
устраняют интерференцию магнитного поля посредством калибровочной модели эллипса электронного компаса; причем калибровочная модель эллипса электронного компаса следующая: , где mx, my - интенсивности магнитного поля, Xoffset и Yoffset - жесткие магнитные интерференции Xsf и Ysf - мягкие магнитные интерференции;
обновляют положения транспортного средства посредством семиразмерной модели расширенного фильтра Калмана (EKF), где семиразмерная фильтрующая модель использует расширенный фильтр Калмана для семиразмерного вектора состояния, и расширенный фильтр Калмана включает уравнение состояния и уравнение наблюдения:
ωr
0
1
2
3
ωr
mag
mag
a
T
где матрица состояния следующая: x=[q b], q - кватернион векторов q, q, q, q, a b - смещение нуля трехосевого XYZ гироскопа, где ω - угловая скорость гироскопа, ω1 - матрица шума процесса, v1 - матрица шума наблюдения, y - это вектор наблюдения, y=[ ψ], где а - это величина трехосевого ускорения, ψ - курсовой угол, вычисленный электронным компасом,
[0008] Согласно одному аспекту настоящего изобретения этап, на котором происходит получение данных, включая ускорение и угловую скорость движения транспортного средства, и интенсивности геомагнитного поля в режиме реального времени посредством девятиосевого датчика на основе МЭМС, описан подробно далее:
получают угловую скорость транспортного средства посредством гироскопа, и компенсируют смещение нуля гироскопа;
получают данные по ускорению транспортного средства посредством датчика ускорения; и
получают интенсивность геомагнитного поля транспортного средства посредством геомагнитного датчика.
[0009] Согласно одному аспекту настоящего изобретения этап, на котором происходит вычисление угла, скорости, положения и курсового угла транспортного средства посредством установленной модели ошибок гироскопа, калибровочной модели эллипса электронного компаса, в соответствии с полученными данными, включая ускорение и угловую скорость движущегося объекта, и интенсивностью геомагнитного поля, описан подробно далее:
получают данные по угловому положению посредством интегрального вычисления угловой скорости с помощью модели ошибок гироскопа;
вычисляют скорость путем интегрирования данных по ускорению, и информацию по положению вычисляют путем дальнейшего интегрирования скорости; и
вычисляют курсовой угол транспортного средства, исходя из данных по интенсивности геомагнитного поля, которые компенсируются параметром калибровки и корректируются углом наклона, и оба показателя, параметр калибровки и угол наклона, вычисляют посредством эллиптической модели.
[0010] Согласно одному аспекту настоящего изобретения этап, на котором происходит совместная обработка данных по углу, скорости, положению и курсовому углу транспортного средства посредством семиразмерной модели расширенного фильтра Калмана (EKF), и обновление в режиме реального времени динамического углового положения транспортного средства, описан подробно далее:
вычисляют данные по положению транспортного средства с помощью семиразмерной модели расширенного фильтра Калмана, посредством алгоритма обновления кватерниона положения, где процесс вычисления алгоритма расширенного фильтра Калмана следующий:
k
k
k
k
Р(+)=[I-KH]Р(-)
k
k
k
k
k
k
где k - точка отсчета времени,  - оценка состояния системы, (-) предыдущая точка отсчета времени, (+) последующая точка отсчета времени, Ф - матрица перехода состояния, Р - матрица минимума среднеквадратической ошибки, Q - ковариационная матрица, соответствующая вектору состояния, K - выигрыш ошибки, y - вектор наблюдения, H - переходная матрица для уравнения наблюдения, R - ковариационная матрица, соответствующая вектору наблюдения;
0
1
2
3
где 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), и задание векторов параметров, соответствующих динамическим положениям транспортного средства, описан в подробности далее:
ib
ωr
ωg
ib
ωr
ωg
[0021] Угловую скорость гироскопа в модели ошибок гироскопа вычисляют через формулу вычисления ошибки гироскопа; где формула вычисления ошибки гироскопа следующая: ω=ω+b+b, где ω - угловая скорость, выдаваемая гироскопом, ω - реальная угловая скорость гироскопа, b - смещение нуля гироскопа, и b - белый шум, выдаваемый гироскопом.
[0022] Интерференцию магнитного поля устраняют посредством калибровочной модели эллипса электронного компаса, где калибровочная модель эллипса электронного компаса такова: , где mx, my - интенсивности магнитного поля, Xoffset и Yoffset - жесткие магнитные интерференции Xsf и Ysf - мягкие магнитные интерференции.
[0023] Положение транспортного средства обновляется посредством семиразмерной модели расширенного фильтра Калмана EKF, где семиразмерная фильтрующая модель использует Расширенный Фильтр Калмана для семиразмерного вектора состояния, и Расширенный Фильтр Калмана EKF включает уравнение состояния и уравнение наблюдения:
ωr
0
1
2
3
ωr
mag
mag
a
T
[0024] Матрица состояния представлена: x=[q b], q - кватернион векторов q, q, q, q, и b - смещение нуля трехосевого гироскопа XYZ, где ω - угловая скорость гироскопа, w1 - матрица шума процесса, v1 - матрица шума наблюдения, y - вектор наблюдения, y=[ ψ], где а - это величина трехосевого ускорения, ψ - курсовой угол, вычисляемый электронным компасом,
.
[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
k
k
k
Р(+)=[I-KH]P(-)
k
k
k
k
k
k
[0040] В приведенной выше формуле: к - точка отсчета времени,  - оценка состояния системы, (-) - предыдущая точка отсчета времени, (+) - последующая точка отсчета времени, Ф - матрица перехода состояния, P - матрица минимума среднеквадратической ошибки, Q - ковариационная матрица, соответствующая вектору состояния, K - выигрыш ошибки, y - вектор наблюдения, H - переходная матрица для уравнения наблюдения, R - ковариационная матрица, соответствующая вектору наблюдения.
0
1
2
3
[0041] В приведенной выше формуле Q - вектор кватерниона, q, q, q, q - скаляры, формирующие вектор кватерниона, i, j, k - единичные вектора в трехмерной системе координат. Обновленная матрица положения представлена ниже:
[0042] В приведенной выше формуле  - матрица ротации для преобразования системы координат носителя в навигационную систему координат.
[0043] В приведенной выше формуле γ, θ, ψ - угол крена, угол наклона в продольной плоскости и курсовой угол соответственно.
[0044] В приведенной выше формуле, девятиосевой датчик на основе МЭМС составлен из трехосевого гироскопа, трехосевого акселерометра и трехосевого геомагнитного датчика.
[0045] Вариант реализации изобретения 2:
[0046] Как показано на Фиг. 2, способ обновления углового положения сельскохозяйственной машины, основанный на девятиосевом датчике на основе МЭМС, включает следующие этапы:
[0047] Этап S1: установки модели ошибок гироскопа, калибровочной модели эллипса электронного компаса и семиразмерной фильтрующей модели на основе расширенного фильтра Калмана (EKF), и определения векторов параметров, соответствующих динамическим положениям транспортного средства.
[0048] Этап S1, на котором происходит установка модели ошибок гироскопа, калибровочной модели эллипса электронного компаса и семиразмерной фильтрующей модели на основе расширенного фильтра Калмана (EKF), и задание векторов параметров, соответствующих динамическим положениям транспортного средства, подробно описан далее:
ib
ωr
ωg
ib
ωr
ωg
[0049] Угловую скорость гироскопа в модели ошибок гироскопа вычисляют через формулу вычисления ошибки гироскопа; где формула вычисления ошибки гироскопа следующая: ω=ω+b+b, где ω - угловая скорость, выдаваемая гироскопом, ω - реальная угловая скорость гироскопа, b - смещение нуля гироскопа, и b - белый шум, выдаваемый гироскопом.
[0050] Интерференцию магнитного поля устраняют посредством калибровочной модели эллипса электронного компаса, где калибровочная модель эллипса электронного компаса такова:  где mx, my - интенсивности магнитного поля, Xoffset и Yoffset - жесткие магнитные интерференции, Xsf и Ysf - мягкие магнитные интерференции.
[0051] Положение транспортного средства обновляют посредством семиразмерной модели Расширенного Фильтра Калмана EKF, где семиразмерная фильтрующая модель использует Расширенный Фильтр Калмана для семиразмерного вектора состояния, и Расширенный Фильтр Калмана EKF включает уравнение состояния и уравнение наблюдения:
ωr
0
1
2
ωr
mag
mag
a
T
[0052] Матрица состояния представлена: x=[q b], где q - кватернион векторов q, q, q, q3, b - смещение нуля трехосевого XYZ гироскопа. В формуле ω - угловая скорость гироскопа, w1 - матрица шума процесса, v1 - матрица шума наблюдения, y - вектор наблюдения, y=[ ψ], где а - это величина трехосевого ускорения, ψ - курсовой угол, вычисляемый электронным компасом,
.
[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
k
k
k
Р(+)=[I-KН]Р(-)
k
k
k
k
k
[0068] В приведенной выше формуле, k - точка отсчета времени,  - оценка состояния системы, (-) - предыдущая точка отсчета времени, (+) - последующая точка отсчета времени, Ф - матрица перехода состояния, Р - матрица минимума среднеквадратической ошибки, Q - ковариационная матрица, соответствующая вектору состояния, K - выигрыш ошибки, y - вектор наблюдения, H - переходная матрица для уравнения наблюдения, Rk - ковариационная матрица, соответствующая вектору наблюдения.
0
1
2
3
[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] Приведенное выше описание является лишь иллюстрацией конкретных воплощений настоящего изобретения, но объем защиты настоящего изобретения этим не ограничивается. Модификации или замены, легко понимаемые специалистом в данной области с обычной квалификацией, в пределах объема раскрытия настоящего изобретения, все должны попадать в объем защиты настоящего изобретения. Таким образом, объем защиты настоящего изобретения должен определяться прилагаемой формулой изобретения.