Метод для оценки ориентации, аппаратура и компьютерный программоноситель

Метод для оценки ориентации, аппаратура и компьютерный программоноситель

Иллюстрации

Показать все

Реферат

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

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

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

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

Пространственное положение отвала бульдозера может быть измерено, например, с использованием приемника Глобальной Навигационной Спутниковой Системы (ГНСС) или лазерной системы.

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

Ориентация отвала бульдозера может быть измерена, например, с использованием двух антенн ГНСС, двух лазерных призм или двух лазерных приемников.

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

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

Датчики могут быть установлены на отвале бульдозера.

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

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

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

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

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

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

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

Кроме этого, подвержен повреждению коаксиальный кабель, идущий от антенны ГНСС, установленной на отвале бульдозера, к навигационному приемнику, расположенному, как правило, в кабине бульдозера.

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

Оптимальным местоположением для установки датчиков положения, таких как антенна ГНСС, лазерная призма, или лазерный приемник, является крыша кабины.

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

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

Наиболее близким к заявляемому решению является решение, предложенное в европейской заявке на патент № ЕР 2187166 в которой раскрывается использование видеокамер, установленных на корпусе бульдозера, для оценки смещения отвала бульдозера относительно корпуса бульдозера путем слежения за специальными пассивными или активными объектами, установленными на отвале бульдозера.

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

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

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

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

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

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

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

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

Расположение датчика линейного перемещения внутри гидроцилиндра устраняет проблемы надежности.

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

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

СУЩЕСТВО ИЗОБРЕТЕНИЯ

Предлагаемое изобретение реализуется следующим образом.

Управляемое рабочее орудие присоединено к корпусу машины.

Рабочее орудие и корпус машины движутся с ускорением.

Ориентация рабочего орудия оценивается путем получения ускорения корпуса машины от акселерометра, установленного на корпусе машины, и ускорения рабочего орудия от акселерометра, установленного на рабочем орудии.

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

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

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

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

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

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

ОПИСАНИЕ РИСУНКОВ

На рис. 1 представлено схематическое изображение бульдозера.

На рис. 2А и 2Б представлены декартовые системы отсчета корпуса и рабочего орудия.

На рис. 3 представлена диаграмма векторов ускорений и угловых скоростей.

На рис. 4 представлена схема системы управления.

На рис. 5 представлена схема первого блока инерциальных измерений.

На рис. 6 представлена схема второго блока инерциальных измерений.

На рис. 7 представлена блок-схема метода оценки ориентации и смещения отвала бульдозера относительно корпуса бульдозера.

На рис. 8 представлена схема блока управления.

ОПИСАНИЕ ИЗОБРЕТЕНИЯ

На рис. 1 представлено схематическое изображение бульдозера 100, который включает в себя корпус 102 и отвал 108.

Корпус 102 включает в себя раму 102F и кабину 102C, в которой находится оператор.

Бульдозер 100 движется по поверхности Земли с помощью левой гусеницы 106L и правой гусеницы 106R.

Отвал 108 соединен с корпусом 102 с помощью левого рычага 110L и правого рычага 110R.

Отвал 108 может перемещаться относительно корпуса 102.

В общем случае, как смещение, так и ориентация (угловое положение) отвала 108 относительно корпуса 102 могут быть управляемыми.

Перемещение левого рычага 110L управляется левым гидроцилиндром 112L; перемещение правого рычага 110R управляется соответствующим правым гидроцилиндром (не показан).

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

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

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

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

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

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

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

В большинстве случаев, бульдозер оборудован неповоротным или поворотным отвалом.

Неповоротный отвал имеет две степени свободы: высота подъема отвала и перекос.

Высота подъема отвала регулируется в двух направлениях (вверх и вниз), перекос регулируется в двух направлениях (по часовой стрелке и против часовой стрелки).

Поворотный отвал имеет три степени свободы: высота подъема отвала, перекос и поворот.

Высота подъема отвала регулируется в двух направлениях (вверх и вниз), перекос регулируется в двух направлениях (по часовой стрелке и против часовой стрелки), поворот регулируется в двух направлениях (вправо и влево).

На практике, управляемые параметры отвала зависят от приложения.

Если приложение требует управления только наклоном отвала (относительно Земли; см. описание ниже), то достаточно иметь оценку ориентации отвала относительно корпуса бульдозера.

Если приложение требует управления как наклоном отвала, так и положением отвала (относительно Земли; см. описание ниже), то требуются обе оценки: ориентации отвала относительно корпуса, и смещения отвала относительно корпуса.

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

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

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

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

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

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

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

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

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

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

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

Опишем вначале систему с тремя относительными степенями свободы; системы с меньшим числом степеней свободы будут описаны далее.

На рис. 2 представлены две декартовых системы отсчета.

Система отсчета корпуса 121 связана с корпусом 102 и определена тремя ортогональными осями (рис. 2А): ось x₁ 221, ось y₁ 223, и ось z₁ 225.

Ось x₁ направлена вдоль оси крена корпуса 102; ось y₁ направлена вдоль оси тангажа корпуса 102; и ось z₁ направлена вдоль оси курса корпуса 102.

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

Система отсчета отвала 131 связана с отвалом 108 и определена тремя ортогональными осями (рис. 2Б): ось x₂ 231, ось y₂ 233, и ось z₂ 235.

Ось x₂ направлена вдоль оси крена отвала 108; ось y₂ направлена вдоль оси тангажа отвала 108; и ось z₂ направлена вдоль оси курса отвала 108.

Обратимся к векторной диаграмме на рис. 3.

Корпус 102 движется с вектором ускорения a ₁ 311 и вектором угловой скорости ω₁ 301.

Вектор ускорения а ₁ 311 представляет собой сумму вектора ускорения свободного падения g и вектора динамического ускорения корпуса бульдозера 102.

Вектор ускорения а ₁ 311 и вектор угловой скорости ω₁ 301 измеряется относительно системы отсчета корпуса 121.

Отвал 108 движется с вектором угловой скорости ω₂ 303, который измеряется относительно системы отсчета отвала 131.

Вектор ускорения отвала 108 описан ниже.

Вектор измерений системы включает в себя два ускорения:

- вектор ускорения корпуса в проекциях на оси системы отсчета корпуса:

- вектор ускорения отвала в проекциях на оси системы отсчета отвала:

и две угловые скорости:

- вектор угловой скорости корпуса в проекциях на оси системы отсчета корпуса:

- вектор угловой скорости отвала в проекциях на оси системы отсчета отвала:

Примечание: Знак транспонирования Т используется, потому что векторы являются столбцами.

Вектор измерений системы записывается как:

Вектор ускорения отвала a ₂ представляет собой сумму вектора ускорения корпуса a ₁ и вектора ускорения отвала относительно корпуса а ₂₁:

В процессе земляных работ, ускорение отвала относительно корпуса а ₂₁, обычно значительно меньше ускорения корпуса; следовательно, можно предположить, что ускорение отвала относительно корпуса приблизительно равно ускорению корпуса:

Ориентация отвала относительно корпуса может быть представлена в одной из трех следующих форм:

- три угла Эйлера: крен, тангаж и курс (ϕ, θ, ψ);

- матрица направляющих косинусов А;

- кватернион вращения

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

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

Используя кватернион вращения q в качестве представления ориентации, вектор ускорения в проекциях на оси системы отсчета корпуса записывается как:

Вектор угловой скорости отвала в проекциях на оси системы отсчета корпуса записывается как:

Вектор угловой скорости отвала относительно корпуса в проекциях на оси системы отсчета корпуса записывается как:

Система описывается динамической моделью во временной области.

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

где χ обозначает вектор состояния системы, и χ ˙ производную от χ по времени t.

Если вектор состояния системы χ(t) в момент t известен, вектор состояния системы в момент времени t+Δt, где Δt период дискретизации, может быть спрогнозирован с использованием (Е12) как:

В дискретные моменты времени t₀, t₁, …, t_k=t₀, t₀+Δt, …, t₀+kΔt, где k целое число, (E13) может быть выражено в виде следующего дифференциального уравнения:

(E14) может быть переписано в более компактном виде как:

Если Δt стремится к нулю, то:

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

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

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

где:

χ = вектор состояния системы;

w = вектор шумов процесса;

Z = вектор измерения;

ν = вектор шумов измерений; и

h = функция измерений.

Вектор шумов процесса w имеет ковариационную матрицу Q; и вектор шумов измерений ν имеет ковариационную матрицу R.

Для оценки вектора состояния системы χ из вектора измерений Z, используется следующая процедура расширенного фильтра Калмана.

В момент времени k рассчитываются следующие матрицы Якоби:

Оценка вектора состояния корректируется новым вектором измерения Z_k как:

где:

P = ковариационная матрица оценки вектора состояния; и

K = коэффициент усиления фильтра Калмана.

Символ χ ^ обозначает оценку χ.

Затем, новая оценка вектора состояния прогнозируется с использованием уравнений системы:

Вернемся к проблеме расчета оценки кватерниона вращения.

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

(E26) является специфическим случаем общего уравнения (E12).

Для расчета оценки кватерниона вращения q ⌢ используется следующая процедура расширенного фильтра Калмана.

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

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

Вектор состояния системы записывается как:

После того как вектор состояния определен, кватернион извлекается из компонентов вектора состояния.

Для процедуры расширенного фильтра Калмана необходима начальная оценка вектора состояния при запуске системы.

Вектор состояния может быть инициализирован единичным кватернионом вращения, .

Единичный кватернион вращения соответствует ориентации, при которой оси системы отсчета отвала 131 направлены параллельно осям системы отсчета корпуса 121.

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

Чтобы уменьшить этот период времени, начальная оценка кватерниона вращения q ⌢ может быть получена с использованием ковариации между ускорением отвала a 2 2 , полученного из вектора измерения, и ускорением отвала, рассчитанного из ускорения корпуса a 1 1 и кватерниона вращения q:

где E(a) обозначает среднее значение а, рассчитанное на интервале времени, определенным пользователем.

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

Эти условия могут быть обнаружены с помощью акселерометров и гироскопов блоков инерциальных измерений.

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

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

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

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

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

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

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

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

Для измерения углов тангажа и крена отвала относительно корпуса используется реакция акселерометров на силу тяжести, а для измерения угла курса отвала относительно корпуса используется реакция акселерометров на динамические движения.

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

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

Гироскопы отфильтровывают паразитные шумы и повышают точность.

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

Объединение акселерометров и гироскопов в БИИ является предпочтительным.

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

БИИ может быть размещен в компактном, надежном корпусе, который может быть установлен непосредственно на отвале бульдозера, и нет необходимости в дополнительных опорах.

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

Кабели передачи данных и электропитания, идущие к БИИ на отвале, являются более надежными, чем коаксиальные кабели, идущие к антенне ГНСС, установленной на отвале.

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

Обратимся к Рис. 1.

БИИ1 120 устанавливается на корпусе 102 (обычно в кабине 102C), и БИИ2 130 устанавливается на отвале 108 (обычно на задней стороне отвала 108).

Если кабина 102C расположена на подвеске, БИИ1 120 должен быть установлен на раме 102F, чтобы избежать паразитных влияний вибрации кабины на БИИ1 120.

БИИ1 120 и БИИ2 130 обмениваются данными с блоком управления 140 (обычно расположенного в кабине 102C) через сеть передачи данных 402 (см. Рис. 4).

Данные могут передаваться, например, через проводные или беспроводные соединения.

Система управления гидроцилиндрами 410 обменивается с блоком управления 140 через сеть передачи данных 402.

Система управления гидроцилиндрами 410 управляет гидроцилиндрами, которые изменяют смещение и ориентацию отвала бульдозера 108.

На рис. 5 представлена схема БИИ1 120.

Он содержит три акселерометра (АКС1 502, АКС2 504 и АКС3 506) и три гироскопа (ГИР1 512, ГИР2 514 и ГИР3 516).

Обратимся к Рис. 2А.

АКС1 502 измеряет ускорение a x 1 вдоль оси x₁ 221; АКС2 504 измеряет ускорение a y 1 вдоль оси y₁ 223; АКС3 506 измеряет ускорение a z 1 вдоль оси z₁, 225. ГИР1 512 измеряет угловую скорость ω x 1 вдоль оси x₁; ГИР2 514 измеряет угловую скорость ω y 1 вдоль оси y₁; ГИР3 516 измеряет угловую скорость ω z 1 вдоль оси z₁.

Вернемся к Рис. 5.

Выходные сигналы акселерометров и гироскопов передаются на вход аналого-цифрового преобразователя (АЦП) 520.

Выходные сигналы АЦП 520 обрабатываются центральным процессорным устройством (ЦПУ) 522, которое передает поток данных через интерфейс сети передачи данных 524 в сеть передачи данных 402.

Поток данных включает в себя вектор ускорения

и вектор угловой скорости .

На рис. 6 представлена схема БИИ2 130.

Он содержит три акселерометра (АКС1 602, АКС2 604 и АКС3 606) и три гироскопа (ГИР1 612, ГИР2 614 и ГИР3 616).

Обратимся к Рис. 2Б.

АКС1 602 измеряет ускорение a x 2 вдоль оси x₂ 231; АКС2 604 измеряет ускорение a y 2 вдоль оси y₂ 233; АКС3 606 измеряет ускорение a z 2 вдоль оси z₂ 235. ГИР1 612 измеряет угловую скорость ω x 2 вдоль оси x₂; ГИР2 614 измеряет угловую скорость ω y 2 вдоль оси y₂; ГИР3 616 измеряет угловую скорость ω z 2 вдоль оси z₂.

Вернемся к Рис. 6.

Выходные сигналы акселерометров и гироскопов передаются на вход аналого-цифрового преобразователя (АЦП) 620.

Выходные сигналы АЦП 620 обрабатываются центральным процессорным устройством (ЦПУ) 622, которое передает поток данных через интерфейс сети передачи данных 624 в сеть передачи данных 402.

Поток данных включает в себя вектор ускорения

и вектор угловой скорости

.

На рис. 7 представлена блок-схема метода, в соответствии с воплощением изобретения, для оценки ориентации и смещения отвала бульдозера относительно корпуса бульдозера.

Обратимся к Рис. 4.

В общем случае, БИИ1 120 выдает измерения в дискретные моменты времени относительно системных часов БИИ1 120; БИИ2 130 выдает измерения в дискретные моменты времени относительно системных часов БИИ2 130; блок управления 140 обрабатывает измерения в дискретные моменты времени относительно системных часов блока управления 140.

Интервалы времени между дискретными моментами времени называются периодами дискретизации.

Периоды дискретизации для БИИ1 120, БИИ2 130 и блока управления 140 могут быть различными или могут быть одинаковыми.

Системные часы БИИ1 120, БИИ2 130, и блока управления 140 могут идти асинхронно, или могут быть синхронизированы с общим системным временем.

Если системные часы в БИИ1 120 и системные часы в БИИ2 130 идут асинхронно, то частота дискретизации каждого БИИ должна быть достаточно высока, так чтобы разница во времени между дискретными моментами времени разных БИИ была не больше чем определенное значение, зависящее от требуемой точности оценки относительной ориентации; например, частота дискретизации 100 Гц обеспечивает достаточную точность для большинства приложений.

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

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

На шаге 702, принимаются измерения вектора ускорения корпуса, , и вектора угловой скорости корпуса, . На шаге 704, принимаются измерения вектора ускорения отвала, , и вектора угловой скорости отвала, .

На практике, в зависимости от конфигурации синхронизации времени, измерения с БИИ1 120 могут быть получены раньше, позже, или в одно время с измерениями с БИИ2 130.

Затем процесс переходит к шагу 706, в котором оценка вектора состояния корректируется новыми измерениями.

Затем процесс переходит к шагу 708, в котором оценка нового вектора состояния

в следующий момент времени прогнозируется с использованием (E26).

Затем процесс переходит к шагу 710, в котором оценка ориентации отвала относительно корпуса рассчитывается из оценки кватерниона вращения q ^ (извлеченного из компонентов ).

Затем процесс переходит к шагу 712, в котором рассчитывается оценка смещения отвала относительно корпуса.

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

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

Примерами параметров геометрии могут быть (см. Рис. 1): размеры отвала 108; размеры левого рычага 110L; размеры правого рычага 11 OR; позиция, в которой левый рычаг 110L присоединяется к отвалу 108; позиция, в которой левый рычаг 110L присоединяется к раме 102F; позиция, в которой правый рычаг 110R присоединяется к отвалу 108; позиция, в которой правый рычаг 110R присоединяется к раме 102F.

Как описано ранее, гироскопы отфильтровывают паразитные шумы и повышают точность.

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

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

Для отвала бульдозера с тремя угловыми степенями свободы, вектор измерения для момента времени t_k, задается как

В (E30), первый элемент вектора "1" относится к квадрату нормы кватерниона; см. (E8).

Вектор состояния для момента врем