МЕТОД ДЛЯ ОЦЕНКИ ОРИЕНТАЦИИ, АППАРАТУРА И КОМПЬЮТЕРНЫЙ ПРОГРАММОНОСИТЕЛЬ

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