МЕТОД ДЛЯ ОЦЕНКИ ОРИЕНТАЦИИ, АППАРАТУРА И КОМПЬЮТЕРНЫЙ ПРОГРАММОНОСИТЕЛЬ Российский патент 2015 года по МПК E02F3/84 G01C19/5776 

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

Наиболее близким к заявляемому решению является решение, предложенное в европейской заявке на патент № ЕР 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 в качестве представления ориентации, вектор ускорения в проекциях на оси системы отсчета корпуса записывается как:

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

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

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

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

где χ обозначает вектор состояния системы, и $$\dot{\chi }$$ производную от χ по времени 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 = коэффициент усиления фильтра Калмана.

Символ $$\widehat{\chi }$$ обозначает оценку χ.

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

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

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

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

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

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

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

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

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

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

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

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

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

Чтобы уменьшить этот период времени, начальная оценка кватерниона вращения $$\stackrel{⌢}{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 измеряет угловую скорость $${\omega }_{x_1}$$ вдоль оси x₁; ГИР2 514 измеряет угловую скорость $${\omega }_{y_1}$$ вдоль оси y₁; ГИР3 516 измеряет угловую скорость $${\omega }_{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 измеряет угловую скорость $${\omega }_{x_2}$$ вдоль оси x₂; ГИР2 614 измеряет угловую скорость $${\omega }_{y_2}$$ вдоль оси y₂; ГИР3 616 измеряет угловую скорость $${\omega }_{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, в котором оценка ориентации отвала относительно корпуса рассчитывается из оценки кватерниона вращения $$\widehat{q}$$ (извлеченного из компонентов ).

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

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

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

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

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

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

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

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

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

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

где ориентация отвала относительно корпуса представлена в виде кватернион q в соответствии с (E8).

В этом случае,

Из (E17), χ_k+1=χ_k+w_k; и из (E18),

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

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

где ψ обозначает угол между осями x₁, и x₂. Из (E17), χ_k+1=χ_k+w_k; и из (E18),

Вариант реализации блока управления 140 показан на Рис. 8.

Блок управления 140 может быть установлен в кабине 102C (Рис. 1).

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

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

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

Блок управления 140 включает в себя вычислительное устройство 802, которое включает в себя процессор (именуемый центральным процессорным устройством (ЦПУ)) 804, оперативное запоминающее устройство (ОЗУ) 806, и устройство хранения данных 808.

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

Блок управления 140 также включает в себя интерфейс сети передачи данных 820, который соединяет вычислительное устройство 802 с сетью передачи данных 402.

Как описано выше со ссылкой на Рис. 4, блок управления 140 может обмениваться данными с БИИ1 120, БИИ2 130, и системой управления гидроцилиндрами 410 через сеть передачи данных 402.

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

Оператор может иметь доступ к вычислительному устройству 802 через удаленный терминал (не показан), соединенный с сетью передачи данных 402.

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

Блок управления 140 может также включать в себя интерфейс пользовательских устройств ввода/вывода 810, который соединяет вычислительное устройство 802 с пользовательскими устройствами ввода/вывода 812.

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

Данные, включая исполняемый машинный код, могут быть переданы к вычислительному устройству 802 и от него через интерфейс пользовательских устройств ввода/вывода 810.

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

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

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

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

Командные инструкции могут храниться в устройстве хранения данных 808 и загружаться в ОЗУ 806, когда требуется выполнение командных инструкций.

Алгоритм, отображенный схематически на Рис. 7, может быть определен командными инструкциями, хранящимися в ОЗУ 806 или в устройстве хранения данных 808 (или в комбинации ОЗУ 806 и устройства хранения данных 808), и реализовываться ЦПУ 804, выполняющим командные инструкции.

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

Соответственно, путем исполнения командных инструкций, ЦПУ 804 выполняет алгоритм, отображенный схематически на Рис. 7.

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

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

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

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

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

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

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

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

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

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

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

- получение значения ускорения корпуса машины от акселерометра, установленного на корпусе машины;

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

- расчет оценки первого вектора состояния, соответствующего первому моменту времени из множества моментов времени, причем:

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

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

2. Метод по п. 1, отличающийся тем, что дополнительно содержит шаг:

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

3. Метод по п. 1, отличающийся тем, что дополнительно содержит шаг:

- прогнозирования оценки второго вектора состояния, соответствующего второму моменту времени из множества моментов времени, причем:

- оценка второго вектора состояния основана, по крайней мере, на оценке первого вектора состояния и динамической модели системы; и

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

4. Метод по п. 1, отличающийся тем, что дополнительно содержит следующие шаги:

- расчет вектора измерений на основе, по крайней мере, значения ускорения корпуса машины и значения ускорения рабочего орудия; и

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

5. Метод по п. 1, отличающийся тем, что дополнительно содержит следующие шаги:

- получение значения угловой скорости корпуса машины от гироскопа, установленного на корпусе машины; и

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

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

6. Метод по п. 5, отличающийся тем, что дополнительно содержит следующие шаги:

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

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

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

- получение второго значения ускорения корпуса машины от второго акселерометра, установленного на корпусе машины; и

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

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

8. Метод по п. 7, отличающийся тем, что дополнительно содержит следующие шаги:

- расчет вектора измерений на основе, по крайней мере, первого значения ускорения корпуса машины, второго значения ускорения корпуса машины, первого значения ускорения рабочего орудия, второго значения ускорения рабочего орудия; и

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

9. Метод по п. 7, отличающийся тем, что дополнительно содержит следующие шаги:

- получение первого значения угловой скорости корпуса машины от первого гироскопа, установленного на корпусе машины;

- получение второго значения угловой скорости корпуса машины от второго гироскопа, установленного на корпусе машины;

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

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

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

10. Метод по п. 9, отличающийся тем, что дополнительно содержит следующие шаги:

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

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

11. Метод по п. 7, отличающийся тем, что дополнительно содержит следующие шаги:

- получение третьего значения ускорения корпуса машины от третьего акселерометра, установленного на корпусе машины; и

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

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

12. Метод по п. 11, отличающийся тем, что дополнительно содержит следующие шаги:

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

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

13. Метод по п. 11, отличающийся тем, что дополнительно содержит следующие шаги:

- получение первого значения угловой скорости корпуса машины от первого гироскопа, установленного на корпусе машины;

- получение второго значения угловой скорости корпуса машины от второго гироскопа, установленного на корпусе машины;

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

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

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

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

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

14. Метод по п. 13, отличающийся тем, что дополнительно содержит следующие шаги:

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

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

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

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

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

- средства для расчета оценки первого вектора состояния, соответствующего первому моменту времени из множества моментов времени, причем:

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

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

16. Аппаратура по п. 15, отличающаяся тем, что дополнительно содержит:

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

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

17. Аппаратура по п. 15, отличающаяся тем, что дополнительно содержит:

- средства для прогнозирования оценки второго вектора состояния, соответствующего второму моменту времени из множества моментов времени, причем:

- оценка второго вектора состояния основана, по крайней мере, на оценке первого вектора состояния и динамической модели системы; и

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

18. Аппаратура по п. 15, отличающаяся тем, что дополнительно содержит:

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

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

19. Аппаратура по п. 15, отличающаяся тем, что дополнительно содержит:

- средства для получения значения угловой скорости корпуса машины от гироскопа, установленного на корпусе машины; и

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

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

20. Аппаратура по п. 19, отличающаяся тем, что дополнительно содержит:

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

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

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

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

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

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

22. Аппаратура по п. 21, отличающаяся тем, что дополнительно содержит:

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

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

23. Аппаратура по п. 21, отличающаяся тем, что дополнительно содержит:

- средства для получения первого значения угловой скорости корпуса машины от первого гироскопа, установленного на корпусе машины;

- средства для получения второго значения угловой скорости корпуса машины от второго гироскопа, установленного на корпусе машины;

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

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

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

24. Аппаратура по п. 23, отличающаяся тем, что дополнительно содержит:

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

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

25. Аппаратура по п. 21, отличающаяся тем, что дополнительно содержит:

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

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

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

26. Аппаратура по п. 25, отличающаяся тем, что дополнительно содержит:

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

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

27. Аппаратура по п. 25, отличающаяся тем, что дополнительно содержит:

- средства для получения первого значения угловой скорости корпуса машины от первого гироскопа, установленного на корпусе машины;

- средства для получения второго значения угловой скорости корпуса машины от второго гироскопа, установленного на корпусе машины;

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

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

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

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

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

28. Аппаратура по п. 27, отличающаяся тем, что дополнительно содержит:

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

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

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

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

- получения значения ускорения корпуса машины от акселерометра, установленного на корпусе машины;

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

- расчета оценки первого вектора состояния, соответствующего первому моменту времени из множества моментов времени, причем:

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

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

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

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

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

- прогнозирования оценки второго вектора состояния, соответствующего второму моменту времени из множества моментов времени, причем:

- оценка второго вектора состояния основана, по крайней мере, на оценке первого вектора состояния и динамической модели системы; и

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

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

- расчет вектора измерений на основе, по крайней мере, значения ускорения корпуса машины и значения ускорения рабочего орудия; и

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

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

- получение значения угловой скорости корпуса машины от гироскопа, установленного на корпусе машины; и

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

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

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

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

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

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

- получение второго значения ускорения корпуса машины от второго акселерометра, установленного на корпусе машины; и

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

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

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

- расчет вектора измерений на основе, по крайней мере, первого значения ускорения корпуса машины, второго значения ускорения корпуса машины, первого значения ускорения рабочего орудия, второго значения ускорения рабочего орудия; и

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

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

- получение первого значения угловой скорости корпуса машины от первого гироскопа, установленного на корпусе машины;

- получение второго значения угловой скорости корпуса машины от второго гироскопа, установленного на корпусе машины;

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

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

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

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

- расчет вектора измерений на основе, по крайней мере, первого значения ускорения корпуса машины, второго значения ускорения корпуса машины, первого

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

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

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

- получение третьего значения ускорения корпуса машины от третьего акселерометра, установленного на корпусе машины; и

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

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

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

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

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

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

- получение первого значения угловой скорости корпуса машины от первого гироскопа, установленного на корпусе машины;

- получение второго значения угловой скорости корпуса машины от второго гироскопа, установленного на корпусе машины;

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

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

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

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

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

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

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

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

Изобретение относится к области управления рабочим орудием, присоединенным к корпусу машины, в частности к оценке ориентации и смещения рабочего орудия бульдозера относительно корпуса машины. Техническим результатом является повышение надежности и точности определения ориентации и смещения отвала бульдозера. Предложен метод для оценки ориентации из множества ориентаций управляемого рабочего орудия, присоединенного к корпусу машины, включающий следующие шаги: получение значения ускорения корпуса машины от акселерометра, установленного на корпусе машины; получение значения ускорения рабочего орудия от акселерометра, установленного на рабочем орудии; расчет оценки первого вектора состояния, соответствующего первому моменту времени из множества моментов времени, причем: оценка первого вектора состояния основана, по меньшей мере, на значении ускорения корпуса машины и значении ускорения рабочего орудия; и оценка первого вектора состояния включает векторное представление первой ориентации рабочего орудия относительно корпуса машины. Предложены также аппаратура для оценки ориентации рабочего оборудования и компьютерный программоноситель для осуществления указанного способа. 3 н. и 39 з.п. ф-лы, 9 ил.

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

2. Метод по п. 1, отличающийся тем, что дополнительно содержит шаг:
- вычисления оценки смещения рабочего орудия относительно корпуса машины на основе, по крайней мере, векторного представления первой ориентации рабочего орудия относительно корпуса машины.

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

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

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

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

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

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

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

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

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

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

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

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

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

16. Аппаратура по п. 15, отличающаяся тем, что дополнительно содержит:
- средства для вычисления оценки смещения рабочего орудия относительно корпуса машины на основе, по крайней мере, векторного представления первой ориентации рабочего орудия относительно корпуса машины.

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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