СПОСОБ ОЦЕНКИ ПОЛОЖЕНИЯ УСТРОЙСТВА УПРАВЛЕНИЯ РАБОТОЙ МАШИН

1. Способ оценки положения устройства управления (10), предназначенного для управления рабочими машинами (20), причем устройство управления (10) содержит средства для управления перемещением рабочей машины (20) вдоль соответствующих направлений, содержащий этапы, на которых:
- устанавливают заданный диапазон значений магнитного поля в отношении магнитного поля Земли,
- выполняют предварительную оценку положения устройства управления (10) с использованием данных акселерометра (34) и магнитометра (38), установленных в устройстве управления (10);
- обновляют предварительную оценку положения устройства управления (10) с использованием данных гироскопа (36), установленного в устройстве управления (10); и
- если измеренные магнитометром (38) значения магнитного поля находятся вне заданного диапазона значений магнитного поля, результаты измерений магнитометра (38) не учитывают на этапе предварительной оценки положения устройства управления.
2. Способ по п. 1, в котором предварительную оценку положения устройства управления (10) выполняют путем оценки кватерниона представляющего указанное положение, при этом кватернион оценивают путем минимизации функции, коррелирующей кватернион с опорным вектором или с вектором результатов измерений, полученных от акселерометра (34) и магнитометра (38).
3. Способ по п. 2, в котором минимизацию выполняют с применением алгоритма градиентного спуска.
4. Способ по п. 2, в котором обновляют предварительную оценку положения устройства управления (10) с использованием данных гироскопа (36) посредством расширенного фильтра Калмана, что позволяет оценить кватернион , описывающий положение устройства управления (10).
5. Способ по п. 2, в котором расширенный фильтр Калмана определяют следующими уравнениями:
где вектор x образован составляющими кватерниона описывающего положение устройства управления (10), параметр состоит из результатов измерений гироскопа (36), H - единичная матрица 4X4, и представляют соответственно шумы процесса и шумы измерений, дисперсии которых определяются соответственно матрицей Q дисперсии шумов и матрицей R дисперсии измерений фильтра Калмана, а переходная функция основана на уравнении , которое коррелирует величины углов ω положения, измеренных гироскопом, с производной кватерниона .
6. Способ по п. 2, в котором применяют корректирующие средства к результатам измерений магнитометра (38).
7. Способ по п. 6, в котором, если измеренные магнитометром (38) значения магнитного поля содержатся в заданном диапазоне в отношении магнитного поля Земли, матрицы Q и R фильтра Калмана изменяют линейно, для того чтобы обеспечить более достоверную предварительную оценку от магнитометра (38).
8. Способ по п. 6, в котором, если измеренные магнитометром (38) значения магнитного поля не содержатся в заданном диапазоне в отношении магнитного поля Земли, матрицы Q и R фильтра Калмана изменяют линейно, для того чтобы обеспечить более достоверную оценку от гироскопа (36).
9. Способ по п. 6, в котором курсовой угол устройства управления (10) вычисляют по следующей формуле:
где q0, q1, q2 и q3 обозначают составляющие кватерниона , оцененного посредством фильтра Калмана.
10. Способ управления рабочей машиной в соответствии с различными направлениями перемещения с использованием устройства управления (10), включающий этапы, на которых:
- оценивают курсовой угол устройства управления (10) согласно способу по любому из пп. 1 – 9;
- переназначают функции управления устройства управления (10) в зависимости от курсового угла устройства управления (10).