Питер Корк

Машинное зрение и управление
в робототехнике
Фундаментальные алгоритмы на Python

Peter Corke

Robotics, Vision
and Control
Fundamental Algorithms
in Python
3rd edition 2023

Питер Корк

Машинное зрение
и управление
в робототехнике
Фундаментальные алгоритмы
на Python
Третье издание

Москва, 2026

УДК 681.5:004.93
ББК 32.816
К66

К66

Корк П.
Машинное зрение и управление в робототехнике / пер. с англ. А. Н. Киселева. – М.: ДМК Пресс, 2026. – 1100 с.: ил.
ISBN 978-5-93700-263-1
Данная книга представляет собой фундаментальное и исчерпывающее введение
в компьютерное зрение и робототехнику. В ней рассмотрен широкий круг тем, включая цветное зрение, передовые методы сегментации, деформацию изображения,
стереозрение, оценку местоположения, глубокое обучение, групповую коррекцию
и визуальную одометрию. Уникальной особенностью является объединение всех
ключевых концепций компьютерного зрения и управления роботами в одном томе.
Материал книги сопровождается примерами на основе портированного на Python
популярного пакета Robotics Toolbox for MATLAB®, что позволяет закрепить знания
при помощи нескольких строк кода.
Издание будет полезно читателям с разным уровнем подготовки – от студентов
и аспирантов, изучающих робототехнику, до опытных инженеров и исследователей,
желающих углубить и систематизировать свои знания.

УДК 681.5:004.93
ББК 32.816

First published in English under the title Robotics, Vision and Control; Fundamental Algorithms
in Python by Peter Corke, edition 3.
This edition has been translated and published under licence from Springer Nature Switzerland AG.
Springer Nature Switzerland AG takes no responsibility and shall not be made liable for the
accuracy of the translation.
Все права защищены. Любая часть этой книги не может быть воспроизведена в какой
бы то ни было форме и какими бы то ни было средствами без письменного разрешения владельцев авторских прав.

ISBN 978-3-031-06468-5 (англ.)	Copyright © Peter Corke, under exclusive license to
Springer Nature Switzerland AG, 2023
ISBN 978-5-93700-263-1 (рус.)
© Перевод, оформление, издание, ДМК Пресс, 2026

Моей семье – Филиппе, Люси и Мадлен –
за их снисходительность и поддержку;
моим родителям Маргарет и Дэвиду
за то, что разожгли мое любопытство;
и Ричарду (Лу) Полу, заронившему семя,
из которого выросла эта книга

Содержание

Вступительное слово.................................................................................................. 16
Предисловие. ................................................................................................................. 18
Специальные обозначения. .................................................................................... 26
1

Введение............................................................................................................... 33
1.1
1.2
1.3
1.4
1.5
1.6

2

Краткая история роботов. ....................................................................... 33
Типы роботов.............................................................................................. 36
Определение робота................................................................................. 42
Зрение роботов. ......................................................................................... 42
Этические аспекты робототехники. ..................................................... 45
О книге......................................................................................................... 47
1.6.1 Python и книга. ............................................................................... 48
1.6.2 Условные обозначения, соглашения и структура книги....... 50
1.6.3 Целевая аудитория......................................................................... 52
1.6.4 Обучение с помощью книги........................................................ 53
1.6.5 Преподавание с помощью книги............................................... 53
1.6.6 Структура книги............................................................................. 54
1.6.7 Дополнительное чтение............................................................... 57

Часть I
Основы............................................................................................ 59
Представление местоположения и ориентации................................ 60
2.1

2.2
2.3
2.4

Основы......................................................................................................... 60
2.1.1 Относительное положение. ......................................................... 60
2.1.2 Системы координат....................................................................... 64
2.1.3 Графы положений. ......................................................................... 66
2.1.4 Краткое резюме.............................................................................. 68
Работа в двух измерениях (2D). ............................................................. 71
2.2.1 Ориентация в двух измерениях. ................................................ 71
2.2.2 Положение в двух измерениях. .................................................. 76
Работа в трех измерениях (3D)............................................................... 86
2.3.1 Ориентация в трех измерениях.................................................. 87
2.3.2 Положение в трех измерениях...................................................109
Дополнительные темы............................................................................117
2.4.1 Прямой и обратный порядок умножения
преобразований.............................................................................117

Содержание 7

2.5
2.6

3

Время и движение...........................................................................................144
3.1

3.2

3.3

3.4

3.5

4

2.4.2 Поворот системы координат и поворот точки.......................118
2.4.3 Матрица направляющих косинусов.........................................119
2.4.4 Эффективность представления.................................................119
2.4.5 Расстояние между ориентациями.............................................120
2.4.6 Нормализация................................................................................121
2.4.7 Понятие экспоненциального отображения............................122
2.4.8 Подробнее о кручениях. ..............................................................125
2.4.9 Конфигурационное пространство. ...........................................131
Использование Toolbox...........................................................................133
Подведение итогов...................................................................................137
2.6.1 Дополнительное чтение..............................................................139
2.6.2 Упражнения....................................................................................141

Изменение положения во времени......................................................144
3.1.1 Скорость изменения ориентации.............................................145
3.1.2 Скорость изменения положения...............................................146
3.1.3 Преобразование пространственных скоростей.....................147
3.1.4 Вращение, задаваемое в приращениях...................................149
3.1.5 Движение твердого тела, задаваемое в приращениях.........152
Ускорение тел и системы отсчета.........................................................153
3.2.1 Динамика движущихся тел.........................................................153
3.2.2 Преобразование сил и моментов..............................................155
3.2.3 Инерциальная система координат. ..........................................156
Создание положения, изменяющегося во времени.........................158
3.3.1 Гладкие одномерные траектории. ............................................158
3.3.2 Многокоординатные траектории..............................................162
3.3.3 Многосегментные траектории...................................................163
3.3.4 Интерполяция ориентации в 3D. ..............................................165
3.3.5 Декартово движение в 3D............................................................168
Применение: инерциальная навигация.............................................170
3.4.1 Гироскопы.......................................................................................171
3.4.2 Акселерометры..............................................................................175
3.4.3 Магнитометры...............................................................................180
3.4.4 Слияние инерциальных датчиков. ...........................................184
Подведение итогов...................................................................................188
3.5.1 Дополнительное чтение..............................................................189
3.5.2 Упражнения....................................................................................190

Часть II Мобильная робототехника...................................................193
Мобильные роботы. .......................................................................................195
4.1

Колесные мобильные роботы................................................................197
4.1.1 Автомобили....................................................................................198
4.1.2 Автомобиль с дифференциальным управлением.................212
4.1.3 Всенаправленное транспортное средство. .............................215

8 Содержание

4.2
4.3
4.4

5

Навигация............................................................................................................238
5.1
5.2
5.3

5.4
5.5
5.6

5.7

5.8

6

Воздушные роботы. .................................................................................218
Дополнительные темы............................................................................227
4.3.1 Неголономные и малоприводные системы............................227
Подведение итогов...................................................................................231
4.4.1 Дополнительное чтение..............................................................232
4.4.2 Упражнения....................................................................................235

Введение в реактивную навигацию.....................................................240
5.1.1 Тележки Брайтенберга.................................................................241
5.1.2 Простые автоматы........................................................................244
Введение в навигацию по картам. .......................................................246
Планирование маршрута с использованием графической
карты. ..........................................................................................................249
5.3.1 Планирование маршрута с минимальным временем.........259
5.3.2 Подведение итогов. ......................................................................260
Планирование маршрута с использованием карты занятости.....261
5.4.1 Преобразование расстояния.......................................................261
5.4.2 Алгоритм D*. ..................................................................................268
Планирование маршрута с использованием дорожных карт. ......271
Планирование проходимого маршрута..............................................277
5.6.1 Планировщик пути Дубинса.......................................................278
5.6.2 Планировщик пути Ридса–Шеппа............................................279
5.6.3 Планирование по решетке..........................................................280
5.6.4 Криволинейные полиномы. .......................................................285
5.6.5 Планирование маршрута в конфигурационном
пространстве..................................................................................287
Дополнительные темы............................................................................292
5.7.1 Алгоритмы поиска A* и Дейкстры. ...........................................292
5.7.2 Преобразование карт сеток занятости в графы.....................293
5.7.3 Преобразование между графами и мат­ри­ца­ми.....................293
5.7.4 Локальное и глобальное планирование. .................................293
Подведение итогов...................................................................................294
5.8.1 Дополнительное чтение..............................................................295
5.8.2 Ресурсы............................................................................................297
5.8.3 Упражнения....................................................................................298

Определение местоположения и картографирование..................301
6.1
6.2
6.3
6.4
6.5
6.6

Расчет пути с использованием одометрии........................................307
6.1.1 Моделирование робота................................................................308
6.1.2 Оценка положения........................................................................311
Локализация с помощью ориентиров на карте................................317
Создание карты ориентиров. ................................................................326
Одновременные локализация и картографирование. ....................330
SLAM на основе графа положений.......................................................335
Последовательная локализация методом Монте-Карло. ...............342

Содержание 9

6.7
6.8

6.9

7

Часть III Роботы-манипуляторы. ..........................................................365
Кинематика робота-манипулятора.........................................................367
7.1

7.2

7.3

7.4
7.5

7.6

8

SLAM на основе теоремы Рао–Блэквелла...........................................349
Применение: лидар..................................................................................350
6.8.1 Одометрия на основе лидара.....................................................352
6.8.2 Картографирование на основе лидара. ...................................354
6.8.3 Локализация на основе лидара..................................................355
Подведение итогов...................................................................................356
6.9.1 Дополнительное чтение..............................................................358
6.9.2 Упражнения....................................................................................362

Прямая кинематика.................................................................................369
7.1.1 Прямая кинематика на основе графа положений.................370
7.1.2 Прямая кинематика как цепочка звеньев робота.................378
7.1.3 Многорукие роботы......................................................................387
7.1.4 Унифицированный формат описания роботов (URDF).......390
7.1.5 Параметры Денавита–Хартенберга..........................................395
Обратная кинематика. ............................................................................399
7.2.1 Двухмерные (планарные) роботизированные
манипуляторы................................................................................400
7.2.2 Трехмерные роботизированные манипуляторы...................403
7.2.3 Малоприводный манипулятор. .................................................407
7.2.4 Многоприводный манипулятор................................................409
Траектории.................................................................................................409
7.3.1 Движение в пространстве сочленений....................................410
7.3.2 Движение в декартовом пространстве....................................412
7.3.3 Представление кинематики в виде блок-схемы....................414
7.3.4 Движение через сингулярность.................................................414
7.3.5 Изменение конфигурации..........................................................416
Применение...............................................................................................417
7.4.1 Создание надписи на поверхности...........................................417
7.4.2 Четвероногий шагающий робот................................................419
Дополнительные темы............................................................................424
7.5.2 Создание кинематической модели робота.............................425
7.5.3 Модифицированные параметры Денавита–Хартенберга....427
7.5.4 Произведение экспонент............................................................429
7.5.5 Определение столкновений. ......................................................432
Подведение итогов...................................................................................434
7.6.1 Дополнительное чтение..............................................................434
7.6.2 Упражнения....................................................................................436

Скорость манипулятора................................................................................438
8.1

Якобиан манипулятора...........................................................................438
8.1.1 Якобиан в мировой системе координат..................................439
8.1.2 Якобиан в системе координат рабочего органа.....................442

10 Содержание

8.2
8.3

8.4

8.5
8.6

8.7

9

8.1.3 Аналитический якобиан..............................................................443
Применение: управление движением с заданной скоростью. .....445
Обусловленность якобиана и манипулируемость............................448
8.3.1 Вырожденные якобианы.............................................................448
8.3.2 Эллипсоид скорости и манипулируемость.............................450
8.3.3 Работа с вырожденным якобианом..........................................454
8.3.4 Работа с неквадратным якобианом..........................................455
Отношения сил. ........................................................................................461
8.4.1 Преобразование динамических винтов в пространство
сочленений. ....................................................................................461
8.4.2 Эллипсоиды сил.............................................................................462
Численная обратная кинематика.........................................................463
Дополнительные темы............................................................................465
8.6.1 Якобиан манипулируемости. .....................................................465
8.6.2 Вычисление якобиана манипулятора с использованием
кручений. ........................................................................................465
8.6.3 Манипулируемость, масштабирование
и единицы измерения..................................................................466
Подведение итогов...................................................................................467
8.7.1 Дополнительное чтение..............................................................468
8.7.2 Упражнения....................................................................................468

Динамика и управление...............................................................................472
9.1

9.2

9.3
9.4
9.5
9.6

Независимое управление сочленениями...........................................473
9.1.1 Приводы. .........................................................................................473
9.1.2 Трение..............................................................................................475
9.1.3 Масса звена.....................................................................................477
9.1.4 Редуктор. .........................................................................................479
9.1.5 Моделирование сочленения робота.........................................481
9.1.6 Контур управления скоростью...................................................483
9.1.7 Контур управления положением...............................................489
9.1.8 Подведение итогов. ......................................................................491
Уравнения движения твердого тела.....................................................492
9.2.1 Член силы тяжести........................................................................494
9.2.2 Матрица инерции. ........................................................................497
9.2.3 Трение..............................................................................................498
9.2.4 Матрица Кориолиса и центростремительной связи............. 499
9.2.5 Влияние полезной нагрузки.......................................................499
9.2.6 Воздействие на основание..........................................................501
9.2.7 Динамическая манипулируемость...........................................501
Прямая динамика.....................................................................................503
Компенсация динамики твердого тела...............................................505
9.4.1 Управление с прямой связью.....................................................506
9.4.2 Регулятор с вычисляемым крутящим моментом..................507
Динамика пространства задач и управление....................................509
Применение...............................................................................................516

Содержание 11

9.7

10

9.6.1 Управление в операционном пространстве...........................516
9.6.2 Приводы переменной жесткости..............................................519
Подведение итогов...................................................................................521
9.7.1 Дополнительное чтение..............................................................522
9.7.2 Упражнения....................................................................................524

Часть IV Компьютерное зрение............................................................529
Свет и цвет..........................................................................................................531
10.1 Спектральное представление света.....................................................531
10.1.1 Поглощение.................................................................................535
10.1.2 Отражение...................................................................................536
10.1.3 Яркость.........................................................................................536
10.2 Цвет..............................................................................................................538
10.2.1 Человеческий глаз. ....................................................................538
10.2.2 Сенсор в цифровой камере......................................................542
10.2.3 Измерение цвета........................................................................543
10.2.4 Воспроизведение цветов. ........................................................545
10.2.5 Цветовое пространство............................................................549
10.2.6 Названия цветов. .......................................................................554
10.2.7 Другие цветовые и хроматические пространства.............555
10.2.8 Преобразование между различными основными
цветами........................................................................................559
10.2.9 Что такое белый цвет?..............................................................562
10.3 Дополнительные темы............................................................................562
10.3.1 Цветовая температура..............................................................563
10.3.2 Постоянство цвета.....................................................................563
10.3.3 Баланс белого..............................................................................564
10.3.4 Изменение цвета из-за поглощения.....................................566
10.3.5 Двухцветное отражение...........................................................568
10.3.6 Гамма............................................................................................569
10.4 Применение: цветное изображение....................................................571
10.4.1 Сравнение цветовых пространств.........................................571
10.4.2 Удаление теней...........................................................................572
10.5 Подведение итогов...................................................................................576
10.5.1 Дополнительное чтение...........................................................577
10.5.2 Источники данных. ...................................................................578
10.5.3 Упражнения.................................................................................579

11

Изображения и их обработка....................................................................582
11.1 Получение изображения.........................................................................583
11.1.1 Изображения из файлов...........................................................583
11.1.2 Изображения из последовательностей файлов..................591
11.1.3 Изображения со встроенной камеры....................................592
11.1.4 Изображения из видеофайла..................................................592
11.1.5 Изображения из интернета.....................................................596

12 Содержание

11.2
11.3
11.4
11.5

11.6

11.7

11.8

12

11.1.6 Снимки из космоса....................................................................597
11.1.7 Изображения из кода................................................................598
Гистограммы изображений....................................................................600
Монадические операции........................................................................602
Диадические операции...........................................................................607
11.4.1 Применения................................................................................609
Пространственные операции................................................................614
11.5.1 Линейная пространственная фильтрация...........................614
11.5.2 Сравнение с шаблоном.............................................................631
11.5.3 Нелинейные операции.............................................................638
Математическая морфология................................................................640
11.6.1 Удаление шума...........................................................................645
11.6.2 Обнаружение границы. ............................................................646
11.6.3 Преобразование «попадание или промах». ........................647
11.6.4 Преобразование расстояния...................................................649
Изменение формы. ..................................................................................651
11.7.1 Обрезка.........................................................................................651
11.7.2 Изменение размера изображения.........................................652
11.7.3 Пирамиды изображений..........................................................654
11.7.4 Деформация изображения. .....................................................655
Подведение итогов...................................................................................659
11.8.1 Дополнительное чтение...........................................................660
11.8.2 Источники изображений. ........................................................662
11.8.3 Программные инструменты. ..................................................662
11.8.4 Упражнения.................................................................................662

Извлечение признаков изображения.....................................................665
12.1 Получение признаков из области.........................................................666
12.1.1 Классификация пикселей. .......................................................668
12.1.2 Представление экземпляра объекта. ....................................681
12.1.3 Описание экземпляра объекта...............................................686
12.1.4 Обнаружение объектов с использованием глубокого
обучения. .....................................................................................701
12.1.5 Итоги.............................................................................................703
12.2 Получение признаков из линий............................................................705
12.2.1 Итоги.............................................................................................710
12.3 Получение признаков из точек.............................................................710
12.3.1 Классические детекторы углов...............................................711
12.3.2 Угловые детекторы в масштабируемом
пространстве...............................................................................718
12.4 Применения...............................................................................................725
12.4.1 Распознавание символов.........................................................725
12.4.2 Поиск изображений. .................................................................727
12.5 Подведение итогов...................................................................................735
12.5.1 Дополнительное чтение...........................................................736
12.5.2 Упражнения.................................................................................740

Содержание 13

13

Формирование изображения. ...................................................................743
13.1 Перспективная камера............................................................................744
13.1.1 Перспективная проекция. .......................................................744
13.1.2 Моделирование перспективной камеры.............................748
13.1.3 Дискретная плоскость изображения.....................................750
13.1.4 Матрица камеры........................................................................753
13.1.5 Точки проекции..........................................................................755
13.1.6 Дисторсия объектива................................................................758
13.2 Калибровка камеры. ................................................................................761
13.2.1 Метод с использованием трехмерной мишени..................761
13.2.2 Калибровка с использованием шахматной доски.............764
13.2.3 Разложение калибровочной мат­ри­цы камеры...................767
13.2.4 Определение положения откалиброванной камеры........770
13.3 Широкоугольные камеры.......................................................................771
13.3.1 Камера с объективом «рыбий глаз».......................................772
13.3.2 Катадиоптрическая камера.....................................................775
13.3.3 Сферическая камера. ................................................................779
13.4 Унифицированная модель формирования изображений..............781
13.4.1 Сопоставление широкоугольных изображений
со сферой. ....................................................................................783
13.4.2 Преобразование сферического изображения
в перспективное.........................................................................786
13.5 Новые типы камер....................................................................................788
13.5.1 Многокамерные массивы........................................................788
13.5.2 Камеры светового поля............................................................789
13.6 Применения...............................................................................................792
13.6.1 Координатные метки................................................................792
13.6.2 Планарная гомография. ...........................................................793
13.7 Дополнительные темы............................................................................796
13.7.1 Проецирование произвольных линий и поверхностей
в трехмерном пространстве....................................................796
13.7.2 Неперспективные камеры.......................................................798
13.8 Подведение итогов...................................................................................800
13.8.1 Дополнительное чтение и ресурсы.......................................801
13.8.2 Упражнения.................................................................................804

14

Использование нескольких изображений...........................................806
14.1 Cовмещение признаков..........................................................................808
14.2 Геометрия нескольких ракурсов...........................................................814
14.2.1 Фундаментальная мат­ри­ца.....................................................817
14.2.2 Существенная мат­ри­ца. ...........................................................820
14.2.3 Вычисление фундаментальной мат­ри­цы по данным
реального изображения. ..........................................................822
14.2.4 Планарная гомография. ...........................................................828
14.3 Разреженное стереозрение....................................................................835
14.3.1 Трехмерная триангуляция.......................................................836

14 Содержание

14.3.2 Пакетная подстройка................................................................841
14.4 Плотное стереозрение.............................................................................850
14.4.1 Уточнение положения пиков..................................................855
14.4.2 Типичные проблемы стереосопоставления........................856
14.4.3 Исправление изображения......................................................867
14.5 Анаглифы. ..................................................................................................870
14.6 Другие технологии определения глубины. ........................................872
14.6.1 Структурированный свет.........................................................872
14.6.2 Определение глубины по времени пролета луча...............875
14.7 Облака точек..............................................................................................875
14.7.1 Поиск плоскости. .......................................................................877
14.7.2 Сопоставление двух наборов точек.......................................878
14.8 Применения...............................................................................................881
14.8.1 Коррекция перспективы..........................................................881
14.8.2 Мозаика........................................................................................884
14.8.3 Визуальная одометрия.............................................................888
14.9 Подведение итогов...................................................................................893
14.9.1 Дополнительное чтение...........................................................894
14.9.2 Дополнительные ресурсы........................................................900
14.9.3 Упражнения.................................................................................902

15

Часть V Управление на основе машинного зрения....................909
Управление на основе зрения...................................................................912
15.1 Визуальное сервоуправление на основе местоположения............914
15.2 Визуальное сервоуправление на основе изображения...................917
15.2.1 Движение камеры и изображения.........................................919
15.2.2 Управление движением объекта............................................927
15.2.3 Оценка глубины объекта..........................................................933
15.2.4 Проблемы с производительностью.......................................936
15.3 Использование других признаков изображений..............................939
15.3.1 Линейные объекты....................................................................939
15.3.2 Эллиптические объекты...........................................................940
15.3.3 Фотометрические признаки. ..................................................943
15.4 Подведение итогов...................................................................................945
15.4.1 Дополнительное чтение...........................................................945
15.4.2 Упражнения.................................................................................949

16

Продвинутое визуальное сервоуправление.......................................952
16.1
16.2
16.3
16.4

IBVS с разделением XY/Z........................................................................953
IBVS с использованием полярных координат...................................956
IBVS для сферической камеры..............................................................959
Применение...............................................................................................962
16.4.1 Робот-манипулятор...................................................................962
16.4.2 Мобильный робот......................................................................964
16.4.3 Воздушный робот. .....................................................................968

Содержание 15

16.5 Подведение итогов...................................................................................970
16.5.1 Дополнительное чтение...........................................................970
16.5.2 Ресурсы.........................................................................................972
16.5.3 Упражнения.................................................................................973

A

Приложения. ......................................................................................................975
Установка наборов инструментов............................................................976

B

Линейная алгебра............................................................................................978

C

Геометрия............................................................................................................990

D

Группы и алгебра Ли.....................................................................................1010

E

Линеаризация, якобианы и гессианы...................................................1017

F

Решение систем уравнений. .....................................................................1023

G

Гауссовы случайные величины................................................................1035

H

Фильтр Калмана.............................................................................................1039

I

Графы...................................................................................................................1047

J

Определение пика.........................................................................................1051

Литература...................................................................................................................1056
Предметный указатель...........................................................................................1078

Вступительное слово
Однажды ко мне на рецензию попала очень толстая папка
с диссертацией из далекой страны. Темой диссертации было
«Визуальное управление роботами», а ее автором был Питер
Корк. Я вспоминаю отрывок из моих комментариев, в котором
говорил, что это мастерская научная работа, качество диссер­
тации, к которому следует стремиться всем студентам, зная,
что очень немногие смогут его достичь, – очень хорошо продуманное и выполненное исследование.
Связь между робототехникой и зрением вот уже более двух
десятилетий является центральной нитью фундаментальных
исследований и успешных прикладных разработок Питера Корка. Этот редкий опыт воплощен во втором издании его книги.
В своем слиянии теории и практики это второе издание значительно выиграло от уникального сочетания академического
и практического опыта автора, основанного на его многолетней работе в области роботизированной горнодобывающей,
авиационной, подводной и полевой техники.
Существует множество учебников по робототехнике и компьютерному зрению, но лишь немногие из них достигли уровня
интеграции знаний, анализа научной области и доходчивости
иллюстраций, представленных в этой книге. Обсуждение подробное, повествование удивительно информативное и доступное, и в целом создается впечатление, что эта книга станет важнейшим ресурсом для сегодняшних и будущих исследователей
в области робототехники. Я могу смело утверждать, что почти
каждый аспект, который можно считать относящимся к задаче,
был проанализирован и включен в эту книгу, и мою убежденность в этом подтверждает эффективное использование программного обеспечения Toolbox.
Читатель получит достаточно полное представление о мобильных роботах, навигации, позиционировании, кинематике
руки-манипулятора, динамике и управлении на уровне сочленений, а также о моделировании камеры, обработке изображений, выделении признаков и многоракурсной геометрии.
И в заключение эти области будут объединены в подробном
обсуждении визуальных сервосистем. Автор также объясняет,
как сложные задачи можно разделить на ряд частных задач, решаемых с использованием мощных численных инструментов
и эффективного программного обеспечения.

Вступительное слово 17

Издательский проект Springer Tracts in Advanced Robotics
(STAR) призван донести до сообщества исследователей последние достижения в области робототехники исходя из их
значимости и качества. Наша цель – путем широкого и свое­
временного распространения важнейших научных разработок
в области робототехники способствовать обмену информацией и сотрудничеству между исследователями и поддерживать
дальнейший прогресс в этой быстро развивающейся области.
Питер Корк дополняет нашу серию STAR авторитетной книгой, охватывающей разные области, идеально задуманной
и блестяще выполненной.
Усама Хатиб
Стэнфорд, Калифорния
Октябрь 2016 г.

Предисловие
кажи мне – и я забуду. Покажи – и я запомню. Заставь меня
« Ссделать
– и я пойму.

Китайская пословица

ростые вещи должны быть простыми, сложные вещи должны
« Пбыть
возможными.

Алан Кей

Сфера робототехники переживает бурное развитие. С момента
выхода первого издания этой книги прошло более десяти лет,
и, оглядываясь назад, мы видим значительный прогресс: появление беспилотных автомобилей на дорогах общего пользования и множества роботов на Марсе (в том числе летающего);
роботизированный сбор проб с астероидов и комет; развитие
таких видов бизнеса, как Amazon и DARPA Subterranean Challenge, где группы наземных и воздушных роботов автономно
картографируют подземные пространства. Мы стали свидетелями революции беспилотников – летательные аппараты, которые когда-то были прерогативой аэрокосмических гигантов,
теперь можно купить всего за несколько десятков долларов. Все
это стало возможным благодаря постоянному росту вычислительной мощности и колоссальному прогрессу в разработке
недорогих инерциальных датчиков и камер, в значительной
степени обусловленному большим спросом на высококачест­
венные мобильные устройства. Создавать роботов становится
все проще – 3D-печать теперь очень доступна, операционная
система для роботов ROS (Robot Operating System) стала достаточно функциональной и получила широкое распространение,
а мощные технологии для хобби, такие как Arduino, Raspberry
Pi и сервоприводы Dynamixel, можно приобрести очень недорого. Это, в свою очередь, способствовало росту мирового сообщества энтузиастов и дало возможность отдельным людям,
работающим дома, и небольшим стартапам создавать то, что
раньше было под силу только крупным корпорациям.
Роботы – это машины, управляемые данными: они собирают
данные, обрабатывают их и предпринимают действия на их основе. Данные поступают от различных датчиков, измеряющих,
например, скорость вращения колес, угол поворота сочленения
руки-манипулятора или интенсивность освещенности миллио­
нов пикселей на светочувствительной мат­ри­це кинокамеры,

Предисловие 19

Когда я начал
заниматься робототехникой и компьютерным зрением
в середине 1980-х,
как раз появился
компьютер IBM PC –
он имел 16-разрядный микропроцессор с частотой
4.77 МГц и 16 Кбайт
памяти (с возможностью расширения
до 256 Кбайт).

См. ► https://ros.org.

составляющих изображение окружающего мира. Многим робототехническим приложениям приходится обрабатывать огромный объем данных в масштабе реального времени.
С одной стороны, прогресс развития робототехники обусловлен ростом доступных вычислительных мощностей. Закон
Мура предсказывает, что количество транзисторов на кристалле будет удваиваться каждые два года, что позволит постоянно увеличивать объем памяти и осуществлять параллельную
обработку данных с помощью нескольких ядер и графических
процессоров (GPU). Одновременно с этим будут уменьшаться
размеры транзисторов и увеличиваться тактовая частота.
С другой стороны, прогрессу способствует развитие алгоритмов, лежащих в основе вычислений для решения задач робототехники. За десятилетия исследовательское сообщество
разработало множество решений важных задач восприятия,
позиционирования, планирования и управления. Однако для
решения любой конкретной задачи имеется широкий выбор
алгоритмов, и каждый из них может иметь несколько реализаций. Они написаны на разных языках, в разных стилях, с применением разных соглашений о структуре API и имеют разное
качество кода, документации, поддержки и условия лицензирования. Это серьезная проблема для современной робототехники, и «склеивание» разрозненных фрагментов программного
обеспечения превратилось в обязательный навык для занимающихся робототехникой. Платформа ROS помогла стандартизировать интерфейсы и функции, необходимые для решения

Стойка VME
 ЦПУ 68030, 25 МГц + ОЗУ 16 Мбайт +
VxWorks
 Пиксельный процессор Datacube,
10 Мпикс/с
Изображение с камеры робота
Управление выборкой с камеры
Стойка ввода/вывода Multibus
 ток моторов
 сжатие + крутящий момент
Фильтры аналоговых сенсоров
Контроллер робота Unimation
для управления роботом

Рис. 0.1. Когда-то для управления роботами с использованием технологии машинного зрения требовалось
много специального оборудования. Здесь автор книги сидит возле стойки, набитой модулями
для обработки изображений в реальном времени и управления роботом PUMA 560 (1992 г.).
За прошедшие 30 с лишним лет количество транзисторов, размещаемых на кристалле,
выросло примерно в 230/2 ≈ 30 000 раз, в полном соответствии с законом Мура

20 Предисловие

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

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

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

задачами, а не только с тривиальными примерами

Для иллюстрации каждой темы используется программное
обеспечение, и такой подход имеет ряд преимуществ. Во-пер­вых,
использование программных инструментов позволит пользователю работать с реальными задачами, а не только с тривиальными примерами. Для управления роботами, с конечностями,
состоящими из двух и более звеньев, и обработки реальных изображений с миллионами пикселей необходимые вычисления
многократно превышают возможности человека. Во-вто­рых,
они помогут получить понятное визуальное представление, которое в противном случае могло бы затеряться на фоне сложных
вычислений. Мы можем быстро и легко экспериментировать,
играть в игру «что, если…» и выводить результаты на экран, используя мощные инструменты отображения 2D- и 3D-графики,
имеющиеся в Python и в библиотеке Matplotlib.
» Разговорное повествование, охватывающее

робототехнику и компьютерное зрение –
как по отдельности, так и вместе

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

Предисловие 21

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

на части и решить

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

меню грандиозной дегустации

Прежде всего я старался написать серьезное введение в компьютерное зрение для робототехников. Большинство учебников
по робототехнике, как правило, сосредоточены лишь на прос­
тых методах бинарного зрения. В этой книге мы рассмотрим
широкий круг тем, включая цветное зрение, передовые методы
сегментации, деформацию изображения, стереозрение, оценку местоположения, глубокое обучение, групповую коррекцию
и визуальную одометрию. Мы также обратим внимание на неперспективную визуализацию с использованием объективов
типа «рыбий глаз», катадиоптрической оптики и новых камер
светового поля. Эти темы приобретают все большую значимость в робототехнике, но обычно не освещаются в массовой
литературе. Зрение – это мощный источник сенсорной информации, и робототехники должны хорошо разбираться в основах
его работы. В последней части книги я покажу, как можно использовать зрение в качестве основного датчика для управления роботом.
Эта книга не похожа на другие учебники, и это сделано намеренно. Хотя существуют отличные учебники, подробно и по
отдельности рассматривающие робототехнику и компьютерное зрение, но среди них мало таких, которые предлагают комплексный охват этих дисциплин. Достижение такой интеграции было главной целью книги.
» Программное обеспечение является важнейшей

частью этой книги

Программное обеспечение – это осязаемая реализация описанных алгоритмов. Его можно прочитать, его можно разобрать,
изменить и снова собрать. Программное обеспечение является

22 Предисловие

важнейшей частью этой книги. Существует ряд классических
книг, в которых программное обеспечение используется в качестве иллюстраций и которые повлияли на мой подход, например LaTeX: A document preparation system (Lamport 1994), Numeri­
cal Recipes in C (Press et al. 2007), The Little Lisper (Friedman et al.
1987) и Structure and Interpretation of Classical Mechanics (Sussman
et al. 2001). Более 2000 строк кода и более 1600 примеров в этой
книге иллюстрируют, как можно использовать программное
обеспечение Toolbox, и, как правило, обеспечивают мгновенный
результат всего парой строк кода на Python.
» Мгновенный результат всего парой строк кода

на Python

В-третьих, построение книги на основе Python и Toolbox
означает, что мы можем решать более реалистичные и более
сложные задачи, чем в других книгах.
» Эта книга может стать прекрасным дополнением

к другим учебникам

Основная цель этой книги – сделать робототехнику, машинное зрение и алгоритмы управления доступными для широкой
аудитории. Доступность имеет ряд аспектов, включая концептуальный, географический и финансовый. Концептуально, без
математики, лежащей в основе робототехники, невозможно
обойтись, но я постарался снизить сложность представляемой
теории, и для понимания книги достаточно наличия математических знаний на уровне бакалавриата по инженерному делу.
Кроме того, обосновать абстрактные концепции и сделать их
наглядными помогут примеры с использованием программного обеспечения. Применение программного обеспечения с открытым исходным кодом снижает финансовые затраты читателя – оно доступно бесплатно в любой точке мира. Эта книга
может стать прекрасным дополнением к другим учебникам,
охватывающим те же темы, но использующим более традиционный подход, основанный на описании теории. Поэтому ее
лучше всего читать вместе с другими учебными материалами,
предлагающими строгую теоретическую базу. В конце каждой
главы есть раздел с перечнем дополнительной литературы
и ссылками на соответствующие учебники, статьи и онлайнресурсы.
В основе робототехники и компьютерного зрения лежат теории, разрабатывавшиеся математиками, учеными и инженерами на протяжении многих сотен лет. Некоторые из их имен стали прилагательными, например кориолисов, гауссов, лапласов
или декартов; существительными, например якобиан, или единицами измерения, например ньютон и кулон. Это интересные

Предисловие 23

персонажи далекой эпохи, когда наука была не профессией,
а хобби, и занимались ей врачи, алхимики, игроки, астрологи,
философы или наемники. Чтобы знать, на плечах каких гигантов мы стоим, я добавил в книгу небольшие вставки о жизни
некоторых из этих людей.
Создание Python-версии известного пакета инструментов
Robotics Toolbox for MATLAB® и этой книги было моей давней
мечтой. Я предпринял было несколько неудачных попыток, но
только в период пандемии наконец смог осуществить задуманное. Джесси Хэвиленд (Jesse Haviland) перевел на Python и расширил нотацию ETS (раздел 7.1.1.1) с помощью своего пакета
ropy, затем перевел на Python весь пакет Robotics Toolbox, а потом разработал среду моделирования Swift. Дориан Цай (Dorian
Tsai) начал перевод на Python пакета Machine Vision Toolbox.
Даниел Петков (Daniel Petkov) разработал графический редактор в поддержку пакета bdsim для работы с блок-схемами. Я глубоко благодарен им за помощь в достижении этой цели.
Эта книга основана на перечисленном выше ПО с открытым
исходным кодом, которое, в свою очередь, основано на других
программах с открытым исходным кодом, разработанных более широким сообществом. Я хотел бы выразить благодарность
разработчикам, стоящим за Python, Matplotlib, NumPy, SciPy,
SymPy, pybullet, Python Robotics, Jupyter, OpenCV, Open3D и PyTorch. Работу над книгой и кодом помогли ускорить такие замечательные инструменты с открытым исходным кодом, как
Visual Studio Code, Git, GitHub, meld, TexStudio, xelatex и LaTeX.
Размах и качество творений сообщества свободного программного обеспечения не перестают меня поражать.
Многие люди помогли мне, высказывая критические замечания по предыдущим изданиям. И благодаря им это издание стало лучше. Я хочу выразить свою особую благодарность
следующим людям: Пола Ньюман (Paul Newman), Даниэла Рас
(Daniela Rus), Седрик Прадалье (Cédric Pradalier), Тим Барфут
(Tim Barfoot), Дмитрий Братанов (Dmitry Bratanov), Дункан
Кэмпбелл (Duncan Campbell), Дональд Дансеро (Donald Dansereau), Том Драммонд (Tom Drummond), Малкольм Гуд (Malcolm Good), Питер Куджала (Peter Kujala), Обадия Лам (Obadiah
Lam), Йорн Мальзан (Jörn Malzahn), Фелипе Насименто Мартинс (Felipe Nascimento Martins), Аджай Панди (Ajay Pandey),
Дэн Ричардс (Dan Richards), Сарех Ширази (Sareh Shirazi), Сурья Сингх (Surya Singh), Райан Смит (Ryan Smith), Бен Талбот
(Ben Talbot), Дориан Цай (Dorian Tsai), Бен Апкрофт (Ben Upcroft), Франсуа Шометт (François Chaumette), Дональд Дансеро
(Donald Dansereau), Кевин Линч (Kevin Lynch), Роберт Махони
(Robert Mahony) и Фрэнк Парк (Frank Park). В этом третьем издании мои соавторы по книге для MATLAB, Витольд Яхимчик
(Witold Jachimczyk) и Ремо Пиллат (Remo Pillat), предложили

24 Предисловие

свежий взгляд и конструктивную критику, которые пошли на
пользу обеим версиям. Я благодарен своим коллегам, давшим
подробные и содержательные отзывы к рукописям последних
глав: Кристине Казанциду (Christina Kazantzidou, которая помогла отшлифовать текст и математические обозначения), Тобиасу Фишеру (Tobias Fischer), Уиллу Брауну (Will Browne), Робу
Махони (Rob Mahony), Джесси Хэвиленд (Jesse Haviland), Ферасу
Даюбу (Feras Dayoub), Дориану Цаю (Dorian Tsai), Алессандро Де
Луке (Alessandro De Luca), Рено Детри (Renaud Detry), Брайану
Фанусу (Brian Fanous) и Ханнесу Деппу (Hannes Daepp).
Я старался устранить ошибки, но некоторые из них неизбежно ускользнут от моего внимания. Пожалуйста, присылайте
по адресу rvc@petercorke.com сообщения об ошибках, а также
предложения по улучшениям и дополнениям книги.
Частичную финансовую поддержку работе над этим изданием оказала премия Australian University Teacher (AAUT) от
министерства образования и профессиональной подготовки
правительства Австралии. Я благодарю руководство технологического университета Квинсленда, что дали время для завершения этого проекта. На протяжении работы над всеми
изданиями я пользовался огромной поддержкой со стороны
сотрудников Springer-Nature: Томаса Дитцингера (Thomas Ditzinger), поддержавшего этот проект еще до выхода первого издания, и Вильмы Макхью (Wilma McHugh). Особую благодарность
хочу выразить Ивонн Шлаттер (Yvonne Schlatter) и команде letex за их помощь в наборе текста.
Наконец, моя глубочайшая благодарность Филиппе, которая
поддерживала меня с присущим ей изяществом и терпением
в течение очень долгого времени и во многих разных ситуациях, – без нее эта книга никогда не была бы написана.

Примечание к третьему изданию
Первые два издания этой книги ► были основаны на MATLAB®
в сочетании с открытым пакетом MATLAB Toolbox, которым уже
тридцать лет – это долгий срок для любого программного обес­
печения. За последнее десятилетие произошло много событий,
которые побудили изменить программную основу книги, что
привело к выходу двух третьих изданий.
„„

Версия, которую вы читаете, основана на Python – популярном языке программирования с открытым исходным
кодом, имеющем широкую поддержку сторонних разработчиков. Старый пакет инструментов MATLAB Toolbox
был переработан и реализован на Python с использовани-

Первое издание
увидело свет
в 2011-м, второе
однотомное
издание – в 2017-м,
а затем в виде двухтомника – в 2022-м.

Предисловие 25

ем популярных пакетов и ресурсов с открытым исходным
кодом, обеспечивающих переносимость между платформами, возможность отображения трехмерной графики
в браузере, доступ к онлайн-документации, быстрые числовые и символьные операции, поддержку блокнотов,
доступных для просмотра в интернете, и все это на базе
GitHub и сообщества разработчиков с открытым исходным
кодом (Corke, 2021).
„„ Альтернативная версия, переписанная вместе с коллегами
Витольдом Яхимчиком (Witold Jachimczyk) и Ремо Пиллатом (Remo Pillat) из MathWorks®, использует MATLAB и современные наборы инструментов, разработанные, лицензированные и поддерживаемые MathWorks.
Помимо изменения программной основы, в этом третьем
издании также были исправлены ошибки, улучшены математические формулы и материал объясняется более понятным
языком. Главы 2 и 7 были существенно переработаны. В это издание также включены новые темы, такие как планирование
пути на основе графов, пути Дубинса и Ридса–Шеппа, разветвленные роботы, модели URDF, проверка столкновений, управление пространством задач, глубокое обучение для обнаружения объектов и семантической сегментации, опорные маркеры
и облака точек.
Питер Корк,
Брисбен, Квинсленд,
март 2022

Специальные обозначения

К сожалению, в книгах и научных статьях, посвященных робототехнике и машинному зрению, нет единообразия, и применяемые в них символы и обозначения могут значительно различаться. Ниже перечислены символы, используемые в этой книге, и их единицы измерения, где это уместно. Некоторые
символы имеют несколько значений, и для их интерпретации необходимо
учитывать контекст.
Обозначение
υ
υ̂
υ̃
υi
υ̃i
υx, υy, υz
A
ai, j
0m×n
1m×n
q̆
q̊
f(x)
Fx(x)
Fxy(x, y)
{F }

Описание
вектор
единичный вектор, параллельный υ
однородное представление вектора υ
i-й элемент вектора υ
i-й элемент однородного вектора υ̃
элементы вектора координат υ
мат­ри­ца
элемент мат­ри­цы A в строке i и столбце j
m×n мат­ри­ца нулей
единичная m×n мат­ри­ца (мат­ри­ца тождествености)
кватернион q̆ ∈ ℍ
единичный кватернион q̊ ∈ S3
функция от x
производная f(x), ∂f = ∂x
вторая производная f(x, y), ∂2f = ∂x∂y
система координат

Векторы обычно обозначаются строчными латинскими или греческими буквами, а мат­ри­цы – заглавными латинскими или греческими буквами. К символам добавляются различные индексы:
Индекс
x∗
x+
x#
x̂
–
x
x〈k〉

Описание
желаемое значение x
спрогнозированное значение x
измеренное или наблюдаемое значение x
ожидаемое значение x
среднее значение x или относительное значение
k-й элемент временного ряда

где x может быть скаляром, вектором или мат­ри­цей.

Специальные обозначения 27
Обозначение
B
B
C
C(q, q̇)
𝒞
e

E
f
f
f(q̇)
g
g(q)
ℍ
J
J
J

JB

A

k, K
K
Ki
Km
L
m
M(q)
ℕ

ℕ0
N(μ, σ2)
p
P
p
P
ℙ2
ℙ3
q
Q
R
ℝ

ℝ >0
ℝ ≥0
ℝ2
ℝ3
s
s
S1

Описание
коэффициент вязкостного трения
плотность магнитного потока
мат­ри­ца камеры, C ∈ ℝ3×4
центростремительный и кориолисов член в уравнении движения
манипулятора
конфигурационное пространство робота с N сочленениями: 𝒞 ⊂ ℝN
математическая константа, основание натуральных логарифмов,
e = 2,71828...
освещенность (люкс)
фокусное расстояние
сила
момент трения манипулятора
ускорение силы тяжести, см. рис. 3.11
гравитационный член в уравнении движения манипулятора
множество всех кватернионов («H» происходит от имени Hamilton)
инерция
тензор инерции, J ∈ ℝ3×3
мат­ри­ца Якоби, якобиан
преобразованная по Якоби скорость в системе координат {B} по
отношению к системе {A}
константа
калибровочная мат­ри­ца камеры, K ∈ ℝ3×3
коэффициент усиления усилителя (крутизна)
постоянный крутящий момент двигателя
яркость (нит, 1 нт = 1 кд / 1 м²)
масса
мат­ри­ца инерции манипулятора
множество натуральных чисел {1, 2, 3, ...}
множество натуральных чисел, включая нуль {0, 1, 2, 3, ...}
нормальное (гауссово) распределение со средним μ и стандартным
отклонением σ
точка в плоскости изображения
мировая точка
вектор координат точки в плоскости изображения, p ∈ ℝ2
вектор координат мировой точки, P ∈ ℝ3
проективное пространство всех двухмерных точек, 3-кортеж
проективное пространство всех трехмерных точек, 4-кортеж
обобщенные (лагранжевы) координаты, конфигурация q ∈ 𝒞
обобщенная сила Q ∈ ℝN
ортонормальная мат­ри­ца поворота, R ∈ SO(2) или SO(3)
множество вещественных чисел
множество положительных вещественных чисел
множество неотрицательных вещественных чисел
множество всех точек в двухмерном пространстве, элементы
множества – 2-кортежи
множество всех точек в трехмерном пространстве, элементы
множества – 3-кортежи
длина пути или траектории, s ∈ [0, 1]
оператор преобразования Лапласа
множество точек на единичной окружности, множество углов [0, 2π)

Единицы
Н·м·с·рад–1
T
кг·м2·с–1

лк
м
Н
Н·м
м·с–2
Н·м
кг·м2
кг·м2

А·V–1
Н·м·A–1
нт
кг
кг·м2

м, рад
Н, Н·м

28 Специальные обозначения
Обозначение Описание
Sn
единичная сфера, вписанная в ℝn+1
se(n)
алгебра Ли для SE(n), ℝ(n+1)×(n+1)-дополненная кососимметричная мат­
ри­ца
so(n)
алгебра Ли для SО(n), кососимметричная мат­ри­ца ℝn×n
SE(n)
особая евклидова группа матриц, T ∈ SE(n), T ⊂ ℝ(n+1)×(n+1),
описывающих все положения в n измерениях, представленная
гомогенной мат­ри­цей преобразования твердого тела
SO(n)
особая ортогональная группа матриц, R ∈ SO(n), R ⊂ ℝn×n,
RT = R–1, det(R) = 1, множество всех ориентаций в n измерениях,
представленная ортогональной мат­ри­цей поворота
S
скручивание в двух или трех измерениях, S ∈ ℝ3 или ℝ6
t
время
𝒯
пространство задач робота, 𝒯 ⊂ SE(3)
T
интервал выборки
T
температура
T
оптическая прозрачность, светопропускание
T
однородная мат­ри­ца преобразования T ∈ SE(2) или SE(3)
A
TB
однородная мат­ри­ца преобразования, представляющая положение
системы координат {B} относительно системы {A}. Если A не дано,
система рассматривается относительно точки {0} мировой системы
координат; обратите внимание, что ATB = (BTA)–1
u, v
координаты на плоскости изображения камеры
координаты главной точки
u0, v0
нормализованные координаты плоскости изображения
u–, –v
относительно главной точки
v
скорость
v
вектор скорости
w
винтовой вектор, вектор сил и моментов (fx, fy, fz, mx, my, mz) ∈ ℝ6
x̂, ŷ, ẑ
единичные векторы, выровненные по осям x, y и z, трехмерный базис
X, Y, Z
декартовы координаты
нормализованные координаты плоскости изображения
x–, –y
ℤ
множество всех целых чисел {..., –2, –1, 0, 1, 2, ...}
∅
нулевое движение
α
угол крена
β
угол тангажа
γ
угол поворота рулевого колеса
δ
приращение
𝜖
ошибка или остаток, в идеале нулевой
γ
угол рыскания
γ
угол поворота рулевого колеса мобильного робота
𝜞
3-угловое представление поворота, 𝜞 ∈ ℝ3
𝜞
крутящий момент тела, 𝜞 ∈ ℝ3
θ
угол, первый угол Эйлера, курсовой угол
λ
длина волны
λ
собственное значение
ν
инновация
ν
трехмерная (пространственная) скорость, (vx, vy, vz, ωx, ωy, ωz) ∈ ℝ6
ξ
«кси», абстрактное представление положения в декартовых
координатах
A
ξB
абстрактное представление относительного положения, системы
координат {B} относительно системы {A} или движения твердого тела
из системы {A} в {B}

Единицы

с
К
с
К
М–1

пиксели
пиксели
пиксели
м·с–1
м·с–1
Н, Н·м

м

рад
рад
рад

рад
рад
рад
Н·м
рад
м

м/с, рад/с

Специальные обозначения 29
Обозначение Описание
ξ ti(d )
абстрактное положение, которое является чистым переносом d вдоль
оси i ∈ {x, y, z}
ξ ri(θ)
абстрактное положение, которое является чистым поворотом вокруг
оси i ∈ {x, y, z}
π
математическая константа, π = 3,14159...
π
плоскость
ρw, ρh
ширина и высота пикселя
σ
стандартное отклонение
σ
тип сочленения робота: σ = R – поворотное, σ = P – призматическое
Σ
в алгебре Ли Σ = [v] ∈ se(3), v ∈ ℝ6
τ
крутящий момент
τС
кулоновский момент трения
φ
угол, второй угол Эйлера
ψ
третий угол Эйлера
ω
скорость вращения
ω
вектор угловой скорости
ϖ
скорость вращения вала двигателя, колеса или пропеллера
Ω
в алгебре Ли Ω = [v]× ∈ so(3), v ∈ ℝ3

Оператор
v1 · v2
||·||
v1 × v2
·
𝜖(·)
A–1
A+
A∗
AT
A–T
⊕
⊖
⊖
•
Δ(·)
Δ–1(·)
[·]t
[·]R
[·]xyθ
[·]×
[·]
∨×(·)
∨(·)
Ad(·)
ad(·)
∘
v̆

Единицы

м

Н·м
Н·м
рад
рад
рад·с–1
рад·с–1
рад·с–1

Описание
скалярное (внутреннее) произведение, также v1⊤v2: ℝn × ℝn ↦ ℝ
норма (длина) вектора, также v · v: ℝn ↦ ℝ ≥0
векторное (перекрестное) произведение: ℝ3 × ℝ3 ↦ ℝ3
ℝn ↦ ℙn
ℙn ↦ ℝn
обращение (инверсия) A: ℝn×n ↦ ℝn×n
псевдообращение (псевдоинверсия) A: ℝn×m ↦ ℝm×n
дополнение (присоединение) A, также det(A)A–1: ℝn×n ↦ ℝn×n
транспонирование A: ℝn×m ↦ ℝm×n
транспонирование A с обращением, (AT)–1 ≡ (A–1)T: ℝn×n ↦ ℝn×n
композиция абстрактного положения: xξy ⊕ yξz ↦ xξz
композиция абстрактного положения с инверсией: xξy ⊖ zξy ≡ xξy ⊕ yξz ↦ xξz
инверсия абстрактного положения: ⊖ xξy ≡ xξz
преобразование точки (вектора координат) в абстрактное относительное
положение: xξy • yp ↦ xp
отображение инкрементного относительного изменения положения
в дифференциальное движение: SE(3) ↦ ℝ6
отображение дифференциального движения в инкрементное изменение
относительного положения: ℝ6 ↦ SE(3)
компонент перемещения в пространственном расположении: SE(n) ↦ ℝn
компонент поворота в пространственном расположении: SE(n) ↦ SO(n)
двухмерное расположение в конфигурационном пространстве: SE(2) ↦ ℝ6 × S1
кососимметричная мат­ри­ца: ℝ ↦ so(2), ℝ3 ↦ so(3)
дополненная кососимметричная мат­ри­ца: ℝ3 ↦ se(2), ℝ6 ↦ se(3)
распаковка кососимметричной мат­ри­цы: so(2) ↦ ℝ, so(3) ↦ ℝ3
распаковка дополненной кососимметричной мат­ри­цы: se(2) ↦ ℝ3, se(3) ↦ ℝ6
сопряженное представление: SE(3) ↦ ℝ6×6
логарифм сопряженного представления: SE(3) ↦ ℝ6×6
произведение кватернионов (гамильтонианов): ℍ × ℍ ↦ ℍ
чистый кватернион: ℝ3 ↦ ℍ

30 Специальные обозначения
Оператор
~
≃
⊝
𝒦(·)
𝒦–1(·)
𝒟–1(·)
𝒫(·)
∗
⊗
≡
⊕
⊖
∘
•
[a, b]
(a, b)
[a, b)
(a, b]

Описание
эквивалентность представлений
эквивалентность однородных координат
наименьшая угловая разность между двумя углами: 𝕊1 × 𝕊1 ↦ [–π, π)
прямая кинематика: 𝒞 ↦ 𝒯
обратная кинематика: 𝒯 ↦ 𝒞
функция обратной динамики манипулятора: 𝒞, ℝN, ℝN ↦ ℝN
функция проекции камеры: ℝ3 ↦ ℝ2
свертка изображения
корреляция
колориметрическая эквивалентность
морфологическая диляция
морфологическая эрозия
морфологическое открывание
морфологическое закрывание
интервал от a до b включительно
интервал от a до b, не включая a и b
интервал от a до b, не включая b
интервал от a до b, не включая a

Специальные обозначения 31

Соглашения Toolbox
Декартова точка, вектор координат, выражается как 1-мерный массив NumPy.
„„ Множество точек выражается в виде двухмерного массива
NumPy со столбцами, представляющими координаты отдельных точек.
„„ Конфигурация робота, описывающая множество углов сочленений, выражается в виде 1-мерного массива NumPy.
„„ Данные временного ряда выражаются в виде двухмерного
массива NumPy, строки которого представляют временные
шаги.
„„ Двумерный массив NumPy имеет индексы [i, j], представляющие номер строки и столбца соответственно. Координаты
изображения записываются как (u, v), поэтому изображение, представленное NumPy-массивом I, индексируется как
I[v, u].
„„

Индексирование начинается с нуля
В Python списки и массивы индексируются начиная с нуля. Алгоритмы в этой книге приведены в соответствие с этим принципом. Например, сочленения робота-манипулятора нумеруются
с нуля, тогда как в других источниках они обычно нумеруются
начиная с единицы. Всякий раз, когда в книге какой-либо элемент упоминается по номеру, например «сочленение один»
или «столбец два», то подразумевается индексация с нуля.
Слова первый, второй и третий имеют свое обычное значение
и применяются к индексам нуль, один и два. Например: первый
столбец мат­ри­цы также называется нулевым столбцом; третье
сочленение робота также называется сочленением два. Слово ну­
левой никогда не используется.

Распространенные сокращения
1D – одномерный
2D – двухмерный
3D – трехмерный
CoM – центр масс (Center of Mass)
DoF – число степеней свободы (Degrees of Freedom)
n-кортеж – группа из n чисел, может представлять точку или
вектор

32 Специальные обозначения

Отзывы и пожелания
Мы всегда рады отзывам наших читателей. Расскажите нам, что
вы ду­маете об этой книге – что понравилось или, может быть,
не понравилось. Отзывы важны для нас, чтобы выпускать книги, которые будут для вас максимально полезны.
Вы можете написать отзыв на нашем сайте www.dmkpress.
com, зайдя­ на страницу книги и оставив комментарий в разделе «Отзывы и рецензии». Также можно послать письмо главному редактору по адресу dmkpress@gmail.com; при этом укажите название книги в теме письма.
Если вы являетесь экспертом в какой-либо области и заинтересованы в написании новой книги, заполните форму на нашем
сайте по адресу http://dmkpress.com/authors/publish_book/ или
напишите в издательство по адресу dmkpress@gmail.com.

Список опечаток
Хотя мы приняли все возможные меры для того, чтобы обеспечить высокое качество наших текстов, ошибки все равно случаются. Если вы найдете ошибку в одной из наших книг, мы будем
очень благодарны, если вы сообщите о ней главному редактору
по адресу dmkpress@gmail.com. Сделав это, вы избавите других
читателей от недопонимания и поможете нам улучшить последующие издания этой книги.

Нарушение авторских прав
Пиратство в интернете по-прежнему остается насущной проблемой. Издательства «ДМК Пресс» и Manning Publications
очень серьезно относятся к вопросам защиты авторских прав
и лицензирования. Если вы столкнетесь в интернете с незаконной публикацией какой-либо из наших книг, пожалуйста, пришлите нам ссылку на интернет-ресурс, чтобы мы могли применить санкции.
Ссылку на подозрительные материалы можно прислать по
адресу элект­ронной почты dmkpress@gmail.com.
Мы высоко ценим любую помощь по защите наших авторов,
благодаря которой мы можем предоставлять вам качественные
материалы.

Глава

1

	Введение

1.1

Это, в свою очередь,
повлияло на сэра
Чарльза Бэббиджа
в его стремлении
механизировать
вычисления, что,
в свою очередь, повлияло на графиню
Аду Лавлейс, формализовавшую вычисления и создавшую
первый алгоритм.

Краткая история роботов

Слово робот для разных людей имеет разное значение. Научнофантастические книги и фильмы оказали сильное влияние на
то, как многие представляют себе робота и на что он способен.
К сожалению, практика робототехники пока сильно отстает от
этого распространенного представления. Слово робот также
вызывает разные эмоции: некоторые искренне опасаются будущего с роботами, а других больше беспокоит, какое влияние
роботы окажут на рабочие места, конфиденциальность или ведение войн. Однако одно несомненно: робототехника станет
важной технологией в этом столетии. Такие продукты, как роботы-пылесосы, существуют уже более двух десятилетий, а на
дорогах общего пользования уже появились беспилотные автомобили. Это авангард волны умных машин, которые появятся
в наших домах и на рабочих местах в ближайшем или чуть более отдаленном будущем.
В XVIII в. европейцы были очарованы автоматами, такими как утка Вокансона, показанная на рис. 1.1а. Эти машины,
сложные по меркам того времени, демонстрировали то, что
казалось людям поведением живого существа. В утке был задействован кулачковый механизм для выполнения последовательности движений, и впоследствии Вокансон механизировал
с его помощью ткацкие станки для производства шелка. Жаккард развил его идеи и разработал ткацкий станок (рис. 1.1б),
в котором узор ткани был закодирован как серия отверстий на
перфокартах. ◄ У этого ткацкого станка было много признаков
современного робота – он выполнял физическую задачу и был
перепрограммируемым.
Термин робот впервые появился в 1920 г. в чешской научнофантастической пьесе «Универсальные роботы Россума» Карела Чапека. Этот термин был придуман его братом Йозефом и на
чешском языке означает крепостной труд, а в просторечии –
тяжелую работу. Роботы в пьесе были искусственными людьми
(андроидами), и, как и во многих других последующих историях, они восстают, и это плохо кончается для человечества. Се-

34 Глава 1 · Введение

рия книг Айзека Азимова о роботах, состоящая из множества
книг и рассказов, написанных между 1950 и 1985 г., исследует
морально-этические вопросы взаимодействия человека и робота. Роботы в этих историях оснащены «позитронными мозгами», в которых закодированы «три закона робототехники». Эти
истории повлияли на последующие книги и фильмы, которые,
в свою очередь, сформировали общественное представление
о том, что такое роботы. В середине XX века также появился
термин кибернетика, который ныне используется довольно
редко, но тогда это был захватывающий раздел науки, стоявший на переднем крае понимания жизни и создания интеллектуальных машин.

а

б

Рис. 1.1. Ранние программируемые машины: а) утка Вокансона (1739) – автомат, умевший
хлопать крыльями, есть зерно и испражняться. Приводилась в движение часовым
механизмом и выполняла одну программу; б) жаккардовый ткацкий станок (1801) –
перепрограммируемое устройство, программа которого хранилась на перфокартах
(изображение Джорджа П. Ландоу с сайта https://www.victorianweb.org)

а

б

Рис. 1.2. Универсальные автоматы: а) вид сверху на машину из патента Девола; б) первый
робот Unimate, запущенный на заводе General Motors (изображение предоставлено
Джорджем К. Деволом)

Первая заявка на патент на то, что мы сейчас называем роботом-манипулятором, была подана в 1954 году Джорджем К. Деволом, а сам патент был выдан в 1961 году. Устройство состояло
из механической руки с захватом, установленной на направляющей, а последовательность движений кодировалась магнитны-

Краткая история роботов 35
а

б

Рис. 1.3. Промышленные роботы, технологические потомки робота Unimate, показанного
на рис. 1.2: а) современный шестикоординатный робот, обладающий высокой точностью
и производительностью (изображение предоставлено ABB robotics); б) коллаборативный робот
Universal Robotics может безопасно работать в паре с человеком (© 2022 Universal Robots A/S.
Все права защищены. Изображение использовано с разрешения Universal Robots A/S)

Отступление 1.1. Джордж Девол
Девол (1912–2011) – американский изобретатель, родившийся в Луисвилле, штат Кентукки.
В 1932 году основал United Cinephone Corp.,
которая производила фонографические рычаги и усилители, приводы для печатных и упаковочных машин. В 1954 году подал заявку на
патент № 2 988 237 с заголовком «Programmed
Article Transfer», где была представлена концепция универсальной автоматизации (Unimation). В частности, в заявке описывался
механизм манипулятора с полярными координатами на рельсах с захватом и программируемым контроллером – предшественник всех
современных роботов.
В 2011 году он был включен в Национальный зал славы изобретателей (изображение предоставлено Джорджем К. Деволом).

Отступление 1.2. Unimation Inc.
Девол искал финансирование для разработки своей технологии Unimation и однажды в 1954 году, будучи на коктейльной вечеринке, познакомился с Джозефом
Энгельбергером, тогдашним инженером компании «Manning, Maxwell and Moore».
В 1956 году они совместно основали Unimation (1956–1982) – первую компанию
по производству робототехники в Дэнбери, штат Коннектикут. Впоследствии компания была приобретена компанией Consolidated Diesel Corp. (Condec) и стала
подразделением Condec под названием Unimate Inc. Их первый робот приступил
к работе в 1961 году на литейном заводе General Motors в Нью-Джерси. В 1968 году
они продали лицензию на технологию компании Kawasaki Heavy Industries, которая выпустила первого японского промышленного робота. Энгельбергер занимал
пост генерального директора до приобретения компании Unimation компанией
Westinghouse в 1982 году. Сотрудники и технологии этой компании впоследствии
оказали огромное влияние на всю сферу робототехники.

36 Глава 1 · Введение

Отступление 1.3. Джозеф Ф. Энгельбергер
Джозеф Энгельбергер (1925–2015) – американский
инженер и предприниматель, которого часто называют «отцом робототехники». Получил степени бакалавра и магистра по физике в Колумбийском университете в 1946 и 1949 годах соответственно. Энгельбергер
был неутомимым популяризатором робототехники.
В 1966 году он появился в программе «Вечернее шоу
с Джонни Карсоном» с роботом Unimate, который наливал пиво, бросал мяч для гольфа и дирижировал
оркестром. Он активно продвигал развитие робототехники в Японии, что привело к значительным инвестициям в эту область.
Энгельбергер занимал должность генерального
директора Unimation Inc. до 1982 года, а в 1984 году основал компанию Transitions
Research Corporation, которая впоследствии получила название HelpMate Robotics
Inc. и стала одним из первых участников рынка роботов для больниц. Был избран
в Национальную инженерную академию, получил премию Бекмана и национальную премию Японии, написал две книги: «Robotics in Practice» (1980) и «Robotics
in Service» (1989). Ежегодно Ассоциация передовой автоматизации (https://www.
automate.org) вручает награду в его честь «лицам, внесшим выдающийся вклад
в развитие науки и практики робототехники».

ми шаблонами, устанавливаемыми на вращающемся барабане.
Первая робототехническая компания Unimation была основана
Деволом и Джозефом Энгельбергером в 1956 году, а их первый
промышленный робот, показанный на рис. 1.2б, был установлен
в 1961 году. Первоначальное видение Девола и Энгельбергера
относительно роботизации впоследствии стало реальностью.
Многие миллионы роботов-манипуляторов, подобных изображенному на рис. 1.3, были построены и запущены в работу для
выполнения таких задач, как сварка, покраска, погрузка и разгрузка машин, сборка, сортировка, упаковка и паллетирование.
Использование роботов повысило производительность и улучшило качество продукции. В настоящее время многие товары,
которые мы покупаем, были собраны или обработаны роботом.

1.2

Типы роботов

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

Типы роботов 37
а

б

Рис. 1.4. Мобильные роботы, перемещающиеся не по суше: а) небольшой автономный подводный аппарат (Todd
Walsh © 2013 MBARI); б) беспилотный летательный аппарат (БПЛА) Global Hawk (изображение предоставлено NASA)

а

б

в

г

Рис. 1.5. Мобильные роботы: а) марсоход Perseverance на Марсе, автопортрет. На мачте установлено множество
камер, включая стереопары, с помощью которых робот может определять трехмерную структуру окружающей среды
(изображение предоставлено NASA/JPL-Caltech/MSSS); б) робот-доставщик Savioke Relay (изображение предоставлено
Savioke); в) беспилотный автомобиль (изображение предоставлено кафедрой информационной инженерии
Оксфордского университета); г) шагающий робот Boston Dynamics Spot (изображение предоставлено Дорианом Цаем)

Другой способ классификации основан на выполняемой роботом функции. Промышленные роботы работают на заводах
и являются технологическими потомками роботов первого поколения, созданных компанией Unimation Inc. Сервисные ро­
боты оказывают людям такие услуги, как уборка помещений,
выполнение гигиенических процедур, медицинская реабили-

38 Глава 1 · Введение

Отступление 1.4. Универсальные роботы Россума
Термин «робот» впервые появился в 1920 г. в чешской
научно-фантастической пьесе «Универсальные роботы
Россума» Карела Чапека. Во вступительной сцене Хелен
Глори встречается с Гарри Домином, генеральным директором Rossum’s Universal Robots, и его роботизированной
секретаршей Суллой.
Домин: Сулла, позволь мисс Глори взглянуть на тебя.
Хелен: (встает и протягивает руку) Приятно познакомиться. Вам, наверно, тяжело быть здесь, отрезанной от
остального мира (фабрика находится на острове).
Сулла: Я незнакома с остальным миром, мисс Глори.
Пожалуйста, садитесь.
Хелен: (садится) Откуда вы?
Сулла: Отсюда, с фабрики.
Хелена: О, вы здесь родились.
Сулла: Да, я была создана здесь.
Хелен: (пораженно) Что?
Домин: (смеется) Сулла не человек, мисс Глори, она – робот.
Хелен: О, пожалуйста, простите меня...
Полный текст пьесы можно найти на https://www.gutenberg.org/ebooks/59112.
Даже сто лет спустя она остается пророческой и заставляет задуматься. (Фото любезно предоставлено Библиотекой Конгресса, артикул 96524672.)

тация или подноска грузов (рис. 1.5б). Полевые роботы (рис. 1.4),
например, работают на открытом воздухе и используются в мониторинге окружающей среды, сельском хозяйстве, горнодобывающей промышленности, строительстве и лесном хозяйстве.
Человекоподобные роботы (рис. 1.6) имеют физическую форму
человека – они одновременно являются мобильными и сервисными роботами.
Промышленный робот обычно представляет собой манипулятор наподобие механической руки на фиксированном основании (рис. 1.3а), который выполняет повторяющиеся задачи
в локальной рабочей ячейке. Детали подаются роботу строго
упорядоченно, что позволяет максимально использовать преимущество высокой скорости и точности робота. Эти роботы
быстро работают, неспособны чувствовать людей и обычно размещаются за защитным барьером. В отличие от них, коллаборативные роботы (рис. 1.3б) безопасны для человека – они работают на низкой скорости и останавливаются при столкновении
с препятствием.
Полевые и сервисные роботы сталкиваются со специфическими и серьезными проблемами. Первая проблема – робот должен
работать и двигаться в сложной, изменчивой и загроможденной предметами среде. Например, робот-доставщик в больнице должен работать в условиях скопления людей и меняющейся

Типы роботов 39

а

б

Рис. 1.6. Человекоподобные роботы: а) человекоподобный робот Asimo компании
Honda (изображение предоставлено Honda Motor Co., Япония); б) робот Hubo,
победивший в конкурсе робототехники DARPA Robotics Challenge в 2015 году
(изображение предоставлено KAIST, Корея)

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

Отступление 1.5. Телероботы
В рамках Манхэттенского проекта во время Второй мировой войны начались разработки первого ядерного оружия, что потребовало работы
с радиоактивными материалами. Рэй Гетц из
Аргоннской национальной лаборатории создал
дистанционно управляемые манипуляторы, использовавшие ловкость рук людей-операторов
и защищавшие их от прямого контакта с рабочими материалами. Операторы наблюдали за
рабочим пространством через толстые окна из
свинцового стекла или по телевизионной связи и управляли ведущей рукой (слева). Ведомая
рука (справа) следовала за движением, и усилия,
ощущаемые ведомой рукой, отражались обратно на ведущую руку, что позволяло
оператору ощущать вес и силу сопротивления. Телеробототехника и сегодня используется для выполнения работы, которую люди не могут выполнять напрямую,
но которая слишком сложна для выполнения машиной, например в подводных
роботах, обследовавших затонувший «Титаник». (Изображение предоставлено
Аргоннской национальной лабораторией.)

40 Глава 1 · Введение

лотные, автомобили, показанные на рис. 1.5в, должны передвигаться по дорогам, объезжать препятствия, соблюдать сигналы
светофора и правила дорожного движения. Вторая сложность
для таких роботов – безопасность для окружающих людей. Робот-доставщик в больнице работает среди людей, беспилотный
автомобиль перевозит людей, а роботизированный хирургический аппарат выполняет операции внутри людей.
Телероботы – это роботоподобные машины, которыми дистанционно управляет человек-оператор. Самым ранним примером, пожалуй, была радиоуправляемая лодка, продемонстрированная Николой Теслой в 1898 году, которую он называл
телеавтоматом. Такие машины были важным предшественником роботов и до сих пор важны для выполнения задач в условиях, где люди не могут работать, но которые слишком сложны
для выполнения машиной самостоятельно. Например, «подводные роботы», обследовавшие место крушения «Титаника»,
технически представляли собой дистанционно управляемые
аппараты. Современный хирургический робот (рис. 1.7) тоже
является телеуправляемым – хирург дистанционно управляет
движением небольших инструментов внутри пациента. Применение таких роботов благотворно сказывается на здоровье
пациента, поскольку процедура выполняется через гораздо
меньший разрез, чем при старом подходе, когда хирург работал
внутри тела руками.

Рис. 1.7. Хирургический
робот, несколько
инструментов проходят
через один небольшой
разрез (изображение © 2015
Intuitive Surgical, Inc)

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

Типы роботов 41

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

Отступление 1.6. Кибернетика, искусственный интеллект и робототехника
Кибернетика как область исследований процветала с 1930-х по 1960-е годы и подпитывалась новыми идеями и результатами исследований в нейрологии, теории
управления и теории информации. Как показали исследования в области нейрологии, мозг представляет собой сеть нейронов, обменивающихся электрическими
сигналами. Гарольд Блэк, Хенрик Боде и Гарри Найквист из Bell Labs занимались
исследованием отрицательной обратной связи и устойчивости электрических
сетей, Клод Шеннон продвигал теорию информации, описывавшую цифровые
сигналы, а Алан Тьюринг исследовал основы вычислений. В 1943 году Уолтер
Питтс и Уоррен Маккалок предложили модель искусственного нейрона и показали, как он может выполнять простые логические функции. В 1951 году Марвин
Мински в рамках своего дипломного проекта построил на базе автопилота от B24
и 3000 электронных ламп машину SNARC (Stochastic Neural Analog Reinforcement
Calculator – стохастический нейронно-аналоговый калькулятор с подкреплением), которая стала первой обучающейся машиной на основе нейронных сетей.
Роботизированные черепахи Уильяма Грея Уолтера демонстрировали поведение,
похожее на поведение живых существ (см. раздел 5.1). Исследователи предвкушали возможность создания электронного мозга!
Важной вехой стал выход книги Винера «Cybernetics or Control and Communication in the Animal and the Machine» (1965). Характерной чертой кибернетической
системы является использование обратной связи, распространенной в инженерных и биологических системах. Эти идеи впоследствии были применены в эволюционной биологии, психологии и экономике.
В 1956 году Джон Маккарти организовал конференцию в Дартмутском колледже, где присутствовали Мински, Шеннон, Герберт Саймон, Аллен Ньюэлл и другие.
На этой конференции был определен термин «искусственный интеллект» (ИИ)
в его современном виде, с акцентом на цифровые компьютеры и символьную обработку, что привело к началу новых исследований в области робототехники, компьютерного зрения, естественного языка, семантики и рассуждений. Маккарти
и Мински сформировали группу ИИ в Массачусетском технологическом институте, которую Маккарти покинул в 1962 году, чтобы основать Стэнфордскую лабораторию ИИ. Мински сосредоточился на искусственно упрощенном «мире блоков».
Саймон и его студент Ньюэлл оказали влияние на исследования ИИ в Университете Карнеги–Меллона, на базе которого в 1979 году был создан Институт робототехники. Эти группы, занимающиеся ИИ, оказали огромное влияние на развитие
робототехники и компьютерного зрения в США. Общества и издания, специализирующиеся на кибернетике, действуют и поныне.

42 Глава 1 · Введение

1.3

Определение робота

« Я не могу дать определение роботу, но я узнаю его, когда вижу.

– Джозеф Энгельбергер

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

ашина, способная ощущать, планировать и целенаправленно
« мдействовать.

Робот ощущает свое окружение и целенаправленно использует эту информацию, чтобы спланировать какое-то действие.
Действие может заключаться, например, в перемещении инструмента манипулятора робота для захвата объекта или в перемещении самого робота в другое место.
Восприятие (sensing) имеет решающее значение для роботов.
Проприоцептивные сенсоры измеряют состояние самого робота: угол изгиба сочленений руки, количество оборотов колеса
мобильного робота или ток, потребляемый электродвигателем.
Экстероцептивные сенсоры измеряют состояние внешнего
мира по отношению к роботу. Сенсор может быть простейшим
датчиком удара на роботе-пылесосе для обнаружения столк­
новений. Это может быть GPS-приемник, измеряющий расстояние до орбитальной группировки спутников, или компас,
измеряющий направление вектора магнитного поля Земли
относительно робота. Это также может быть активный сенсор,
испускающий акустические, оптические или радиоимпульсы
для измерения расстояния до некоторой точки в пространстве
и оценивающий ее удаленность по времени, которое необходимо, чтобы эхо импульса вернулось обратно.

1.4

Зрение роботов

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

Зрение роботов 43
а

Рис. 1.8. Разнообразие
глаз: а) муха-разбойница,
Holocephala fusca;
б) паук-скакун, Phidippus
putnami (изображения
а и б предоставлены
Томасом Шаханом, https://
thomasshahan.com);
в) морской гребешок
(снимок предоставил
Шонке Джонсен), каждая из
маленьких синих сфер – это
глаз; г) человеческий глаз.

в

б

г

Интересно отметить, что даже очень простые животные, например пчелы с мозгом, насчитывающим всего 106 нейронов
(по сравнению с нашими 1011), способны с помощью одного
лишь зрения решать сложные и жизненно важные задачи, такие как поиск пищи и доставка ее в улей. Для более высокоразвитых животных, таких как млекопитающие, преимущества
зрения перевешивают очень высокую биологическую цену обладания глазом: сам сложно устроенный глаз, мышцы, управляющие им, веки и слезные протоки для его защиты, а также
обширная зрительная кора (треть нашего мозга) для обработки
получаемых данных.
Зрение давно интересует исследователей в области робототехники: камеры могут имитировать функцию глаза и создавать изображения, а алгоритмы компьютерного зрения способны извлекать из изображений полезную информацию. Вместе
они позволяют реализовать такие полезные возможности для
роботов, как распознавание объектов и манипулирование ими,
а также навигация в пространстве. Например, робот-футболист может определить координаты круглого красного объекта
на поле, а дрон – оценить траекторию своего движения в пространстве, основываясь на том, как мир движется относительно
него. На рис. 1.9 показан робот с несколькими типами камер,
обеспечивающих навигацию на открытом воздухе.
Технологические разработки позволили оснащать роботы
камерами, выполняющими функцию глаз, и компьютерами,
выполняющими функции мозга. На протяжении большей части истории компьютерного зрения, начиная с 1960-х годов,
электронные камеры были громоздкими и дорогими, а вычис-

44 Глава 1 · Введение

лительные устройства (компьютеры) – недостаточно мощными. В настоящее время существуют КМОП-камеры, разработанные для мобильных телефонов и стоимостью всего в несколько
долларов, а современные персональные компьютеры обладают
огромной вычислительной мощностью и поддерживают параллельные вычисления. Новые алгоритмы, недорогие датчики
и высокая вычислительная мощность сделали зрение практичным датчиком, и этому посвящена данная книга.

Рис. 1.9. Группа камер
на мобильном роботе
для наружного применения:
стереопара переднего обзора,
широкоугольная камера бокового
обзора, зеркало с панорамной
камерой над головой (мобильный
робот CSIRO)

Существенное ограничение камеры, или глаза, – потеря
трехмерной структуры сцены в результирующем двумерном
изображении. Несмотря на это, люди способны очень хорошо
определять трехмерность сцены, основываясь на получаемой
ими зрительной информации. Один из подходов, используемых людьми и роботами, – стереозрение, когда информация,
поступающая от двух глаз, объединяется для оценки трехмерной структуры окружения. Марсоход, показанный на рис. 1.5а,
оснащен стереокамерой на мачте, а робот на рис. 1.9 – стереокамерой на турели.
Если окружающая среда робота неизменна, то он может
обойтись точной картой и отказаться от отслеживания состоя­
ния мира, за исключением определения своего собственного
положения. Представьте, что вы едете в машине с полностью

Этические аспекты робототехники 45

непрозрачными стеклами и все, что вам доступно, – это GPSнавигатор. Если бы дорога целиком была в вашем распоряжении, то вы, вероятно, смогли бы успешно доехать из пункта
А в пункт Б, хотя и пришлось бы понервничать.
Однако если на дороге будут также другие машины, пешеходы, светофоры или дорожные работы, то такой подход не сработает. Чтобы справиться с этой более реалистичной ситуацией,
необходима возможность видеть окружающий мир, ощущать
его и планировать свои действия соответствующим образом.
Люди с легкостью справляются с этой задачей, даже без участия
сознания, но пока сложно запрограммировать машину на то же
самое – это задача роботизированного зрения.
Отступление 1.7. Развитие глаза
Считается, что все глаза животных имеют общего предка в виде
протоглаза, который развился 540 млн лет назад. Однако наиболее крупные эволюционные успехи, по-видимому, произошли
всего за последние несколько миллионов лет. Самые ранние глаза, называемые глазными пятнами, представляли собой простые
участки белка-фоторецептора у одноклеточных животных, которые могли ощущать яркость света, но не его направление. Многоклеточные животные развили несколько глазных пятен, выстилающих небольшую зрительную ямку, что давало ограниченную
способность различать направленную яркость в зависимости от
того, какие клетки были освещены. Со временем ямка углубилась, отверстие стало меньше, а количество фоторецепторных
клеток увеличилось, образуя камеру-обскуру, способную различать формы. Затем последовало разрастание прозрачных клеток
для защиты глазного пятна, что привело к заполнению глазной
камеры и в конечном итоге – к глазу, который мы знаем сегодня. Глаз с хрусталиком развивался независимо семь раз у разных
видов. Природа создала десять совершенно разных конструкций
глаз, включая те, что показаны на рис. 1.8.

1.5

Этические аспекты робототехники

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

46 Глава 1 · Введение

в настоящее время навыки роботов в выполнении повседневных задач оставляют желать лучшего, их надежность и скорость
невысоки, а обходятся они довольно дорого. Однако весьма вероятно, что со временем эти проблемы будут преодолены – мы
не можем игнорировать тот факт, что многие задачи, выполняемые сейчас людьми, в будущем смогут выполняться роботами.
Проблема роботов и рабочих мест даже сегодня остается
сложной. Очевидно, что есть работы, которые людям не следовало бы выполнять, например работа во вредных или опасных
условиях. Существует множество низкоквалифицированных
работ, где найти человеческую рабочую силу становится все
сложнее, например сбор фруктов. Во многих развитых странах
люди не стремятся заниматься тяжелым физическим трудом на
открытом воздухе в отдаленных районах. Но кто-то же должен
выполнять эту работу! Возьмем тот же пример со сбором фруктов и рассмотрим возможные альтернативы: перестать есть
фрукты; резко повысить зарплату сборщикам фруктов (и увеличить стоимость фруктов); импортировать фрукты из других
мест (и увеличить стоимость фруктов, а также их влияние на
окружающую среду); использовать роботов для сбора местных
фруктов.
В таких областях, как обрабатывающая промышленность,
особенно автомобилестроение, развитие робототехники сыгра­
ло решающую роль в повышении производительности труда,
что позволило этой отрасли стать экономически жизнеспособной в странах с высоким уровнем заработной платы, таких
как Европа, Япония и США. Без роботов эти отрасли не смогли бы существовать: они не нанимали бы людей, не платили
бы налоги и не потребляли товары и услуги из других секторов
экономики. Да, автоматизированная промышленность создает
меньше рабочих мест, но она все равно вносит важный вклад
в развитие общества. Роботизация и автоматизация не отнимают рабочие места, а, наоборот, обеспечивают жизнеспособность обрабатывающей промышленности в странах с высокой
стоимостью рабочей силы. Как сбалансировать благо общества
и благо каждого человека?
Помимо рабочих мест, есть и другие проблемы. Взять, к примеру, беспилотные автомобили. Мы на удивление спокойно
относимся к автомобилям, которыми управляют люди, даже
притом что они ежегодно убивают более миллиона человек, однако многим не нравится идея беспилотных автомобилей, хотя
они значительно сократят количество аварий, в том числе и со
смертельным исходом. Мы беспокоимся о том, кто будет виноват, если роботизированный автомобиль совершит ошибку,
и это в то время, когда на дорогах продолжается кровавая бойня, устроенная водителями-людьми. Похожие опасения возникают, когда речь идет о роботизированной медицине и хи-

О книге 47

рургии: хирурги-люди не идеальны, но роботы, по-видимому,
несут гораздо большую ответственность. Много говорят об
использовании роботов для ухода за пожилыми людьми, но
ухудшает ли это качество их жизни, лишая их человеческого
контакта, разговоров и общения? Должны ли мы использовать
роботов для ухода за нашими детьми и даже для их обучения?
Что мы думаем об армиях роботов, сражающихся и убивающих
людей?
Роботизированный сбор фруктов, автомобили, здравоохранение, уход за пожилыми и детьми могут принести экономическую выгоду нашему обществу, но правильно ли это? Хотим
ли мы, чтобы наше общество развивалось в этом направлении?
Как нам сбалансировать благо общества с благом отдельного
человека? Это глубокие этические вопросы, которые не могут
и не должны решаться робототехниками в одиночку. Но робототехники не должны их игнорировать. Мы не должны идти
в технологическое будущее, как лунатики, просто «потому что
мы можем это». Наша обязанность – помогать формировать
будущее осознанно и гарантировать, что оно будет благом для
всех. Это важная дискуссия для всего общества, и робототехники обязаны принимать в ней активное участие.

1.6

О книге

Эта книга посвящена робототехнике и компьютерному зрению
по отдельности и их совокупности – роботизированному зрению. Это большая тема, и охват обязательно должен быть широким. Цели этой книги:
сформировать широкую и прочную основу для понимания
с помощью теории и примеров, чтобы сделать абстрактные понятия осязаемыми;
„„ научить читателя решать более сложные задачи в других
специализированных учебниках благодаря мощным вычислительным инструментам и лежащему в их основе программному обеспечению;
„„ предоставлять моментальную отдачу, решая сложные задачи с относительно небольшим количеством кода;
„„ дополнить множество отличных публикаций по робототехнике и компьютерному зрению;
„„ развивать интуицию посредством вычислительных экспериментов.
„„

Подход, использованный в этой книге, заключается в объединении предыстории, теории и примеров. Код и примеры яв-

48 Глава 1 · Введение

ляются важнейшими элементами этой книги и не помещаются
в конец главы или на веб-сайт издателя, а включены непосредственно в текст и выглядят примерно так:
>>> R = rot2(0.3)
array([[ 0.9553, -0.2955],
[ 0.2955, 0.9553]])

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

1.6.1

Python и книга
Хорошая работа начинается с хороших инструментов.
– Китайская пословица

Все вычисления в этой книге основаны на использовании Python 3 – мощного и популярного языка программирования,
который наверняка знаком студентам, исследователям и любителям. Базовую функциональность Python можно расширить
с помощью дополнительных пакетов. Python в сочетании с популярными пакетами, такими как NumPy, SciPy и Matplotlib ►,
предоставляет мощную интерактивную среду, которая упрощает использование линейной алгебры, анализ данных и создание высококачественной графики. Функциональность для
робототехники и компьютерного зрения обеспечивается дополнительными легко устанавливаемыми пакетами, которые
перечисляются ниже.
Spatial Maths Toolbox for Python включает функции для манипулирования данными, такими как векторы, мат­ри­цы
поворота, однородные преобразования, трехмерные представления, повороты и единичные кватернионы, необходимые для представления трехмерного положения, ориентации и позы.
„„ Robotics Toolbox for Python предоставляет широкий спектр
функций для моделирования мобильных роботов и манипуляторов. Toolbox поддерживает самый общий метод
представления структуры последовательных манипуляторов и предлагает функции для прямой и обратной кинематики, дифференциальной кинематики, динамики, визуализации и анимации. Toolbox включает и функции для
моделирования мобильных роботов, в том числе колесных
„„

Импортируйте
numpy, scipy
и matplotlib.
pyplot
соответственно.

О книге 49

транспортных средств и квадрокоптеров, а также контроллеров для них. Кроме того, в нем имеются стандартные
алгоритмы для планирования движения робота, определения местоположения, построения карт и SLAM.
„„ Machine Vision Toolbox for Python предоставляет богатый набор функций для моделирования камер, обработки изобра­
жений, извлечения признаков из изображений, многоракурсной геометрии и управления на основе машинного
зрения. Toolbox также содержит функции для получения
и отображения изображений, фильтрации, выделения
пятен, точек и линий, математической морфологии, деформации изображения, стереозрения, гомографии и вычисления фундаментальной мат­ри­цы, робастной оценки,
регулировки пучка, визуальных якобианов, геометрического моделирования камеры, калибровки камеры и операций
с цветовым пространством. Многие базовые операции реа­
лизованы с использованием популярного, эффективного
и зрелого пакета OpenCV.
„„ Block Diagram Simulator (bdsim) позволяет моделировать
и симулировать блок-схемы на Python. Он поддерживает
более 70 типов блоков, позволяет использовать в качестве
сигналов между блоками любые типы данных, допустимые
в Python, и поддерживает деление на подсистемы. Сложные модели можно выразить с помощью компактного кода
Python, а также используя графический редактор.
Отступление 1.8. Python
Python – интерпретируемый язык программирования высокого уровня, неизменно пользующийся высочайшей популярностью. Python был разработан Гвидо ван
Россумом в 1989 году как любительский проект. Свое название он получил в честь
британского сериала «Летающий цирк Монти Пайтона» (Monty Python’s Flying Circus), и в документации есть множество других подобных отсылок к спаму и сыру,
постоянно упоминаемых в сериале. Первая версия вышла в 1991 году, в 2000-м
появилась версия Python 2 (ее поддержка была прекращена в 2020 году на номере
версии 2.7.18), а в 2008-м – Python 3. Python наследует идеи многих предшествовавших ему языков программирования и поддерживает процедурные, объектноориентированные и функциональные парадигмы программирования. Обладает
понятным и простым синтаксисом, динамической типизацией и сборкой мусора.
Ключевая особенность Python – его высокая расширяемость с помощью пакетов. Основной репозиторий стороннего программного обеспечения для Python –
Python Package Index (PyPI; https://pypi.org) – содержит более 300 000 пакетов, которые можно установить с помощью команды pip.
Гвидо ван Россум (1956–) – голландский математик и информатик. Работал
в Centrum Wiskunde & Informatica (CWI) над языком программирования ABC, который оказал большое влияние на Python. До 2018 года был «пожизненным великодушным диктатором» Python, а в настоящее время является членом руководящего совета.

50 Глава 1 · Введение

Первые три пакета – это повторные реализации известного
пакета Toolboxes for MATLAB®.
Если вы только начинаете заниматься робототехникой или
машинным зрением, то наборы инструментов станут важной
исходной базой кода для вашего проекта. Перечисленные выше
наборы инструментов предоставляются в виде исходного кода.
В целом код инструментов написан как можно проще для облегчения понимания, возможно, в ущерб вычислительной эффективности. Приложение A содержит подробные сведения об
установке наборов пакетов и о том, как получить другие ресурсы книги, такие как примеры и рисунки.
В этой книге приводятся примеры использования многих
функций Toolbox в контексте решения конкретных задач, но
она не является справочным руководством. Полная документация по всем функциям Toolbox доступна в онлайн-документации, созданной на основе исходного кода.

1.6.2

У словные обозначения, соглашения
и структура книги

Математические обозначения, используемые в книге, сведены
в таблицу, размещенную во введении. Поскольку охват книги
широк, удобных символов не хватает, и некоторые символы неизбежно обозначают разные понятия в разных частях книги.
В книге много кода на Python, и он обозначен моноширинным шрифтом, например
>>> a = 6 * 7
a =
42

Три символа >>> – это приглашение к вводу командной строки Python, а за ними следует команда пользователя на языке
Python. Последующие строки без приглашения представляют
ответ Python. Длинные команды после переноса на следующую
строку начинаются с многоточия (...). Все функции, классы
и методы, упомянутые в тексте или в сегментах кода, снабжены перекрестными ссылками и имеют собственные указатели
в конце книги, позволяющие найти различные способы использования конкретных функций.
Для выделения и разграничения частей контента используются различные блоки.
В таких блоках сообщаются особенно важные сведения.

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

О книге 51
 Так оформляются предупреждения о некоторых аспектах, которые
часто становятся ловушками для новичков.

Они имеют вид
заметок на полях
с соответствующим
маркером в тексте.

Мне, как автору, пришлось преодолевать противоречия между
полнотой, ясностью и краткостью. По этой причине многие детали были помещены в выноски на полях и в выделенные блоки,
и при первом чтении их можно пропустить. В конце некоторых
глав есть раздел «Дополнительные материалы», который тоже
можно пропустить при первом чтении. Однако если вы пытаетесь понять конкретный алгоритм и применить его к своей собственной задаче, то понимание деталей и нюансов может быть
важным, и не стоит игнорировать примечания и дополнения.
Отступление 1.9
Такие блоки содержат техническую, историческую или биографическую информацию, которая дополняет основной текст, но
не имеет решающего значения для его понимания.

chap2.ipynb

go.sn.pub/ntdGFo

CoLaboratory
является товарным
знаком Google LLC,
и эта книга никак не
одобрена и не связана с Google. Некоторые анимации
и 3D-визуализации
(пока) не поддерживаются в CoLaboratory.

Каждая глава заканчивается разделом «Подведение итогов»,
в котором обобщаются важные уроки из главы, обсуждаются
рекомендации для дальнейшего чтения и предлагаются упражнения. В разделе «Дополнительная литература» перечислены
предшествующие работы, ссылки на них и более полное описание алгоритмов. В разделе «Ресурсы» приводятся ссылки на
соответствующий онлайн-код и наборы данных. Упражнения
укрепляют понимание материала главы и обычно связаны
с конкретными примерами кода в главе. Упражнения различаются по сложности: от простого дополнения примеров кода до
более сложных задач.
Многие примеры кода включают команду plot, которая генерирует рисунок. Рисунки в книге обычно снабжены подписями
осей, сеткой, легендами и рисками на осях. Для всего этого требуется множество дополнительных строк кода, которые загромождают примеры, поэтому я поместил в репозиторий GitHub
более детальные сценарии на Python, генерирующие все опуб­
ликованные рисунки.
Наконец, в книге содержатся ссылки на онлайн-ресурсы, к которым легко получить доступ через веб-браузер. Они имеют короткие URL-адреса (кликабельные в электронной версии) и QRкоды, размещенные на полях. Каждая глава содержит ссылку на
блокнот Jupyter Notebook в Google CoLaboratory™ (Colab), что
позволяет запустить код этой главы без установки на свой компьютер. Некоторые рисунки изображают трехмерные сцены,
и плоское двухмерное изображение не передает их в полной
мере – соответствующие им ссылки ведут к интерактивным
веб-средствам 3D-просмотра этих сцен.

52 Глава 1 · Введение

1.6.3

Целевая аудитория

Книга предназначена в первую очередь для студентов третьего
или четвертого курса инженерного бакалавриата, магистрантов и аспирантов. Студентам бакалавриата книга будет служить
вспомогательным пособием по курсу робототехники, механотроники или компьютерного зрения или для поддержки крупного проекта в области робототехники или машинного зрения.
Учащиеся должны изучить часть I и приложения для усвоения
основных понятий, а затем соответствующую часть книги:
мобильная робототехника, роботы-манипуляторы, машинное зрение или управление на основе зрения. Пакеты Toolbox
предоставляют надежный набор инструментов для различных
целей, а упражнения в конце каждой главы предлагают дополнительные задачи, помимо детальных примеров.
Студентам, поступающим в аспирантуру и ранее изучавшим
инженерное дело или компьютерные науки, книга поможет заполнить пробелы между знаниями, полученными в бакалавриа­
те, и тем, что потребуется для более глубокого изучения робототехники и машинного зрения. Примеры кода в книге помогут
начать исследования, быстрее приступить к работе и продуктивно трудиться над своими проблемами и идеями. Поскольку
исходный код свободно доступен, вы можете изменить его в соответствии с вашими потребностями, а когда придет время (как
это обычно бывает), реализовать ваши алгоритмы на каком-либо другом языке программирования, тогда можно использовать
Toolbox для перекрестной проверки вашей реализации.
Для тех, кто уже не является студентом, – исследователей или
практикующих специалистов, – книга послужит полезным дополнением к литературе по широкому кругу тем робототехники
и машинного зрения, а также справочником и руководством по
пакетам Toolbox.
Книга предполагает знание линейной алгебры (мат­ри­цы, векторы, собственные значения), базовой теории множеств и теории графов, исчисления, динамики (силы, моменты, инерция)
и теории управления на уровне бакалавра. В приложениях приводится сокращенное описание основных понятий. Студенты,
изучающие информатику, могут быть незнакомы с понятия­ми,
представленными в главах 4 и 9, такими как преобразование
Лапласа, передаточные функции, линейное управление (пропорциональное управление, пропорционально-дифференциальное управление, пропорционально-интегральное управление), и обозначениями блок-схем. При первом чтении эти
главы можно просто пробежать глазами, а работа Альбертоса
и Марелиса (Albertos и Mareels, 2010) может стать полезным
введением в некоторые из этих тем. Книга также предполага-

О книге 53

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

1.6.4

И версия 9
исходного кода
Robotics Toolbox
для MATLAB.

Обучение с помощью книги

Лучший способ обучения – это практика. Хотя в книге показаны
команды Python и результаты их выполнения, ничто не заменит
самостоятельную работу. Воспринимайте книгу как приглашение поработать. Выполняя команды самостоятельно, вы можете просматривать результаты так, как вам удобно, отображать
их по-другому или опробовать алгоритм на других данных или
с другими параметрами. Книга специально разработана так,
чтобы дать вам возможность вводить команды во время чтения. Вы также можете просмотреть онлайн-документацию по
функциям пакетов Toolbox, открыть для себя дополнительные
функции и параметры и поэкспериментировать с ними или
прочитать код, чтобы увидеть, как они работают на самом деле,
и, возможно, изменить их.
Большинство примеров довольно короткие, поэтому не составит труда ввести их, однако их очень много – более 1600.
Код для каждой главы доступен в виде блокнота Jupyter Notebook (см. приложение А), который позволяет выполнять код
ячейку за ячейкой или копировать и вставлять в свои файлы
сценариев.
Также предоставляется инструмент командной строки rvctool – оболочка для IPython, – который предварительно загружает все наборы инструментов с помощью инструкции import *
в текущее пространство имен. Возможно, это не лучшая практика программирования на Python, но она позволяет запускать
примеры именно так, как они написаны в книге.
Robot Academy (https://robotacademy.net.au/) – это бесплатный и открытый учебный ресурс, который содержит более
200 коротких видеоуроков по многим темам этой книги. Во
многих уроках для иллюстрации концепций используется код
MATLAB вместо Python. Однако в версиях пакетов Toolboxes
для Python существуют функции с похожими именами.

1.6.5

Преподавание с помощью книги

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

54 Глава 1 · Введение

робототехники или машинного зрения достаточно описать
только двухмерный случай. При преподавании роботизированных манипуляторов и многоракурсной геометрии необходимо
изучать­двухмерные и трехмерные случаи.
Весь код и большинство рисунков, что приводятся в этой
книге, доступны на веб-сайте книги, и вы можете свободно
использовать их при условии указания авторства. Весь код из
этой книги доступен в виде блокнотов Jupyter Notebook, которые можно использовать в качестве основы для демонстраций
на лекциях или в учебных пособиях. Рисунки представлены
в виде PDF-файлов, а для рисунков, сгенерированных кодом на
Python, доступны соответствующие сценарии, позволяющие их
воссоздать. Подробности ищите в приложении А.
Упражнения в конце каждой главы можно использовать в качестве основы для заданий, в качестве примеров для работы
в классе или при разработке учебных пособий. Некоторые вопросы специально рассчитаны на открытую дискуссию, чтобы стимулировать исследование влияния параметров и поиск
пределов производительности алгоритмов. Это исследование
должно сопровождаться дискуссиями о показателях эффективности и о том, какие из них наиболее информативны. Истинное
понимание алгоритмов включает оценку не только влияния параметров, но и того, как и при каких обстоятельствах алгоритмы терпят неудачу.
Подход к обучению также может быть обратным: сначала погрузиться в конкретную задачу, а затем разобрать соответствующий базовый материал. Подходящие задачи можно выбрать
из разделов «Применение» глав или из любых упражнений.
Особо сложные упражнения снабжены пометкой.
Если вы хотите использовать обратный подход к обучению,
то ряд задач можно найти в вышеупомянутой Robot Academy
(https://robotacademy.net.au). Студенты будут смотреть видео
и готовиться вне аудитории, а вы можете использовать время
в классе для работы над наборами задач.
Для преподавания в аспирантуре следует использовать
статьи и учебники, упомянутые в разделе «Дополнительные
материалы», которые также могут стать основой списка дополнительной литературы. Они также могут пригодиться при
проведении исследований и подготовке научных публикаций.

1.6.6

Структура книги

Я обещал книгу с моментальной отдачей, но, прежде чем мы
сможем приступить к изучению робототехники, необходимо
разобраться с фундаментальными понятиями. Часть I знако-

О книге 55

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

мит читателей с понятиями положений (поз, pose) и систем
координат – как мы представляем положение и ориентацию
робота, камеры или объектов, с которыми робот должен работать. Мы обсудим, как движение между двумя положениями
(позами) можно разложить на последовательность элементарных перемещений и поворотов и как из элементарных движений можно составить более сложные движения. В главе 2
обсуждается компьютерное представление поз, а в главе 3 –
взаимосвязь между скоростью и производной позы, генерация
последовательности поз, плавно следующих некоторому пути
в пространстве и времени, и оценка движения с помощью датчиков.
Оставив позади эти формальности, мы перейдем к главной
теме – роботам. Существует два важных класса роботов: мобильные роботы и манипуляторы, которые рассматриваются
в частях II и III соответственно.
Часть II начинается с главы 4, где рассматриваются модели
движения нескольких типов колесных транспортных средств
и многовинтового летательного аппарата. Рассматриваются различные законы управления колесными транспортными
средствами, такие как движение к точке, следование по заданной траектории и перемещение в заданную позу. Глава 5 посвящена навигации, т. е. как робот находит путь между точками А и
Б в реальном мире. Здесь рассматриваются два важных случая:
с картой и без нее. Большинство методов навигации требуют
знания местоположения робота, и в главе 6 мы рассмотрим различные подходы к решению этой задачи, основанные на счислении пути или наблюдении за ориентирами и карте. Мы также
посмотрим, как робот может составить карту и даже определить
свое местоположение, одновременно картографируя неизвестную область – задача SLAM.
Часть III посвящена роботам-манипуляторам, а точнее манипуляторам с последовательным интерфейсом. Манипуляторы используются для таких задач, как сборка, сварка, погрузка-разгрузка материалов и даже хирургия. Глава 7 вводит
понятие кинематики, которая связывает углы сочленений
робота с положением его инструмента в трехмерном пространстве. Обсуждаются методы создания плавных траекторий движения инструмента, а также показаны два примера,
как робот-манипулятор может нарисовать букву на поверхности и как несколько рук (действующих как ноги) можно
использовать для создания модели простого шагающего робота. Глава 8 рассматривает взаимосвязь между скоростями
изменения углов в сочленениях и положением инструмента,
мат­ри­цу Якоби и такие понятия, как сингулярность, управляемость, движение в нуль-пространстве и управление движением с заданной скоростью. Также обсуждаются роботы мало-

56 Глава 1 · Введение

приводные (underactuated) и многоприводные (overactuated)
и общее численное решение обратной кинематики. Глава 9
знакомит с системами управления сочленениями, динамическими уравнениями движения для манипуляторов с последовательным интерфейсом и взаимосвязью между силами,
действующими в сочленениях. В ней рассматриваются такие
важные темы, как изменение инерции, влияние полезной нагрузки, гибкие трансмиссии, сравнение стратегий управления с независимыми и нелинейными сочленениями, а также
управление в пространстве задач.
Компьютерное зрение – это обширная область, связанная
с обработкой и улучшением изображений для удобства человека, интерпретации содержимого сцены или создания трехмерной модели, соответствующей сцене. Часть IV посвящена
машинному зрению, подвиду компьютерного зрения, которое
здесь определяется как извлечение числовых характеристик
из изображений с целью формирования входных данных для
управления роботом. Обсуждение начинается в главе 10 с описания основных понятий, таких как свет, освещение и цвет.
Глава 11 обсуждает обработку изображений – область обработки двумерных сигналов, в ходе которой одно изображение
преобразуется в другое. Обсуждение начинается с получения
изображений реального мира, а потом рассматриваются различные арифметические и логические операции с изображениями. Затем вводятся пространственные операторы, такие
как свертка, сегментация, морфологическая фильтрация и, наконец, изменение формы и размера изображения. Эти операции лежат в основе обсуждения в главе 12, которое описывает,
как извлекать числовые признаки из изображений. Признаки
описывают однородные области (пятна), линии или отдельные
точки на сцене и являются основой для управления роботом
на основе зрения. Также рассматривается применение методов глубокого обучения для обнаружения объектов. Глава 13
описывает геометрическую модель создания перспективного изображения с помощью линз и обсуждает такие темы, как
калибровка камеры и оценка положения (позы). Здесь будет
представлена тема неперспективной визуализации с использованием широкоугольных объективов и зеркальных систем,
массивов камер и камер светового поля. В главе 14 рассматриваются приемы оценки геометрии простой трехмерной сцены
с использованием классических методов, таких как структурированное освещение, а также комбинирование характеристик,
обнаруженных в разных ракурсах одной и той же сцены. Эти
приемы помогают получать информацию о геометрии и пространственном соотношении между ракурсами камер, которая
кодируется в фундаментальных, существенных и гомографических мат­ри­цах. Мы также обсудим тему определения струк-

О книге 57

туры объекта по движению, а также такие приложениям, как
коррекция перспективы, монтирование и поиск изображений
и визуальная одометрия.
В части V обсуждается, как можно использовать визуальные
признаки, получаемые с камеры, для управления манипуляторами и мобильными роботами. Этот подход известен как
управление на основе зрения или визуальное сервоуправление. В данной части обобщаются концепции, представленные
в предыдущих частях книги. Глава 15 знакомит с классическими подходами к визуальному сервоуправлению, известными
как визуальное сервоуправление на основе оценки положения
и визуальное сервоуправление на основе изображения, а также
рассматривает их ограничения. Глава 16 обсуждает более современные подходы, устраняющие эти ограничения, а также
использование неперспективных камер, малоприводных и мобильных роботов.
Это большая книга, но любую из частей можно читать отдельно, периодически обращаясь к рассмотренным ранее понятиям. По-настоящему обязательной для чтения в этой книге
является только глава 2. Части II, III или IV могут быть использованы соответственно для преподавания вводного или продвинутого курса мобильных роботов, роботов-манипуляторов или
компьютерного зрения. Альтернативный подход, основанный
на теме мгновенной отдачи, состоит в том, чтобы сразу же перейти к любой главе и начать изучение, заглядывая в предыдущие
главы по мере необходимости.

1.6.7

Дополнительное чтение

«Handbook of Robotics» (Siciliano and Khatib, 2016) представляет
энциклопедическое освещение современной робототехники,
включая теорию, технологию и различные типы роботов, такие
как телероботы, сервисные роботы, полевые роботы, воздушные роботы, подводные роботы и т. д. В классической работе
Шеридана (Sheridan, 2003) рассматривается спектр автономности: от дистанционного управления, через совместное и коммерческое управление, до полной автономности.
Подробный обзор компьютерного зрения можно найти
в книге Селиски (Szeliski, 2022). Увлекательные статьи о глазах и биологии зрения можно найти в работах Шринивасана
и Венкатеша (Srinivasan and Venkatesh, 1997), Лэнда и Нильссона (Land and Nilsson, 2002), Ингса (Ings, 2008), Фрисби и Стоуна
(Frisby and Stone, 2010) и Стоуна (Stone, 2012). Введение в искусственный интеллект можно найти в книге Рассела и Норвига
(Russell and Norvig, 2020).

58 Глава 1 · Введение

В ряде очень интересных книг обсуждается будущее влияние
робототехники и искусственного интеллекта на общество, например в книгах Форда (Ford, 2015), Бриньолфссона и Макафи
(Brynjolfsson and McAfee, 2014) и Бострома (Bostrom, 2016). Видеоролик на YouTube «Grey» (Grey, 2014) содержит ряд важных
тезисов о будущем труда и может служить отличным поводом
для обсуждения.

Часть I Основы
Глава 2
Глава 3

Представление местоположения и ориентации
Время и движение

Глава

2

	Представление
местоположения
и ориентации

Мы уже умеем использовать числа для счета и измерения,
а кортежи чисел (координаты) – для описания местоположений
точек в двух- или трехмерном пространстве. В робототехнике
особый интерес для нас будет представлять местоположение
объектов в пространстве – таких как мобильные роботы, звенья
и инструменты роботизированного манипулятора, камеры или
заготовки. В разделе 2.1 мы познакомимся с основными понятиями, а затем в разделах 2.2 и 2.3 применим эти понятия к двумерным и трехмерным сценариям соответственно, с которыми
приходится сталкиваться в робототехнике. Раздел 2.4 охватывает некоторые дополнительные темы, и его можно пропустить
при первом прочтении, а раздел 2.5 содержит дополнительные
сведения о Python Toolbox – библиотеке, которую мы будем использовать на протяжении всей книги.

2.1

chap2.ipynb

go.sn.pub/ntdGFo

Основы

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

2.1.1

Относительное положение

В двух- и трехмерных примерах на рис. 2.1 показаны красные
и синие версии объекта, имеющие разное местоположение
и ориентацию. Сочетание местоположения и ориентации 
объекта определяет его позу, или положение, т. е. красная и синяя версии объекта имеют разные позы (положение).
Исходное положение объекта, показанное синим цветом,
было преобразовано в положение, показанное красным. Фор-

Ориентацию
часто называют
пространственным
положением.

Основы 61

ма объекта не изменилась, поэтому такое преобразование называется преобразованием твердого тела. Это преобразование
можно рассматривать как движение: объект переместился по
некоторой сложной траектории. Изменились его местоположение (он был перемещен) и ориентация (он был повернут).

Рис. 2.1. Движение твердого
тела: а) прямоугольный
двухмерный объект;
б) трехмерный объект
в форме кубоида. Начальное
положение (изображено
синим цветом) – это вид
«сбоку» трехмерного кубоида а

ξ

ξ¢

б

Для обозначения движения мы будем использовать символ ξ
(произносится «кси»), а на графиках то же движение будет представляться в виде толстой изогнутой стрелки. Важно отметить,
что, как показано на рис. 2.2, движение всегда определяется относительно исходного положения.

ξ

ξ

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

Рассмотрим простой двухмерный пример, показанный на
рис. 2.3. Мы используем специальную систему обозначений, чтобы было понятно, откуда и куда происходит движение. Верхний
индекс определяет начальное положение, а нижний – конечное.
То есть xξy означает движение из положения x в положение y.
Движения можно объединять в цепочки для создания движений по траекториям произвольной сложности – этот процесс
называется композицией, или составлением. Алгебраически это
можно записать как
ξ2 = 0ξ1 ⊕ 1ξ2.

0

(2.1)

62 Глава 2 · Представление местоположения и ориентации

То есть движение 0ξ2 можно представить как движение 0ξ1, за
которым следует движение 1ξ2. Мы используем специальный
символ ⊕, чтобы показать, что в данном случае складывают­
ся движения, а не простые числа. Движения выполняются последовательно, а это означает, что названия положений по обе
стороны от ⊕ должны быть одинаковыми. На рис. 2.3 показаны
только поступательные движения, но составляемые движения
могут включать повороты. В трехмерном случае выбор движений шире – больше направлений для поступательных движений и больше осей вращения.

ξ2

0

положение 0

положение 2

0

ξ1

положение 1

1

ξ2
Рис. 2.3. Движение из положения 0
в положение 2 эквивалентно движению
из положения 0 в положение 1, а затем
в положение 2

Важно отметить, что порядок движений важен. Например,
если пройти на 1 м вперед, а затем повернуться по часовой
стрелке на 90°, то получится совсем другое положение, чем если
сначала повернуться по часовой стрелке на 90°, а затем пройти
на 1 м вперед. Композиция движений некоммутативна.
Как следствие любое движение можно разложить на ряд более простых движений. Для двухмерного случая, представленного на рис. 2.1, одно из возможных разложений движения ξ
представляет собой перемещение по горизонтали, за которым
следует перемещение по вертикали, а затем поворот вокруг
центра фигуры.
Для любого движения существует обратное, или противо­
положное, движение – движение, возвращающее объект в исходное положение. Например, обратным перемещению на 2 м
вправо является перемещение на 2 м влево, а обратным повороту на 30° по часовой стрелке является поворот на 30° против
часовой стрелки. В общем случае обратным движению из положения X в положение Y является движение из положения Y
в положение X, то есть
X

ξY ⊕ YξX = XξX = Ø,

(2.2)

где Ø – нулевое (пустое) движение и означает отсутствие дви­
жения.
Введем оператор ⊖, превращающий любое движение в обратное ему движение

Основы 63

ξX ≡ ⊖ XξY,

Y

и теперь мы можем записать (2.2) как
ξY ⊖ XξY = Ø.

X

Это действие очень похоже на вычитание, но чтобы напомнить себе, что вычитаются не простые числа, а движения, мы
используем специальный символ ⊖. Из вышесказанного следует, что добавление или вычитание нулевого движения из некоторого движения не меняет самого движения, то есть
ξY ⊕ Ø = XξY ,

X

ξY ⊖ Ø = XξY.

X

Например, движение на рис. 2.3 можно записать алгебраически так:
ξ1 = 0ξ2 ⊕ 2ξ1,

0

и хотя на рис. 2.3 нет стрелки для 2ξ1, это будет просто стрелка, обратная стрелке для 1ξ2. Поскольку 2ξ1 = ⊖ 1ξ2, мы можем
записать­
ξ1 = 0ξ2 ⊖ 1ξ2.

0

Что очень похоже на (2.1), за исключением того, что мы фактически вычитаем 1ξ2 из обеих частей уравнения – точнее, мы
вычитаем его из правого члена в обеих частях уравнения, поскольку порядок имеет важное значение. Разворачивая эту
запись­по шагам, получаем
ξ2 = 0ξ1 ⊕ 1ξ2

0

ξ2 ⊖ 1ξ2 = 0ξ1 ⊕ 1ξ2 ⊖ 1ξ2

0
0

ξ2 ⊖ 1ξ2 = 0ξ1 ⊕ ∅
= 0ξ1.

ξ2 = 0ξ1 ⊕ 1ξ2

0

Так же как в обычной алгебре, все то же самое можно сделать
за один шаг, «перенеся 1ξ2 на другую сторону» и изменив знак
на «противоположный», как показано в предыдущем уравнении
справа.
В начале мы говорили о положениях объектов (рис. 2.1). Не
существует способа описать абсолютное положение объекта –
его можно описать только относительно какого-либо другого
положения, и для этой цели мы введем понятие опорного по­
ложения. Любое положение всегда является относительным
и описывается движением, которое необходимо выполнить,
чтобы переместиться в это положение из опорного.

64 Глава 2 · Представление местоположения и ориентации

2.1.2

Системы координат

Для описания относительного положения необходимо описать
две его составляющие: перемещение и поворот. Для этого мы
жестко привяжем к объекту правую систему координат, как показано на рис. 2.4. Система координат – это знакомое понятие
из математики. Она состоит из двух или трех ортогональных
осей, пересекающихся в точке, называемой началом координат.

https://go.sn.pub/
VH3R8d

Рис. 2.4. Объект с привязанной
системой координат в двух различных
положениях. Оси трехмерных систем
координат часто окрашены в красный,
зеленый и синий цвета (X, Y и Z
соответственно). Опорная система
координат показана черным цветом

Для полного описания положения и ориентации объекта достаточно знать положение и ориентацию системы координат,
к которой он прикреплен. Каждая система координат имеет
имя, в данном случае {A} или {B}, и это имя также отражается
в индексах на обозначениях осей. Смещение – это расстояние
между началами двух систем координат, а поворот может быть
описан набором углов между соответствующими осями систем
координат. Вместе они представляют собой изменение положения, относительное положение или движение, описываемое ξ.
Система координат, связанная с исходным положением, обозначается как {0} и называется базовой системой координат, ми­
ровой системой координат (обозначается как {W}) или глобаль­
ной системой координат. Если первый верхний индекс опущен,
то подразумевается исходная система координат. Система координат, связанная с движущимся телом, например транспортным средством, часто называется системой координат тела
(body frame), или системой координат, связанной с телом (bodyfixed frame), и обозначается как {B}. Любая точка движущегося
тела определяется постоянным вектором координат в системы
координат тела.
Точку P на рис. 2.5 можно описать относительно любой системы координат векторами Ap (относительно системы {A}) и Bp
(относительно системы {B}). Базисом систем координат являются единичные системы координат. Эти два вектора координат
связаны соотношением
A

p = AξB • Bp,

Основы 65

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

Рис. 2.5. Точка P может быть описана
координатными векторами относительно
любой системы координат {A} или {B}.
Положение {B} описывается относительно
{A} – AξB

Правая сторона выражает движение от {A} к {B}, а затем к P.
Такая композиция движения и вектора обозначается оператором •, чтобы отличить ее от композиции собственного движения, обозначаемой оператором ⊕.
Системы координат – чрезвычайно полезный способ представления реальных задач робототехники, таких как на рис. 2.6.
Здесь мы прикрепили системы координат к ключевым объектам и далее будем рассматривать взаимосвязи между положениями, которые они представляют.

Рис. 2.6. Примеры систем координат в реальной ситуации. Здесь показаны системы
координат: мобильного робота {M}, основания манипулятора {B}, рабочий орган
манипулятора {E}, камеры {C}, обрабатываемого объекта {P} – и базовая система
координат {0} (фотография Джона Скиннера и Дориана Цая)

66 Глава 2 · Представление местоположения и ориентации

Мы разработали неформальный набор правил для работы с движениями. Движения можно комбинировать:
ξZ = XξY ⊕ YξZ.

(2.3)

ξX =⊖ XξY.

(2.4)

X

Это выражение читается как: «движение от X к Z, эквивалентно движению от X к Y, а затем от Y к Z». Имена положений по обе
стороны от оператора ⊕ должны совпадать.
Существует оператор обратного движения

Y

А нулевое движение обозначается как ∅, таким образом:
ξX = ∅,

X

ξY ⊖ XξY =∅,

X

ξY ⊕ ∅ = XξY,

X

⊖XξY ⊕ XξY = ∅,

ξY ⊖ ∅ =XξY,

X

∅ ⊕ XξY =XξY.

(2.5)

Движение преобразует вектор координат точки так, чтобы отразить изменение в системе координат, обусловленное этим движением.
p = XξY • Yp.

X

(2.6)

Хотя все это выглядит как набор обычных алгебраических
выражений, важно помнить, что ξ – это не число, а относительное положение, или движение. В частности, порядок операций
имеет значение, особенно когда появляются повороты. Мы не
должны предполагать коммутативность операций, как при работе с обычными числами. Формально ξ принадлежит неабелевой
группе с оператором ⊕, инверсией ⊖ и единичным элементом ∅.
В литературе вместо ⊕ принято использовать символ * или •,
а инверсию обозначать как X -1 вместо ⊖X.
Отступление 2.1. Правило системы координат
В композиции относительного положения мы можем
проверить правильность наших систем отсчета, убедившись, что нижний и верхний индексы по обе стороны
оператора ⊕ совпадают. Затем мы можем сократить
нижние и верхние индексы в середине и оставить только
крайние.

2.1.3

совпадают

Графы положений

На рис. 2.7 показан граф положения – ориентированный граф
►, который состоит из вершин (синие круги), представляющих
положения, и ребер (стрелок), представляющих относительные
положения, или движения. Это наглядное, но абстрактное представление пространственных отношений, присутствующих
в задаче, показанной на рис. 2.6. Графы положений можно ис-

Более подробную
информацию
о графах см.
в приложении I.

Основы 67
Отступление 2.2. Группы
Группы – очень удобное и важное математическое понятие, непосредственно связанное с нашим обсуждением движения и относительного положения. Формально
говоря, группа – это абстрактная система, представленная упорядоченной парой 𝒢
= (G, ⟡), которая включает множество объектов G, поддерживающих единственную
бинарную операцию, обозначаемую как ⟡, которая объединяет любые два элемента группы, образуя третий элемент, также принадлежащий этой группе. Группа
удовлетворяет следующим аксиомам:
Замыкание
Ассоциативность

a⟡b∈G
(a ⟡ b) ⟡ с =
a ⟡ (b ⟡ с)
a⟡e=e⟡a=a

Нейтральный
(нулевой) элемент
Обратный элемент a ⟡ a-1 =
a-1 ⟡ a = e

∀a, b ∈ G
∀a, b, c ∈ G

∀a ∈ G, ∃e ∈ G

∀a ∈ G, ∃a-1 ∈ G

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

рабочий орган
манипулятора

основание
манипулятора
камера

Рис. 2.7. Граф положений
для пространственного
примера на рис. 2.6. Синие
круги – это вершины графа,
они представляют положения,
а стрелки – это ребра графа, они
представляют относительные
положения, или движения

мобильный
робот

обрабатываемый
объект

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

пользовать для представления двух- и трехмерных задач – все
зависит от того, как реализуется ξ.
Черные стрелки представляют известные относительные положения, а серые – неизвестные, которые нужно определить.
Чтобы робот мог захватить обрабатываемый объект, необходимо знать его положение относительно рабочего органа ро-

68 Глава 2 · Представление местоположения и ориентации

бота, то есть EξP. Сначала нужно отыскать пару эквивалентных
траекторий – двух различных траекторий с одинаковыми начальным и конечным положениями, одна из которых включает
неизвестную траекторию. Для данного примера мы выберем
траектории, показанные красными пунктирными линиями.
Обе траектории имеют одинаковые начальное и конечное положения, соответственно, они представляют эквивалентные
движения, и их можно приравнять.
ξM ⊕ MξB ⊕ BξE ⊕ EξP = 0ξC ⊕ CξP.

0

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

ξP = ⊖ BξE ⊖ MξB ⊖ 0ξM ⊕ 0ξC ⊕ CξP.

Такое выражение легко записать, просто взглянув на него.
Чтобы определить XξY, найдите любую траекторию в графе из
положения {X} в положение {Y} и запишите ребра, представляющие относительные положения слева направо. Если переход по
ребру производится в направлении стрелки, то перед ним следует поставить оператор ⊕, в противном случае используйте ⊖.

2.1.4

Краткое резюме

На рис. 2.8 представлено графическое изображение только что
рассмотренных основных понятий. Вот главное, что следует запомнить:
местоположение и ориентация объекта называются его
положением, или позой;
„„ движение, обозначаемое как ξ, вызывает изменение положения – это относительное положение, определяемое относительно исходного положения;
„„ такого понятия, как абсолютное положение, не существует; положение всегда относительно к исходному;
„„ с выражениями, записанными в терминах относительных
положений, можно выполнять алгебраические манипуляции, используя операторы ⊕ и ⊖, а также концепцию нулевого движения ∅;
„„ набор положений с известными относительными положениями можно представить в виде графа положений;
„„ чтобы количественно определить положение, мы жестко
привязываем к объекту систему координат. Начало этой
системы соответствует местоположению объекта, а направления осей системы описывают его ориентацию;
„„

Основы 69
преобразует вектор
координат из кадра
{Y} в кадр {X}

υ = XξY · Yυ

X

произносится как «кси»

= ⊖ YξX
положение системы
координат {Y} относительно
системы координат{X}

ξY = ξX ⊕ XξY

Рис. 2.8. Все, что нужно
знать об относительном
положении

относительное движение
из системы координат {X}
в систему координат{Y}

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

До сих пор мы игнорировали детали происходящего на самом деле. Это было сделано намеренно, чтобы заставить вас думать о положениях объектов и движениях между ними в целом,
не вникая в конкретные детали реализации. Истинная природа ξ зависит от задачи. Является это двумерной или трехмерной
задачей? Содержит ли она перемещения, повороты или и то,
Отступление 2.3. Евклид Александрийский
Евклид (325–265 гг. до н. э.) – греческий математик,
который родился и жил в Александрии (Египет)
и считается «отцом геометрии». Его великий труд
«Начала», состоящий из 13 книг, собрал и систематизировал многие ранние знания о геометрии
и числах. Он выводит свойства плоских и объемных
геометрических фигур из набора 5 аксиом и 5 постулатов.
«Начала» – это, пожалуй, самая успешная книга в истории математики. Она описывает планиметрию, и с ее постулатов начинается знакомство
большинства людей с геометрией и формальными
доказательствами, а также является основой того,
что мы сейчас называем евклидовой геометрией.
Евклидово расстояние – это просто расстояние между двумя точками на плоскости. Евклид также написал «Оптику», которая описывает геометрию зрения и перспективу.

70 Глава 2 · Представление местоположения и ориентации
Отступление 2.4. Евклидова геометрия против декартовой
Евклидова геометрия связана с точками и прямыми на евклидовой плоскости (2D) или в евклидовом пространстве
(3D). Она полностью основана на наборе аксиом и не использует арифметику. Декарт добавил систему координат (2D или
3D) и смог описать точки, прямые и различные кривые с помощью алгебраических уравнений. Изучение таких уравнений называется аналитической геометрией и лежит в основе всей современной геометрии. Декартова плоскость (или
пространство) представляет собой евклидову плоскость (или
пространство) со всеми ее аксиомами и постулатами плюс
дополнительные возможности, предоставляемые системой
координат. Термин «евклидова геометрия» часто используется для обозначения того, что выполняется пятый постулат
Евклида (параллельные прямые никогда не пересекаются),
что справедливо для плоской поверхности, но не работает на
искривленной.

B
A
C
Евклидова

B
A
C
Декартова

и другое? Реализацией может быть вектор, мат­ри­ца или что-то
более экзотическое, например кручение, единичный кватернион или единичный дуальный кватернион.
Отступление 2.5. Рене Декарт
Декарт (1596–1650) – французский философ,
математик и по совместительству наемник.
Он известен своим философским утверждением «Cogito, ergo sum», или «Я мыслю,
следовательно, я существую». Он был болезненным ребенком и на всю жизнь выработал
привычку лежать в постели и думать до позднего утра. Легенда гласит, что в одно такое
утро он наблюдал за мухой, прогуливающейся по потолку, и понял, что может описать ее
положение с точки зрения расстояния от двух
краев потолка. Эта догадка легла в основу
декартовой системы координат и современной (аналитической) геометрии, которые он
описал в своей книге 1637 г. «Геометрия».
Впервые математика и геометрия слились
воедино, и на этом фундаменте Ньютон и Лейбниц построили современный математический анализ. Находясь в Швеции по приглашению королевы Кристины,
Декарт был вынужден вставать в 5 утра, нарушив жизненную привычку, – вероятно, оттого он вскоре заболел воспалением легких и умер. Позже его останки были
перевезены в Париж и в настоящее время утеряны, за исключением его черепа,
который находится в Музее человека. После его смерти Римско-католическая церковь поместила его произведения в Индекс запрещенных книг, который был отменен в 1966 году.

Работа в двух измерениях (2D) 71

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

2.2
Относительная
ориентация осей x
и y подчиняется
правилу правой
руки. В двумерной
системе координат
ось y получается
поворотом оси x
против часовой
стрелки на 90°.

Работа в двух измерениях (2D)

Двухмерный мир, или плоскость, знаком нам из школьного
курса евклидовой геометрии. Мы используем правостороннюю
◄ декартову систему координат – систему координат с ортогональными осями, обозначаемыми x и y, и обычно рисуем с горизонтальной осью x и вертикальной осью y. Точка пересечения
осей называется началом координат.
Базисные векторы – это единичные векторы, параллельные
осям, и обозначаются как x̂ и ŷ. Точка представляется своими
координатами x и y в виде записи (x, y) или в виде вектора координат, исходящего из начала координат:
p = xx̂ + yŷ,

(2.7)

который является линейной комбинацией базисных векторов.
На рис. 2.9 показана красная система координат {B}, которую нужно описать относительно синей системы отсчета {A}.
Мы ясно видим, что начало координат {B} смещено, а оси повернуты против часовой стрелки. Рассмотрим эту задачу в два
приема: сначала обсудим один только поворот, а затем поворот
и перенос вместе.

Рис. 2.9. Две двухмерные системы
координат {A} и {B}, определенные
относительно мировой системы
координат {0}. Система {B} повернута
и смещена относительно {A}

2.2.1

Ориентация в двух измерениях

2.2.1.1

Матрица вращения в двухмерном пространстве

На рис. 2.10 показаны две системы координат {A} и {B} с общим
началом координат, но разной ориентацией. Система {B} по-

72 Глава 2 · Представление местоположения и ориентации

Рис. 2.10. Повернутые системы координат
в двухмерном пространстве. Базисные
векторы показаны толстыми стрелками

лучается поворотом системы {A} на угол в положительном направлении (против часовой стрелки) вокруг начала координат.
Система координат {A} описывается направлениями ее осей,
параллельных базисным векторам x̂A и ŷA, заданных относительно системы координат {0}. В этом двухмерном примере базисные векторы состоят из двух элементов, которые принято
записывать как векторы-столбцы и располагать рядом так, чтобы образовывалась мат­ри­ца 2×2 (x̂A ŷA). Эта мат­ри­ца полно­
стью описывает систему координат {A}.
Аналогично система координат {B} полностью описывается
своими базисными векторами x̂B и ŷB, которые можно выразить
через базисные векторы системы координат {A}:
x̂B = x̂A cos θ + ŷA sin θ
ŷB = -x̂A sin θ + ŷA cos θ
или в матричной форме:
(2.8)

где

является особой мат­ри­цей, которая называется мат­ри­цей вра­
щения и преобразует систему координат {A}, описываемую мат­
ри­цей (x̂A ŷA), в систему координат {B}, описываемую мат­ри­цей
(x̂B ŷB). Матрицы вращения обладают рядом особых свойств:
столбцы – это базисные векторы, которые определяют оси
повернутой системы координат и по определению являются как единичными, так и ортогональными;
„„ она ортогональная (также называется ортонормированной), ► и вследствие этого обратная и транспонированная
мат­ри­цы совпадают, т. е. R-1 = RT;
„„

См. приложение B,
в котором можно
освежить свои
знания о векторах,
мат­ри­цах и линейной алгебре.

Работа в двух измерениях (2D) 73

умножение мат­ри­цы на вектор Rυ сохраняет длину и относительную ориентацию вектора υ, а это означает, что
определитель мат­ри­цы равен +1;
„„ принадлежит группе специальных ортогональных матриц
с размерностью 2, что можно записать как R ∈ SO(2) ⊂ ℝ2×2.
Это означает, что произведение любых двух матриц принадлежит группе, как и обратная мат­ри­ца.
На рис. 2.5 показаны точка P и две системы координат, {A}
и {B}. Точку можно описать вектором координат (Apx, Apy) относительно системы {A} или вектором координат (Bpx, Bpy) относительно системы координат {B}. Опираясь на (2.7), координатный
вектор можно записать в матричной форме в виде линейной
комбинации базисных векторов:
„„

(2.9)
(2.10)
Подстановка (2.8) в (2.10) дает:
(2.11)
и меняет систему координат (Ap вместо Bp) в левой части, потому что теперь она выражается через базисные векторы {A}.
Приравнивая два определения Ap в (2.9) и (2.11), можно записать
(2.12)
и затем, приравнивая подчеркнутые коэффициенты, получить
(2.13)
Это показывает, что мат­ри­ца вращения ARB(θ) преобразует
вектор координат из системы координат {B} в систему{A}.
Матрица вращения имеет все характеристики относительного положения ξ, описанные формулами с (2.3) по (2.6).
ξ как мат­ри­ца SO(2)
В случае простого поворота в двухмерном пространстве движение ξ можно реализовать как мат­ри­цу вращения R ∈ SO(2), обладающую следующими свойствами:

74 Глава 2 · Представление местоположения и ориентации
композиция
ξ1 ⊕ ξ2
обращение
⊖ξ

тождественный ∅
элемент
векторное
ξ•υ
преобразование

↦ R1R2, умножение матриц;
↦ R–1 = RT, транспонирование
мат­ри­цы;
↦ R(0) = 12×2, единичная мат­ри­ца;

↦ Rυ, умножение мат­ри­цы
на вектор.

Композиция коммутативна, т. е. R1R2 = R2R1 и R(-θ) = RT(θ).

Для большей наглядности создадим мат­ри­цу вращения SO(2)
с помощью программного обеспечения Toolbox:
>>> R = rot2(0.3)
array([[ 0.9553, -0.2955],
[ 0.2955, 0.9553]])

где углы указаны в радианах. Ориентация, определяемая мат­
ри­цей вращения, может быть представлена в виде системы координат.
>>> trplot2(R);

Мы можем исследовать некоторые из только что упомянутых
свойств, например убедиться, что ее определитель равен единице:
>>> np.linalg.det(R)
1

Умножение двух матриц вращения дает в результате мат­ри­
цу вращения
>>> np.linalg.det(R @ R)
1

Обратите внимание, что здесь для обозначения матричного
умножения массивов NumPy вместо * использован оператор @.
Toolbox также поддерживает символьную математику, ► например
>>> from sympy import Symbol, Matrix, simplify
>>> theta = Symbol('theta')
theta
>>> R = Matrix(rot2(theta)) # преобразование в мат­ри­цу SymPy
Matrix([
[cos(theta), -sin(theta)],
[sin(theta), cos(theta)]])
>>> simplify(R * R)
Matrix([

Вам потребуется
установить SymPy.
Обратите внимание, что в SymPy
для умно­жения
скаляров и массивов используется
оператор *.

Работа в двух измерениях (2D) 75
[cos(2*theta), -sin(2*theta)],
[sin(2*theta), cos(2*theta)]])
>>> R.det()
sin(theta)**2 + cos(theta)**2
>>> R.det().simplify()
1

2.2.1.2

Матричная экспонента для определения поворота

Существует интересная и очень полезная связь между мат­ри­
цей вращения и экспонентой кососимметричной мат­ри­цы. Это
легко продемонстрировать, взяв чистый поворот на 0,3 радиана, выраженный в виде мат­ри­цы вращения.
>>> R = rot2(0.3);
Функция logm отМы можем вычислить логарифм мат­ри­цы, используя функличается от функции цию logm из пакета SciPy :
np.log, которая вычисляет логарифм
>>> L = linalg.logm(R)
каждого элемента
0,
-0.3],
мат­ри­цы. Логарифм array([[
[
0.3,
0]])
можно вычислить
как степенной ряд,
только с мат­ри­цей и получить простую мат­ри­цу, содержащую всего два ненулевых
вместо скалярного
элемента со значением 0,3, определяющим угол поворота. Эта
аргумента. Логарифм мат­ри­цы не мат­ри­ца является примером кососимметричной мат­ри­цы 2×2.
является уникаль- Она имеет только один уникальный элемент:
ным и, в частности,
logm вычисляет >>> S = vex(L)
главный логарифм.

array([0.3])

определяющий угол поворота в виде массива с одним элементом, а не скаляра. Это наше первое знакомство с теорией групп
Ли, и мы продолжим изучать эту тему в оставшейся части главы.
Возведение в степень логарифма мат­ри­цы вращения L с помощью функции вычисления экспоненты мат­ри­цы expm дает
исходную мат­ри­цу вращения:

Функция expm отличается от функции
exp, которая вычисляет экспоненту
каждого элемента
мат­ри­цы. Матрич- >>> linalg.expm(L)
ную экспоненту array([[ 0.9553, -0.2955],
можно вычислить
[ 0.2955, 0.9553]])
как степенной ряд,
только с мат­ри­цей
Экспонента кососимметричной мат­ри­цы всегда является
вместо скалярмат­
ри­цей вращения, обладающей всеми особыми свойствами,
ного аргумента:
expm(A) = 1 + A + описанными выше. Кососимметричную мат­ри­цу можно восA2/2! + A3/3! + ...

становить по мат­ри­це S с единственным элементом:
>>> linalg.expm(skew(S))
array([[ 0.9553, -0.2955],
[ 0.2955, 0.9553]])

76 Глава 2 · Представление местоположения и ориентации
Отступление 2.6. Двухмерная кососимметричная мат­ри­ца
В двух измерениях косо- и антисимметричная мат­ри­ца имеет
вид:
(2.14)
Она имеет четкую структуру с нулевой диагональю и единственным уникальным элементом ω ∈ ℝ, и [ω]×T = -[ω]. Векторное пространство двухмерных кососимметричных матриц обозначается
как so(2) и является алгеброй Ли SO(2). Пакет Toolbox реализует
оператор [·]× как:
>>> X = skew(2)
array([[ 0, -2],
[ 2, 0]])
а оператор обращения ∨×(·) как
>>> vex(X)
array([

2])

Формально мы можем написать выражение
R = e[θ]× ∈ SO(2),

где θ – угол поворота, а обозначение [·]× : ℝ ↦ so(2) ⊂ ℝ2×2 указывает на отображение скалярной мат­ри­цы в кососимметричную.

2.2.2

Положение в двух измерениях

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

Рис. 2.11. Поворот и сдвиг системы координат,
где оси системы {A¢} параллельны осям
системы {A}

2.2.2.1

Двухмерная однородная мат­ри­ца преобразования

Первый шаг – преобразование вектора координат Bp = (Bx, By)
в системе координат {B} в A¢p = (A¢x, A¢y) в системе координат {A¢}

Работа в двух измерениях (2D) 77

с использованием мат­ри­цы вращения A¢ RB(θ), которая является
функцией ориентации θ. Поскольку системы {A¢} и {A} параллельны, вектор координат Ap получается сложением AtB = (tx, ty)T
c A¢p.
(2.15)
(2.16)

(2.17)

Более компактно это можно записать как
(2.18)

где AtB – перенос начала системы координат {B} относительно
системы координат {A}, а ARB(θ) – изменение ориентации системы координат {B} относительно системы координат {A}.
Если рассматривать однородное преобразование как относительное положение или движение твердого тела, то в таком представлении система координат сначала перемещается с помощью
A
tB относительно системы {A}, а затем поворачивается с помощью
A
RB(θ).

Векторы координат для точки P теперь выражаются в одно­
родной форме, и мы обозначаем это тильдой (˜):

где ATB называется гомогенным преобразованием. Матрица имеет очень специфическую структуру и принадлежит специальной евклидовой группе (Special Euclidean, SE) размерности 2,
или T ∈ SE(2) ⊂ ℝ3×3.
Матрица ATB представляет перемещение и ориентацию, или
относительное положение, и имеет все характеристики относительного положения, описанные в формулах с (2.3) по (2.6).

78 Глава 2 · Представление местоположения и ориентации
ξ как мат­ри­ца SE(2)
В случае поворота и перемещения в двухмерном пространстве
движение ξ можно реализовать как однородную мат­ри­цу T ∈
SE(2), которая иногда записывается как упорядоченная пара (R, t)
∈ SO(2) × ℝ2 и обладает следующими свойствами:

композиция

ξ1 ⊕ ξ2

↦ T1T2 =

⊖ξ

↦ T–1 =


обращение


тождественный ∅
элемент
векторное
ξ•υ
преобразование

умножение матриц;

обращение мат­ри­цы;
↦ 13×3, единичная мат­ри­ца;

↦ 𝜖(Tυ), умножение мат­ри­цы
на вектор,

где · : ℝ2 ↦ ℙ2 и 𝜖(·) : ℙ2 ↦ ℝ2. Композиция некоммутативна, т. е.
T1T2 ≠ T2T1.

Отступление 2.7. Однородные векторы
Вектор p = (x, y) записывается в однородной форме как p̃ = (x1, x2,
x3) ∈ ℙ2, где ℙ2 – двухмерное проективное пространство, а тильда
(~) указывает на однородность вектора.
Однородные векторы обладают тем важным свойством, что p̃
эквивалентно λp̃ для всех λ ≠ 0, что записывается как p̃ ≃ λp̃. То
есть p̃ представляет одну и ту же точку на плоскости независимо
от общего коэффициента масштабирования. Однородное представление важно для компьютерного зрения, которое мы обсудим в части IV. Дополнительные подробности приведены в разделе С.2.
Для преобразования точки в однородную форму обычно добавляется элемент, равный единице. В двухмерном случае это p̃ =
(x, y, 1). Размерность вектора увеличивается на единицу, и точка
на плоскости теперь представлена трехмерным вектором. Евклидовы, или неоднородные, координаты связаны соотношением
x = x1 /x3, y = x2 /x3 и x3 ≠ 0.

Итак: мат­ри­ца, описывающая поворот на 0.3 рад, имеет вид:
>>> rot2(0.3)
array([[ 0.9553, -0.2955],
[ 0.2955, 0.9553]])

Ее можно комбинировать с другими мат­ри­ца­ми вращения
или использовать для преобразования векторов координат.
Матрица однородного преобразования для поворота на 0.3 рад
имеет вид:

Работа в двух измерениях (2D) 79
>>> trot2(0.3)
array([[ 0.9553, -0.2955,
[ 0.2955, 0.9553,
[ 0,
0,

0],
0],
1]])

В левом верхнем углу можно видеть мат­ри­цу вращения,
а в нижней строке – нули и единицу. Два верхних элемента правого столбца равны нулю, что означает нулевое смещение. Эта
мат­ри­ца представляет собой относительное положение, и ее
можно комбинировать с другими относительными положениями или использовать для преобразования однородных векторов
координат.
Далее мы сконструируем два относительных положения: перемещение на (1, 2) с последующим поворотом на 30°:
>>> TA = transl2(1, 2) @ trot2(30,
array([[ 0.866,
-0.5,
[
0.5,
0.866,
[
0,
0,

"deg")
1],
2],
1]])

Функция transl2 создает двухмерное относительное положение с конечным перемещением, но нулевым поворотом, в то
время как trot2 создает относительное положение с конечным
поворотом, но нулевым перемещением. Мы можем построить систему координат, представляющую это положение, относительно мировой системы координат:

Для конструирования относительных положений
в Toolbox имеются
функции: rot2
создает чистую >>> plotvol2([0, 5]); # новый график с двумя осями от 0 до 5
мат­ри­цу вращения >>> trplot2(TA, frame="A", color="b");
и trot2 создает
матрицу вращения
Параметры в вызове trplot задают метку {A} для системы кос нулевым перемещением. ординат и синий цвет, как показано на рис. 2.12. Для полноты

картины добавим в график мировую систему координат:

>>> T0 = transl2(0, 0);
>>> trplot2(T0, frame="0", color="k"); # мировая система координат

Теперь создадим другое относительное положение, представляющее собой перемещение на (2, 1) с нулевым поворотом:
>>> TB = transl2(2, 1)
array([[
1,
0,
[
0,
1,
[
0,
0,

2],
1],
1]])

и нарисуем получившуюся систему координат красным:
>>> trplot2(TB, frame="B", color="r");

Теперь составим композицию двух относительных поло­
жений:

80 Глава 2 · Представление местоположения и ориентации

Рис. 2.12. Системы координат,
нарисованные с помощью функции
trplot2 из пакета Toolbox

>>> TAB = TA @ TB
array([[ 0.866,
[
0.5,
[
0,

-0.5,
0.866,
0,

2.232],
3.866],
1]])

и нарисуем ее зеленым цветом:
>>> trplot2(TAB, frame="AB", color="g");

Мы видим, что перемещение на (2, 1) было применено относительно системы {A}. Важно отметить, что конечное перемещение не равно (3, 3), потому что относится к повернутой
системе координат. Некоммутативность композиции наглядно
демонстрирует следующий код, где изменен порядок сомножителей:
>>> TBA = TB @ TA;
>>> trplot2(TBA, frame="BA", color="c");

и мы видим, что система {BA} отличается от системы {AB}.
Теперь определим точку (3, 2) относительно мировой системы координат:
>>> P = np.array([3, 2]);

создав одномерный вектор-столбец, и добавим ее в график:
>>> plot_point(P, "ko", text="P");

Чтобы определить координату точки относительно {A}, воспользуемся уравнением
p = 0ξA • Ap,

0

Работа в двух измерениях (2D) 81

а затем выполним перестановку:
p = (⊖0ξA) • 0p.

A

Подставим числовые значения:
>>> np.linalg.inv(TA) @ np.hstack([P, 1])
array([ 1.732,
-1,
1])

где мы сначала преобразовали евклидовы координаты точки
в однородную форму, добавив единицу. Результат тоже имеет
однородную форму и отрицательную координату y в системе {A}. Используя Toolbox, мы также могли бы выразить это как:
>>> h2e(np.linalg.inv(TA) @ e2h(P))
array([[ 1.732],
[
-1]])

где результат выражен в евклидовых координатах. Вспомогательная функция e2h преобразует евклидовы координаты в однородные, а h2e выполняет обратное преобразование. То же самое можно записать еще более кратко:
>>> homtrans(np.linalg.inv(TA), P)
array([[ 1.732],
[
-1]])

Функция homtrans автоматически выполняет преобразование
векторов координат в однородную форму и обратно.

2.2.2.2

Поворот системы координат

Положение системы координат полностью описывается мат­ри­
цей SE(2), и здесь мы поговорим о том, как выполняется поворот систем координат. Сначала создадим и построим опорную
систему координат {0} и целевую систему координат {X}:
>>>
>>>
>>>
>>>
>>>

plotvol2([-5, 4, -1, 5]);
T0 = transl2(0, 0);
trplot2(T0, frame="0", color="k");
TX = transl2(2, 3);
trplot2(TX, frame="X", color="b");

Далее создадим мат­ри­цу SE(2), описывающую поворот на
2 радиана (почти 120°).
>>> TR = trot2(2);

и выполним композицию двумя возможными способами:
>>> trplot2(TR @ TX, frame="RX", color="g");
>>> trplot2(TX @ TR, frame="XR", color="g");

82 Глава 2 · Представление местоположения и ориентации

Результаты показаны в виде зеленых систем координат на
рис. 2.13. Как видите, система {RX} была повернута вокруг начала координат опорной системы, а система {XR} – вокруг начала координат {X}.

Рис. 2.13. Система {X} повернута на 2 рад относительно {0}, чтобы получить систему {RX},
относительно {X}, чтобы получить систему {XR}, и относительно точки C, чтобы получить
систему {XC}

А что, если нам понадобится повернуть систему координат
вокруг произвольной точки C? Прежде всего зададим координаты этой точки и отобразим ее:
>>> C = np.array([3, 2]);
>>> plot_point(C, "ko", text="C");

затем вычислим преобразование для поворота вокруг точки C:
>>> TC = transl2(C) @ TR @ transl2(-C)
array([[ -0.4161, -0.9093,
6.067],
[ 0.9093, -0.4161, 0.1044],
[
0,
0,
1]])

и применим его к системе {X}:
>>> trplot2(TC @ TX, frame="XC", color="r");

Теперь на графике можно увидеть, что красная система координат действительно была повернута вокруг точки C.
Чтобы лучше понять происходящее, разберем применение TC
к TX. Поскольку первое действие – это умножение TC на TX, первое преобразование, применяемое к {X}, – это самый правый
вызов transl2(-C), который выполняет сдвиг начала координат
и перемещает его в точку C. Затем применяется TR – поворот,
который поворачивает сдвинутую версию {X} вокруг нового начала координат в точке C, то есть {X} поворачивается вокруг C.

Работа в двух измерениях (2D) 83

И в заключение выполняется вызов transl2(C), который применяет обратный сдвиг начала координат из точки C в начало
базовой системы. Это преобразование является примером сопряжения, но его конструкция довольно сложна. Предварительное умножение матриц обсуждается в разделе 2.4.1. Более понятный способ достижения того же результата – использование
кручений, о которых рассказывается в разделе 2.2.2.4.

2.2.2.3

Матричная экспонента для определения положения

Мы можем взять логарифм TC – мат­ри­цы SE(2) из предыдущего
примера
>>> L = linalg.logm(TC)
array([[
0,
-2,
[
2,
0,
[
0,
0,

4],
-6],
0]])

и получить расширенную кососимметричную мат­ри­цу: в которой вверху слева находится кососимметричная мат­ри­ца 2×2,
вверху справа – двухмерный вектор-столбец и внизу – строка,
заполненная нулями. Три уникальных элемента мат­ри­цы можно распаковать в массив:
>>> S = vexa(L)
array([
4,

-6,

2])

где последний элемент – угол поворота в радианах.
Отступление 2.8. Двухмерная расширенная кососимметричная мат­ри­ца
В двух измерениях расширенная кососимметричная мат­ри­ца, соответствующая
вектору S = (υx, υy, ω), имеет вид:
(2.19)
которая имеет четкую структуру: нули на главной диагонали и в нижней строке и кососимметричная мат­ри­ца в левом верхнем углу. Векторное пространство
двухмерных расширенных кососимметричных матриц обозначается как se(2)
и является алгеброй Ли SE(2). В пакете Toolbox оператор [·] реализован как:
>>> X = skewa([1, 2, 3])
array([[
0,
-3,
[
3,
0,
[
0,
0,
А оператор обращения ∨(·) – как:
>>> vexa(X)
array([

1,

2,

1],
2],
0]])

3])

84 Глава 2 · Представление местоположения и ориентации

Возведение L в степень дает исходную мат­ри­цу SE(2), а еще L
можно реконструировать всего из трех элементов S:
>>> linalg.expm(skewa(S))
array([[ -0.4161, -0.9093,
[ 0.9093, -0.4161,
[
0,
0,

6.067],
0.1044],
1]])

В общем виде это можно записать так:
T = e[S] ∈ SE(2),

где S ∈ ℝ3 и [·] : ℝ3 ↦ se(2) ⊂ ℝ3×3. Логарифм и экспоненту можно
эффективно вычислять с помощью функций из пакета Toolbox:
trlog2 и trexp2 соответственно. Вектор S – это вектор кручения,
который мы обсудим далее.

2.2.2.4

Кручения в 2D

В разделе 2.2.2.2 мы преобразовывали систему координат, поворачивая ее вокруг заданной точки, и выяснили, что для любых двух систем координат можно найти центр и угол поворота, которые повернут первую систему во вторую. Это ключевая
идея, лежащая в основе понятия кручения (twist).
Мы можем создать кручение вокруг точки, заданной координатным вектором C:
>>> S = Twist2.UnitRevolute(C)
(2 -3; 1)

использовав метод класса UnitRevolute. Результатом является
объект Twist2, инкапсулирующий двухмерный вектор кручения
(υ, ω) ∈ ℝ3, включающий момент υ ∈ ℝ2 и скаляр ω. Это конкретное кручение представляет единичное кручение на 1 рад вокруг
точки C.
Для примера в разделе 2.2.2.2, где выполняется поворот на
2 рад вокруг точки C, нам необходимо масштабировать единичное кручение и возвести его в степень:
>>> linalg.expm(skewa(2 * S.S))
array([[ -0.4161, -0.9093,
6.067],
[ 0.9093, -0.4161, 0.1044],
[
0,
0,
1]])

где S.S – вектор кручения в виде массива NumPy. У объекта Twist2
есть сокращенный метод для выполнения этой операции:
>>> S.exp(2)
-0.4161 -0.9093
0.9093 -0.4161
0
0

6.067
0.1044
1

Работа в двух измерениях (2D) 85
Результат на самом
Это то же значение, полученное в предыдущем разделе, но
деле не является более кратко определено в терминах центра и угла поворота.
массивом NumPy
3×3, хотя и выглядит Центр поворота называется полюсом и закодирован в кручении:
как таковой. На самом деле это объект >>> S.pole
SE2, инкапсулирую- array([
3,
2])
щий массив NumPy.
Подробнее об этом
В случае чисто поступательного движения центр поворорассказывается
та
находится в бесконечности. Поэтому поступательное, или
в разделе 2.5.

призматическое, единичное кручение определяется только направлением движения. Например, движение вдоль оси y создается так:
>>> S = Twist2.UnitPrismatic([0, 1])
(0 1; 0)

представляет собой смещение на 1 вдоль оси y. Чтобы создать
преобразование SE(2) для конкретного перемещения, необходимо выполнить масштабирование и возведение в степень:
>>> S.exp(2)
1
0
0
1
0
0

0
2
1

Полученное преобразование описывает нулевой поворот
и перемещение на 2 вдоль оси y.
Для произвольного двухмерного однородного преобразования
>>> T = transl2(3, 4) @ trot2(0.5)
array([[ 0.8776, -0.4794,
3],
[ 0.4794, 0.8776,
4],
[
0,
0,
1]])

кручение
>>> S = Twist2(T)
(3.9372 3.1663; 0.5)

описывает поворот
>>> S.w
0.5

вокруг точки
>>> S.pole
array([ -3.166,

3.937])

а экспонента этого кручения
>>> S.exp(1)
0.8776 -0.4794

3

86 Глава 2 · Представление местоположения и ориентации
0.4794
0

0.8776
0

4
1

дает исходное преобразование SE(2) – полностью описываемое
тремя элементами вектора кручения.
ξ как кручение в 2D
В случае поворота и перемещения в двухмерном пространстве
движение ξ можно реализовать как кручение S ∈ ℝ3, обладающее
следующими свойствами:

композиция
ξ1 ⊕ ξ2

обращение
⊖ξ
тождественный ∅
элемент
векторное
ξ•υ
преобразование

↦ log(e[S1] e[S2]), произведение
экспонент;
↦ –S, отрицание;
↦ 01×3, нулевой вектор;

↦ 𝜖(e[S] υ̃), умножение мат­ри­цы
на однородный вектор,

где ˜· : ℝ2 ↦ ℙ2 и 𝜖(·) : ℙ2 ↦ ℝ2. Композиция некоммутативна, т. е.
e[S1] e[S2] ≠ e[S2] e[S1]. Обратите внимание: log и exp имеют эффективные аналитические решения, что делает композицию относительно недорогой с вычислительной точки зрения.

2.3

Работа в трех измерениях (3D)

Трехмерное операционное пространство является расширением двухмерного случая, рассмотренного в предыдущем разделе.
Мы добавляем дополнительную ось координат, обычно обозначаемую буквой z, которая ортогональна осям x и y. Направление
оси z подчиняется правилу правой руки и образует правосто­
роннюю систему координат. Базисные векторы – единичные
векторы, параллельные осям, – обозначаются как x̂, ŷ и ẑ
так, что
ẑ = x̂ × ŷ,

x̂ = ŷ × ẑ,

ŷ = ẑ × x̂.

(2.20)

Точка P представлена своими координатами по осям x, y и z
(x, y, z) или вектором из начала координат в точку:
p = xx̂ + yŷ + zẑ,
который является линейной комбинацией базисных векторов.
На рис. 2.14 показаны две трехмерные системы координат,
и нам нужно описать красную систему координат {B} относительно синей системы {A}. Мы ясно видим, что начало координат {B} было смещено вдоль трехмерного вектора AtB, а затем

Иногда эти базисные векторы
обозначаются как e1,
e2 и e3.
Во всех этих тождествах символы
слева направо
(через знак равенства) представляют
собой циклическое
вращение последовательности ... z, x, y,
z, x, y, ....

Работа в трех измерениях (3D) 87
Отступление 2.9. Правило правой руки
Правосторонняя система координат определяется тремя первыми пальцами правой руки,
которые указывают относительные направления осей x, y и z соответственно.

X
Y
Z

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

https://go.sn.pub/
ATBZR0

Рис. 2.14. Две системы трехмерных координат {A} и {B}, определенные относительно мировой
системы координат. Система {B} смещена относительно {A}. При просмотре трехмерных
систем координат на книжных страницах может возникать путаница. Ваш мозг может путать
случаи, когда начало координат направлено к вам и от вас – знание правила правой руки
поможет устранить эту путаницу

2.3.1

Ориентация в трех измерениях

На рис. 2.14 показаны две системы координат с очень разными
ориентациями, и нам нужно каким-то образом описать ориентацию одной по отношению к другой. Мы можем представить,
что берем систему {A} в руку и поворачиваем ее, пока она не
станет такой же, как система {B}.
Для начала рассмотрим поворот вокруг одной координатной
оси. На рис. 2.15 показана правосторонняя система координат

88 Глава 2 · Представление местоположения и ориентации

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

а Исходная

б π/2 вокруг
оси x

в π
 вокруг
оси x

г -π/2 вокруг
д π/2 вокруг
е π/2 вокруг
оси x
оси y
оси z
Рис. 2.15. Поворот трехмерной системы координат: а) исходная система координат;
b–f) системы после различных поворотов, как указано на рисунке

Поворот в трехмерном пространстве имеет свои особенности, которые показаны на рис. 2.16. Здесь изображена последовательность двух поворотов, применяемых в разном порядке.
Как видите, окончательная ориентация зависит от порядка
применения поворотов. Это многозначительное и таинственное свойство трехмерного мира долгое время сбивало с толку
математиков. Соответственно, есть трехмерное продолжение
алгебры положений, которую мы использовали в этой главе:
 В трехмерном пространстве поворот не является коммутативной
операцией – порядок применения поворотов влияет на результат.

Математики разработали множество способов представления поворотов, и мы обсудим те из них, которые чаще всего
встречаются в робототехнике: ортонормированные мат­ри­цы
вращения, углы Эйлера и Кардана, ось и угол вращения, эксОтступление 2.10. Поворот вокруг вектора
Обхватите правой рукой вектор так, чтобы большой палец указывал в направлении стрелки. Изогнутые пальцы указывают направление увеличения угла.

Работа в трех измерениях (3D) 89

а

go.sn.pub/XU7D6b
б

исходное
после первого
после второго
положение
поворота
поворота
Рис. 2.16. Пример, демонстрирующий некоммутативность вращения. В верхнем ряду система
координат поворачивается на π/2 вокруг оси x, а затем на π/2 вокруг оси y. В нижнем ряду
повороты применяются в обратном порядке и результаты явно разные

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

2.3.1.1

Трехмерная мат­ри­ца вращения

Так же как и в случае двухмерных систем, мы можем представить ориентацию трехмерной системы координат с помощью
ее базисных векторов, выраженных через мировую систему координат. Каждый единичный вектор состоит из трех элементов,
и они образуют столбцы ортонормированной мат­ри­цы ARB 3×3
(2.21)
которая преобразует описание вектора, определенного относительно системы {B}, в вектор относительно {A}. Трехмерная
мат­ри­ца вращения R обладает теми же специальными свойствами, что и ее двухмерный аналог:
столбцы являются базисными векторами, определяющими
оси повернутой трехмерной системы координат, и поэтому имеют единичную длину и являются ортогональными;
„„ она ортогональная (или ортонормированная)
, и поэтому
операция обращения совпадает с операцией транспонирования, R-1 = RT;
„„ произведение мат­ри­цы на вектор Rυ сохраняет длину
и относительную ориентацию векторов υ, и поэтому ее
определитель равен +1;
„„

См. приложение B,
в котором можно
освежить свои знания
о векторах, мат­ри­цах
и линейной алгебре.

90 Глава 2 · Представление местоположения и ориентации
„„

она принадлежит специальной ортогональной группе размерности 3, R ∈ SO(3) ⊂ ℝ3×3. Это означает, что произведение любых двух матриц внутри группы также принадлежит группе, как и обращенная мат­ри­ца.

Матрицы вращения, соответствующие повороту системы координат на угол θ вокруг осей x, y и z, имеют вид:

Toolbox предоставляет функции для вычисления этих элементарных матриц вращения, например Rx(π/2):
>>> R = rotx(pi / 2)
array([[
1,
[
0,
[
0,

0,
0,
1,

0],
-1],
0]])

Функции roty и rotz вычисляют Ry(θ) и Rz(θ) соответственно.
Если учесть, что мат­ри­ца вращения представляет положение,
то соответствующую систему координат можно отобразить графически:
>>> trplot(R);

как показано на рис. 2.17a. Движение или относительное положение можно отобразить более наглядно в виде анимации:
>>> tranimate(R)

показывающей, как мировая система вращается в выбранной
системе координат. Если у вас есть пара анаглифных стереоочков ►, вы можете увидеть анимацию в более реалистичном
3D-представлении с помощью функции, воспользовавшись одним из следующих способов:
>>> trplot(R, anaglyph=True)
>>> tranimate(R, anaglyph=True);

Работа в трех измерениях (3D) 91

Для иллюстрации комбинирования поворотов снова повернем систему на рис. 2.17а, на этот раз вокруг оси y:
>>> R = rotx(pi / 2) @ roty(pi / 2)
array([[
0,
0,
1],
[
1,
0,
0],
[
0,
1,
0]])
>>> trplot(R);

что даст результат, изображенный на рис. 2.17b. Теперь ось x указывает в направлении мировой оси y. Получившаяся система координат совпадает с крайним правым изображением на рис. 2.16a.

а

б

Рис. 2.17. Системы координат, отображенные с помощью trplot: а) мировая система,
повернутая на π/2 вокруг оси x; b) система a, повернутая на π/2 вокруг оси y

Некоммутативность поворота можно показать, изменив порядок операций в предыдущем коде:
>>> roty(pi / 2) @ rotx(pi / 2)
array([[
0,
1,
[
0,
0,
[
-1,
0,

0],
-1],
0]])

что даст совсем другой результат.
Отступление 2.11. Чтение мат­ри­цы вращения
Столбцы слева направо сообщают нам направления осей новой системы относительно текущей системы координат.
В этом примере ось x новой системы имеет прежнее направление (1, 0, 0), новая ось y – направление прежней оси z (0, 0, 1) и новая ось z – отрицательное направление прежней оси y (0, −1, 0).
То есть ось x не изменилась, поскольку вокруг нее
новая ось z
новая ось x
осуществлялся поворот. Строки, наоборот, отрановая ось y
жают текущие оси системы с точки зрения осей
новой системы.

92 Глава 2 · Представление местоположения и ориентации
ξ как мат­ри­ца SO(3)
В случае простого поворота в трехмерном пространстве движение ξ можно реализовать как мат­ри­цу вращения R ∈ SO(3), обладающую следующими свойствами:

композиция
обращение
тождественный
элемент
векторное
преобразование

ξ1 ⊕ ξ2 ↦ R1R2, умножение матриц;
⊖ξ
↦ R–1 = RT, транспонирование мат­ри­цы;
∅
↦ R(0) = 13×3, единичная мат­ри­ца;
ξ•υ

↦ Rυ, умножение мат­ри­цы на вектор.

Композиция некоммутативна, т. е. R1R2 ≠ R2R1.

2.3.1.2

Представления на основе трех углов

юбые две независимые ортонормированные системы коор­
« Лдинат
можно связать последовательностью поворотов (не
более трех) вокруг координатных осей, причем никакие два по­
следовательных поворота не будут совершаться вокруг одной
и той же оси.
Теорема вращения Эйлера (Kuipers, 1999)

Теорема вращения Эйлера утверждает, что преобразование из
одной системы координат в другую в трехмерном пространстве
можно представить как последовательность поворотов, каждый
вокруг определенной оси координат. Повороты выполняются
последовательно: первый поворачивает мировую систему координат {0} вокруг некоторой оси и создает новую систему координат {1}; второй поворачивает систему {1} вокруг некоторой
Отступление 2.12. Леонард Эйлер
Эйлер (1707–1783) – швейцарский математик
и физик, фактически возглавивший математическую науку XVIII в. Он был учеником Иоганна
Бернулли и применял новые математические методы, такие как исчисление, ко многим задачам
механики и оптики. Он также предложил способ
записи функций y = f(x). В робототехнике мы используем его теорему о вращении и его уравнения движения во вращательной динамике.
Он был весьма плодовитым ученым, его собрание сочинений составляет 75 томов. Почти половина из них была произведена в течение последних 17 лет его жизни, когда он полностью ослеп.

Работа в трех измерениях (3D) 93

другой оси и создает систему координат {2}; и, наконец, третий
поворот поворачивает систему {2} вокруг некоторой третьей
оси и создает систему координат {3}.
Всего существует двенадцать уникальных последовательностей поворотов. Шесть из них предполагают повторение поворотов вокруг одной конкретной оси: XYX, XZX, YXY, YZY, ZXZ
и ZYZ. Еще шесть определяют повороты вокруг всех трех осей:
XYZ, XZY, YZX, YXZ, ZXY и ZYX.
 Неоднозначность термина «углы Эйлера»
 По умолчанию принято считать, что все трехугловые представления поворотов относятся к углам Эйлера, но это не совсем так, поскольку существует 12 различных последовательностей. Конкретная последовательность углов часто определяется соглашением
в определенной технологической области.

В механической динамике обычно используется последовательность ZYZ.
R(ϕ, θ, ψ) = Rz(ϕ)Ry(θ)Rz(ψ).

(2.22)

(S1)3, или S1 × S1 × S1,
Углы Эйлера записываются как трехэлементный вектор Г =
обозначает кортеж (ϕ, θ, ψ) ∈ (S1)3.
Например, вот как можно вычислить эквивас тремя элементами, а S3 – угол лентную мат­ри­цу вращения для Г = (0.1, 0.2, 0.3):
в четырехмерном
пространстве. >>> R = rotz(0.1) @ roty(0.2) @ rotz(0.3);

или более удобный вариант:
>>> R = eul2r(0.1,
array([[ 0.9021,
[ 0.3875,
[ -0.1898,

0.2, 0.3)
-0.3836, 0.1977],
0.9216, 0.01983],
0.05871, 0.9801]])

Обратная задача – поиск углов Эйлера, соответствующих заданной мат­ри­це вращения:
>>> gamma = tr2eul(R)
array([
0.1,
0.2,

0.3])

Но при отрицательном значении θ
>>> R = eul2r(0.1, -0.2, 0.3)
array([[ 0.9021, -0.3836, -0.1977],
[ 0.3875, 0.9216, -0.01983],
[ 0.1898, -0.05871, 0.9801]])

обратная функция
>>> gamma = tr2eul(R)
array([ -3.042,
0.2,

-2.842])

94 Глава 2 · Представление местоположения и ориентации

возвращает положительное значение для θ и значения ϕ и ψ,
смещенные на величину -π. Однако соответствующая мат­ри­ца
вращения
>>> eul2r(gamma)
array([[ 0.9021, -0.3836, -0.1977],
[ 0.3875, 0.9216, -0.01983],
[ 0.1898, -0.05871, 0.9801]])

осталась прежней, то есть два разных набора углов Эйлера соответствуют одной мат­ри­це вращения. Преобразование мат­ри­
цы вращения в углы Эйлера не является уникальным, и Toolbox
всегда возвращает положительный угол для θ.
Для случая θ = 0
>>> R = eul2r(0.1, 0, 0.3)
array([[ 0.9211, -0.3894,
[ 0.3894, 0.9211,
[
0,
0,

0],
0],
1]])

обратная функция возвращает
>>> tr2eul(R)
array([
0,

0,

0.4])

что явно отличается от исходных значений, но в результате
получается та же мат­ри­ца вращения. Объясняется это тем, что
если θ = 0, то Ry = I3×3 и уравнение (2.22) принимает форму
R = Rz(ϕ)Rz(ψ) = Rz(ϕ + ψ),
которая является функцией суммы ϕ + ψ. Поэтому обратная
операция может определить только эту сумму, и по соглашению
мы принимаем ϕ = 0. Случай θ = 0 является особым, поэтому мы
обсудим его более подробно в следующем разделе.
Еще одним повсеместно используемым соглашением являются углы Кардано: крен, тангаж и рыскание, которые обозначаются как α, β и γ соответственно .

Углы крена, тангажа
и рыскания известны также как
углы Тейта–Брайа­
на в честь Питера
Тейта, шотландского
физика и сторон Неоднозначность названий углов крен, тангаж и рыскание
ника кватернионов, и Джорджа
 Как ни странно, широко используются две разные версии. В учеб- Брайана – одного из
никах последовательность крена-тангажа-рыскания определяется первых валлийских
как ZYX или XYZ в зависимости от того, к чему привязана система аэродинамиков. Их
координат: к корпусу мобильного робота или к его манипулятору. также называют
навигационными
В авиации
При описании положения транспортных средств, таких как углами.
эти углы называют
корабли, самолеты и автомобили, принято считать, что ось x углом крена (bank),
направлена вперед, а ось z указывает либо вверх, либо вниз. углом положения
(attitude) и курсоНачнем с мировой системы координат и рассмотрим повороты вым углом (heading)
соответственно.
в следующем порядке:

Работа в трех измерениях (3D) 95

поворот вокруг мировой оси z на угол рыскания (γ) ориентирует ось x в направлении движения;
„„ поворот предыдущей системы координат вокруг оси y на
угол тангажа (β) задает угол между продольной осью транспортного средства и горизонтальной плоскостью (задирает или опускает нос);
„„ поворот предыдущей системы координат вокруг оси x на
угол крена (α) поворачивает корпус транспортного средства вокруг его продольной оси.
Эта последовательность поворотов XYZ записывается как:
„„

R(α, β, γ) = Rz(γ)Ry(β)Rx(α),

(2.23)

а углы крена, тангажа и рыскания записываются как трехмерный вектор Г = (α, β, γ) ∈ (S1)3. Например:
>>> R = rpy2r(0.1,
array([[ 0.9363,
[ 0.2896,
[ -0.1987,

0.2, 0.3, order="zyx")
-0.2751, 0.2184],
0.9564, -0.03696],
0.09784,
0.9752]])

где аргументы приведены в порядке «крен, тангаж, рыскание»,
противоположном порядку в (2.23).

Отступление 2.13. Джероламо Кардано
Кардано (1501–1576) – итальянский математик эпохи Возрождения, врач, астролог и игрок. Он родился
в Павии (Италия) и был внебрачным ребенком математически одаренного юриста. Он изучал медицину в университете Падуи и позже первым описал
брюшной тиф. Кардано частично зарабатывал на
жизнь азартными играми, а его книга об азартных
играх Liber de ludo aleae содержит первое систематическое исследование вероятности, а также эффективных методов мошенничества. Его семейная
жизнь была полна проблем: старшего сына казнили за отравление жены, а дочь была проституткой,
умершей от сифилиса (о чем он написал трактат). Он
вычислил и опубликовал гороскоп Иисуса, был обвинен в ереси и провел некоторое время в тюрьме,
пока не отрекся от своего профессорства.
Он опубликовал решения уравнений кубической и четвертой степени в своей
книге Ars magna в 1545 г., а также изобрел кодовый замок, карданный подвес, состоящий из трех концентрических колец, позволяющих свободно вращаться компасу или гироскопу (рис. 2.19), и вал с карданным шарниром – всем известный
карданный вал, используемый сегодня в автомобилях.

96 Глава 2 · Представление местоположения и ориентации

Обратное преобразование:
>>> gamma = tr2rpy(R, order="zyx")
array([
0.1,
0.2,
0.3])

При описании ориентации рабочего инструмента робота, показанного на рис. 2.20, принято считать, что ось z в его системе
координат направлена вперед, а ось y параллельна линии, соединяющей концы пальцев. В результате получается последовательность углов XYZ.
R(α, β, γ) = Rx(γ)Ry(β)Rz(α),

(2.24)

например:
>>> R = rpy2r(0.1, 0.2, 0.3,
array([[ 0.9752, -0.09784,
[ 0.1538,
0.9447,
[ -0.1593,
0.313,

order="xyz")
0.1987],
-0.2896],
0.9363]])

И обратное преобразование:
>>> gamma = tr2rpy(R, order="xyz")
array([
0.1,
0.2,
0.3])

Последовательность «крен-тангаж-рыскание» позволяет всем
углам иметь произвольный знак и имеет сингулярность, когда
β = ±π/2.
Toolbox содержит интерактивный графический инструмент
>>> %run -m tripleangledemo

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

2.3.1.3

Сингулярности и блокировка подвеса

Фундаментальной проблемой всех только что описанных трехугловых представлений является сингулярность. Это явление
также известно как блокировка подвеса (gimbal lock), термин,
получивший известность после фильма «Аполлон-13». Этот
термин относится к механическим карданным системам.
Одним из примеров может служить механический гироскоп
(рис. 2.19), используемый для навигации космического аппарата. Внутри находится собственно активный узел – это ста­
билизированный элемент (stable member, или стабилизирован-

Его также можно
запустить из
командной строки
операционной
системы как
tripleangledemo.

Работа в трех измерениях (3D) 97

Рис. 2.18. Приложение tripleangledemo из пакета Robotics Toolbox позволяет
экспериментировать с углами Эйлера и углами крена-тангажа-рыскания и видеть, как
изменяется положение тела в трехмерном пространстве. Это изображение получено
в браузере с помощью визуализатора Swift

«Система координат
LM (Lunar Module)
Body – правосторонняя, с осью +X,
направленной вверх
через ось тяги, осью
+Y, направленной
вправо, если смот­
реть вперед вдоль
оси +Z. Матрица
преобразования
поворота строится
с помощью последовательности
Эйлера 2-3-1 (YZX),
т. е. тангаж вокруг
Y, затем крен вокруг Z и, наконец,
рыскание вокруг X.
Положительные повороты – это тангаж
вверх, крен вправо,
рыскание влево».
(Hoag, 1963.)

ная платформа), который имеет три ортогональных гироскопа,
удерживающих его в постоянной ориентации относительно
вселенной. Он механически связан с космическим кораблем
через шарнирный механизм, который позволяет космическому
кораблю двигаться вокруг стабильной платформы, не прикладывая к ней никакого крутящего момента. Положение космического корабля определяется непосредственно путем измерения углов осей подвеса по отношению к стабилизированной
платформе, прямо соответствующих углам крена-тангажа-рыс­
кания, которые в этой конструкции представляют карданову
последовательность YZX. Ориентация системы координат {B},
связанной с корпусом космического аппарата, относительно
системы координат {S}, связанной с неподвижной платформой,
определяется как
RB = Ry(β)Rz(α)Rx(γ).

S

(2.25)

Рассмотрим ситуацию, когда угол поворота среднего подвеса (α, поворот вокруг оси z космического корабля) составляет
90°, – оси внутреннего (β) и внешнего (γ) подвесов совмещены
и имеют одну и ту же ось вращения. Вместо первоначальных
трех осей вращения, поскольку две из них параллельны, теперь

98 Глава 2 · Представление местоположения и ориентации
Моментный двигатель /
Дуплексное контактное кольцо
на шарикоподшипниках
(50 контактов)
Резольвер ошибки гироскопа
(1x) / Дуплексное контактное
кольцо на шарикоподшипниках
(40 контактов) /
Многоскоростной резольвер
(1x и 16x)

LM
ось +x
Ось OG

Моментный двигатель /
Дуплексное контактное кольцо
на шарикоподшипниках
(40 контактов)

LM
ось +z

Внешний
карданный
подвес

Ось MG

Средний
карданный
подвес

Стабильный
компонент

Ось IG
LM
ось +y

Корпус IMU
(в разрезе)

Моментный двигатель /
Дуплексное контактное кольцо
на шарикоподшипниках
(40 контактов)

Это было сущест­
венным эксплуатационным
Дуплексное контактное кольцо
Дуплексное контактное кольцо
недостатком данного
на шарикоподшипниках
на шарикоподшипниках
(40 контактов)
(50 контактов)
конкретного гироМногоскоростной резольвер
Многоскоростной резольвер
скопа (Hoag, 1963),
(1x и 16x)
(1x и 16x)
и его можно было
Рис. 2.19. Схема инерциального измерительного блока (inertial measurement unit, IMU)
частично скомпенлунного модуля Apollo (Lunar module, LM). В системе координат корабля ось x направлена
сировать, добавив
вверх через ось тяги, ось z – вперед, а ось y – вправо. Начиная со стабилизированной
четвертый подвес,
платформы {S} и продвигаясь наружу к корпусу космического корабля {B},
как это делалось на
последовательность углов поворота равна YZX. Компоненты, подписанные как Xg, Yg и Zg, – это других космических
гироскопы по осям x, y и z, а компоненты, подписанные как Xa, Ya и Za, – это акселерометры по аппаратах. Четверосям x, y и z (иллюстрация взята из руководства «Apollo Operations Handbook, LMA790-3-LM») тый подвес был
убран из лунного
модуля для эконоесть только две эффективные оси – мы говорим, что одна сте- мии веса и места.

пень свободы была потеряна.

Подстановка

Ry(θ)Rz(π/2) ≡ Rz(π/2)Rx(θ)
вместо первых двух членов в (2.25) приводит к уравнению
RB = Rz(π/2)Rx(β)Rx(γ) = Rz(π/2)Rx(β + γ),

S

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

Повороты
подчиняются правилам циклического
вращения:
Rx( )Ry(θ)RTx( ) ≡ Rz(θ)
Ry( )Rz(θ)RTy( ) ≡ Rx(θ)

Rz( )Rx(θ)RTz( ) ≡ Ry(θ)
и правилам ациклического вращения:

RTy( )Rx(θ)Ry( ) ≡ Rz(θ)

RTz( )Ry(θ)Rz( ) ≡ Rx(θ)

RTx( )Rz(θ)Rx( ) ≡ Ry(θ)

Работа в трех измерениях (3D) 99
Отступление 2.14. Аполлон 13
Протокол миссии Аполлон-13, время: 02 08 12 47
Руководитель полета: «На связи, что у вас?»
Офицер системы навигации: «Он приближается к блокировке подвеса».
Руководитель полета: «Вас понял. Оператор,
порекомендуйте ему увеличить тягу двигателей
C3, C4, B3, B4, C1 и C2, чтобы максимально приблизиться к блокировке подвеса».
Оператор ЦУП: «Вас понял».
«Аполлон-13», запись канала связи с центром
управления полетом (Lovell and Kluger 1994,
p 131; NASA 1970).

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

Потеря степени свободы означает невозможность математически обратить преобразование, можно только установить
линейную зависимость между двумя углами. В этом случае лучшее, что можно сделать, – это определить сумму углов тангажа
и рыскания. Аналогичные явления с сингулярностью угла Эйлера ZYZ мы наблюдали в предыдущем разделе.
Все трехугловые представления положения, будь то эйлеровы или кардановы, страдают этой проблемой блокировки
карданного подвеса, когда две оси совпадают. Для эйлеровых
углов ZYZ это происходит, когда θ = kπ, k ∈ ℤ, а для углов крена-тангажа-рыскания – когда тангаж β = ±(2k + 1)π/2. Лучшее,
на что можно надеяться, – это то, что сингулярность возникает
при положении, которое не встречается при нормальной работе
транспортного средства, – это требует разумного выбора последовательности углов и системы координат.
Сингулярности – неудачное следствие использования минимального представления, в данном случае это результат использования всего трех параметров для представления ориентации. Чтобы устранить проблему, необходимо использовать
другие представления ориентации. Члены команды лунного
модуля «Аполлон» предпочли бы систему с четырьмя карданами, и ключ к успеху, как мы вскоре увидим в разделе 2.3.1.7, состоит в том, чтобы ввести четвертый параметр.

2.3.1.4

Двухвекторное представление

Для роботов манипуляторного типа полезно использовать систему координат {E}, прикрепленную к рабочему органу, как
показано на рис. 2.20. По общепринятому соглашению ось инструмента связана с осью z и называется вектором подхода (approach vector), который обозначается как â = (ax, ay, az). Для не-

100 Глава 2 · Представление местоположения и ориентации

Рис. 2.20. Система координат
рабочего органа робота определяет
положение через вектор подхода â
и вектор ориентации ô, из которых
можно вычислить n̂. Векторы n̂, ô и â
соответствуют осям x, y и z согласно
системе координат рабочего органа
(изображение любезно предоставлено
Kinova Robotics)

которых приложений удобнее указать вектор подхода, чем углы
Эйлера ZYZ или углы крена-тангажа-рыскания.
Однако знания направления оси z недостаточно для описания
системы координат – необходимо также указать направление
осей x и y. Ортогональный вектор, обеспечивающий ориентацию, например между двумя «пальцами» захвата робота, называется вектором ориентации (orientation vector) ô = (ox, oy, oz).
Этих двух единичных векторов достаточно, чтобы полностью
определить мат­ри­цу вращения
(2.26)

поскольку оставшийся столбец, вектор нормали (normal vector)
n̂ = (nx, ny, nz), можно вычислить с помощью уравнения 2.20 как
n̂ = ô × â. Рассмотрим пример, когда рабочий орган робота направлен вниз, к рабочему столу
>>> a = [0, 0, -1];

и кончики его пальцев лежат на линии, параллельной вектору
(1, 1, 0):
>>> o = [1, 1, 0];

С помощью Toolbox мат­ри­ца вращения вычисляется так: ►
>>> R = oa2r(o, a)
array([[ -0.7071, 0.7071,
[ 0.7071, 0.7071,
[
0,
0,

0],
0],
-1]])

Для определения системы координат достаточно любых двух
непараллельных векторов. Даже если два вектора â и ô неортогональны, они все равно определяют плоскость, и вычисленный вектор n̂ будет перпендикулярен этой плоскости. В этом
примере нам нужно вычислить новое значение для ô¢ = â × n̂,
которое лежит в плоскости, но ортогонально каждому из â и n̂.

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

Работа в трех измерениях (3D) 101

Существует множество других ситуаций, в которых ориентация определяется двумя векторами. Для камеры мы могли
бы использовать в качестве â оптическую ось (по соглашению
ось z), а в качестве n̂ – правую сторону камеры (которая по соглашению является осью x). Для мобильного робота мы могли
бы использовать в качестве â вектор гравитационного ускорения, измеренный с помощью акселерометров (который по соглашению является осью z), а в качестве n̂ – направление курса,
измеренное с помощью электронного компаса (которое по соглашению является осью x).

2.3.1.5

Поворот вокруг произвольного вектора

Две системы координат произвольной ориентации связаны
единственным поворотом вокруг некоторой оси в пространстве.
Для примера поворота, использованного ранее:
>>> R = rpy2r(0.1, 0.2, 0.3);

мы можем определить такой угол и ось как
>>> theta, v = tr2angvec(R)

где
>>> theta
0.3655

– угол поворота, а
>>> v
array([ 0.1886, 0.5834,

0.79])

– единичный вектор, параллельный оси поворота. Обратите
внимание, что это не единственное решение, поскольку поворот на -θ вокруг вектора -υ приводит к той же ориентации,
что и поворот на θ вокруг вектора υ, – это называется двойным
отоб­ражением.
Эта информация закодирована в собственных значениях
и собственных векторах R
Матрицы e и x >>> e, x = np.linalg.eig(R)
являются комплексными. В NumPy где e – собственные значения
мнимая часть
обозначается как >>> e
j = –1. Некоторые array([0.934+0.357j, 0.934-0.357j, 1.
+0.j ])
элементы являются
действительными, то
есть имеют нулевую а соответствующие им собственные векторы являются столбцамнимую часть. ми мат­ри­цы ◄.

102 Глава 2 · Представление местоположения и ориентации
Отступление 2.15. Олинде Родригес
Родригес (1795–1850) – французский банкир и математик, много писавший о политике, социальных
реформах и банковском деле. Получил докторскую
степень по математике в 1816 г. в Парижском университете за работу над своей первой широко известной
формулой, связанной с полиномами Лежандра. Названная его именем формула поворота была опубликована в 1840 г. и, возможно, впервые представила
поворот как скаляр и вектор. Его формулу до него изобрел Гаусс, но не опубликовал ее. После смерти Родригес был похоронен на кладбище Пер-Лашез в Париже.

>>> x
array([[-0.694+0.j , -0.694-0.j , 0.189+0.j ],
[ 0.079+0.569j, 0.079-0.569j, 0.583+0.j ],
[ 0.107-0.42j , 0.107+0.42j , 0.79 +0.j ]])

Из определения собственных значений и собственных векторов мы помним, что
Rυ = λυ,
где υ – собственный вектор, соответствующий собственному
значению λ. Для случая λ = 1
Rυ = υ,
откуда следует, что соответствующий собственный вектор υ не
меняется при повороте. Такой вектор только один, и вокруг него
происходит поворот. В примере третье собственное значение
равно единице, поэтому осью вращения является третий столбец x.
Матрица вращения всегда будет иметь одно действительное
собственное значение при λ = 1 и, как правило, комплексную
пару λ = cos θ ± j sin θ, где θ – угол поворота. Угол поворота в этом
случае равен
>>> theta = np.angle(e[0])
0.3655

Обратная задача преобразования угла и вектора в мат­ри­цу
вращения решается с помощью формулы Родригеса:
R = I3×3 + sin θ[υ̂]× + (1 - cos θ)[υ̂]2×,

(2.27)

Угол поворота
также связан со
следом мат­ри­цы R,
как обсуждается
в разделе B.2.1, но
ограничен решения­
ми в квадрантах 1
и 2. Функция angle
находит решение
в любом квадранте.

Работа в трех измерениях (3D) 103

где [υ̂]× – кососимметричная мат­ри­ца. Мы можем использовать
эту формулу, чтобы определить поворот на угол 0.3 рад вокруг
оси x
>>> R = angvec2r(0.3, [1, 0, 0])
array([[
1,
0,
0],
[
0, 0.9553, -0.2955],
[
0, 0.2955, 0.9553]])

что эквивалентно вызову rotx(0.3).
Формула Родригеса – это вариация, которая напрямую преобразует координатный вектор x
Фактически это
единичный кватернион, который будет
представлен в разделе 2.3.1.7.

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

x¢ = R(θ, υ̂)x = x + 2s(ω × x) + 2(ω × (ω × x))
где s = cos θ/2 и ω = υ̂ sin θ/2. В совокупности (s, ω) ∈ ℝ4 известны
как параметры Эйлера.
Представление угол–ось параметризуется четырьмя числами: одно определяет угол поворота и три задают единичный
вектор, определяющий ось. Эти четыре числа можно упаковать
в единственный вектор υ ∈ ℝ3 – направление оси поворота задается единичным вектором υ̂, а угол поворота – некоторой функцией от величины вектора ||υ||.
Это приводит к некоторым распространенным трехпарамет­
рическим представлениям, таким как:
„„
„„
„„

„„

υ̂θ – вектор Эйлера, или экспоненциальные координаты;
υ̂ tan(θ/2) – вектор Родригеса;
υ̂ sin(θ/2) – векторный компонент единичного кватерниона;
υ̂ tan θ.

Эти формы минимальны и эффективны с точки зрения хранения данных, но направление υ̂ не определено, когда θ = 0.
Этот случай соответствует повороту на нулевой угол и должен
выявляться и обрабатываться явно.

logm отличается от
функции np.log, 2.3.1.6
Матричные экспоненты
которая вычисляет
логарифм каждого Рассмотрим поворот относительно оси x, выраженный в виде
элемента мат­ри­цы.
мат­ри­цы вращения
Логарифм можно
вычислить, используя степенной >>> R = rotx(0.3)
1,
0,
0],
ряд с мат­ри­цей array([[
вместо скалярного
[
0, 0.9553, -0.2955],
аргумента. Для
[
0, 0.2955, 0.9553]])
мат­ри­цы логарифм
не уникален, и logm
Как и для двухмерного случая, мы можем вычислить логавычисляет главный
логарифм мат­ри­цы. рифм этой мат­ри­цы, используя функцию logm :

104 Глава 2 · Представление местоположения и ориентации
Отступление 2.16. Кососимметричная мат­ри­ца
В трехмерном пространстве кососимметричная мат­ри­ца представлена в форме
(2.28)
которая имеет четкую структуру, нулевую диагональ и всего три уникальных элемента ω ∈ ℝ3 и [ω]×T = -[ω]×. Векторное пространство трехмерных кососимметричных матриц обозначается как so(3) и является алгеброй Ли SO(3). Пакет Toolbox
реализует оператор [·]× как
>>> X = skew([1, 2, 3])
array([[ 0, -3, 2],
[ 3, 0, -1],
[-2, 1, 0]])
а оператор обращения ∨×(·) как
>>> vex(X)
array([ 1, 2, 3])

Обе функции работают для трехмерного случая, показанного здесь, и двухмерного
случая, когда вектор является одноэлементным.
>>> L = linalg.logm(R)
array([[
0,
0,
[
0,
0,
[
0,
0.3,

0],
-0.3],
0]])

и результатом является разреженная мат­ри­ца с двумя элементами, имеющими величину 0.3 – исходный угол поворота. Эта
мат­ри­ца имеет нулевую диагональ и является еще одним примером кососимметричной мат­ри­цы, в данном случае 3×3.
Распаковка кососимметричной мат­ри­цы дает
>>> S = vex(L)
array([
0.3,

0,

0])

и мы видим исходный угол поворота в первом элементе, соответствующем оси x, вокруг которой выполняется поворот. Это
вычислительная альтернатива подходу на основе собственных
векторов, представленному в разделе 2.3.1.5. Эффективный
с вычислительной точки зрения способ вычисления логарифма
мат­ри­цы SO(3) – использовать функцию trlog:
>>> L = trlog(R);

Возведение в степень логарифма мат­ри­цы вращения L с помощью функции вычисления экспоненты мат­ри­цы expm дает
исходную мат­ри­цу вращения:
>>> linalg.expm(L)
array([[
1,

0,

0],

Функция expm отличается от функции
exp, которая вычисляет экспоненту
каждого элемента
мат­ри­цы. Матричную экспоненту
можно вычислить
как степенной ряд,
только с мат­ри­цей
вместо скалярного
аргумента:
expm(A) = 1 + A +
A2/2! + A3/3! + ...

Работа в трех измерениях (3D) 105
[
[

0,
0,

0.9553, -0.2955],
0.2955, 0.9553]])

Эффективный с вычислительной точки зрения способ вычисления экспоненты мат­ри­цы SO(3):
>>> trexp(L);

Экспонента кососимметричной мат­ри­цы всегда является
мат­ри­цей вращения, обладающей всеми особыми свойствами,
описанными выше. Кососимметричную мат­ри­цу можно восстановить по вектору S с тремя элементами:
>>> linalg.expm(skew(S))
array([[
1,
0,
0],
[
0, 0.9553, -0.2955],
[
0, 0.2955, 0.9553]])

Фактически функция
>>> R = rotx(0.3);

эквивалентна
>>> R = linalg.expm(0.3 * skew([1, 0, 0]));

где поворот задается через угол и ось поворота (в виде единичного вектора).
Общий случай можно записать так:
R = eθ[ω̂]× ∈ SO(3),

В пакете Toolbox
реализуется функцией trexp, которая
эквивалентна функции expm, но более
эффективна.

где θ – угол поворота, ω̂ – единичный вектор, параллельный оси
вращения, а запись [·]× : ℝ3 ↦ so(3) ⊂ ℝ3×3 указывает на отображение вектора в кососимметричную мат­ри­цу. Поскольку θ[ω̂]× =
[θω̂]×, мы можем описать поворот вектором θω̂ ∈ ℝ3, который
называют экспоненциальными координатами. Для трехмерных систем формула вращения Родригеса (2.27) – эффективное
в вычислительном отношении средство вычисления матричной экспоненты для особого случая, когда аргумент является
кососимметричной мат­ри­цей.

2.3.1.7

Единичные кватернионы

ватернионы Гамильтона могут считаться чистейшим злом,
« Ккоторое
не принесло ничего хорошего тем, кто работал с ними,
включая даже таких гениев, как Клерк Максвелл.
– Лорд Кельвин, 1892 г.

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

106 Глава 2 · Представление местоположения и ориентации

графике и аэрокосмических навигационных системах. Кватернион был изобретен почти 200 лет назад как расширение комплексного числа – гиперкомплексное число – с одной действительной и тремя комплексными частями
q̆ = s + υxi + υy j + υzk ∈ ℍ,

(2.29)

i2 = j2 = k2 = ijk = -1.

(2.30)

где ортогональные комплексные числа i, j и k определены так,
что

В настоящее время кватернион чаще рассматривается как
упорядоченная пара q̆ = (s, υ), иногда записываемая как s + υ, где
s ∈ ℝ – скалярная часть, а υ ∈ ℝ3 – векторная часть. В этой книге
мы также будем записывать ее в компонентной форме как
q̆ = s〈υx + υy + υz〉.

Кватернионы образуют векторное пространство q̆ ∈ ℍ и поэтому поддерживают такие операции, как сложение и вычитание, выполняемые поэлементно, а также умножение на скаляр.
Кватернионы также поддерживают сопряжение:
q̆∗ = s〈-υx, -υy, -υz〉 ∈ ℍ,

(2.31)

q̆1 ∘ q̆2 = s1s2 – υ1 · υ2〈s1υ2 + s2υ1 + υ1 × υ2〉 ∈ ℍ,

(2.32)

q̆1 · q̆2 = s1s2 + υx1υx2 + υy1υy2 + υz1υz2 ∈ ℝ,

(2.33)

а еще умножение на кватернион:

которое называется произведением кватернионов, или произведением Гамильтона, и является некоммутативной операцией. ► Внутреннее произведение – это скаляр:

а величина кватерниона вычисляется как

Если записать
кватернион в виде
4-элементного
вектора (s, υx, υy, υz),
то умножение
можно выразить как
умножение мат­ри­цы
на вектор, где

(2.34)
Чистый кватернион – это кватернион, скалярная часть которого равна нулю.
Для представления поворотов мы используем единичные
кватернионы, или версоры – кватернионы с единичной величиной ||q̆|| = 1 и обозначаемые как q̊. ► Их можно рассматривать
как поворот на угол θ относительно единичного вектора υ̂, который связан с компонентами единичного кватерниона соотношением
q̊ = cos 〈υ̂ sin 〉 ∈ S 3,

(2.35)

Матрица 4×4 определяется свойством
matrix класса
Quaternion.
Четыре элемента
единичного кватерниона образуют
вектор и известны
как параметры
вращения Эйлера.

Работа в трех измерениях (3D) 107

что схоже с представлением «угол-ось» из раздела 2.3.1.5. Оно
также имеет двойное отображение, что означает, что q̊ и -q̊
представляют одну и ту же ориентацию.
В Toolbox они реализованы классом UnitQuaternion, конструктор которого преобразует переданный аргумент, такой как мат­
ри­ца вращения, в единичный кватернион, например:
>>> q = UnitQuaternion(rpy2r(0.1, 0.2, 0.3))
0.9833 << 0.0343, 0.1060, 0.1436 >>
Композиция двух
Этот класс поддерживает ряд стандартных операторов и меортонормированных тодов. Например, операция умножения кватернионов реалиматриц вращения
требует 27 умноже- зована через перегрузку оператора умножения
ний и 18 сложений.
В форме кватернио- >>> q = q * q;
нов для этого нужно
16 умножений а инверсия (сопряжение единичного кватерниона) – в форме
и 12 сложений. Эта
метода
экономия особенно
важна для встраи­
ваемых систем. >>> q.inv()

0.9339 << -0.0674, -0.2085, -0.2824 >>

Отступление 2.17. Сэр Уильям Роуэн Гамильтон
Гамильтон (1805–1865) – ирландский математик, физик и астроном. Вдобавок он был
полиглотом и к 13 годам знал классические
и современные европейские языки, а также
персидский, арабский, хиндустани, санскрит
и малайский. Гамильтон выучил математику в 17 лет и обнаружил ошибку в «Небесной
механике» Лапласа. Всю жизнь он проработал в Тринити-колледже в Дуб­лине и был
назначен профессором астрономии и королевским астрономом Ирландии, еще будучи
студентом. Помимо создания теории кватернионов, он внес свой вклад в развитие
оптики, динамики и алгебры. Он также писал стихи и переписывался с Вордс­вортом,
который посоветовал ему посвятить свою
энергию математике.
Согласно легенде, ключевое уравнение кватерниона (2.30) пришло в голову
Гамильтону в 1843 г., когда он прогуливался со своей женой вдоль Королевского
канала в Дублине, и это событие увековечено мемориальной доской на Брумском
(Брогемском) мосту:
Проходя здесь 16 октября 1843 года, сэр Уильям Роуэн Гамильтон в озарении
гения открыл фундаментальную формулу умножения кватернионов i2 = j2 =
k2 = ijk = -1 и вырезал ее на камне этого моста.
Эта надпись больше не видна, но мост остается местом паломничества математиков и физиков.

108 Глава 2 · Представление местоположения и ориентации

Умножение кватерниона на обратный кватернион дает тождественный (нейтральный) кватернион
>>> q / q
1.0000 << 0.0000, 0.0000, 0.0000 >>

Единичный кватернион q̊ можно преобразовать в ортонормированную мат­ри­цу вращения следующим образом:
>>> q.R
array([[ 0.7536, -0.4993,
0.4275],
[ 0.5555, 0.8315, -0.008145],
[ -0.3514, 0.2436,
0.904]])

Вектор координат можно повернуть на кватернион, применив перегруженный оператор умножения
>>> q * [1, 0, 0]
array([ 0.7536, 0.5555, -0.3514])

И можно построить диаграмму, представляющую кватер­нион
>>> q.plot();

похожую по стилю на рис. 2.17. Класс UnitQuaternion реализует много методов и операторов, которые полностью описаны
в онлайн-документации.
ξ как единичный кватернион
В случае простого поворота в трехмерном пространстве движение
ξ можно реализовать с использованием единичного кватерниона
q̊ ∈ S3. Такая реализация обладает следующими свойствами:

композиция
ξ1 ⊕ ξ2
обращение
⊖ξ
тождественный ∅
элемент
векторное
ξ•υ
преобразование

↦ q̊ ∘ q̊, произведение Гамильтона;
↦ q̊∗ = s(-υ), сопряжение кватерниона;
↦ 1〈0〉;
↦ q̊ ∘ ῠ ∘ q̊∗, где ῠ = 0〈υ〉 – чистый
кватернион.

Композиция некоммутативна, т. е. q̊1 ∘ q̊2 ≠ q̊2 ∘ q̊1.

При работе с кватернионами сохранять можно только векторную часть единичного кватерниона, потому что скалярный
элемент можно восстановить по формуле s = ±(1 - υx2 - υy2 - υz2)1/2.
Неоднозначность знака можно устранить, убедившись, что скалярная часть положительна, при необходимости обратив кватернион перед вычислением векторной части. Такое представление единичного кватерниона в форме трехмерного вектора
часто используется в задачах оптимизации, таких как релаксация графа положения или регулировка пучка.

Работа в трех измерениях (3D) 109
 Будьте внимательны с левосторонними кватернионами!
 Гамильтон определил комплексные числа так, что ijk = -1, но в аэрокосмическом сообществе принято использовать кватернионы, где
ijk = 1. Они известны как JPL, перевернутые, или левосторонние,
кватернионы. Это обстоятельство может стать источником путаницы, и поэтому вы должны понимать соглашения, принятые в любых библиотеках или пакетах ПО, используемых вами. В этой книге
и в пакете Toolbox используются соглашения Гамильтона.
 порядочение элементов кватерниона
У
 Мы описали кватернионы с компонентами, неявно упорядоченными как s, υx, υy, υz, и функции для работы с кватернионами в пакете
spatialmath.base рассматривают кватернион как одномерный массив NumPy с 4 элементами, следующими в том же порядке. Некоторые программные инструменты, в частности ROS, рассматривают
кватернион как вектор υx, υy, υz, s.

2.3.2

Положение в трех измерениях

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

2.3.2.1

Матрица однородного преобразования

Вывод трехмерной мат­ри­цы однородного преобразования осуществляется так же, как в двухмерном случае (2.18), но расширен для учета измерения z:

где AtB ∈ ℝ3 – вектор, как показано на рис. 2.14, определяющий
начало координат системы {B} по отношению к системе {A},
а ARB ∈ SO(3) – мат­ри­ца вращения, которая описывает ориентацию осей системы {B} относительно системы {A}:

(2.36)

110 Глава 2 · Представление местоположения и ориентации
Отступление 2.18. Чтение мат­ри­цы однородного преобразования SE(3)
Однородная мат­ри­ца преобразования представляет относительное положение или движение.
Она определяет новую систему
координат относительно предыдущей, и ее легко записать
следую­щим образом:

новое начало
координат

новая ось x

новая ось x

новая ось x

где ATB – мат­ри­ца однородного преобразования 4×4, которую
часто называют «трансформацией». Эта мат­ри­ца имеет очень
специфическую структуру и принадлежит специальной евклидовой группе размерности 3, т. е. T ∈ SE(3) ⊂ ℝ4×4.
Матрица однородного преобразования 4×4 часто используется в робототехнике, компьютерной графике и компьютерном
зрении. Оно поддерживается пакетомToolbox и будет использоваться в этой книге как конкретное представление ξ – трехмерного относительного положения.
В Toolbox имеется множество функций для создания трехмерных однородных преобразований. Вот, например, демонст­
рация композиции преобразований:
>>> T = transl(2, 0, 0) @ trotx(pi / 2) @ transl(0, 1, 0)
array([[
1,
0,
0,
2],
[
0,
0,
-1,
0],
[
0,
1,
0,
1],
[
0,
0,
0,
1]])

Функция transl создает относительное положение с конечным смещением, но без поворота, а trotx создает относительное положение, соответствующее повороту на π/2 вокруг оси x
с нулевым смещением. Это выражение можно рассматривать
как перемещение вдоль оси x на 2 единицы, затем поворот на
90° вокруг оси x и далее перемещение на единичное расстояние
вдоль новой оси y, которая раньше была осью z. Отобразить новую систему координат можно командой

Для описания поворотов в Toolbox
имеются функции,
которые создают
мат­ри­цу вращения
(например, rotx,
rpy2r) или однородное преобразование
без перемещения
(например, trotx,
rpy2tr). Некоторые
функции Toolbox
принимают мат­ри­цу
вращения или однородное преобразование и игнорируют
поступательный
компонент, например tr2rpy.

>>> trplot(T);

Компонент мат­ри­цы T, описывающий поворот:
>>> t2r(T)
array([[
[
[

1,
0,
0,

0,
0,
1,

0],
-1],
0]])

и компонент, описывающий перемещение
>>> transl(T)
array([[
2,

0,

1])

:

По историческим
причинам transl
преобразует
ℝ3 ↦ SE(3), а также
SE(3) ↦ ℝ3, в зави­
симости от переданных аргументов.
transl2 ведет себя
аналогично.

Работа в трех измерениях (3D) 111
ξ как мат­ри­ца SE(3)
В случае поворота и перемещения в трехмерном пространстве
движение ξ можно реализовать как однородную мат­ри­цу T ∈
SE(3), которая иногда записывается как упорядоченная пара (R, t)
∈ SO(3) × ℝ3 и обладает следующими свойствами:

композиция

ξ1 ⊕ ξ2

↦ T1T2 =

⊖ξ

↦ T–1 =


обращение


тождественный ∅
элемент
векторное
ξ•υ
преобразование

умножение матриц;

обращение мат­ри­цы;
↦ 14×4, единичная мат­ри­ца;

↦ 𝜖(Tυ̃), умножение мат­ри­цы
на вектор,

где ˜· : ℝ3 ↦ ℙ3 и 𝜖(·) : ℙ3 ↦ ℝ3. Композиция некоммутативна, т. е.
T1T2 ≠ T2T1.

2.3.2.2

Матричная экспонента для определения положения

Рассмотрим мат­ри­цу SE(3)
>>> T = transl(2, 3, 4) @ trotx(0.3)
array([[
1,
0,
0,
[
0, 0.9553, -0.2955,
[
0, 0.2955, 0.9553,
[
0,
0,
0,

2],
3],
4],
1]])

и ее логарифм
>>> L = linalg.logm(T)
array([[
0,
0,
[
0,
0,
[
0,
0.3,
[
0,
0,

0,
-0.3,
0,
0,

2],
3.577],
3.52],
0]])

Это разреженная мат­ри­ца, и в ней отчетливо видна величина
угла поворота 0.3. Матрица имеет структуру расширенной кососимметричной мат­ри­цы: верхний левый угол – кососиммет­
ричная мат­ри­ца 3×3, верхний правый столбец – трехмерный
вектор, а нижняя строка содержит только нули. Мы можем извлечь шесть уникальных элементов, как показано ниже:
>>> S = vexa(L)
array([
2,

3.577,

3.52,

0.3,

0,

0])

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

112 Глава 2 · Представление местоположения и ориентации

Возведение логарифма мат­ри­цы в степень дает исходную
мат­ри­цу SE(3), а логарифм можно восстановить всего из шести
элементов S:
>>> linalg.expm(skewa(S))
array([[
1,
0,
0,
[
0, 0.9553, -0.2955,
[
0, 0.2955, 0.9553,
[
0,
0,
0,

2],
3],
4],
1]])

Запись общего случая выглядит так:
T = e[S] ∈ SE(3),

где S ∈ ℝ6 и [·] : ℝ6 ↦ se(3) ⊂ ℝ4×4. Вектор S является вектором
кручения, и обсудим его далее.
Отступление 2.19. Трехмерная расширенная кососимметричная
мат­ри­ца
Трехмерная расширенная кососимметричная мат­ри­ца, соответствующая вектору S = (υx, υy, υz, ωx, ωy , ωz), имеет вид:

(2.37)

которая имеет четкую структуру: нули на главной диагонали
и в нижней строке и кососимметричную мат­ри­цу в левом верхнем углу. Векторное пространство трехмерных расширенных кососимметричных матриц обозначается как se(3) и является ал­
геб­рой Ли SE(3). В пакете Toolbox оператор [·] реализован как
>>> X = skewa([1, 2, 3, 4, 5, 6])
array([[
0,
-6,
5,
[
6,
0,
-4,
[
-5,
4,
0,
[
0,
0,
0,
А оператор обращения ∨(·) – как
>>> vexa(X)
array([

2.3.2.3

1,

2,

3,

1],
2],
3],
0]])

4,

5,

6])

Кручения в 3D

Любое движение твердого тела в трехмерном пространстве эквивалентно винтовому движению (screw motion) – движению
вокруг и вдоль некоторой линии в пространстве, как показано
на рис. 2.24. Движение твердого тела между любыми двумя положениями можно представить таким винтовым движением.

Работа в трех измерениях (3D) 113

В случае простого поступательного движения вращение происходит вокруг винтовой оси, находящейся в бесконечности.
Представим винт как вектор кручения (υ, ω) ∈ ℝ6, где υ ∈ ℝ3
описывает момент, и вектор ω ∈ ℝ3, параллельный оси винта.
Момент υ кодирует положение линии оси кручения в пространстве, а также шаг винта – отношение расстояния вдоль оси винта к полному обороту вокруг оси винта.
Рассмотрим пример кручения вокруг оси x, когда ось винта
параллельна оси x и проходит через начало координат.
>>> S = Twist3.UnitRevolute([1, 0, 0], [0, 0, 0])
(0 0 0; 1 0 0)

Это конкретное кручение является единичным кручением
и описывает поворот на 1 рад вокруг оси x.
Чтобы создать преобразование SE(3) для определенного поворота, масштабируем его и возведем в степень
>>> linalg.expm(0.3 * skewa(S.S));

где атрибут .S – это вектор кручения в виде массива NumPy, или
более компактно:
>>> S.exp(0.3)
1
0
0
0.9553
0
0.2955
0
0
Результатом является объект SE3,
инкапсулирующий
массив NumPy
размером 4×4.
Подробнее об этом
рассказывается
в разделе 2.5.

0
-0.2955
0.9553
0

0
0
0
1

и получаем результат, аналогичный тому, который получили
бы, используя trotx(0.3). Однако кручение позволяет выразить вращение вокруг оси, проходящей через любую точку, а не
только через начало координат.
Для иллюстрации простого винтового движения будем последовательно поворачивать систему координат вокруг винта. Для этого определим винт с шагом 0.5, параллельный оси z
и проходящий через точку (2, 3, 2):
>>> S = Twist3.UnitRevolute([0, 0, 1], [2, 3, 2], 0.5)
(3 -2 0.5; 0 0 1)

а система координат, которую нужно повернуть, описывается
мат­ри­цей SE(3):
>>> X = transl(3, 4, -4);
Атрибут .A полученного объекта SE3 –
это мат­ри­ца SE(3)
в форме массива
NumPy.

Для значений в диапазоне от 0 до 15 рад мы вычисляем кручение для каждого значения θ, применяем его к системе {X}
и отображаем результат.
>>> for theta in np.arange(0, 15, 0.3):
...
trplot(S.exp(theta).A @ X, style="rgb", width=2)

114 Глава 2 · Представление местоположения и ориентации

График на рис. 2.21 наглядно демонстрирует последовательные положения системы {X} в процессе ее вращения вокруг оси
винта. Ось винта – это трехмерная линия
>>> L = S.line()
{ -3 2 -1; 0 0 1}
>>> L.plot("k:", linewidth=2);

которая показана на графике черной пунктирной линией. ►

Трехмерная линия
описывается объектом Line3 в координатах Плюккера,
о которых подробнее рассказывается
в разделе C.1.2.2.

Рис. 2.21. Система координат
отображается для различных
значений θ относительно винта,
параллельного оси z и проходящего
через точку (2, 3, 2). Оси x, y и z
обозначены красной, зеленой и синей
линиями соответственно

Поступательное движение вдоль оси y, сопровождающее вращение вокруг оси, определяется как
>>> S = Twist3.UnitPrismatic([0, 1, 0])
(0 1 0; 0 0 0)

и описывает смещение на 1 в указанном направлении. Для создания преобразования SE(3), соответствующего конкретному
смещению, мы масштабируем и возводим в степень единичный оборот
>>> S.exp(2)
1
0
0
1
0
0
0
0

0
0
1
0

0
2
0
1

Это преобразование определяет нулевой угол поворота и перемещение на 2 единицы в направлении оси y.
Аналогично можно любую мат­ри­цу SE(3) преобразовать
в кручение. Для примера преобразуем вектор кручения из раздела 2.3.2.2:

Работа в трех измерениях (3D) 115
>>> T = transl(1, 2, 3) @ eul2tr(0.3, 0.4, 0.5);
>>> S = Twist3(T)
(1.1204 1.6446 3.1778; 0.041006 0.4087 0.78907)

и, как видите, вектор кручения является уникальным элементом логарифма мат­ри­цы SE(3). В этом примере винт параллелен прямой
>>> S.w
array([ 0.04101, 0.4087, 0.7891])

проходит через точку
>>> S.pole
array([0.001138, 0.8473, -0.4389])

имеет шаг
>>> S.pitch
3.226

и при движении твердого тела вдоль винта вызывает его поворот на угол
>>> S.тета
0.8896

радиан, что составляет около 1/7 оборота.
ξ как кручение в 3D
В случае поворота и перемещения в трехмерном пространстве
движение ξ можно реализовать как кручение S ∈ ℝ6, обладающее
следующими свойствами:

композиция
ξ1 ⊕ ξ2
обращение
⊖ξ
тождественный ∅
элемент
векторное
ξ•υ
преобразование

↦ log(e[S1] e[S2]), умножение экспонент;
↦ –S, отрицание;
↦ 01×6, нулевой вектор;
↦ 𝜖(e[S]υ̃), умножение мат­ри­цы
на однородный вектор,

где ˜· : ℝ3 ↦ ℙ3 и 𝜖(·) : ℙ3 ↦ ℝ3. Композиция некоммутативна, т. е.
e[S1] e[S2] ≠ e[S2] e[S1]. Обратите внимание: log и exp имеют эффективные аналитические решения, что делает композицию относительно недорогой с вычислительной точки зрения.

2.3.2.4

Пара вектор–кватернион

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

116 Глава 2 · Представление местоположения и ориентации
ξ как пара вектор–кватернион
В случае поворота и перемещения в трехмерном пространстве
движение ξ можно реализовать как комбинацию вектора и единичного кватерниона в виде упорядоченной пары (t, q̊) ∈ ℝ3 × S3.
Такая реализация обладает следующими свойствами:

композиция
обращение
тождественный элемент
векторное преобразование

ξ1 ⊕ ξ2
⊖ξ
∅
ξ•υ

↦ (t1 + q̊1 · t2, q̊1 ∘ q̊2);
↦ (–q̊∗ · t, q̊∗);
↦ (03, 1〈0〉);
↦ q̊ · υ + t.

Композиция некоммутативна, т. е. ξ1ξ2 ≠ ξ2ξ1. Описание оператора
векторного преобразования • : S 3 × ℝ3 ↦ ℝ3 можно найти в разделе 2.3.1.7.

2.3.2.5

Единичный дуальный кватернион

Ранее для представления положения с помощью кватернионов использовались бикватернионы Гамильтона, в которых
коэффициенты были комплексными числами. Позже Уильям
Клиффорд ► разработал дуальное число, определяемое в виде
упорядоченной пары d = (x, y), которую можно записать как d =
x + yε, где ε2 = 0. Клиффорд создал кватернионное дуальное число с x, y ∈ ℍ, которое он назвал бикватернионом, но в настоящее
время такие кватернионы называются дуальными.
ξ как единичный дуальный кватернион
В случае поворота и перемещения в трехмерном пространстве
движение ξ можно реализовать как единичный дуальный кватернион (q̊r, q̆d) ∈ S3 × ℍ, который включает единичный кватернион
q̊r (вещественная часть), представляющий поворот, и кватернион
q̆d =1/2t̆ ∘ q̊r (дуальная часть), где t̆ – чистый кватернион, представляющий перемещение. Такая реализация обладает следующими
свойствами:
композиция
ξ1 ⊕ ξ2
обращение
⊖ξ
тождественный ∅
элемент
векторное
ξ•υ
преобразование


↦ (q̊r1 ∘ q̊r2, q̊r1 ∘ q̆d2 + q̆d1 ∘ q̊r2);
↦ (q̊r, q̆d)∗ = (q̊∗r , q̆d∗ ), сопряжение;
↦ (1〈0〉, 1〈0〉);

↦ (q̊r, q̆d) ∘ (1〈0〉, ῠ) ∘ (q̊r, q̆d)∗,
где средний член – чистый дуальный
кватернион.

Композиция некоммутативна, т. е. ξ1ξ2 ≠ ξ2ξ1. Обратите внимание,
что в общем случае произведение Гамильтона общего кватернио­
на p̆ и единичного кватерниона q̊ не является единичным кватернионом.

Уильям Клиффорд
(1845–1879) –
английский математик и геометр.
Внес существенный
вклад в развитие
геометрической
алгебры, и алгебра
Клиффорда названа
в его честь.

Дополнительные темы 117

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

2.4

Дополнительные темы

2.4.1

 рямой и обратный порядок умножения
П
преобразований

Во многих книгах проводится различие между обратным умно­
жением относительных преобразований (pre-multiplying)
и прямым (post-multiplying). В случае применения последовательности матриц вращения первая вызывает преобразование
относительно мировой системы координат, а вторая – относительно текущей, уже повернутой системы. Различие лучше
всего пояснить на примере диаграммы положений, представленной на рис. 2.22. Положение {A} определяется относительно
опорной системы координат {0}.

Рис. 2.22. Демонстрация влияния
умножения слева и справа в виде
графа положений

Теперь суть прямого умножения должна быть очевидна – оно
представляет собой преобразование из {A} в {B}, выраженное
в системе отсчета {A} – повернутой системе отсчета. Результат,
0
ξB, представляет преобразование из {0} в {A}, а затем в {B}.
Умножение в обратном порядке показано красной стрелкой.
Оно требует добавления нового положения, откуда должна начинаться стрелка (обозначим ее как {0¢}). Мы можем объединить эти движения (нижний индекс левого сомножителя должен совпадать с верхним индексом правого сомножителя) как

118 Глава 2 · Представление местоположения и ориентации

ξ0 ⊕ 0ξA. Фактически это изменение системы отсчета – с {0} на
{0¢}. Таким образом, преобразование тоже выполняется относительно системы отсчета, но другой – новой.
Последовательность трех поворотов относительно осей zyx
(на углы крена, тангажа, рыскания) из (2.23)
0¢

RB = Rz(γ)Ry(β)Rx(α)

0

можно интерпретировать двумя различными, но эквивалентными способами. Во-первых, используя умножение в обратном
порядке, эту последовательность можно рассматривать справа
налево как следующие друг за другом повороты вокруг опорной системы координат на Rx(α), затем на Ry(β) и потом на Rz(γ).
Повороты, выполняемые в такой последовательности, называются внешними поворотами. С другой стороны, используя
умножение в прямом порядке, эту последовательность можно
рассматривать справа налево как последовательные повороты,
каждый относительно предыдущей системы координат: сначала Rz(γ), затем на Ry(β) и, наконец, на Rx(α). Повороты, выполняемые в такой последовательности, называются внутренними
поворотами.

2.4.2

Поворот системы координат и поворот точки

Во многих источниках различают преобразование вектора координат точки p путем поворота системы координат (пассивное вращение)
q = Rp p
и путем поворота точки (активное вращение)
q = Ra p.
Для большей ясности перепишем эти уравнения, использовав надстрочные и подстрочные обозначения. Пассивное вращение
q = 0RpB Bp

0

преобразует вектор координат из повернутой системы координат тела в мировую, тогда как активное вращение
q = BRa0 0p

B

преобразует вектор координат из мировой системы координат
в систему координат повернутого тела. Активное и пассивное
вращения являются обратными, и Ra = (Rp)T.

Дополнительные темы 119

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

2.4.3

Матрица направляющих косинусов

Рассмотрим две системы координат, {A} и {B}. Матрицу вращения ARB можно записать как
(2.38)
где cos(ûA, υ̂B), u, v ∈ {x, y, z} – косинус угла между единичным
вектором ûA в {A} и единичным вектором υ̂B в {B}. Поскольку ûA
и υ̂B – единичные векторы, cos(ûA, υ̂B) = ûA · υ̂B. Эта мат­ри­ца называется мат­ри­цей направляющих косинусов (direction cosine
matrix, DCM) и часто используется в аэрокосмической технике.

2.4.4

Эффективность представления

Многие из представлений, обсуждаемых в этой главе, имеют больше параметров, чем необходимо. Например, мат­ри­ца
SO(2) имеет размер 2×2 и четыре элемента, представляющих
один угол поворота. Эту избыточность можно объяснить ограничениями: каждый столбец имеет единичную длину (||c1|| =
||c2|| = 1), что накладывает два ограничения, и столбцы ортогональны (c1 · c2 = 0), что добавляет еще одно ограничение. Четыре
элемента мат­ри­цы с тремя ограничениями фактически представляют одно независимое значение.
В табл. 2.1 приводятся различные представления, соответствующее им количество параметров N и минимальное количество требуемых параметров Nmin. Матричные представления
наименее эффективны, имеют наименьшее отношение Nmin /N
и требуют значительных затрат памяти. Зато они обеспечивают простоту композиции, которая представляет собой простое
умножение матриц и на современном компьютерном оборудовании выполняется очень эффективно. Если объем доступной
памяти ограничен, то, например, можно использовать простой
прием: не хранить нижнюю строку матриц, что эквивалентно
представлению однородной мат­ри­цы преобразования в виде

120 Глава 2 · Представление местоположения и ориентации

упорядоченной пары (R, t). Этот прием часто встречается в компьютерном зрении.
Таблица 2.1. Количество параметров, необходимых для различных
представлений положений. Значком † обозначен вариант без сохранения
константы в нижней строке мат­ри­цы
Тип
Матрица SO(2)
Матрица SE(2)
Матрица SE(2) в виде мат­ри­цы 2×3 †
Вектор кручения в 2D
Матрица SO(3)
Единичный кватернион
Единичный кватернион, векторная часть
Параметры Эйлера
Матрица SE(3)
Матрица SE(3) в виде мат­ри­цы 3×4 †
Вектор кручения в 3D
Вектор + единичный кватернион
Единичный дуальный кватернион

2.4.5

N
4
9
6
3
9
4
3
3
16
12
6
7
8

Nmin
1
3
3
3
3
3
3
3
6
6
6
6
6

Nmin /N
25 %
33 %
50 %
100 %
33 %
75 %
100 %
100 %
38 %
50 %
100 %
86 %
75 %

Расстояние между ориентациями

Расстояние между двумя евклидовыми точками равно длине
прямой линии между ними, то есть ||p1 - p2||, pi ∈ ℝn. Расстояние
между двумя ориентациями – более сложная величина и, в отличие от расстояния перемещения, имеет верхнюю границу.
Некоторые распространенные метрики для матриц вращения
и кватернионов:
d(R1, R2) = ||13×3 - R1R2T|| ∈ [0, 2];

(2.39)

d(q̊1, q̊2) = 1 - |q̊1 · q̊2| ∈ [0, 1];

(2.41)

d(R1, R2) = ||log R1R2T|| ∈ [0, π];

(2.40)

d(q̊1, q̊2) = cos-1|q̊1 · q̊2| ∈ [0, π/2];

(2.42)

d(q̊1, q̊2) = 2 cos-1|q̊1 · q̊2| ∈ [0, π],

(2.44)

d(q̊1, q̊2) = 2tan-1

∈ [0, π/2];

(2.43)

где оператор · для единичных кватернионов – это скалярное
произведение (2.33). Все эти метрики являются собственными
метриками расстояния, т. е. d(x, x) = 0, d(x, y) = d(y, x), и выполняется неравенство треугольника d(x, z) ≤ d(x, y) + d(y, z). Метрика
d(x, y) = 0 подразумевает, что x = y, но для единичных кватер-

Дополнительные темы 121

нионов это неверно, поскольку из-за двойного отображения
также верно, что d(q̊, -q̊) = 0. Этой проблемы можно избежать,
гарантировав неотрицательность скалярной части единичных
кватернионов. (2.42) и (2.43) эквивалентны, но последний численно более устойчив.
Эти метрики можно вычислить с помощью метода angdist
классов SO3 и UnitQuaternion, которые по умолчанию используют
(2.43). Например:
>>> UnitQuaternion.Rx(pi / 2).angdist(UnitQuaternion.Rz(-pi / 2))
1.047

2.4.6

Нормализация

Стандарт IEEE для Арифметика с плавающей запятой имеет конечную точность
чисел с плавающей ◄, и последовательные операции будут накапливать ошибку.
запятой двойной
точности (64 разря- Матрица вращения по определению имеет единичный опредеда) имеет точность литель:
около 16 знаков
после запятой. >>> R = np.eye(3,3);

>>> np.linalg.det(R) - 1
0

Но если несколько раз выполнить умножение на допустимую
мат­ри­цу вращения, то результат
>>> for i in range(100):
...
R = R @ rpy2r(0.2, 0.3, 0.4);
>>> np.linalg.det(R) - 1
5.773e-15
На разных
компьютерах
величина ошибки
может быть разной.

будет содержать небольшую ошибку – определитель больше
не равен единице, и мат­ри­ца больше не является правильной
ортонормированной мат­ри­цей поворота. Чтобы исправить
проблему, нужно нормализовать мат­ри­цу – процесс, который
налагает ограничения на столбцы ci ортонормированной мат­
ри­цы R = [c0, c1, c2]. Нам нужно предположить, что один столбец
имеет правильное направление
c¢2 = c2,
тогда первый столбец делается ортогональным двум последним:
c¢0 = c1 × c¢2.
Однако последние два столбца, возможно, не были ортогональны, поэтому
c¢1 = c¢2 × c¢0.

122 Глава 2 · Представление местоположения и ориентации

Наконец, все столбцы нормализованы к единичной величине:
.
В Toolbox нормализация реализована при помощи следующей функции:
>>> R = trnorm(R);

И ее применение делает определитель намного ближе к единице ►:
>>> np.linalg.det(R) - 1
2.22e-16

Нормализацию можно также применить к мат­ри­це SE(3), при
этом нормализована будет только подмат­ри­ца вращения SO(3).
Аналогичная проблема возникает с единичными кватернио­
нами, когда норма или величина единичного кватерниона не
равна единице. Однако эту проблему проще исправить, потому
что нормализация кватерниона заключается в простом делении всех элементов на норму

которое реализовано в методе
>>> q = q.unit();

Классы SO2, SE2, SO3, SE3 и UnitQuaternion также поддерживают
разновидность умножения с использованием оператора @:
>>> T = T1 @ T2
>>> q = q1 @ q2

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

2.4.7

Понятие экспоненциального отображения

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

Величина ошибки
теперь находится на
пределе арифметики с двойной точностью,
которая составляет
2.2204×10−16 и определяется функцией
np.finfo.eps
в NumPy.

Дополнительные темы 123

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

лежит математика групп Ли, описанная в учебниках по алгеб­
раической топологии. Работа с ней требует глубоких знаний
в области продвинутой математики, и многие читатели, начинающие заниматься робототехникой, сочтут содержание таких
учебников совершенно недоступным для понимания. Введение
в основы этой темы дано в приложении D. В этом разделе мы
будем использовать интуитивный подход, основанный на математике уровня инженерного бакалавриата, чтобы прояснить
эти взаимосвязи.
На рис. 2.23 показана точка P, заданная координатным вектором p, которая поворачивается вокруг оси, параллельной
вектору ω, величина которого ||ω|| определяет угловую скорость
вращения. Нам нужно повернуть точку на угол θ вокруг этой
оси, а скорость точки, как известно из механики, равна
ṗ = ω × p,
и мы заменим векторное произведение умножением кососимметричной мат­ри­цы на вектор:
ṗ = [ω]× p.

(2.45)

Рис. 2.23. Точка P движется по окружности
в плоскости, перпендикулярной оси ω

Мы можем найти решение этого дифференциального уравнения первого порядка по аналогии с простым скалярным случаем
ẋ = ax,
чье решение имеет вид:
x(t) = eatx(0).
Это означает, что решение уравнения (2.45) будет следующим:
p(t) = e[ω]×t p(0).

124 Глава 2 · Представление местоположения и ориентации

Если ||ω|| = 1, то через t секунд вектор повернется на t радиан.
Нам требуется поворот на θ, поэтому мы можем задать t = θ,
чтобы получить уравнение
p(θ) = e[ω̂]×θ p(0),
которое описывает вектор p(0), поворачиваемый в p(θ). Мат­
рица, поворачивающая вектор, является мат­ри­цей поворота,
и это означает, что наша экспоненциальная мат­ри­ца – это мат­
ри­ца вращения:
R(θ, ω̂) = e[ω̂]×θ ∈ SO(3).

Теперь рассмотрим более общий случай вращательного и поступательного движения. Мы можем написать уравнение
ṗ = [ω]× p + υ
и переписать его в матричном виде:

После введения однородных координат уравнение принимает вид:

где Σ(υ, ω) – дополненная кососимметричная мат­ри­ца 4×4.
Опять же, по аналогии со скалярным случаем мы можем записать решение в виде:
p̃(θ) = eΣ(υ, ω)θp̃(0).
Матрица, которая поворачивает и переводит точку в однородные координаты, является мат­ри­цей однородного преобразования, а это означает, что наша экспоненциальная мат­ри­ца
тоже является мат­ри­цей однородного преобразования:

где [ω̂]×θ определяет величину и ось вращения, а υθ – перенос.

Дополнительные темы 125

2.4.8

Подробнее о кручениях

сякое перемещение тела в пространстве может быть осу­
« Вществлено
посредством поворота тела вокруг единствен­
ной прямой в пространстве, сопровождаемого перемещением
тела параллельно этой прямой.
– Теорема Шаля

В этой главе мы ввели и применили кручения в 2D и 3D, а здесь
дадим им более формальное определение. Мы также подчерк­
нем очень тесную связь между кручениями и однородными
мат­ри­ца­ми преобразования посредством экспоненциального
отображения.
Теорема Шаля описывает движение тела, жестко закрепленного на гайке, вращающейся вокруг винта, как показано на
рис. 2.24. При вращении гайки вместе с ней вращается и перемещается в пространстве ее система координат. Математика тео­
рии винтов была разработана сэром Робертом Боллом в конце
XIX в. для анализа механизмов.

go.sn.pub/tyNyom

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

Общее смещение твердого тела в трехмерном пространстве
можно представить вектором кручения
S = (υ, ω) ∈ ℝ6,

где υ ∈ ℝ3 называется моментом и описывает положение оси
винта в пространстве и его шаг, а ω ∈ ℝ3 – направление оси винта.

126 Глава 2 · Представление местоположения и ориентации

Начнем с единичного кручения. В случае чистого вращения
единичный поворот вокруг оси винта, параллельной единичному вектору â и проходящей через точку Q, которая определяется координатным вектором q, задается как
Ŝ = (q × â, â)
и соответствует единичному повороту на 1 радиан вокруг оси
винта.
Шаг винта – это отношение расстояния вдоль оси винта к углу
поворота вокруг оси. При чистом вращении шаг равен нулю.
В более общем случае, когда ось винта параллельна вектору â
и проходит через точку, определяемую вектором q, а шаг имеет
величину p, единичное кручение равно
Ŝ = (q × â + pâ, â)
и шаг равен
p = ω̂Tυ.
В одном крайнем случае мы имеем чистое вращение, когда шаг винта равен нулю. В другом крайнем случае мы имеем
чис­тое поступательное движение, когда шаг винта бесконечен,
вследствие чего поворот не выполняется. В этом случае единичное кручение равно
Ŝ = (â, 0)
и соответствует единичному перемещению вдоль оси винта.
Единичное кручение описывает движение, вызванное поворотом на 1 рад вокруг оси винта. Единичное поворотное кручение имеет ||ω|| = 1, а единичное поступательное кручение имеет
ω = 0 и ||υ|| = 1. Мы также можем рассматривать единичный поворот как определение семейства движений
S = θŜ,
где Ŝ полностью определяет винт, а скалярный параметр θ описывает угол поворота вокруг винта.
Вектор кручения можно записать в некомпактной форме как
расширенную кососимметричную мат­ри­цу, которая для трехмерного случая имеет вид

.

Дополнительные темы 127
Отступление 2.20. Мишель Шаль
Шаль (1793–1880) – французский математик, родившийся в Эперноне. Он учился в Политехнической
школе в Париже у Пуассона и в 1814 г. был призван
защищать Париж в войне Шестой коалиции. В 1837 г.
опубликовал работу о происхождении и развитии методов в геометрии, которая принесла ему значительную известность, и был назначен профессором Политехнической школы в 1841 г. и Сорбонны в 1846 г.
Он был заядлым коллекционером и купил более
27 000 поддельных писем, якобы принадлежащих
перу Ньютона, Паскаля и других исторических личностей, – все они были написаны на французском
языке! В одном из писем от имени Паскаля утверждалось, что он открыл законы гравитации раньше Ньютона. В 1867 г. Шаль передал письма во Французскую
академию наук, но ученые пришли к выводу, что это
подделки. В конце концов Шаль признал, что его обманули, и рассказал, что потратил на письма почти 150 000 франков. Он похоронен на кладбище Пер-Лашез
в Париже.

Матрица принадлежит векторному пространству se(3), ал­
геб­ре Ли SE(3) и является генератором смещения твердого тела.
Смещение в виде мат­ри­цы SE(3) получается экспоненциальным отображением
T = e[S] ∈ SE(3).

Если движение выразить через единичное кручение, то можно записать
Т(θ, Ŝ) = eθ[Ŝ] ∈ SE(3),

и экспоненциальная мат­ри­ца получает эффективную аналитическую форму


(2.46)
где R(θ, ω̂) вычисляется по формуле вращения Родригеса (2.27).
Этот алгоритм реализован в trexp и trexp2.
В Toolbox кручения реализованы как классы: Twist3 для трехмерного случая и Twist2 для двухмерного. Оба класса имеют
множество свойств и методов. Трехмерное единичное кручение
создается как

128 Глава 2 · Представление местоположения и ориентации
>>> S = Twist3.UnitRevolute([1, 0, 0], [0, 0, 0])
(0 0 0; 1 0 0)

и описывает единичный поворот вокруг оси x. Вектор кручения
и его компоненты можно получить:
>>> S.S
array([
>>> S.v
array([
>>> S.w
array([

0,

0,

0,

0,

0,

0])

1,

0,

0])

1,

0,

0])

Мы можем отобразить мат­ри­цу кручения se(3) из алгебры Ли:
>>> S.se3()
array([[
[
[
[

0,
0,
0,
0,

0,
0,
1,
0,

0,
-1,
0,
0,

0],
0],
0],
0]])

Если умножить ее на 0.3 и вычислить экспоненту
>>> trexp(0.3
array([[
[
[
[

* S.se3())
1,
0,
0,
0, 0.9553, -0.2955,
0, 0.2955, 0.9553,
0,
0,
0,

0],
0],
0],
1]])

то получится результат, который можно также получить вызовом trotx(0.3).
>>> S.exp(0.3)
1
0
0
0.9553
0
0.2955
0
0

0
-0.2955
0.9553
0

0
0
0
1

Композиция поддерживается с использованием перегруженного оператора умножения.
>>> S2 = S * S
(0 0 0; 2 0 0)
>>> S2.printline(orient="angvec", unit="rad")
t = 0, 0, 0; angvec = (2 | 1, 0, 0)

И результатом в этом случае является поворот на две единицы, или на 2 радиана, вокруг оси x.
Метод line возвращает объект Line3, выражающий ось винта
в координатах Плюккера:
>>> line = S.line()
{ 0 0 0; 1 0 0}

Дополнительные темы 129

которую можно отобразить на диаграмме как
>>> line.plot("k:", linewidth=2);

Любое движение твердого тела, описываемое уравнением
T ∈ SE(n), можно также описать с помощью кручения:
S = ∨(log T).

Винт определяется единичным крутящим моментом
Ŝ = S/θ
и параметром движения

Рассмотрим пример
>>> T = transl(1, 2, 3) @ eul2tr(0.3, 0.4, 0.5);
>>> S = Twist3(T)
(1.1204 1.6446 3.1778; 0.041006 0.4087 0.78907)

Единичное кручение вычисляется как
>>> S / S.theta
(1.2594 1.8488 3.5722; 0.046096 0.45943 0.88702)

что дает тот же результат, что и вызов
>>> S.unit();

Масштабирование кручения нулем приводит к получению
нулевого движения, которое выражается мат­ри­цей SE(3)
>>> S.exp(0)
1
0
0
0

0
1
0
0

0
0
1
0

0
0
0
1

а масштабирование единицей дает исходное смещение твердого тела:
>>> S.exp(1)
0.6305 -0.6812
0.6969
0.7079
-0.3417
0.1867
0
0

0.372
0.1151
0.9211
0

1
2
3
1

Если взять только половину необходимого поворота
>>> S.exp(0.5)
0.9029 -0.3796

0.2017

0.5447

130 Глава 2 · Представление местоположения и ориентации
0.3837
-0.1937
0

0.9232
0.05949
0

0.01982
0.9793
0

0.9153
1.542
1

то мы получим промежуточное смещение. Это не линейная,
а геликоидальная (helicoidal) интерполяция.
Пакет Toolbox включает интерактивный графический инструмент
>>> %run -m twistdemo

который позволяет экспериментировать с кручениями и визуализировать влияние параметров кручения и экспоненты на положение объекта, как показано на рис. 2.25.►

Рис. 2.25. Пример twistdemo из Robotics Toolbox позволяет исследовать кручение и увидеть,
как кручение изменяет ориентацию тела в трехмерном пространстве. Летательный аппарат
центрирован в начале координат, а параметры кручения, такие как точка пересечения
с плоскостью xy, направление и тангаж, можно задать с помощью ползунков. Ползунок twist
rotation (поворот кручения) поворачивает летательный аппарат вокруг оси закрутки (тонкий
синий цилиндр). Это изображение получено в браузере с помощью визуализатора Swift

 Правильный способ композиции кручений
 Поскольку кручения являются логарифмами преобразований положения твердого тела, возникает соблазн объединить преобразования путем сложения кручений вместо умножения матриц SE(3).
Этот прием сработает для действительных чисел: если x = log X
и y = log Y, то
Z = XY = e x e y = ex+y,
 но в случае с мат­ри­ца­ми это верно, только если мат­ри­цы коммутативны, но мат­ри­цы вращения некомутативны, поэтому

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

Дополнительные темы 131
Z = XY = e x e y ≠ e x+y, если x, y ∈ so(n) или se(n).

 Проще говоря, нет способа быстрого вычисления композиции положений, и мы должны вычислить z = log(e xe y), а не просто z = x + y.
К счастью, эти мат­ри­цы имеют особую структуру, и существуют эффективные способы вычисления логарифма и экспоненты (2.46).

2.4.9

Конфигурационное пространство

До сих пор мы рассматривали положения объектов с точки зрения местоположения и ориентации системы координат, прикрепленной к ним. Для робота-манипулятора мы можем связать
систему координат с его рабочим органом, а для передвижного
робота или беспилотного летательного аппарата (БПЛА) – с его
корпусом. Этого достаточно для описания состояния робота
в привычном двух- или трехмерном евклидовом пространстве,
которое называется пространством задач, или операционным
пространством, поскольку именно в нем робот выполняет задачи или действует. Однако это ничего не говорит о сочленениях
и форме руки, позиционирующей рабочий орган.
Альтернативный подход основан на классической механике
и называется конфигурацией системы. Конфигурация – это наименьший набор параметров, называемых обобщенными координатами, необходимых для полного описания местоположения каждой частицы в системе. Это не так сложно, как может
показаться, поскольку объекты реального мира, как правило,
жесткие, и в каждой из них частицы имеют постоянный вектор
координат относительно системы координат объекта.
Если система представляет собой поезд, движущийся по
рельсам, то все составляющие его частицы движутся вместе,
Отступление 2.21. Сэр Роберт Болл
Болл (1840–1913) – ирландский астроном, родившийся в Дублине. Он стал профессором прикладной
математики в Королевском научном колледже в Дуб­
лине в 1867 г., а в 1874 г. – королевским астрономом
Ирландии и профессором астрономии в Дублинском
университете. В 1892 г. он был назначен профессором
астрономии и геометрии в Кембриджском университете и стал директором Кембриджской обсерватории.
Он был членом Королевского общества, а в 1900 г. стал
первым президентом Общества кватернионов.
Наиболее известен своим вкладом в науку о кинематике, описанным в его трактате «Теория винтов»
(1876 г.), но также опуб­ликовал «Трактат о сферической астрономии» (1908 г.) и ряд популярных статей
по астрономии. Похоронен в приходе Вознесенского могильника в Кембридже.

132 Глава 2 · Представление местоположения и ориентации

и для описания их местоположения достаточно одной обобщенной координаты q – удаленности от некоторой точки отсчета вдоль рельсов. Роботизированная рука с неподвижным
основанием и двумя жесткими звеньями, соединенными двумя
поворотными сочленениями, имеет конфигурацию, которая
полностью описывается двумя обобщенными координатами –
двумя углами между сочленениями (q1, q2). Обобщенные координаты, как следует из их названия, могут определять смещения или повороты.
Количество независимых обобщенных координат N называется числом степеней свободы системы. Любая конфигурация
системы представлена точкой q в N-мерном конфигурационном пространстве, или C-пространстве, которое обозначается
как 𝒞 и q ∈ 𝒞. Можно также сказать, что dim 𝒞 = N. В примере
с поездом 𝒞 ⊂ ℝ, это означает, что смещение – ограниченное
действительное число. Для двухшарнирного манипулятора
обобщенные координаты – это углы, поэтому 𝒞 ⊂ S1 × S1.
Вернемся к примеру с поездом, движущимся по рельсам.
Возможно, будет интересно описать поезд с точки зрения его
местоположения на плоскости, которую мы называем пространством задачи; в данном случае пространство задачи – это
𝒯 ⊂ ℝ2. Нас также могут интересовать широта и долгота поезда, в этом случае пространство задач будет выражаться как
𝒯 ⊂ S2. Мы также могли бы выбрать пространство задачи, совпадающее с привычным трехмерным миром, т. е. 𝒯 ⊂ ℝ3 × S3,
в котором учитывается изменение высоты при движении поезда вверх и вниз по склонам, а также изменение его ориентации при движении по закруглениям поворотов. Любую точку
в конфигурационном пространстве можно отобразить в точку
в пространстве задач q ∈ 𝒞 ↦ τ ∈ 𝒯, и это отображение зависит от
конкретного выбранного пространства задач. Однако в общем
случае не все точки в пространстве задачи можно отобразить
в конфигурационное пространство, потому что некоторые точки в пространстве задачи недоступны. Каждую точку на рельсах
можно отобразить в точку в пространстве задачи, но большинство точек в пространстве задачи нельзя отобразить в точку на
рельсах. Перемещение поезда ограничено фиксированными
рельсами – он может двигаться только в подмножестве пространства задачи. Если размерность пространства задачи превышает размерность конфигурационного пространства, dim 𝒯 >
dim 𝒞, то система может получить доступ только к подпространству меньшей размерности во всем пространстве задачи.
Простой манипулятор с двумя шарнирными сочленениями
может получить доступ к подмножеству точек на плоскости,
поэтому полезным пространством задачи может быть 𝒯 ⊂ ℝ2.
Размерность пространства задачи равна размерности конфигурационного пространства, dim 𝒯 = dim 𝒞, и это означает, что

То есть не существует голономных ограничений на систему.

Использование Toolbox 133

отображение между операционным и конфигурационным
пространствами является двунаправленным, но не обязательно уникальным – в общем случае для робота этого типа в одну
точку пространства задачи отображаются две различные конфигурации. Точки в пространстве задачи, находящиеся вне
физической досягаемости робота, не отображаются в конфигурационное пространство. Если выбрать пространство задачи с бо́льшим количеством измерений, например двумя или
тремя, то dim 𝒯 > dim 𝒞, и робот сможет получить доступ только
к точкам внутри подмножества этого пространства.
Теперь рассмотрим робота, показанного на рис. 8.10, со змее­
видной рукой-манипулятором, насчитывающей 20 сочленений, т. е. 𝒞 ⊂ (S1)20 и dim 𝒯 < dim 𝒞. В этом случае одной и той же
точке в пространстве задачи будет соответствовать бесконечное число конфигураций в 20 - 6 = 14-мерном подпространстве
20-мерного конфигурационного пространства. Это означает,
что, помимо позиционирования рабочего органа робота, можно одновременно выполнять движения в подпространстве конфигураций, управляя формой руки и обходя препятствия. Такой робот называется чрезмерно, или избыточно, подвижным
(overactuated), и мы затронем эту тему в разделе 8.3.4.2.
Рама квадрокоптера, показанного на рис. 4.22г, представляет
собой единое твердое тело, конфигурация которого полностью
описывается шестью обобщенными координатами – его положением и ориентацией в трехмерном пространстве 𝒞 ⊂ ℝ3 × (S1)3,
где ориентация выражена в некотором представлении с тремя
углами. Для такого робота наиболее логичным пространством
задачи было бы ℝ3 × S3, что эквивалентно пространству конфигураций и размерности 𝒯 = dim 𝒞. Однако у квадрокоптера всего четыре привода, т. е. он не может напрямую получить доступ
ко всем точкам своего пространства конфигураций и, следовательно, к своему пространству задачи. Такой робот называется малоприводной (underactuated) системой, и мы вернемся
к этой важной теме в разделе 4.2.

2.5

Использование Toolbox

В этой книге используется большое количество программного обеспечения с открытым исходным кодом, как показано на
рис. 2.26. В данной главе представлены функциональные возможности пакета пространственной математики Spatial Maths
Toolbox для Python, положенного в основу пакетов Robotics
Toolbox и Machine Vision Toolbox, которые мы будем использовать в последующих главах.

134 Глава 2 · Представление местоположения и ориентации
пользовательские приложения

Рис. 2.26. Стек открытого программного
обеспечения. Компоненты, выделенные
оранжевым, являются темой данной
главы и используются компонентами,
выделенными бирюзовым цветом,
которые рассматриваются в остальной
части книги. Компоненты, выделенные
серым, – это стандартные пакеты
экосистемы Python

Spatial Maths Toolbox делится на два уровня. Доступ к базово­
му уровню осуществляется инструкцией
>>> from spatialmath.base import *

импортирующей функции, представленные в этой главе. Этот
модуль содержит богатый набор функций для работы с массивами NumPy, таких как transl2, rotx и trexp, позволяющих
работать с двухмерными и трехмерными мат­ри­ца­ми вращения и однородных преобразований. Он реализует множество
функций кватернионов, представленных четырехэлементным
одномерным массивом NumPy. Он предоставляет богатый набор графических функций, таких как trplot и plot_point, а также
множество вспомогательных функций. Многие из этих функций имеют те же имена, что и функции в открытом пакете Spatial Math Toolbox для MATLAB®.
Доступ к верхнему уровню осуществляется инструкцией
>>> from spatialmath import *

импортирующей богатый набор классов, часть из которых мы
уже использовали, например UnitQuaternion, Twist2, Twist3 и Line3.
Также верхний уровень определяет классы, представляющие
мат­ри­цы вращения и мат­ри­цы однородного преобразования
с соответствующими именами SO2 и SE2 для двух измерений
и SO3 и SE3 для трех измерений.
Продемонстрируем разницу на простом примере:
>>> R = rotx(0.3) # создать мат­ри­цу SO(3) как массив NumPy
array([[
1,
0,
0],
[
0, 0.9553, -0.2955],
[
0, 0.2955, 0.9553]])
>>> type(R)
numpy.ndarray
>>> R = SO3.Rx(0.3) # создать объект SO3
1
0
0

Использование Toolbox 135
0
0

0.9553
0.2955

-0.2955
0.9553

>>> type(R)
spatialmath.pose3d.SO3

Визуально объект SO3 отображается несколько иначе, чем
обычная мат­ри­ца 3×3 NumPy.
Экземпляры этих классов инкапсулируют массив NumPy, который хранит фактическую мат­ри­цу вращения или однородного преобразования. Получить доступ к этой инкапсулированной мат­ри­це можно с помощью атрибута array ◄:

Все объекты
прост­ранственной
математики имеют >>> R.A
этот атрибут. array([[

[
[

1,
0,
0,

0,
0],
0.9553, -0.2955],
0.2955, 0.9553]])

Существует множество методов, действующих подобно конст­
Это методы класса,
и в Toolbox их име- рукторам, которые создают объекты SO3, например ◄
на принято писать
с заглавной буквы. >>> R = SO3(rotx(0.3));
# мат­ри­ца преобразования SO(3)
>>> R = SO3.Rz(0.3);
# поворот вокруг оси z
>>> R = SO3.RPY(10, 20, 30, unit="deg"); # из углов
# крена-тангажа-рыскания
>>> R = SO3.AngleAxis(0.3, (1, 0, 0)); # из угла и осей
>>> R = SO3.EulerVec((0.3, 0, 0));
# из вектора Эйлера

а также методы преобразования
>>> R.rpy();
# преобразование в углы крена-тангажа-рыскания
>>> R.eul();
# преобразование в углы Эйлера
>>> R.printline(); # компактный вывод в одну строку
rpy/zyx = 10°, 20°, 30°

Классы полиморфны, то есть имеют множество методов
с одинаковыми именами, которые выполняют похожие функции. Например:
>>>
>>>
>>>
>>>

R
T
S
q

=
=
=
=

SO3.RPY(10, 20, 30, unit="deg");
SE3.RPY(10, 20, 30, unit="deg");
Twist3.RPY(10, 20, 30, unit="deg");
UnitQuaternion.RPY(10, 20, 30, unit="deg");

#
#
#
#
#

мат­ри­ца SO(3)
мат­ри­ца SE(3)
кручение в 3D
единичный
кватернион

все они представляют поворот, описываемый в терминах углов
крена-тангажа ZYX.
Для иллюстрации использования этих классов снова рас­
смотрим пример из конца раздела 2.2.2.1, который теперь приобретает следующий вид:
>>> TA = SE2(1, 2) * SE2(30, unit="deg");
>>> type(TA)
spatialmath.pose2d.SE2

136 Глава 2 · Представление местоположения и ориентации

и значение инкапсулированной мат­ри­цы SE(2):
>>> TA
0.866
0.5
0

-0.5
0.866
0

1
2
1

В некоторых средах элементы могут иметь цветовую кодировку: подмат­ри­ца вращения – красного цвета, вектор перемещения – синего, остальные элементы – серого. Более кратко это
можно записать так:
>>> TA = SE2(1, 2, 30, unit="deg");

Получить доступ к инкапсулированному массиву SE(2) можно с помощью атрибута .A, как уже упоминалось выше. Аналогично можно получить доступ к компонентам, описывающим
поворот и перемещение
>>> TA.R
array([[
[
>>> TA.t
array([

0.866,
0.5,
1,

-0.5],
0.866]])
2])

в виде массивов NumPy. При желании можно нарисовать систему координат, представленную этим положением:
>>> TA.plot(frame="A", color="b");

или представить его в компактной однострочной форме:
>>> TA.printline()
t = 1, 2; 30°

Вернемся к предыдущему примеру и попробуем применить
к вектору обратное преобразование:
>>> P = [3, 2];
>>> TA.inv() * P
array([[ 1.732],
[
-1]])

Кроме того, класс реализует перегруженную версию оператора * и поддерживает преобразование векторов между евклидовой и однородной формами.
Вот некоторые преимущества классов перед функциями:
улучшают читаемость кода и могут использоваться практически так же, как массивы NumPy;
„„ обеспечивают безопасность типов. NumPy позволяет складывать мат­ри­цы SO(3), даже притом что эта операция не
„„

Подведение итогов 137

определена. Сложение двух объектов SO3 дает в результате
массив NumPy, а не объект SO3;
„„ полиморфны, что позволяет легко переключаться между
использованием, например, матриц вращения и кватернионов или переносить решение из двухмерного в трехмерное пространство простой заменой конструктора объекта;
„„ все объекты одного типа можно объединять с помощью
перегруженного оператора *;
„„ все классы наследуют класс list, то есть они могут хранить
несколько значений и поддерживать такие операции, как
обращение по индексу, append и len, что очень удобно для
представления набора взаимосвязанных положений или
ориентаций, скажем положений всех систем координат
в многозвенном манипуляторе или положений вдоль траектории. Например:

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

>>> R = SO3.Rx(np.linspace(0, 1, 5));
>>> len(R)
5
>>> R[3]
1
0
0
0
0.7317 -0.6816
0
0.6816 0.7317

Это экземпляр SO3, содержащий пять матриц вращения вокруг оси x на углы от 0 до 1 рад за 5 шагов. Класс обрабатывает перемещение при объединении с другими объектами
того же типа или в случае векторного преобразования:
>>> R * [1, 2, 3]
array([[
1,
[
2,
[
3,

1,
1.196,
3.402,

1,
1,
0.3169, -0.5815,
3.592,
3.558,

1],
-1.444],
3.304]])

где результат имеет столбцы, которые являются вектором
(1, 2, 3)T, повернутым на последовательные элементы в R.
В оставшейся части этой книги мы будем использовать исключительно классы. ◄

2.6

Подведение итогов

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

138 Глава 2 · Представление местоположения и ориентации

ли, как такие движения можно комбинировать и разбивать на
составляющие, выработали некоторые алгебраические правила
для управления положениями и показали, как наборы взаимо­
связанных положений можно представить в виде графа. Важно
помнить, что композиция некоммутативна – порядок применения относительных положений имеет значение.
Для количественного выражения положения объекта к нему
привязывается двух- или трехмерная система координат. Относительное положение – это расстояние между началами координатных систем и относительные углы между их осями. Точки
внутри объекта представлены постоянными координатными
векторами относительно начала системы координат.
Мы обсудили различные математические объекты, позволяющие наглядно представлять положение. Использовали ортонормированные мат­ри­цы вращения для представления ориентации в двух- и трехмерном пространствах и показали, как они
могут поворачивать координатный вектор точки, преобразуя
его из одной системы координат в другую. Их расширенный вариант, мат­ри­цы однородного преобразования, можно использовать для представления как ориентации, так и перемещения,
и мы показали, как они могут поворачивать и перемещать точку, выраженную в однородных координатах, из одной системы в другую. Поворот в трех измерениях имеет свои тонкости
и сложности, поэтому мы рассмотрели различные способы параметризации, такие как углы Эйлера ZYZ, углы крена-тангажарыскания и единичные кватернионы. Используя теорию групп
Ли, мы показали, что мат­ри­цы вращения из группы SO(2) или
SO(3) являются результатом возведения в степень кососиммет­
ричных порождающих матриц. Точно так же мат­ри­цы однородного преобразования из группы SE(2) или SE(3) являются
результатом возведения в степень дополненных кососиммет­
ричных порождающих матриц. Мы также ввели понятие кручения как краткий способ описания относительного положения
через вращение вокруг оси винта – понятие, которое пришло
к нам из теории винтов, и эти кручения являются уникальными
элементами порождающих матриц.
Простое графическое представление ключевых понятий показано на рис. 2.8. Из этой главы можно извлечь два важных
урока. Во-первых, существует много математических объектов,
которые можно использовать для представления положения,
и они сведены в табл. 2.4. Нет правильного или неправильного
способа – у каждого есть сильные и слабые стороны, и мы обычно выбираем представление, подходящее для рассматриваемой
задачи. Иногда требуется векторное представление, например
для интерполяции, и в этом случае может подойти представление (x, y, θ) или (x, y, z, Γ), где Γ – последовательность из трех
углов, но такие представления сложно комбинировать. Иног­

Подведение итогов 139

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

Рис. 2.27. Физические системы координат широко используются для решения задач
робототехники. Подробнее о создании собственных систем координат рассказывается на
сайте https://petercorke.com/resources/3dframe. Системы координат, которые можно
распечатать на обычном или 3D-принтере и сложить, как показано здесь
(изображение Дориана Цая)

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

2.6.1

Дополнительное чтение

Материал этой главы основан на гибриде математического

140 Глава 2 · Представление местоположения и ориентации

и графического подходов, охватывающих случаи 2D и 3D с помощью абстрактных представлений и операторов, которые постепенно обретают более осязаемые очертания. Стандартные
учебники по робототехнике, такие как Kelly (2013), Siciliano et
al. (2009), Spong et al. (2006), Craig (2005) и Paul (1981), вводят
однородные мат­ри­цы преобразования для трехмерного случая,
но различаются в своих подходах. В этих книгах также хорошо
обсуждаются другие представления, такие как угловой вектор
и трехугольное представление. В учебнике Spong et al. (2006,
раздел 2.5.1) хорошо обсуждают сингулярности. В Siegwart et al.
(2011) детально описывается случай 2D в контексте мобильной
робототехники.
В книге Lynch, Park (2017) рассматриваются стандартные мат­
ричные подходы, но также вводятся кручения и винты. В Solà
et al. (2018) описываются основы теории групп Ли для робототехники. Алгебраическая топология – сложная тема, но в Selig
(2005) предлагается доступное объяснение. В Grassia (1998) дается введение в экспоненциальное отображение для поворотов.
Кватернионы обсуждаются в Kelly (2013) и кратко в Siciliano
et al. (2009). Книга Kuipers (1999) представляет собой очень доступное и исчерпывающее введение в кватернионы. Интерполяция кватернионов широко используется в компьютерной
графике и анимации, и классическая статья Shoemake (1985)
является очень популярным и легко читаемым введением
в эту тему. В книге Solà (2017) подробно рассматриваются основные концепции кватернионов, включая кватернионы JPL,
а в книге Kenwright (2012) описываются основы кватернионов и дуальных кватернионов. Дуальные кватернионы и библиотека для работы с ними в Python представлены в работах
Adorno, Marhinho (2021). Первой публикацией о кватернионах
для робототехники, вероятно, является статья Taylor (1979),
за которой последовала работа Funda et al. (1990). В Terzakis
et al. (2018) описываются и сравниваются несколько распространенных представлений поворотов, а в Huynh (2009) вводятся и сравниваются несколько показателей расстояния для
3D-поворотов.
В учебниках и научных статьях вы встретите множество различных обозначений поворотов и преобразований. В этой книге запись ATB используется для обозначения движения твердого тела SE(3), задающего положение системы {B} относительно
системы {A}. Часто встречается альтернативное обозначение
TBA или даже ABT. Для обозначения точек в этой книге используется запись ApB, соответствующая вектору от начала системы
координат {A} до точки B, тогда как другие авторы используют
pBA или даже CpBA для обозначения вектора от начала системы
координат {A} до точки B, но относительно системы коорди-

Подведение итогов 141

нат {C}. Кручения могут быть записаны либо как (υ, ω), как
в этой книге, либо как (ω, υ).

Инструменты и ресурсы
В этой главе представлен набор инструментов на языке Python,
предназначенных для создания и манипулирования такими
объектами, как мат­ри­цы вращения, мат­ри­цы однородного преобразования, кватернионы и кручения. Существует множество
других пакетов, реализующих аналогичные возможности, но
преследующих иные цели. Manif – это библиотека теории Ли,
написанная на C++, для которой имеется промежуточная биб­
лиотека для использования в Python (https://artivis.github.io/
manif/python). Sophus – реализация групп Ли на C++ для двухи трехмерных геометрических задач (https://github.com/strasdat/Sophus). Библиотека tf2, являющаяся частью экосистемы
ROS, поддерживает возможность одновременного использования нескольких систем координат, сохраняя их в древовидной
структуре, похожей на граф положений, который был представлен в этой главе. tf2 может манипулировать векторами, кватернионами и однородными преобразованиями, а также читать
и записывать объекты этих типов как сообщения геометрии
ROS (http://wiki.ros.org/tf2).

Исторические обзоры
Гамильтон и его сторонники, включая Питера Тейта, энергично защищали первенство Гамильтона в изобретении кватернионов и продвигали кватернионы как альтернативу векторам, которые тогда только начинали понимать и использовать.
В настоящее время векторы хорошо знакомы нам, но в ту пору
Джозайя Гиббс только-только начал разрабатывать их. В статье
Альтманна (Altmann, 1989) приводится интересное описание
этой борьбы идей.
Родригес разработал названную его именем формулу
в 1840 г., хотя Гаусс открыл ее в 1819 г., но, как обычно, не опуб­
ликовал. Его труд был опубликован лишь в 1900 г. В статье Pujol
(2012) дается общий обзор истории развития математических
идей, связанных с представлением поворотов. Кватернионы
даже были вплетены в художественную литературу (Pynchon,
2006).

2.6.2
1.

Упражнения

Создайте 2D-мат­ри­цу вращения. Визуализируйте вращение с помощью trplot2. Используйте мат­ри­цу для преобразования вектора. Инвертируйте ее и умножьте на исходную

142 Глава 2 · Представление местоположения и ориентации

2.
3.

4.
5.

6.
7.

8.
9.
10.

11.

12.

мат­ри­цу; что получилось? Измените порядок умножения
на обратный; что получилось? Что такое определитель мат­
ри­цы и обратная мат­ри­ца?
Создайте модель системы координат, как показано на
рис. 2.27, и воспроизведите трехмерные повороты, изображенные на рис. 2.15 и 2.16.
Создайте 3D-мат­ри­цу вращения. Визуализируйте вращение с помощью trplot или tranimate. Используйте мат­ри­цу
для преобразования вектора. Инвертируйте ее и умножьте
на исходную мат­ри­цу; что получилось? Измените порядок
умножения на обратный; что получилось? Что такое определитель мат­ри­цы и обратная мат­ри­ца?
Изучите набор параметров функции trplot.
Анимируйте вращающийся куб.
a. 	Напишите функцию для построения ребер куба с цент­
ром в начале координат.
b. 	Измените функцию, чтобы она принимала аргумент, являющийся однородным преобразованием, которое применяется к вершинам куба перед построением графика.
c. Анимируйте вращение вокруг оси x.
d. Анимируйте вращение вокруг всех осей.
Создайте вектор-кватернионный класс для описания положения, который поддерживает композицию, обратное
и точечное преобразования.
Вычислите матричную экспоненту, используя степенной
ряд. Сколько членов ряда требуется, чтобы результат соответствовал стандартной точности 64-битного представления чисел с плавающей точкой?
Сгенерируйте последовательность графиков, показанную
на рис. 2.16.
Напишите функцию na2r, аналогичную функции oa2r, которая создает мат­ри­цу вращения из векторов нормали и приближения.
Для трехмерного поворота вокруг вектора [2, 3, 4] на 0.5 рад
вычислите мат­ри­цу вращения SO(3), используя: матричные экспоненциальные функции scipy.linalg, expm и trexp,
формулу поворота Родригеса (напишите для нее код самостоятельно) и функцию Toolbox angvec2tr. Вычислите эквивалентный единичный кватернион.
Создайте две разные мат­ри­цы вращения в 2D или 3D,
представляющие системы {A} и {B}. Определите мат­ри­цы
вращения ARB и BRA. Выразите их как ось вращения и угол
и сравните результаты. Выразите их как кручение.
Создайте мат­ри­цу однородного преобразования в 2D или
3D. Визуализируйте смещение твердого тела с помощью
tranimate. Используйте мат­ри­цу для преобразования векто-

Подведение итогов 143

13.

14.
15.
16.
17.
18.

19.
20.

21.

ра. Инвертируйте ее и умножьте на исходную мат­ри­цу; что
получится в результате? Измените порядок умножения на
обратный; что получилось?
Создайте три символьные переменные для представления
углов крена, тангажа и рыскания, а затем используйте их
для вычисления мат­ри­цы вращения с помощью rpy2r. Вы
можете прибегнуть к символическому упрощению результата. Используйте ее, чтобы преобразовать единичный
вектор в z-направлении. Глядя на элементы мат­ри­цы вращения, разработайте алгоритм для определения углов крена, тангажа и рыскания. Совет: сначала определите угол
тангажа.
Поэкспериментируйте с приложением tripleangle в Toolbox. Исследуйте движения по крену, тангажу и рысканию
относительно номинального положения и сингулярностей.
Используя правило композиции матриц SE(3) из раздела 2.3.2.1, покажите, что TT −1 = I.
Совпадает ли обратная и транспонированная мат­ри­цы однородного преобразования?
В разделе 2.2.2.2 мы поворачивали систему координат вокруг произвольной точки. Выведите выражение для вычисления TC, которое было дано в готовом виде.
Изучите влияние отрицательных углов крена, тангажа или
рыскания. Дает ли преобразование из углов RPY к мат­ри­це
вращения, а затем обратно к углам RPY результат, отличный от начального значения, как и для углов Эйлера?
Докажите, что e xe y ≠ e x+y в случае с мат­ри­ца­ми. Совет: разложите первые несколько членов степенного ряда.
Ось z камеры параллельна вектору [0, 1, 0] в мировой системе координат, а ось y параллельна вектору [0, 0, −1]. Как выглядит позиция камеры по отношению к мировой системе
координат, выраженная в виде мат­ри­цы вращения и единичного кватерниона?
Выберите случайную мат­ри­цу SE(3) и нарисуйте диаграмму с ее системой координат. Вычислите и отобразите ось
винта, который поворачивает систему координат относительно выбранной системы. Совет: преобразуйте выбранную систему координат в кручение.

Глава

3

	Время и движение

ремя существует только для того, чтобы события не проис­
« Входили
одновременно.
– Альберт Эйнштейн

Предыдущая глава была посвящена приемам представления положений объектов в двух- и трехмерном пространствах. В этой
главе мы распространим те же концепции на положения, изменяющиеся во времени. В разделе 3.1 будут представлены производные изменяющихся во времени позиции, ориентации
и положения и показана их связь с понятиями классической механики, такими как скорость и угловая скорость. Мы также рассмотрим дискретные по времени аппроксимации производных,
которые можно использовать в реализации компьютерных алгоритмов, таких как инерциальная навигация. В разделе 3.2 дается краткое введение в динамику, движение объектов под действием сил и моментов и обсуждается важное различие между
инерциальными и неинерциальными системами отсчета.
В разделе 3.3 рассказывается, как создать временную последовательность положений – траекторию перехода из начального положения в конечное. Это может быть траектория движения
рабочего органа робота или полета БПЛА. В разделе 3.4 многие
из этих тем будут рассматриваться в контексте их применения
в инерциальной навигации. Мы рассмотрим три распространенных типа инерциальных датчиков и научимся использовать
их измерения для обновления оценки положения движущегося
объекта, например робота.

3.1

Изменение положения во времени

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

chap3.ipynb

https://go.sn.pub/
f0pWWU

Изменение положения во времени 145

3.1.1

Скорость изменения ориентации

Рассмотрим изменяющуюся во времени ориентацию системы
координат тела {B} относительно фиксированной системы отсчета {A}. Существует множество способов представления ориентации, но наиболее удобны мат­ри­ца вращения и экспоненциальная форма, описанные в разделе 2.3.1.6:
Aω̂

RB(t) = eθ(t)[

A

B(t)]×

∈ SO(3),

где поворот описывается осью вращения Aω̂B(t), определяемой
относительно системы {A} и углом поворота θ(t), а [·]× является
кососимметричной или антисимметричной мат­ри­цей.
Предположим, что в момент времени t ось имеет фиксированное направление и система координат поворачивается вокруг этой оси. Производная по времени равна
Aω̂

ṘB(t) = θ̇(t)[Aω̂B]× eθ(t)[

A

B]×

= θ̇(t)[Aω̂B]× ARB(t)

∈ ℝ3×3

и представляет собой обобщенную мат­ри­цу 3×3, а не мат­ри­цу
вращения. Коротко это можно записать так:
ṘB = [Aω B]× ARB ∈ ℝ3×3,

A

(3.1)

где Aω B = θ˙ Aω̂ B – угловая скорость системы {B} относительно системы {A}. Это векторная величина AωB = (ωx, ωy, ωz), определяющая текущую ось и скорость вращения. Единичный вектор Aω̂B
параллелен оси, вокруг которой вращается система в определенный момент времени, а величина ||AωB|| представляет скорость вращения θ˙ вокруг этой оси.
Предположим, что угловая скорость измеряется в подвижной
системе отсчета {B}, например с помощью гироскопических
датчиков на борту движущегося транспортного средства. Мы
знаем, что
ωB = ARB BωB.

A

В таком случае из тождества [Rυ]× = R[υ]×RT следует, что
ṘB = ARB [BωB]× ∈ ℝ3×3,

A

(3.2)

и мы видим, что мат­ри­ца вращения и кососимметричная мат­
ри­ца поменялись местами.
Производные единичного кватерниона, кватернионный эквивалент уравнений (3.1) и (3.2):

146 Глава 3 · Время и движение

q̊˙B = ½Aω̆B ∘ Aq̊ B = ½ Aq̊ B ∘ Bω̆B ∈ ℍ.

A

(3.3)

Это обычный, а не единичный кватернион, а ω̆ – чистый кватернион, образованный вектором угловой скорости. Эти уравнения реализуются методами dot и dotb класса UnitQuaternion соответственно.

3.1.2

Скорость изменения положения

Производную положения можно определить, выразив положение как мат­ри­цу SE(3)

Взяв производную по времени и подставив уравнение (3.1),
получаем:

(3.4)

Скорость изменения можно описать с точки зрения текущей
ориентации ARB и двух скоростей. Линейная, или поступательная, скорость AυB = AṫB – это скорость начала координат {B} относительно {A}. Угловую скорость AωB мы уже представили. Мы
можем объединить эти два вектора скорости, чтобы создать
вектор пространственной скорости
νB = (AυB, AωB) ∈ ℝ6,

A

являющийся мгновенной скоростью системы координат {B} относительно {A}.
Каждая точка тела имеет одинаковую угловую скорость, но
поступательная скорость точки зависит от ее положения внутри
тела. Обычно начало системы координат {B} помещают в центр
масс тела.
 Соглашение о пространственной скорости
 Не существует четкого соглашения о порядке следования скоростей
в векторе пространственной скорости. В этой книге всегда используется обозначение v = (υ, ω), но в других источниках, включая третье издание этой книги для MATLAB, используется обозначение v =
(ω, υ).

Изменение положения во времени 147

3.1.3

Преобразование пространственных скоростей

На рис. 3.1а показаны две неподвижные и одна подвижная системы координат. Наблюдатель в неподвижной системе {B} наблюдает объект, движущийся с пространственной скоростью Bv
относительно системы {B}. Для наблюдателя в системе {A} пространственная скорость этой системы равна
(3.5)
Из-за ограничений
на использование
символов в именах
в языке Python
A
TB обозначается
как aTb.

где AJB(·) – якобиан, или мат­ри­ца взаимодействия, которая является функцией относительной ориентации двух систем и не
зависит от смещения между ними. Мы можем исследовать конфигурацию на рис. 3.1a численно :
>>> aTb = SE3.Tx(-2) * SE3.Rz(-pi/2) * SE3.Rx(pi/2);

go.sn.pub/bKBuTA

go.sn.pub/rp7jDE
а
б
Рис. 3.1. Пространственную скорость можно выразить относительно системы {A} или
системы {B}: а) обе системы неподвижны, а система объекта движется со скоростью v;
б) обе системы движутся вместе

Если пространственная скорость в системе координат {B}
равна
>>> bV = [1, 2, 3, 4, 5, 6];

то в системе {A} пространственная скорость равна
>>> aJb = aTb.jacob();
>>> aJb.shape
(6, 6)
>>> aV = aJb @ bV
array([ -3, -1, 2, -6, -4, 5])

Здесь метод jacob вернул массив NumPy размером 6×6. Как
видите, скорости были транспонированы, а иногда и инверти-

148 Глава 3 · Время и движение

рованы из-за разной ориентации систем координат. Скорости
поступательного и вращательного движений относительно
оси x в системе {B} были отображены в скорости поступательного и вращательного движений относительно оси y в системе {A},
скорости относительно оси y были отображены в скорости относительно оси z, а скорости относительно оси z – в скорости
относительно оси x.
Далее рассмотрим случай, когда две системы отсчета жестко соединены и движутся вместе, как показано на рис. 3.1б.
В каждой системе отсчета находится наблюдатель, имеющий
возможность оценить свою скорость относительно своей собственной системы координат. Наблюдаемые ими скорости связаны соотношением

которое использует сопряженную мат­ри­цу относительного положения и зависит от смещения между системами координат.
Например, используя относительное положение из предыдущего примера и учитывая, что система {B} просто перемещается, наблюдатель в системе {A} увидит скорость
>>> aV = aTb.Ad() @ [1, 2, 3, 0, 0, 0]
array([ -3, -1, 2, 0, 0, 0])

Это тоже чистое перемещение, но оси перевернуты из-за
разной ориентации двух систем. Если скорость в системе B – это
чисто угловая скорость относительно оси x
>>> aV = aTb.Ad() @ [0, 0, 0, 1, 0, 0]
array([ 0, 0, 2, 0, -1, 0])

то наблюдатель в системе {A} измерит ту же величину угловой
скорости, но транспонированную относительно оси y. Сущест­
вует также поступательная скорость в направлении z, обусловленная тем, что начало координат {A} движется по окружности
вокруг оси x системы {B}. Если объединить эти поступательную
и вращательную скорости в системе {B}, то пространственная
скорость в системе {A} будет равна
>>> aV = aTb.Ad() @ [1, 2, 3, 1, 0, 0]
array([ -3, -1, 4, 0, -1, 0])
 Путаница с кручением
 Во многих источниках пространственная скорость, как она определена выше, называется кручением. Не путайте эту скорость с кручения­
ми, описанными в разделах 2.2.2.4 и 2.3.2.3. Линч и Парк (2017) ис–
пользуют термин velocity twist V (кручение скорости), отличающийся
–
от введенной выше пространственной скорости (черта над буквой V

Изменение положения во времени 149
добавлена для четкого разделения обозначений). Кручение скорости
–
–, Bυ–) и имеет вранеподвижной системы координат {B} равно BV = (Bω
щательную и поступательную составляющие, где Bυ– – это скорость
системы координат воображаемой точки, жестко прикрепленной
к телу и расположенной в начале мировой системы. Кручения скорости тела и системы отсчета тела и мировой системы связаны сопряженной мат­ри­цей, а не уравнением (3.5). В более ранней книге
Murray et al. (1994) кручение скорости (velocity twist) называется
пространственной скоростью (spatial velocity).

3.1.4

Единственными
допустимыми
операторами для
группы SO(n) являются композиция
⊕ и инверсия ⊖,
поэтому результат
вычитания не может
принадлежать
группе. Результатом
является мат­ри­ца
3×3 поэлементных
разностей. Группы
представлены в разделе 2.1.2 и в приложении D.

Вращение, задаваемое в приращениях

В робототехнике существует множество задач, где необходимо
интегрировать угловую скорость, измеряемую датчиками в системе координат тела, для оценки его ориентации. Это ключевая операция в системах инерциальной навигации, о которых
мы поговорим в разделе 3.4. Если предположить, что ось вращения постоянна на интервале времени δt, то изменение ориентации на этом интервале можно выразить в виде «угол-ось»,
где угол равен ||ωδt||. Изменение ориентации равно
RB〈t+δt〉 = RB〈t〉eδt[ω]×

(3.6)

и включает в себя матричную экспоненту, вычисление которой
требует больших затрат.
В разделе 3.1.1 мы ввели Ṙ – обозначение скорости изменения элементов мат­ри­цы вращения с течением времени. Это
можно записать как аппроксимацию производной первого порядка ◄
(3.7)
где R(t) – мат­ри­ца вращения в момент времени t, а δt – бесконечно малый шаг по времени. Рассмотрим объект, системы координат которого {B} на двух последовательных временных шагах
RB〈t〉 и RB〈t+δt〉 связаны
RB〈t+δt〉 = RB〈t〉 B〈t〉RB〈t+δt〉 = RB〈t〉RΔ

небольшим поворотом RΔ ∈ SO(3), выраженным в системе координат тела. Подставляя уравнение (3.2) в (3.7), получаем уравнение
RΔ ≈ δt[Bω]× + 13×3,
Это первые два
члена формулы
вращения Родригеса
(2.27), когда θ = δt ω.

(3.8)

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

150 Глава 3 · Время и движение
>>> rotx(0.001)
array([[
1,
[
0,
[
0,

0,
1,
0.001,

0],
-0.001],
1]])

Перестановка членов в (3.8) позволяет вычислить приближенный вектор угловой скорости
ω≈

⋁× (RTB〈t〉 RB〈t+δt〉 - 13×3)

из двух последовательных матриц вращения, где оператор ⋁×(·)
распаковывает уникальные элементы кососимметричной мат­
ри­цы в вектор. Точное значение можно вычислить по формуле
⋁× log(RTB〈t〉 RB〈t+δt〉).
Возвращаясь к задаче интегрирования вращения, можно
подставить (3.2) в (3.7) и переписать как
RB〈t+δt〉 ≈ RB〈t〉(13×3 + δt[ω]×),

(3.9)

что обходится дешево в плане вычислений и не требует тригонометрических операций, но дает лишь приближенное решение. Мы можем численно исследовать компромиссы в этой аппроксимации:
>>> import time
>>> Rexact = np.eye(3); Rapprox = np.eye(3); # нулевой поворот
>>> w = np.array([1, 0, 0]); # поворот на 1 рад/с вокруг оси x
>>> dt = 0.01;
# шаг во времени
>>> t0 = time.process_time();
>>> for i in range(100):
# точное интегрирование за 100 временных
шагов
...
Rexact = Rexact @ trexp(skew(w*dt)); # обновить путем композиции
>>> print(time.process_time() - t0)
0.008975
>>> t0 = time.process_time();
>>> for i in range(100): # приближенное интегрирование за 100 временных
шагов
...
Rapprox += Rapprox @ skew(w*dt); # обновить путем сложения
>>> print(time.process_time() - t0)
0.000851

Приближенное решение вычисляется в десять раз быстрее
точного,
но повторение операций негруппового сложения
приведет к неправильной мат­ри­це вращения. В этом легко убедиться, вычислив определитель и разность с правильным значением +1:
>>> np.linalg.det(Rapprox) - 1
0.01

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

Изменение положения во времени 151

Это довольно много. Даже точное решение дает неправильную мат­ри­цу вращения.
>>> np.linalg.det(Rexact) - 1
-2.89e-15

хотя в этом случае ошибка очень мала и вызвана конечной точностью компьютерной арифметики, как обсуждалось в разделе 2.4.6.
В обоих случаях можно нормализовать мат­ри­цы вращения
и проверить общий результат, представляющий поворот вокруг
оси x на 1 радиан:
>>>
(1,
>>>
(1,

tr2angvec(trnorm(Rexact))
array([
1,
0,
tr2angvec(trnorm(Rapprox))
array([
1,
0,

0]))
0]))

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

которая в сочетании с уравнением (3.3) дает приближение
(3.10)
где ω̆ – чистый кватернион. Это еще более простое решение
в вычислительном отношении, чем подход с мат­ри­цей вращения, и хотя результат не является полноценным единичным кватернионом, его легко нормализовать, как обсуждалось
в разделе 2.4.6 7.
 Сложение не является групповой операцией
для матриц вращения
 В этом разделе мы обновляли мат­ри­цы вращения, используя операцию сложения, но сложение не является групповой операцией
для SO(3). Получившаяся мат­ри­ца не будет принадлежать SO(3) –
ее определитель не будет равен единице, а столбцы не будут ортогональными единичными векторами. Аналогично мы обновляли
единичные кватернионы, используя операцию сложения, которая
не является групповой операцией для S3. Результат не будет собст­
венным единичным кватернионом, принадлежащим S3, – его величина не будет равна единице.

152 Глава 3 · Время и движение
 Однако если добавляемые значения малы, то эта проблема незначительна и может быть ослаблена нормализацией, как обсуждалось
в разделе 2.4.6. Мы можем гарантировать малость этих значений,
обеспечив малость δt, что подразумевает высокую частоту дискретизации. Для инерциальных навигационных систем, работающих
на низкопроизводительном вычислительном оборудовании, су­
щест­вует компромисс между частотой дискретизации и точностью
при использовании приближенных методов обновления.

3.1.5

Движение твердого тела, задаваемое
в приращениях

Рассмотрим два положения ξ1 и ξ2, которые бесконечно мало
различаются и связаны соотношением
ξ2 = ξ1 ⊕ ξ Δ,

где ξ Δ = ⊖ξ1 ⊕ ξ2. В форме мат­ри­цы однородного преобразования SE(3) это записывается так:

где t Δ ∈ ℝ3 – приращение смещения, а RΔ ∈ SO(3) – мат­ри­ца приращения вращения, которая, согласно (3.8), будет кососиммет­
ричной с тремя уникальными элементами r Δ = ⋁×(RΔ − I3×3). Следовательно, пошаговое движение твердого тела можно описать
всего шестью параметрами:
Δ(ξ1, ξ2) ↦ δ = (t Δ, r Δ) ∈ ℝ6,

(3.11)

Δ-1(δ) ↦ ξ Δ,

(3.12)

которое называют пространственным смещением. Тело с постоянной пространственной скоростью ν в течение δt секунд испытывает пространственное смещение δ = δt ν. Это аппроксимация более затратной операции ⋁(log TΔ).
Обратный оператор
если представить ξ в форме мат­ри­цы SE(3), можно выразить как

что является аппроксимацией более затратной операции e[δ].
Оператор пространственного смещения и его инверсия реа­
лизуются в пакете Toolbox функциями tr2delta и delta2tr со-

Такое представление может пригодиться в процедурах
оптимизации, цель
которых – минимизировать ошибку
между двумя
положениями: мы
можем выбрать
функцию стоимости
e = ||Δ(ξ1, ξ2)||, которая равна нулю, когда ξ1 ≡ ξ2. Это очень
приблизительное
равенство, когда положения существенно различаются, но
оно становится все
точнее при ξ1 → ξ2.

Ускорение тел и системы отсчета 153
Отступление 3.1. Сэр Исаак Ньютон
Ньютон (1642–1727) – английский математик и алхимик. Он был лукасовским профессором математики
в Кембридже, магистром Королевского монетного
двора и 13-м президентом Королевского общества.
В число его главных научных достижений входят три
закона движения, математический аппарат гравитационного притяжения, теория движения небесных
тел и теория света и цвета (Отступление 10.1), а также
создание первого телескопа-рефлектора.
Многие из этих результатов были опубликованы
в 1687 г. в его большом трехтомном труде The Phi­
losophiae Naturalis Principia Mathematica («Математические начала натуральной философии»). В 1704 г.
он опубликовал «Оптику», посвященную изучению
природы света и цвета и явлениям дифракции. В его
честь названа единица силы в системе СИ – ньютон. Похоронен в Вестминстерском аббатстве в Лондоне.

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

3.2

Ускорение тел и системы отсчета

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

3.2.1

Динамика движущихся тел

Второй закон Ньютона в инерциальной системе {0} описывает
ускорение частицы с положением x и массой m
m0ẋ˙B = 0fB,

(3.13)

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

154 Глава 3 · Время и движение

JB Bω̇B + BωB × (BJB BωB) = BτB

B

(3.14)

с приложенным крутящим моментом или моментом BτB и положительно определенным тензором инерции вращения BJB ∈ ℝ3×3.
Ненулевое угловое ускорение подразумевает, что угловая скорость, ось и угол поворота изменяются со временем. ►
Мы будем моделировать вращательное движение объекта,
перемещающегося в пространстве. Тензор инерции определяется как мат­ри­ца
>>> J = np.array([[ 2, -1, 0],
...
[-1, 4, 0],
...
[ 0, 0, 3]]);

которая положительно определена ► и имеет ненулевые недиагональные элементы, которые вызывают поворачивание объекта. Начальные условия для ориентации и угловой скорости:
>>> orientation = UnitQuaternion(); # единичный кватернион
>>> w = 0.2 * np.array([1, 2, 2]);

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

В отсутствие момента силы угловая
скорость тела не
обязательно постоянна, например
угловая скорость
спутника, кувыркающегося в космосе,
непостоянна. Это
существенно отличается от случая
линейной скорости,
когда в отсутствие
силы скорость остается постоянной.
При вращательном
движении в инерциальной системе
отсчета постоянным
является момент
импульса h = Jω.
Недиагональные
элементы положительно определенной мат­ри­цы могут
быть отрицательными, положительными должны быть
только собственные
значения.

>>> dt = 0.05; # временной шаг
>>> def update():
...
global orientation, w
...
for t in np.arange(0, 10, dt):
Отступление 3.2. Тензор инерции вращения
Вращательная инерция тела, движущегося в трех измерениях, описывается тензором второго ранга, элементы которого зависят от выбора системы координат.
Тензор инерции можно записать и использовать как мат­ри­цу 3×3
,
симметричную и положительно определенную, то есть имеющую положительные
собственные значения. Собственные значения также удовлетворяют неравенству
треугольника: сумма любых двух собственных значений всегда больше или равна
другому собственному значению.
Диагональные элементы мат­ри­цы J – это положительные моменты инерции,
а недиагональные элементы – произведения инерции. Только шесть из этих девяти элементов являются уникальными: три момента и три произведения инерции.
Все произведения инерции равны нулю, если распределение массы тела симмет­
рично относительно системы координат.

Ускорение тел и системы отсчета 155
...
wd = -np.linalg.inv(J) @ (np.cross(w, J @ w)) # (3.14)
...
w += wd * dt
...
orientation *= UnitQuaternion.EulerVec(w * dt)
...
yield orientation.R
>>> tranimate(update())

Метод EulerVec действует как конструктор и создает новый
UnitQuaternion из вектора Эйлера с тремя элементами. Функция
tranimate принимает последовательность значений из генератора и анимирует графическую систему координат.

3.2.2

Преобразование сил и моментов

Поступательная сила f = (fx, f y, fz) и крутящий момент (или прос­
то момент) m = (mx, my, mz), приложенные к телу, можно объединить в 6-мерный вектор, который называется динамическим
винтом, w = (fx, f y, fz, mx, my, mz) ∈ ℝ6.
Динамический винт Bw определяется относительно системы
координат {B} и применяется к ее началу. Для системы координат {A}, связанной с тем же телом, динамический винт Aw
эквивалентен Bw, если вызывает то же движение тела при приложении к началу системы координат {A}. Динамические винты
связаны соотношением
(3.15)
которое равноценно преобразованию пространственной скорости (3.5), но использует транспонирование сопряженной мат­ри­
цы обратного относительного положения.
Продолжая пример на рис. 3.1б, определим динамический
винт относительно системы координат {B}, который прикладывает силу вдоль каждой оси:
>>> bW = [1, 2, 3, 0, 0, 0];

Эквивалентный динамический винт в системе {A}
>>> aW = aTb.inv().Ad().T @ bW
array([ -3, -1, 2, 0, 4, 2])

определяет те же компоненты силы, приложенной в системе {B},
но смещенные и инвертированные, что отражает различную
ориентацию систем координат. Силы, приложенные к началу
системы координат {B}, также будут создавать момент силы на
теле, поэтому эквивалентный момент силы в {A} должен включать: крутящие моменты 4 и 2 Н·м вокруг оси y и z соответствен-

156 Глава 3 · Время и движение

но. Обратите внимание, что момент относительно оси x в системе {A} отсутствует.
Соглашение о динамических винтах
Не существует четкого соглашения о порядке перечисления сил
и моментов в векторе динамического винта. В этой книге всегда
используется w = ( f, m), но в других источниках, включая третье издание данной книги для MATLAB, используется w = (m, f ).

3.2.3

Инерциальная система координат

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

« система координат, которая не ускоряется и не вращается.

Рассмотрим частицу P, находящуюся в состоянии покоя относительно неподвижной системы координат {0}. Система {B}
движется с постоянной скоростью 0υB относительно системы
{0}. С точки зрения системы {B}, частица движется с постоянной
скоростью, фактически BυP = -0υB. Частица не ускоряется и подчиняется первому закону Ньютона:

отсутствие приложенной силы частица движется с посто­
« вянной
скоростью,

и, следовательно, система {B} является инерциальной системой
координат.
Теперь предположим, что система {B} ускоряется с постоянным ускорением 0aB относительно {0}. С точки зрения системы {B} частица ускоряется без приложения силы, фактически
B
aP = -0aB. Это нарушает первый закон Ньютона, и наблюдателю в системе {B} пришлось бы привлечь некую неосязаемую
силу, чтобы объяснить наблюдаемое. Мы называем такую силу
фиктивной, кажущейся, инерциальной или силой Даламбера
(d’Alembert) – они существуют только в ускоряющейся, или неинерциальной, системе координат. То есть ускоряющаяся система {B} не является инерциальной.
Гравитацию можно считать неосязаемой силой, поскольку
она заставляет объекты ускоряться относительно системы отсчета, которая не ускоряется. Однако в ньютоновской механике гравитация считается реальной силой, действующей на тело
mg, под влиянием которой свободный объект будет ускоряться
относительно инерциальной системы координат. ►
Примером неинерциальной системы координат из повсе­
дневной жизни может служить ускоряющийся автомобиль или
самолет. Внутри ускоряющегося транспортного средства мы

Принцип эквивалентности Эйнштейна заключается
в следующем: «мы
предполагаем
полную физическую
эквивалентность
гравитационного
поля и соответствующего
ускорения системы
отсчета» – мы не
способны отличить
гравитацию от нахождения в ракете,
движущейся с ускорением 1 g, вдали
от гравитационного
воздействия какоголибо небесного
объекта.

Ускорение тел и системы отсчета 157

наблюдаем фиктивные силы, толкающие объекты, включая нас
самих, так, что это не объясняется первым законом Ньютона.
Во вращающейся системе координат все еще сложнее. Представьте двух человек, стоящих на вращающемся столе и бросающих мяч друг другу. Они наблюдают движение мяча по искривленной траектории, и им также придется привлечь некую
неосязаемую силу, чтобы объяснить наблюдение.
Если система {B} вращается с угловой скоростью ω вокруг
своей точки начала координат, то второй закон Ньютона (3.13)
превращается в

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

– Альберт Эйнштейн,
«Основы общей теории относительности».

Отступление 3.3. Гаспар-Гюстав Кориолис
Кориолис (1792–1843) – французский математик, инженер-механик и ученый. Родился в Париже. В 1816 году стал преподавателем в Политехнической школе, где проводил эксперименты
по трению и гидравлике, а затем стал профессором Школы мостов и дорог (École des Ponts and
Chaussées). Распространил идеи о кинетической
энергии и работе на вращающиеся системы
и в 1835 году написал знаменитую работу «Об
уравнениях относительного движения системы
тел» (Sur les équations du mouvement relatif des
systèmes de corps), где рассматривается перенос
энергии во вращающихся системах, таких как
водяные колеса. В конце XIX века его идеи были использованы метеорологическим сообществом для учета эффектов, вызванных вращением Земли. Похоронен
на кладбище Монпарнас в Париже.

158 Глава 3 · Время и движение

В робототехнике термины «инерциальная система отсчета»
и «мировая система координат» обычно используются взаимозаменяемо и обозначают систему отсчета, закрепленную
в некоторой точке на Земле. Это необходимо, чтобы отличать
системы отсчета, прикрепленные к роботу или транспортному
средству. Поверхность Земли является приближением к инерциальной системе отсчета – эффект вращения Земли выражается в конечном ускорении, составляющем менее 0.03 м/с²,
обусловленном центростремительным ускорением. С точки
зрения наблюдателя, находящегося на Земле, движущееся тело
будет испытывать ускорение Кориолиса. Эти эффекты незначительны, зависят от широты и обычно игнорируются.

3.3

С оздание положения, изменяющегося
во времени

В робототехнике часто требуется сгенерировать движение, осуществляющее плавное перемещение из положения ξ0 в положение ξ1, например рабочего органа робота или дрона. Положение
и ориентация должны плавно меняться со временем. Проще говоря, движение должно осуществляться плавно и вдоль гладкой
траектории. Под плавностью в данном контексте подразумевается, что первые несколько временных производных положения и ориентации непрерывны. Как правило, требуется, чтобы
скорость и ускорение были непрерывными, а иногда также желательно, чтобы непрерывной была производная ускорения.
Первая задача – построить плавный путь из ξ0 в ξ1, который
можно было бы определить некоторой функцией ξ(s), s ∈ [0, 1].
Вторая задача – построить гладкую траекторию, то есть плавное
движение по пути, требующее, чтобы s(t) была гладкой функцией времени.
Для начала обсудим, как сгенерировать гладкую траекторию
в одном измерении. Затем найденное решение мы распространим на многомерный случай, а потом на кусочно-линейные
траектории, обеспечивающие посещение ряда промежуточных
точек без остановки.

3.3.1

Гладкие одномерные траектории

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

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

Создание положения, изменяющегося во времени 159

печивают требуемую гладкость и граничные условия. Обычно
используется полином пятого порядка:
q(t) = At5 + Bt4 + Ct3 + Dt2 + Et + F,

(3.16)

где время t ∈ [0, T]. Первые и вторые производные также являются полиномами:
q̇(t) = 5At4 + 4Bt3 + 3Ct2 + 2Dt + E,

(3.17)

q̇˙(t) = 20At3 + 12Bt2 + 6Ct + 2D,

(3.18)

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

а

t (секунды)

б

t (секунды)

Рис. 3.2. Траектория, определяемая полиномом пятой степени. Сверху вниз: положение, скорость и ускорение
в зависимости от шага времени: а) начальная и конечная скорости равны нулю; б) начальная скорость равна 10,
а конечная – 0. Точки дискретного времени обозначены точками.

Траектория имеет заданные граничные условия для положения (q0, qT), скорости (q̇0, q̇T) и ускорения (q̇˙0, q̇˙T). Записав уравнения с (3.16) по (3.18) для граничных условий t = 0 и t = T, получаем шесть уравнений, которые можно представить в мат­ричной
форме:

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

Матрица квадратная , и если T ≠ 0, то ее можно инвертировать, чтобы получить решение для вектора коэффициентов
(A, B, C, D, E, F).

160 Глава 3 · Время и движение

В Toolbox имеется функция для генерирования траектории,
описанной формулой (3.16):
>>> traj = quintic(0, 1, np.linspace(0, 1, 50));

где аргументами являются начальное и конечное положение
и время, изменяющееся от 0 до 1 за 50 шагов. Функция возвращает объект, содержащий определение траектории и метод ее
построения:
>>> traj.plot();

который создает график, показанный на рис. 3.2а. Данные о положении, скорости и ускорении доступны как traj.q, traj.qd
и traj.qdd – массивы NumPy с 50 элементами. Мы видим, что по
умолчанию начальные и конечные скорость и ускорение равны
нулю. ►
Для начальной и конечной скоростей можно задать ненулевые значения. Например, если передать начальную скорость 10
и конечную скорость 0

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

>>> quintic(0, 1, np.linspace(0, 1, 50), qd0=10, qdf=0);

то в результате будет создана траектория, изображенная на
рис. 3.2б. Этот пример иллюстрирует важную проблему, связанную с полиномами: ненулевая начальная скорость приводит
к тому, что полином превышает конечное значение, в данном
примере достигая значения выше 2 на траектории от 0 до 1.
ускорение
линейное движение
замедление

а

t (секунды)

б

t (секунды)

Рис. 3.3. Траектория, описываемая полиномом пятой степени: а) траектория для линейного сегмента со скоростью
по умолчанию; б) траектория для линейного сегмента с заданными скоростями

Еще одна проблема с полиномами видна на среднем графике
рис. 3.2а. Скорость достигает пика при t = 0.5 с, то есть большую
часть времени скорость значительно меньше максимальной.
Средняя скорость

Создание положения, изменяющегося во времени 161
>>> qd = traj.qd;
>>> qd.mean() / qd.max()
0.5231

составляет всего 52 % от пиковой, а значит, двигатель используется не на полную мощность. У настоящего сочленения роботизированного манипулятора есть четко определенная максимальная скорость, и для минимизации времени движения
желательно, чтобы движения выполнялись со скоростью как
можно ближе к этому максимуму, то есть кривая скорости должна быть более пологой в верхней части.
Хорошо известной альтернативой является трапециевидная
гибридная траектория:
>>> traj = trapezoidal(0, 1, np.linspace(0, 1, 50));
>>> traj.plot();

где аргументы имеют тот же смысл, что и для quintic, а траектория, построенная таким способом, показана на рис. 3.3a. Как
и в quintic, траектории скорости и ускорения хранятся внутри
объекта.
Траектория скорости имеет трапециевидную форму, отсюда
и название trapezoidal, и состоит из трех прямолинейных сегментов. Соответствующие сегменты траектории положения
представляют собой прямую линию (сегмент постоянной скорости) с параболическими переходами. Термин «переход» относится к сегменту траектории, плавно соединяющему прямолинейные сегменты. Этот тип траектории широко используется
в промышленных электроприводах. Он непрерывен по положению и скорости, но не по ускорению.
Функция trapezoidal выбрала скорость линейного сегмента
>>> traj.qd.max()
1.5

но ее можно переопределить, передав дополнительный аргу-

Система имеет мент
одну проектную
степень свободы. >>> traj1_2 = trapezoidal(0, 1, np.linspace(0, 1, 50), V=1.2);
Всего имеется шесть
>>> traj2_0 = trapezoidal(0, 1, np.linspace(0, 1, 50), V=2);
степеней свободы
(время перехода,
Траектории для этих различных случаев наложены на
три параболических
коэффициента и два рис. 3.3б. Как видите, с увеличением скорости линейного участлинейных коэфка его продолжительность уменьшается и в конечном итоге
фициента) и пять
ограничений (общее становится равной нулю. Фактически скорость нельзя выбрать
время, а также на- произвольно – слишком большое или слишком малое значение
чальные и конечные
положение максимальной скорости приведет к получению недопустимой
и скорость). траектории, и функция вернет ошибку. ◄

162 Глава 3 · Время и движение

k (шаги)

3.3.2

Рис. 3.4. Многокоординатное
движение. q0 изменяется от
0 до 1, а q1 – от 2 до –1

Многокоординатные траектории

Большинство полезных роботов имеют несколько осей движения, и мы без особого труда можем распространить гладкую скалярную траекторию на векторный случай. В терминах
конфигурационного пространства (раздел 2.4.9) оси движения
соответствуют измерениям конфигурационного пространства
робота – его степеням свободы. Представим конфигурацию
робота как вектор q ∈ ℝN, где N – число степеней свободы. Конфигурация робота с тремя сочленениями будет иметь координаты сочленений q = (q0, q1, q2). Вектором конфигурации колесного мобильного робота может быть его положение q = (x, y)
или положение и курсовой угол q = (x, y; θ). Для трехмерного
тела, имеющего ориентацию в SO(3), мы будем использовать
углы крена-тангажа-рыскания q = (α, β, γ), а для положения
в SE(3): q = (x, y, z, α, β, γ). ► Во всех этих случаях нам потребуется плавное многомерное движение от начальной конфигурации к конечной.
Рассмотрим 2-координатного робота типа xy, перемещающегося из конфигурации (0, 2) в конфигурацию (1, -1) за 50 шагов
трапециевидного профиля. В Toolbox это реализуется с помощью функции mtraj:
>>> traj = mtraj(trapezoidal, [0, 2], [1, -1], 50);

где traj.q – массив NumPy 50×2, по одной строке на временной
шаг и по одному столбцу на каждую координатную ось. Первый
аргумент – это объект функции, которая генерирует гладкую
скалярную траекторию как функцию времени. В данном случае
trapezoidal, но также можно было бы использовать quintic. Ре-

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

Создание положения, изменяющегося во времени 163

зультатом является объект траектории и 2-координатная траектория
>>> traj.plot();

как показано на рис. 2.4.
Если бы потребовалось создать траекторию для трехмерного
положения, то мы могли бы преобразовать T – экземпляр SE3 –
в 6-мерный вектор с помощью команды, такой как
>>> q = np.array([Tt, T.rpy()])

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

3.3.3

Многосегментные траектории

В робототехнических приложениях часто возникает необходимость плавного перемещения по траектории через одну или
несколько промежуточных или контрольных точек без остановок. Это может быть необходимо для обхода препятствий на
рабочем месте или для выполнения задачи, требующей следования по кусочно-линейной траектории, например при сварке
шва или нанесении слоя герметика в производственном процессе.
Для формализации задачи предположим, что траектория задана M конфигурациями qk, k = 0; ..., M - 1, то есть число сегментов движения равно M - 1. Как и в предыдущем разделе,
qk ∈ ℝN – вектор конфигурации.
Робот начинает движение в точке покоя q0 и завершает в точке покоя qM-1, проходя через промежуточные конфигурации
(или близко к ним) без остановки. Задача имеет избыточные
ограничения, и для достижения непрерывной скорости мы отказываемся от возможности достижения каждой промежуточной конфигурации. Это проще всего понять на примере одномерного случая, показанного на рис. 3.5. Движение состоит из
прямолинейных сегментов с полиномиальными переходами.
Мы могли бы использовать trapezoidal, но решили отдать предпочтение полиномам пятой степени, потому что они позволяют сопоставлять граничные условия по положению, скорости
и ускорению в начальной и конечной точках.
Первый сегмент траектории – ускорение от начальной конфигурации q0 и нулевой скорости, переходящее в линейное перемещение ко второй конфигурации q1. Время перехода задается константой tacc, и за tacc /2 до достижения точки q1 траектория
выполняет полиномиальный переход продолжительностью tacc

164 Глава 3 · Время и движение

на линию от q1 до q2, и затем процесс повторяется. Постоянную
скорость q̇k можно задать для каждого сегмента. Среднее ускорение во время перехода составляет

q

сегмент 0

сегмент 1

сегмент 2

время

Рис. 3.5. Траектория с четырьмя
точками и тремя сегментами
движения. Синим цветом
обозначено движение
с постоянной скоростью,
красным – участки движения
с ускорением

Если известно максимально возможное ускорение для оси, то
можно вычислить минимальное время перехода.
Оси могут иметь двигатели, развивающие разные максимальные скорости, и на конкретном участке движения им, как
правило, предстоит пройти разные расстояния. Первый шаг
при планировании участка – определение оси, которая будет
последней, завершающей этот участок, а скорость остальных
осей уменьшается для координации движения, то есть для достижения всеми осями следующей цели qk одновременно.
Рассмотрим снова двухкоординатный робот типа xy, движущийся по квадратной траектории в форме квадрата. Траекторию можно сгенерировать следующим образом:
>>> via = SO2(30, unit="deg") * np.array([
...
[-1, 1, 1, -1, -1], [1, 1, -1, -1, 1]]);
>>> traj0 = mstraj(via.T, dt=0.2, tacc=0, qdmax=[2, 1]);

где аргументами являются: массив промежуточных точек, каждый столбец – координата точки маршрута; время выборки;
время ускорения; ►и вектор максимальной скорости для каждой оси. ► Функция возвращает объект, содержащий данные
траектории, как было показано в примерах выше. Мы можем
построить график зависимости y от x и увидеть траекторию
движения робота
>>> xplot(traj0.q[:, 0], traj0.q[:, 1], color="red");

как показано красной линией на рис. 3.6а. Если увеличить время ускорения до двух секунд:

Реальным пределом
для оси является
пиковое ускорение,
а не среднее. Пиковое ускорение для
перехода можно
определить из (3.18),
как только станут
известны коэффициенты полинома.

Внутреннее время
округляется до
максимального
вектора, кратного
шагу времени.
Альтернативно, время для преодоления
сегмента можно
указать с помощью
аргумента tsegment.

Создание положения, изменяющегося во времени 165
>>> traj2 = mstraj(via.T, dt=0.2, tacc=2, qdmax=[2, 1]);

то траектория, показанная синим цветом, станет более округ­
лой из-за применения полиномиальных функций перехода.
Более плавная траектория также требует больше времени для
достижения конечной точки.
>>> len(traj0), len(traj2)
(28, 80)

промежуточные
точки

а

б

Время (с)

Рис. 3.6. Многосегментные и многокоординатные траектории: а) конфигурация робота (положение инструмента)
для разных времен ускорения; б) зависимость конфигурации от времени для случая tacc = 2 с с переходами между
сегментами, обозначенными серыми полосами

Переменные конфигурации, как функции времени, показаны на рис. 3.6б, где хорошо видны линейные сегменты и переходы. Эта функция также принимает необязательные аргументы: начальную и конечную скорости, а tacc может быть вектором,
определяющим различные времена ускорения для каждого из
M переходов.
Имейте в виду, что эта функция просто интерполирует конфигурацию, представленную вектором. В данном примере
предполагалось, что вектор представлен в декартовых координатах. Для вращения мы могли бы рассмотреть применение
этой функции к углам Эйлера или углам крена-тангажа-рыскания, однако это не идеальный способ интерполяции ориентации, но об этом мы поговорим в следующем разделе.

3.3.4

Интерполяция ориентации в 3D

В робототехнике часто требуется интерполировать ориентацию, например чтобы рабочий орган робота плавно менялся,
перемещаясь из ξ0 в ξ1 в S3. А для этого нужна некоторая функция
ξ(s) = σ(ξ0, ξ1, s), где s ∈ [0, 1], с граничными условиями σ(ξ0, ξ1, 0) =

166 Глава 3 · Время и движение

ξ0 и σ(ξ0, ξ1, 1) = ξ1, где σ(ξ0, ξ1, s) плавно изменяется, охватывая
промежуточные значения s. Реализация такой функции во многом зависит от конкретного представления ξ.
Типичный и широко используемый подход заключается в использовании представления с тремя параметрами, такого как
углы Эйлера (или крена-тангажа-рыскания), Γ ∈ (S1)3 или экспоненциальные координаты и линейная интерполяция:
σ(Γ0, Γ1, s) = (1 - s)Γ0 + sΓ1, s ∈ [0, 1]

– и преобразования интерполированного вектора обратно в мат­
ри­цу вращения. Для примера определим две ориентации:
>>> R0 = SO3.Rz(-1) * SO3.Ry(-1);
>>> R1 = SO3.Rz(1) * SO3.Ry(1);

найдем эквивалентное представление в форме углов кренатангажа-рыскания ZYX
>>> rpy0 = R0.rpy(); rpy1 = R1.rpy();

и создадим между ними траекторию с 50 равноудаленными шагами:
>>> traj = mtraj(quintic, rpy0, rpy1, 50);

Атрибут traj.q – это массив 50×3 с углами крена-тангажа-рыс­
кания. Для наглядности лучше всего использовать анимацию,
поэтому сначала преобразуем углы обратно в мат­ри­цы SO(3):
>>> pose = SO3.RPY(traj.q);
>>> len(pose)
50

и создадим один объект SO3 с 50 значениями, которые можем
анимировать:
>>> pose.animate();

При значительном изменении ориентации мы видим, что
ось, вокруг которой вращается система координат, изменяет
направление по мере продвижения вдоль траектории. Движение, хотя и плавное, может выглядеть несколько нескоординированным. Также могут возникнуть проблемы, если ξ0 или ξ1
близки к сингулярности в конкретном векторном представлении ориентации.
Интерполяция единичных кватернионов немного сложнее
интерполяции 3-угловых векторов и приводит к повороту вокруг фиксированной оси в пространстве. Используя Toolbox, сначала найдем два эквивалентных единичных кватерниона
>>> q0 = UnitQuaternion(R0); q1 = UnitQuaternion(R1);

Создание положения, изменяющегося во времени 167

и затем интерполируем их в 50 равноудаленных шагов
>>> qtraj = q0.interp(q1, 50);
>>> len(qtraj)
50

где объект q0 – начальная ориентация, а аргументы метода interp определяют конечную ориентацию и количество шагов. Результатом является объект UnitQuaternion, содержащий 50 значений, которые можно анимировать:
>>> qtraj.animate()
Большой круг на
сфере – это линия
пересечения сферы
и плоскости, проходящей через ее
центр. На Земле
большими кругами
являются экватор
и все меридианы.
Корабли и самолеты
обычно следуют по
дугам большого круга, потому что они
являются кратчайшим путем между
двумя точками на
поверхности сферы.

Интерполяция кватернионов осуществляется методом сферической линейной интерполяции (spherical linear interpolation, slerp), в которой единичные кватернионы описывают большой круг на поверхности 4-мерной гиперсферы. Результатом
в трехмерном пространстве является поворот вокруг фиксированной оси в пространстве.
Матрицу вращения нельзя интерполировать линейно
Если положение представлено ортогональной мат­ри­цей вращения

R ∈ SO(3), то мы не сможем использовать линейную интерполяцию
σ(R0, R1, s) = (1 - s)R0 + sR1. Скалярное умножение и сложение не
являются допустимыми операциями для группы матриц SO(3),
а значит, результирующая мат­ри­ца R не является мат­ри­цей SO(3),
так как нарушаются норма столбца и ограничение ортогональности
между столбцами.

3.3.4.1

Направление вращения

Двигаться между двумя точками на окружности можно по часовой стрелке или против – конечный результат будет один и тот
же, но пройденное расстояние может быть разным. То же касается движения по большой окружности. В этом примере мы
анимируем поворот вокруг оси z от угла -2 рад до угла +2 рад.
>>> q0 = UnitQuaternion.Rz(-2); q1 = UnitQuaternion.Rz(2);
>>> q = q0.interp(q1, 50);
>>> q.animate()

В этом случае мы прошли по длинному пути вдоль окружности и переместились на 4 радиана, тогда как могли бы пройти
всего 2π - 4 ≈ 2.28 рад, если бы направились в противоположном
направлении. Узнать кратчайший путь можно так:
>>> q = q0.interp(q1, 50, shortest=True);
>>> q.animate()

и анимация наглядно показывает разницу.

168 Глава 3 · Время и движение

3.3.5

Декартово движение в 3D

Другим распространенным требованием в робототехнике является поиск гладкого пути между двумя положениями ℝ3 × S3
в трехмерном пространстве, что подразумевает изменение как
положения, так и ориентации. В робототехнике это часто называют декартовым движением.
Представим начальное и конечное положения как мат­ри­цы
SE(3):
>>> T0 = SE3.Trans([0.4, 0.2, 0]) * SE3.RPY(0, 0, 3);
>>> T1 = SE3.Trans([-0.4, -0.2, 0.3]) * SE3.RPY(-pi/4, pi/4, -pi/2);

Объект SE3 имеет метод interp, который выполняет интерполяцию вдоль пути между двумя положениями для нормализованного расстояния s ∈ [0, 1], например промежуточное положение между T0 и T1:
>>> T0.interp(T1, 0.5)
0.09754 -0.702
0.702
0.551
-0.7055
0.4512
0
0

0.7055
0.4512
0.5465
0

0
0
0.15
1

где объект T0 – это начальное положение, а аргументами метода
interp являются конечное положение и нормализованное расстояние. Компонент, определяющий перемещение, интерполируется линейно, а поворот – с помощью сферической линейной
интерполяции единичных кватернионов, описанной выше.
Траектория между двумя положениями, включающая 51 шаг,
создается вызовом
>>> Ts = T0.interp(T1, 51);

результатом которого является объект SE3, содержащий 51 значение:
>>> len(Ts)
51

представляющие положение на каждом шаге. И снова самый
простой способ визуализировать результат – анимация
>>> Ts.animate()

показывающая перемещение и поворот системы координат из
положения T0 в положение T1. Значение SE(3) в средней точке
пути равно
>>> Ts[25]
0.09754 -0.702

0.7055

0

Создание положения, изменяющегося во времени 169
0.702
-0.7055
0

0.551
0.4512
0

0.4512
0.5465
0

0
0.15
1

и совпадает с результатом, приведенным выше.
Поступательная часть этой траектории имеет форму:
>>> P = Ts.t;
>>> P.shape
(51, 3)

то есть это массив, строки которого представляют положение
в последовательные моменты времени. Данное движение показано на графике
>>> xplot(P, labels="x y z");

на рис. 3.7 вместе с ориентацией в виде углов крена-тангажарыскания

положение

Углы крена, тангажа и рыскания

>>> rpy = Ts.rpy();
>>> xplot(rpy, labels="roll pitch yaw");

крен
тангаж
рыскание

а

k (шаги)

б

k (шаги)

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

Описывается
в разделе 2.3.1.3.

Как видите, координаты положения меняются линейно, а уг­
лы крена-тангажа-рыскания – нелинейно, и тому есть две причины. Во-первых, углы крена-тангажа-рыскания являются
нелинейным преобразованием линейно изменяющейся ориентации кватерниона. Во-вторых, эта конкретная траектория
проходит очень близко к сингулярности крена-тангажа-рыс­
кания в районе шагов 24 и 25, и признаком этого является
быстрая скорость изменения углов крена-тангажа-рыскания
вокруг этой точки. Система координат в этой точке не вращается быстрее – в этом можно убедиться на анимации, – но параметры вращения изменяются очень быстро, и это является
следствием конкретного представления.

170 Глава 3 · Время и движение

Однако движение имеет разрыв скорости и ускорения в первой и последней точках. Траектория гладкая в пространстве, но
расстояние s вдоль траектории изменяется не гладко. Скорость
в начале траектории скачкообразно увеличивается от нуля до
некоторого конечного значения, а в конце падает до нуля – начальное ускорение и конечное замедление отсутствуют. Чтобы
добиться плавности движения по траектории, для генерирования интерполяционной переменной s можно использовать
скалярные функции quintic и trapezoidal. Во втором аргументе
функции interp можно передать вектор нормализованных расстояний вдоль траектории:
>>> Ts = T0.interp(T1, trapezoidal(0, 1, 50).q);

Траектория не меняется, но теперь система координат ускоряется до постоянной скорости вдоль траектории и замедляется в конце, что дает более плавную траекторию, показанную на
рис. 3.8. В Toolbox для этой цели предусмотрена удобная функция ctraj:
>>> Ts = ctraj(T0, T1, 50);

положение

Углы крена, тангажа и рыскания

которая принимает начальное и конечное положения, а также
количество шагов.

крен
тангаж
рыскание

а

k (шаги)

б

k (шаги)

Рис. 3.8. Декартово движение с трапециевидным профилем расстояния: а) зависимость декартова положения
от времени; б) зависимость углов крена, тангажа и рыскания от времени

3.4

Применение: инерциальная навигация

Инерциальная навигационная система (ИНС) – это автономное
устройство, оценивающее свою скорость, ориентацию и местоположение, измеряя ускорения и угловые скорости и интегрируя их с течением времени. Важно отметить, что такое устрой-

Применение: инерциальная навигация 171

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

ство не принимает сигналов извне, таких как радиосигналы со
спутников. Это делает ее хорошо подходящей для таких применений, как управление подводными лодками, космическими
кораблями и ракетами, не имеющими возможности связаться
с радионавигационными средствами или которые должны быть
устойчивы к радиопомехам. Эти конкретные области применения стимулировали развитие технологии во время холодной войны и космической гонки 1950-х и 1960-х годов. Ранние
устройства инерциальной навигации имели большие размеры
(рис. 3.9a), были чрезвычайно дорогими, а детали их внутреннего устройства считались государственной тайной. Современные ИНС значительно дешевле и меньше (рис. 3.9б). Сенсорные
датчики, показанные на рис. 3.9в, могут стоить всего несколько
долларов и встраиваются в каждый смартфон.
ИНС оценивает свое положение относительно инерциальной системы отсчета, которая обычно обозначается как {0}.
Ось z системы координат обычно направлена вверх или вниз,
а оси x и y образуют локально горизонтальную касательную
плоскость. Два распространенных соглашения предполагают,
что оси x, y и z параллельны направлениям север-восток-вниз
(north-east-down, NED) или восток-север-вверх (east-northup, ENU) соответственно. Система координат {B} прикреплена
к движущемуся транспортному средству или роботу и называется системой координат корпуса, или неподвижной системой
координат.

а

б

в

Рис. 3.9. Инерциальные датчики: а) SPIRE (Space Inertial Reference Equipment) 1953 года имел диаметр 1,5 м и весил
1200 кг; б) современная инерциальная навигационная система LORD Micro-Strain 3DM-GX4-25, содержит трехосный
гироскоп, акселерометр, магнитометр и барометрический высотомер, имеет размеры 36×24×11 мм и весит 16 г
(изображение предоставлено LORD MicroStrain); в) блок инерциальных измерителей 9 Degrees of Freedom IMU
Breakout (LSM9DS1-SEN-13284, выпускается Spark-Fun Electronics), сам модуль имеет размеры всего 3.5×3 мм

3.4.1

Гироскопы

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

172 Глава 3 · Время и движение

3.4.1.1

Как работают гироскопы

Термин «гироскоп» вызывает в памяти образ детской игрушки –
волчка, вращающегося диска в круглой оправе, который может
балансировать на острие оси. Гироскопы – сложные устройства:
при попытке повернуть их в одну сторону они сопротивляются и поворачиваются (прецессируют) в другую сторону. Это неконтролируемое поведение описывается упрощенной версией
уравнения (3.14):
τ = ω × h,

(3.19)

где h – момент импульса гироскопа, вектор, параллельный оси
вращения ротора и имеющий величину ||h|| = Jϖ, где J – момент
инерции ротора, а ϖ – скорость его вращения. Именно векторное произведение в (3.19) заставляет гироскоп двигаться в противоположном направлении.
Если к гироскопу не приложен крутящий момент, его момент
импульса в инерциальной системе координат остается постоянным, то есть ось будет сохранять постоянное направление
в этой системе. Два гироскопа с ортогональными осями образуют устойчивую платформу, сохраняющую постоянную ориента­
цию относительно инерциальной системы координат, фиксированной относительно Вселенной. Этот принцип лежал в основе
многих ранних навигационных систем космических аппаратов,
например показанной на рис. 2.19: карданные подвесы позволяли космическому аппарату вращаться вокруг устойчивой
платформы, а относительная ориентация определялась путем
измерения углов карданного подвеса.
В качестве альтернативы можно закрепить гироскоп на
транспортном средстве в бесплатформенной конфигурации,
как показано на рис. 3.10. Если транспортное средство вращается с угловой скоростью ω, то закрепленный на нем гироскоп
будет прецессировать и создавать ортогональный крутящий момент τ, который можно измерить. Если величина h велика,
то датчик этого типа будет очень чувствительным – даже очень
малая угловая скорость породит легко измеримый крутящий
момент.
За последние несколько десятилетий эта технология вращающихся дисков уступила место датчикам, основанным на
оптических принципах, таким как кольцевой лазерный гироскоп (ring-laser gyroscope, RLG) и волоконно-оптический
гироскоп (fiber-optic gyroscope, FOG). Это высокоточные, но
дорогие и громоздкие датчики. Недорогие датчики, используемые в смартфонах и дронах, основаны на микроэлектромеханических системах (micro-electro-mechanical systems MEMS),
изготовленных на кремниевых кристаллах. Конкретные дета-

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

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

Применение: инерциальная навигация 173

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

Рис. 3.10. Конструкция
бесплатформенного гироскопа.
Угловая скорость ω создает
крутящий момент τ, который можно
измерить как силы, действующие на
подшипники, показанные красным

Гироскопические датчики угловой скорости измеряют вращение вокруг одной оси. Обычно в одну конструкцию объединяются три гироскопа и размещаются так, чтобы их оси
чувствительности были ортогональны. Три выходных сигнала
такого трехосного гироскопа представляют компоненты вектора угловой скорости BωB#, измеренного в системе координат {B},
где обозначение x# представляет значение x, измеренное датчиком.
Интересно отметить, что гироскопические датчики были
изобретены самой природой. Все позвоночные имеют датчики
угловой скорости, являющиеся частью вестибулярного аппарата. В каждом внутреннем ухе есть три полукружных канала – три
органа, заполненных жидкостью, которые измеряют угловую
скорость. Они расположены ортогонально, на манер трехосного
гироскопа, с двумя осями измерения в вертикальной плоскости
и одной по диагонали головы.

3.4.1.2

Оценка ориентации

Если изначально система координат сенсора имела ориентацию ξB, то эволюцию оценочной ориентации можно записать
в дискретной временной форме как
ξˆB〈k+1〉 → ξˆB〈k〉 ⊕ B〈k〉ξB〈k+1〉,

(3.20)

где значок «крышки» используется для обозначения оценки
ориентации, а k ∈ ℕ0 – индекс временного шага. B〈k〉ξB〈k+1〉 – это
приращение поворота за временной шаг, которое можно вычислить по измеренной угловой скорости.

174 Глава 3 · Время и движение

Обычно предполагается, что BωB постоянна в течение интервала времени δt. В терминах матриц вращения SO(3) обновление ориентации равно
R̂ B〈k+1〉 ← R̂ B〈k〉 eδt[ ωB ]×,
B

#

(3.21)

и результат должен периодически нормализовываться, чтобы
исключить накопленную числовую ошибку, как обсуждалось
в разделе 2.4.6.
Мы продемонстрируем эту интеграцию, используя единичные кватернионы и данные моделирования угловой скорости
падающего тела. Функция IMU из Toolbox
>>> from imu_data import IMU
>>> true, _ = IMU()

возвращает объект, описывающий истинное движение тела.
Строки массива true.omega представляют последовательные измерения угловой скорости тела относительно системы отсчета
в соответствующие моменты времени, определяемые элементами вектора true.t. В качестве начальной ориентации мы выбираем нулевой поворот
>>> orientation = UnitQuaternion();

# единичный кватернион

а затем для каждого временного шага обновляем и сохраняем
ориентацию, используя свойства класса UnitQuaternion:
>>> for w in true.omega[:-1]:
...
next = orientation[-1] @ UnitQuaternion.EulerVec(w * true.dt);
Отступление 3.4. Лаборатория приборостроения Массачусетского технологического
института
В лаборатории приборостроения Массачусетского технологического института
(MIT), которой руководил Чарльз Старк Дрейпер, было проведено важное исследование в области технологий инерциальной навигации. В 1953 году возможность
инерциальной навигации для самолетов была продемонстрирована в серии летных испытаний системы SPIRE (Space Inertial Reference Equipment), показанной на
рис. 3.9a. Устройство имело 1.5 м в диаметре и весило 1200 кг. Оно управляло бомбардировщиком B29 в ходе 12-часового перелета из Массачусетса в Лос-Анджелес
без пилота, с Дрейпером на борту. В 1954 году была введена в эксплуатацию первая автономная инерциальная навигационная система для подводных лодок (submarine inertial navigation system, SINS). В лаборатории приборостроения также был
разработан управляющий Apollo Guidance Computer – компьютер объемом один
кубический фут, который в 1969 году вывел лунный модуль Аполлона на поверхность Луны.
Сегодня высокопроизводительные инерциальные навигационные системы на
основе волоконно-оптических гироскопов широко доступны и весят около 1 кг,
а недорогие системы на основе технологии MEMS могут весить всего несколько
граммов и стоить несколько долларов.

Применение: инерциальная навигация 175
...
orientation.append(next);
>>> len(orientation)
400

Метод .increment
класса UnitQuaternion делает все это
за один вызов.

Ориентация обновляется единичным кватернионом, который возвращается конструктором EulerVec и соответствует углу
поворота и ориентации, заданным величиной и направлением
аргумента. Оператор @ выполняет умножение единичного кватерниона и нормализует произведение, гарантируя, что результат имеет единичную норму. Мы можем анимировать изменение ориентации тела
>>> orientation.animate(time=true.t)

или исследовать углы крена-тангажа-рыскания как функцию
времени
>>> xplot(true.t, orientation.rpy(), labels="roll pitch yaw");

3.4.2

Акселерометры

Акселерометры – это датчики, измеряющие ускорение. Даже
в состоянии покоя они измеряют ускорение свободного падения, определяющее направление, которое мы называем нисхо­
Отступление 3.5. Чарльз Старк (Док) Дрейпер
Дрейпер (1901–1987) – американский ученый
и инженер, которого часто называют «отцом
инерциальной навигации». Родился в Виндзоре,
штат Миссури, учился в университете Миссури,
а затем в Стэнфорде, где в 1922 году получил степень бакалавра по психологии. В 1928 и 1938 годах
в Массачусетском технологическом институте получил степень бакалавра в области электрохимической инженерии и степень магистра и доктора
наук в области физики соответственно.
Он начал преподавать во время работы в Массачусетском технологическом институте и в 1939 го­
ду стал профессором на кафедре авиационной
техники. Основал и руководил Лабораторией приборостроения в Массачусетском технологическом
институте, которая внесла существенный вклад
в теорию и практику инерциальной навигации для нужд космической программы.
В 1961 году признан Человеком года по версии журнала Time. В 1981 году введен в Национальный зал славы изобретателей. После смерти Дрейпера основанная им Лаборатория приборостроения была переименована в его честь в Лабораторию имени Чарльза Старка Дрейпера (Charles Stark Draper Laboratory, CSDL).
(Фотография предоставлена Лабораторией Чарльза Старка Дрейпера.)

176 Глава 3 · Время и движение

дящим. Ускорение свободного падения зависит от расстояния
до центра Земли и плотности пород под поверхностью в точке измерения. Земля не является идеальной сферой , и точки
в экваториальной области находятся дальше от центра. Ускорение свободного падения можно приблизительно выразить
формулой
g ≈ 9.780327(1 + 0.0053024 sin2θ - 0.0000058 sin2 2θ)
- 0.000003086h,
где θ – угол широты, а h – высота над уровнем моря. Карта гравитации, показывающая влияние широты и рельефа, представлена на рис. 3.11.

Рис. 3.11. Карта изменения ускорения свободного падения на поверхности Земли отчасти
отражает границы континентов и горных хребтов. Центры полушарий находятся на нулевом
меридиане (слева) и на меридиане смены дат (справа) соответственно (по данным Hirt et al.,
2013)

3.4.2.1

Как работают акселерометры

Концептуально акселерометр имеет очень простое устройство – он состоит из груза, который называется инерциальной
массой и поддерживается пружиной, как показано на рис. 3.12.
В инерциальной системе отсчета второй закон Ньютона для
инерциальной массы выглядит следующим образом:
mẋ˙m = Fs - m g,

(3.22)

и для пружины с естественной длиной l0 соотношение между
силой и удлинением d равно
Fs = kd.

Технически Земля
имеет форму
приплюснутого сфероида, расширяющегося на экваторе
из-за центробежного ускорения,
обусловленного
вращением Земли.
Экваториальный
диаметр примерно
на 40 км больше
полярного.

Применение: инерциальная навигация 177

инерциальная
масса

акселерометр
Рис. 3.12. Основные элементы
акселерометра и обозначения

инерциальная
система координат

Различные смещения связаны соотношением
xb - (l0 + d) = xm.
Взяв двойную производную с последующей заменой (3.22),
получаем

Мы предполагаем,
что ḋ˙ = 0 в состоянии покоя. Обычно
для гашения
колебаний инерциальной массы
используется фрикционный элемент.
Это добавляет член
–Bẋ m в правую часть
уравнения (3.22).

Величина, которую нужно измерить, – это ускорение акселерометра a = ẋ˙b и относительное смещение инерциальной массы ◄
,
линейно зависящее от этого ускорения. В акселерометре смещение измеряется и масштабируется по формуле k / m, поэтому
выходной сигнал датчика равен
a# = a + g.
Если акселерометр неподвижен, то измеренное ускорение
будет равно a# = 0 + g = g в направлении вверх. Это связано с тем,
что в нашей модели учитывается ньютоновская сила тяжести
mg, как обсуждалось в разделе 3.2.3. Выходной сигнал акселерометра иногда называют удельным, инерционным или собст­
венным ускорением.

178 Глава 3 · Время и движение
Показания акселерометра малопонятны
Выглядит совершенно нелогичным, что неподвижный акселерометр показывает ускорение в 1g, направленное вверх, потому что
он явно не ускоряется. Интуиция подсказывает, что ускорение
должно быть направлено вниз, так как именно так устройство будет ускоряться при падении. Усугубляет ситуацию еще и тот факт,
что некоторые приложения для смартфонов ошибочно сообщают
положительное ускорение вниз, когда телефон неподвижен.

Акселерометры измеряют ускорение вдоль одной оси. Обычно в одну конструкцию объединяются три акселерометра и размещаются так, чтобы их оси чувствительности располагались
ортогонально друг другу. Три выходных сигнала такого трехосного акселерометра представляют компоненты вектора ускорения BaB#, измеренные в системе координат тела {B}.
Природа тоже изобрела свой акселерометр. Все позвоночные
обладают датчиками ускорения, называемыми ампулами, которые являются частью вестибулярного аппарата. У нас их два
в каждом внутреннем ухе, и они помогают нам поддерживать
равновесие:
саккула (сферический мешочек) измеряет вертикальное ускорение, а утрикула (эллипсовидный мешочек) –
ускорение в направлении вперед-назад. Роль инерциальной
массы в ампулах играет скопление кристаллов карбоната кальция, называемых отолитами (буквально «ушные камни»), в желатиновом субстрате, который служит пружиной и демпфером.
Волосковые клетки, встроенные в субстрат, измеряют смещение отолитов под действием ускорения.

3.4.2.2

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

Оценка положения и ускорения тела

На рис. 3.13 показана система координат {0} с осью z, направленной вертикально вверх, и с ускорением

где g – локальное ускорение свободного падения, указанное на
рис. 3.11. В системе координат {B}, зафиксированной на теле,
при произвольной ориентации, выраженной через ZYX крентангаж-рыскание,
ξB = ξ (γ) ⊕ ξ (β) ⊕ ξ (α),

0

rz

ry

rx

ускорение свободного падения будет равно
(3.23)

Мы могли бы использовать любую
последовательность
из трех углов.

Применение: инерциальная навигация 179

Рис. 3.13. Ускорение свободного падения
определяется как ось z неподвижной системы {0}
и измеряется акселерометром в системе
вращающегося тела {B}

Вектор ускорения, измеренного датчиком в системе координат {B}, равен
aB# = (a#x, ay#, az#)T.

B

Приравняв его (3.23), можно найти углы крена и тангажа

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

Если взглянуть
с другой точки
зрения, то мы,
по сути, измеряем
направление
вектора
силы тяжести
относительно
системы отсчета {B},
а вектор
предоставляет
только два
уникальных
фрагмента
информации
о направлении,
поскольку один
компонент
единичного вектора
всегда можно
выразить через два
других.

(3.24)

где значок «крышки» показывает, что это оценки углов. Обратите внимание на отсутствие решения для угла рыскания γ,
и он вообще отсутствует в (3.23). Вектор силы тяжести параллелен вертикальной оси, и вращение вокруг этой оси (рыскание)
никак не повлияет на измеряемое значение. Также отметьте,
что β̂ зависит от локального значения ускорения свободного
падения, изменение которого показано на рис. 3.11.
 Акселерометры измеряют ускорение свободного падения u
движение тела
Мы сделали очень сильное предположение, что измеренное ускорение BaB# обусловлено только силой тяжести. В реальности датчик, установленный в роботе, будет испытывать дополнительное
ускорение по мере его движения, что может приводить к ошибкам
в оценке ориентации.

Часто бывает нужно оценить движение транспортного средства в инерциальной системе координат {0}, где общее измеренное ускорение обусловлено силой тяжести и движением
a = 0g + 0aB.

0 #

Мы наблюдаем ускорение в системе координат тела, поэтому
ускорение в мировой системе координат равно
âB = 0R̂B BaB# - 0g,

0

(3.25)

180 Глава 3 · Время и движение

при этом мы предполагаем, что 0R̂B и g оба известны. ► Интегрирование ускорения по времени
(3.26)
дает скорость системы координат тела, а повторное интегрирование
(3.27)
дает местоположение. Мы можем предположить, что ускорение
транспортного средства равно нулю, и оценить его ориентацию,
а имея ориентацию, можем оценить ускорение. Мы не можем
оценить обе характеристики сразу, потому что неизвестных получается больше, чем результатов измерений.

3.4.3

Магнитометры

Земля – массивный, но слабый магнит. Полюса этого геомагнита – северный и южный – постоянно движутся и находятся на
значительном расстоянии от оси вращения планеты. В любой
точке планеты силовые магнитные линии можно рассматривать как вектор m, величину и направление которого можно
точно предсказать и нанести на карту, как показано на рис. 3.14.
На рис. 3.14б,в показано направление вектора в терминах
двух углов: склонения и наклонения. Горизонтальная проекция вектора m направлена в сторону магнитного севера, а угол
склонения D отсчитывается от истинного севера по часовой
стрелке. Угол наклонения или падения I вектора измеряется
в вертикальной плоскости вниз от горизонтали. Длина вектора, плотность магнитного потока, измеряется магнитометром
в единицах тесла (Тл) и для Земли варьируется от 25 до 65 мкТл,
как показано на рис. 3.14а.

3.4.3.1

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

Как работают магнитометры

Ключевым элементом большинства современных магнитомет­
ров является датчик Холла – полупроводниковый прибор, создающий напряжение, пропорциональное плотности магнитного потока в направлении, перпендикулярном току. Обычно три
датчика Холла собираются вместе и располагаются так, чтобы
их оси чувствительности были ортогональны. Три выходных
сигнала такого трехосного магнитометра определяют компоненты вектора плотности магнитного потока Земли Bm#, измеренные в системе отсчета {B}.

Направление на
северный полюс
Земли, где ось вращения пересекает
поверхность северного полушария.
В северном полушарии наклонение
положительное, то
есть вектор направлен в землю.
Для сравнения, современный аппарат
МРТ имеет напряженность магнитного поля 4–8 Тл.

Применение: инерциальная навигация 181

а

б

в
Рис. 3.14. Прогнозируемая модель магнитного поля Земли на 2015 год: а) плотность потока
магнитного поля (нТл); б) магнитное склонение (градусы); в) магнитное наклонение (градусы).
Магнитные полюса обозначены звездочками (карты представлены организациями NOAA/
NGDC и CIRES http://ngdc.noaa.gov/geomag/WMM и опубликованы в декабре 2014)

182 Глава 3 · Время и движение
истинный север

угол склонения
S

S

вид сверху

угол наклонения

разрез Ю-Ю

Рис. 3.15. Стрелка компаса направлена вдоль
горизонтальной составляющей вектора
магнитного поля Земли, которая смещена
от истинного севера на угол склонения D.
Вектор магнитного поля имеет угол
наклонения I относительно горизонтали

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

3.4.3.2

Оценка направления

На рис. 3.16 показана инерциальная система координат {0}
с осью z, направленной вертикально вверх, и осью x, расположенной в горизонтальной плоскости и направленной на магнитный север. Следовательно, вектор магнитного поля лежит
в плоскости xz.

где B – плотность магнитного потока, а I – угол наклонения; оба
угла известны из рис. 3.14. В системе координат {B}, связанной
с телом, при произвольной ориентации выражается в терминах
углов крена-тангажа-рыскания ZYX,
ξB = ξ rz(γ) ⊕ ξ ry(β) ⊕ ξ rx(α),

0

а плотность магнитного потока будет определяться как

(3.28)

Мы могли бы использовать любую
последовательность
из трех углов.

Применение: инерциальная навигация 183

Рис. 3.16. Система координат {0} имеет ось x,
совмещенную с горизонтальной проекцией m,
и вертикальную ось z. Магнитное поле
измеряется магнитометром в системе координат
вращающегося тела {B}

Вектор плотности магнитного потока, измеренной датчиком
в системе координат {B}, выражается как
m# = (m#x, my#, mz#)T.

B

И, приравняв его к (3.28), можно найти угол рыскания

который определяет магнитный курс и связан с истинным курсом как
γ̂ = γ̂ - D.

tn

Это предполагает, что нам известны локальный угол склонения, а также углы крена и тангажа, α и β. Последний можно оценить измерением ускорения с помощью (3.24). Именно для этой
цели многие устройства с трехосным датчиком Холла включают
в себя также трехосный акселерометр.
Магнитометры хороши в теории, но на практике имеют сомнительную ценность для ориентации. Во-первых, современный мир полон магнитов и электромагнитов. Здания содержат
Отступление 3.6. Эдвин Холл
Холл (1855–1938) – американский физик, родившийся
в штате Мэн. В 1880 году, защитив докторскую диссертацию по физике в университете Джонса Хопкинса, открыл, что магнитное поле оказывает воздействие на ток
в проводнике. Он пропускал ток через тонкую золотую
пластину и в присутствии магнитного поля, перпендикулярного пластине, смог измерить очень малую разность
потенциалов между ее сторонами. Это явление теперь
известно как эффект Холла. Тогда уже было известно, что
магнитное поле оказывает воздействие на проводник
с током, но считалось, что эта сила действует на провод­
ник, а не на сам ток – электроны еще не были открыты.
В 1895 году был назначен профессором физики в Гарварде, где изучал термоэлект­рические эффекты.

184 Глава 3 · Время и движение

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

3.4.4

Слияние инерциальных датчиков

Инерциальная навигационная система использует датчики,
которые мы только что обсудили, для определения положения
транспортного средства – его местоположения и ориентации.
Ранние инерциальные навигационные системы, такие как на
рис. 2.19, использовали механические карданные подвесы для
поддержания постоянной ориентации акселерометров относительно звезд с помощью гиростабилизированной платформы.
Ускорение, измеренное на этой платформе, по определению
относится к инерциальной системе отсчета и требует лишь интегрирования для определения скорости платформы, а затем
повторного интегрирования для определения ее местоположения. Для получения точной оценки местоположения в течение
нескольких часов или дней карданные подвесы и гироскопы
должны были быть исключительно высокого качества, чтобы
исключить возможный дрейф платформы. Датчики ускорения
тоже должны были быть чрезвычайно точными.
Современная бесплатформенная инерциальная измерительная система не использует карданные подвесы. Датчики угловой
скорости, ускорения и магнитного поля жестко закреплены на
транспортном средстве. Совокупность инерциальных датчиков
называется инерциальным измерительным блоком (inertial measurement unit, IMU). Шестиосевой IMU включает в себя трехосные
гироскопы и акселерометры, а 9-осевой состоит из трехосных гироскопов, акселерометров и магнитометров. Система, которая
определяет только ориентацию, называется системой определения курса и вертикали (attitude and heading reference system, AHRS).
Датчики, используемые в гражданских устройствах, особенно в недорогих смартфонах и дронах, далеки от совершенства.
Выходной сигнал любого датчика x# – гироскопа, акселерометра
или магнитометра – представляет собой искаженную версию
истинного значения x и обычно моделируется как
x# = sx + b + ε,
где s – масштабный коэффициент, b – смещение, а ε – случайная ошибка, или «шум». Обычно s указывается производителем

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

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

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

с некоторым допуском, например ±1 %, и для конкретного датчика его можно определить с помощью процедуры калибровки. Смещение b в идеале равно нулю, но может меняться от
устройства к устройству. Смещение, изменяющееся со временем, часто называется дрейфом датчика. Масштабный коэффициент и смещение обычно зависят от температуры. ◄
На практике смещение является самой большой проблемой,
поскольку оно меняется со временем и температурой и сущест­
венно искажает расчетные местоположение и ориентацию. Рассмотрим положительное смещение на выходе гироскопического
датчика – выходной сигнал выше, чем должен быть. На каждом
шаге (3.20) приращение поворота будет больше, чем должно быть,
что означает линейный рост ошибки ориентации со временем. ◄
При использовании (3.25) для оценки ускорения транспортного средства ошибка определения ориентации приведет
к не­верной компенсации измеренного ускорения свободного
падения, и оно будет неотличимо от фактического ускорения
транспортного средства. Смещение ускорения становится линейной по времени ошибкой определения скорости и квадратичной по времени ошибкой определения местоположения.
Ошибка определения ориентации уже линейна по времени,
поэтому мы получаем кубическую по времени ошибку определения местоположения, и это без учета влияния смещения
акселерометра. Смещение датчика – серьезная проблема! Как
правило, гироскопы со стабильным смещением 0.01 грд/ч приведут к увеличению ошибки определения местоположения со
скоростью 1.85 км/ч. Системы военного уровня обладают весьма впечатляющей стабильностью: для ракет она составляет менее 0.00002 грд/ч, что резко контрастирует со стабильностью
устройств потребительского уровня, которая находится в диапазоне 0.01-0.2 градуса в секунду.
Увидеть влияние смещения на предполагаемую ориентацию
можно, например, с помощью функции IMU из Toolbox:
>>> from imu_data import IMU
>>> true, imu = IMU()

Не совсем неизвестное, его можно
увидеть в исходном
коде imu_data.py.

которая возвращает два объекта: истинное движение вращающегося тела и смоделированные данные инерциального измерительного блока (IMU), закрепленного на этом теле. Последний
содержит «измерения» гироскопа, акселерометра и магнито­
метра, включающие фиксированное, но неизвестное смещение.
◄ Данные организованы в виде строк в массивах imu.gyro, imu.
accel и imu.magno соответственно, по одной строке на шаг времени, а соответствующие моменты времени определяются элементами вектора imu.t. Повторим пример из раздела 3.4.1.2, но
теперь со смещением датчика:

186 Глава 3 · Время и движение
>>> q = UnitQuaternion();
>>> for wm in imu.gyro[:-1]:
...
q.append(q[-1] @ UnitQuaternion.EulerVec(wm * imu.dt))

Угловая погрешность
тацией

между расчетной и истинной ориен-

Как обсуждалось
в разделе 2.4.5.

>>> xplot(true.t, q.angdist(true.orientation), color="red");

Ошибка ориентации (рад)

показана красной линией на рис. 3.17а. Мы ясно видим рост
угловой погрешности со временем из-за смещения сигналов
датчика.
Простое интегрирование
Явный дополнительный фильтр

Оценка смещения
гироскопа (рад/с)

а

б

Многие любительские дроны делают
это буквально за
несколько секунд до
взлета.

Время (с)

Рис. 3.17. а) Влияние смещения гироскопа на простую интеграцию гироскопа с явным
дополнительным фильтром; б) оценка смещения гироскопа с использованием явного
дополнительного фильтра

Зная величину смещения, мы могли бы вычесть его из показаний датчика перед интегрированием. Простой способ получить оценку смещения – оставить инерциальный измерительный блок неподвижным на некоторое время и затем вычислить
среднее значение по всем выходным сигналам датчиков.
Полученные оценки смещений остаются верными только в течение короткого периода времени, потому что само смещение
дрейфует с течением времени.
Более сложный подход – оценивать смещение прямо в процессе работы, ► но для этого необходимо объединить информацию
с разных датчиков. Этот подход известен как слияние датчиков.
Он основан на предположении, что разные датчики обладают взаи­
модополняющими характеристиками. Смещение датчиков угловой скорости приводит к росту ошибки оценки ориентации, но
смещение акселерометров приводит только к ошибке определе-

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

Применение: инерциальная навигация 187

ния местоположения. Акселерометры реагируют на поступательное движение, а хорошие гироскопы – нет. Магнитометры предоставляют частичную информацию о крене, тангаже и рыскании,
они невосприимчивы к ускорению, но реагируют на рассеянные
магнитные поля и другие искажения. Существует множество
способов добиться такого слияния. Один из них: использовать
инструмент оценки, называемый дополнительным фильтром
Калмана и описанный в приложении H. При наличии полной нелинейной математической модели, связывающей сигналы датчиков и их смещения с положением транспортного средства, а также
знаний о шуме (неопределенности) в сигналах датчиков фильтр
может дать оптимальную оценку положения и смещения, наилучшим образом объясняющую сигналы датчиков.
Далее мы рассмотрим простую, но эффективную альтернативу, называемую явным дополнительным фильтром. Шаг обновления вращения выполняется с использованием расширенной
версии (3.21).
ξ∆〈k〉 = eδt[ ωB 〈k〉 - b̂〈k〉 + kP σR〈k〉]×.
B

B

#

(3.29)

Ключевые отличия заключаются в том, что оценка смещения
b̂ вычитается из показаний датчика и добавляется член, основанный на ошибке ориентации σR. Оценка смещения изменяется со временем согласно закону
b̂〈k+1〉 ← b̂〈k〉 - kI σR〈k〉

(3.30)

и также зависит от ошибки ориентации σR. kP > 0 и kI > 0 – специально подобранные константы. Ошибка ориентации определяется по N измерениям векторов Bυi#

где
υi〈k〉 = (⊖ 0ξˆB〈k〉) · 0υi

B

– векторный сигнал, известный в инерциальной системе координат (например, ускорение свободного падения), повернутый
в систему координат тела, на величину, обратную оценке ориентации 0ξˆB. Любая ошибка в направлении между этими векторами даст ненулевое векторное произведение, которое является осью, поворот вокруг которой преобразует один вектор
в другой. Фильтр использует эту разницу – нововведение – для
улучшения оценки ориентации, возвращая ее обратно в (3.29).
Этот фильтр позволяет объединять неограниченное количество
векторных измерений Bυi в системе координат тела, например
мы можем включить измерения магнитного поля или любые

188 Глава 3 · Время и движение

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

kI = 0.2; kP = 1;
b = np.zeros(imu.gyro.shape);
qcf = UnitQuaternion();
data = zip(imu.gyro[:-1], imu.accel[:-1], imu.magno[:-1]);
for k, (wm, am, mm) in enumerate(data):
qi = qcf[-1].inv()
sR = np.cross(am, qi * true.g) + np.cross(mm, qi * true.B)
wp = wm - b[k,:] + kP * sR
qcf.append(qcf[k] @ UnitQuaternion.EulerVec(wp * imu.dt))
b[k+1,:] = b[k,:] - kI * sR * imu.dt

и построим график угловой разницы между вычисленной и истинной ориентацией (синяя линия на рис. 3.17а):
>>> xplot(true.t, qcf.angdist(true.orientation), color="blue");

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

3.5

Подведение итогов

В этой главе мы обсудили важный вопрос, связанный с изменением положения с течением времени. Сначала мы рассмотрели
подход с позиции дифференциального исчисления и показали,
что временная производная мат­ри­цы вращения или кватернио­
на является функцией угловой скорости тела. В случае с мат­ри­
цей вращения появляется кососимметричная мат­ри­ца, и вас
больше не должно это удивлять, учитывая ее тесную связь с вращением через теорию групп Ли, как обсуждалось в предыдущей
главе. Затем мы рассмотрели конечную временную разность
как приближение к производной и показали, что это приводит
к недорогим с вычислительной точки зрения методам обновления матриц вращения и кватернионов при наличии знания
угловой скорости. Мы также обсудили динамику движущихся
тел, совершающих поступательные и вращательные движения
под действием сил и моментов, инерциальные и неинерциальные системы координат и понятие фиктивных сил.
Затем мы рассмотрели задачу создания движения – последовательности положений – траектории, – по которой может

Подведение итогов 189

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

3.5.1

Дополнительное чтение

Первые работы по построению декартовой траектории робота-манипулятора: Paul (1972, 1979) и Taylor (1979). Многосегментная траектория обсуждается в Paul (1979, 1981), а концепция переходов между сегментами – в Lloyd, Hayward (1991). Эти
и другие ранние работы включены в сборник «Robot Motion»
(Brady et al. 1982). Полиномиальные и трапециевидные траектории подробно описаны в Spong et al. (2006), а многосегментные
траектории подробно рассматриваются в Siciliano et al. (2009)
и Craig (2005). Мы рассмотрели лишь простой генератор многосегментных траекторий, вдохновленный работой Paul (1981),
однако существуют более сложные подходы, основанные на
сплайнах. Пакет на языке Python для создания плавных траекторий с учетом кинематических и динамических ограничений
можно найти по адресу https://hungpham2511.github.io/toppra.
Существует множество литературы, посвященной теории
и практике инерциальных навигационных систем. В диссертации Achtelik (2014) описывается сложный дополнительный
фильтр Калмана для оценки положения, скорости и смещения
датчика для небольшого воздушного робота. Явный дополнительный фильтр, используемый в этой главе, описан в Hua et al.
(2014). Книга Groves (2013) посвящена инерциальной и наземной радио- и спутниковой навигации, а также хорошо описывает методы применения фильтра Калмана для оценки состояния.
В Titterton, Weston (2005) дается ясное и краткое описание принципов, лежащих в основе инерциальной навигации, и особое
внимание уделяется самим датчикам, но с появлением современных недорогих датчиков эта работа несколько устарела. Тех-

190 Глава 3 · Время и движение

нические описания многих недорогих устройств инерциальной
навигации и датчиков магнитного поля можно найти на сайте
https://www.sparkfun.com в категории «Sensors» (Датчики).
Книга «Digital Apollo» (Mindell, 2008) представляет собой увлекательное повествование о разработке инерциальной навигационной системы для лунного модуля в рамках программы
«Аполлон». В статье Corke et al. (2007) описаны принципы работы инерциальных датчиков и функционально эквивалентных
датчиков, расположенных во внутреннем ухе млекопитающих
и играющих ключевую роль в поддержании равновесия.

3.5.2

Упражнения

Выразите приращение поворота RΔ в виде степенного ряда
и проверьте (3.9).
2. Выведите уравнение обновления единичного кватерниона
(3.10).
3. Выведите выражение для фиктивных сил во вращающейся
системе координат из раздела 3.2.3.
4. Смоделируйте частицу, движущуюся с постоянной скоростью во вращающейся системе координат. Постройте графики местоположения частицы в инерциальной и вращающейся системах и проследите, как движение в последней
изменяется в зависимости от поступательной скорости час­
тицы и угловой скорости вращающейся системы.
5. Рассчитайте центростремительное ускорение, вызванное
орбитальным вращением Земли вокруг Солнца. Как оно
соотносится с центростремительным ускорением, вызванным осевым вращением Земли?
6. Для примера вращательного интегрирования (раздел 3.4.1.2):
а)	поэкспериментируйте, изменяя интервал выборки и добавляя нормализацию мат­ри­цы вращения на каждом
N-м шаге;
б)	используйте пакет timeit для измерения времени выполнения приближенного решения с нормализацией и точного решения с использованием матричной экспоненты;
в)	переделайте пример, используя кватернионы, и поэкспериментируйте, изменяя интервал выборки и добавляя нормализацию на каждом N-м шаге.
7. Повторите интегрирование угловой скорости на основе кватернионов (раздел 3.4.1.2), используя мат­ри­цы вращения.
8. Определите величину и направление центростремительного ускорения в своем местоположении. Если вы едете
на восток со скоростью 100 км/ч, то каковы будут величина и направление ускорения Кориолиса, испытываемого
вами? А при движении на север с той же скоростью? Верти-

1.

Подведение итогов 191

9.

10.

11.
12.

13.

14.

15.

16.

17.
18.

кальная составляющая называется эффектом Этвеша: насколько вы становитесь легче в каждом случае?
Для траектории quintic от 0 до 1 за 50 шагов исследуйте
влияние различных начальных и конечных скоростей, как
положительных, так и отрицательных. При каких обстоятельствах полином пятой степени выходит за пределы траектории и почему?
Для траектории trapezoidal от 0 до 1 за 50 шагов исследуйте
влияние различных значений скорости на участке с постоянной скоростью. Определите минимальные и максимальные границы.
Для траектории от 0 до 1 за T секунд с максимально возможной скоростью 0.5 сравните общее время, необходимое для
преодоления каждой из траекторий quintic и tra­pezoidal.
Используйте метод animate для сравнения интерполяции
вращения с использованием углов Эйлера, углов кренатангажа-рыскания, кватернионов и кручений. Подсказка:
используйте метод interp кватернионов и mtraj.
Разработайте процедуру количественного сравнения эффективности различных методов интерполяции ориентации. Подсказка: постройте группу точек (локус) на единичной сфере.
Повторите пример на рис. 3.7 для случая, когда:
а)	интерполяция не проходит через сингулярность. Подсказка: измените начальный или конечный угол тангажа. Что произойдет в такой ситуации?
б)	конечная ориентация находится в сингулярности. Что
произойдет в такой ситуации?
Преобразуйте мат­ри­цу SE(3) в кручение и возведите кручение в степень со значениями в интервале [0, 1]. Чем эта
форма интерполяции отличается от других, рассмотренных в данной главе?
Для примера mstraj (раздел 3.3.3):
а) повторите с другой начальной и конечной скоростью;
б)	исследуйте влияние увеличения времени ускорения
и постройте график зависимости общего времени от
времени ускорения;
в)	функция имеет третий выходной аргумент с параметрами для каждого сегмента, исследуйте их.
Измените mstraj так, чтобы при определении времени преодоления сегмента учитывались пределы ускорения.
Существует ряд приложений для iOS и Android, которые
отображают и записывают данные с гироскопов, акселерометров и магнитометров устройства. В качестве альтернативы можно использовать плату расширения с инерциальным датчиком для Raspberry Pi или Arduino и написать
свою программу для регистрации данных. Запустите одно

192 Глава 3 · Время и движение

19.

20.

21.
22.

23.
24.
25.
26.

27.

28.

из этих приложений и понаблюдайте, как меняются сигналы датчиков в зависимости от ориентации и движения.
Если приложение поддерживает регистрацию данных, то
выясните, какое ускорение регистрируется при подбрасывании устройства вверх.
Пусть есть гироскоп со стальным ротором диаметром 20 мм
и толщиной 4 мм, вращающимся со скоростью 10 000 об/мин.
Какой будет величина h? Какой крутящий момент будет
создаваться при угловой скорости 5 грд/с?
Используя формулу (3.19), объясните, как игрушечный гироскоп может балансировать на одном конце оси, ориентированной горизонтально. Что удерживает его в вертикальном положении?
Акселерометр упал со стола. Какое значение он покажет
при падении, если сопротивление воздуха и ориентация не
меняются?
Реализуйте алгоритм определения углов крена и тангажа
по показаниям акселерометра.
а)	Разработайте алгоритм, различающий книжную и альбомную ориентации.
б)	Создайте с помощью функции quintic траекторию для
акселерометра, генерирующую движение в направлении оси x или y. Как ускорение вдоль траектории влияет
на расчетный угол?
в)	Рассчитайте ориентацию, используя кватернионы вмес­
то углов крена-тангажа-рыскания.
Определите углы Эйлера как функцию измеренного ускорения. Для этого вы можете установить и использовать пакет SymPy.
Представьте, что вы находитесь в самолете, летящем на высоте 30 000 футов над вашим текущим местоположением.
Насколько вы будете легче, находясь в самолете?
Определите напряженность, склонение и наклонение магнитного поля в вашем регионе. Посетите веб-сайт http://
www.ngdc.noaa.gov/geomag-web.
Используя приложение для чтения показаний датчиков,
сори­ентируйте телефон так, чтобы вектор магнитного поля
имел только компоненту вдоль оси z. Как проходит вектор
магнитного поля относительно вашего телефона?
Используя приложение для чтения показаний датчиков, запишите данные инерциальных датчиков телефона при его
перемещении. Используя эти данные, оцените изменение
ориентации и местоположения телефона в целом. Можно
ли это сделать в режиме реального времени?
Поэкспериментируйте с изменением параметров явного
дополнительного фильтра (раздел 3.4.4). Измените смещение или добавьте гауссовский шум к моделируемым показаниям датчика.

Часть II Мобильная
робототехника
Глава 4
Глава 5
Глава 6

Мобильные роботы
Навигация
Определение местоположения
и картографирование

194 Глава 3 · Время и движение

В этой части мы рассмотрим мобильных роботов – роботов, способных перемещаться в окружающей среде. Мобильные роботы – это более современная
технология, чем роботы-манипуляторы, о которых мы поговорим в части III,
но поскольку большинство мобильных роботов работают в двух измерениях,
концептуальное знакомство с ними может быть более поверхностным.
В настоящее время наблюдается большой интерес к мобильным роботам –
новому рубежу в робототехнике – с развитием беспилотных автомобилей,
планетоходов, гуманоидных роботов и дронов, а также новых областей их
применения, таких как складская логистика и воздушные такси. Первые коммерческие применения мобильных роботов имели место в 1980-х, когда были
разработаны автоматически управляемые транспортные средства (automated
guided vehicles, AGV) для транспортировки материалов по заводам, и с тех пор
эта технология достигла высокого уровня. Эти первые свободно перемещающиеся автоматические колесные транспортные средства обычно использовали стационарную инфраструктуру для навигации, например нарисованную
линию на полу, подземный кабель, излучающий радиосигнал, или настенные
реперные маркеры. Последнее десятилетие ознаменовалось значительными
достижениями в разработке мобильных роботов, способных работать без навигационной инфраструктуры.
Главы этой части книги охватывают основы мобильной робототехники. Глава 4 рассматривает управление движением двух образцов роботизированных
платформ: колесных транспортных средств, перемещающихся на плоской поверхности, и воздушных роботов, перемещающихся в трехмерном пространстве, таких как квадрокоптеры. Глава 5 посвящена навигации, то есть приемам
поиска пути к цели в среде с препятствиями с использованием прямой сенсорной информации или карты. Большинство стратегий навигации требуют
знания местоположения робота, и это тема главы 6, где мы исследуем такие
методы, как счисление координат и использование карт в сочетании с наблюдением за ориентирами. Мы также посмотрим, как робот может создавать карту и даже определять свое местоположение, одновременно перемещаясь по
неизвестной местности. Для этого мы применим знания из раздела 2.2 о представлении мес­тоположения, ориентации и положения в двух измерениях.

Глава

4

	Мобильные роботы

chap4.ipynb

https://go.sn.pub/
lSs5pg

На рис. 4.1–4.3 показаны образцы мобильных роботов разной
формы и решающих разные задачи. Мобильные роботы могут действовать не только на поверхности земли, и как доказательство на рис. 4.2 показаны примеры воздушных роботов,
известных как беспилотные летательные аппараты (БПЛА),
подводных роботов, известных как автономные подводные
аппараты (АПА), и роботизированных лодок, известных как
безэкипажные катера (БЭК). В совокупности их часто называют автономными беспилотными системами (АБС). На рис. 4.3
показаны примеры полевых роботизированных систем, таких
как планетоходы, грузовики в горнодобывающей промышленности, контейнеровозы в портах и беспилотные тракторы для
выполнения полевых работ в сельском хозяйстве. Некоторые из
них представляют собой узкоспециализированные или исследовательские прототипы, тем не менее спектр коммерческих
полевых роботов, доступных для практического применения,
продолжает расширяться.
Основные видимые различия заключаются в роботизированной платформе – физическом воплощении робота, его средствах передвижения и датчиках. Датчики робота хорошо видны
на рис. 4.1б, рис. 4.2а,г и рис. 4.3а. Но внутренне, что касается
устройства конвейера обработки данных, поступающих с датчиков, и алгоритмов навигации, эти роботы будут сильно похожи друг на друга. Робот-пылесос (рис. 4.1a) имеет мало датчиков
и использует реактивные стратегии для уборки пола. В отличие

а

б

Рис. 4.1. Некоторые мобильные наземные роботы: а) робот-пылесос Roomba®, 2008
(изображение предоставлено iRobot®); б) Boss, беспилотный автомобиль команды Tartan
Racing, победивший в гонке DARPA Urban Challenge, 2007 (© университет Карнеги–Меллона)

196 Глава 4 · Мобильные роботы

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

а

б

в

г

Рис. 4.2. Некоторые воздушные, надводные и подводные роботы: а) вертолет Yamaha RMAX®
с диаметром лопастей 3 м (фотография Санджива Сингха); б) ScanEagle, беспилотный
летательный аппарат самолетного типа (фотография предоставлена компанией Insitu);
в) RangerBot, подводный робот с 6 двигателями (2020) (фотография предоставлена
компанией Biopixel); г) безэкипажный катер (фотография Мэтью Данбабина)

а

б

в

г

Рис. 4.3. а) Исследования: марсоход Curiosity, созданный в лаборатории Mars Science
Laboratory (MSL), проходит испытания (изображение предоставлено NASA/Фрэнк Мартин);
б) логистика: автоматизированный контейнеровоз, перевозящий контейнеры; порт Брисбена,
2006 (изображение предоставлено компанией Port of Brisbane Pty Ltd); в) добыча полезных
ископаемых: автономный самосвал (Copyright © 2015 Рио Тинто); г) сельское хозяйство:
робот для прополки сорняков AgBot-2 от компании QUT (фотография Оуэна Боудена)

Колесные мобильные роботы 197

В этой главе обсуждаются вопросы движения роботизированной платформы, то есть изменение ее положения со временем в зависимости от управляющих воздействий. Существует
множество различных типов роботизированных платформ, но
в этой главе мы рассмотрим только четыре наиболее распространенные. В разделе 4.1 мы разберем три различных типа
колесных роботов, работающих в двухмерном пространстве.
Они могут двигаться вперед или назад и изменять направление с помощью рулевого механизма. В разделе 4.2 описывается квадрокоптер – беспилотный летательный аппарат, являющийся примером робота, перемещающегося в трехмерном
пространстве. Квадрокоптеры, или «дроны», приобретают все
большую популярность в качестве роботизированной платформы благодаря своей низкой стоимости, простоте моделирования и управления.
В разделе 4.3 мы снова вернемся к концепции конфигурационного пространства и более глубоко рассмотрим важные вопросы малоприводности (underactuation) и неголономности.

4.1

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

Колесные мобильные роботы

Колесо – одно из величайших изобретений человечества, сделанное около 3000 года до н. э., а около 2000 года до н. э. была
изобретена двухколесная повозка. В настоящее время повсеместное распространение получили четырехколесные транспортные средства, а общее количество автомобилей на планете
давно перевалило за миллиард. Эффективность автомобилей
и наша близость к ним делают их естественным выбором для
создания роботизированных платформ, перемещающихся по
земле.
Из повседневного опыта вождения автомобилей мы знаем,
что существуют ограничения в свободе их передвижения. Ездить боком невозможно, но, попрактиковавшись, можно научиться парковаться по траектории, которая приведет к тому,
что автомобиль окажется сбоку от своего первоначального положения – это параллельная парковка. Автомобиль также не
может развернуться на месте, но мы можем парковаться по такой траектории, что автомобиль окажется в том же местоположении, но повернется на 180° – парковка в три приема. Необходимость выполнения таких маневров является отличительной
чертой неголономной системы – важной концепции, которая
обсуждается далее в разделе 4.3. Несмотря на эти незначительные ограничения, автомобиль является самым простым и эффективным средством передвижения в плоском мире. Модель

198 Глава 4 · Мобильные роботы

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

4.1.1

Автомобили

Автомобили с управляемыми колесами – очень эффективный
класс транспортных средств и прототип большинства наземных
роботов, подобных тем, что показаны на рис. 4.3. В этом разделе мы создадим модель транспортного средства, похожего на
автомобиль, и разработаем регуляторы, способные управлять
автомобилем и приводить его в заданную точку, заставлять его
следовать вдоль линии или по произвольному пути и, наконец,
занимать определенное положение.
Для моделирования поведения низкоскоростного четырехколесного автомобиля широко используется двухколесная кинематическая модель, изображенная на рис. 4.4. Заднее колесо этой модели закреплено на корпусе, а плоскость вращения
переднего колеса может поворачиваться вокруг вертикальной
оси, обеспечивая возможность управления направлением движения.
Положение транспортного средства представлено системой
координат кузова {B}, показанной на рис. 4.4, ось x которой направлена вперед, а начало координат находится в центре задней оси. Ориентация оси x относительно мировой системы
координат определяется углом рыскания транспортного средства (или просто курсом). Конфигурация транспортного средства
представлена обобщенными координатами q = (x, y, θ) ∈ 𝒞, где
𝒞 ⊂ ℝ2 × S1 – конфигурационное пространство. Мы предполагаем, что каждое колесо вращается в своей плоскости и что колеса
катятся без проскальзывания или бокового скольжения:
υ = (υ, 0).

B

(4.1)

Пунктирные линии показывают направление, вдоль которого колеса не могут двигаться, – это линии отсутствия движения,
и они пересекаются в точке, известной как мгновенный центр
поворота (МЦП). Начало системы координат {B} транспортного
средства движется по окружности, ее угловая скорость равна

Из Sharp (1896)

Часто ее ошибочно
называют моделью
Аккермана.

Колесные мобильные роботы 199

,

(4.2)

радиус которой, согласно простой геометрии, равен
Передаточное отношение рулевого
механизма – это
отношение угла
поворота рулевого колеса к углу
поворота управляемых колес. Для
автомобилей оно
обычно находится
в диапазоне от 12:1
до 20:1. Некоторые автомобили
имеют «переменное
передаточное отношение» рулевого
механизма, при котором передаточное
отношение выше
(менее чувствительное) около нулевого
угла.

(4.3)
где L – длина транспортного средства, или колесная база. Как
нетрудно догадаться, радиус поворота увеличивается с длиной
транспортного средства: у автобуса радиус поворота больше,
чем у автомобиля. Угол поворота управляемого колеса γ обычно
ограничен механически, и его максимальное значение определяет минимальное значение RB.
В автомобиле управление углом поворота осуществляется с помощью рулевого колеса, и эти два угла линейно связаны
передаточным отношением рулевого механизма. Повороты
на дорогах строятся в виде дуг окружностей, или клотоид, чтобы водителю было проще следовать по дороге, плавно изменяя
угол поворота рулевого колеса. Поскольку RF > RB, переднее колесо должно пройти более длинный путь и, следовательно, вращаться быстрее заднего.

МЦП мгновенный центр поворота

угол поворота
управляемого колеса

скорость

управляемое колесо

угол направления

колесная база

Рис. 4.4. Двухколесная модель транспортного средства, похожего на автомобиль.
Четырехколесный автомобиль показан светло-серым цветом, а эквивалентная двухколесная
модель – темно-серым. Система координат кузова автомобиля показана красным цветом,
а мировая система координат – черным. Угол поворота управляемого колеса γ определяется
относительно системы координат кузова, а скорость заднего колеса в направлении оси x
равна υ. Пунктирными линиями нарисованы продолжения осей колес, и они пересекаются
в мгновенном центре поворота (МЦП). Задние и передние колеса движутся вокруг МЦП по
дугам окружностей с радиусами RB и RF соответственно

200 Глава 4 · Мобильные роботы

При прохождении поворота четырехколесным транспортным средством два управляемых колеса движутся по окружностям с немного разными радиусами, поэтому углы поворота управляемых колес γL и γR должны немного отличаться. Это
достигается благодаря широко распространенному механизму
рулевого управления Аккермана, который снижает износ шин.
Ведомые колеса должны вращаться с разной скоростью на поворотах, поэтому между двигателем и ведущими колесами необходим дифференциальный редуктор.
Скорость робота в мировой системе отсчета описывается соотношениями
ẋ = υ cos θ,
ẏ = υ sin θ,
(4.4)
которые представляют собой уравнения, или модель, движения. Это кинематическая модель, поскольку она описывает
скорости транспортного средства, но не силы или моменты,
обуславливающие эту скорость. Скорость изменения курса θ˙
называется угловой скоростью поворота, курсовой скоростью
или скоростью рыскания и может быть измерена гироскопом.
Ее также можно определить по угловой скорости колес с левой
и правой сторон транспортного средства, которые движутся по
дугам разного радиуса и, следовательно, вращаются с разной
скоростью.
Уравнение (4.4) отражает некоторые другие важные характеристики транспортного средства, подобного автомобилю.
Когда υ = 0, то θ˙ = 0, то есть невозможно изменить ориентацию
транспортного средства, когда оно не движется. Как мы знаем
из опыта вождения, чтобы изменить ориентацию, мы должны
двигаться. При угле поворота рулевого колеса γ = π/2 переднее
колесо занимает перпендикулярное положение по отношению
к заднему, из-за чего автомобиль не может двигаться вперед
и модель попадает в область неопределенности.
Отступление 4.1. Система координат транспортного средства
Система координат, которую мы будем использовать, является
общепринятой для всех типов транспортных средств и имеет:
ось x, направленную вперед (продольное движение), ось y, направленную влево (движение вбок), и ось z, направленную вверх.
В аэрокосмической и подводной технике ось x направлена вперед, а ось z часто направлена вниз.

Колесные мобильные роботы 201
Отступление 4.2. Рулевое управление Аккермана
Рудольф Аккерман (1764–1834) – немецкий изобретатель, родившийся в Шнееберге, в Саксонии. Из-за
финансовых проблем он не смог учиться в университете и так же, как его отец, стал шорником. Он работал шорником и дополнительно занимался изготовлением экипажей, а в 1795 году основал типографию
и школу рисования в Лондоне. Издавал популярный
журнал «The Repository of Arts, Literature, Commerce,
Manufactures, Fashion and Politics», в котором публиковались статьи о водяных насосах, газовом освещении и литографских станках, а также иллюстрации
модной одежды и дизайн мебели. Он производил
бумагу для пейзажистов и миниатюристов, запатентовал метод обработки ткани и бумаги для придания
водонепроницаемых свойств и построил фабрику
в Челси для их производства. Похоронен на кладбище Кенсал-Грин в Лондоне.
В 1818 году Аккерманн получил британский патент № 4212 от имени немецкого изобретателя Георга Ланкенспергера на рулевое устройство для карет, обес­
печивающее движение управляемых колес по окружностям с общим центром.
Аналогичная схема была предложена и испытана Эразмом Дарвином (дедушкой
Чарльза Дарвина) в 1760-х. В результате дальнейшего усовершенствования французом Шарлем Жанто появился механизм, который и по сей день используется
в автомобилях и известен как рулевое управление Аккермана.
Подробную информацию об установке пакета
bdsim и его использовании для запуска
моделей блок-схем,
показанных в этой книге, вы найдете в прило­
жении A.
Модель также включает
ограничитель максимальной скорости, ограничитель скорости для моделирования конечного
ускорения и ограничитель
угла поворота рулевого
колеса для моделирования конечного диапазона
поворота рулевого колеса. Их можно изменить,
переопределив параметры в начале файла
lanechange.py.

Выражая (4.1) в мировой системе координат, скорость
транспортного средства в боковом направлении (в направлении y) относительно корпуса равна
ẏ cos θ - ẋ sin θ ≡ 0,

(4.5)

называется неголономным ограничением и будет обсуждаться далее в разделе 4.3.1. Это уравнение невозможно проинтегрировать, чтобы определить связь между x, y и θ.
Мы можем исследовать поведение транспортного средства при простом управлении рулем, с помощью пакета
bdsim для моделирования блок-схем. Модель, показанная
на рис. 4.5, содержит блок Bicycle, реализующий (4.4). Входная скорость является константой, а угол поворота рулевого
колеса – положительным импульсом, за которым следует отрицательный импульс.
Запуск модели имитирует движение транспортного средства

Предполагается, что
папка models находится >>> %run -m lanechange -H
в пути поиска модулей
Python, что имеет место где параметр -H сообщает модели, что она не должна блопри запуске примера
с использованием кироваться в конце выполнения – обычно она блокируется,
rvctool. пока не будут закрыты все окна моделирования. Запуск мо-

202 Глава 4 · Мобильные роботы

Скорость
Блок Bicycle

Траектория движения
транспортного средства

Управление
рулевым колесом
Рис. 4.5. Блок-схема модели models/lanechange, демонстрирующая простой маневр смены
полосы движения. Ступенчатый блок управляет углом поворота рулевого колеса, влево
и вправо (сигнал, показанный в блоке, приведен только для иллюстрации). Автомобиль имеет
стандартную колесную базу L = 1

дели таким способом добавляет объект out с результатами моделирования в глобальное пространство имен.
>>> out
results:
t
x
xnames
ynames

|
|
|
|

ndarray (115,)
ndarray (115, 3)
list = ['x', 'y', '$\\theta$']
list = []

Этот объект имеет ряд атрибутов, таких как out.t (время моделирования) и out.x (состояние системы). В данном случае состояние системы – это конфигурация транспортного средства,
массив, хранящий строку (x, y, θ) для каждого временного шага.
Есть возможность вывести график зависимости конфигурации
от времени.
>>> plt.plot(out.t, out.x); # q в зависимости от времени

как показано на рис. 4.6a, и график изменения положения
транспортного средства в плоскости xy
>>> plt.plot(out.x[:,0], out.x[:,1]); # зависимость x от y

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

4.1.1.1

Движение к точке

Рассмотрим задачу движения к целевой точке (x∗, y∗) на плоскости xy. Будем управлять скоростью робота так, чтобы она была
пропорциональна расстоянию до цели

Колесные мобильные роботы 203

Время (с)
а
б
Рис. 4.6. Простой маневр смены полосы движения с использованием модели models/lanechange, показанной
на рис. 4.5: а) реакция транспортного средства как функция времени; б) движение в плоскости xy, транспортное
средство движется в положительном направлении оси x

,
Этот угол находится
в интервале [-π, π)
и вычисляется
с использованием
функции atan2.

и направлять автомобиль к цели, которая находится под углом
к направлению движения транспортного средства в мировой
системе координат
(4.6)
для чего достаточно простого пропорционального регулятора
γ = Kh(θ∗ ⊝ θ), Kh > 0,

(4.7)

Функция angdiff который поворачивает рулевое колесо в сторону цели. Мы исв пакете Toolbox пользуем оператор ⊝, поскольку θ∗, θ ∈ S1 – это углы, а не дейвычисляет разность
между двумя углами ствительные числа, и результат всегда находится в интервале
и возвращает раз- [-π, π) .
ность в интервале
Блок-схема модели показана на рис. 4.7. Задаем координату
[–π, π). Функция
цели
wrap_mpi_pi
гарантирует, что
угол находится >>> pgoal = (5, 5);
в интервале [–π, π).
Это также кратчай- и первоначальную конфигурацию транспортного средства
шее расстоя­ние по
окружности, как >>> qs = (8, 5, pi / 2);
обсуждалось в разделе 3.3.4.1. Чтобы
и затем запускаем моделирование движения
сложить или вычесть
углы в блок-схеме,
присвойте параме- >>> %run -i -m drivepoint -H
тру mode блока Σ
значение c (от англ. где параметр -i предоставляет модели доступ к пространству
circle – окружность). имен Python, в котором определены qs и pgoal.

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

204 Глава 4 · Мобильные роботы
команда
изменения
скорости

контроль
расстояния

скорость
рулевое
управление

Место­
положение
цели

ошибка
место­
положения

расстояние
до цели

Kv

Блок
Bicycle

Блок
Vehicleplot

xy

угол направления на цель

θ

ошибка
направления

команда
поворота
рулевого
колеса

Kh

управление
рулевым колесом
текущее положение

Рис. 4.7. Блок-схема модели models/drivepoint, которая направляет транспортное средство в заданную точку

гурацию как функцию времени. Построим траекторию транспортного средства на плоскости xy:
>>> q = out.x; # зависимость конфигурации от времени
>>> plt.plot(q[:, 0], q[:, 1]);

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

Рис. 4.8. Движение транспортного средства в заданную точку из различных начальных
конфигураций с использованием модели models/drivepoint, показанной на рис. 4.7. Целевые
координаты (x∗, y∗) равны (5, 5), а параметры регулятора: Kυ = 0.5 и Kh = 1.5

Колесные мобильные роботы 205
Отступление 4.3. atan и atan2
y
В робототехнике часто требуется найти θ по заданному выражению с одной или несколькими
функциями sin θ, cos θ или tan θ. Обратные функatan(s/c)
acos(c)
ции не являются взаимно однозначными и имеют
диапазон значений [-π/2, π/2] соответственно, все
из которых являются подмножествами полного
acos
Q2
Q1 asin atan
acos
x
возможного диапазона [-π, π).
asin atan
Q3
Q4
В Python эти обратные функции реализованы в виде функций math.asin, math.acos, math.atan
и math.atan2. На рисунке показаны тригонометри- atan2(s,c)
asin(s)
ческие квадранты, соответствующие знаку аргумента обратных функций: синим обозначены
положительные значения, красным – отрицательные. В NumPy эти же функции
реализованы как np.arcsin, np.arccos, np.arctan и np.arctan2.
Рассмотрим пример для θ в квадранте 3 (Q3), где sin θ = s = -1/ 2 и cos θ = c =
-1/ 2. Тогда asin(s) → -π/4, acos(c) → 3π/4 и atan(s/c) → π/4 – три допустимых,
но совершенно разных результата. В случае atan(s/c) частное двух отрицательных
чисел положительно, что помещает угол в первый квадрант. Напротив, числитель
и знаменатель передаются функциям math.atan2 и np.arctan2 по отдельности, поэтому их знаки учитываются для определения квадранта угла. В данном случае
atan2(s, c) → -3π/4, что соответствует квадранту 3.

+

+
+
+

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

4.1.1.2
Двухмерные линии
в однородной
форме обсуждаются
в разделе C.2.1.

Движение вдоль линии

Еще одна полезная задача для мобильного робота – следование вдоль линии на плоскости xy, определяемой уравнением
ax + by + c = 0. Мы реализуем эту задачу, используя два регулятора рулевого управления. Первый регулятор
γd = Kd d, Kd > 0
поворачивает робота к линии, чтобы минимизировать расстояние робота от линии

где d > 0, если робот находится справа от линии. Второй регулятор корректирует угол направления, или ориентацию, транспортного средства, чтобы оно двигалось параллельно линии

206 Глава 4 · Мобильные роботы

с помощью пропорционального регулятора
γh = Kh(θ∗ ⊝ θ), Kh > 0.

Закон комбинированного управления
γ = γd + γh = Kd d + Kh(θ∗ ⊝ θ)

поворачивает рулевое колесо так, чтобы направить робота к линии и двигаться вдоль нее.
Блок-схема модели показана на рис. 4.9. В ней мы указываем
целевую линию как трехмерный вектор (a, b, c)
>>> L = (1, -2, 4);

начальную конфигурацию
>>> qs = (8, 5, pi / 2);

и затем запускаем моделирование движения
>>> %run -i -m driveline -H

контроль
расстояния
Kd –
увеличение
расстояния
расстояние

xy

скорость

расстояние от линии

управление
направлением
Блок Vehicle

курс относительно Kh – увеличение
угла курса
линии

Наклон линии

комбинированное
рулевое
управление

Блок
Vehicleplot

θ
курс

Рис. 4.9. Блок-схема модели models/driveline, моделирующей движение транспортного средства вдоль линии.
Параметры линии (a, b, c) задаются в переменной L рабочей области L

Траектория движения транспортного средства для различных начальных конфигураций показана на рис. 4.10.

Колесные мобильные роботы 207

Рис. 4.10. Движение транспортного средства вдоль прямой из нескольких разных начальных
конфигураций с использованием модели models/driveline, показанной на рис. 4.9. Линия
(1, –2, 4) показана пунктиром, а параметры регулятора: Kd = 0.5 и Kh = 1

4.1.1.3

Движение вдоль произвольного пути

Вместо прямой линии можно выбрать произвольную траекторию на плоскости xy и заставить робота двигаться вдоль нее.
Траектория может быть задана одним из планировщиков траекторий, представленных в главе 5.
Простой и эффективный алгоритм следования по траектории – чистое преследование, когда робот движется с постоянной скоростью и преследует точку, находящуюся на траектории
на постоянном удалении впереди. Траектория определяется
набором N точек pi, i = 0, ..., N - 1 и текущей позицией робота pr.
Первый шаг – найти индекс точки на траектории, ближайшей
к роботу

Затем нужно найти первую точку на пути, которая находится
по крайней мере на расстоянии d впереди этой точки

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

208 Глава 4 · Мобильные роботы

Фактически это та же задача, что и в разделе 4.1.1.1, – движение к точке, но на этот раз точка движется. Скорость робота постоянна, поэтому управлять нужно только направлением (4.6)
и (4.7). Блок-схема на рис. 4.11 определяет модель, движущуюся
вдоль произвольного пути, заданного несколькими точками. Ее
можно запустить командой:
>>> %run -m drivepursuit -H

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

скорость
p∗

Местопо­
ложение
цели

точка цели

θ

ошибка
местополо­
жения

угол
направления
на цель

Блок Bicycle

Блок
Vehicleplot

∗

ошибка
направления

Kh

команда поворота
рулевого колеса

θ
p

xy

Рис. 4.11. Блок-схема модели models/drivepursuit, движущейся вдоль произвольной траектории

Рис. 4.12. Чистое преследование вдоль произвольной траектории, заданной точками,
с использованием модели models/drivepursuit, показанной на рис. 4.11. Робот начинает
движение из точки начала координат и «заглядывает» вперед на расстояние d = 5. Параметр
регулятора управления направлением движения Kh = 0.5

Колесные мобильные роботы 209

4.1.1.4

Занятие заданного положения

Последняя задача, которую мы обсудим, – занятие определенного положения (x∗, y∗, θ∗). Регулятор на рис. 4.7 может переместить робота в заданное положение, но не меняет его начальную ориентацию. Для таких задач, как парковка, конечная
ориентация транспортного средства очень важна.
Чтобы контролировать окончательную ориентацию, сначала
перепишем (4.4) в матричной форме

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

где входными данными для модели транспортного средства являются скорость υ и скорость поворота ω, которая достигается
выбором угла поворота рулевого колеса как ◄

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

β = -θ - α,
что дает в результате

,

(4.8)

и предполагает, что целевая система {G} находится перед автомобилем. Линейный закон управления
υ = kρ ρ,
ω = kαα + kβ β
приводит робота в уникальное равновесие при (ρ, α, β) = (0, 0, 0).
Идея, лежащая в основе этого регулятора, заключается в том,

210 Глава 4 · Мобильные роботы

что члены kρ ρ и kαα перемещают робота по прямой в точку {G},
а член kβ β поворачивает робота так, что β → 0. Замкнутая система

стабильна до тех пор, пока
kρ > 0,

kβ < 0,

kα - kρ > 0.

Цель

Рис. 4.13. Полярная система координат для двухколесной модели, движущейся к цели:
ρ – расстояние до цели, β – угол вектора цели относительно мировой системы координат
и α – угол вектора цели относительно системы координат транспортного средства

При реализации всего этого на практике расстояние и угол
направления на цель (ρ, α) можно измерить камерой или лазерным дальномером, а угол β – вывести из α и направления
движения транспортного средства θ, измеренного с помощью
компаса.
Для случая, когда цель находится позади робота, то есть α ∉
(-π/2, π/2), нужно поменять направление движения транспортного средства, изменив знак υ и γ в законе управления. Это изменение зависит от начального значения α и остается постоянным на протяжении всего перемещения.
До сих пор мы описывали регулятор, который приводит
транспортное средство в положение (0, 0, 0). Чтобы переместить

Колесные мобильные роботы 211

робота в произвольную конфигурацию (x∗, y∗, θ∗), мы выполняем изменение координат
x¢ = x - x∗,

y¢ = y - y∗,

θ¢ = θ,

β = β¢ + θ∗.

Этот регулятор реализован с помощью модели, блок-схема
которой показана на рис. 4.14 и наглядно демонстрирует кинематику перехода от двухколесного к одноколесному транспортному средству, отображая угловую скорость ω в угол поворота
рулевого колеса γ. Мы задаем конфигурацию, целевую
>>> qg = (5, 5, pi / 2);

и начальную
>>> qs = (9, 5, 0);

и затем запускам моделирование
>>> %run -i -m driveconfig -H

направление +1 или –1
ρ

α

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

цель достигнута?

β

v∗

в полярные

q

скорость
со знаком

Kp

Блок Bicycle
ω∗

ошибка
положения

atan(ωL/v)
Kα

скорость
поворота ω
со знаком

ω

β∗
β

Kβ
q∗

X∗
Y∗
(x∗, y∗, θ∗)

θ∗

Блок Vehicle

|v|L

(x∗, y∗, 0)

Демультиплексор

0

Рис. 4.14. Блок-схема модели models/driveconfig, которая перемещает автомобиль в заданное положение.
Начальная и конечная конфигурации задаются переменными рабочего пространства qs и qg соответственно

Как и прежде, результаты моделирования сохраняются в переменной out, и их можно отобразить на графике
>>> q = out.x; # зависимость конфигурации от времени
>>> plt.plot(q[:, 0], q[:, 1]);

чтобы увидеть траекторию движения транспортного средства
в плоскости xy. На рис. 4.15 показаны траектории движения для
ряда начальных конфигураций. Транспортное средство дви-

212 Глава 4 · Мобильные роботы

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

Регулятор основан
на двухколесной
модели (4.4), однако
модель транспортного средства,
реализуемая классом Bicycle, имеет
дополнительные
жесткие нелинейности, включая
ограничения угла
поворота рулевого
колеса и скорости.
Нарушение этих
ограничений может
привести к ошибке
регулятора конфигурации.

Рис. 4.15. Движение транспортного средства в заданное положение из различных начальных
конфигураций с использованием модели models/driveconfig, показанной на рис. 4.14.
Целевая конфигурация (5, 5, π/2) показана пунктиром, а параметры регулятора: Kρ = 15, Kα = 5
и Kβ = –2. Обратите внимание, что в некоторых случаях робот занимает заданное положение,
двигаясь задом

4.1.2

Автомобиль с дифференциальным управлением

Транспортное средство, подобное автомобилю, имеет управляемые колеса, имеющие сложное механическое устройство.
Дифференциальное рулевое управление несколько упрощает
эту сложность и обеспечивает независимое управление скоростью колес с каждой стороны транспортного средства: если скорости не равны, то транспортное средство повернет в сторону
колес, вращающихся с меньшей скоростью. Простейшие роботы
с дифференциальным управлением имеют два ведущих колеса
и переднее и/или заднее опорное колесо для обеспечения устойчивости. Более крупные роботы с дифференциальным управлением, такие как на рис. 4.16, используют по паре колес с каждой
стороны, причем каждая пара приводится в движение одним
и тем же двигателем через механическую трансмиссию. Очень
крупные машины с дифференциальным управлением, такие как
бульдозеры и танки, иногда используют гусеницы вместо колес.
Для моделирования поведения низкоскоростного транспортного средства с дифференциальным управлением широко
используется одноколесная кинематическая модель моноцикла, показанная на рис. 4.17. Моноцикл имеет одно колесо, расположенное в центре масс всех колес. ►

Из Sharp (1896)

Колесные мобильные роботы 213

Рис. 4.16. Робот Clearpath Robotics™ Husky™ с дифференциальным управлением
(изображение Тима Барфута)

Положение транспортного средства представлено системой
координат {B} (рис. 4.17) с осью x, направленной вперед по ходу
движения транспортного средства, и началом координат в центре
тяжести колеса моноцикла. Конфигурация транспортного средства представлена обобщенными координатами q = (x, y, θ) ∈ 𝒞,
где 𝒞 ⊂ ℝ2 × S1 – конфигурационное пространство. Скорость транспортного средства равна υ в направлении оси x, и мы снова предполагаем, что колеса катятся без бокового проскальзывания
υ = (υ, 0).

B

МЦП

Рис. 4.17. Модель моноцикла
(одноколесная модель)
четырехколесного транспортного
средства с дифференциальным
приводом. Четырехколесное
транспортное средство
показано светло-серым цветом,
а эквивалентная одноколесная
модель – темно-серым. Система
координат кузова транспортного
средства показана красным цветом,
а мировая система координат –
черным. Левое и правое колеса
движутся по дугам окружностей
вокруг мгновенного центра
поворота (МЦП) с радиусами RL и RR
соответственно. Для управления
траекторией движения можно
использовать альтернативную
систему координат {B¢}

мгновенный центр поворота

скорость

ширина колеи

214 Глава 4 · Мобильные роботы

Автомобиль движется по криволинейной траектории с цент­
ром в мгновенном центре поворота (МЦП). Левые колеса катятся со скоростью υL и движутся по дуге с радиусом RL. Аналогично
правые колеса катятся со скоростью υR и движутся по дуге с радиусом RR. Угловая скорость системы координат{B} равна
,
и поскольку RR = RL + W, мы можем записать скорость поворота как
(4.9)
в терминах дифференциальной скорости и ширины колеи W.
Таким образом, уравнение движения приобретает вид
ẋ = υ cos θ
ẏ = υ sin θ
(4.10)
где υ = 1/2(υR + υL) и υΔ = υR - υL – средняя и дифференциальная
скорости соответственно. При заданной скорости υ и угловой
скорости θ̇ скорости вращения колес определяются как

(4.11)
Эта модель имеет как сходства, так и различия с двухколесной моделью (4.4). Скорость поворота одноколесного транспортного средства прямо пропорциональна υΔ и не зависит от
скорости υ – моноцикл может поворачивать, даже не двигаясь вперед. Для четырехколесного варианта, показанного на
рис. 4.17, оси колес не пересекают МЦП, поэтому при повороте
автомобиля векторы скорости колес υL и υR не касаются траектории – имеется компонент в боковом направлении, который
нарушает ограничение проскальзывания. Это приводит к проскальзыванию (или пробуксовке), которое особенно заметно
при повороте автомобиля на месте, поэтому дифференциальное рулевое управление также называют бортовым управлением. Мы можем записать выражение для скорости автомобиля
вдоль оси y в мировой системе координат как

Колесные мобильные роботы 215

ẏ cos θ - ẋ sin θ ≡ 0,

(4.12)

что является неголономным ограничением. Важно отметить,
что способность разворачиваться на месте не делает транспортное средство голономным и принципиально отличается от
способности двигаться в произвольном направлении, которую
мы обсудим далее.
Если переместить систему координат транспортного средства в {B¢}, как показано на рис. 4.17, и проигнорировать ориентацию, то (4.10) можно переписать в матричной форме как
,
и если положить, что a ≠ 0, то эту форму можно инвертировать:
(4.13)
чтобы получить скорость движения вперед и скорость поворота
точки начала координат {B¢} при заданной произвольной скорости (ẋ, ẏ).

4.1.3

Меканум –
шведская компания,
в стенах которой
Бенгт Илон
изобрел это колесо
в 1973 году. На
изобретение был
получен патент
США за номером
3876255 (его срок
действия уже истек).

Всенаправленное транспортное средство

Транспортные средства, обсуждавшиеся выше, имеют неголономное ограничение на боковое движение, требующее выполнения сложных маневров для достижения целевой конфигурации. Альтернативные конструкции колес, как, например, на
рис. 4.18, устраняют это ограничение и обеспечивают возможность свободного движения в любую сторону. Еще более радикальным решением является сферическое колесо, показанное
на рис. 4.20.
В этом разделе мы обсудим меканум, колесо Илона или
шведское колесо,
показанное на рис 4.18б и схематически
на рис. 4.19. Оно состоит из ряда роликов, установленных по
окружности колеса, оси которых расположены под углом α к оси
колеса. Темным цветом на рисунке выделен ролик, находящийся в нижней части колеса и в данный момент соприкасающийся
с землей. Ролики имеют бочкообразную форму, поэтому в любой момент времени имеют очень небольшое пятно контакта
с землей.
На рис. 4.19 показана система координат колеса {W} с осью x,
направленной в сторону движения. Вращение колеса вызывает
поступательную скорость Rϖx̂ w, где R – радиус колеса, а ϖ – скорость его вращения. Однако поскольку ролик может свободно

216 Глава 4 · Мобильные роботы

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

(4.19)
где υw – скорость вращения колеса, а υr – скорость качения вбок.
Выражая скорость в виде суммы компонентов υ = υx x̂w + υy ŷw,
можно найти скорость качения υr = υy /sin α, и, подставляя ее на
место первого члена, можно найти искомую скорость колеса
υw = υx - υy cot α.

а

(4.15)

б

Рис. 4.18. Два типа всенаправленных колес, обратите внимание на различную ориентацию
роликов: а) позволяет колесу катиться вбок (изображение предоставлено VEX Robotics);
б) позволяет колесу двигаться вбок (изображение предоставлено Nexus Robotics)

Тогда требуемая скорость вращения колеса будет равна ϖ =
υw /R. Если α = 0, то υw не определена, потому что оси роликов
параллельны оси колеса, и колесо не будет обеспечивать сцеп­
ление с поверхностью. Если α = π/2, как на рис. 4.18а, то колесо
допускает боковое качение, но не движение вбок, потому что
сцепление между υw и υy равно нулю.
Одно колесо Илона не позволяет управлять направлением
бокового качения, но в случае трех или более колес Илона, расположенных соответствующим образом, движение в направлении качения любого одного колеса будет передаваться другими

Колесные мобильные роботы 217

колесами. На рис. 4.21 показано транспортное средство с четырьмя колесами Илона. Его положение представлено системой
координат кузова {B} с осью x в направлении к носу транспортного средства и с началом в центроиде четырех колес. Конфигурация транспортного средства представлена обобщенными
координатами q = (x, y, θ) ∈ 𝒞, где 𝒞 ⊂ ℝ2 × S1. Оси роликов колес
ортогональны, поэтому когда колеса не вращаются, транспортное средство не может ни катиться вбок, ни поворачиваться –
оно фактически заблокировано.

R

ось колеса

е

ни
я

ни

че

ка

ь

ос

ка

ли

ро

ле

Рис. 4.20. Шаровой робот Rezero,
разработанный в ETH Zürich (фотография
Петера Фанкхаузера)

ав

пр

на

Рис. 4.19. Схема колеса
Илона в плане. Ролики
показаны серым цветом.
Темно-серый ролик
соприкасается с землей.
Зеленая стрелка указывает
направление качения

218 Глава 4 · Мобильные роботы

а

б

Рис. 4.21. а) Робот KUKA youBot®, имеющий четыре механических колеса (фотография
предоставлена магазином youBot); б) схема движения транспортного средства с четырьмя
колесами Илона в конфигурации youBot

Четыре пятна контакта колес, обозначенных черными точками, имеют координатные векторы Bpi, i = 0, 1, 2, 3. Для желаемой
скорости тела BυB и скорости поворота BωB скорость в каждой
точке контакта равна
υi = BυB + BωB ẑ B × Bpi,

B

далее применяем (4.14) и (4.15) для определения скоростей вращения колес ϖi и отмечаем при этом, что α имеет противоположный знак для колес 1 и 3 в (4.14).
Колесо Илона применяется в робототехнике (рис. 4.21), но
не получило широкого распространения. Сами колеса дороже
обычных, а из-за их повышенной сложности они имеют большее количество точек износа и чаще выходят из строя. Лучше
всего колеса Илона работают на твердой гладкой поверхности,
что делает их более подходящими для применения в закрытых
помещениях, например в складской логистике. С другой стороны, они плохо подходят для использования на открытом воздухе, где поверхность может быть мягкой или неровной, а ролики
могут засоряться мусором. Чтобы получить всенаправленную
базу, необходимо не менее трех колес Илона, расположенных
в вершинах равностороннего треугольника с осями роликов,
пересекающимися в центре.

4.2

Воздушные роботы

« Чтобы летать, достаточно не касаться земли.
– Дуглас Адамс

Воздушные роботы или беспилотные летательные аппараты
(БПЛА) становятся все более распространенными и имеют самые разные формы и размеры, как показано на рис. 4.22. Они

Воздушные роботы 219

широко применяются в военной сфере, спасательных операциях, метеорологических наблюдениях, робототехнических
исследованиях, коммерческой фотографии, а также как хобби
любителей. В последнее время бурно развивается класс летательных аппаратов, известный как летательные микроаппараты (micro air vehicles, MAV), размеры которых не превышают
15 см по всем измерениям. БПЛА с фиксированным крылом
в принципе похожи на пассажирские самолеты с крыльями, которые обеспечивают подъемную силу, воздушным винтом или
реактивной струей для создания прямой тяги и управляющими поверхностями для маневрирования. Винтокрылые БПЛА
имеют различные конфигурации, включая традиционную конструкцию вертолета с несущим и хвостовым винтом, соосную
с соосными винтами противоположного вращения и квадрокоптеры. Преимущество винтокрылых БПЛА в том, что они могут взлетать вертикально и зависать.

а

б

в

г

Рис. 4.22. Воздушные роботы: а) беспилотный летательный аппарат (БПЛА) Global Hawk
(изображение предоставлено NASA); б) летательный микроаппарат (MAV) (изображение
предоставлено AeroVironment, Inc.); в) вертолет Ingenuity на Марсе, имеет два соосных
винта, вращающихся в противоположных направлениях (изображение предоставлено NASA);
г) квадрокоптер с четырьмя винтами и блоком сенсорной и управляющей электроники
в середине (изображение предоставлено 3D Robotics)

Воздушные роботы отличаются от наземных рядом важных
признаков. Во-первых, они имеют 6 степеней свободы, и их
конфигурация q ∈ 𝒞, где 𝒞 ⊂ ℝ3 × (S1)3 – это трехмерное конфигурационное пространство. Во-вторых, они приводятся в действие силами, то есть их модель движения выражается в тер-

220 Глава 4 · Мобильные роботы

минах сил, моментов и ускорения, а не скоростей, как в случае
с наземными моделями, поэтому для их описания мы будем
использовать динамическую, а не кинематическую модель.
Подводные роботы имеют много общего с воздушными и могут рассматриваться как транспортные средства, летающие под
водой. Среди них существуют подводные эквиваленты моделей
с неподвижным крылом и с винтами. Главные различия под водой – сила плавучести, направленная вверх, более значительные силы сопротивления и присоединенная масса.
В этом разделе мы исследуем модель квадрокоптера, показанного на рис. 4.22г. Квадрокоптеры широко доступны как
в виде коммерческих продуктов, так и в виде проектов с открытым исходным кодом. По сравнению с самолетами, они
обладают более высокой маневренностью и могут безопасно
летать в помещении, что делает их пригодными для лабораторных исследований и использования любителями. По сравнению с обычными вертолетами, имеющими большой несущий
и дополнительный хвостовой винты, квадрокоптеры проще
в управлении, не нуждаются в сложном механизме перекоса
и проще в моделировании и управлении.
Обозначения модели квадрокоптера показаны на рис. 4.23.
В системе координат корпуса {B} ось z направлена вниз в соответствии с принятыми в аэрокосмической отрасли соглашениями. Квадрокоптер оснащен четырьмя пронумерованными
винтами 0–3, установленными на концах диагоналей. Также
большой популярностью пользуются шести- и восьмивинтовые аппараты, в которых дополнительные винты обеспечивают
большую грузоподъемность. Описанную далее модель можно
обобщить для случая N винтов, где N – четное число.

front

Рис. 4.23. Схема устройства
квадрокоптера. Здесь изображены
четыре винта, их векторы тяги
и направления вращения. Начало
системы координат корпуса
{B} находится в центре масс
квадрокоптера. Винты 0 и 2,
показанные синим цветом, вращаются
против часовой стрелки (вид сверху),
а винты 1 и 3, показанные красным, –
по часовой стрелке

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

Воздушные роботы 221

мента. Скорость вращения ротора обозначается как ϖi, а вектор
тяги направлен вверх
Ti = bϖi2,
Диск, описываемый
концами лопастей
ротора. Его радиус равен длине
лопасти.
При полете близко
к поверхности
земли винтокрылый
аппарат испытывает
влияние эффекта
воздушной подушки – увеличенной
подъемной силы
за счет воздушной
подушки, образующейся под ним.
У вертолетов этот
эффект проявляется на высотах
до 2–3 радиусов
винта, тогда как для
аппаратов с несколькими винтами
влияние эффекта
простирается до высоты 5–6 радиусов
винта (Sharf et al,
2014).

i ∈ {0, 1, 2, 3}

(4.16)

в отрицательном направлении оси z, где b > 0 – константа подъемной силы, зависящая от плотности воздуха, куба радиуса
диска винта, количества лопастей винта и длины хорды лопасти.
Динамика движения летательного аппарата в мировых координатах определяется вторым законом Ньютона
(4.17)
где υ – скорость центра масс транспортного средства в мировой
системе координат, g – ускорение свободного падения, m – общая масса транспортного средства, B – аэродинамическое трение, а T – общая восходящая тяга:
(4.18)

Отступление 4.4. Маховое движение лопастей винта
Лопасти винта на винтокрылом аппарате обладают удивительной динамикой. При полете против ветра лопасть, движущаяся вперед, испытывает бо́льшую подъемную силу, чем лопасть,
движущаяся назад. Это эквивалентно крутящему моменту вокруг оси, направленной против ветра, и лопасти несущего винта ведут себя как гироскоп (см. раздел 3.4.1.1), в результате чего
плоскость диска винта поднимается на величину, пропорциональную кажущейся или чистой скорости ветра, чему противостоят жесткость лопасти на изгиб и изменение подъемной силы
как функции изгиба лопасти. Наклонный диск винта заставляет
компонент вектора тяги замедлять поступательное движение
аппарата, и эта сила, зависящая от скорости, действует как сила
трения. Это известно как маховое движение лопасти и является
важной характеристикой лопастей на всех типах винтокрылых
аппаратов.

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

222 Глава 4 · Мобильные роботы

Попарная разность тяги винтов вызывает вращение транспортного средства. Крутящий момент относительно оси x
транспортного средства, момент качения, создается моментами
τx = dT3 - dT1,
где d – расстояние от оси винта до центра масс. Это можно выразить через скорость винта, подставив (4.16)
τx = db(ϖ32 - ϖ12),

(4.19)

и аналогично для оси y крутящий момент тангажа равен
τy = db(ϖ02 - ϖ22).

(4.20)

Крутящий момент, прилагаемый к каждому винту двигателем, противодействует аэродинамическому сопротивлению
Q i = k ϖi2,
где k > 0 зависит от тех же факторов, что и b. Этот момент создает реактивный момент на планере, который вращает планер вокруг вала винта в направлении, противоположном его вращению. Суммарный реактивный момент относительно оси z равен
τz = Q0 - Q1 + Q2 - Q3
= k(ϖ02 - ϖ12 + ϖ22 - ϖ32),

(4.21)

где разные знаки обусловлены разным направлением вращения винтов. Крутящий момент рыскания можно создать просто
путем скоординированного управления скоростями вращения
всех четырех винтов.
Суммарный крутящий момент, приложенный к планеру согласно (4.19)–(4.21), равен τ = (τx, τy, τz)T, а вращательное ускорение определяется уравнением движения Эйлера из (3.14)
Jω̇ = -ω × Jω + τ,

(4.22)

где J – тензор 3×3 инерции транспортного средства, а ω – вектор угловой скорости. Движение квадрокоптера определяется
интегрированием уравнений прямой динамики (4.17) и (4.22).
Силы и моменты на планере, заданные соотношениями
(4.18)–(4.21), можно записать в матричной форме

(4.23)

Воздушные роботы 223

Они являются функциями квадратов скоростей винтов. Мат­
рица смешивания M постоянна и имеет полный ранг, поскольку
b, k, d > 0, и может быть инвертирована

(4.24)

Динамика вращения
имеет передаточную
функцию второго
порядка
β(s)/τy(s) = 1/(Js2 + Bs),
где J – момент
инерции вращения,
B – аэродинамическое демпфирование, которое обычно
довольно мало, а s –
оператор Лапласа.
Для регулирования
системы второго
порядка требуется
пропорциональнодифференциальный
регулятор, потому
что необходимо
учитывать как
скорость изменения, так и величину
сигнала.
Член β˙∗ обычно
игнорируется.

для определения скоростей вращения винтов, необходимых
для приложения заданного момента τ и тяги T к планеру самолета.
Для управления транспортным средством обычно используется вложенная структура, описывающая тангаж и поступательное движение вдоль оси x. Во внутреннем цикле используется
пропорционально-дифференциальный регулятор,
вычисляющий необходимый крутящий момент на планере на основе
ошибки между желаемым и расчетным углами тангажа, а также
скорости их изменения. Коэффициенты усиления Kτ,p и Kτ,d
определяются классическими методами проектирования систем управления путем аппроксимации приближенной динамической модели и затем подстраиваются для достижения
оптимальных характеристик. Расчётный угол тангажа β̂ будет
получен из инерциальной навигационной системы, как обсуждалось в разделе 3.4, а фактический угол тангажа β̇ˆ – из гироскопических датчиков. Требуемые скорости вращения винта затем
определяются с помощью формулы (4.24).
τy∗ = Kτ,p(β∗ - β̂) + Kτ,d(β̇∗ - β̇ˆ ).

(4.25)

Рассмотрим систему координат {B¢}, прикрепленную к аппарату, начало координат которой совпадает с началом координат
системы {B}, но с осями x и y, расположенными в горизонтальной плоскости и параллельными земле. Вектор тяги параллелен
оси z системы {B}, и при опускании носа аппарата поворот вокруг оси y на угол β создает силу

который имеет компонент
fx = -T sin β ≈ -Tβ,

B¢

ускоряющий аппарат в направлении -B¢x, если исходить из предположения, что угол β имеет небольшую величину. Управлять

224 Глава 4 · Мобильные роботы

скоростью в этом направлении можно с помощью пропорционального закона управления.
fx∗ = mKf (B¢υx∗ - B¢υ̂x),

B¢

где Kf > 0 – коэффициент усиления. ► Объединяя эти два уравнения, получаем искомый угол тангажа
β∗ ≈ -

Kf (B¢υx∗ - B¢υ̂x),

(4.26)

необходимый для достижения желаемой скорости поступательного движения. Используя (4.25), можно вычислить требуемый
крутящий момент по тангажу, а затем, используя (4.24), – требуемые скорости вращения винтов. Для аппарата, находящегося в вертикальном равновесии, полная сила тяги равна весу, то
есть m/T ≈ 1/g.
Расчетную скорость аппарата Bυ̂x можно получить с помощью
инерциальной навигационной системы, как описано в разделе 3.4, или GPS-приемника. Если желаемое местоположение
транспортного средства в плоскости xy мировой системы координат равно 0p ∈ ℝ2, то желаемая скорость определяется законом пропорционального управления
υ∗ = Kp(0p∗ - 0p̂)

0

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

(4.27)

с учетом ошибки между желаемым и фактическим положениями. Желаемая скорость в плоскости xy системы {B¢} равна
υ̂ = ⊝0ξ B¢r (γ) · 0υ,

B

то есть является функцией угла рыскания

На рис. 4.24 показана блок-схема модели управления квад­
рокоптером. Двигаясь слева направо и сверху вниз, мы получаем искомое местоположение квадрокоптера в мировой
системе координат. Ошибка местоположения переносится из
мировой системы координат в систему координат тела и становится искомой скоростью. Регулятор скорости реализует уравнение (4.26) и его эквивалент для оси крена и выдает требуемые
углы тангажа и крена квадрокоптера. Регулятор ориентации –
это пропорционально-дифференциальный регулятор, который
определяет необходимые крутящие моменты тангажа и крена
для достижения этих углов на основе обратной связи по текущей
ориентации и скорости ее изменения. Блок управления рыс­
канием определяет погрешность угла курса и реализует про-

Эта модель является
иерархической
и организована по
подсистемам.
Используя условные
обозначения координат, показанные
на рис. 4.23, для
движения в направлении x необходим
отрицательный
угол тангажа, а для
движения в направлении y необходим
положительный
угла крена, поэтому
коэффициенты усиления имеют разные
знаки для контуров
крена и тангажа.

Воздушные роботы 225
Скорость
вращения винта
(абс. значение)

контроль
местоположения

желаемое
местоположение

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

управление скоростью

силы,
действующие
на планер

желаемая
высота

|ω|

скорость
вращения
винта

x∗

ошибка
местополо­
жения в {0}

(x∗, y∗)
фактическое
местоположение

Преобразование
в {B}

управление
скоростью

предельные углы
тангажа и крена

управление
высотой
Управление
винтами

SO(2)

Динамика
многовинтовой
модели

Анимация
многовинтовой
модели

x – словарь состояний

y∗
(x, y)

['trans']
Ориентация

угол рыскания

Местоположение

['rot']

SO(2)(yaw).inv()

график
изменения
рыскания

управление
рысканием

Блок Main
время

управление
высотой

график изменения
высоты

Рис. 4.24. Блок-схема модели models/quadrotor для моделирования квадрокоптера в замкнутом контуре. Сигнал x
отображает состояние квадрокоптера: положение, ориентацию, скорость и скорость изменения ориентации. Аппарат
отрывается от земли и летит по кругу на постоянной высоте, рыская вокруг собственного центра

порционально-дифференциальный регулятор для вычисления
необходимого крутящего момента рыскания, который достигается за счет ускорения одной пары винтов и замедления другой.
Высота контролируется пропорционально-дифференциальным регулятором
T = Kp(z∗ - ẑ) + Kd (ż∗ - żˆ ) + Tw,

Этот блок может
моделировать летательный аппарат
с N винтами, значение по умолчанию
N = 4.

который определяет среднюю скорость вращения винтов. Tw =
mg – вес аппарата и пример управления с прямой связью, используемого здесь для сопротивления силе тяжести, которая
является постоянным возмущением для контура управления
высотой. Вместо управления с прямой связью в контуре управления высотой можно было бы использовать очень высокое
значение Kp, что привело бы к потере устойчивости, или пропорциональный интегрально-дифференциальный (ПИД) регулятор, которому может потребоваться много времени для
устранения установившейся ошибки интегральным членом,
что привело бы к запаздыванию регулирующих воздействий.
Мы еще вернемся к проблеме компенсации силы тяжести в главе 9 в контексте робота-манипулятора.
Блок управления винтами объединяет три крутящих момента и вертикальную тягу и использует (4.24) для определения
скоростей вращения винтов. Здесь применяются ограничения
скорости вращения. Необходимые значения подаются на вход
блока,
который реализует прямую динамику, интегрируя
(4.17)–(4.22) для определения местоположения, ориентации,

226 Глава 4 · Мобильные роботы

скорости и скорости изменения ориентации. Выход этого блока – вектор состояния с 12 элементами x = (0p, 0Γ, Bṗ, BΓ̇ ). Как
принято в аэрокосмических приложениях, мы представляем
ориентацию Γ ∈ (S1)3 и скорость изменения ориентации Γ̇ ∈ ℝ3
в терминах углов крена-тангажа-рыскания ZYX. Обратите внимание, что местоположение и ориентация указаны в мировой
системе координат, а скорости – в системе координат тела. ►
Симуляцию можно запустить командой
>>> %run -m quadrotor -H

которая загружает набор параметров квадрокоптера по умолчанию и отображает анимацию и различные временные диаграммы в отдельных окнах. Аппарат взлетает и летит по кругу,
медленно вращаясь вокруг своей оси z. Снимок экрана показан
на рис. 4.25. Временная диаграмма сохраняется в переменной
out:
>>> t = out.t; x = out.x;
>>> x.shape
(213, 12)

а состояние x содержит по одной строке с 12 элементами для
каждого временного шага. Мы можем построить график зависимости проекции местоположения на плоскости от времени
с помощью
>>> plt.plot(t, x[:, 0], t, x[:, 1]);

Рис. 4.25. Один из кадров, созданных моделью models/quadrotor, показанной на рис. 4.24.
Диски винтов показаны кругами, а их ориентация учитывает эффект махового движения
лопастей. Маркер на поверхности земли показывает проекцию центроида аппарата

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

Выход блока
фактически
представляет
собой словарь,
содержащий
элементы:
"x" = (0p, 0Γ, Bṗ, BΓ̇ ),
"rot" = (0Γ, BΓ̇ )
и "trans" = (0p, Bṗ).

Дополнительные темы 227

ступательную скорость для ее исправления. Для достижения
требуемой скорости корректируется тангаж крена аппарата так,
чтобы составляющая тяги аппарата действовала в горизонтальной плоскости и создавала силу, ускоряющую его. Для достижения требуемых углов тангажа и крена к планеру прикладываются моменты посредством изменения тяги винтов, что, в свою
очередь, требует управления скоростью вращения винтов. При
таком движении аппарата общая тяга должна быть увеличена
так, чтобы вертикальная составляющая по-прежнему уравновешивала силу тяжести. При приближении к цели аппарат должен развернуться в противоположном направлении, чтобы составляющая тяги замедлила движение.
Этот переход от поступательного движения к вращательному
является следствием малоприводности: мы можем управлять
скоростью вращения всего четырех винтов, тогда как конфигурационное пространство шестимерно. В конфигурационном
пространстве мы не можем двигаться в направлениях x и y, но
можем двигаться в направлениях тангажа или крена, что приводит к движению в направлениях x и y. Из-за малоприводности (недостаточности управления) приходится маневрировать.
Углы тангажа и крена помогают управлять поступательным
движением, но не могут устанавливаться независимо, если требуется сохранять постоянное местоположение.

4.3

Дополнительные темы

4.3.1

Неголономные и малоприводные системы

Понятие конфигурационного пространства мы ввели в разделе 2.4.9, и теперь, после обсуждения нескольких типов мобильных роботизированных платформ, будет полезно вернуться
к нему. Распространенные транспортные средства, такие как
автомобили, суда на воздушной подушке, корабли и самолеты, способны эффективно двигаться вперед, но не способны
мгновенно двигаться вбок. Это очень разумный компромисс,
упрощающий конструкцию и отвечающий наиболее востребованным движениям. Боковое движение для таких задач, как
парковка автомобиля, швартовка корабля или посадка самолета, возможно, но требует некоторых сложных маневров. Впрочем, люди вполне могут освоить этот навык.
В разделе 2.4.9 было показано конфигурационное пространство поезда. Для полного описания всех составляющих его компонентов необходимо задать всего одну обобщенную координату: ее расстояние q вдоль пути от некоторой точки отсчета.

228 Глава 4 · Мобильные роботы

У поезда одна степень свободы, а его конфигурационное пространство – 𝒞 ⊂ ℝ. Поезд оснащен одним двигателем, или актуа­
тором, для перемещения вдоль пути, и это число равно числу
степеней свободы. Мы говорим, что поезд – полноприводный
(fully actuated).
Теперь рассмотрим судно на воздушной подушке, движущее­
ся по плоской поверхности. Для описания его конфигурации
необходимо указать три обобщенные координаты: местоположение на плоскости xy и курсовой угол. Судно имеет три степени свободы, а его конфигурационное пространство – 𝒞 ⊂ ℝ2 × S1.
Судно на воздушной подушке имеет два толкающих винта, оси
которых параллельны, но неколлинеарны. Сумма их тяг создает поступательную силу, а разность – рыскающий момент для
управления. Число приводов – два, меньше, чем число степеней
свободы dim 𝒞 = 3. Такие системы мы называем малоприводными (underactuated). Малоприводность накладывает существенные ограничения на способ, которым судно может двигаться.
В любой момент времени мы можем контролировать прямое
(параллельное векторам тяги) ускорение и ускорение рыскания судна на воздушной подушке, но боковое (или поперечное)
ускорение равно нулю, потому что отсутствует привод, который создавал бы поперечную тягу. Тем не менее при умелом
маневрировании, как в случае с автомобилем, судно на воздушной подушке может следовать по траектории, которая приведет
его в точку, расположенную в стороне от точки старта. В трехмерном пространстве конфигурации судна на воздушной подушке это означает, что в любой точке существуют определенные направления, в которых ускорение невозможно. Мы можем
достичь точек в этих направлениях, но не напрямую, а только
окольным путем.
Конфигурация любого воздушного или подводного аппарата полностью описывается шестью обобщенными координатами – местоположением и ориентацией в трехмерном пространстве. В конфигурационном пространстве 𝒞 ⊂ ℝ3 × (S1)3, где
ориентация выражается в виде трехуглового представления.
Поскольку dim 𝒞 = 6, аппараты имеют шесть степеней свободы.
Квадрокоптер имеет четыре привода (четыре винта, создающих
тягу), и это меньше числа степеней свободы, что делает квадрокоптеры малоприводными. Управляя четырьмя винтами, можно реализовать движение вверх/вниз и изменение углов крена,
тангажа и рыскания, но нет возможности напрямую реализовать движение вперед/назад или влево/вправо. Чтобы получить
доступ к этим степеням свободы, необходимо выполнить ма­
невр: уменьшить тангаж так, чтобы вектор тяги создал горизонтальную составляющую силы, разогнаться вперед, увеличить
тангаж, чтобы вектор тяги создавал горизонтальную составляющую силы для замедления, а затем выровнять аппарат.

Дополнительные темы 229

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

Для вертолета практическую пользу представляют только
четыре из шести степеней свободы: вверх/вниз, вперед/назад, влево/вправо и рыскание. То есть вертолету требуется как
минимум четыре привода: несущий винт создает вектор тяги,
величина которого может управляться изменением шага, а направление может задаваться поперечным и продольным наклонами винта. Рулевой винт создает момент рыскания. Без
привода остаются две степени свободы – углы крена и тангажа. Конструктивно эта проблема решена за счет влияния силы
тяжести, удерживающей вертолет подобно килю надводного
корабля, – без силы тяжести вертолет не сможет летать. Самолет с неподвижным крылом движется вперед очень эффективно и тоже имеет четыре привода: тяга двигателя обеспечивает ускорение в прямом направлении, а элероны, руль высоты
и руль направления создают моменты крена, тангажа и рыс­
Чтобы получить доступ к степеням
кания соответственно.
свободы, для которых не предусмотрены отдельные приводы,
таким как перемещение вверх/вниз и влево/вправо, самолет
должен изменять угол тангажа или рыскания во время движения вперед.
Одно из преимуществ малоприводности – уменьшенное
количество приводов. На практике это означает уменьшение
сложности конструкции и экономию средств и веса. Как следствие в любой точке конфигурационного пространства сущест­
вуют определенные направления, в которых аппарат не может
двигаться. Полноприводные конструкции возможны, но мало
распространены. Например, подводный робот RangerBot, показанный на рис. 4.2в, имеет шесть степеней свободы и шесть
приводов. Они могут прилагать к аппарату произвольные силы
и крутящие моменты, позволяя ускоряться в любом направлении или поворачиваться вокруг любой оси.
Четырехколесный автомобиль имеет много общего с судном
на воздушной подушке, рассмотренным выше. Он движется по
плоской поверхности, а его конфигурация полностью описывается положением в плоскости xy и углом поворота. Он имеет три
степени свободы, а его конфигурационное пространство определяется как 𝒞 ⊂ ℝ2 × S1. Автомобиль имеет два привода: двигатель
обеспечивает движение вперед/назад, а рулевой механизм – изменение направления движения. Автомобиль, как и судно на
воздушной подушке, – малоприводный. Из опыта эксплуатации автомобилей мы знаем, что можем двигаться прямо только
в определенных направлениях и иногда должны совершать маневры, чтобы достичь цели, как показано на рис. 4.26.
Транспортное средство с дифференциальным, или бортовым,
управлением, например танк, тоже является малоприводным –
у него всего два привода, по одному на каждую гусеницу. Этот
тип транспортного средства может разворачиваться на месте,

230 Глава 4 · Мобильные роботы

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

а

б

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

Мы часто мечтаем иметь возможность ехать на машине боком, однако обычное колесо дает реальное преимущество в поворотах: боковое трение между колесами и дорогой создает
центростремительную силу, для создания которой в противном
случае потребовался бы дополнительный привод. Судно на воздушной подушке во многом похоже на автомобиль, но его можно толкать вбок, чего нельзя сделать с обычным автомобилем.
Боковое трение – отличительная особенность автомобиля.
Невозможность скользить вбок является ограничением (ограничением качения) скорости автомобиля, как и малопривод­
ность. Автомобиль с одним или несколькими ограничениями
скорости, вызванными малоприводностью или ограничением
качения, называется неголономной системой. Отличительная
черта таких систем состоит в том, что они не всегда могут перейти из одной конфигурации в другую напрямую. Иногда приходится выбирать более длинный непрямой путь, который мы
называем маневром. Автомобиль имеет ограничение скорости
из-за своих колес и также малоприводности.
Голономное ограничение, напротив, ограничивает возможные конфигурации, которых может достичь система, – его можно выразить в виде уравнения, записанного в терминах переменных конфигурации.
Неголономное ограничение, такое
как (4.5) и (4.12), – это ограничение скорости (или ускорения)
системы в конфигурационном пространстве – его можно выразить только через производные конфигурационных перемен-

Управление судами
на воздушной
подушке, а также
воздушными и подводными аппаратами осуществляется
силами, поэтому
в данном случае
ограничения накладываются на ускорение транспортного
средства, а не на
скорость.
Например, фиксация
конца 10-суставной
руки робота вводит
шесть голономных
ограничений (на
местоположение
и ориентацию),
поэтому рука будет
иметь только 4
степени свободы.

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

ных. Неголономное ограничение не ограничивает возможные
конфигурации, которые может принять система, но исключает
мгновенную скорость или ускорение в определенных направлениях, как, например, в направлении черных отрезков линий
на рис. 4.26.
В теории управления имеется теорема Брокетта, которая утверждает, что неголономные системы управляемы, но их невозможно стабилизировать до желаемого состояния с помощью дифференцируемого или даже непрерывного регулятора
с чистой обратной связью по состоянию. Необходима нелинейная или нестационарная стратегия управления, а это означает, что робот следует некоторой, в общем случае нелинейной
траектории. Например, регулятор (4.8) вносит разрыв в точке
ρ = 0. Исключением является малоприводная система, движущаяся в трехмерном пространстве в силовом поле, например
в гравитационном поле: гравитация действует как дополнительный привод и делает систему линейно управляемой (но
не голономной), как было показано на примере квадрокоптера
в разделе 4.2.
В табл. 4.1 перечислены параметры мобильности различных
роботов, рассмотренных здесь и ранее в разделе 2.4.9. Подробнее малоприводность и избыточная приводность будут рассматриваться при обсуждении роботов-манипуляторов в разделе 8.3.4.

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

Поезд
2-суставная рука робота
6-суставная рука робота
10-суставная рука робота
Судно на воздушной
подушке
Автомобиль
Вертолет
Самолет с фиксированным
крылом
RangerBot

4.4

dim 𝒞 Степеней
свободы
1
1
2
2
6
6
10
10
3
3

Количество
приводов
1
2
6
10
2

Приводность Ограничение
качения
Полная
Полная
Полная
Избыточная
Малая

3
6
6

2
6
6

2
4
4

Малая
Малая
Малая

6

6

6

Полная

Голономность
Да
Да
Да
Да

Да

Да

Подведение итогов

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

232 Глава 4 · Мобильные роботы

транспортных средств, подобных автомобилям, мы разработали кинематическую модель с рядом контроллеров, помогающих платформе выполнять такие полезные задачи, как движение к заданной точке, движение вдоль линии, движение вдоль
произвольной траектории и движение к заданной конфигурации. Затем мы обсудили транспортные средства с дифференциальным управлением, которое используется во множестве
роботов, и всенаправленные аппараты на основе колес новых
типов. Потом мы рассмотрели простой, но распространенный
летательный аппарат – квадрокоптер – и разработали динамическую модель с иерархической системой управления, которая
позволяет квадрокоптеру летать по кругу. Этот иерархический
подход к управлению более подробно описан в разделе 9.1.7
в контексте роботизированных манипуляторов.
Мы также дополнили предыдущее обсуждение конфигурационного пространства, включив в него ограничение скорости изза малоприводности и ограничение качения со стороны колес.
В следующих главах этой части мы поговорим о том, как планировать траектории движения роботов в сложных условиях
при наличии препятствий, а затем – как определять местоположение робота.

4.4.1

Дополнительное чтение

Комплексное моделирование мобильных наземных роботов
представлено в книге Siegwart et al. (2011). Помимо представленных здесь моделей, в ней подробно рассматриваются различные комбинации ведущих, управляемых и пассивных опорных колес. Книга Kelly (2013) тоже посвящена моделированию
транспортных средств и управлению ими. Обе книги дают хорошее введение в методы определения местоположения и навигацию, которые мы обсудим в следующих главах.
В статье Martins et al. (2008) рассматриваются кинематика, динамика и управление роботами с дифференциальным
управлением. Astolfi (1999) иллюстрирует систему управления
положением транспортного средства, подобного автомобилю.
В «The Handbook of Robotics» (Siciliano and Khatib 2016, part E)
рассматривается моделирование и управление различными типами транспортных средств, включая воздушные и подводные.
Теория вертолетов с акцентом на робототехнику представлена
в книге Mettler (2003), но основополагающим источником информации по динамике вертолетов является объемная книга
Prouty (2002). Книга Antonelli (2014) содержит исчерпывающую
информацию о моделировании и управлении подводными роботами.

Подведение итогов 233

Одни из самых ранних работ по моделированию и управлению квадрокоптерами были опубликованы Pounds, Mahony
(Hamel et al., 2002; Pounds et al., 2004, 2006). В диссертации Pounds
(2007) представлена комплексная аэродинамическая модель
квадрокоптера с особым акцентом на маховом движении лопастей – явлении, хорошо известном в вертолетостроении, но
практически не учитываемом в квадрокоптерах. Вводное руководство по управлению многовинтовыми летательными аппаратами представлено в Mahony, Kumar, and Corke (2012).
Мобильные наземные роботы представляют собой довольно зрелую технологию транспортировки деталей по производственным предприятиям. В настоящее время активно исследуются вопросы создания транспортных средств, способные
работать автономно в условиях окружающей среды (Siciliano
and Khatib, 2016, part F). Исследования в области автоматизации легковых автомобилей ведутся с 1980-х годов, и в новых
автомобилях появляются все более широкие возможности автономного вождения.

Исторические и интересные факты
В 1984 году в университете Карнеги–Меллона стартовал проект Navlab, в рамках которого была создана серия беспилотных
транспортных средств и проведено большое количество исследований. Все транспортные средства активно использовали
компьютерное зрение для навигации. В 1995 году беспилотный
автомобиль Navlab 5 совершил 3000-мильное путешествие, получившее название «No Hands Across America» «Без рук через
всю Америку» (Pomerleau, Jochem, 1995, 1996). 98 % расстояния
автомобиль преодолел под управлением автопилота, в основном визуально ориентируясь по белым линиям на обочине.
Вопросами автоматического управления транспортными
средствами в Европе занимался Эрнст Дикманнс со своей командой в университете Бундесвера в Мюнхене. В 1988 году они
продемонстрировали способность системы VaMoRs вести 5-тонный фургон Mercedes-Benz со скоростью более 90 км/ч (Dickmanns, Graefe 1988b; Dickmanns, Zapp 1987; Dickmanns 2007).
В рамках европейского проекта «Прометей», существовавшего
с 1987 по 1995 год, были созданы роботизированные автомобили VaMP и VITA-2, которые в 1994 году проехали более 1000 км
по многополосному парижскому шоссе в условиях стандартного плотного движения со скоростью до 130 км/ч. Они продемонстрировали возможность автономного вождения по свободным
полосам, движения в составе колонны, автоматического слежения за другими транспортными средствами, а также перестрое­
ния и обгона. В 1995 году беспилотный S-Class Mercedes-Benz
проехал 1600 км из Мюнхена в Копенгаген и обратно. На немец-

234 Глава 4 · Мобильные роботы

ком автобане скорость превышала 175 км/ч, и в процессе движения автомобиль совершал маневры, такие как обгон. Среднее
время между вмешательствами оператора составило 9 км, и он
проезжал до 158 км без какого-либо участия человека. В британской части проекта было продемонстрировано автономное
вождение XJ6 Jaguar с применением видеокамер (Matthews et
al. 1995) и радарных датчиков для удержания полосы движения
и предотвращения столкновений. В США в 2000-х годах DARPA провело серию конкурсов Grand Challenges для беспилотных автомобилей. Испытания в пустыне в 2005 году и в городе
в 2007 го­ду подробно описаны в сборниках статей различных
групп в Buehler et al. (2007, 2010). Из более поздних демонстраций беспилотных автомобилей можно назвать путешествие
по легендарному Шелковому пути, описанное в Bertozzi et al.
(2011), и классическое дорожное путешествие по Германии,
описанное в Ziegler et al. (2014).
В интернете можно найти журнал Аккермана (http://
smithandgosling.wordpress.com/2009/12/02/ackermanns-repository-of-arts), в мартовском и апрельском выпусках за 1818 год
которого был опубликован механизм рулевого управления каретой года (стр. 162 и 234). В King-Hele (2002) приводится всестороннее обсуждение предшествующих работ по геометрии
рулевого управления и более раннего изобретения Дарвина.

Примечания к Toolbox
В дополнение к блокам для конструирования моделей, используемым в этой главе, Toolbox предоставляет классы Python,
которые реализуют кинематические уравнения и которые мы
будем использовать в главе 6. Например, с их помощью можно
создать модель транспортного средства с ограничениями угла
поворота рулевого колеса и скорости.
>>> veh = Bicycle(speed_max=1, steer_max=np.deg2rad(30));
>>> veh.q
array([
0,
0,
0])

которая имеет начальную конфигурацию (0, 0, 0). Мы можем
применить (υ, γ), и после одного временного шага конфигурация будет иметь вид
>>> veh.step([0.3, 0.2])
array([
0.03, 0.006081])
>>> veh.q
array([
0.03,
0, 0.006081])

Мы можем вычислить (4.4) для конкретной конфигурации
и набора управляющих входов (υ, γ):
>>> veh.deriv(veh.q, [0.3, 0.2])
array([
0.3, 0.001824, 0.06081])

Подведение итогов 235

Этот класс наследует абстрактный базовый класс VehicleBase,
как, впрочем, и другие классы транспортных средств. Например, класс Unicycle реализует кинематику моноколеса с входными данными (υ, ω), а класс DiffSteer реализует робота с дифференциальным управлением и входными данными (ωL, ωR) – все
они имеют много общих методов.

4.4.2
1.

2.

3.
4.
5.
6.
7.

8.

Упражнения

Для 4-колесного транспортного средства с длиной L = 2 м
и шириной между центрами колес 1.5 м.
а)	На какой угол нужно повернуть управляемые колеса,
чтобы обеспечить скорость поворота 10 грд/с при скорости движения вперед 20 км/ч?
б)	Вычислите разницу в углах для левого и правого управляемых колес в системе рулевого управления Аккермана
при движении по дугам с радиусом 10, 50 и 100 м.
в)	Автомобиль движется со скоростью 80 км/ч, вычислите
разницу в скорости вращения задних колес при движении по дугам с радиусом 10, 50 и 100 м.
Запишите выражение для вычисления скорости поворота
через угловую скорость вращения двух задних колес. Исследуйте влияние погрешностей радиуса колес и ширины
транспортного средства.
Рассмотрим автомобиль и автобус с L = 4 и L = 12 м соответственно. Определите углы поворота управляемых колес
при движении по дугам с радиусом 10, 20 и 50 м.
Нарисуйте несколько траекторий движения автомобиля на
плоскости xy для разных углов поворота управляемых колес в диапазоне от -45° до 45° и скорости 2 м/с.
Реализуйте оператор ⊝, используемый в разделе 4.1.1.1,
и проверьте его, сравнив результаты, возвращаемые им
и функцией angdiff.
Постройте график зависимости x и y от времени для случая
движения к заданной точке (раздел 4.1.1.1).
Для примера чистого преследования (раздел 4.1.1.3):
а)	посмотрите, что произойдет, если изменить расстояние
прогнозирования;
б)	измените пример так, чтобы робот следовал по слаломной траектории;
в)	измените пример так, чтобы робот преследовал цель,
движущуюся вперед и назад вдоль линии.
Для примера занятия заданного положения (раздел 4.1.1.4):
а)	повторите пример с другой начальной ориентацией;
б)	выполните параллельную парковку. Насколько практична полученная траектория?

236 Глава 4 · Мобильные роботы

9.

10.
11.
12.

13.
14.

15.

16.

в)	поэкспериментируйте с различными параметрами регулятора.
Создайте графический интерфейс с простым рулем и регулятором скорости и используйте его для создания простого
симулятора вождения. В качестве альтернативы можно использовать игровой руль и педали.
Адаптируйте различные регуляторы из раздела 4.1.1 для
управления роботом с дифференциальным управлением.
Выведите (4.9) из предшествующего ему уравнения.
Для случая постоянной скорости поступательного движения постройте график зависимости υL и υR от положения
точки мгновенного центра поворота (МЦП). При каких условиях υL и υR имеют разные знаки?
Реализуйте регулятор, используя (4.13), который перемещает одноколесного робота вдоль его оси y. Как изменяется
ориентация робота в процессе движения?
Нарисуйте конструкцию робота с тремя колесами Илона.
Убедитесь, что он не может свободно катиться вбок и способен двигаться в любом направлении. Напишите код
для преобразования желаемой скорости поступательного
и вращательного движения транспортного средства в скорость вращения колес.
Для 4-колесного всенаправленного робота из раздела 4.1.3
напишите алгоритм, который позволит ему двигаться по
окружности радиусом 0.5 м вокруг заданной точки так, что
его нос всегда будет направлен к центру окружности.
Управление квадрокоптером (раздел 4.2).
а)	Вычислите скорость вращения всех винтов для случая,
когда квадрокоптер находится в состоянии равновесия.
б)	Поэкспериментируйте с различными коэффициентами
усиления управления. Что произойдет, если уменьшить
коэффициенты усиления, применяемые к скорости изменения величин?
в)	Удалите константу прямой связи по гравитации и поэкс­
периментируйте с большим набором высоты или ПИрегулятором.
г)	При ненулевых углах крена и тангажа величина вертикальной тяги уменьшается, и аппарат начинает медленно снижаться. Для исправления этого недостатка добавьте компенсацию вертикальной тяги.
д)	Смоделируйте полет квадрокоптера в перевернутом положении, то есть когда его ось z направлена вверх.
е)	Запрограммируйте баллистическое движение. Заставьте квадрокоптер взлететь под углом 45 градусов к горизонту, затем полностью выключите тягу.
ж)	Запрограммируйте мягкую посадку. Что означает «мягкая»?

Подведение итогов 237

з)	Запрограммируйте маневр «бочка». Заставьте квадрокоптер лететь горизонтально вдоль оси x, а затем увеличьте угол крена с 0 до 2π.
и)	Запрограммируйте маневр «мертвая петля». Заставьте
квадрокоптер летать горизонтально вдоль оси x, а затем
увеличьте угол тангажа с 0 до 2π.
к)	Повторите упражнение, но для конфигурации с 6 вин­
тами.
л)	Используйте функцию mstraj, чтобы проложить траекторию через десять промежуточных точек (Xi, Yi, Zi, θy),
и модифицируйте регулятор (рис. 4.24) для плавного
следования по этой траектории.
м)	Создайте графический интерфейс с простым управлением джойстиком и используйте его для создания очень
простого симулятора полетов. В качестве альтернативы
можно использовать игровой джойстик.

Глава

5

	Навигация

Навигация роботов – это задача наведения робота на цель.
Цель может быть задана в терминах каких-либо характеристик
окружающей среды, например движение к источнику света или
в точку с определенными координатами.
Для навигации люди создают карты и устанавливают указатели, и на первый взгляд кажется очевидным, что роботы должны
действовать аналогично. Однако многие задачи роботы могут
выполнять вообще без карты, используя подход, называемый
реактивной навигацией, оценивая окружающую обстановку
и реагируя на полученные данные. Например, роботизированная черепаха Elsie, показанная на рис. 5.1а, реагировала на
окружающую среду и могла искать источник света и обходить
препятствия, не имея четкого плана или знания о своем местоположении или местоположении источника света.

а

б

Рис. 5.1. а) Elsie – роботизированная черепаха, разработанная в 1940 году Уильямом Греем
Уолтером. Институт Бердена, Бристоль (1948). Сейчас находится в коллекции Смитсоновского
института, но не экспонируется (изображение получено из коллекции Рубена Хоггетта);
б) Shakey. SRI International (1968). Сейчас находится в музее вычислительной техники
в Маунтин-Вью (изображение предоставлено SRI International)

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

chap5.ipynb

https://go.sn.pub/
yLoW7Z

Навигация 239

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

Рис. 5.2. Покадровая съемка перемещений первого робота-пылесоса iRobot® Roomba,
убирающего комнату (изображение предоставил Крис Бартлетт)

Альтернативный подход был воплощен в 1960-х в роботе
Shakey, показанном на рис. 5.1б. Этот робот, способный воспринимать трехмерное пространство, создает карту окружающей
среды, а затем, основываясь на ней, планирует маршрут к месту
назначения. Данный способ навигации ближе к человеческому
способу навигации по карте и подобен тому, что используется
в автопилотах самолетов, кораблей, автомобилей и мобильных
роботов. Задача использования карты для поиска пути к цели
известна как планирование маршрута, или планирование движения. Этот подход позволяет решать более сложные задачи,
но и сам по себе он более сложен, требуя наличия карты окружающей среды и постоянного знания местоположения робота
относительно карты – обе эти нетривиальные задачи мы рассмотрим в главе 6.
Реактивный подход и подход с использованием карт находятся на противоположных концах спектра навигации мобильных
роботов. Реактивные системы действуют быстрее и устроены
проще, потому что действия напрямую связаны с восприятием.
То есть они не нуждаются в получении и сохранении представления о мире, а также в способности рассуждать об этом представлении. В природе такие стратегии используются наиболее
простыми организмами, такими как насекомые. Системы, создающие карты и выстраивающие рассуждения на их основе,
требуют больше ресурсов, но способны решать более сложные

240 Глава 5 · Навигация

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

5.1

Введение в реактивную навигацию

Робот может выполнять удивительно сложные задачи, даже не
имея карты и понятия о своем местоположении. Роботы-пылесосы Roomba первого поколения осуществляли уборку, перемещаясь в случайных направлениях и используя информацию от
контактных датчиков (рис. 5.2). Насекомые, такие как муравьи
и пчелы, собирают пищу и возвращают ее в свое гнездо, основываясь на информации от органов чувств. У них слишком мало
нейронов, чтобы создать какую-либо ментальную карту мира
и спланировать свой путь. Даже одноклеточные организмы,
такие как простейшие жгутиковые, демонстрируют целенаОтступление 5.1. Уильям Грей Уолтер
Уильям Грей Уолтер (1910–1977) – нейрофизиолог
и пионер кибернетики. Родился в Канзас-Сити, штат
Миссури. Учился в Королевском колледже Кембриджа
и, не получив гранта в Кембридже, начал занимался
нейрофизиологическими исследованиями в лондонских больницах, а с 1939 года – в Неврологическом
институте Бердена в Бристоле. Разработал электроэнцефалографическую топографию мозга, основанную
на использовании нескольких электродов, закрепляемых на коже головы, и алгоритма триангуляции для
определения амплитуды и локализации мозговой
активности.
Уолтер оказал влияние на новую в ту пору область
кибернетики. Он создавал роботов, чтобы изучать возникновение сложных рефлекторных реакций из нейронных взаимосвязей. В 1948 году построил черепаху
Elsie (вид Machina Speculatrix). Это был трехколесный робот, способный к фототаксису и находящий дорогу к зарядной станции. Черепаха второго поколения (была
собрана в 1951 году) находится в коллекции Смитсоновского института. Уолтер
публиковал статьи в журнале Scientific American (1950 и 1951), пользовавшиеся
большой популярностью, и книгу «The Living Brain» (1953). В 1970 году получил тяжелые травмы в автокатастрофе, от которых так и не оправился полностью. (Изображение предоставлено коллекцией Рубена Хоггетта.)

Введение в реактивную навигацию 241

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

машина, способная ощущать, планировать и целенаправленно
« действовать.

В более общем
случае таксис – это
реакция организма
на градиент стимула.

Роботизированная черепаха Уолтера, показанная на рис. 5.1,
стремится двигаться к источнику света, демонстрируя, по словам создателя, «поведение, подобное поведению живых существ», что стало важным достижением в тогдашней кибернетике. Это высказывание может показаться преувеличением,
учитывая технологическую примитивность 1940-х, но, вообще
говоря, такое поведение наблюдается в природе у простейших
и называется фототаксис. ◄

блок
управления

робот

датчики

максимальная
скорость

q

скорость

скорость
Блок Bicycle

ЛЕВЫЙ датчик

Формула (5.1)
скорость
поворота
рулевого колеса

анимация движения
транспортного средства

ПРАВЫЙ датчик

Формула (5.2)

K_s

скорость
поворота

Рис. 5.3. Блок-схема модели models/braitenberg, которая направляет транспортное средство к максимуму
скалярного поля, определяемого внутри блоков f(x), которые моделируют работу датчиков. Транспортное средство
плюс блок управления – это пример тележки Брайтенберга

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

5.1.1

Тележки Брайтенберга

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

242 Глава 5 · Навигация

на рис. 5.3, позволяет достичь этого, используя лишь сигналы,
информирующие о скорости и величине угла поворота, получаемые непосредственно от датчиков.
Чтобы переместиться вдоль градиента, нужно оценить направление градиента в точке, где находится робот, а для этого
требуется выполнить не меньше двух измерений поля. В этом
примере мы используем прием, широко распространенный
в природе, – двустороннюю сенсорику, когда два датчика расположены симметрично по обе стороны корпуса робота. Датчики
моделируются функциональными блоками (рис. 5.3) и пара­мет­
ризуются их местоположением относительно корпуса робота
и функцией восприятия. Датчики расположены на расстоянии
±2 единицы в поперечном направлении (или по оси y) робота.
Поле, подлежащее измерению, – это простое поле, величина которого обратно пропорциональна квадрату расстояния и определяется функцией
>>> def sensorfield(x, y):
...
xc, yc = (60, 90)
...
return 200 / ((x - xc) ** 2 + (y - yc) ** 2 + 200)

которая возвращает значение датчика s(x, y) ∈ [0, 1] как функцию местоположения датчика в плоскости. Эта функция имеет
пиковое значение в точке с координатами (60, 90).
Скорость автомобиля равна
υ = 2 - sR - sL,

(5.1)

где sR и sL – показания правого и левого датчиков соответственно. В точке цели, где sR = sL = 1, скорость становится равной нулю.
Отступление 5.2. Тележка Брайтенберга
Валентино Брайтенберг (1926–2011) – итало-австрийский нейробиолог и кибернетик, бывший директор
Института биологической кибернетики имени Макса
Планка в Тюбингене, Германия.
Тележка Брайтенберга – это автомат, имеющий датчики и исполнительные механизмы и связывающий
их напрямую для реализации целенаправленного поведения. Они были представлены в его книге 1986 го­
да «Vehicles: Experiments in Synthetic Psychology», в которой описываются реактивные роботы, способные
двигаться к определенной цели. Книга описывает аналоговые схемы реализации роботов, но в настоящее
время они обычно реализуются с помощью микро­
конт­роллеров. Черепаха Уолтера появилась раньше
книги Брайтенберга, но сегодня ее тоже можно считать
тележкой Брайтенберга. Изображение предоставлено
издательством The MIT Press, © MIT 1984.

Это похоже
на тележку
Брайтенберга 4а.

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

Введение в реактивную навигацию 243

Угол поворота рулевого колеса определяется на основе разности показаний датчиков
γ = Ks(sR - sL),

(5.2)

Аналогичные стра- где Ks – коэффициент, учитывающий передаточное число рулетегии используют вого управления. Когда левый и правый датчики выдают одинамотыльки, чьи две
антенны – чрез- ковые замеры величины поля, то робот движется прямо.
вычайно чувствиСимуляция запускается командой
тельные детекторы
запахов. Самец мо- >>> %run -m braitenberg -H
тылька использует
дифференциальный и демонстрирует движение робота в сторону максимума скасигнал для определения направления лярного поля, заставляя его поворачивать в сторону цели, зана самку, выделяю- медляться по мере приближения к ней и асимптотически дощую феромоны. стигать ее. На рис. 5.4 показан пример траектории робота.

Начальную конфигурацию, коэффициент рулевого управления
и скорость можно настроить, изменяя константы в начале файла models/braitenberg.py.

Рис. 5.4. Путь тележки Брайтенберга
с использованием модели models/
braitenberg, показанной на рис. 5.3.
Транспортное средство движется
к максимуму двухмерного скалярного
поля, величина которого обозначена
цветом

траектория тележки

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

244 Глава 5 · Навигация

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

5.1.2

Простые автоматы

Другой класс реактивных роботов называют жуками. Это прос­
тые автоматы, которые ищут цель и способны обходить препятствия. Было предложено множество алгоритмов для жуков,
и все они могут определять приближение к препятствию. В этом
отношении они похожи на тележки Брайтенберга, но жук включает конечный автомат и другую логику между датчиком и двигателями. Автоматы имеют память, отсутствующую в тележках
Брайтенберга. В этом разделе мы исследуем конкретный алгоритм поведения жука, известный как bug2.
Начнем с загрузки модели дома, которая входит в состав
Toolbox:
>>> house = rtb_load_matfile("data/house.mat");

Это словарь, содержащий два элемента. План этажа
>>> floorplan = house["floorplan"];
>>> floorplan.shape
(397, 596)

– это двухмерный массив NumPy с нулями и единицами, обозначающими свободные участки и препятствия соответственно. Матрица представлена на рис. 5.5. Это пример сетки занятости, которая будет подробно рассматриваться в разделе 5.2.
Словарь также включает позиции именованных мест в доме
в форме объектов, функционально подобных словарю.
>>> places = house["places"];
>>> places.keys()
dict_keys(['kitchen', 'garage', 'br1', 'br2', 'br3', 'nook', ...

Например, центр спальни 3 имеет координаты
>>> places.br3
array([50, 50], dtype=uint8)

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

Введение в реактивную навигацию 245
старт
цель
сад
линия направления
путь робота

подъездная
дорога

веранда
каминная

спальня1

гостиная

кухня
гараж

спальня2 спальня3

кабинет

прихожая

Рис. 5.5. Участки, свободные для движения, обозначены белым цветом, препятствия –
красным, а именованные места – синим. Начальная точка – закрашенный кружок внизу слева,
а конечная – звездочка в центре. Примерный масштаб – 45 мм на клетку

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

Создадим экземпляр класса Bug2
>>> bug = Bug2(occgrid=floorplan);

и передадим ему сетку занятости, которая будет использована
для генерирования сенсорных сигналов. Мы можем отобразить
окружение робота командой
>>> bug.plot();

и запустить симуляцию командой
>>> bug.run(start=places.br3, goal=places.kitchen, animate=True);

Метод принимает начальную и конечную позиции (x, y) робота внутри дома и в процессе симуляции отображает анимацию
движения робота к цели, траектория которого изображена на
рис. 5.5 как последовательность зеленых.
Стратегия алгоритма bug2 довольно проста. Сначала он вычисляет прямую линию от начальной точки до цели – путевую
линию – и пытается двигаться вдоль нее. В случае столкновения с препятствием производится поворот направо, и движе-

246 Глава 5 · Навигация

ние продолжается, пока не встретится точка на путевой линии,
которая находится ближе к цели, чем в момент встречи с препятствием. ►
Метод run возвращает траекторию движения робота
>>> path = bug.run(start=places.br3, goal=places.kitchen);
>>> path.shape
(1307, 2)

в виде двухмерного массива, каждая строка которого соответствует одной клетке, которую посетил робот. В данном случае
траектория включает 1307 клеток.
В этом примере алгоритм bug2 достиг цели, но выбрал очень
неоптимальный маршрут, пройдя по внутренней части шкафа,
за дверями и посетив два туалета. В этом примере, возможно,
цель была бы достигнута раньше, если бы у первого препятствия алгоритм повернул налево, а не направо, но в другом
месте эта стратегия может дать худший результат. Исследователями разработано множество вариантов алгоритма bug, которые эффективно работают в окружениях одного типа, но, как
правило, плохо работают в других. Главная проблема робота –
отсутствие карты. Не имея «общей картины», он обречен выбирать пути, которые являются оптимальными локально, а не
глобально.

5.2

Введение в навигацию по картам

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

« машина, способная ощущать, планировать и целенаправлен­
но действовать,

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

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

Введение в навигацию по картам 247

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

Ключевыми элементами графа, показанного на рис. 5.6a, являются вершины (точки) и ребра (линии), которые представляют места и дороги между ними соответственно. Графы могут
быть ориентированными, где ребра представлены стрелками,
указывающими направление движения, или неориентированными, где ребра представлены отрезками и допускают возможность движения в любом направлении. При картографировании
мы обычно считаем, что вершины размещены или встроены
в декартову систему координат мест, которые они представляют, – это называется вложенным, или погруженным, графом. Реб­
ра представляют проезжие участки между парами вершин
и имеют связанную стоимость пересечения – обычно расстояние или время, но это может быть и неровность дороги. Ребра
могут также иметь другие атрибуты, важные для планирования,
например ограничение скорости, стоимость проезда или количество кафе.

а

б

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

Отступление 5.3. Граф
Граф – это абстрактное представление наборов объектов, соединенных связями, обычно обозначаемых как
упорядоченная пара (V, E). Объекты в наборе V называются вершинами, или узлами, а объекты в наборе
E представляют соединения между объектами V и называются ребрами, или дугами. Ребра могут быть ориентированными (стрелки) или неориентированными,
как показано здесь, иметь связанный с ними вес или
стоимость перемещения от одной вершины к другой через это ребро. Последовательность ребер от одной вершины к другой называется путем. Графы могут
использоваться для представления транспортных или коммуникационных сетей
и даже социальных отношений, а соответствующий раздел математики называется теорией графов. Классы навигации используют пакет графовых вычислений на
Python, который называется pgraph, см. приложение I.

248 Глава 5 · Навигация
Отступление 5.4. Люди и карты
Животные, особенно млекопитающие,
обладают способностью ориентироваться на местности. Они могут распо­
знавать знакомые места и ориентиры,
планировать, анализировать и корректировать маршрут при необходимости.
В мозге позвоночных имеется структура
гиппокампа, которая отвечает за пространственную память и играет ключевую роль в навигации.
Люди – млекопитающие с уникальной способностью к письменной и графической коммуникации, поэтому неудивительно, что запись и обмен навигационной информацией в виде карт – древнейшая практика.
Древнейшая из сохранившихся карт высечена на бивне мамонта и имеет возраст более 25 000 лет. Археологи полагают, что на ней изображены ландшафт вокруг
деревни Павлов в Чехии, охотничьи угодья и река Тейя. В настоящее время карты
встречаются повсеместно, и мы активно пользуемся ими в повседневной жизни.

Отступление 5.5. Происхождение теории графов
Истоки теории графов восходят к задаче о мостах в Кенигсберге. В этом
городе, ранее принадлежавшем Германии, а ныне входящем в состав России
и известном как Калининград, имеется
остров на реке Преголя. Этот остров был
соединен с обоими берегами реки семью
мостами (из которых сохранились только три).
Темой спора был вопрос о том, можно
ли пройти по городу так, чтобы пересечь
каждый мост только один раз. В 1736 го­
ду Леонард Эйлер доказал, что это невозможно, и его решение положило начало теории графов.

Планирование маршрута с использованием графической карты 249

Универсальная
поперечная проекция Меркатора
(Universal Transverse
Mercator, UTM) – это
картографическая
система, которая
отображает сферу
Земли, в шестьдесят
полос, или зон, пронумерованных от
1 до 60, простирающихся с севера
на юг и имеющих
ширину 6°. Каждая
зона разделена на
горизонтальные
полосы, обозначенные буквами от
C до X, высотой 8°
каждая. Зона и полоса определяют
плоскую область
сетки с началом
системы координат
в центре.
49L

50L

51L

52L

53L

54L

55L

56L

49K

50K

51K

52K

53K

54K

55K

56K

49J

50J

51J

52J

53J

54J

55J

56J

49H

50H

51H

52H

53H

54H

55H

56H

49G

50G

51G

52G

53G

54G

55G

56G

Непогруженные графы тоже широко распространены. Например, станции на картах метро не обязательно отображаются в соответствии с их географическими координатами, а расположены так, чтобы облегчить чтение карты. Топологические
графы, часто используемые в робототехнике, состоят из вершин, представляющих места, которые можно распознать, даже
если их положение в мире неизвестно, и путей, соединяющих
эти места. Например, робот может распознать, что место – это
кухня и что, пройдя через определенный дверной проем, он
попадает в место, которое он может распознать как гостиную.
Этого достаточно для создания двух вершин и одного ребра топологического графа, даже если вершины не имеют связанных
с ними декартовых координат.
На рис. 5.6б показана карта на основе сетки. Здесь окружающая среда рассматривается как сетка ячеек, обычно квад­
ратных, каждая из которых содержит информацию о ее проходимости. Простейший случай называется сеткой занятости,
а ячейка такой сетки может иметь одно из двух состояний: одно
означает, что ячейка занята и в нее нельзя войти – это препятствие; или ноль, если ячейка свободна. Размер ячейки зависит
от приложения. Объем памяти, необходимый для хранения
сетки занятости, увеличивается прямо пропорционально размеру представленной области пространства и обратно пропорционально размеру ячейки. Современные компьютеры вполне
могут уместить в своей памяти такое представление даже для
очень больших областей.

5.3

Планирование маршрута с использованием
графической карты

Мы будем работать с погруженными графами, положение вершин которых в мире известно. В этом примере мы рассмотрим
города, показанные на рис. 5.7, и импортируем файл данных,
входящий в состав Toolbox:
>>> data = rtb_load_jsonfile("data/queensland.json");

где data – это словарь, содержащий два элемента. Первый,
data["places"], – это словарь, отображающий названия мест
в словари атрибутов, таких как широта, долгота и координаты
карты UTM в километрах. Мы можем легко нанести эти места
на карту вместе с их названиями:

(Изображение
создано с помощью
MATLAB® Mapping >>> for name, info in data["places"].items():
plot_point(info["utm"], text=name)
Toolbox™) ...

250 Глава 5 · Навигация

Рис. 5.7. Карта Квинсленда (Австралия) с городами и поселками, используемая для
графического планирования (изображение создано Ремо Пиллатом с помощью MATLAB®
Mapping Toolbox™)

Второй, data["routes"], – это список словарей, каждый из которых определяет ребро графа: название начальной и конечной точек маршрута, его протяженность и скорость движения.
Например:
>>> data["routes"][0]
{'start': 'Camooweal', 'end': 'Mount Isa', 'distance': 188,
'speed': 100}

Теперь воспользуемся пакетом pgraph для создания графического представления данных. Сначала создадим объект неориентированного графа, а затем добавим вершины и ребра из
только что загруженных данных.
>>>
>>>
>>>
...
>>>
...
...

import pgraph
g = pgraph.UGraph() # создать пустой неориентированный граф
for name, info in data["places"].items():
g.add_vertex(name=name, coord=info["utm"]) # добавить место
for route in data["routes"]:
g.add_edge(route["start"], route["end"],
cost=route["distance"]) # добавить маршрут

Объект UGraph имеет множество методов, например мы можем нарисовать только что созданный граф

Планирование маршрута с использованием графической карты 251
>>> g.plot()

и получить изображение, показанное на рис. 5.6a. Объект графа
имеет свойства, которые сообщают количество вершин и ребер.
>>> g.n
20
>>> g.ne
29

Мы можем получить вершину по ее имени:
>>> g["Brisbane"]
UVertex[Brisbane, coord=(1096, 6947)]

экземпляр класса UVertex, представляющий вершину в неориентированном графе. Ее соседей, или смежные вершины:
>>> g["Brisbane"].adjacent()
[UVertex[Rockhampton, coord=(858.9, 7410)],
UVertex[Roma, coord=(677.7, 7060)]]

Степень вершины – это число соседей, с которыми она соединена ребрами:
>>> g["Brisbane"].degree
2

и среднюю степень всего графа
>>> g.average_degree()
2.9

Средняя степень показывает, насколько хорошо связан граф.
Чем выше значение, тем больше путей между вершинами
и, следовательно, больше возможностей для перемещения.
Компонент – это набор связанных друг с другом вершин,
и этот граф
>>> g.nc
1

имеет лишь один компонент и потому называется полностью связанным, то есть между любой парой вершин можно проложить
путь. Несвязанный граф состоит из нескольких компонентов.
В таком графе можно пройти от одной вершины к другой внутри
компонента, но нельзя перейти из одного компонента в другой.
Есть возможность получить ссылки на объекты Edge, представляющие ребра. Список ребер вершины, соответствующей
городу Brisbane (Брисбен), можно получить так:
>>> edges = g["Brisbane"].edges()
[Edge{[Rockhampton] -- [Brisbane], cost=682},
Edge{[Brisbane] -- [Roma], cost=482}]

252 Глава 5 · Навигация

и каждое ребро имеет ссылки на свои конечные точки:
>>> edges[0].endpoints
[UVertex[Rockhampton, coord=(858.9, 7410)],
UVertex[Brisbane, coord=(1096, 6947)]]

Теперь мы готовы приступить к планированию маршрутов. Давайте рассмотрим конкретный пример путешествия из
Hughenden в Brisbane. Простейшее решение – случайным образом выбрать одну из соседних вершин, переместиться туда
и повторить процесс. При таком подходе мы когда-нибудь сможем достичь цели, но крайне маловероятно, что выбранный
путь будет оптимальным. Требуется более системный подход,
и нам повезло, что эта задача хорошо изучена – это известная
задача поиска кратчайшего пути между двумя вершинами на
графе.
Начнем с Hughenden. У нас есть всего четыре дороги, по которым можно добраться до соседнего города. Из одного из этих
городов, скажем из Winton, можно по очереди посетить каждого
из его соседей. Однако один из этих соседей – Hughenden – наша
отправная точка, и, как мы знаем, возврат не может привести
к оптимальному пути. Нужно как-то отслеживать посещавшиеся вершины. Для этого можно определить два мно­жества: FRONTIER – открытое множество вершин, запланированных для исследования, и EXPLORED – замкнутое множество исследованных
(посещавшихся) вершин.
Рассмотрим три алгоритма, начав с поиска в ширину.
>>> path, length = g.path_BFS("Hughenden", "Brisbane")
>>> length
1759
>>> ", ".join([p.name for p in path])
'Hughenden, Barcaldine, Emerald, Rockhampton, Brisbane'

Этот алгоритм вернул два значения: список вершин, составляющих путь от начальной точки до конечной, и общую длину
пути в километрах. Мы можем визуализировать путь, сначала
отобразив граф, а затем наложив путь на граф:
>>> g.plot()
>>> g.highlight_path(path)

как показано на рис. 5.8а. Алгоритм действует последовательно,
и будет поучительно рассмотреть его действия по шагам.
>>> g.path_BFS("Hughenden", "Brisbane", verbose=True)
FRONTIER: Hughenden
EXPLORED:
expand Hughenden
add Cloncurry to the frontier

Планирование маршрута с использованием графической карты 253
add
add
add
move

Charters Towers to the frontier
Barcaldine to the frontier
Winton to the frontier
Hughenden to the explored list

FRONTIER: Cloncurry, Charters Towers, Barcaldine, Winton
EXPLORED: Hughenden
expand Cloncurry
add Mount Isa to the frontier
move Cloncurry to the explored list
FRONTIER: Charters Towers, Barcaldine, Winton, Mount Isa
EXPLORED: Hughenden, Cloncurry
expand Charters Towers
add Townsville to the frontier
move Charters Towers to the explored list
...

а

б

Рис. 5.8. Результаты планирования маршрута: а) поиск в ширину (breadth-first search, BFS) с длиной пути 1759 км;
б) поиск с равномерной стоимостью (uniform-cost search, UCS) и поиск A* с длиной пути 1659 км

Множество FRONTIER инициализируется начальной вершиной.
На каждом шаге это множество расширяется за счет добавления
соседей текущей вершины. После посещения вершина удаляется и добавляется в множество EXPLORED. На последнем шаге:
FRONTIER: Rockhampton, Bedourie, Birdsville, Roma
EXPLORED: Hughenden, Cloncurry, Charters Towers, Barcaldine, Winton,
Mount Isa, Townsville, Blackall, Emerald, Longreach, Boulia,
Windorah, Camooweal, Mackay, Charleville
expand Rockhampton
goal Brisbane reached
15 vertices explored, 3 remaining on the frontier

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

254 Глава 5 · Навигация

f(υ) = g(υ) + h(υ),

(5.3)

где g(υ) – стоимость пути на данный момент, или стоимость
пройденного пути (от начала), а h(υ) – оставшееся расстояние до
цели, или стоимость перехода. Однако мы не знаем h(υ), потому
что еще не закончили планирование. Пока что мы довольствуемся выбором следующей вершины υ, которая минимизирует
f(υ) = g(υ). То есть мы выбираем
(5.4)
в качестве следующей вершины для расширения. Интуиция
подсказывает: чтобы найти кратчайший общий путь, на каждом шаге следует выбирать вершину, находящуюся на наименьшем расстоянии от текущей.
Это приводит к другому известному алгоритму поиска на
графе – поиску с равномерной стоимостью (uniform-cost search,
UCS). Предыдущий алгоритм расширял вершины из множества
FRONTIER по принципу «первым пришел – первым ушел», но теперь выбирается вершина с наименьшим накопленным рас­
стоя­нием от начала, то есть с наименьшей стоимостью пройденного пути.
Запустив алгоритм UCS
>>> path, length, parents = g.path_UCS("Hughenden", "Brisbane")
>>> length
1659
>>> ", ".join([p.name for p in path])
'Hughenden, Barcaldine, Blackall, Charleville, Roma, Brisbane'

получаем маршрут на 100 км короче, как показано на рис. 5.8б.
Алгоритм UCS нашел самый короткий маршрут, тогда как BFS
нашел маршрут с наименьшим количеством вершин.
По мере выполнения алгоритма для каждой следующей вершины запоминается вершина, из которой алгоритм пришел
к этой, – родительская вершина. Эта информация возвращается
в словаре parent, например родительская вершина для Winton –
Hughenden.
>>> parents["Winton"]
'Hughenden'

Этот словарь можно преобразовать в непогруженный ориентированный граф
>>> tree = pgraph.DGraph.Dict(parents);

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

showgraphs использует пакет GraphViz
и dot, чтобы создать
графическое изоб­
ражение.

Планирование маршрута с использованием графической карты 255
>>> tree.showgraph()

как показано на рис. 5.9. Это окончательная версия дерева поиска, которая строится постепенно по мере исследования графа.

Рис. 5.9. Дерево поиска, получающееся при использовании алгоритма с равномерной стоимостью

Снова рассмотрим работу алгоритма по шагам.
>>> g.path_UCS("Hughenden", "Brisbane", verbose=True)
FRONTIER: Hughenden(0)
EXPLORED:
expand Hughenden
add Cloncurry to the frontier
add Charters Towers to the frontier
add Barcaldine to the frontier
add Winton to the frontier
move Hughenden to the explored list
FRONTIER: Cloncurry(401), Charters Towers(248), Barcaldine(500),
Winton(216)
EXPLORED: Hughenden
expand Winton
add Longreach to the frontier
add Boulia to the frontier
add Windorah to the frontier
move Winton to the explored list
FRONTIER: Cloncurry(401), Charters Towers(248), Barcaldine(500),
Longreach(396), Boulia(579), Windorah(703)
EXPLORED: Hughenden, Winton
expand Charters Towers
add Townsville to the frontier

256 Глава 5 · Навигация
move Charters Towers to the explored list
...

Значение g(υ), стоимость пройденного пути, указано в скобках, и на каждом шаге для расширения выбирается вершина
с наименьшим его значением. ►
FRONTIER: Emerald(807), Bedourie(773), Charleville(911),
Birdsville(1083), Rockhampton(1105)
EXPLORED: Hughenden, Winton, Charters Towers, Townsville, Longreach,
Cloncurry, Barcaldine, Mount Isa, Boulia, Blackall, Windorah,
Camooweal, Mackay
expand Bedourie
reparent Birdsville: cost 966 via Bedourie is less than cost 1083
via Windorah, change parent from Windorah to Bedourie
move Bedourie to the explored list

Один из соседних городов – Birdsville, который все еще находится в FRONTIER. Стоимость поездки до Birdsville через Windorah
составляет 1083 км, тогда как стоимость поездки до Bedourie составляет 773 км и затем до Birdsville
>>> g["Bedourie"].edgeto(g["Birdsville"]).cost
193

Это означает, что Birdsville находится всего в 773 + 193 = 966 км
от начала маршрута, и мы нашли маршрут, более короткий,
чем уже вычисленный. Алгоритм изменяет родительский узел
Birdsville в дереве поиска, показанном на рис. 5.9, чтобы отра­
зить это.
На третьем и последнем шаге в FRONTIER попадает Brisbane:
FRONTIER: Rockhampton(1077), Roma(1177)
EXPLORED: Hughenden, Winton, Charters Towers, Townsville, Longreach,
Cloncurry, Barcaldine, Mount Isa, Boulia, Blackall, Windorah,
Camooweal, Mackay, Bedourie, Emerald, Charleville, Birdsville
expand Rockhampton
add Brisbane to the frontier
move Rockhampton to the explored list
FRONTIER: Roma(1177), Brisbane(1759)
EXPLORED: Hughenden, Winton, Charters Towers, Townsville, Longreach,
Cloncurry, Barcaldine, Mount Isa, Boulia, Blackall, Windorah,
Camooweal, Mackay, Bedourie, Emerald, Charleville, Birdsville,
Rockhampton
expand Roma
reparent Brisbane: cost 1659 via Roma is less than cost 1759 via
Rockhampton, change parent from Rockhampton to Roma
move Roma to the explored list
FRONTIER: Brisbane(1659)
EXPLORED: Hughenden, Winton, Charters Towers, Townsville, Longreach,

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

Планирование маршрута с использованием графической карты 257
Cloncurry, Barcaldine, Mount Isa, Boulia, Blackall, Windorah,
Camooweal, Mackay, Bedourie, Emerald, Charleville, Birdsville,
Rockhampton, Roma
expand Brisbane
19 vertices explored, 0 remaining on the frontier

но алгоритм не останавливается немедленно, чтобы не пропустить более короткий путь в Brisbane. Фактически на предпоследнем шаге алгоритм находит самый короткий путь через
Roma, а не через Rockhampton. Алгоритм UCS нашел кратчайший путь, но при этом он исследовал каждую вершину, что
может быть затратно в очень больших графах. Следующий алгоритм, который мы представим, может получить тот же результат, исследуя лишь подмножество вершин.
Отступление 5.6. Shakey и трюк A*
Shakey (рис. 5.1) – это мобильная роботизированная система, разрабатывавшаяся в Стэнфордском исследовательском институте
с 1966 по 1972 год. Она служила испытательным стендом для
компьютерного зрения, планирования, навигации и коммуникации с использованием естественного языка – искусственного интеллекта, как его тогда понимали. Многие новые алгоритмы были
впервые интегрированы в физическую систему именно в этом
роботе. Робот представлял собой мобильную тележку с телекамерой, дальномером и датчиками столкновения, подключенными
по радиоканалу к большой ЭВМ PDP-10. Shakey можно увидеть
в Музее компьютерной истории в Маунтин-Вью, штат Калифорния.
Оптимизированный алгоритм поиска на графе A* был опубликован в 1966 году в статье «A formal basis for the heuristic determination of minimum cost paths», написанной членами команды
создателей Shakey Питером Хартом, Нильсом Нильссоном и Бертрамом Рафаэлем.

Хитрость заключается в том, чтобы переписать (5.3) как
f(υ) = g(υ) + h∗(υ),

(5.5)

где h∗(υ) – это оценка расстояния до цели, называемая эвристи­
ческим расстоянием. Часто в качестве такой оценки используется расстояние «по прямой», или евклидово расстояние, которое
легко вычисляется, поскольку наш граф – погруженный и положение каждой вершины известно.
Последний алгоритм, который мы рассмотрим, – это широко
известный поиск A*, использующий эту эвристику. Применяется он аналогично UCS:
>>> path, length, parents = g.path_Astar("Hughenden", "Brisbane",
...
summary=True)

258 Глава 5 · Навигация
12 vertices explored, 3 remaining on the frontier
>>> length
1659
>>> ", ".join([p.name for p in path])
'Hughenden, Barcaldine, Blackall, Charleville, Roma, Brisbane'

и возвращает тот же оптимальный путь, что и UCS, но исследует
только 12 вершин, а не 19, как UCS. Мы можем показать это, выделив исследованные вершины, которые включены в возвращаемое дерево поиска:
>>>
>>>
>>>
>>>

# поиск уникальных названий вершин
visited = set(list(parents.keys()) + list(parents.values()));
g.plot()
g.highlight_vertex(visited, color="yellow")

и показаны на рис. 5.10. Как видите, вершины Camooweal,
Bedourie и Birdsville, находящиеся далеко от маршрута, не были
посещены.

Рис. 5.10. Вершины, исследованные алгоритмом поиска A*

Первые два шага A*:
>>> g.path_Astar("Hughenden", "Brisbane", verbose=True)
FRONTIER: Hughenden(0)
EXPLORED:
expand Hughenden
add Cloncurry to the frontier
add Charters Towers to the frontier
add Barcaldine to the frontier
add Winton to the frontier
move Hughenden to the explored list
FRONTIER: Cloncurry(1878), Charters Towers(1319), Barcaldine(1390),
Winton(1371)

Планирование маршрута с использованием графической карты 259
EXPLORED: Hughenden
expand Charters Towers
add Townsville to the frontier
move Charters Towers to the explored list

И на этот раз значение в скобках – это стоимость g(υ) + h∗(υ),
то есть сумма стоимости пройденного пути (используемой алгоритмом UCS) и оценка стоимости предстоящего перехода.
Производительность алгоритма A* критически зависит от
эвристики. Он гарантированно возвращает путь с наименьшей
стоимостью, только если эвристика допустима, то есть не пере­
оценивает стоимость достижения цели. Евклидово расстояние
является допустимой эвристикой, поскольку представляет минимально возможную стоимость и по определению не может
быть переоценено. Конечно, h∗(υ) = 0 допустимо, но в таком случае A* превращается в эквивалент UCS и посетит все вершины.
По мере увеличения эвристической составляющей стоимости
от нуля до максимально допустимого значения эффективность
возрастает с уменьшением числа исследуемых вершин. Недопустимая эвристика может привести к получению неоптимального пути.

5.3.1

Планирование маршрута
с минимальным временем

В некоторых задачах может потребоваться минимизировать
время, а не расстояние. Минимизация времени важна, когда
есть возможность заменить короткий и медленный маршрут на
более длинный, но быстрый. Продолжим пример из предыдущего раздела, но изменим стоимость ребер, чтобы она отражала скорость движения по дорогам: 100 км/ч для главной дороги
и 50 км/ч для второстепенной. Построим новый граф:
>>> g =
>>> for
...
>>> for
...
...

pgraph.UGraph()
name, info in data["places"].items():
g.add_vertex(name=name, coord=info["utm"])
route in data["routes"]:
g.add_edge(route["start"], route["end"],
cost=route["distance"] / route["speed"])

в котором стоимость ребра теперь измеряется в часах. Перед
запуском A* необходимо определить эвристическую стоимость
для этого случая. В случае минимального расстояния эвристикой служило расстояние по прямой от вершины до цели. В случае минимального времени роль эвристики будет играть время,
необходимое для преодоления пути по прямой от вершины до
цели, но возникает вопрос: с какой скоростью? Напомню, что

260 Глава 5 · Навигация

эвристика не должна переоценивать стоимость достижения
цели. Если выбрать слишком низкую скорость, то на быстрой
дороге мы переоценим время, что сделает эту эвристику недопустимой. А если выбрать слишком высокую скорость, то на
медленной дороге мы недооценим время в пути, а это вполне
допустимо. Поэтому выберем эвристическую стоимость, которая предполагает движение со скоростью 100 км/ч и определяется как
>>> g.heuristic = lambda x: np.linalg.norm(x) / 100

Теперь можно запустить алгоритм A*:
>>> path, time, _ = g.path_Astar("Hughenden", "Brisbane")
>>> time
16.61
>>> ", ".join([p.name for p in path])
'Hughenden, Winton, Longreach, Barcaldine, Blackall, Charleville,
Roma, Brisbane'

И он вернет продолжительность пути в часах. Для сравнения
маршруты с минимальным расстоянием и минимальным временем показаны на рис. 5.11. Как видите, маршрут с минимальным временем минует медленный участок между Hughenden
и Barcaldine, по более быстрому, но и более длинному участку
пути через Winton и Longreach.

а
б
Рис. 5.11. Маршрут A* а) с минимальным расстоянием и б) минимальным временем

5.3.2

Подведение итогов

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

Планирование маршрута с использованием карты занятости 261

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

5.4

Планирование маршрута с использованием
карты занятости

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

5.4.1
Расстояние между
двумя точками
(x1, y1) и (x2, y2),
где Δx = x2 – x1
и Δy = y2 – y1 можно
вычислить как евклидово расстояние
(L2-норму)
или как манхэттенское расстояние
(расстояние городских кварталов,
L1-норму) |Δx| + |Δy|.

Преобразование расстояния

Рассмотрим двухмерный массив нулей, содержащий всего
один ненулевой элемент, представляющий цель, как показано
на рис. 5.12a. Преобразование расстояния в этом массиве – это
другой двухмерный массив той же формы, каждый элемент которого содержит расстояние от исходной ненулевой ячейки.
При планировании маршрута в таком массиве обычно применяются два подхода к измерению расстояний. В Toolbox по
умолчанию используется евклидово расстояние (L2), показанное
на рис. 5.12б, – расстояние до соседних ячеек по горизонтали
и вертикали равно единице, а до соседних ячеек по диагонали –
2. Альтернативой является манхэттенское расстояние (расстоя­

262 Глава 5 · Навигация

ние городских кварталов, или L1), показанное на рис. 5.12в. Расстояние до соседних ячеек по горизонтали и вертикали равно
единице, а перемещение к соседним ячейкам по диагонали запрещено. Преобразование расстояний – это метод обработки
изображений, который мы обсудим далее в разделе 11.6.4.

4

4

2.41

2.0

2.41

2.83

3.83

3

3

1.41

1.0

1.41

2.41

3.41

3.5

3.0

1.0

0.0

1.0

2.0

3.0

1

1

1.41

1.0

1.41

2.41

3.41

0

0

2.41

2.0

2.41

2.83

3.83

0

1

2

3

4

2.0

1.5

а

0

1

3

4

б

4

3.0

2.0

3.0

4.0

5.0

3

2.0

1.0

2.0

3.0

4.0

2

1.0

0.0

1.0

2.0

3.0

1

2.0

1.0

2.0

3.0

4.0

0

3.0

2.0

3.0

4.0

5.0

1

2

3

4

2
x

x

Расстояние

2

2

y

y

2.5

1.0

0.5

0.0

5

4

Расстояние

y

3

2

1

в

0

x

0

Рис. 5.12. Преобразование расстояния: а) целевая ячейка с единичным значением, все
остальные ячейки содержат нули; б) преобразование евклидова расстояния (L2), где значения
в ячейках являются евклидовым расстоянием от целевой ячейки; в) преобразование
манхэттенского расстояния (L1)

Для демонстрации преобразования расстояния для нужд навигации сначала создадим простую сетку занятости:
>>> simplegrid = np.zeros((6, 6));
>>> simplegrid[2:5, 3:5] = 1
>>> simplegrid[3:5, 2] = 1

которая показана на рис. 5.13a. Затем реализуем планировщик
маршрута с преобразованием расстояния и передадим ему сетку занятости:
>>> dx = DistanceTransformPlanner(occgrid=simplegrid);

Планирование маршрута с использованием карты занятости 263
Отступление 5.7. Создание сетки занятости
Создать карту занятости можно множеством способов. Например, можно создать
двухмерный массив NumPy, заполненный нулями (все ячейки свободны):
>>> map = np.zeros((100, 100));
– и использовать операции NumPy, такие как
>>> map[40:50, 20:80] = 1; # установить признак "занято"
чтобы создать препятствия. Установить признак «занято» в 100 случайно выбранных
ячейках можно так:
>>> k = np.random.choice(map.size, 100, replace=False);
>>> map.ravel()[k] = 1;
Мы также могли бы использовать любую программу для рисования, которая создает черно-белое изображение, экспортировать файл изображения и импортировать его в Python с помощью Pillow, OpenCV или SciPy. Например, сетка занятости
на рис. 5.5 была получена из файла изображения, но точно так же можно использовать планы зданий и карты улиц, как будет обсуждаться в разделе 11.1.6.

 Координаты сетки занятости и индексы мат­ри­цы –
не одно и то же
Сетка занятости – это двухмерный массив, координаты которого
традиционно задаются как map[row, column], где row – это вертикальный размер мат­ри­цы. В декартовой системе координат первым
следует индекс, соответствующий горизонтальной координате x,
а вторым – вертикальной координате y, поэтому в программном
коде мат­ри­ца всегда индексируется как map[y, x].

Потом создадим маршрут к конкретной ячейке (1, 1):
>>> dx.plan(goal=(1, 1))

Полученное преобразование расстояния можно визуализировать:
>>> dx.plot(labelvalues=True);

как показано на рис. 5.13б. Числа определяют расстояние от
клетки до цели с учетом обхода препятствий. Для манхэттенского расстояния используется аналогичный набор команд:
>>> dx = DistanceTransformPlanner(occgrid=simplegrid,
...
distance="manhattan");
>>> dx.plan(goal=(1, 1))
>>> dx.plot(labelvalues=True);

а план показан на рис. 5.13в.

264 Глава 5 · Навигация

5

5

4.41

4.0

4.41

5.41

6.41

7.41

4

4

3.41

3.0

nan

nan

nan 6.41

3

3

2.41

2.0

nan

nan

nan 5.41

7

6

а

Расстояние

4

y

y

5

2

2

1.41

1.0

1.41

nan

nan 4.41

1

1

1.0

0.0

1.0

2.0

3.0

4.0

0

0

1.41

1.0

1.41

2.41

3.41

4.41

0

1

2

3

4

5

0

1

2

3

x

4

5

б

x

3

2

1

0

8
5

5.0

4.0

5.0

6.0

7.0

8.0

4

4.0

3.0

na0

na0

na0 7.0

3

3.0

2.0

nan

nan

nan

6.0

2

2.0

1.0

2.0

nan

nan

5.0

1

1.0

0.0

1.0

2.0

3.0

4.0

0

2.0

1.0

2.0

3.0

4.0

5.0

0

1

2

3

4

5

7
6
5

в

x

Расстояние

y

4
3
2
1
0

Рис. 5.13. Планирование маршрута с помощью преобразования расстояния: а) сетка
занятости представлена изображением, где белые пиксели соответствуют занятым
ячейкам; б) преобразование евклидова расстояния (L2), красные пиксели – это препятствия;
в) преобразование манхэттенского расстояния (L1). Числа определяют расстояние от данной
ячейки до цели, обозначенной звездочкой

Маршрут к цели неявно заложен в карту расстояний. Оптимальный путь к цели из любой ячейки пролегает через соседнюю ячейку с наименьшим расстоянием до цели. Процесс выбора ячейки для следующего шага повторяется до тех пор, пока
робот не достигнет ячейки с нулевым расстоянием, которая
и является целью. Если отобразить расстояние до цели в виде
трехмерной поверхности:
>>> dx.plot_3d();

как показано на рис. 5.14а, то планирование маршрута к цели
сводится к простому спуску по функции расстояния от любой
начальной точки. Мы преобразовали довольно сложную задачу
планирования в задачу, которая под силу роботу класса тележки
Брайтенберга, принимающему локальные решения на основе
расстояния до цели.
Оптимальный путь от произвольно выбранной начальной точки (5, 4) до цели получается путем выполнения запроса к плану:

Планирование маршрута с использованием карты занятости 265
>>> path =
array([[5,
[5,
[5,
[4,
[3,
[2,
[1,

dx.query(start=(5, 4))
4],
3],
2],
1],
1],
1],
1]])

В этом типе плани- который возвращает список ячеек, составляющих маршрут от
ровщика имеет мес­ начала до цели.
Этот маршрут можно наложить на сетку зато двойственность
нятости
между начальной
и конечной ячейками. По умолчанию >>> dx.plot(path);
план основывается
на местоположении как показано на рис. 5.14б. Также можно анимировать движецели, и к нему выние робота по траектории от начальной ячейки к цели
полняется запрос,
определяющий
маршрут к началь- >>> dx.query(start=(5, 4), animate=True);
ной ячейке, однако
план можно осно- и увидеть последовательность зеленых точек, отражающих путь
вать на начальной робота к цели.
позиции запрашивать маршрут
к конечной цели.
7
5

6
4
5

Расстояние

4

5

y

Расстояние до цели

3

4

2

3
2

3

6

1

5

0

2

1

4
6

5

3
4
Y

3

2
2

1

X

1
0

1
0

0

0

1

2

3

4

x
а
б
Рис. 5.14. а) Преобразование расстояния как трехмерная функция, где высота – это
расстояние до цели. Путь к цели – это просто спуск; б) преобразование расстояния
с наложенным маршрутом движения

5

0

Преобразование расстояний значительно упрощает задачу навигации по дому. Мы загружаем план дома и именованные места:
>>> house = rtb_load_matfile("data/house.mat");
>>> floorplan = house["floorplan"];
>>> places = house["places"];

Задаем цель в кухне и выполняем преобразование расстояний:
>>> dx = DistanceTransformPlanner(occgrid=floorplan);
>>> dx.plan(goal=places.kitchen)
>>> dx.plot();

266 Глава 5 · Навигация

Результат изображен на рис. 5.15. Расстояние от любой точки до цели на карте показано в количестве ячеек сетки, которые нужно миновать с учетом обхода препятствий. Вычислим
маршрут до цели от спальни номер три:
>>> path = dx.query(start=places.br3);
>>> path.shape
(338, 2)

600
350
500

300

400

200

300

150
200

100

100

50
0

Расстояние

y

250

0

100

200

300
x

400

500

600

0

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

Для большей наглядности его можно наложить на сетку занятости и карту расстояний (рис. 5.15):
>>> dx.plot(path);

Этот алгоритм навигации использует свое глобальное видение мира и с помощью исчерпывающих вычислений отыскивает кратчайший возможный путь длиной в 338 шагов. Для сравнения, алгоритм bug2 из раздела 5.1.2, не имеющий глобального
представления, проложил маршрут длиной в 1307 шагов – почти
в четыре раза длиннее. Платой за прокладывание оптимального маршрута являются первоначальные вычислительные затраты. Данная реализация преобразования расстояний является
итеративной. Стоимость каждой итерации составляет O(N 2),
а количество итераций не менее O(N), где N – размер карты.
В данном примере выполнение с параметром summary
>>> dx.plan(places.kitchen, summary=True);
571 iterations, 9925 unreachable cells

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

Планирование маршрута с использованием карты занятости 267

можно использовать для построения маршрута от любой начальной точки до цели. Однако для сеток с большим количест­
вом препятствий такой подход к планированию становится
непрактичным. Методы построения дорожных карт, которые
мы обсудим далее в этой главе, предлагают более эффективные
алгоритмы поиска путей на больших картах при значительно
меньших вычислительных затратах.
Итерации алгоритма преобразования расстояний можно визуализировать
>>> dx.plan(places.kitchen, animate=True);
Особенность
распространения
значения расстоя­
ния от цели послужила причиной
появления других
названий, таких как
алгоритм фронта
волны, алгоритм
лесного пожара или
алгоритм степного
пожара.

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

и увидеть значения расстояний, распространяющихся в виде
волнового фронта наружу от цели. Фронт волны движется наружу, распространяется через дверные проемы в соседние комнаты и за пределы дома. Вдоль левого и правого краев карты
расстояний имеются разрывы, где сталкиваются два волновых
фронта. Эти столкновения определяют два разных пути к цели.
С одной стороны столкновения оптимальный путь направлен
вверх к цели, а с другой – вниз.
Ячейки, полностью окруженные препятствиями, никогда не
могут быть достигнуты фронтом волны. В примере выше таких
ячеек 9925, и они отмечены синим цветом на рис. 5.15.
Ячейки сетки занятости имеют размер 45×45 мм, и мы
предполагаем, что робот занимает одну ячейку сетки – это
очень маленький робот. Поэтому планировщик смог бы найти пути через зазоры, через которые более крупный реальный
робот не смог бы пройти. Распространенное решение этой
проблемы – расширить размеры препятствий: более крупные
препятствия и маленький робот эквивалентны фактическим
препятствиям и более крупному роботу. Например, если робот помещается в круг диаметром 500 мм (11 ячеек), то можем уменьшить его размеры на 5 ячеек во всех направлениях так, чтобы на карте он занимал только одну ячейку сетки,
а для компенсации расширяем размеры всех препятствий на
5 ячеек во всех направлениях – это похоже на нанесение очень
толс­того слоя краски .
>>>
>>>
>>>
>>>

dx = DistanceTransformPlanner(occgrid=floorplan, inflate=5);
dx.plan(places.kitchen);
p = dx.query(places.br3);
dx.plot(p);

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

268 Глава 5 · Навигация

600

350
300

y

250

400

200
300

Расстояние

500

150
200

100

100

50
0

0

100

200

300
x

400

500

0

Рис. 5.16. Маршрут, построенный алгоритмом преобразования расстояния при расширении
размеров препятствий на 5 ячеек. Прежние препятствия показаны красным, а ячейки,
расширяющие препятствия, – розовым

5.4.2

Алгоритм D*

D* – хорошо известный и популярный алгоритм вычисления
маршрута движения робота. Подобно преобразованию расстоя­
ний, он использует сетку занятости и находит оптимальный
путь. За кулисами D* преобразует сетку занятости в граф и затем отыскивает оптимальный путь с помощью A*-подобного
алгоритма. D* обладает некоторыми свойствами, полезными
в реальных приложениях. Во-первых, он обобщает сетку занятости до карты стоимости, которая представляет стоимость
c ∈ ℝ>0 пересечения каждой ячейки в горизонтальном или вертикальном направлении. Стоимость пересечения ячейки по
диагонали равна 2. Для ячеек, соответствующих препятствиям, c = ∞ (np.inf в NumPy). Чтобы найти маршрут, кратчайший
по времени, нужно присвоить каждой ячейке время ее пересечения. Чтобы найти маршрут, минимизирующий вероятность
повреждения транспортного средства или максимизирующий
комфорт пассажиров, стоимость пересечения ячейки может
быть выражена степенью неровности рельефа.
Во-вторых, D* поддерживает инкрементное перепланирование. Это важно, если во время движения обнаружится, что мир
отличается от карты. Если вдруг фактическая стоимость пересечения ячейки изменится в большую или меньшую сторону,
то алгоритм сможет инкрементно перепланировать остаток
пути. Инкрементное перепланирование требует меньших вычислительных затрат, чем полное перепланирование, которое
потребовалось бы при использовании только что рассмотренного метода преобразования расстояний.

Название «D*»
обозначает «dynamic A*». D* – это
расширенная версия
алгоритма A* поиска
маршрута с минимальной стои­
мостью через граф,
поддерживающая
эффективное перепланирование при
изменении стои­
мости маршрута.
Стоимости пересечения ячеек также
зависят от характеристик транспортного средства
и особенностей
окружения. Например, для большого
полноприводного
автомобиля преодоление неровностей
может иметь конечную стоимость, тогда
как для небольшого
автомобиля эта стои­
мость может быть
бесконечной, проще
говоря, преодолимый маршрут для
внедорожника может
оказаться непреодолимым для седана.

Планирование маршрута с использованием карты занятости 269

Для реализации планировщика на основе алгоритма D* с помощью Toolbox можно использовать тот же шаблон, что и раньше: создать объект навигации D*
>>> dstar = DstarPlanner(occgrid=floorplan);

который преобразует исходную сетку занятости в карту стоимости
>>> c = dstar.costmap;

Элементы c будут содержать значения 1 или ∞, представляющие свободные и занятые ячейки соответственно. Это граничные стоимости во внутреннем графовом представлении сетки.
Планировщику D* также вместо сетки занятости можно передать карту стоимости, объект CostMap. Маршрут движения к цели
на кухне в этом случае формируется вызовом
>>> dstar.plan(goal=places.kitchen);

который создает плотный ориентированный граф, в котором
каждая ячейка (вершина) хранит расстояние до цели и ссылку
на соседнюю ячейку, ближайшую к цели. Как и в алгоритмах поиска по графу, обсуждавшихся в разделе 5.3, здесь есть список
ячеек для исследования и исследованных. Реализация на Python работает довольно медленно: карта для нашего примера
обрабатывается десятки секунд, а общее количество развернутых вершин равно
>>> nexpand0 = dstar.nexpand
359764

Маршрут от произвольной начальной точки к цели
>>> path = dstar.query(start=places.br3);

почти идентичен тому, что дает планировщик, основанный на
алгоритме преобразования расстояний (рис. 5.15).
Настоящая мощь D* заключается в возможности эффективно менять карту стоимости в процессе движения. Это довольно
распространенное требование в робототехнике, потому что реальные датчики имеют ограниченный диапазон, и робот продолжает обнаруживать все больше объектов по мере движения.
Мы информируем D* об изменениях с помощью аргумента sensor, представляющего функциональный объект, который обновляет карту стоимости по мере движения. Чтобы заблокировать
нижний дверной проем на кухню, когда робот почти достиг ее,
мы определяем функцию
>>> def sensorfunc(pos):
...
if pos[0] == 300: # вблизи от двери?

270 Глава 5 · Навигация
...
...
...
...
...

changes = []
for x in range(300, 325):
for y in range(115,125):
changes.append((x, y, np.inf))
return changes

где pos – текущее местоположение робота. Функция возвращает
список кортежей изменений, включающий координаты ячейки
карты и новую стоимость. Если теперь запросить маршрут на
кухню
>>> dstar.query(start=places.br3, sensor=sensorfunc);

то алгоритм проложит оптимальный маршрут до нижнего дверного проема на кухню, пока робот не прибудет туда и не обнаружит, что дверь закрыта. Функция датчика sensorfunc присваивает бесконечную стоимость небольшой прямоугольной области
поперек дверного проема, которая показана заштрихованной
на рис. 5.17. Изменения вынуждают D* пересчитать маршрут
и перестроить вершины в дереве поиска. После завершения
пересчета оптимальным маршрутом с учетом изменившейся
среды опять будет перемещение в соседнюю ячейку с наименьшей стоимостью. Робот вернется назад и пройдет в кухню через
верхний дверной проем, как показано на рис. 5.17. Число вершин, дополнительно развернутых для учета изменения, равно
>>> dstar.nexpand - nexpand0
42792

что составляет около 12 % от первоначального количества. D*
позволяет изменять карту в любой момент во время движения
робота.

350
300

y

250
200
150
100
50
0

0

100

200

300
x

400

500

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

Планирование маршрута с использованием дорожных карт 271

5.5

Планирование маршрута
с использованием дорожных карт

Анализ карты при планировании маршрута называется фазой
планирования, а использование результатов фазы планирования
для поиска пути из точки A в точку Б называется фазой запро­
са. Алгоритмы, представленные в разделе 5.4, – преобразования
расстояний и D* – требуют выполнения значительного объема
вычислений в фазе планирования, тогда как фаза запроса очень
экономична. Однако в обоих случаях маршрут зависит от цели,
поэтому при ее изменении дорогостоящая фаза планирования
должна быть выполнена повторно. D* позволяет пересчитать
маршрут во время запроса, но не поддерживает изменение цели.
Различия в затратах на фазы планирования и запроса привели к разработке методов дорожных карт, в которых запрос
может включать как начальную, так и конечную позицию.
В фа­зе планирования выполняется анализ карты, на которую
нанесены все начальные и конечные точки. Хорошей аналогией может служить поездка на поезде. Сначала мы находим путь
к ближайшей железнодорожной станции, проезжаем по железнодорожной сети, выходим на станции, ближайшей к цели,
а затем выбираем путь к цели. Железнодорожная сеть инвариантна, и спланировать маршрут через нее с использованием
методов, подобных тем, что мы обсуждали в разделе 5.3, совсем
несложно. Планирование путей к железнодорожной станции от
начальной точки и от железнодорожной станции к цели тоже
несложно, поскольку они, скорее всего, будут короткими и для
их планирования можно использовать методы сетки занятости,
представленные в разделе 5.4.
Задача навигации робота сводится к построению сети путей
без препятствий в окружающей среде, которая играет роль железнодорожной сети. В планировании маршрута робота такая
сеть называется дорожной картой. Дорожную карту нужно вычислить только один раз, и затем ее можно использовать, как
и сеть движения поездов, для перемещения из любой начальной в любую конечную точку.
Проиллюстрируем эти принципы, создав дорожную карту
на основе сетки занятости и используя некоторые методы обработки изображений, как показано на рис. 5.18. Первый шаг –
создание копии сетки занятости, импортированной ранее:
>>> occgrid = floorplan.copy();

и добавление периметра препятствия по внешней границе:
>>> occgrid[0, :] = 1
>>> occgrid[-1, :] = 1

272 Глава 5 · Навигация
>>> occgrid[:, 0] = 1
>>> occgrid[:, -1] = 1

Далее воспользуемся инструментами обработки изображений из Toolbox, чтобы преобразовать сетку занятости в изображение свободного пространства и отобразить его:
>>> freespace = Image(occgrid == 0)
Image: 596 x 397 (bool)
>>> freespace.disp();

Результат показан на рис. 5.18а. Свободные ячейки, имеющие значение True, отображаются белыми пикселями, а занятые
ячейки, имеющие значение False, отображаются черными пикселями. Топологический скелет свободного пространства вычисляется с помощью морфологического алгоритма истончения

Также известен как
алгоритм скелетизации. Подробнее
о нем мы поговорим
в разделе 11.6.3.

>>> skeleton = freespace.thin().disp();

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

50
100

150

150

200

200

y

y

50
100

250

250

300

300

350

350

а

в

0

0

100

200

300
x

400

б

500

0

0

50

50

100

100

150

150

200

200

y

y

0

250

250

300

300

350

350

0

100

200

300
x

400

500

г

0

100

200

300

400

500

0

100

200

300

400

500

x

x

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

Планирование маршрута с использованием дорожных карт 273
Черными точками
на карте обозначены перекрестки.
Перекрестки, или
тройные точки,
найдены с помощью
метода морфологической обработки
изображений
triplepoint.

На рис. 5.18в показана сеть свободного пространства, наложенная на исходную карту. Мы создали сеть путей, охватывающую свободное пространство и позволяющую беспрепятственно перемещаться по карте. Эти пути являются ребрами
обобщенной диаграммы Вороного. Аналогичный результат
можно получить, вычислив преобразование расстояний до препятствий (рис. 5.18а), как показано на рис. 5.18г. Значение каждого пикселя соответствует расстоянию до ближайшего препятствия, как следствие, наиболее яркие линии соответствуют
скелету (рис. 5.18б). Истончение, или скелетизация, как и преобразование расстояний, – это итеративный алгоритм, требующий больших вычислительных затрат, но он иллюстрирует
принципы поиска путей в свободном пространстве.
Высокая вычислительная стоимость методов преобразования расстояний и скелетизации делает их неприменимыми
для больших карт, что привело к появлению вероятностных
методов. Эти методы используют разреженную выборку карты,
и наиболее известным из них является метод вероятностной
дорожной карты (probabilistic roadmap, PRM).
Поиск маршрута – это двухэтапный процесс: планирование
и поиск. Сначала создается объект планировщика PRM

Аргумент seed=0 >>> prm = PRMPlanner(occgrid=floorplan, seed=0);
устанавливает наИ затем он используется для создания плана
чальное значение генератора
случайных чисел, >>> prm.plan(npoints=50)
используемого этим
планировщиком. Это представляющего дорожную карту в виде погруженного графа
гарантирует повтос 50 вершинами. Обратите внимание, что план – дорожная карряемость получаемого результата. та – не зависит от начальной и конечной точек маршрута.

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

Наличие препятствий проверяется
выборкой фиксированного количества
>>> prm
точек по всей длине
пути. PRMPlanner:

BinaryOccupancyGrid: 596 x 397, cell size=1, x = [0.0, 595.0],
y = [0.0, 396.0], 8.8% occupied
UGraph: 50 vertices, 222 edges, 12 components

в которой указывается количество ребер и связных компонентов графа. Граф можно визуализировать (рис. 5.19а) командой
>>> prm.plot();

Точки – это случайно выбранные вершины погруженного графа. Линии – пути без препятствий между парами точек – ребра

274 Глава 5 · Навигация
Отступление 5.8. Диаграмма Вороного
Диаграмма Вороного отображает множест­
во точек на плоскости, известных как узлы,
в виде набора многоугольных ячеек. Каждая
ячейка соответствует узлу и состоит из всех
точек, расположенных ближе к этому узлу, чем
к любому другому узлу. Грани ячеек – это точки, равноудаленные от двух ближайших узлов.
Вот как можно сгенерировать диаграмму Вороного на Python:
>>> sites = np.random.uniform(size=(2,9));
>>> from scipy.spatial import Voronoi
>>> vor = Voronoi(sites.T);
Обобщенная диаграмма Вороного состоит из ячеек, определяемых путем измерения расстояний до объектов конечного размера, а не до точек.
Георгий Вороной (1868–1908) – русский математик, родившийся на территории
современной Украины. Учился в Санкт-Пе­тер­бургском университете, был учеником Андрея Маркова.
Один из его учеников, Борис Делоне, определил одноименную триангуляцию,
имеющую двойственные свойства с диаграммой Вороного.

графа, стоимость которых равна длине линии. Этот граф имеет
222 ребра и содержит 12 компонентов. Каждому компоненту
назначен уникальный цвет, соответственно, цвет вершин и ребер указывает, к какому компоненту графа они принадлежат.
В графе имеется два больших компонента, но несколько комнат не связаны с другими, и это означает, что нет возможности
спланировать путь к этим комнатам или из них. Преимущество
PRM заключается в том, что для подтверждения отсутствия
препятствий в точках и путях между ними требуется проверить
относительно небольшое количество точек.
Чтобы улучшить связность, можно запустить планировщик
еще раз – он выберет другие случайные точки, которые, возможно, обеспечат лучшую связность. Обычно планировщик повторяет итерации до тех пор, пока не будет найден путь к заданной
цели, при этом нет гарантии, что будет найден правильный план.
Вероятность успеха можно увеличить, указав больше точек.
>>> prm.plan(npoints=300)
>>> prm.plot();
>>> prm
PRMPlanner:
BinaryOccupancyGrid: 596 x 397, cell size=1, x = [0.0, 595.0],
y = [0.0, 396.0], 8.8% occupied
UGraph: 300 vertices, 10000 edges, 13 components

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

350

350

300

300

250

250

200

200

150

150

100

100

50

50

0

а

y

y

Планирование маршрута с использованием дорожных карт 275

0

100

200

300
x

400

500

0

б

0

100

200

300
x

400

500

Рис. 5.19. Планировщик вероятностной дорожной карты (PRM) и случайные графы, созданные на этапе
планирования: а) слабосвязный граф дорожной карты с 50 вершинами; б) хорошо связный граф дорожной карты
с 300 вершинами

Фаза запроса отыскивает маршрут от начальной точки до
цели. Для этого просто производится перемещение от вершины
к вершине до цели, следуя алгоритму A*. Следующий код решает задачу навигации по дому
>>> path = prm.query(start=places.br3, goal=places.kitchen);
>>> prm.plot(path)

Отступление 5.9. Случайные числа
Генерирование хороших случайных чисел – нетривиальная задача. Генератор случайных чисел (ГСЧ) генерирует очень длинную, но конечную последовательность
чисел, которая является хорошим приближением к случайной последовательности. Генератор поддерживает внутреннее состояние, которое фактически является
позицией внутри последовательности. При запуске NumPy обращается к операционной системе для получения начальной случайной позиции в этой последовательности. Благодаря этому при каждом запуске программа будет получать разное
первое случайное число.
Многие алгоритмы, обсуждаемые в данной книге, используют случайные числа, а это означает, что результаты не получится воспроизвести в точности. Чтобы
решить эту проблему, можно принудительно задавать начальное значение для последовательности случайных чисел.
>>> np.random.rand(5) #
array([ 0.7512, 0.4447,
>>> np.random.seed(42)
>>> np.random.rand(5) #
array([ 0.3745, 0.9507,
>>> np.random.seed(42)
>>> np.random.rand(5) #
array([ 0.3745, 0.9507,

начальное число задает NumPy (не повторяется)
0.2153, 0.9116, 0.692])
с заданным начальным числом 42
0.732, 0.5987, 0.156])
с заданным начальным числом 42
0.732, 0.5987, 0.156])

И как видите, случайную последовательность можно воспроизвести повторно.
Чтобы сделать примеры в этой книге повторяемыми, объекты Toolbox, использующие случайные числа, имеют свой собственный «приватный» ГСЧ, и его начальное число можно задать явно, обычно с помощью аргумента seed конструктора.

276 Глава 5 · Навигация

и генерирует довольно оптимальный маршрут, как показано на
рис. 5.20. Обратите внимание, что фазе запроса передаются начальная и конечная точки. Преимущество этого планировщика
в том, что после создания дорожной карты на этапе планирования он позволяет изменить начальную и конечную точки, достаточно лишь повторить фазу запроса. Путь
>>> path = prm.query(start=places.br2, goal=places.kitchen);
>>> path.shape
(8, 2)

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

220
200

350

180

250

160

y

y

300

200

140

150

120

100

100

50
80
0

а

0

100

200

300
x

400

500

б

260

280

300

320

x

340

360

380

400

Рис. 5.20. Планировщик вероятностной дорожной карты (PRM): а) путь робота через вершины дорожной карты,
выделенный линией с желто-черными полосами; б) крупный план целевой области, где виден кратчайший путь от
вершины дорожной карты до цели

Для большей вычислительной эффективности необходимо
принять ряд важных компромиссов:
случайная выборка точек свободного пространства озна­
чает, что при каждом запуске планировщик создает другую дорожную карту и в результате получает разные марш­
руты;
„„ планировщик может потерпеть неудачу, создав сеть, со­
стоя­щую из непересекающихся компонентов, как показано на рис. 5.19a. Если начальная и конечная вершины
не связаны дорожной картой, то есть находятся близко
„„

Планирование проходимого маршрута 277

к разным компонентам, то метод query сообщит об ошибке. Единственное решение – перезапустить планировщик
и/или увеличить количество вершин. Планировщик можно
запускать снова и снова, пока не будет найден путь между
начальной и конечной точками;
„„ длинные узкие промежутки между препятствиями, такие
как коридоры, вряд ли будут использоваться, потому что
вероятность случайно выбрать точки, лежащие в пределах
таких пространств, очень мала;
„„ генерируемые пути не такие гладкие, как те, которые генерируются с помощью преобразования расстояния и D*,
и могут включать неоптимальные изгибы или очень крутые повороты.
Вычислительные преимущества PRM распространяются и на
задачи многомерного планирования, такие как трехмерная
сборка или укладка белков. Применение PRM для планирования пути без препятствий в пространстве конфигураций транспортных средств мы рассмотрим в разделе 5.6.5.

5.6

Планирование проходимого маршрута

Планировщики, представленные выше, могут создавать оптимальные и допустимые, но не обязательно реализуемые пути, то
есть реальное транспортное средство может не иметь возможности проехать по этому пути. Контроллер следования вдоль
произвольного пути, например описанный в разделе 4.1.1.3,
будет направлять робота по заданному пути, но ограничения
рулевого механизма не смогут обеспечить точность следования этому пути. Не имея возможности точно следовать по пути
без столкновений, нельзя гарантировать движение робота без
столк­новения с препятствиями.
Альтернатива – спланировать такой маршрут с учетом проходимости робота. Далее мы рассмотрим два планировщика,
которые учитывают особенности движения модели транспортного средства и убирают наше предположение о способности
робота двигаться во всех направлениях.
В главе 4 мы моделировали движение колесных транспортных средств и поговорили о том, как меняется их конфигурация
в зависимости от таких входных данных, как параметры рулевого управления и скорость. Итак, двухколесная модель движется
по дуге радиусом L /tan γ, где γ – угол поворота управляемого колеса, а L – длина колесной базы. В этом разделе нам пригодится
кривизна траектории, проходимой роботом, которая является

278 Глава 5 · Навигация

обратной величиной радиуса дуги. Для двухколесной модели ее
можно записать так:
(5.6)
При γ = 0 кривизна равна нулю, что соответствует прямой линии. Для реального транспортного средства γ ∈ [-γm, γm], и траектория с наибольшей кривизной, по которой может следовать
транспортное средство, равна
(5.7)
Рассмотрим задачу о перемещении вбок колесного транспортного средства, похожую на задачу о параллельной парковке. Определим начальную и конечную конфигурации:
>>> qs = (0, 0, pi/2);
>>> qg = (1, 0, pi/2);

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

5.6.1

Планировщик пути Дубинса

Путь Дубинса – это кратчайшая кривая, соединяющая две плос­
кие конфигурации с ограничением на кривизну траектории
и предполагающая только поступательное движение вперед.
Хо­тя требование на поступательное движение исключительно вперед может показаться весьма ограничивающим, следует
помнить, что это ограничение свойственно многим транспортным средствам, таким как корабли и самолеты.
Создадим экземпляр планировщика пути Дубинса для максимальной кривизны, равной единице:
>>> dubins = DubinsPlanner(curvature=1)

и используем его для вычисления пути Дубинса:
>>> path, status = dubins.query(qs, qg)
>>> path.shape
(74, 3)

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

Планирование проходимого маршрута 279

цию транспортного средства (x, y, θ). Мы можем построить этот
путь (рис. 5.21а) командой
>>> dubins.plot(path);
1.00
1.00

0.75

0.75

0.50

0.50

0.25

0.25
y

y

0.00

0.00
−0.25

−0.25

−0.50

−0.50

−0.75

−0.75
−1.00

а

−1.00
−2.0

−1.5

−1.0

−0.5
x

0.0

0.5

1.0

б

0.00

0.25

0.50

0.75

1.00
x

1.25

1.50

1.75

2.00

Рис. 5.21. Непрерывный путь для перемещения вбок: а) путь Дубинса; б) путь Ридса–Шеппа. Начальная
конфигурация обозначена пустым контуром транспортного средства, а конечная – закрашенным. Движение вперед
обозначено линией c желто-черными полосами, движение назад – с красно-черными полосами

Планировщик возвращает также дополнительную информацию о состоянии:
>>> status

Здесь можно DubinsStatus(segments=['L', 'S', 'L'], length=7.283,
использовать форseglengths=[4.712, 1.0, 1.5708])
мальные понятия
непрерывности.
Непрерывность C 0 сообщающую, что путь включает левый поворот, прямой учас­
означает, что конец ток и еще один левый поворот. Общая длина пути равна 7.283,
одного сегмента а длины сегментов определяются массивом seglengths. Они
совпадает с началом следующего. составляют 75 % окружности (3π/2), прямую линию длиной 1
Непрерывность C 1 и 25 % окружности (π/2). Точки пути отстоят друг от друга через
означает, что каса- дискретные интервалы, и в данном случае интервал, равный
тельные в этих двух
точках одинаковы. 0.1, является параметром конструктора DubinsPath.
Также можно построить траекторию в трехмерном конфигуНепрерывность C2
означает, что кри- рационном пространстве (рис. 5.22а).
визна в этих двух
точках одинакова. >>> dubins.plot(path, configspace=True);
В данном случае
мы имеем непреПуть непрерывен, и наклон касательных изменяется непрерывность C 0 и C 1,
но не C 2. рывно, но кривизна не непрерывна. ◄

5.6.2

Планировщик пути Ридса–Шеппа

Для планирования маршрутов транспортных средств, способных двигаться вперед и назад, можно использовать планировщик Ридса–Шеппа

280 Глава 5 · Навигация
>>> rs = ReedsSheppPlanner(curvature=1)
>>> path, status = rs.query(qs, qg)
>>> path.shape
(66, 3)
>>> status
ReedsSheppStatus(segments=['R', 'L', 'R'], length=6.283,
seglengths=[4.460, -0.5054, 1.318],
direction=[1, 1, 1,... -1, -1, -1, -1, -1, ... 1, 1, 1])

Объект status в этом случае сообщает, что путь включает поворот направо, поворот налево, но с движением назад, которое
обозначается знаком при значении длины соответствующего
сегмента, а затем еще один поворот направо с движением вперед. Направление в каждой точке пути задается атрибутом direction – эти значения равны 1 для движения вперед и -1 для
движения назад. Мы можем нарисовать путь (рис. 5.21б), представив направление цветом.
>>> rs.plot(path, direction=status.direction);

Транспортное средство движется вперед по большой дуге,
делает короткий разворот назад, а затем движется вперед к заданной конфигурации.
Траектория в трехмерном конфигурационном пространстве
>>> rs.plot(path, configspace=True);

показана на рис. 5.22б.

8
1

7

θ

5

−1

θ

0

6

−2

4

−3

3

−4

2

−5

−2.0

−1.5

а

−1.0

−0.5
x

0.0

0.5

1.0

1.00
0.75
0.50
0.25
0.00
y
−0.25
−0.50
−0.75
−1.00

0.00

б

0.25

0.50

0.75

1.00
1.25
1.50
x

1.75

2.00

1.00
0.75
0.50
0.25
0.00
y
−0.25
−0.50
−0.75
−1.00

Рис. 5.22. Пути в конфигурационном пространстве: а) путь Дубинса; б) путь Ридса–Шеппа. Начальная конфигурация
обозначена кружком, а конечная – звездочкой

5.6.3

Планирование по решетке

Планировщик маршрута по решетке вычисляет путь между
дискретными точками в трехмерном конфигурационном про-

Планирование проходимого маршрута 281
Шаг сетки, горизонтальное и вертикальное расстояния
между вершинами,
определяется радиусом поворота
транспортного
средства.

странстве робота. Взгляните на диаграмму слева на рис. 5.23,
где робот, находящийся в начале координат, начинает движение вперед к трем узлам решетки, обозначенным синими точками. Каждый путь представляет собой дугу с кривизной κ ∈
{1, 0, -1}, а направление движения робота в конце каждой дуги
равно θ ∈ {0, π/2, π, -π/2} радиан. Создать эту диаграмму можно
следующим образом:
>>> lp = LatticePlanner();
>>> lp.plan(iterations=1, summary=True)
4 vertices and 3 edges created
>>> lp.plot()

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

3

3

2

2

2

1

1

1

0

0

0

y

3

y

y

>>> lp.plan(iterations=2, summary=True)
13 vertices and 12 edges created
>>> lp.plot()

−1

−1

−1

−2

−2

−2

−3
−1

0

1
x

2

3

−3
−1

0

1
x

2

3

−3
−1

0

1
x

2

3

Рис. 5.23. План решетки после 1, 2 и 8 итераций. Начальная конфигурация: (0, 0, 0)

Теперь граф содержит 13 вершин и 9 путей длиной по 2 сегмента каждый. Каждая вершина представлена конфигурацией
(x, y, θ), а не просто местоположением, и мы можем построить
вершины и пути в конфигурационном пространстве (рис. 5.24):
>>> lp.plot(configspace=True)

Увеличив количество итераций:
>>> lp.plan(iterations=8, summary=True)
780 vertices and 1698 edges created
>>> lp.plot()

282 Глава 5 · Навигация

можно добавить больше возможных траекторий (диаграмма
справа на рис. 5.23), и теперь траектории выходят далеко за
пределы показанной области. Для каждой двухмерной системы
координат существует всего четыре возможных значения угла
курса θ ∈ {0, π/2, -π, -π/2}. Дуга поворота влево из θ = π/2 даст
угол курса θ = -π.

3
2

0

θ

1

−1
−2
−3
−1

0
x

1

2

3

−3

−2

−1

0

y

1

2

3

Рис. 5.24. План решетки после
двух итераций в трехмерном
конфигурационном пространстве

Теперь, создав граф, можно вычислить путь между любыми
двумя конфигурациями, используя метод query. Для той же задачи, что и на рис. 5.21:
>>> path, status = lp.query(qs, qg);
>>> path.shape
(6, 3)

путь с наименьшей стоимостью представляет собой ряд точек
в конфигурационном пространстве, каждая из которых определяет координаты (x, y, θ) вершины в решетке вдоль пути от начальной до конечной конфигурации. Путь, наложенный на решетку:
>>> lp.plot(path)

показан на рис. 5.25а и напоминает кривую Дубинса, которую
мы видели на рис. 5.21а.
Внутренне для поиска пути в графе решетки используется
алгоритм A*, а в качестве стоимости ребер принимается длина
дуг. Атрибут status возвращает объект
>>> status
LatticeStatus(cost=7.283, segments=['L', 'L', 'L', 'S', 'L'],
edges=[Edge{[0SRRRS] -- [0SLSLLR], cost=1.571},
Edge{[0SLSLLR] -- [0LLSLS], cost=1.571},
Edge{[0LLSLS] -- [0LLSLSL], cost=1.571},
Edge{[0LLSLSL] -- [0LLLSL], cost=1},
Edge{[0LLLSL] -- [0SSRRRS], cost=1.571}])

содержащий сегменты пути, список ребер
и их стоимости.
Стоимость ребра может быть равна π/2 или 1, но мы можем при-

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

Планирование проходимого маршрута 283

менить штраф к ребрам определенного типа, например увеличить стоимость поворота налево:

3

3

2

2

1

1

0

0

y

y

>>> lattice = LatticePlanner(costs=[1, 10, 1]) # S, L, R
>>> lp.plan(iterations=8)
>>> path, status = lp.query(qs, qg)

−1

−1

−2

−2

−3
−2

−1

0

1

2

3

−3
−2

−1

0

1

2

x
а
б
Рис. 5.25. Планирование маршрута перемещения вбок по решетке: а) с равномерной
стоимостью; б) с увеличенным штрафом за повороты налево

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

x

3

Результат показан на рис. 5.25б. Возможность управлять стои­
мостью ребер позволяет контролировать выбор направления,
чего не позволяет планировщик Дубинса.
Еще одно преимущество планирования по решетке по сравнению с планировщиком Дубинса – возможность интеграции
с двухмерной сеткой занятости. Для иллюстрации создадим
сначала объект двухмерной сетки с ячейками единичного
размера, инициализированными значением False (нет препятствия):
>>> og = BinaryOccupancyGrid(workspace=[-5, 5, -5, 5], value=False)
BinaryOccupancyGrid: 11 x 11, cell size=1, x = [-5.0, 5.0],
y = [-5.0, 5.0], 0.0% occupied

охватывающей [-5, 5] в направлениях x и y. Затем определим
области препятствий
>>> og.set([-2, 0, -2, -1], True)
>>> og.set([2, 3, 0, 4], True)
>>> og.set([0, 2, -2, -2], True)

где первый аргумент – это диапазон ячеек по осям x и y, заданный координатами рабочего пространства. И повторим процесс планирования:

284 Глава 5 · Навигация
>>> lattice = LatticePlanner(occgrid=og)
>>> lattice.plan(iterations=None)
iteration 1, frontier length 3
iteration 2, frontier length 3
iteration 3, frontier length 6
iteration 4, frontier length 15
...
iteration 23, frontier length 4
iteration 24, frontier length 2
finished after 25 iterations

В данном случае мы не указали количество итераций, поэтому планировщик будет повторять их, пока не добавит все вершины в свободном пространстве. Наконец, запросим путь
>>> path, status = lattice.query(qs, qg)
>>> lattice.plot(path)

Результат показан на рис. 5.26.

4

y

2

0

−2

−4

−4

−2

0
x

2

4

Рис. 5.26. Планирование маршрута
перемещения вбок при наличии
препятствий

Планирование по решетке похоже на PRM в том, что для
перемещения из начальной конфигурации в ближайшую дискретную конфигурацию в решетке и от другой дискретной вершины решетки к цели требуется локальный планировщик. Мы
рассмотрели пример очень простой решетки с тремя ветвями
в каждой вершине. Более сложные планировщики на основе решеток используют более богатый набор примитивов движения,
например показанный на рис. 5.27. В сценарии автономного
вождения эта палитра кривых может использоваться для поворотов или смены полосы движения. Чтобы сохранить управляемый размер графа, обычно принято генерировать дуги с применением ограниченного числа итераций. В каждой итерации
кривые, пересекающие препятствия, отбрасываются. Из этого

Планирование проходимого маршрута 285

набора допустимых путей автопилот выбирает кривую, которая
соответствует его текущей цели, и делает шаг по этому пути,
пос­ле чего процесс повторяется.
0.20
0.15
0.10

y

0.05
0.00
−0.05
−0.10
−0.15
−0.20
0.000

0.025

0.050

0.075

0.100
x

0.125

0.150

0.175

0.200

Рис. 5.27. Более сложная решетка с 43 путями, сгенерированная пакетом sbpl и основанная
на кинематической модели моноколеса. Обратите внимание, что показанные здесь пути
повернуты на 90° по часовой стрелке для соответствия рис. 5.23

5.6.4

Также называются
спиралями Эйлера
или Корню.

Криволинейные полиномы

Планировщики Дубинса, Ридса–Шеппа и на основе решетки
генерируют пути, которые кажутся гладкими, и если кривизна
пути оказывается преодолимой для транспортного средства, то
оно сможет следовать по нему. Однако кривизна меняется прерывисто – в любой точке пути κ ∈ {κm, 0, -κm} нет промежуточных
значений. Если бы вы ехали по такому пути на автомобиле, вам
пришлось бы мгновенно поворачивать рулевое колесо между
тремя углами, соответствующими резкому левому повороту,
движению прямо и резкому правому повороту.
Повернуть мгновенно невозможно, а конечное время, необходимое для поворота рулевого колеса, приведет к ошибкам
в следовании траектории, что повышает вероятность столкновения.
Проектировщикам дорог давно известно, что кривизна пути
должна непрерывно меняться с расстоянием. Как правило, дороги описываются клотоидными кривыми, где кривизна изменяется линейно с расстоянием. Местоположение как функция длины пути s определяется интегралами Френеля:
(5.8)

286 Глава 5 · Навигация

который можно интегрировать численно, а a – это жесткость
кривой.
Чтобы рассчитать путь с непрерывной кривизной между двумя конфигурациями, нужен кубический полином
κ(s) = κ0 + as + bs2 + cs3,

0 ≤ s ≤ sf,

где sf – неизвестная длина пути, а κ0, a, b и c – неизвестные коэффициенты полинома. Интегрирование кривизны вдоль пути
дает курсовой угол

и местоположение на кривой пути
(5.9)
Для заданной начальной и целевой конфигураций можно
определить неизвестные κ0, a, b, c и sf, используя численную
оптимизацию. Для этого нужно лишь определить функцию
стоимости, которая представляет собой расстояние между конечным положением, заданным формулой (5.9), и желаемым
целевым положением. Это реализуется следующим образом
(решение показано на рис. 5.28):
>>> cpoly = CurvaturePolyPlanner()
>>> path, status = cpoly.query(qs, qg)
>>> status
CurvaturePolyStatus(length=1.299, maxcurvature=9.9549,
poly=array([ 3.059, 3.095, 2.234, -4.865]))
>>> cpoly.plot(path);

0.2

y

0.1
0.0
−0.1
0.0

0.2

0.4

x

0.6

0.8

1.0

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

В отличие от кривой Дубинса, на рис. 5.21 этот путь имеет непрерывную кривизну, поэтому колесное транспортное средство
сможет точно следовать по нему. На рис. 5.29 сравниваются кривизна пути, полученного с помощью метода Дубинса и полино-

Планирование проходимого маршрута 287

миального подхода. Во-первых, обратите внимание на разрыв
кривизны кривой Дубинса. Во-вторых, полиномиальный подход имеет непрерывную кривизну, но превышает ограничение
максимальной кривизны. Другие подходы к решению этой задачи включают использование сплайнов Безье или B-сплайнов.
Dubins
CubicPoly

10
8
6

кривизна

4
2
0
−2
−4

0.0

0.2

0.4

0.6

нормализованное расстояние пути

0.8

1.0

Рис. 5.29. Зависимость кривизны от расстояния вдоль пути Дубинса и полиномиальной кривой

5.6.5

 ланирование маршрута
П
в конфигурационном пространстве

Последний планировщик, который мы представим, – это алгоритм быстрого исследования случайного дерева (rapidly exploring random tree, RRT). RRT учитывает модель движения транспортного средства, но в отличие от планирования по решетке
RRT использует вероятностный подход, как и планировщик PRM.
Далее мы рассмотрим классическую задачу перемещения пиани­
но (рис. 5.30a), по условию которой нужно переместить пиа­нино
из исходного положения в конечное через узкий проход.
Создадим несколько полигональных препятствий вместо
сетки занятости, потому что между полигональным контуром
транспортного средства и полигональными препятствиями
проще найти пересечение. Инициализируем рабочее пространство карты, охватывающее диапазон от 0 до 10 в обоих направлениях, а затем добавим препятствия, выраженные координатами вершин:
>>> map = PolygonMap(workspace=[0, 10]);
>>> map.add([(5, 50), (5, 6), (6, 6), (6, 50)])
>>> map.add([(5, 4), (5, -50), (6, -50), (6, 4)])

288 Глава 5 · Навигация
10

10

начальное
положение

8

8

6

Y

Y

6

4

4

конечное
положение

2

0

а

0

2

4

X

6

8

10

2

0

б

0

2

4

X

6

8

10

Рис. 5.30. Задача о перемещении пианино: а) начальная и конечная конфигурации. Здесь показано, что при
правильной ориентации пианино пройдет через промежуток между препятствиями, которые изображены как
красные прямоугольники. Точка отсчета пианино обозначена черной точкой. Пунктирная окружность – это
окружность, описанная вокруг пианино, а бледно-красные круги указывают необходимое расширение препятствия;
б) десять случайных конфигураций пианино. Красным окрашены конфигурации, в которых обнаруживается
столкновение с препятствиями, а зеленым – конфигурации, не имеющие контакта с препятствиями

Обратите внимание, что препятствия значительно выступают вверх и вниз за пределы рабочей зоны. Это сделано, чтобы
не позволить планировщику срезать путь. Мы можем нарисовать карту командой
>>> map.plot()

которая который отобразит многоугольники препятствий красного цвета, как показано на рис. 5.30a. Начальная и конечная
конфигурации пианино находятся по обе стороны от промежутка между препятствиями:
>>> qs = (2, 8, -pi/2);
>>> qg = (8, 2, -pi/2);

и их можно визуализировать командами
>>> piano = VehicleIcon("piano", scale=3)
>>> piano.plot(qs);
>>> piano.plot(qg);

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

Планирование проходимого маршрута 289

этой окружности, как показано бледно-красными кругами, то
промежуток будет полностью перекрыт. Значит, нужно какоето другое решение.
Ключевым компонентом алгоритма RRT является проверка
столкновения перемещаемого объекта с препятствиями. В данном примере объект представляет собой прямоугольник размером 3×1.5, который в программном коде мы представляем
с помощью многоугольника
>>> l, w = 3, 1.5;
>>> vpolygon = Polygon2([(-l/2, w/2), (-l/2, -w/2),
...
(l/2, -w/2), (l/2, w/2)]);

симметричного относительно начала координат. Любую конкретную конфигурацию
>>> q = (2, 4, 0);

можно проверить на столкновение с препятствием
>>> map.iscollision(vpolygon.transformed(SE2(q)))
False

где метод transformed объекта Polygon2 возвращает новый объект
Polygon2, вершины которого были преобразованы с помощью
преобразования твердого тела SE(2), построенного на основе
конфигурации (x, y; θ). На рис. 5.30б показаны десять случайных
конфигураций и их статус столкновения.
RRT поддерживает граф конфигураций робота, как показано
на рис. 5.31, и каждая вершина представляет собой конфигурацию q = (x, y, θ) ∈ ℝ2 × S1 в виде трехмерного вектора. Целевая
конфигурация добавляется как вершина.

Рис. 5.31. RRT строит ориентированный граф
конфигураций qi, где qg – конечная конфигурация.
Ребра представляют пути, проходимые для
перемещаемого объекта и не пересекающие
препятствий

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

290 Глава 5 · Навигация

Затем отыскивает ближайшую вершину с конфигурацией qnear.
► С помощью планировщика путей Дубинса вычисляет путь от
конфигурации qnew до qnear. Если путь не пересекает препятствий
►, то qnew добавляется в граф, а ребро qnear копируется в путь Дубинса. Процесс повторяется, пока в граф не будет добавлено необходимое количество вершин.
Результатом является набор траекторий, следуя которым,
неголономное транспортное средство может переместиться из
начальной конфигурации в конечную без столкновений с препятствиями. ► Для любой желаемой начальной конфигурации
можно найти ближайшую конфигурацию в графе, а затем использовать поиск по графу, чтобы отыскать путь к цели.
Чтобы использовать RRT, необходимо сначала создать модель, описывающую кинематику транспортного средства:
>>> vehicle = Bicycle(steer_max=1, L=2, polygon=vpolygon);

В данном случае используется двухколесная модель, похожая
на автомобиль, с максимальным углом поворота управляемого
колеса 1 рад. Зная этот угол, планировщик сможет определить
максимально допустимую кривизну пути
>>> vehicle.curvature_max
0.7787

Многоугольник транспортного средства тоже передается как
параметр и используется алгоритмом RRT для проверки столк­
новений.
Процедура создания объекта планировщика RRT уже зна­
кома:
>>> rrt = RRTPlanner(map=map, vehicle=vehicle, npoints=50,
...
showsamples=True, seed=0)

Этот объект обладает множеством методов, например
>>> q = rrt.qrandom()
array([
6.37,
2.698,
>>> rrt.iscollision(q)
True

-2.884])

для создания случайной конфигурации в рабочем пространстве
и проверки ее столкновения с препятствием.
Создадим RRT с целью в конечной точке:
>>> rrt.plan(goal=qg, animate=True)

Результат показан на рис. 5.32. Запрос пути к цели из начальной конфигурации
>>> path, status = rrt.query(start=qs);

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

Планирование проходимого маршрута 291

вернет траекторию в конфигурационном пространстве, а также
дополнительную информацию о состоянии. Мы можем наложить граф RRT на карту рабочего пространства
>>> rrt.g.plot()

и выделить путь через граф
>>> rrt.g.highlight_path(status.vertices, color="red")

как показано на рис. 5.32а. Путь справа от промежутка между
препятствиями содержит петлю и потому неоптимален, однако
при повторном запуске планировщик RRT может найти более
удачное решение.
10

8

8

6

6
y

Y

10

4

4

2

2

0

а

0

2

4

X

6

8

10

0

б

0

2

4

x

6

8

10

Рис. 5.32. а) Вершины и ребра, построенные алгоритмом RRT, показаны темно-синим цветом. Черные точки –
случайно выбранные местоположения в свободном пространстве, которые не были добавлены в граф. Бледноголубые прямоугольники – конфигурации пианино, не имеющие контакта с препятствиями. Путь через граф
выделен красным цветом; б) траектория перемещения пианино представляет собой конкатенацию нескольких
кривых Дубинса

Траектория перемещения пианино представляет собой конкатенацию нескольких кривых Дубинса между конфигурациями в графе, и мы можем построить ее проекцию на плоскость
>>> rrt.plot(path);

которая показана на рис. 5.32б.
Этот пример иллюстрирует некоторые важные особенности
RRT. Первая касается применения планировщика PRM, потому
что между начальной конфигурацией и ближайшей вершиной
в RRT может иметься некоторое расстояние (и они могут отличаться ориентацией). В данном случае:
>>> status.initial_d
0.5996

292 Глава 5 · Навигация

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

5.7

Дополнительные темы

5.7.1

Алгоритмы поиска A* и Дейкстры

Широко известный подход к поиску в графах – это алгоритм по­
иска кратчайшего пути, предложенный Дейкстрой еще в 1956 го­
ду. Алгоритм формулируется в терминах кратчайшего пути от
исходной вершины до всех остальных вершин графа и создает
дерево кратчайших путей. Исходная вершина может быть начальной или конечной. Алгоритм похож на UCS тем, что посещает каждую вершину и вычисляет расстояние от каждой
вершины до начальной. Однако UCS требует передать ему начальную и конечную вершины, тогда как алгоритму Дейкстры
нужна только начальная (или конечная) вершина. Алгоритм
Дейкстры поддерживает два множества вершин: посещавшиеся и непосещавшиеся, которые аналогичны множествам исследованных и подлежащих исследованию вершин в UCS: первоОтступление 5.10. Эдсгер Вибе Дейкстра
Дейкстра (1930–2002) – пионер голландской информатики и лауреат премии Тьюринга 1972 года. Физик-теоретик по образованию, он работал в промышленности
и академических кругах в то время, когда вычислительная техника еще только
превращалась из ремесла в строгую дисциплину, теоретических основ не сущест­
вовало, а компьютерных языков было очень мало. Он был одним из первых, кто
продвигал новые стили программирования для повышения качества программ,
придумал термин «структурное программирование» и разрабатывал языки и компиляторы для его поддержки. В 1960-х работал в области параллельных и распределенных вычислений, занимаясь такими темами, как мьютексы, семафоры
и блокировки. Разработал алгоритм поиска кратчайших путей в графах. Многие
из его вкладов в информатику имеют огромное значение для современных робототехнических систем.

Дополнительные темы 293

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

5.7.2

Преобразование карт сеток занятости в графы

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

5.7.3

Преобразование между графами и мат­ри­ца­ми

Граф с n вершинами можно представить как мат­ри­цу n×n расстояний D. Например:
>>> import pgraph
>>> g = pgraph.UGraph()
>>> for i in range(4): # add 4 vertices
...
g.add_vertex()
>>> g[0].connect(g[1], cost=1); # 0 -- 1
>>> g[0].connect(g[3], cost=2); # 0 -- 3
>>> g[1].connect(g[2], cost=3); # 1 -- 2
>>> g.distance()
array([[
0,
1,
0,
[
1,
0,
3,
[
0,
3,
0,
[
2,
0,
0,

2],
0],
0],
0]])

Элементы мат­ри­цы di,j – это стоимость ребра, соединяющего
вершины i и j или 0, если вершины i и j не связаны. Для неориентированного графа, как показано здесь, мат­ри­ца симметрична.

5.7.4

Локальное и глобальное планирование

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

294 Глава 5 · Навигация

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

карта

глобальный
планировщик

планирование
с использованием карты

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

положение

локальный
планировщик

реактивная навигация

рулевое управление, скорость

транспортное
средство

Рис. 5.33. Архитектура
глобального и локального
планирования маршрута

Все это отражено в общей архитектуре планирования движения робота, показанной на рис. 5.33. Задача локального планировщика – удержать транспортное средство как можно ближе
к глобально оптимальному пути, учитывая заранее неизвестные
местные условия. Локальное планирование основано на сенсорных данных и представляет собой особую сложность, поскольку
диапазон действия датчиков ограничен и, следовательно, глобальная информация недоступна. Например, пытаясь избежать
одного локального препятствия, мы можем повернуть налево
и встретить другое препятствие, чего бы не произошло, если бы
мы повернули направо. Именно с такой проблемой мы столкнулись при обсуждении планировщика bug2 в разделе 5.1.2.

5.8

Подведение итогов

В этой главе мы рассмотрели несколько подходов к решению
задачи навигации робота – направления робота к цели. Прос­

Подведение итогов 295

тейший – чисто реактивный подход – был реализован в тележ­
ке Брайтенберга. Затем мы добавили ограниченную память для
создания решений на основе конечных автоматов, таких как
bug2, позволяющих обходить препятствия, однако маршруты,
обнаруживаемые этими решениями, могут быть далеки от оптимальных.
После этого мы рассмотрели ряд различных алгоритмов планирования маршрутов на основе карт. Мы начали с графовых
карт и исследовали ряд алгоритмов поиска оптимальных путей
в дорожной сети, минимизирующих длину маршрута или время в пути. Потом мы перешли к картам на основе сетки и начали с преобразования расстояний для поиска оптимального пути
к цели. Алгоритм D* отыскивает похожий маршрут, но позволяет ячейкам сетки иметь непрерывную меру проходимости, а не
только признак занятости. D* также поддерживает экономичное с вычислительной точки зрения пошаговое перепланирование для небольших изменений карты. Алгоритм PRM снижает вычислительную нагрузку за счет вероятностной выборки,
но из-за этого находит менее оптимальные маршруты и не
дает гарантий, что отыщется проходимый маршрут, даже если
он существует. Для учета ограничений движения, которые имеют реальные транспортные средства, мы ввели планировщики
Дубинса и Ридса–Шеппа, которые находят проходимые маршруты, состоящие из дуг и прямых линий, а также планировщик
на основе решеток, поддерживающий прокладку маршрутов
с учетом известных препятствий. Эти маршруты, хотя и технически проходимы, имеют разрывную кривизну, и для преодоления этого ограничения были введены криволинейные полиномы. Наконец, мы рассмотрели применение алгоритма RRT для
планирования маршрута в пространстве конфигураций с учетом имеющихся препятствий и ограничений движения транспортного средства. Все подходы, основанные на применении
карт, требуют наличия карты и информации о местонахождении робота, и эти обе темы мы рассмотрим в следующей главе.

5.8.1

Дополнительное чтение

Подробный обзор планирования маршрутов движения роботов представлен в двух учебных пособиях. В Choset et al. (2005)
рассматриваются геометрические и вероятностные подходы
к планированию, а также их применение к роботам с динамическими и неголономными ограничениями. В LaValle (2006)
исследуются вопросы планирования движения, планирования
в условиях неопределенности и планирования на основе датчиков, обучение с подкреплением, нелинейные системы, планирование траектории, неголономное планирование. Эта кни-

296 Глава 5 · Навигация

га доступна бесплатно по адресу http://lavalle.pl/planning. Обе
книги предлагают гораздо более совершенный подход к представлению препятствий в конфигурационном пространстве
и охватывают методы планирования на основе потенциального
поля, которые мы не обсуждали. Мощные методы планирования, обсуждаемые в этих книгах, могут применяться не только в робототехнике, но и в системах очень высокого порядка,
таких как транспортные средства с прицепами, роботизированные манипуляторы и даже формирование молекул. LaValle
(2011a) и LaValle (2011b) – краткое учебное пособие из двух частей. Освещение вопросов планирования маршрутов можно
найти в Kelly (2013), Siegwart et al (2011), а также в книге «Robo­
tics Handbook» (Siciliano and Khatib 2016, § 7) и еще в Spong et al.
(2006) и Siciliano et al. (2009).
Алгоритмы bug1 и bug2 были описаны в Lumelsky and Stepanov (1986), а в Ng and Bräunl (2007) представлены реализации
одиннадцати вариантов алгоритма Bug в различных условиях и их сравнение. Планирование на основе графов подробно
описано в работе Russell and Norvig (2020), а прообразом примера дорожной сети, который мы рассмотрели в разделе 5.3,
стала карта Румынии. Преобразование расстояний хорошо
описано в Borgefors (1986), а его раннее применение в роботизированной навигации было исследовано в Jarvis and Byrne
(1988). Эффективные подходы к реализации преобразования
расстояний включают двухпроходной метод Hirata (1996),
быст­рые маршевые методы или преобразование в задачу поиска на графе, которую можно решить методом Дейкстры; последние два подхода сравниваются в Alton andMitchell (2006).
Алгоритм поиска кратчайших путей в графе был предложен
в Dijkstra (1959), а более поздний алгоритм A* (Hart et al. 1968)
обеспечивает более высокую эффективность поиска. Любую
карту в форме сетки занятости можно преобразовать в граф,
что соответствует подходу, использованному в алгоритме D*,
предложенном в Stentz (1994), который позволяет экономно
перепланировать маршрут при изменении карты. Существует
множество других расширений, включая алгоритм «Field D*»
(Ferguson и Stentz, 2006) и «D* lite» (Koenig и Likhachev, 2005).
Алгоритм D* используется во многих робототехнических системах, и существует множество его реализаций, в том числе
с открытым исходным кодом.
Идеи, положенные в основу метода PRM, начали формироваться в середине 1990-х и были впервые описаны в Kavraki et
al. (1996). В Geraerts and Overmars (2004) приводится сравнение
эффективности ряда последующих предложенных вариантов
с базовым алгоритмом PRM. В числе подходов к планированию,
учитывающих динамику транспортного средства, можно назвать выборку из пространства состояний (Howard et al., 2008)

Подведение итогов 297

и RRT, описанный в LaValle (1998, 2001). Совсем недавно в Karaman et al. (2011) был предложен алгоритм RRT*.
Подходы к соединению двух положений посредством дуг
и отрезков были предложены в Dubins (1957) и Reeds and Shepp
(1990). В Owen et al. (2015) алгоритм Дубинса был распрост­
ранен на трехмерные модели, что полезно для применения
в авиа­ции. Планировщики на основе решеток рассматриваются
в Pivtoraiko et al. (2009). Хорошее введение в кривизну траектории, клотоиды и криволинейные полиномы дано в Kelly (2013),
а подробное исследование – в Bertolazzi and Frego (2014).

Исторические и интересные факты
В 1948 году Винер написал основополагающую книгу по кибернетике и обновил ее в 1965 году (Wiener 1965). Уолтер опубликовал ряд популярных статей (Walter 1950, Walter 1951) и написал
книгу (Walter 1953), основанную на его теориях и экспериментах с роботизированными черепахами, а Холланд (Holland 2003)
описал вклад Уолтера.
Исчерпывающим источником информации о тележках Брайтенберга является его собственная книга (Braitenberg 1986).
Она представляет собой причудливый, почти поэтический набор мысленных экспериментов. В ней исследуются тележки все
возрастающей сложности (всего четырнадцать семейств), в том
числе имеющие память и реализующие некоторую логику, которым Брайтенберг приписывает такие антропоморфные характеристики, как любовь, страх, агрессия и эгоизм. Во второй
части книги излагается фактическая основа этих тележек – нейронная структура животных.
Среди первых роботов, основанных на поведении, был
«Зверь» (Beast), созданный в университете имени Джонса Хопкинса в 1960-х, и Чингисхан (Genghis), построенный в 1989-м
(Brooks 1989). Поведенческая робототехника описана в ранней
статье Brooks (1986), книге Arkin (1999) и «Robotics Handbook»
(Siciliano and Khatib 2016, § 13). Книга «Robotics Primer» (Mataric
2007) и сопутствующие обширные веб-ресурсы тоже являются отличным введением в реактивное управление роботами,
управление на основе поведения и навигацию. Богатая коллекция архивных материалов о ранних кибернетических машинах,
включая черепаху Уолтера и «Зверя» из университета имени
Джонса Хопкинса, представлена на сайте Cybernetic Zoo http://
cyberneticzoo.com/.

5.8.2

Ресурсы

Python Robotics (https://github.com/AtsushiSakai/PythonRobo­
tics) – это обширная и постоянно пополняющаяся коллекция

298 Глава 5 · Навигация

алгоритмов на Python для планирования маршрута движения
робота, а также определения местоположения, картографирования и SLAM. OMPL (Open Motion Planning Library, https://
ompl.kavrakilab.org), написанная на C++, – это мощный и широко используемый набор планировщиков движения. В нем
есть приложение на Python, предоставляющее удобные средст­
ва для исследования задач планирования. На веб-сайте Стива
ЛаВаля (http://lavalle.pl/software.html) представлено множество ресурсов с исходным кодом (на C++ и Python), связанных
с планированием движения. Планировщики на основе решеток
входят в пакет sbpl из Search-Based Planning Lab (https://github.
com/sbpl/sbpl), который содержит инструменты MATLAB для
создания примитивов движения и код C++ для планирования
на основе решеток, а pysbpl (https://github.com/poine/pysbpl) –
биб­лиотеку для Python 3.

5.8.3

Упражнения

1. Тележки Брайтенберга (раздел 5.1.1).
а) Поэкспериментируйте с различными начальными конфигурациями и коэффициентами усиления управления.
б) Измените знак сигнала управления, чтобы тележка не
стремилась к источнику света, а наоборот – старалась
удалиться от него.
в) Измените функцию sensorfield так, чтобы пик перемещался со временем.
г) Тележка асимптотически приближается к максимальному значению поля. Добавьте правило, останавливающее
тележку, когда любой из датчиков обнаруживает значение больше 0.95.
e) Создайте скалярное поле с двумя пиками. Существует ли
такое начальное положение, оказавшись в котором, робот
запутается?
2. Сетки занятости. Создайте несколько разных сеток занятости и протестируйте на них разных планировщиков.
а) Создайте сетку занятости, содержащую лабиринт, и протестируйте на ней работу разных планировщиков (см.
http://rosettacode.org/wiki/Maze_generation).
б) Создайте сетку занятости на основе реального плана этажа.
в) Создайте сетку занятости на основе карты улиц города
(раздел 11.1.6). При необходимости можете применить
цветовую сегментацию (раздел 12.1.1.2), чтобы отделить
дороги от других объектов. Можно ли преобразовать ее
в карту стоимости, пригодную для передачи алгоритму
D*, где разные дороги и перекрестки могут иметь разную
стоимость?

Подведение итогов 299

3.

4.
5.

6.

7.
8.

г) Поэкспериментируйте с расширением размеров препятствий.
д) Какой объем памяти потребуется для представления поверхности Земли при размере ячейки 1×1 м? Сколько
памяти потребуется для представления только площади
суши? Какой размер должна иметь ячейка, чтобы карта
вашей страны уместилась в 1 Гбайт памяти?
Алгоритмы поведения жуков (раздел 5.1.2).
а) Создайте карту для проверки алгоритма bug2. Попробуйте разные отправные точки.
б) Создайте карту препятствий, содержащую лабиринт.
Сможет ли алгоритм bug2 найти выход из него?
в) Поэкспериментируйте с различными начальными и конечными точками.
г) Создайте ловушку для жуков. Сделайте полый ящик и поместите жука внутрь ящика, задайте цель снаружи. Что
произойдет?
д) Измените алгоритм bug2 так, чтобы при столкновении
с препятствием выбиралось случайное направление для
поворота.
е) Реализуйте другие алгоритмы поведения жука, такие как
bug1 и tangent bug. Будут ли они работать лучше, чем bug2?
Преобразование расстояния (раздел 5.4.1).
а) Создайте карту препятствий с лабиринтом, найдите выход из него, используя преобразование расстояния.
Планировщик D* (раздел 5.4.2).
а) Добавьте путь с низкой стоимостью в гостиную. Можно
ли добиться того, чтобы робот выбирал этот маршрут до
кухни?
б) Заблокируйте дополнительные дверные проемы, чтобы
усложнить задачу роботу.
Планировщик PRM (раздел 5.5).
а) Запустите планировщик PRM 100 раз и соберите статис­
тику по полученной длине пути.
б) Измените порог расстояния и понаблюдайте за произведенным эффектом.
в) Используйте выходные данные планировщика PRM в качестве входных данных для планировщика движения
вдоль произвольного пути, обсуждавшегося в главе 4.
Планировщики Дубинса и Ридса–Шеппа (разделы 5.6.1 и 5.6.2).
а) Найдите путь для реализации трехточечного поворота.
б) Исследуйте влияние изменения максимальной кривизны.
Планировщики на основе решеток (раздел 5.6.3).
а) Сколько итераций потребуется для полного заполнения
интересующей области, показанной на рис. 5.23в?
б) Как увеличивается число вершин и пространственная
протяженность решетки с числом итераций?

300 Глава 5 · Навигация

в) Определите размер ячейки сетки для автомобиля с колесной базой 4.5 м и максимальным углом поворота
управляемых колес ±30°?
г) Вычислите и постройте график кривизны как функции
длины пути через решетку, как показано на рис. 5.25а.
д) Разработайте контроллер, использующий модель моноцикла с конечным углом поворота управляемых колес
(существует параметр, позволяющий его задать), который будет вести транспортное средство по траекториям,
показанным на рис. 5.25.
9. Криволинейные полиномы (раздел 5.6.4).
а) Узнайте, как можно использовать сплайны Безье, Эрмита и B-сплайны для создания плавных траекторий между местоположениями или конфигурациями.
б) Измените планировщик на основе полиномов, чтобы
можно было задать верхнюю границу кривизны.
10. Планировщик RRT (раздел 5.6.5).
а) Поэкспериментируйте с параметрами RRT, такими как
количество точек и максимальный угол поворота управляемого колеса транспортного средства.
б) Рассчитайте и нанесите на график угол поворота управляемого колеса, необходимый для перехода из начального положения в конечное.
в) Добавьте локальный планировщик для перемещения из
начального положения к ближайшей вершине графа путей и из конечной вершины в целевое положение.
г) Определите путь через граф, который минимизирует
количество изменений направления.

Глава

6

	Определение
местоположения
и картографирование
« Чтобы куда-то попасть, нам нужно знать, где мы находимся.

chap6.ipynb

https://go.sn.pub/
DORGc6

До сих пор, говоря о навигации по карте, мы предполагали, что
робот обладает средствами определения своего местоположения. В этой главе мы рассмотрим некоторые распространенные
методы оценки местоположения робота в пространстве – известные под общим названием локализация.
Современная система геопозиционирования GPS настолько упрощает определение местоположения вне помещений,
что мы часто воспринимаем эту возможность как нечто само
собой разумеющееся. К сожалению, GPS – не идеальный датчик, каким его представляют многие. Он использует очень слабые радиосигналы, принимаемые с удаленных спутников, а это
означает, что GPS не работает или работает неправильно вне
прямой видимости спутников, например в помещениях, под
водой, под землей, в глубоких каньонах или шахтах. Сигналы
GPS также могут глушиться, что для некоторых приложений неприемлемо.
Система GPS начала эксплуатироваться относительно недавно, с 1995 года, хотя человечество уже тысячи лет занимается навигацией и определением местоположения на планете.
В этой главе мы рассмотрим классические приемы навигации,
такие как счисление пути и использование ориентиров, на которых основана и современная роботизированная навигация.
Счисление пути – это оценка местоположения на основе расчетной скорости, направления и времени движения относительно предыдущей оценки. На рис. 6.1 показано, как местоположение судна обновляется на карте. Учитывая средний курс
по компасу за предыдущий час и пройденное расстояние, его
местоположение в 15:00 можно определить с помощью элементарной геометрии по местоположению в 14:00. Однако измерения, на которых основано определение местоположения,
подвержены как систематическим, так и случайным погреш-

302 Глава 6 · Определение местоположения и картографирование

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

Рис. 6.1. Определение местоположения
с помощью счисления пути.
Местоположение судна в 15:00
определяется на основе его
местоположения в 14:00, предполагаемого
пройденного расстояния с того момента
и среднего курса по компасу

Финикийцы ходили по морю более 4000 лет назад, и у них
даже не было компаса – его изобрели 2000 лет спустя в Китае.
Финикийцы ориентировались на местности с помощью грубого счисления пути, но везде, где это было возможно, они использовали дополнительную информацию для корректировки
оценки своего местоположения – сведения об островах и мысах, примитивные карты и наблюдения за Солнцем и Полярной
звездой.
Ориентир – это видимый объект, местоположение которого известно относительно некоторой системы координат. На
рис. 6.2 схематически изображены карта и несколько маяковОтступление 6.1. Измерение скорости в море
Судовой лаг – это прибор, позволяющий оценить пройденное расстояние. Древнейшим методом определения скорости судна в море был
голландский лаг: плавающий предмет, который бросали в воду у носа судна, и с помощью
песочных часов измеряли время, за которое
корма судна достигнет этого предмета. Позже появился лаг в виде плоского деревянного
бруска в форме четверти круга со свинцовым
грузом на круглой стороне, который позволял
ему держаться вертикально и сопротивляться
буксировке. Лаг бросали за борт и вытравливали линь с узлами, расположенными через каждые 50 футов (15.24 метра). Для измерения скорости использовали специальные
песочные часы, отмеряющие интервал 30 с. Прохождение каждого узла за этот интервал соответствовало примерно 1 морской миле в час (NMI), или 1 узлу. Морская
миля (NMI) теперь определяется как 1.852 км. (Изображение взято из учебника
«Seamanship», написанного капитаном С. Б. Люсом 1891.)

Определение местоположения и картографирование 303
Обратная засечка – это процедура
определения местоположения путем
измерения углов пеленга на известные
ориентиры. Триангуляция – это оценка
местоположения
путем измерения
углов направления
на неизвестную
точку от каждого из
ориентиров.

ориентиров. Сначала мы используем компас, чтобы совместить
направление на север на карте с фактическим направлением
на Северный полюс. Затем на карту наносятся азимуты на ориентиры, и их пересечение дает наше местоположение. Этот
процесс известен как обратная засечка. Например, пеленг на
маяк A сообщает, что судно находится где-то на синей линии,
а пеленг на маяк C – что судно находится на красной линии. Соответственно, истинное местоположение p находится на пересечении этих двух линий.

Рис. 6.2. Оценка местоположения по карте. Линии прямой видимости от двух маяков, A и C, и их соответствующие
местоположения на карте дают оценку местоположения p. Если по ошибке принять маяк B за C, то получится
неверная оценка q

Однако этот процесс критически зависит от правильной
идентификации наблюдаемого ориентира. Если ошибочно
принять один маяк за другой, например если мы видим B, но
думаем, что это C, то, как показывает красная пунктирная линия, это приводит к значительной ошибке в оценке местоположения – мы будем полагать, что находимся в точке q, тогда как
на самом деле находимся в точке p. Это приведет к переоценке
расстояния до береговой линии. Если мы решим плыть к берегу,
то сядем на мель или врежемся в скалы и будем сильно удивлены, так как они окажутся не там, где мы ожидали. К сожалению, это очень распространенная ошибка, и из-за нее затонуло
бесчисленное множество кораблей. Вот почему маяки мигают!
В XVIII веке технический прогресс позволил маякам испускать
уникальные последовательности световых сигналов, что помогает надежно определить идентичность конкретного маяка
и связать его с точкой на навигационной карте.
У первых мореплавателей не было ни карт, ни маяков, ни
даже компасов. Им приходилось создавать карты и постепенно

304 Глава 6 · Определение местоположения и картографирование

добавлять новые ориентиры по мере расширения границ исследованной области. Поэтому совершенно неудивительно, что
многие ранние исследователи терпели неудачу ► и карты были
строго охраняемой государственной тайной.
Современные роботы, работающие в условиях отсутствия
GPS, сталкиваются с теми же проблемами, что и древние навигаторы, и потому заимствуют многие стратегии навигации,
появившиеся несколько столетий тому назад. Оценка роботом
пройденного расстояния может быть несовершенной, и у него
вообще может не быть карты, или карта может быть несовершенной или неполной. Дополнительная информация о наблюдаемых объектах окружающей среды критически важна для
минимизации ошибки локализации (определения местоположения) робота, но вероятность ошибок идентификации ориентиров сохраняется.

В экспедиции
Магеллана, начавшейся в 1519 году,
приняли участие
5 кораблей
и 237 человек,
но большинство
их них, включая
Магеллана, погибли
в пути. Вернулись
только 18 человек
и 1 корабль.

Отступление 6.2. Астронавигация
Положение небесных тел на небосводе является
предсказуемой функцией времени, а также широты и долготы наблюдателя. В свое время эта
информация была сведена в таблицу, которая
известна как эфемериды (что означает ежеднев­
ные), и такие данные публикуются ежегодно
в Великобритании с 1767 года в виде морского
альманаха «The Nautical Almanac». Высоту небесного тела над горизонтом можно измерить
с помощью секстанта – ручного оптического
прибора.
Время и долгота связаны, и звездное поле
часом позже будет выглядеть так же, как оно
выглядит сейчас в 15° к востоку. Однако Полярная звезда находится очень близко к небесному полюсу, и ее угол возвышения не зависит от
долготы и времени, что позволяет легко определить широту одним измерением секстанта.
Определение долготы было величайшей научной задачей для европейских правительств
XVIII века, поскольку она существенно затрудняла мировое судоходство и морское гос­
подство. Британский закон о долготе 1714 года учредил премию в 20 000 фунтов
стерлингов за ее решение, что стимулировало разработку морских хронометров –
часов, способных сохранять высокую точность на борту кораблей. Более пятидесяти лет спустя Джон Харрисон разработал подходящий хронометр, копию которого
использовал капитан Джеймс Кук во время своего второго путешествия 1772–
1775 годов. После трехлетнего путешествия ошибка в оценке долготы составила
всего 13 км. Благодаря точному знанию времени можно было по углу возвышения
звезд определять широту и долготу. Этот технологический прогресс способствовал
развитию исследований и торговли по всему миру. (Изображение предоставлено
archive.org.)

Определение местоположения и картографирование 305

а

б

Плотность вероятности ×10–2

Плотность вероятности ×10–4

Плотность вероятности ×10–2

В более широком
смысле функция
плотности вероятности для положения
робота (положение, в отличие
от позиции, или
местонахождения,
робота включает
еще и информацию
об ориентации) f(x,
y, θ) будет представлять собой
четырехмерную
поверхность. Объем
под поверхностью
всегда равен 1.

Мы можем определить задачу локализации формально: x – истинное, но неизвестное местоположение робота, а x̂ – наилучшая
оценка этого местоположения. Нам также необходимо знать не­
определенность оценки, которую можно рассматривать в статис­
тических терминах как стандартное отклонение оценки x̂.
Предполагаемое положение робота будет полезно описать
с помощью функции плотности вероятности (probability density function, PDF) по всем возможным положениям робота.
Несколько примеров функций плотности показаны на рис. 6.3,
где величина функции f(x, y) в любой точке (x, y) является относительной вероятностью нахождения робота в этой позиции
или положнии. Функция Гаусса имеет широкое применение
и может быть кратко описана с помощью таких понятий, как
среднее значение и стандартное отклонение. Робот, скорее всего, находится в точке пика (среднем значении), и вероятность
его местонахождения в других точках уменьшается с удалением от пика. На рис. 6.3a показан пик с небольшим стандартным
отклонением. Такая форма графика говорит нам, что местоположение робота очень хорошо известно. Вероятность, что робот
находится в точке, обозначенной вертикальной черной линией, практически равна нулю. Напротив, пик на рис. 6.3б имеет
большое стандартное отклонение, и это означает меньшую уверенность в его местоположении. Существует разумная вероятность, что робот находится в точке, обозначенной вертикальной
линией. Использование функции плотности вероятности (PDF)
также позволяет выдвигать несколько гипотез о местоположении робота. Например, функция на рис. 6.3в описывает местоположение робота, который совершенно уверен, что находится
в одном из четырех мест. И эта ситуация не лишена смысла.
Представьте робота, который видит дверь и на его карте обозначены четыре двери. В отсутствие другой информации робот

в

Рис. 6.3. Понятия местоположения робота и неопределенности в плоскости xy, ориентация θ не рассматривается.
Вертикальная ось отражает относительную вероятность нахождения робота в этом местоположении, иногда
называемую «убеждением», или «верой». Контурные линии отображены на нижней плоскости: a) робот имеет
низкую неопределенность положения, σ = 1; б) робот имеет значительно более высокую неопределенность
положения, σ = 20; в) робот имеет несколько гипотез о своем положении, для каждой из которых σ = 1

306 Глава 6 · Определение местоположения и картографирование

с равной вероятностью может оказаться вблизи любой из четырех дверей. Мы еще вернемся к этой ситуации в разделе 6.6.
Определение плотности вероятности на основе знаний о движении робота и его наблюдений за окружающим миром представляет собой задачу оценки, которую можно определить как:
Отступление 6.3. Локализация по радио
Одной из самых ранних систем геопозиционирования была система LORAN, основанная на британской системе GEE времен Второй мировой войны. Сеть передатчиков по всему миру излучала синхронизированные радиоимпульсы, а приемник
измерял разницу во времени получения импульсов от пары радиопередатчиков.
Знание идентификаторов двух передатчиков и разницы во времени (TD) позволяло предположить, что приемник находится на гиперболической кривой (такие
кривые на навигационных картах назывались линиями TD). Использование второй
пары передатчиков (которая может включать один из первой пары) дает еще одну
гиперболическую кривую. На пересечении этих кривых и должен находиться приемник.
Глобальная система позиционирования (Global Positioning System, GPS) была
предложена в 1973 году, но полностью функционировать начала только в 1995-м.
Она включает около 30 активных спутников, вращающихся вокруг Земли в шести
плоскостях на расстоянии 20 200 км. GPS-приемник измеряет время прохождения радиосигналов от четырех или более спутников, орбитальное положение которых закодировано в GPS-сигнале. Имея четыре известные точки в пространстве
и четыре измеренные временные задержки, можно вычислить координаты (x, y, z)
приемника и время. Если GPS-сигналы принимаются после отражения от какойлибо поверхности, то расстояние, пройденное радиосигналом, увеличивается, что
приводит к ошибке в оценке местоположения. Этот эффект известен как многолучевое распространение и является распространенной проблемой на крупных
промышленных предприятиях.
Непостоянство скорости распространения радиоволн в атмосфере является основной причиной погрешности определения местоположения. Однако эти
погрешности медленно меняются со временем и остаются приблизительно постоянными на больших территориях. Это позволяет измерять погрешность на
опорной станции и передавать ее в качестве дополнения на совместимые близлежащие приемники, которые могут ее компенсировать. Данная система получила
название дифференциальная GPS (Differential GPS, DGPS). Эта информация может
передаваться через интернет, по береговым радиосетям на суда или по спутниковым сетям, таким как WAAS, EGNOS или OmniSTAR, на самолеты или другим
пользователям. RTK GPS обеспечивает гораздо более высокую точность измерения
времени, обусловленную использованием информации о фазе несущей частоты.
Оригинальная система GPS намеренно добавляла погрешность, иносказательно
именуемую «избирательной доступностью», чтобы снизить ее полезность для военных противников, но эта функция была отключена в мае 2000 года. Среди других
спутниковых навигационных систем можно назвать европейскую систему Galileo,
китайскую систему Beidou и российскую систему ГЛОНАСС. Общее название для
этих систем – глобальная навигационная спутниковая система (Global Navigation
Satellite System, GNSS). GNSS сделала систему LORAN устаревшей, и поэтому большая ее часть была выведена из эксплуатации. Однако в последнее время обеспокоенность зависимостью от GNSS и ее уязвимостью привела к разработке усовершенствованной системы LORAN (eLORAN).

Расчет пути с использованием одометрии 307

« процесс вывода значения некоторой интересующей величины x
путем обработки данных, которые каким-то образом зави­
сят от x.

Например, штурман судна или геодезист оценивает положение, измеряя пеленг на известные ориентиры или небесные
объекты, а приемник GPS оценивает широту и долготу, измеряя
временную задержку получения сигнала от спутников, чьи мес­
тоположения известны.
Для нашей задачи локализации робота истинное и предполагаемое состояния являются векторными величинами. Неопределенность представлена ковариационной мат­ри­цей,
диагональные элементы которой определяют дисперсии соответствующих состояний, а недиагональные элементы – корреляции между состояниями, указывающие на взаимосвязь неопределенностей в состояниях.
Вероятность нахождения в положении
Значение плотности распределения в некоторой точке не является
вероятностью нахождения в этой точке. Рассмотрим, для примера,
небольшую область плоскости xy: объем между этой плоскостью
и поверхностью функции плотности распределения в этой области
определяет вероятность нахождения в этой области.

6.1

Расчет пути с использованием одометрии

Счисление пути – это определение положения робота на основе
предполагаемой скорости, направления и времени его движения относительно предыдущей оценки.
Одометр – это датчик, измеряющий пройденное расстояние.
В колесных транспортных средствах это обычно достигается
путем измерения угла поворота колес с помощью датчика. Направление движения можно измерить с помощью электронного компаса, а изменение направления – с помощью гироскопа
или по разнице угловых скоростей левого и правого колес. Эти
датчики несовершенны из-за систематических ошибок, таких
как неточное определение радиуса колеса или смещение гироскопа, а также из-за случайных ошибок, таких как пробуксовка
колес. В робототехнике термин «одометрия» обычно означает
измерение расстояния и направления движения.
Для роботов, не имеющих колес, таких как летательные, надводные или подводные аппараты, существуют альтернативные
решения. В инерциальной навигации, представленной в разделе 3.4, используются акселерометры и гироскопы для оценки
изменения положения. Существуют также визуальная одомет­

308 Глава 6 · Определение местоположения и картографирование

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

6.1.1

Моделирование робота

Первый шаг в оценке положения робота – написание функции
f(·), которая описывает изменение конфигурации робота, происходящее между двумя отметками времени. Модель, такая
как (4.4) или (4.9), описывает изменение конфигурации робота
в зависимости от состояния его управляющих входов, однако в случае с реальными роботами мы редко имеем прямой
доступ к этим управляющим входам. Большинство роботизированных платформ имеют свои системы управления движением, которые принимают команды пользователя, например
целевое местоположение, и сообщают одометрическую информацию.
Вместо прямого использования (4.4) или (4.9) запишем дискретную модель изменения конфигурации на основе одомет­
рии, где δ〈k〉 = (δd, δθ)T – пройденное расстояние и изменение
курса за предыдущий интервал, а k – временной шаг. Начальное
положение представлено мат­ри­цей SE(2)

Сделаем упрощающее предположение, что пройденное расстояние и изменение ориентации, происшедшие за этот промежуток времени, малы, поэтому порядок применения перемещений не имеет значения. Выберем движение вперед вдоль оси
x корпуса робота (рис. 4.4), повернем на δd, а затем повернем на
δθ и получим положение

которое можно компактно представить в виде вектора конфигурации q = (x, y, θ)

Расчет пути с использованием одометрии 309

,

Мы предполагаем,
что шум аддитивен.
Другой вариант –
мультипликативный
шум, который умес­
тен, если величина
шума связана с величиной сигнала.
Шум одометрии
находится внутри
модели процесса
(движения робота)
и называется шумом
процесса.
Нормальное распределение углов
на окружности
фактически невозможно, поскольку
θ ∈ S1 ∉ ℝ, то есть
углы не превышают
2π. Однако если
ковариация угловых
состояний мала, то
эта особенность
не представляет
проблемы. Распределение углов на
окружности, подобное нормальному,
называется распределением фон
Мизеса, см. numpy.
random.vonmises.

(6.1)

– и получим новую конфигурацию с точки зрения предыдущей
конфигурации и одометрии.
На практике одометрия не идеальна, поэтому смоделируем
ошибку, представив себе генератор случайных чисел, который
искажает показания идеального одометра. Выход реального
одометра представляет собой сумму неизвестных значений
идеального одометра (δd, δθ) и генератора случайных чисел
(υd, υθ). Такие случайные ошибки часто называют шумом, или,
точнее, шумом датчика. Случайные числа неизвестны и не
могут быть измерены, но мы предполагаем, что нам известно
распределение, из которого они взяты.
Конфигурация робота на следующем временном шаге, включая ошибку одометрии, равна

(6.2)

где k – временной шаг, δ〈k〉 = (δd, δθ)T ∈ ℝ2×1 – измерение одомет­
рии и υ〈k〉 = (υd, υθ)T ∈ ℝ2×1 – случайный шум измерения за предыдущий интервал.
В отсутствие какой-либо информации об обратном мы моделируем шум одометрии как υ = (υd, υθ)T ~ N(0, V), многомерный
гауссовский процесс с нулевым средним и ковариацией

Эта мат­ри­ца ковариации одометрии является диагональной,
поэтому ошибки в измерении расстояния и направления не зависят друг от друга. Выбор V не всегда прост, но мы вольны
проводить эксперименты или делать обоснованные инженерные предположения. В следующих примерах мы выбрали σd =
2 см и σθ = 0.5° на интервал выборки и получили ковариационную мат­ри­цу

На самом деле это
не совсем верно,
потому что погреш- >>> V = np.diag([0.02, np.deg2rad(0.5)]) ** 2;
ности измерения
расстояния, как праОбъекты, производные от суперкласса VehicleBase, в Toolbox
вило, увеличиваются
предоставляют
метод f(), который реализует соответствующее
при значительном
изменении курса. уравнение обновления конфигурации. Для имитации робота

310 Глава 6 · Определение местоположения и картографирование

с двухколесной кинематикой, моделью движения (4.4) и изменением конфигурации (6.2) создадим объект Bicycle
>>> robot = Bicycle(covar=V, animation="car")
Bicycle: x = [ 0, 0, 0 ]
L=1, steer_max=1.41372, speed_max=inf, accel_max=inf

со значениями по умолчанию параметров, таких как длина робота, скорость, ограничение угла поворота и интервал выборки,
который по умолчанию равен 0.1 с. Параметр animation определяет форму многоугольника, используемого для анимации
движения робота при вызове метода run. Объект предоставляет
метод для моделирования движения в течение одного временного шага
>>> odo = robot.step((1, 0.3))
array([ 0.1025, 0.02978])

в вызове которого мы задали скорость 1 м/с и угол поворота управляемого колеса 0.3 рад. Функция обновляет истинную
конфигурацию робота и возвращает искаженную шумом информацию одометрии. При интервале измерения 0.1 с робот
сообщает, что за это время он перемещается примерно на 0.1 м
и поворачивает примерно на 0.03 рад. Истинную (но «неизвестную») конфигурацию робота можно увидеть, выполнив команду
>>> robot.q
array([
0.1,

0, 0.03093])

Учитывая возвращаемую одометрию, мы можем с помощью
(6.2) оценить конфигурацию робота после одного временного
шага:
>>> robot.f([0, 0, 0], odo)
array([ 0.1025,
0, 0.02978])

где расхождение с точным значением обусловлено использованием зашумленного измерения одометрии.
Для целей исследования нам нужно, чтобы моделируемый
робот двигался в течение длительного времени в определенной
пространственной области. Класс RandomPath – это драйвер, который направляет робота к случайно выбранным точкам маршрута в заданной области. Создадим экземпляр драйвера и подключим его к роботу
>>> robot.control = RandomPath(workspace=10)

Аргумент workspace определяет рабочую область, охватывающую ±10 м по осям x и y. Мы можем отобразить анимацию робота, движущегося под управлением драйвера.
>>> robot.run(T=10);

Мы моделируем
шум одометрии
с помощью случайных чисел, распределение которых
имеет нулевое
среднее и ковариа­
цию, заданную V.
Случайный характер
шума означает, что
повторные вызовы
этой функции будут
возвращать разные
значения.

Расчет пути с использованием одометрии 311

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

6.1.2

Оценка положения

Проблема, которую мы, как и штурманы кораблей, должны преодолеть, заключается в оценке нового положения по предыдущему положению и зашумленным данным одометрии. Математический инструмент, который мы будем использовать для решения
этой задачи, называется фильтр Калмана. Это рекурсивный алгоритм, который на каждом временном шаге обновляет оптималь­
ную оценку неизвестной истинной конфигурации и связанную
с ней неопределенность на основе предыдущей оценки, управляющего сигнала и зашумленных данных измерений.
Фильтр Калмана более подробно описывается в приложении H. Можно показать, что этот фильтр обеспечивает оптимальную оценку состояния системы, предполагая, что шум
имеет гауссовское распределение с нулевым средним. Фильтр
Калмана создавался для линейных систем, а наша модель движения робота (6.2) нелинейна, поэтому мы воспользуемся расширенным фильтром Калмана (extended Kálmán filter, EKF).
Для начала рассмотрим одномерный пример (рис. 6.4а). Исходная плотность вероятности имеет среднее значение x̂〈k〉 = 2
и дисперсию P〈k〉 = 0.25. Одометрия равна 2, а ее дисперсия
V = 0, поэтому прогнозируемая функция плотности вероятности (PDF) на следующем временном шаге просто сдвигается
к среднему значению x̂〈k + 1〉 = 4. На рис. 6.4б дисперсия одомет­
рии V = 0.5, поэтому прогнозируемая PDF по-прежнему имеет
среднее значение x̂〈k + 1〉 = 4, но распределение теперь шире,
что учитывает неопределенность в одометрии.
PDF на шаге k
среднее на шаге k
прогноз PDF на шаге k + 1
прогноз среднего на шаге k + 1

PDF на шаге k
среднее на шаге k
прогноз PDF на шаге k + 1
прогноз среднего на шаге k + 1

одометрия

а

б

Рис. 6.4. Одномерный пример прогнозирования с помощью фильтра Калмана. Исходная плотность распределения
имеет среднее значение 2 и дисперсию 0.25, а значение одометрии равно 2: а) прогнозируемая PDF для случая
отсутствия шума одометрии V = 0; б) обновленная PDF для случая V = 0.5

312 Глава 6 · Определение местоположения и картографирование

Для многомерной задачи локализации робота вектор состояния – это конфигурация робота
x = (xυ, yυ, θυ)T,
а уравнение прогнозирования ►
x̂+(k+1) = f(x̂〈k〉, û〈k + 1〉),

(6.3)

P̂ +〈k + 1〉 = Fx P̂〈k〉FxT + FυV̂FυT

(6.4)

описывает изменение состояния и ковариации со временем.
Член x̂+(k+1) обозначает оценку x в момент времени k + 1, основанную на информации, полученной до момента времени k
включительно. û〈k〉 – это входные данные процесса, каковыми в данном случае является измеренная одометрия, поэтому
û〈k〉 = δ〈k〉. V̂ – это наша оценка ковариации шума одометрии,
которая в действительности нам неизвестна.
Ковариационная мат­ри­ца симметрична

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

(6.5)
и описывает неопределенность в предполагаемой конфигурации робота, а также зависимость между элементами конфигурации. Дисперсия ► рассматривает одно состояние и cov(u, u) =
σu2. Ковариация между состояниями u и v записывается как
cov(u, υ) = cov(υ, u) и равна нулю, если u и υ не зависят друг от
друга. Выделенный верхний левый угол отображает ковариацию местоположения робота, а не его конфигурации.
Два члена в (6.4) имеют особую форму – преобразование
подобия FΣF T, где Σ – ковариационная мат­ри­ца. Для функции
y = f(x) ковариация между x и y определяется соотношением
Σy = FΣxF T, где F – мат­ри­ца Якоби (якобиан), векторная версия
производной, которая подробнее описывается в приложении E.
Матрицы Якоби получаются путем дифференцирования (6.2)
и оценки результата при υ = 0 ►
(6.6)

Ковариация
определяется как
N-1
cov(u, υ) = ∑i=0
(ui - –
u)
(υi - –
υ ).

Значение шума
υ невозможно
измерить, поэтому
мы оцениваем производную, используя
среднее значение
υ = 0.

Расчет пути с использованием одометрии 313
Отступление 6.4. Преподобный Томас Байес
Байес (1702–1761) был нонконформистским пресвитерианским священником и изучал логику и теологию в университете Эдинбурга. Жил и работал в Танбридж-Уэллсе (графство Кент, Англия). Благодаря
связи со вторым графом Стэнхоупом заинтересовался
математикой и был избран в Королевское общество
в 1742 году. После смерти его друг Ричард Прайс отредактировал и опубликовал в 1763 году работу Томаса Байеса под названием «An Essay towards solving a
Problem in the Doctrine of Chances», содержащую формулировку частного случая теоремы Байеса. Байес похоронен на кладбище Банхилл-Филдс в Лондоне.
Теорема Байеса – одна из основных теорем элементарной теории вероятностей, которая позволяет
определить вероятность события при условии, что
произошло другое статистически взаимозависимое с ним событие. Рассмотрим
гипотезу о том, что робот находится в точке X и наблюдает известный ориентир S.
Апостериорная вероятность того, что робот находится в точке X, если наблюдается S, равна

где P(X) – априорная вероятность, что робот находится в точке X (без учета какой-либо сенсорной информации), P(S|X) – вероятность наблюдения ориентира
S при условии, что робот находится в точке X, а P(S) – априорная вероятность
наблюдения S. Фильтр Калмана и метод Монте-Карло, которые мы обсудим далее
в этой главе, по сути, представляют собой два разных подхода к решению этой
обратной задачи.

(6.7)
которые являются функциями текущего состояния робота и одометрии. Мы опустили обозначение шага времени 〈k〉,
чтобы уменьшить путаницу. Все объекты суперкласса Vehicle
предоставляют методы Fx и Fv для вычисления этих матриц, например
>>> robot.Fx([0, 0, 0], [0.5, 0.1])
array([[
1,
0,
0],
[
0,
1,
0.5],
[
0,
0,
1]])

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

314 Глава 6 · Определение местоположения и картографирование
Отступление 6.5. Фильтр Калмана
Рудольф Калман (1930–2016) – системный теоретик, родившийся в Будапеште. Получил степени бакалавра и магистра по электротехнике
в Массачусетском технологическом институте,
а также степень доктора философии в Колумбийском университете в 1957 году. Работал
математиком-исследователем в научно-исследовательском институте в Балтиморе. С 1958
по 1964 год развивал свои идеи по оценке. Они
были встречены его коллегами с некоторым
скептицизмом, поэтому он выбрал журнал, посвященный механике (а не электротехнике), для
публикации своей статьи «A new approach to linear filtering and prediction problems», объясняя
свое решение так: «если вы опасаетесь ступить
на священную территорию с укоренившимися
интересами, то лучше обойдите ее стороной».
Он получил множество наград, включая Почетную медаль IEEE, Премию Киото и Премию Чарльза Старка Дрейпера.
Стэнли Ф. Шмидт (1926–2015) – исследователь, работавший в исследовательском центре Эймса (NASA), и один из первых сторонников фильтра Калмана. Он
разработал первую реализацию, а также нелинейную версию, ныне известную как
расширенный фильтр Калмана. Его реализация была использована в навигационном компьютере Аполлона для оценки траектории. Вот выдержка из знаменитой
статьи Калмана (1960) (публикуется с разрешения ASME).

Для моделирования робота и EKF с помощью Toolbox необходимо определить начальную ковариацию. Для этого выберем
диагональную мат­ри­цу
>>> x_sdev = [0.05, 0.05, np.deg2rad(0.5)];
>>> P0 = np.diag(x_sdev) ** 2;

записанную в терминах стандартного отклонения местоположения и ориентации, которые составляют 5 см и 0.5° соответственно. Это подразумевает наличие хорошего представления
о начальной конфигурации. Затем передадим мат­ри­цу в конст­
руктор объекта EKF:
>>> ekf = EKF(robot=(robot, V), P0=P0)
EKF object: 3 states, estimating: vehicle pose, map
robot: Bicycle: x = [ 0, 0, 0 ]
L=1, steer_max=1.41372, speed_max=inf, accel_max=inf
V_est: [ 0.0004, 0 | 0, 7.62e-05 ]

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

Расчет пути с использованием одометрии 315

Запустим фильтр на 20 секунд
>>> ekf.run(T=20);

Робот начнет движение с исходной точки и будет двигаться
по случайной траектории. На каждом шаге фильтр будет обновлять оценку состояния, используя различные методы, предоставляемые объектом транспортного средства.
Мы можем нарисовать истинный путь, пройденный роботом,
который хранится в объекте суперкласса VehicleBase:
>>> robot.plot_xy(color="b")

и оценку пути с использованием фильтра, хранимую в объекте EKF:
>>> ekf.plot_xy(color="r")

Они показаны на рис. 6.5. Как видите, между истинной
и предполагаемой траекториями робота имеется некоторое
расхождение.
Ковариация на временном шаге 150 равна
>>> P150 = ekf.get_P(150)
array([[ 0.1666, -0.04335, 0.03476],
[-0.04335, 0.09977, -0.01525],
[ 0.03476, -0.01525, 0.01158]])

10.0

Истинная
Оценка EKF

7.5
5.0

Y

2.5
0.0
−2.5

Рис. 6.5. Расчет траектории
с использованием EKF. Здесь
показаны истинная и вычисленная
траектории. Зеленые эллипсы
обозначают области 95%-ной
уверенности. Робот стартует из
точки в начале координат

−5.0
−7.5
−10.0
−10.0

−7.5

−5.0

−2.5

0.0
X

2.5

5.0

7.5

10.0

Матрица симметрична, а диагональные элементы представляют собой оценки дисперсии, связанной с состояниями,
то есть σx2, σy2 и σθ2 соответственно. Стандартное отклонение σx

316 Глава 6 · Определение местоположения и картографирование

функции плотности вероятности, связанной с координатой x
робота, равно
>>> np.sqrt(P150[0, 0])
0.4081

Существует 95%-ная вероятность, что координата x робота
находится в пределах ±2σ или ±0.81 м в данном случае. Аналогично можно вычислить неопределенность для y и θ.
Недиагональные члены являются коэффициентами корреляции и определяют силу связи между соответствующими переменными. Например, значение p0,2 = p2,0 = 0.0348 указывает, что
неопределенности x и θ связаны – ошибка в курсовом угле приводит к ошибке в координате x, и наоборот. Точно так же новая
информация о θ может быть использована для коррекции θ,
а также x. Неопределенность местоположения описывается
верхней левой ковариационной подмат­ри­цей 2×2 мат­ри­цы P̂,
которую можно интерпретировать как эллипс, определяющий
доверительную границу положения. Мы можем наложить такие
эллипсы на траекторию, используя
>>> ekf.plot_ellipse(filled=True, facecolor="g", alpha=0.3)

как показано на рис. 6.5. Они соответствуют 95%-ной доверительной границе по умолчанию и отображаются через каждые
10 шагов. Робот начал движение в начале координат, и по мере
его продвижения мы видим, что постепенно увеличиваются,
что говорит об увеличении оцениваемой неопределенности.
Эллипсы показывают только неопределенность положения по
осям x и y, но неопределенность курсового угла θ также увеличивается.
Оцененная неопределенность положения и направления
определяется формулой det(P̂), которую можно отобразить как
функцию времени (сплошная кривая на рис. 6.6)

Элементы P имеют
разные единицы
измерения: м²
и рад². Поэтому
неопределенность
представляет собой
смесь пространственной и угловой
неопределенностей
с неявными весовыми коэффициентами. Если диапазон
переменных x и y
≫ π, то преобладает
пространственная
неопределенность.

>>> t = ekf.get_t();
>>> pn = ekf.get_Pnorm();
>>> plt.plot(t, pn);

Как видите, неопределенность никогда не уменьшается, потому что в (6.4) мы прибавляем положительно определенную
мат­ри­цу (второй член) к оценочной ковариации.
Значения V и V̂ определяют скорость роста неопределенности, как показано на рис. 6.6. Если V̂ > V, то P̂ будет больше,
чем должно быть, и фильтр будет слишком пессимистичен,
переоценивая неопределенность и сообщая о меньшей определенности, чем есть на самом деле. Если V̂ < V, то P̂ будет
меньше, чем должно быть, и фильтр будет слишком уверен
в своей оценке, то есть фактическая неопределенность будет

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

Локализация с помощью ориентиров на карте 317

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

0.5
1
2

4

(detP) 0.5

3

2

1

0
0.0

2.5

5.0

7.5

10.0

Время (с)

12.5

15.0

17.5

20.0

Рис. 6.6. Общая неопределенность, задаваемая как det(P̂), с течением времени
демонстрирует монотонный рост. Изменение величины V̂ влияет на скорость роста
неопределенности. Кривые показаны для V̂ = αV, где α = 0.5, 1, 2

Матрицы V и V̂
Мы дважды использовали мат­ри­цу ковариации одометрии V. Первый раз – в вызове конструктора Vehicle, где она представляла ковариацию V гауссовского шума с нулевым средним значением,
добавляемого к истинной одометрии для моделирования ошибки
в (6.2). В реальных ситуациях этот шум генерируется некими физическими процессами, скрытыми внутри робота, параметры которого нам неизвестны. Второй раз – в вызове конструктора EKF, где
она представляла ковариацию V̂ – наилучшую оценку ковариации
одометрии и использовалась в уравнении обновления ковариации
состояния фильтра (6.4).

6.2

Локализация с помощью ориентиров
на карте

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

318 Глава 6 · Определение местоположения и картографирование
Отступление 6.6. Эллипсы ошибок
Мы рассматриваем функцию распределения вероятностей местоположения робота (без учета ориентации) как двухмерную (рис. 6.3) гауссовскую функцию плотности вероятности

где x = (x, y)T – местоположение робота, μx = (x̂, ŷ)T – это оценочное среднее мес­
тоположение, а Pxy ∈ ℝ2×2 – это ковариационная мат­ри­ца местоположения, левая
верхняя часть ковариационной мат­ри­цы P, как показано в (6.5). Горизонтальное
сечение функции распределения вероятностей – контур постоянной вероятности,
представляющий собой эллипс, определяемый точками x так, что
(x - μx)T Pxy-1(x - μx) = s.
Такие эллипсы ошибок часто используются для представления пространственной неопределенности (рис. 6.5). Большой эллипс соответствует более широкому
пику плотности вероятности и меньшей уверенности в определении местоположения. Для получения определенного контура достоверности (например, 99 %)
нужно выбрать s как обратную величину χ2 ­кумулятивного распределения для
двух степеней свободы. В Python можно использовать функцию chi2inv(C, 2) из
пакета scipy.stats.distributions, где C ∈ [0, 1] – это значение уверенности. Значения
уверенности можно передавать нескольким методам EKF, чтобы задать эллипсы
ошибок.
Удобной скалярной мерой полной неопределенности местоположения является
площадь эллипса πr1r2, где радиусы ri = λi и λi являются собственными значения­ми
Pxy. Поскольку det(Pxy) = ∏i λi, площадь эллипса (скалярная неопределенность) пропорциональна det(Pxy). См. также раздел C.1.4 и приложение G.

Продолжая одномерный пример рис. 6.4б, предположим,
что робот оснащен датчиком, измеряющим местоположение
робота относительно некоторой внешней точки отсчета. Плотность распределения вероятностей (PDF) измерений датчика
показана синей кривой на рис. 6.7a и имеет среднее значение
x̂ = 5, а также некоторую неопределенность, обусловленную собственной дисперсией Ŵ = 1. Среднее значение измерения датчика отличается от среднего значения прогноза, основанного
на одометрии, x̂ = 4, что можно объяснить занижением одомет­
рии из-за пробуксовки колес. Теперь у нас есть две плотности
распределения вероятностей, имеющих отношение к местоположению робота: пунктирная красная линия соответствует
прог­нозу на основе одометрии, а синяя представляет измерения
с датчика. Истинная плотность вероятностей является произве­
дением этих двух гауссовых распределений, которое тоже является гауссовой функцией (показана сплошной красной кривой).
Мы объединили две неопределенные оценки для получения новой, более оптимальной оценки, и получили заметное снижение
неопределенности, связанной с местоположением робота.

Локализация с помощью ориентиров на карте 319
PDF на шаге k
прогноз PDF на k + 1
PDF датчика на k + 1
оценка PDF на k + 1

PDF на шаге k
прогноз PDF на k + 1
PDF датчика на k + 1
оценка PDF на k + 1

а

б

Рис. 6.7. Одномерный пример прогнозирования с помощью фильтра Калмана, синяя и красная пунктирная кривые
взяты из рис. 6.4б. Датчик предоставляет дополнительную информацию для улучшения прогноза: а) для случая,
когда датчик имеет дисперсию Ŵ = 1; б) для случая, когда датчик имеет дисперсию Ŵ = 0.3

При использовании более качественного датчика с дисперсией Ŵ = 0.3, как показано на рис. 6.7б, скорректированный
прогноз, представленный сплошной красной кривой, имеет
меньшую дисперсию и ближе к среднему значению показаний
датчика. Имея две оценки, одну по одометрии, а другую по показаниям датчика, мы больше доверяем оценке с меньшей дисперсией.
Решая задачу робототехники, мы предполагаем, что робот
способен наблюдать за набором ориентиров с помощью встроенного датчика. На данный момент будем считать, что местоположение ориентиров известно, и мы можем использовать
карту, содержащую N фиксированных, но случайно расположенных ориентиров, чьи местоположения известны. Пакет
Toolbox предоставляет объект LandmarkMap
>>> map = LandmarkMap(20, workspace=10)
LandmarkMap object with 20 landmarks,
workspace=(-10.0: 10.0, -10.0: 10.0)

который в этом примере содержит N = 20 ориентиров, равномерно распределенных случайным образом по области, охватывающей ±10 м в направлениях x и y, и их можно отобразить
командой
>>> map.plot()

Робот оснащен датчиком, который обеспечивает возможность наблюдения за ориентирами относительно робота, как
описывает уравнение
z = h(q, pi),

(6.8)

где q = (xυ, yυ, θυ)T – конфигурация робота, а pi = (xi, yi)T – известное
местоположение i-го ориентира в мировой системе координат.

320 Глава 6 · Определение местоположения и картографирование

Для придания обсуждению большей конкретики рассмотрим
распространенный тип датчика, который измеряет дальность
до ориентира и угол пеленга на него, например радар или лидар, ► такой как на рис. 6.25б–г. Датчик установлен на борту
робота, поэтому наблюдение за i-м ориентиром осуществляется согласно формуле

(6.9)

где z = (r, β)T, r – диапазон, β – угол пеленга и w = (wr, wβ)T – гауссовская случайная величина с нулевым средним, которая моделирует ошибки в датчике

Постоянная диагональная ковариационная мат­ри­ца указывает, что ошибки определения дальности и пеленга не зависят
друг от друга. ►
В этом примере установим неопределенность датчика равной σr = 0.1 м и σβ = 1° и получим ковариационную мат­ри­цу датчика
>>> W = np.diag([0.1, np.deg2rad(1)]) ** 2;

Этот тип датчика можно смоделировать с помощью объекта
RangeBearingSensor ►:
>>> sensor = RangeBearingSensor(robot=robot, map=map, covar=W,
...
angle=[-pi/2, pi/2], range=4, animate=True)
RangeBearingSensor sensor class
LandmarkMap object with 20 landmarks,
workspace=(-10.0: 10.0, -10.0: 10.0)
W = [ 0.01, 0 | 0, 0.000305 ]
sampled every 1 samples
range: (0: 4)
angle: (-1.57: 1.57)

Здесь мы связываем датчик с объектами, представляющими
робота и карту, а также задаем ковариационную мат­ри­цу датчика W, максимальную дальность и пределы угла пеленга. Метод
reading объекта датчика возвращает пеленг и дальность до видимого ориентира ► вместе с его идентификатором, например
>>> z, i = sensor.reading()
>>> z
array([
3.04, -1.559])

Радио- или оптические датчики
обнаружения
и измерения
дальности (radio/
light detection and
ranging) оценивают
расстояние, измеряя
время прохождения
туда и обратно
микроволнового
сигнала или инфракрасного импульса,
отраженного от объекта и полученного
датчиком.

А также что ковариация не зависит
от расстояния, но
в действительности
ковариация может
увеличиваться с расстоянием, потому
что интенсивность
отраженного сигнала быстро падает
(1/r 4) с расстоянием r до цели.
Наследует класс
SensorBase.

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

Локализация с помощью ориентиров на карте 321
>>> i
15

Идентификатор – это целое число i = 0, ..., 19, поскольку карта
была создана с 20 ориентирами. Мы избежали проблемы идентификации, предположив, что нам известен идентификатор
обнаруженного ориентира. Местоположение ориентира, например с идентификатором 15, можно найти на карте
>>> map[15]
array([ -6.487,

-3.795])

Используя (6.9), робот может оценить пеленг и расстояние
до ориентира, основываясь на собственном предполагаемом
местоположении и известном местоположении ориентира на
карте. Разница между наблюдаемым z и предполагаемым мес­
тоположением
v〈k〉 = z〈k〉 - h(x̂+〈k〉, pi)

(6.10)

x̂〈k + 1〉 = x̂+〈k + 1〉 + Kv〈k + 1〉

(6.11)

K = P+〈k + 1〉HxT(Hx P+〈k + 1〉HxT + HwŴHwT)-1,

(6.12)

определяет ошибку в оценке положения робота x̂+ – робот находится не там, где ожидалось. Это различие называется инно­
вацией, потому что представляет ценную новую информацию
и является ключевым для фильтра Калмана.
На втором шаге фильтр Калмана обновляет предсказанное
состояние и ковариацию, вычисленные с использованием (6.3)
и (6.4), и обозначается верхним индексом +. Шаг обновления
оценки состояния

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

Это можно записать
как
S = HxP+〈k + 1〉HxT
+ HwŴHwT ,
+
K = P 〈k + 1〉HxT S–1,
где S – расчетная
ковариация инновации.

где Ŵ – предполагаемая ковариация шума датчика, а Hx и Hw –
якобианы.
K называется мат­ри­цей усиления Калмана. Она
распределяет инновации, полученные из наблюдения ориентира, для обновления каждого элемента вектора состояния –
местоположения и ориентации робота. Внедиагональные элементы ковариационной мат­ри­цы представляют корреляции
ошибок между состояниями и используются фильтром Калмана

322 Глава 6 · Определение местоположения и картографирование

так, чтобы изменение в одном состоянии оптимально обновляло другие состояния. Величина обновления оценки состояния
зависит от ковариации соответствующего датчика: чем ниже
неопределенность, тем больше величина обновления оценки
состояния.
Обновление ковариации также зависит от коэффициента
усиления Калмана.
P̂〈k + 1〉 = P̂+〈k + 1〉 - KHx P̂+〈k + 1〉.

(6.13)

Обратите внимание, что второй член в (6.13) вычитается из
оценки ковариации. Это дает возможность уменьшить ковариа­
цию, что было невозможно в случае счисления пути (6.4).
Якобианы Hx и Hw получаются путем дифференцирования
(6.9), что дает
(6.14)
то есть функцию местоположения ориентира и робота, а также
расстояния до ориентира; и
(6.15)
Объект RangeBearingSensor, представленный выше, включает
методы h, реализующий (6.9), и Hx и Hw для вычисления соответствующих якобианов.
EKF выполняется в два этапа: прогнозирования и обновления, которые показаны на рис. 6.8.
Прогнозирование состояния на шаг вперед
Прогнозирование ковариации на шаг вперед

Этап прогнозирования

Новая информация – инновация
Распределение инновации по состояниям –
усиление Калмана
Состояние обновлено с учетом инновации
Обновление ковариации

Этап обновления
Рис. 6.8. Обобщенное представление алгоритма расширенного фильтра Калмана с этапами
прогнозирования и обновления

Локализация с помощью ориентиров на карте 323

Теперь у нас есть все необходимое для оценки местоположения с использованием одометрии и наблюдений за ориентирами на карте. Вот как выглядит реализация с использованием
Toolbox:
>>> map = LandmarkMap(20, workspace=10);
>>> V = np.diag([0.02, np.deg2rad(0.5)]) ** 2
array([[ 0.0004,
0],
[
0, 7.615e-05]])
>>> robot = Bicycle(covar=V, animation="car");
>>> robot.control = RandomPath(workspace=map)
>>> W = np.diag([0.1, np.deg2rad(1)]) ** 2
array([[
0.01,
0],
[
0, 0.0003046]])
>>> sensor = RangeBearingSensor(robot=robot, map=map, covar=W,
...
angle=[-pi/2, pi/2], range=4, animate=True);
>>> P0 = np.diag([0.05, 0.05, np.deg2rad(0.5)]) ** 2;
>>> ekf = EKF(robot=(robot, V), P0=P0, map=map, sensor=(sensor, W));

Аргумент workspace конструктора LandmarkMap устанавливает
диапазон карты в ±10 м в каждом измерении, и эта информация также используется конструкторами RandomPath и RangeBearingSensor.
Запуск симуляции на 20 секунд
>>> ekf.run(T=20)

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

map.plot()
robot.plot_xy();
ekf.plot_xy();
ekf.plot_ellipse()

чтобы получить картину, изображенную на рис. 6.9а. Как видите, эллипсы ошибок сначала увеличиваются по мере движения
робота, так же как в случае счисления пути (рис. 6.5), а затем
сжимаются, как только в поле зрения робота появляется ориентир с известным местоположением и происходит корректировка вычисленного состояния робота. Крупный план на рис. 6.9б
показывает фактическую и предполагаемую траектории движения робота, причем последняя имеет резкий изгиб в месте
коррекции. Для коррекции состояния на этапе обновления
фильтра Калмана, помимо одометрии, была использована новая информация. Эллипсы ошибок теперь значительно меньше,
и многие из них едва заметны.
На рис. 6.10а показано, что общая неопределенность больше
не растет монотонно. Когда в поле зрения робота появляется
ориентир, то оценочная ковариация резко уменьшается. Ри-

10.0

2.00

7.5

1.75

5.0

1.50

2.5

1.25

0.0

1.00

y

y

324 Глава 6 · Определение местоположения и картографирование

−2.5

0.75
ориентир
95%-ная достоверность
Истинная
Оценка EKF

−5.0

0.50

−7.5
−10.0
−10.0

ориентир
95%-ная достоверность
Истинная
Оценка EKF

0.25

−7.5

−5.0

−2.5

0.0

2.5

5.0

7.5

0.00
−3.50

10.0

−3.25

−3.00

−2.75

−2.50

−2.25

−2.00

−1.75

−1.50

0

5

10

15

20

25

30

35

40

0

5

10

15

20

25

30

35

40

0

5

10

15

20

25

30

35

40

0

5

10

x
x
а
б
Рис. 6.9. а) Локализация EKF со счислением пути и ориентирами, показывающая истинный и предполагаемый путь
робота. Черные крестики – это ориентиры, а зеленые эллипсы – 95%-ные доверительные границы. Робот начинает
движение в точке начала координат; б) увеличенное изображение истинного и предполагаемого пути робота,
показывающее уменьшение влияния наблюдения за ориентиром

×10 −3

0.5
Ошибка x

1.0

0.0
−0.5

Ошибка y

0.8

Ошибка θ

(detP) 0.5

0.6

0.4

0.25
0.00
−0.25

0.1
0.0
−0.1

id ориентира

0.2

0.0
0

5

10

15

20

25

30

35

40

20
10
0

15

20

25

30

35

Время (с)
Время (с)
а
б
Рис. 6.10. а) Величина ковариации как функция времени. Общая неопределенность вычисляется как det(P)
и, как показано здесь, не увеличивается непрерывно со временем; б) вверху: ошибка оценки положения с 95%ным доверительным интервалом (окрашенным в коралловый цвет); внизу: диаграмма рассеяния наблюдаемых
ориентиров и периоды отсутствия наблюдений

сунок 6.10б показывает ошибку, связанную с каждым компонентом положения, а розовый фон отражает предполагаемую
95%-ную доверительную границу (выводится из ковариационной мат­ри­цы), и, как видите, ошибка в основном находится
в пределах этой области. Ниже представлены наблюдения за
ориентирами, и мы видим, что доверительные границы очень
узкие (что указывает на низкую неопределенность) при наличии информации о наблюдении ориентиров, но они начинают
расти в периоды, когда ориентиры не наблюдаются.
EKF – чрезвычайно мощный инструмент, позволяющий обновлять состояние на основе данных, получаемых от множества

40

Локализация с помощью ориентиров на карте 325
Отступление 6.7. Идентификация ориентиров
До сих пор мы предполагали, что можем абсолютно точно идентифицировать наблюдаемый ориентир, но в действительности
такое редко бывает возможно. Однако мы
можем сравнить наблюдение с прогнозируемым местоположением всех известных
в данный момент ориентиров и решить,
какой из них наблюдается, или, может
быть, это новый ориентир. Это решение
должно учитывать неопределенность, связанную с положением робота, измерением датчика и ориентирами на карте. Эта
проблема называется проблемой идентификации ориентира (или ассоциации
данных). Ошибки на данном этапе потенциально катастрофичны – неправильная
идентификация повлечет неправильную корректировку состояния робота, что
увеличит вероятность ошибочной идентификации ориентира в следующем цик­
ле. На практике фильтры используют ориентир, только когда высока уверенность
в правильной идентификации, которая включает вычисление расстоян­ия Махаланобиса и использование критерия χ­2 для проверки уверенности. Если ситуация
неоднозначна, то лучше не использовать ориентир, потому что ошибочное обновление может принести больше вреда, чем пользы.
Альтернативой является использование оценки с множеством гипотез, например фильтра частиц (обсуждается в разделе 6.6), который может моделировать
вероятность наблюдения ориентира А или ориентира Б, а будущие наблюдения
подтвердят одну гипотезу и ослабят другие. Расширенный фильтр Калмана использует гауссовскую вероятностную модель с одним пиком, что позволяет выдвигать только одну гипотезу о положении робота.
(На изображении: крушение парахода «Тараруа», 1881 г.)

различных датчиков. Именно поэтому задачу оценки также называют слиянием данных от датчиков. Например, для обновления состояния можно использовать курс по компасу, угловую
скорость рыскания по гироскопу, угол пеленга цели по камере
и местоположение по GPS. Для каждого датчика достаточно указать только функцию наблюдения h(·), якобианы Hx и Hw и некоторую оценку ковариации датчика Ŵ. Функция h(·) может быть
нелинейной и даже необратимой – все остальное сделает EKF.
Отступление 6.8. Искусственные ориентиры
Для некоторых приложений нередко целесообразно использовать искусственные ориентиры (реперные маркеры), которые легко распознать камерой. Они не только визуально
выделяются в окружающей среде, но и могут кодировать уникальный идентификатор. К часто используемым двухмерным штрихкодам относятся маркеры ArUco, ARTag и AprilTag
(показаны здесь). Если камера откалибрована, то можно даже
определить расстояние до маркера и направление нормали
к его поверхности. Toolbox поддерживает обнаружение и чтение маркеров ArUco и AprilTag, которые обсуждаются в разделе 13.6.1.

326 Глава 6 · Определение местоположения и картографирование
Матрицы W и Ŵ
Подобно обсуждавшейся ранее мат­ри­це V, мат­ри­ца W тоже используется дважды. Первый раз она используется в конструкторе RangeBearingSensor и представляет ковариацию W гауссовского шума с нулевым
средним, которая добавляется к вычисленным оценкам дальности
и пеленга для имитации погрешности датчика, как показано в (6.9).
В реальном приложении этот шум генерируется неким скрытым
физическим процессом внутри датчика, и его параметры нам не известны. Второй раз мат­ри­ца используется как Ŵ – наилучшая оценка
ковариации датчика, используемая фильтром Калмана (6.12).
Относительная величина W и Ŵ влияет на скорость роста ковариации. Чем больше Ŵ, тем меньше следует доверять показаниям
датчиков, поэтому снижение ковариации с каждым наблюдением
будет уменьшаться.

6.3

Создание карты ориентиров

До сих пор мы принимали существование карты как нечто само
собой разумеющееся, и это понятно, учитывая, что в настоящее
время карты широко распространены и доступны бесплатно
в интернете. Но карты, которые мы используем, должны кем-то
или чем-то создаваться. В этом разделе мы рассмотрим задачу
создания карты – определения местоположения ориентиров –
в среде, по которой движется робот.
Как и прежде, будем считать, что на роботе установлен датчик, определяющий расстояние и пеленг ориентиров относительно робота, пусть и неточно. Будем считать, что датчик может точно идентифицировать каждый наблюдаемый ориентир,
а местоположение робота точно известно – он обладает идеальной локализацией. Это нереальный сценарий, но он является
важным концептуальным шагом к следующему разделу.
Поскольку положение робота точно известно, ее не нужно
оценивать, но нужно оценить координаты ориентиров. В этой
задаче вектор состояния включает координаты M наблюдаемых
в данный момент ориентиров
x = (x0, y0, x1, y1, ..., xM-1, yM-1)T ∈ ℝ2M × 1.

Ковариационная мат­ри­ца симметрична:

,

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

(6.16)

Создание карты ориентиров 327

и каждый блок 2×2 на главной диагонали представляет ковариа­
цию местоположения соответствующего ориентира, а блоки
вне диагонали представляют ковариацию между ориентирами.
Изначально число M = 0 и увеличивается при обнаружении
каждого не наблюдавшегося ранее ориентира. Вектор состояния имеет переменную длину, потому что количество ориентиров в окружающей среде заранее неизвестно. Оценочная ковариация P̂ тоже имеет переменный размер.
Уравнение прогнозирования в этом случае выглядит просто, так
как предполагается, что ориентиры являются стационарными:
x̂+〈k + 1〉 = x̂〈k〉;

P̂+〈k + 1〉 = P̂〈k〉.

(6.17)
(6.18)

Введем функцию g(·), которая является обратной к функции
h(·) и возвращает мировые координаты наблюдаемого ориентира на основе известного положения робота (xυ, yυ, θυ) и наблюдения, полученного от датчика:
(6.19)
которая также известна как обратная функция измерения или
обратная модель датчика. Поскольку x̂ имеет переменную длину, то при обнаружении ранее не наблюдавшегося ориентира
необходимо расширить вектор состояния и ковариационную
мат­ри­цу. Вектор состояния расширяется функцией y(·):
x¢〈k〉 = y(x〈k〉, z〈k〉, xυ〈k〉)

(6.20)
(6.21)

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

где Yz – якобиан вставки,
(6.22)

328 Глава 6 · Определение местоположения и картографирование

связывающий скорость изменения расширенного вектора состояния с новым наблюдением. M – это размерность P̂ до расширения, а якобианы
(6.23)

(6.24)
Gx будет нулевой мат­ри­цей, потому что g(·) не зависит от карты и, следовательно, от x. Дополнительный якобиан для h(·)
(6.25)
описывает, как изменяются показания датчика в зависимости
от местоположения ориентира i для конкретного положения
робота, и создается методом Hp.
В случае отображения якобиан Hx, используемый в (6.13), описывает, как наблюдение ориентира изменяется относительно
полного вектора состояния. Однако наблюдение зависит только
от положения наблюдаемого ориентира, поэтому этот якобиан
в основном состоит из нулей
(6.26)
где Hpi – это элемент вектора состояния, соответствующий ориентиру i. Такая структура отражает тот факт, что наблюдение за
конкретным ориентиром дает информацию для оценки местоположения именно этого ориентира, но не других.
Реализация с использованием Toolbox выглядит так:
>>> map = LandmarkMap(20, workspace=10);
>>> robot = Bicycle(covar=V, animation="car");
>>> robot.control = RandomPath(workspace=map);
>>> W = np.diag([0.1, np.deg2rad(1)]) ** 2
array([[
0.01,
0],
[
0, 0.0003046]])
>>> sensor = RangeBearingSensor(robot=robot, map=map, covar=W,
...
range=4, angle=[-pi/2, pi/2], animate=True);
>>> ekf = EKF(robot=(robot, None), sensor=(sensor, W));

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

Создание карты ориентиров 329

не указана, так как она изначально является мат­ри­цей 0×0. Запускаем симуляцию на 100 секунд:
>>> ekf.run(T=100);

и наблюдаем анимацию движения робота и эллипсы ковариации, связанные с объектами карты, которые меняются с течением времени. Оценки местоположения ориентиров
>>> map.plot();
>>> ekf.plot_map();
>>> robot.plot_xy();

показаны на рис. 6.11а в виде эллипсов с 95%-ными доверительными границами вместе с истинными положениями ориентиров и траекторией движения робота. На рис. 6.11б показан
крупный план ориентира 8, и здесь видно, что оценка близка
к истинному значению и находится в пределах эллипса достоверности.
Ковариационная мат­ри­ца графически представлена на
рис. 6.11в и имеет блочно-диагональную структуру. Недиагональные элементы равны нулю, и это означает, что оценки мес­
тоположения ориентиров независимы. Это вполне ожидаемо,
потому что наблюдение за одним ориентиром не дает новой
информации о другом.
10.0

3.7200

7.5

3.7175

0

ориентир

−4.75

95%-ная достоверность

5

−5.00
−5.25

3.7150

10

2.5

3.7125

15

0.0

3.7100

−2.5

Состояние

5.0

y

y

истинный путь

−5.50
−5.75

20

−6.00

3.7075

25

3.7050

30

3.7025

35

−6.25

−5.0

−6.50

ориентир
предполагаемый ориентир

−7.5

95%-ная достоверность

−6.75

истинный путь

−10.0
−10.0

а

−7.5

−5.0

−2.5

0.0
x

2.5

5.0

логарифмическая ковариация

предполагаемый ориентир

7.5

10.0

3.7000
0.860

б

0.865

0.870
x

0.875

0.880

0

в

5

10

15

20

Состояние

25

30

35

Рис. 6.11. а) Результаты картографирования EKF, эллипсы неопределенности слишком малы, чтобы их можно было
увидеть; б) увеличенный вид ориентира 8 и его эллипса ковариации; в) окончательная ковариационная мат­ри­ца
имеет блочно-диагональную структуру

Объект EKF использует словарь для сопоставления идентификатора ориентира, возвращаемого объектом RangeBearingSensor,
с координатами этого ориентира в векторе состояния. Например, ориентир 10
>>> ekf.landmark(10)
(12, 22)

замечен 22 раза во время симуляции и был двенадцатым (счет
начинается с 0) встреченным ориентиром, а его предполагае-

330 Глава 6 · Определение местоположения и картографирование

мое местоположение можно найти в векторе состояния EKF, начиная с элементов 24 (12 × 2):
>>> ekf.x_est[24:26]
array([ 6.312,
3.756])

Его оценочная ковариация является подмат­ри­цей внутри P̂:
>>> ekf.P_est[24:26, 24:26]
array([[0.0001317, 5.822e-06],
[5.822e-06, 0.0002383]])

6.4

Одновременные локализация
и картографирование

Наконец, решим задачу одновременного определения местоположения робота и создания карты. Это старая задача морской
навигации и картографии: постепенное расширение карт с одновременным использованием уже готовых карт для навигации. На рис. 6.12 показано, как решить эту задачу без GPS, имея
движущееся судно с плохой одометрией и редкими сеансами
определения местоположения по звездам. В робототехнике эта
задача известна как одновременная локализация и картографирование (simultaneous localization and mapping, SLAM) или
параллельное картографирование и локализация (concurrent
mapping and localization, CML). Это проблема «курицы и яйца»,
поскольку для локализации нужна карта, а для ее построения –
локализация. Однако, исходя из того, что мы узнали в предыдущих разделах, эта проблема решается довольно просто.
Вектор состояния включает конфигурацию робота и координаты M ориентиров, которые были замечены к текущему моменту:
x = (xυ, yυ, θυ, x0, y0, x1, y1, ..., xM-1, yM-1)T ∈ ℝ(2M+3)×1,

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

где P̂υυ ∈ ℝ3×3 – ковариация положения робота, описанная формулой (6.5), P̂mm ∈ ℝ2M×2M – ковариация предполагаемых местоположений ориентиров, как описано в (6.16), и P̂υm ∈ ℝ3×2M – корреляция между состояниями робота и ориентиров.

Одновременные локализация и картографирование 331

Рис. 6.12. Карта побережья
территории Эора (сегодня это
регион Сиднея на востоке
Австралии), созданная
капитаном Джеймсом
Куком в 1770 году. Путь
корабля и карта побережья
определялись одновременно.
Цифры обозначают
глубину в саженях (1.83 м)
(Национальная библиотека
Австралии, карта NK 5557 A)

Прогнозируемое состояние робота и ковариация определяются соотношениями (6.3) и (6.4), а обновление по данным
датчиков – соотношениями (6.11)–(6.15). При обнаружении нового объекта вектор состояния обновляется с использованием
якобиана вставки Yz, определяемого соотношением (6.22), но
в этом случае Gx уже не будет нулевой мат­ри­цей
(6.27)
потому что оценка нового ориентира зависит от вектора состоя­
ния, который теперь содержит конфигурацию робота.
В случае SLAM якобиан Hx, используемый в (6.13), описывает
изменение наблюдения ориентира относительно вектора состояния. Наблюдение зависит только от двух элементов вектора
состояния: местоположения робота и местоположения наблюдаемого ориентира.
(6.28)

332 Глава 6 · Определение местоположения и картографирование

где Hpi находится в точке вектора состояния, соответствующей
i-му ориентиру. Это похоже на (6.26), но с дополнительным ненулевым блоком Hpυ, заданным формулой (6.14).
Матрица усиления Калмана K распределяет инновации из
наблюдения за ориентиром (двухмерный вектор) для обновления каждого элемента вектора состояния – конфигурации робота и местоположения каждого ориентира на карте.
Реализация с использованием Toolbox теперь должна казаться знакомой.
>>> map = LandmarkMap(20, workspace=10);
>>> W = np.diag([0.1, np.deg2rad(1)]) ** 2
array([[
0.01,
0],
[
0, 0.0003046]])
>>> robot = Bicycle(covar=V, x0=(3, 6, np.deg2rad(-45)),
...
animation="car");
>>> robot.control = RandomPath(workspace=map);
>>> W = np.diag([0.1, np.deg2rad(1)]) ** 2
array([[
0.01,
0],
[
0, 0.0003046]])
>>> sensor = RangeBearingSensor(robot=robot, map=map, covar=W,
...
range=4, angle=[-pi/2, pi/2], animate=True);
>>> P0 = np.diag([0.05, 0.05, np.deg2rad(0.5)]) ** 2;
>>> ekf = EKF(robot=(robot, V), P0=P0, sensor=(sensor, W));

И снова объект карты не передается в EKF, потому что он неизвестен фильтру, а P0 – начальная 3×3 ковариация для состояния робота. Начальная конфигурация робота: (3, 6, 45°), но EKF
использует конфигурацию робота по умолчанию (0, 0, 0).
Запускаем симуляцию на 40 секунд.
>>> ekf.run(T=40);

Как обычно, этот вызов отображает анимацию движения робота. Мы также видим эллипсы ковариации, связанные с объектами карты, которые меняются с течением времени. Мы можем
нанести на карту ориентиры и путь, пройденный роботом:
>>> map.plot();
# вывести истинную карту
>>> robot.plot_xy(); # вывести истинный путь

как показано на рис. 6.13a. Далее выведем оценки EKF:
>>> ekf.plot_map();

#
#
>>> ekf.plot_ellipse(); #
>>> ekf.plot_xy();
#

вывести ориентиры в вычисленных
местоположениях
вывести вычисленные ковариации
вывести вычисленный путь робота

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

Одновременные локализация и картографирование 333

Вычисленный путь и вычисленные координаты ориентиров
указаны относительно рамки карты. Основная разница связана
с тем, что EKF использовал начальную конфигурацию робота по
умолчанию (0, 0, 0), которая отличается от фактической начальной конфигурации (3, 6, 45°). Шум датчика тоже влияет на оси
координат карты: их положение зависит от первоначальной
оценки положения робота и ошибки одометрии на первом этапе фильтрации, а ориентация осей координат зависит от счисленной ориентации и шума датчика на момент первого измерения. Знание истинных и вычисленных координат по крайней
мере двух ориентиров позволяет вычислить преобразование
SE(2) из координат карты в мировые координаты:
>>> T = ekf.get_transform(map)
0.7088 -0.7054
2.101
0.7054
0.7088 -6.315
0
0
1

10.0

5.0

7.5

2.5

5.0

0.0

2.5

−2.5

0.0

−5.0

−2.5

−7.5

−5.0

−10.0

−7.5

а

Y

y

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

−10.0
−10.0

−12.5

ориентир
истинный путь робота

−7.5

−5.0

−2.5

вычисленный путь
вычисленный ориентир
99%-ная достоверность
путь робота в координатах SLAM
ориентир в координатах SLAM

0.0
x

2.5

5.0

7.5

10.0

−15.0
−10.0

б

−7.5

−5.0

−2.5

0.0
X

2.5

5.0

7.5

10.0

Рис. 6.13. Одновременная локализация и картографирование: а) истинный путь робота и местоположения
ориентиров в мировой системе координат мира; б) вычисленные путь робота и местоположения ориентиров
в системе координат карты. Истинный путь робота и местоположения ориентиров накладываются друг на друга
после преобразования в систему координат карты. Эллипсы показывают 95%-ный доверительный интервал

Как показано на рис. 6.14а, неопределенность уменьшается
со временем. Окончательная ковариационная мат­ри­ца графически представлена на рис. 6.14б, и мы видим сложную структуру. В отличие от случая картографирования на рис. 6.11, P̂mm

334 Глава 6 · Определение местоположения и картографирование

−1.5

10 −11

5

−2.0

10 −19

10

−2.5

10 −27

15

10 −35

−3.0

20

−3.5

25

10 −43

−4.0

логарифмическая ковариация

0

Состояние

(detP) 0.5

не является блочно-диагональной, и конечные недиагональные
члены представляют корреляции между ориентирами на карте.
Неопределенности ориентиров никогда не увеличиваются, модель прогнозирования местоположения предполагает, что они
не перемещаются, но они никогда не могут опуститься ниже начальной неопределенности местоположения робота в P̂0. Блок
P̂υm – это корреляция между ошибками конфигурации робота
и местоположениями ориентиров.

30

10 −51

−4.5
35

10 −59

−5.0
40

а

0

5

10

15

20

Время (с)

25

30

35

40

б

0

5

10

15

20

25

Состояние

30

35

40

Рис. 6.14. Одновременные локализация и картирование: а) ковариация в зависимости от времени; б) окончательная
ковариационная мат­ри­ца

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

SLAM на основе графа положений 335

6.5

SLAM на основе графа положений

Альтернативный подход к задаче SLAM заключается в ее формулировке в виде графа положений, как показано на рис. 6.15,
где траектория робота рассматривается как последовательность различных положений, и задача состоит в их оценке. На
рисунке изображено два типа вершин: положения робота обозначены кружками, а ориентиры – звездочками. Ребро между
двумя вершинами представляет пространственное ограничение между ними, обусловленное некоторым наблюдением zi,j.
Ограничение между позициями обычно определяется одомет­
рией – пройденным расстоянием и изменением направления.
Ограничение между положением и ориентиром обычно определяется лидаром или камерой – расстоянием и углом направления от робота до ориентира.
Рис. 6.15. Основы формулировки задачи
SLAM в виде графа положений. Робот
движется против часовой стрелки по
квадрату. Предполагаемые положения
обозначены синими кружками,
а сплошные стрелки – это одометрические
измерения, выполняемые роботом при
перемещении между положениями. Синяя
звездочка – это ориентир, а наблюдения
за этой точкой из положений в графе
показаны пунктирными линиями. zi,j – это
данные датчика, связанные с ребром,
соединяющим вершины i и j. Мы
предполагаем, что ξˆ0 = ξ0

По мере того как робот движется по квадрату против часовой
стрелки, он накапливает все больше неопределенных относительных положений, определяемых одометрией, поэтому кумулятивная ошибка в оценке положения вершин увеличивается –
это та самая проблема счисления пути, о которой мы говорили
в разделе 6.1. К тому времени, как робот достигнет четвертого
местоположения, ошибка в оценке его положения становится
значительной. Однако он может наблюдать за ориентиром A,
который видел ранее, и это добавляет ограничение, показанное
красным цветом, которое образует петлю в графе положений, –
такое событие называется замыканием петли.
Функция g(·), например (6.19), оценивает мировые координаты ориентира, учитывая положение робота и данные, полученные от датчика. Оценка g(ξˆ0, z0,A) будет подвержена погрешности датчика, а оценка g(ξˆ3, z3,A) – погрешности датчика
и накопленной погрешности одометрии в ξˆ3. Это расхождение,
как и инновацию фильтра Калмана, можно использовать для
корректировки вершин – положений и ориентиров, – чтобы
минимизировать общую погрешность. Однако имеющейся

336 Глава 6 · Определение местоположения и картографирование

информации недостаточно для точного определения места
ошибки, поэтому простая корректировка ξˆ3 вершины для приведения в соответствие с данными ориентира может увеличить погрешность в другой части графа. Чтобы минимизировать ошибку во всем графе, сформулируем это как задачу
минимизации, которая гарантирует, что ошибка, обнаруженная в одной части графа, будет оптимально распределена по
всему графу.
Рассмотрим сначала случай без ориентиров, когда все вершины представляют положение. Вектор состояния содержит
положения всех вершин
x = {ξ0, ξ1, ..., ξN-1},

(6.29)

и мы должны найти оценку x̂, которая минимизирует ошибку
по всем ребрам
(6.30)
где Fk(x) ∈ ℝ≥0 – стоимость, связанная с ребром k. Минимизируемое значение – это общая ошибка ребра, аналогичная потенциальной энергии в гибкой структуре, которую мы хотим
перевести в состояние с минимальной энергией.
На рис. 6.16 показаны две вершины графа положений – вершина i и вершина j, – представляющие предполагаемые положения ξˆi и ξˆj, и соединяющее их ребро k. У нас имеются два
значения относительного положения между вершинами. Одно
из них – явное, полученное на основе измерений датчика zi,j,
а другое, показанное пунктиром, – неявное и основанное на
текущих оценках положений в графе. Любое различие между
ними указывает на необходимость перемещения одной или
обеих вершин.
что предполагает граф положений

ребро k
что наблюдает робот

Рис. 6.16. Одно ребро графа положений, показывающее предполагаемые положения
робота ξˆi и ξˆj, относительное положение, основанное на измерении датчика zi,j, и неявное
относительное положение, показанное пунктирной линией

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

SLAM на основе графа положений 337

ний вершин относительно окружающего мира. Для ребра k относительное положение, основанное на текущих оценках в графе положений, равно
ξˆj = ⊝ξˆi ⊕ ξˆj.

i

Его можно записать в виде SE(2)-мат­ри­цы i Tj . Также можно
оценить относительную позицию, исходя из наблюдения, одометрии zk = zi,j = (δd, δθ), что приводит к

Разница между этими двумя относительными положениями
составляет Te = Tz-1 i Tj, что можно записать как
fk(x, z) = [Tz-1(zi,j)T -1(xi)T(xj)]xyθ ∈ ℝ3,

то есть как конфигурацию (xe, ye, θe) ∈ ℝ2 × S1, которая будет равна
нулю, если вершины наблюдения и относительного положения
совпадут. Получить скалярную стоимость из вектора ошибки e,
требуемую выражением (6.30), можно с помощью квадратичного выражения
Fk(x, zk) = fkT(x, zk)Ωk fk(x, zk),
На практике эта
мат­ри­ца является
диагональной и отражает уверенность
в направлениях x,
y и θ. Чем больше
Ω (в матричном
смысле), тем важнее
ребро в процедуре
оптимизации. Разные датчики имеют
разную точность,
и это необходимо
учитывать: информации от высококачественного датчика
следует придавать
больший вес, чем
информации от низкокачественного.

(6.31)

где Ωk ∈ ℝ3×3 – положительно определенная информационная мат­ри­ца, играющая роль весового члена. Несмотря на
то что уравнение (6.31) записано как функция всех положений x, на самом деле оно зависит только от элементов i и j
множества x и измерения zk. Уравнение (6.30) не имеет аналитического решения, но его можно решить численно при
наличии хорошей начальной оценки x̂. Подробнее эта разреженная нелинейная задача наименьших квадратов обсуждается в разделе F.2.4.
Ошибку ребра fk(x) можно линеаризовать (см. приложение E)
относительно текущего состояния x0 графа положений
fk¢(Δ) ≈ f0,k + Jk Δ,
где Δ – смещение относительно x0, f0,k = fk(x0) и

– мат­ри­ца Якоби, которая зависит только от положения ее двух
вершин ξi и ξj и потому в основном состоит из нулей

338 Глава 6 · Определение местоположения и картографирование

Jk = (0  Ai  Bj  0), где

Вычислить мат­ри­цу Якоби можно множеством способов, но
здесь мы продемонстрируем подход с использованием SymPy.
>>>
>>>
>>>
>>>
...
>>>

import sympy
xi, yi, ti, xj, yj, tj = sympy.symbols("xi yi ti xj yj tj")
xm, ym, tm = sympy.symbols("xm ym tm")
xi_e = SE2(xm, ym, tm).inv() * SE2(xi, yi, ti).inv() \
* SE2(xj, yj, tj);
fk = sympy.Matrix(sympy.simplify(xi_e.xyt()));

Якобиан, описывающий изменение функции fk относительно i, вычисляется как
>>> Ai = sympy.simplify(fk.jacobian([xi, yi, ti]))
Matrix([
[-cos(ti + tm), -sin(ti + tm), xi*sin(ti + tm) - xj*sin(ti + tm)
- yi* cos(ti + tm) + yj*cos(ti + tm)],
[sin(ti + tm), -cos(ti + tm), xi*cos(ti + tm) - xj*cos(ti + tm)
+ yi* sin(ti + tm) - yj*sin(ti + tm)],
[0, 0, -1]])
>>> Ai.shape
(3, 3)

Аналогично вычисляется Bj. SymPy может автоматически генерировать код на разных языках программирования, вычисляющий якобианы.
Этого вида задача с графом положений решается довольно
просто с помощью Toolbox. Для этого нужно загрузить из файла данных граф положений, аналогичный изображенному на
рис. 6.15 ►:
>>> pg = PoseGraph("data/pg1.g2o");
loaded g2o format file: 4 vertices, 4 edges

который возвращает объект PoseGraph, описывающий граф положений. Его можно визуализировать: ►
>>> pg.plot();

Оптимизация уменьшает ошибку в сети при анимировании
изменения положения вершин:
>>> pg.optimize(animate=True)
done in 4.60 msec. Total cost 316.88
done in 2.66 msec. Total cost 47.2186
...
done in 2.09 msec. Total cost 3.14139e-11

Этот простой текстовый формат файлов
используется популярным пакетом
оптимизации
графов положений
g2o, который можно
найти по адресу
https://openslamorg.github.io/g2o.
html.

Вершины имеют
ориентацию вдоль
оси z. Поверните
граф, чтобы убедиться в этом.

SLAM на основе графа положений 339

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

4

y

10

3

8
6
4
2

Этот простой
текстовый формат
1
2
0
файла используется
−7.5
−5.0
−2.5
0.0
2.5
5.0
7.5
10.0
популярным пакеx
том оптимизации
графов положений Рис. 6.17. Оптимизация графа положений, показывающая результаты последовательных
TORO, который мож- итераций (отображаются все более темным цветом)
но найти по адресу
https://openslamТеперь рассмотрим более масштабный пример, основанный
org.github.io/
на
данных реального робота :
toro.html.

>>> pg = PoseGraph("data/killian-small.toro");

Вершин много,
и построение графа loaded TORO/LAGO format file: 1941 vertices, 3995 edges
занимает несколько
секунд. и отобразим результат (см. рис. 6.18а).

125

100

100
75

50

y

y

50
0

25
0

−50

−25
−50

−100

а

−200

−150

−100
x

−50

0

б

−200

−150

−100

x

−50

0

Рис. 6.18. Граф положений с 1941 вершиной синего цвета и 3995 ребрами разных цветов, полученный из набора
данных MIT Killian Court: а) начальная конфигурация; б) конечная конфигурация после оптимизации

340 Глава 6 · Определение местоположения и картографирование

Обратите внимание на вес ребер в центре – увеличив масштаб, вы сможете рассмотреть их в деталях. Теперь оптимизируем граф положений:
>>> pg.optimize()
solving....done in 0.91 sec. Total cost 1.78135e+06
.
.
solving....done in 1.1 sec. Total cost 5.44567

Окончательная конфигурация показана на рис. 6.18б. Исходный граф положений имел серьезные неточности из-за накоп­
ленной ошибки одометрии, в результате чего два маршрута по
коридору изначально были очень плохо согласованы.
Граф положений также может включать ориентиры, как показано на рис. 6.19. Ориентиры описываются вектором координат pj ∈ ℝ2, а не положением, и поэтому отличаются от вершин,
рассмотренных выше. Поэтому давайте переопределим вектор
состояния следующим образом:
x = {ξ0, ξ1, ..., ξN-1 | p0, p1, ..., pM-1},

(6.32)

включив в него N положений робота и M местоположений ориентиров. Робот в местоположении i наблюдает ориентир j на
расстоянии и с пеленгом zi,j = (r #, β #), где ·# обозначает измеренное значение, и преобразуется в декартову форму в системе координат {i}:
pj# = (r # cos β #, r # sin β #) ∈ ℝ2.

i

По вершинам графа положений также можно определить предполагаемое местоположение ориентира в системе координат {i}
p̂ j = (⊝0ξˆi) · p̂ j ∈ ℝ2

i

и вектор ошибок

fk(x, zk) = ip̂ j - ipj# ∈ ℝ2.

что предполагает граф положений

ребро k
что наблюдает робот

Рис. 6.19. Одно ребро графа положений, показывающее предполагаемое положение ξˆi
и предполагаемое местоположение ориентира робота p̂ j. Относительное местоположение на
основе измерения датчика zi,j и неявное относительное положение, показанное пунктирной
линией

SLAM на основе графа положений 341

Стоимость ребра задается уравнением (6.31) и Ω ∈ ℝ2 × 2.
Мы следуем тому же подходу, что и раньше, однако на этот
раз мат­ри­ца Якоби определяется как

но точно так же в основном состоит из нулей
Jk = (0  Ai  Bj  0).
Два ее ненулевых блока имеют другую ширину:
,

Это похоже на оценивание в системе
координат карты
с помощью EKF,
как было показано
в разделе 6.4.

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

тем не менее решение можно получить тем же способом, что
и прежде (см. раздел F.2.4).
В результате оптимизации получается граф, имеющий оптимальные относительные положения, но абсолютные положения и местоположения могут быть неверными. Эту проблему
можно исправить, зафиксировав одну или несколько вершин
(положений или ориентиров) и не обновляя их во время оптимизации. Подробнее этот подход обсуждается в разделе F.2.4.
На практике система SLAM на основе графа положений состоит из двух асинхронных подсистем, как показано на рис. 6.20:
интерфейсной и внутренней, соединенных графом положений.
Интерфейсная часть добавляет новые вершины по мере движения робота
и ребра, определяющие ограничения между
вершинами. Например, при перемещении с одного места на
другое одометрия колес дает оценку расстояния и изменения
ориентации, что является ограничением. Кроме того, внешние
датчики робота могут следить за относительным местоположением ориентира, что также добавляет ограничение. Каждое
измерение добавляет ограничение в граф – ребро. Количество
ребер, входящих в вершину и исходящих из нее, не ограниче-

одометрия
лазерный
сканер
камера

интерфейсная
front end
часть

внутренний
механизм

сенсоры
граф положений

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

342 Глава 6 · Определение местоположения и картографирование

но. Внутренний механизм периодически выполняет оптимизацию графа положений и корректирует положения вершин так,
чтобы максимально соблюсти ограничения, то есть чтобы наблюдения с датчиков были максимально объяснимы. Поскольку граф всегда расширяется только в локальной области, можно
ограничиться оптимизацией лишь локального подмножества
графа и оптимизировать весь граф в редких случаях. Если пос­
ле оптимизации вершины оказываются эквивалентными, то их
можно объединить. Неправильная ассоциация с ориентиром
исказит весь граф и увеличит ∑k Fk(x̂). Одно из возможных решений этой проблемы – постепенное удаление ребер с самой
высокой стоимостью, пока граф не сможет перейти в состояние
с низкой энергией.

6.6

Последовательная локализация
методом Монте-Карло

В примерах, рассматривавшихся до сих пор, предполагалось,
что погрешность датчиков, таких как одометры, а также измерения расстояния и пеленга до ориентира, следует нормальному (гауссовому) закону распределения. На практике ошибка
датчика может следовать одностороннему распределению (например, распределению Пуассона) или многомодальному расОтступление 6.9. Метод Монте-Карло
Это класс вычислительных алгоритмов, основанных на многократной случайной
выборке для вычисления результатов. Одним из первых примеров применения
этой идеи может служить задача Бюффона об иголке, предложенная в XVIII веке
Жоржем-Луи Леклером (1707–1788), графом де Бюффоном: представьте пол, со­
бранный из параллельных деревянных планок одинаковой ширины t, и на пол пада­
ет иголка длиной l. Какова вероятность, что упавшая на пол иголка пересечет шов
между планками? Если было брошено n иголок и из них h пересекли швы между
планками, то можно показать, что вероятность равна h/n = 2l/πt. В 1901 году итальянский математик Марио Лаццарини провел эксперимент, подбросив иголку
3408 раз, и получил оценку π ≈ 355/13 (3.14159292).
Метод Монте-Карло часто используется при моделировании систем с большим
числом взаимосвязанных степеней свободы и значительной неопределенностью
входных данных и, как правило, применяется, когда невозможно получить точный результат с помощью детерминированного алгоритма. Зависимость от повторяющихся вычислений и случайных или псевдослучайных чисел делает этот
метод удобным для компьютерных вычислений. Он был разработан в Лос-Аламосе
в рамках Манхэттенского проекта во время Второй мировой войны математиками
Джоном фон Нейманом, Станиславом Уламом и Николасом Метрополисом. Название «Монте-Карло» отсылает к азартным играм и было кодовым названием
секретного проекта.

Последовательная локализация методом Монте-Карло 343

пределению с несколькими пиками. Функции, использованные
нами в фильтре Калмана, такие как (6.2) и (6.8), являются сильно
нелинейными, а это означает, что шум датчика с гауссовым распределением не приведет к гауссовому распределению ошибки
значения функции (подробнее этот вопрос обсуждается в разделе H.2). Функция плотности вероятности, связанная с конфигурацией робота, может иметь несколько пиков, отражающих несколько гипотез, которые одинаково хорошо объясняют
данные, получаемые от датчиков, как показано, например, на
рис. 6.3в.
В методе Монте-Карло, который обсуждается в этом разделе,
не делается никаких предположений о распределении ошибок.
Он также может обрабатывать несколько гипотез о состоянии
системы. Основная идея поразительно проста. Мы допускаем
множество различных значений конфигурации робота или вектора состояния и при появлении нового измерения оцениваем,
насколько хорошо каждое конкретное значение состояния объясняет наблюдение, сделанное датчиком. Мы сохраняем наиболее подходящие состояния и случайным образом выбираем
их из распределения прогноза, чтобы сформировать новое поколение состояний. В совокупности эти состояния и их оценки
образуют дискретное приближение функции распределения
состояния, которую мы пытаемся оценить. Никаких предположений о гауссовых распределениях не делается, и нет необходимости линеаризовать систему. Несмотря на довольно высокую
вычислительную сложность, этот метод вполне применим на
типичных современных компьютерах и хорошо распараллеливается. Если изобразить эти векторы в виде точек в пространстве состояний, то получится облако частиц, каждая из которых
представляет некоторое вероятное состояние робота. Поэтому
подобные способы оценки часто называют фильтрами частиц.
Мы применим оценку Монте-Карло к задаче локализации,
используя одометрию и карту. Оценка только трех состояний
x = (x, y, θ) вычислительно легко решается с помощью простого
кода на Python. Алгоритм оценки инициализируется созданием
N частиц xi, i = 0, ..., N - 1, случайно разбросанных в конфигурационном пространстве робота. Все частицы имеют одинаковый
начальный вес, важность или вероятность wi = 1/N. В каждой
итерации алгоритм выполняет следующие основные шаги:
1. Обновление состояния каждой частицы
xi+〈k + 1〉 = f(xi〈k〉, u〈k〉 + r〈k〉),

где û〈k〉 – входные данные или измеренная одометрия
û〈k〉 = δ〈k〉 и r〈k〉 ∈ ℝ2 – случайный вектор, представляющий
неопределенность в одометрии. Часто r определяется на
основе гауссовой случайной величины с ковариацией R,

344 Глава 6 · Определение местоположения и картографирование

но вообще можно использовать любое физически значимое распределение. Обновление состояния часто упрощается до
xi+〈k + 1〉 = f(xi 〈k〉, u〈k〉) + q〈k〉,

где q〈k〉 ∈ ℝ2 × S1 представляет неопределенность положения робота.
2. Выполнение наблюдения zj за ориентиром j, имеющим известные координаты pj. Для каждой частицы вычисляется
инновация
vi = h(xi+, pj) - zj,
определяющая ошибку между предсказанными и фактически наблюдаемыми координатами за ориентиром.
Функция правдоподобия представляет скалярную меру,
насколько хорошо конкретная частица объясняет текущее наблюдение. В этом примере мы будем использовать
функцию правдоподобия

и с ее помощью определять вес частицы, где L – ковариа­
ционная мат­ри­ца, а w0 > 0 гарантирует конечную вероятность удержания частицы, несмотря на ошибку датчика.
Мы используем квадратичную экспоненциальную функцию только для удобства – в общем случае функция не
обязательно должна быть гладкой или обратимой, она
должна лишь адекватно описывать вероятность наблюдения. ►
3. Выбор частиц, которые лучше других описывают наблюдение. Этот процесс известен как повторная выборка
(resampling), ► или выборка по важности. Типичная схема – случайный выбор частиц в соответствии с их весом.
Имея N частиц xi с весами wi, сначала выполняется нормализация их весов
(6.33)
и построение кумулятивной гистограммы
j
cj = ∑ i=0
wi¢.

(6.34)

Как показано на рис. 6.21, затем из равномерного распределения выбирается случайное число r ∈ [0, 1], вычисляется

Этот тип оценки
известен как фильтр
бутстрэп-типа. Вес
вычисляется на каждом шаге независимо от предыдущих
значений.
Существует множество стратегий
повторной выборки
для фильтров час­
тиц, основанных на
разных алгоритмах
повторной выборки
и выполнении
выборки с разной
частотой. Здесь мы
используем прос­
тейшую стратегию,
известную под
разными названия­
ми: «мультиномиальная повторная
выборка», «простая
случайная повторная выборка» или
«выборка с заменой, на каждом
временном шаге».
Иногда ее называют
«бутстрэп-фильтрацией частиц» или
«конденсацией».

Последовательная локализация методом Монте-Карло 345

Кумулятивные
веса частиц

Рис. 6.21. Случайная выборка.
Кумулятивная гистограмма (вверху)
формируется на основе весов
частиц (внизу). Для выбора частицы i
используется случайное число r.
Вероятность выбора частицы
с высоким весом, например 1 или 3,
явно выше вероятности выбора
частицы с низким весом, например
2 или 4

Веса частиц

и выбирается частица i для следующей итерации. Процесс
повторяется N раз. Конечным результатом является новый набор из N частиц, в который могут попасть несколько копий частиц с высоким весом, а некоторые частицы
с низким весом могут вообще не скопироваться. Шаг 1
следующей итерации распределит идентичные частицы
путем добавления случайного вектора r〈k〉. Повторная выборка – критически важный компонент фильтра частиц,
без которого фильтр быстро создаст вырожденный набор
частиц, где несколько частиц имеют высокий вес, а основная масса – практически нулевой, что плохо отражает истинное распределение.
Эти шаги показаны на рис. 6.22. Реализация с применением
Toolbox в целом аналогична предыдущим примерам. Создаем
карту
>>> map = LandmarkMap(20, workspace=10);

и робота с зашумленной одометрией и начальным состоянием
>>> V = np.diag([0.02, np.deg2rad(0.5)]) ** 2;
>>> robot = Bicycle(covar=V, animation="car", workspace=map);
>>> robot.control = RandomPath(workspace=map)

а затем датчик, генерирующий показания погрешностью
>>> W = np.diag([0.1, np.deg2rad(1)]) ** 2;
>>> sensor = RangeBearingSensor(robot, map, covar=W, plot=True);

346 Глава 6 · Определение местоположения и картографирование

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

Этап прогнозирования

прибавить случайное значение
для имитации неопределенности

новая информация – инновация –
из наблюдения за ориентиром j
вес – это функция вероятности измерения
нормализовать веса в диапазон от 0 до 1

Этап обновления

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

Рис. 6.22. Алгоритм фильтра частиц с этапами прогнозирования и обновления

Для фильтра частиц необходимо определить две ковариационные мат­ри­цы. Первая – ковариация случайного шума, добавляемого к состояниям частиц в каждой итерации для представления неопределенности конфигурации. Выберем значения
ковариации, сопоставимые со значениями W:
>>> Q = np.diag([0.1, 0.1, np.deg2rad(1)]) ** 2;

и ковариацию функции правдоподобия, применяемой к инновациям:
>>> L = np.diag([0.1, 0.1]);

Наконец, сконструируем экземпляр ParticleFilter
>>> pf = ParticleFilter(robot, sensor=sensor, R=R, L=L,
...
nparticles=1000);

Конфигурация состоит из 1000 частиц. Изначально частицы
равномерно распределены по трехмерному конфигурационному пространству.
Запускаем симуляцию на 10 секунд
>>> pf.run(T=10);

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

Последовательная локализация методом Монте-Карло 347
10.0

5.0

5.0

2.5

2.5

0.0

0.0

−2.5

−2.5

−5.0

−5.0

−7.5

−7.5

−10.0
−10.0

−7.5

−5.0

−2.5

0.0

2.5

x

5.0

7.5

ориентир
частица

7.5

y

y

7.5

а

10.0

landmark
ориентир
particle
частица

10.0

−10.0
−10.0

б

−7.5

10.0

−5.0

−2.5

0.0
x

2.5

5.0

7.5

10.0

ориентир
частица

7.5
5.0
2.5

y

0.0
−2.5
−5.0
−7.5
−10.0
−10.0

в

−7.5

−5.0

−2.5

0.0
x

2.5

5.0

7.5

10.0

Рис. 6.23. Результаты фильтрации частиц, показывающие эволюцию конфигурации
частиц на плоскости xy с течением времени: а) начальное распределение частиц
в момент времени t = 0; б) распределение частиц в момент времени t = 1;
в) распределение частиц в момент времени t = 3. Начальная конфигурация робота
показана серым пятиугольником
В этом подходе
Частицы аппроксимируют функцию плотности вероятности
статистики вы- конфигурации робота. Наиболее вероятная конфигурация – это
числяются по всем
частицам. Другие ожидаемое значение или среднее, значение всех частиц. Местратегии пред- рой неопределенности оценки является разброс облака частиц,
полагают оценку
или его стандартное отклонение. Объект ParticleFilter хранит
плотности ядра для
каждой частицы – историю изменения среднего значения и стандартного отклосумму весов всех нения состояния частицы на каждом временном шаге с учетом
соседей в пределах
фиксированного ра- ее веса. Как обычно, выведем результаты моделирования
диуса – и выбор
частиц с наиболь- >>> map.plot();
шим значением. >>> robot.plot_xy();

и наложим сверху среднее значение облака частиц
>>> pf.plot_xy();

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

348 Глава 6 · Определение местоположения и картографирование

сти построим график зависимости стандартного отклонения от
времени (рис. 6.24б).
>>> plt.plot(pf.get_std()[:100,:]);
10.0

ориентир

x
y
θ (×10)

6

истинная траектория

7.5

среднее фильтра частиц

5

стандартное отклонение

5.0

y

2.5
0.0
−2.5

4

3

2

−5.0
1
−7.5
0

а

−10.0
−10.0

−7.5

−5.0

−2.5

0.0
x

2.5

5.0

7.5

10.0

б

0

5

10

Временной шаг

15

20

Рис. 6.24. Результаты работы фильтра частиц: а) траектория движения робота; б) среднеквадратичное отклонение
частиц в зависимости от времени

На графике до t = 2 с наблюдается резкое уменьшение стандартного отклонения, обусловленное быстрым удалением час­
тиц, далеких от истинного решения. Как упоминалось в начале,
частицы представляют выборочное приближение к функции
распределения вероятностей, и это можно представить это как
>>> pf.plot_pdf()

Задача, которую мы только что решили, известна в робототехнике как задача похищенного робота: робот помещается
в мир без оценки его первоначального положения и нуждается
в максимально быстрой локализации. Чтобы представить эту
большую неопределенность, частицы равномерно распределяются по трехмерному конфигурационному пространству, и их
разреженность может привести к тому, что фильтр частиц будет долго сходиться, если только не используется очень большое количество частиц. Вопрос о реалистичности этой задачи –
спорный. Чаще на практике есть некоторая приблизительная
оценка положения робота, и частицы создаются в этой части
конфигурационного пространства. Например, если известно,
что робот находится в коридоре, то частицы будут размещены
в областях карты, которые соответствуют коридорам. Или если
известно, что робот направлен лицом на север, то все частицы
будут иметь эту ориентацию.
Настройка параметров фильтра частиц не требует большого
опыта, и лучший способ научиться этому – экспериментиро-

SLAM на основе теоремы Рао–Блэквелла 349

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

6.7

Такую, как фильтр
частиц, обсуждавшийся в разделе 6.6.

SLAM на основе теоремы Рао–Блэквелла

В этом разделе кратко и неформально рассказывается об основополагающем принципе SLAM на основе теоремы Рао–Блэквелла (Rao-Blackwellized SLAM), хорошо известным примером
реализации которого является FastSLAM. Мотивацией для разработки этого подхода стал размер ковариационной мат­ри­цы
для EKF SLAM, равный квадрату числа ориентиров, из-за чего
в крупномасштабных средах этот подход становится вычислительно невыполнимым.
Сравнив ковариационные мат­ри­цы на рис. 6.11в и 6.14б,
можно заметить разительный контраст. В обоих случаях создавалась карта неизвестных ориентиров, но на рис. 6.11в мат­ри­
ца в основном состоит из нулей и имеет блочно-диагональную
структуру, тогда как мат­ри­ца на рис. 6.14б не имеет нулевых
значений. Разница заключается в том, что в случае, соответствующем рис. 6.11в, мы предположили, что траектория робота точно известна, и это делает оценки ориентиров независимыми – наблюдение за одним ориентиром дает информацию
только о нем. Ориентиры некоррелированы, отсюда и все нули
в ковариационной мат­ри­це. Если траектория робота неизвестна, как в случае, которому соответствует рис. 6.14б, то оценки
ориентиров коррелированы – ошибка в местоположении одного ориентира привносит ошибки в положение робота и мес­
тоположения других ориентиров. Фильтр Калмана использует
информацию о корреляции, поэтому измерение любого ориентира дает информацию для уточнения оценки всех остальных
ориентиров и положения робота.
На практике мы не знаем истинного положения робота, но
где
представляем себе оценку с несколькими гипотезами,
каждая гипотеза представляет верную (по нашему мнению)
траекторию движения робота. Это означает, что ковариационная мат­ри­ца будет блочно-диагональной, как на рис. 6.11б.
Вместо фильтра с ковариационной мат­ри­цей 2N×2N мы можем
использовать N простых фильтров, каждый из которых неза­
висимо оценивает местоположение одного ориентира и имеет
ковариационную мат­ри­цу 2×2. Независимая оценка приводит

350 Глава 6 · Определение местоположения и картографирование

к значительной экономии памяти и вычислительных ресурсов.
Важно отметить, что такое возможно только потому, что мы решили считать верной вычисленную траекторию движения робота.
Каждая гипотеза также содержит оценку траектории движения робота на текущий момент. Гипотезы, наилучшим образом
объясняющие измерения ориентиров, сохраняются и распространяются, а остальные удаляются. При наличии M гипотез
общая вычислительная нагрузка снижается с O(N 2) (в подходе
EKF SLAM) до O(M log N ) и на практике хорошо подходит для
десятков и сотен значений M, но вполне может работать и для
таких низких значений, как M = 1.

6.8

Применение: лидар

Для реализации локализации робота и SLAM необходим датчик, способный измерять пеленг и расстояние до ориентиров.
Существует множество таких датчиков, основанных на различных физических принципах, включая ультразвуковую локацию
(рис. 6.25a), компьютерное зрение или радар. Одним из наиболее распространенных датчиков, используемых для навигации
роботов, является лидар (рис. 6.25б–г). Лидар излучает короткие импульсы инфракрасного лазерного излучения и измеряет время возврата отраженных импульсов. Дальность действия
может достигать 100 м и точности порядка сантиметров.
Типичные конфигурации лидаров показаны на рис. 6.26. Одномерный лидар (рис. 6.25б) измеряет расстояние r ∈ ℝ вдоль
луча до поверхности. Такие датчики также называют лазерными
дальномерами. Двухмерный лидар (рис. 6.25в) содержит одномерный лидар, поворачивающийся вокруг фиксированной оси.
Он возвращает плоскостное поперечное сечение в полярной
системе координат ℝ × S1 и излучает фиксированное количест­
во импульсов за один оборот. ► В мобильных робототехнических устройствах лидар обычно настраивается на сканирование
в плоскости, параллельной плоскости земли и немного выше
нее. Трехмерный лидар (рис. 6.25г) содержит несколько одномерных лидаров, формирующих веер лучей в вертикальной
плоскости и поворачивающихся вокруг фиксированной оси.
Он возвращает сферические координаты ℝ × S2 – трехмерные
координаты точки ℝ3, которые можно визуализировать в виде
облака точек, например как показано на рис. 6.27. Некоторые
лидары также измеряют силу отраженного сигнала (ремиссию),
которая является функцией инфракрасной отражательной способности поверхности.

Обычно через
каждую четверть,
половину или один
градус. Дальномеры могут иметь
ограниченный угол
поворота: 180° или
270°.

Применение: лидар 351

а

б

в

г

Рис. 6.25. Датчики-дальномеры для роботов: а) недорогой ультразвуковой дальномер с максимальной дальностью
6.5 м, выполняющий 20 измерений в секунду (LVMaxSonar-EZ1, изображение предоставлено SparkFun Electronics®);
б) недорогой времяпролетный одномерный лидар с максимальной дальностью 20 см, выполняющий 10 измерений
в секунду (VL6180, изображение предоставлено SparkFun Electronics); в) двухмерный лидар с максимальной дальностью
30 м, угловым диапазоном 270° и шагом 0.25°, выполняющий 40 измерений в секунду (UTM-30LX, изображение
предоставлено Hokuyo Automatic Co. Ltd.); г) трехмерный лидар с 16 лучами, полем зрения 30×360°, дальностью 100 м
и скоростью вращения 5–20 оборотов в секунду (VLP-16 Puck, изображение предоставлено Velodyne Lidar®)
Одномерный лидар

Двухмерный лидар

Трехмерный лидар

Рис. 6.26. Типичные
конфигурации лидаров,
красная стрелка представляет
лазерный луч

Рис. 6.27. Облако точек, созданное трехмерным лидаром Velodyne Alpha Prime, установленным на транспортном
средстве, которое изображено ближе всего (это модель, добавленная в рисунок, а не часть полученного
облака точек). Лидар имеет веер из 128 лучей, расположенных в вертикальной плоскости, которые описывают
концентрические окружности на поверхности земли, при этом углы лучей неравномерно распределены. Деревья
справа отбрасывают тени, и видны только их передние поверхности – иногда это называют «2.5-мерным»
представлением. Точки окрашены в зависимости от интенсивности (регрессии) отраженного лазерного импульса
(изображение предоставлено Velodyne Lidar)

352 Глава 6 · Определение местоположения и картографирование

Лидары имеют свои достоинства и недостатки по сравнению
с камерами и системами компьютерного зрения, которые будут обсуждаться в частях IV и V этой книги. Главное достоинство – лидары предоставляют метрические данные, то есть они
определяют фактическое расстояние до точек в метрах, и могут работать в темноте. Однако на открытом воздухе лидары
работают хуже, чем камеры, потому что отраженный лазерный
импульс перекрывается инфракрасным излучением солнца.
К другим недостаткам можно отнести: ограниченное время
сканирования, что порождает проблемы при движении датчика; невозможность различения мелкой текстуры или цвета; наличие движущихся частей; громоздкость, высокое энергопотребление и дороговизна.

6.8.1

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

Типичное применение лидаров – лидарная одометрия, помогающая оценивать изменение положения робота на основе
данных лидарного сканирования, а не данных с датчиков вращения колес. Проиллюстрируем это на данных лидарного сканирования реального робота.
>>> pg = PoseGraph("data/killian.g2o.zip", lidar=True);
loaded g2o format file: 3873 vertices, 4987 edges
3873 lidar scans: 180 beams, fov -90.0° to 90.0°, max range 50.0

Каждая вершина этого оптимизированного графа положений является результатом сканирования. Например, данные
о дальности и пеленге в вершине 100
>>> [r, theta] = pg.scan(100);
>>> r.shape
(180,)
>>> theta.shape
(180,)

представлены двумя векторами со 180 элементами в каждом.
Их можно отобразить в полярной системе координат:
>>> plt.clf()
>>> plt.polar(theta, r);

Мы будем использовать результаты лидарного сканирования
из двух соседних вершин:
>>>
>>>
>>>
(2,

p100 = pg.scanxy(100);
p101 = pg.scanxy(100);
p100.shape
180)

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

Применение: лидар 353

Это массивы, столбцы которых хранят декартовы координаОбратите внимание,
что точки, близкие ты точек, которые для наглядности изображены на рис. 6.28a.
к лидару, который
находится в точке Робот движется вправо, поэтому точки на стене справа оказы(0, 0) в системе ваются ближе во втором сканировании.
координат датчика,
Чтобы определить изменение положения робота между двусгруппированы гомя
сканированиями, необходимо сопоставить эти два набора
раздо более плотно.
Это характерно для точек. Это можно сделать с помощью алгоритма сопоставления
лидаров, когда точ- результатов сканирования
ки равномерно распределены по углу,
>>> T = pg.scanmatch(100, 101);
а не по площади.
>>> T.printline()
t = 0.506, -0.00872; 2.93°

который возвращает относительное положение 100T101 ∈ SE(2),
представляющее перемещение робота примерно на 0.5 м вдоль
оси x и небольшой поворот. 100T101 также можно использовать
для преобразования точек из системы координат {101} в систему {100}, и, как показано на рис. 6.28б, точки хорошо соответствуют друг другу. Вершины графа также содержат информацию об отметках времени. В частности, эти два скана были
получены с интервалом
>>> pg.time(101) - pg.time(100)
1.76

секунды, откуда нетрудно вычислить скорость движения робота, которая чуть меньше 0.3 м/с.

1

1

0

0
y

2

y

2

−1

−1

−2

−2
скан 100
скан 101

−3

а

0

2

4

x

6

8

скан 100
скан 101 (преобразованный)
10

−3

б

0

2

4

x

6

8

10

Рис. 6.28. Сопоставление результатов лазерного сканирования: а) лазерное сканирование из точек 100 и 101;
б) точки, полученные сканированием из точки 100, и точки, полученные сканированием из точки 101, после
применения преобразования

Это сопоставление результатов сканирования основано на
алгоритме итеративного поиска ближайших точек (iterated
closest point, ICP), который довольно подробно описывается
в разделе 14.7.2. В каждой итерации ICP связывает каждую точку во втором наборе с ближайшей точкой в первом наборе, а за-

354 Глава 6 · Определение местоположения и картографирование

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

6.8.2

Картографирование на основе лидара

Если положение робота достаточно хорошо известно, то, используя определенный процесс локализации, можно преобразовать все лидарные сканы в глобальную систему координат
и построить карту. Карты могут быть самыми разными, но здесь
мы расскажем, как построить сетку занятости, подобную той,
что описывалась в главе 5.
Для робота в заданном положении каждый луч в сканировании – это направленный луч, сообщающий нам несколько сведений. Во-первых, дальность – расстояние вдоль луча до ближайшего препятствия, что позволяет определить координаты
ячейки с препятствием. Однако мы ничего не можем сказать
о ячейках, расположенных дальше вдоль луча, потому что лидар «не видит», что находится за препятствием. Во-вторых, совершенно очевидно, что все ячейки между датчиком и первым
препятствием свободны.
При перемещении робота каждая ячейка пересекается мно­
жеством лучей, и мы можем использовать простую схему голосования для определения занятости ячейки. Основываясь на положении робота и луче, мы вычисляем уравнение луча и по очереди

Применение: лидар 355

посещаем каждую ячейку, используя алгоритм рисования линий
Брезенхэма. Голоса всех ячеек, находящихся вдоль луча ближе
к датчику, чем расстояние, полученное в результате измерения,
уменьшаются (они объявляются менее занятыми). Голос ячейки,
находящейся на луче на измеренном расстоянии, увеличивается (она объявляется более занятой), а следующие за ней ячейки
вдоль луча отмечаются как непосещавшиеся. Если датчик не получает отраженного импульса, то это может означать, что препятствие не отражает свет или находится слишком далеко. В таком
случае устанавливается диапазон на некоторое максимальное
значение, в данном случае 50 м, и луч пропускается.
>>> og = OccupancyGrid(workspace=[-100, 250, -100, 250],
cellsize=0.1, value=np.int32(0));
>>> pg.scanmap(og, maxrange=40)
>>> og.plot(cmap="gray")

Результаты показаны на рис. 6.29.

6.8.3

Локализация на основе лидара

Мы много раз говорили об ориентирах в этой главе, но избегали
конкретных примеров. Это могут быть отличительные визуальные особенности (см. раздел 12.3) или искусственные маркеры
(см. раздел 13.6.1). Если рассмотреть результаты лидарного
сканирования, как, например, на рис. 6.28a или 6.29б, то можно
увидеть довольно характерное расположение точек – геометрическую сигнатуру, – которую можно использовать в качестве
70.0

250

67.5

200

65.0

150

62.5

y

y

100
60.0

50
57.5
0

55.0

−50

−100
−100

52.5

−50

0

50

100

150

200

250

50.0
25.0

27.5

30.0

32.5

35.0

37.5

40.0

42.5

45.0

x
x
а
б
Рис. 6.29. а) Результаты лазерного сканирования, преобразованные в сетку занятости. Увеличенное изображение
области, заключенной в квадрат в центре, показано справа; б) белые ячейки – свободное пространство, черные –
занятые, а серые – неизвестные. Размер ячейки сетки 10×10 см

356 Глава 6 · Определение местоположения и картографирование

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

6.9

Подведение итогов

В этой главе мы рассмотрели два разных способа определения
местоположения робота: счисление пути и наблюдение за ориентирами, истинное положение которых известно. Счисление
пути основано на интеграции одометрической информации,
пройденного расстояния и изменения угла курса. Для этого метода характерно накопление ошибок со временем и увеличение неопределенности положения робота.
Мы смоделировали погрешность одометрии, добавив шум
в выходные сигналы датчика. Значения шума берутся из некоторого распределения, описывающего погрешность конкретного датчика. Для моделирования мы использовали гауссовский шум с нулевым средним и заданной ковариацией, но
так было сделано только из-за отсутствия другой информации
о конкретном датчике. На практике желательно использовать
наиболее реалистичную из доступных модель шума. Затем мы
представили фильтр Калмана, который помогает получить оптимальную оценку истинной конфигурации робота (в смысле
наименьших квадратов) на основе зашумленных измерений.
Однако фильтр Калмана дает оптимальные результаты, только если шум имеет гауссовское распределение с нулевым средним и модель изменения конфигурации робота линейна. Со
временем изменение конфигурации может стать нелинейным,
и в этом случае его можно аппроксимировать линейной моделью, включающей некоторые частные производные, выраженные в виде матриц Якоби. Этот подход известен как расширенная фильтрация Калмана.
Фильтр Калмана также оценивает неопределенность, связанную с оценкой положения объекта, и мы видим, что она никогда не уменьшается и, как правило, растет неограниченно. Лишь

Подведение итогов 357

наличие дополнительных источников информации может
уменьшить этот рост, и мы рассмотрели, как в роли таких источников информации использовать наблюдения за ориентирами
с известными местоположениями. Мы снова использовали
фильтр Калмана, но на этот раз разбили процесс на два этапа: этап прогнозирования и этап обновления фильтра. В этом
случае, как мы видели, неопределенность может уменьшаться
наблюдением за ориентиром и в долгосрочной перспективе не
растет. Потом мы применили фильтр Калмана к задаче оценки
местоположения ориентиров при известном точном местоположении робота. В данном случае вектор состояния фильтра
включал координаты ориентиров.
Далее мы собрали все это вместе и оценили конфигурацию
робота, местоположения ориентиров и их неопределенности –
одновременную локализацию и картографирование (simultaneous localization and mapping, SLAM). Вектор состояния в этом
случае содержал конфигурацию робота и координаты ориентиров.
Важной задачей при использовании ориентиров является
ассоциация данных – идентификация ориентиров, зафиксированных датчиком, чтобы их местоположения можно было найти на карте известных или предполагаемых ориентиров. При
неверной идентификации ориентира местоположение робота
будет определено с ошибкой, что может повлечь ошибки ассоциации данных в будущем и катастрофическое расхождение
между предполагаемым и истинным состоянием.
Фильтр Калмана плохо масштабируется с увеличением числа ориентиров, поэтому было предложено несколько альтернативных подходов. Во-первых, SLAM на основе графа положений
решает большую, но разреженную нелинейную задачу наименьших квадратов, превращая задачу фильтрации Калмана
в задачу оптимизации.
Во-вторых, мы рассмотрели метод Монте-Карло и представили фильтр частиц. Этот метод требует значительных вычислительных затрат, но не требует делать предположения о распределении ошибок датчика или линейности модели движения
робота и поддерживает множество гипотез. Фильтры частиц
можно рассматривать как приближенное решение истинной
модели системы, тогда как фильтр Калмана обеспечивает точное решение приближенной модели системы. SLAM на основе
теоремы Рао–Блэквелла сочетает в себе методы Монте-Карло
и фильтра Калмана.
Мы завершили обсуждение лидаров, которые очень широко
используются в робототехнике, и показали, как их можно применять для навигации роботов, одометрии и создания подробных карт этажей.

358 Глава 6 · Определение местоположения и картографирование

6.9.1

Дополнительное чтение

Локализация и SLAM
Руководства Bailey and Durrant-Whyte (2006) и Durrant-Whyte
and Bailey (2006) послужат хорошим введением в эту тему,
а книга «Probabilistic Robotics»1 (Thrun et al., 2005) достаточно
полно излагает все то, что мы рассмотрели в данной главе. Книга Barfoot (2017) охватывает все концепции этой главы и даже
больше, хотя и использует несколько отличающуюся терминологию. В книге Siegwart et al. (2011) тоже очень подробно исследуется тема локализации роботов.
Фильтры частиц описаны в работах Thrun et al. (2005), Stachniss and Burgard (2014) и в учебном руководстве Rekleitis (2004).
Существует множество вариантов фильтров, например с фиксированным или адаптивным количеством частиц, а также использующие разные способы повторной выборки – в Li et al.
(2015) предлагается всесторонний обзор стратегий повторной
выборки. Определение наиболее вероятного положения было
продемонстрировано с помощью вычисления средневзвешенного значения частиц, но использовались и многие другие
подходы. Метод на основе плотности ядра выбирает частицу
с наибольшим весом среди соседних частиц в окружающей гиперсфере фиксированного размера.
Алгоритм оптимизации графа положений, также известный
как GraphSLAM, имеет долгую историю, начатую Lu and Milios
(1997). В последнее время наблюдается значительный прогресс благодаря многочисленным публикациям (Grisetti et al.,
2010) и инструментам с открытым исходным кодом, включая
g2o (Kümmerle et al., 2011), SAM (Dellaert and Kaess, 2006), iSAM
(Kaess et al., 2007) и факторные графы, ведущие к GTSAM https://
gtsam.org/tutorials/intro.html. В Agarwal et al (2014) представлено краткое введение в SLAM на основе графов положений и обсуждается связь с наземной геодезической съемкой, которая
существует уже несколько столетий.
Реализации SLAM на основе теоремы Рао–Блэквелла включают FastSLAM (Montemerlo et al. 2003; Montemerlo и Thrun 2007),
а также gmapping (https://openslamorg.github.io/gmapping.html).
К числу популярных реализаций можно отнести Hector
SLAM (https://wiki.ros.org/hector_slam) и Cartographer (https://
github.com/cartographer-project/cartographer). Обе они, наряду
с gmapping, доступны как часть экосистемы ROS.
Существует множество онлайн-ресурсов, связанных с SLAM.
Коллекция реализаций SLAM с открытым исходным кодом до1

Существует перевод этой книги на русский язык, выполненный доб­
ровольцами: https://github.com/literator1996/veroyatnostnaya_robototehnica. – Прим. перев.

Подведение итогов 359

ступна на OpenSLAM по адресу http://www.openslam.org. Реализация сглаживания и картирования с использованием графов
факторов доступна на сайте https://gtsam.org и имеет библиотеки для C++, Python и MATLAB. Реализации алгоритмов SLAM
в MATLAB можно найти на сайте https://www.iri.upc.edu/people/
jsola/JoanSola/eng/toolbox.html и https://www-personal.acfr.
usyd.edu.au/tbailey. В интернете доступно множество отличных
обучающих ресурсов, включая бесплатную электронную книгу
Пола Ньюмана «C4B Mobile Robots and Estimation Resources»,
доступную по адресу https://www.free-ebooks.net/ebook/C4BMobile-Robotics.
V-SLAM (visual SLAM) способен оценивать перемещение камеры в трех измерениях по изображениям, используя естест­вен­ные
характеристики (см. главу 12), без использования искусственных ориентиров. VI-SLAM – это система V-SLAM, которая дополнительно использует информацию с инерциальных датчиков,
таких как гироскопы и акселерометры. VIO-SLAM – это система
VI-SLAM, которая дополнительно использует одометрическую
информацию. Одной из первых систем V-SLAM была система
PTAM (parallel tracking and mapping – параллельное отслеживание и картографирование), описанная в Klein and Murray (2007).
PTAM имеет два параллельных вычислительных потока. Один
из них строит карту, добавляет ориентиры в граф положений
на основе предполагаемого положения камеры (робота) и выполняет оптимизацию графа. Другой осуществляет локализацию, сопоставляя наблюдаемые ориентиры с предполагаемой
картой и оценивая положения камеры. ORB-SLAM – мощное
и зрелое семейство реализаций V-SLAM. Текущая версия ORBSLAM3 (Campos 2021) поддерживает V- и VI-SLAM с монокулярными, стерео- и RGB-D камерами с обычными и широкоугольными объективами. PRO-SLAM (https://gitlab.com/srrg-­software/
srrg_proslam) – простая стереосистема V-SLAM, предназначенная для обучения.

Сопоставление результатов сканирования и создание карт
Существует множество версий и вариантов алгоритма ICP, который более подробно обсуждается в разделе 14.9.1. Улучшить
сходимость и точность можно с помощью преобразования нормального распределения (normal distribution transform, NDT),
первоначально предложенного для двухмерных данных в Biber
and Straßer (2003). Реализация версии, расширенной до трех измерений и предложенная в Magnusson et al. (2007), доступна на
pointclouds.org. Сравнение ICP и NDT для полевых роботизированных приложений описано в Magnusson et al. (2009). Быст­
рый и популярный подход к сопоставлению данных лидарного
сканирования предложен в Censi (2008), и он составляет осно-

360 Глава 6 · Определение местоположения и картографирование

ву канонической реализации в ROS. Дополнительные ресурсы
доступны по адресу https://censi.science/research/robot-perception/plicp.
При попытке сопоставить локальную геометрическую сигнатуру в большом облаке точек (двух- или трехмерных) мы часто
стремимся ограничить поиск некоторой пространственной областью. Одно из эффективных решений – организовать данные
с помощью kdtree, входящего в пакет scipy.spatial. FLANN (Muja
и Lowe 2009) – это быстрая аппроксимация с возможностью использования из Python и доступная по адресу https://github.
com/flann-lib/flann.
Для создания карты на основе данных сканирования роботизированным лидаром (раздел 7.6.8) мы использовали упрощенный подход. Более сложное решение основывается на модели
луча или поля правдоподобия, описанной в работе Thrun et al.
(2005).

Фильтрация Калмана
Существует множество печатных и электронных ресурсов по
фильтрации Калмана. Оригинальная статья Kálmán (1960), написанная более 50 лет назад, вполне актуальна. Первое применение фильтра в аэрокосмической навигации в 1960-х годах
описано в McGee and Schmidt (1985). Книга Zarchan and Musoff
(2005) – очень понятное и доступное введение в фильтрацию
Калмана. А недавно переизданную классическую книгу Jazwinski (2007) я всегда считал очень доступной для широкого круга. В Bar-Shalom et al. (2001) дается исчерпывающее описание
тео­рии оценивания и использования GPS. Фильтрация Калмана
также рассматривается в Groves (2013). По адресу https://www.
cs.unc.edu/~welch/kalman можно найти ссылки на статьи, курсы, программное обеспечение и другие соответствующие вебсайты.
Существенным ограничением фильтра EKF является его линеаризация первого порядка, особенно для процессов с сильной
нелинейностью. Среди альтернатив можно назвать итерационный фильтр EKF, описанный в Jazwinski (2007), или сигма-точечный фильтр Калмана (Unscented Kalman Filter, UKF) (Julier and
Uhlmann 2004), который использует дискретные точки выборки
(сигма-точки) для аппроксимации функции плотности вероятности. Некоторые из этих тем рассматриваются в книге «Robo­
tics Handbook» Siciliano and Khatib (2016, § 5 и § 35). Информационный фильтр – это эквивалентный фильтр, поддерживающий
обратную ковариационную мат­ри­цу с некоторыми полезными
свойствами, который обсуждается в Thrun et al. (2005) как разреженный расширенный информационный фильтр.
Вставка якобиана (6.22) играет важную роль, но подробную
формулировку этого решения трудно найти, а вывод приво-

Подведение итогов 361

дится по адресу https://petercorke.com/robotics/ekfcovariancematrix-update-for-a-new-landmark.

Ассоциация данных
Методы SLAM критически зависят от точности ассоциации данных между наблюдениями и ориентирами на карте – идентификации ориентиров. Обзор методов ассоциации данных представлен в Neira and Tardós (2001). FastSLAM (Montemerlo и Thrun,
2007) позволяет оценивать ассоциацию данных, а также местоположения ориентиров. В качестве искусственных ориентиров,
устраняющих проблему ассоциации данных, могут использоваться реперные знаки, такие как AprilTags и ArUco, обсуждаемые в разделе 13.6.1, поскольку их идентификация закодирована
в самом реперном знаке. Мобильные роботы могут однозначно
идентифицировать места по их внешнему виду, используя такие
инструменты, как OpenFABMAP (Glover et al., 2012).
Ассоциация данных для фильтрации Калмана описана в «Robotics Handbook» (Siciliano and Khatib, 2016). Ассоциация данных в контексте отслеживания подробно описана в уже очень
старой книге Bar-Shalom and Fortmann (1988).

Датчики
В Kelly (2013) подробно рассматриваются датчики, в частности
лидарные дальномеры. Для воздушных и подводных роботов
нет возможности определять одометрию по движению колес,
поэтому в них используются альтернативные решения (пригодные также для колесных роботов), такие как визуальная
одометрия (visual odometry, VO). Метод визуальной одометрии
представлен в руководствах Fraundorfer and Scaramuzza (2012)
и Scaramuzza and Fraundorfer (2011) и будет рассмотрен в главе 14. В книге «Robotics Handbook» (Siciliano and Khatib, 2016)
подробно описан широкий спектр датчиков для использования в роботах. Принцип работы GPS и других радиолокационных систем довольно подробно описан в книге Groves (2013),
а на сайте этой книги представлен ряд ссылок на технические
данные по GPS. Задачу SLAM можно сформулировать в терминах измерений только пеленга или только дальности. Камеру
тоже можно считать датчиком, определяющим направление на
объект. Система VSLAM выполняет SLAM, используя лишь визуальную информацию о направлении, то есть только камеру.
Введение в эту тему дано в Neira et al. (2008) и дополнительном
специальном издании. Интересно, что задача VSLAM для роботов подобна задаче регулировки пучка, известной специалис­
там по компьютерному зрению и обсуждаемой в главе 14.
Книга Borenstein et al. (1996), хотя и сильно устарела, содержит превосходное обсуждение датчиков в целом и одометрии

362 Глава 6 · Определение местоположения и картографирование

в частности. Она уже не переиздается, но ее можно найти в интернете. Книга Everett (1995) посвящена одометрии, датчикам
дальности и пеленга, а также радио-, ультразвуковым и оптическим системам локализации. К сожалению, обсуждение датчиков определения расстояния и пеленга в настоящее время устарело, поскольку эта технология быстро развивалась в течение
последнего десятилетия.
Bray (2014) предлагает обзор истории методов определения
местоположения на планете. Если вы когда-нибудь задумывались, как ориентироваться по звездам или использовать секстант, то в этом вам поможет книга Blewitt (2011), предлагающая простое для понимания введение.
Книга «Longitude» (Sobel 1996) очень доступно излагает проб­
лемы определения долготы и попыток Джона Харрисона создать морской хронометр.

6.9.2

Упражнения

1. Каков размер премии «Longitude Prize» в современном выражении?
2. Реализуйте объект драйвера (раздел 6.1.1), который управляет роботом в пределах круга с указанным центром и ра­
диусом.
3. Выведите уравнение изменения направления в терминах
скорости вращения левого и правого колес для моделей, подобных автомобилю, и робота с дифференциальным управлением.
4. Счисление траектории (раздел 6.1).
а) Поэкспериментируйте с различными значениями P0, V
и V̂.
б) На рис. 6.5 сравниваются фактическое и расчетное мес­
тоположения. Постройте графики фактического и расчетного курсового углов.
в) Сравните дисперсию направления с дисперсией местоположения. Как они изменяются с увеличением дисперсии дальности и угла пеленга в датчике?
г) Выведите якобианы в (6.6) и (6.7) для робота с дифференциальным управлением.
5. Использование карты (раздел 6.2).
а) Попробуйте изменять характеристики датчика (ковариацию, частоту дискретизации, пределы измеряемого расстояния и пределы угла пеленга) и посмотрите, как они
влияют на производительность.
б) Попробуйте изменять W и Ŵ и посмотрите, что происходит с погрешностью оценки и окончательной ковариацией.

Подведение итогов 363

в) Измените RangeBearingSensor так, чтобы он измерял только
пеленг, то есть возвращал только угол, но не дальность.
Реализация должна включать все якобианы. Проверьте ее
производительность.
г) Измените модель датчика так, чтобы она возвращала случайные ошибки (и позволяла задавать их частоту), такие
как ошибка определения дальности или идентификации
ориентира. Что случится в этой ситуации?
д) Измените EKF так, чтобы реализация сама выполняла ассоциацию данных, а не пользовалась идентификатором
ориентира, который возвращается датчиком.
е) На рис. 6.9 сравниваются фактическое и расчетное местоположения. Постройте график фактического и расчетного курсового угла.
ж) Сравните дисперсию направления с дисперсией местоположения. Как они изменяются с увеличением дисперсии дальности и угла пеленга в датчике?
6. Создание карты (раздел 6.3).
а) Попробуйте изменять характеристики датчика (ковариацию, частоту дискретизации, пределы измеряемого расстояния и пределы угла пеленга) и посмотрите, как они
влияют на производительность.
б) Используйте датчик, возвращающий только пеленг, как
описано выше, и сравните его использование по сравнению с датчиком, возвращающим и расстояние, и пеленг.
в) Измените EKF так, чтобы реализация сама выполняла ассоциацию данных, а не пользовалась идентификатором
ориентира, который возвращается датчиком.
7. Одновременная локализация и картографирование (раздел 6.4).
а) Попробуйте изменять характеристики датчика (ковариацию, частоту дискретизации, пределы измеряемого расстояния и пределы угла пеленга) и посмотрите, как они
влияют на производительность.
б) Используйте датчик, возвращающий только пеленг, как
описано выше, и сравните его использование по сравнению с датчиком, возвращающим и расстояние, и пеленг.
в) Измените EKF так, чтобы реализация сама выполняла ассоциацию данных, а не пользовалась идентификатором
ориентира, который возвращается датчиком.
г) На рис. 6.13 сравниваются фактическое и расчетное мес­
тоположения. Постройте график фактического и расчетного курсовых углов.
д) Сравните дисперсию направления с дисперсией местоположения. Как они изменяются с увеличением дисперсии дальности и угла пеленга в датчике?

364 Глава 6 · Определение местоположения и картографирование

8. Измените оптимизатор на основе графа положений и протестируйте его с помощью простого графа pg1.g2o.
a) Закрепите одну вершину в определенном положении.
б) Добавьте один или несколько ориентиров. Сначала выведите соответствующие якобианы, а затем добавьте
в файл данных координаты ориентиров, ограничения
и информационную мат­ри­цу.
9. Создайте симулятор для задачи Бюффона об игле и оцените величину π, выполнив 10, 100, 1000 и 10 000 бросков иглы.
Как меняется сходимость с длиной иглы?
10. Фильтр частиц (раздел 6.6).
а) Запустите фильтр несколько раз. Всегда ли он сходится?
б) Измените параметры Q, L, w0 и N и посмотрите, как они
влияют на скорость сходимости и конечное стандартное
отклонение.
в) Исследуйте различные варианты задачи о похищенном
роботе. Разместите начальные частицы вокруг начального положения. Разместите частицы равномерно по
плоскости xy, но задайте их ориентацию в соответствии
с фактическим значением.
г) Используйте другой тип функции правдоподобия, например обратное расстояние, и сравните результаты.
11. Поэкспериментируйте с маркерами ArUCo или AprilTags.
Напечатайте несколько маркеров и извлеките их из изобра­
жений, используя функции Toolbox, описанные в разделе 13.6.1.
12. Реализуйте лидарный одометр и протестируйте его на примере траектории в killian.g2o. Сравните показания одомет­
ра с относительными изменениями положений в файле.
13. Какая точность синхронизации необходима для измерения
расстояния с помощью лидара, чтобы достичь разрешения
1 см по глубине?
14. Переформулируйте задачи локализации, картографирования и SLAM, используя только датчик, определяющий ориентиры.
15. Реализуйте систему локализации или SLAM с использованием внешнего симулятора, например CoppeliaSim. Получайте замеры дальности от моделируемого робота, проводите лидарную одометрию и распознавание ориентиров,
а также отправляйте роботу команды на выполнение движений. Взаимодействовать с CoppeliaSim можно через Python API. Также можно использовать симуляторы Gazebo
и общаться с ними по протоколу ROS.

Часть III Роботы-манипуляторы
Глава 7
Глава 8
Глава 9

Кинематика робота-манипулятора
Скорость манипулятора
Динамика и управление

Роботы-манипуляторы – распространенный и хорошо знакомый тип роботов.
Чаще всего они используются на заводах для выполнения таких работ, как сборка, сварка и погрузка-разгрузка. В числе других областей применения можно
назвать сбор товаров с полок в интернет-магазинах, упаковка фруктов в лотки, проведение хирургических операций под наблюдением человека и даже
захват грузов на международной космической станции. Первые такие роботы
начали эксплуатироваться более 60 лет назад и с тех пор продолжали неуклонно развиваться: в настоящее время по всему миру работают миллионы таких
роботов, собирая, упаковывая и перемещая множество товаров, которые мы
покупаем. В отличие от мобильных роботов, о которых рассказывалось в предыдущей части, роботы-манипуляторы не перемещаются по поверхности. Они
закреплены на статическом основании и, следовательно, действуют в ограниченном рабочем пространстве.
В главах этой части рассматриваются основы многозвенных манипуляторов
с последовательным соединением, подмножества всех роботов-манипуляторов. В главе 7 дается введение в тему и рассматривается кинематика – геометрическая связь между углами или местоположениями сочленений робота
и положением его рабочего органа. Мы обсудим создание плавных траекторий,
по которым может следовать робот, и рассмотрим пример робота, рисующего
букву на плоскости, а также четырехногого шагающего робота. В главе 8 будет
представлена взаимосвязь между скоростью изменения координат сочленения
и скоростью движения рабочего органа, которая описывается мат­ри­цей Якоби
манипулятора. Мы также рассмотрим альтернативные методы генерирования
траекторий в трехмерном пространстве и введем взаимосвязь между силами,
действующими на конечный эффектор, и моментами, или силами, в сочленениях. В главе 9 обсуждается независимое управление сочленениями и рассмат­
риваются некоторые факторы, ограничивающие производительность, такие
как вес и переменная инерция. Опираясь на это обсуждение, мы перейдем
к исследованию полной нелинейной динамики многозвенных манипуляторов,
включая инерцию, гироскопические силы, трение и гравитацию, а также более
сложных подходов к управлению на основе моделей и пространства задач.

Глава

7

	Кинематика
робота-манипулятора
« Займись кинематикой. Это благодатная сфера.

Она более благодатна, чем геометрия, ибо добавляет к про­
странству четвертое измерение.
– Чебышев Сильвестру, 1873

chap7.ipynb

https://go.sn.
pub/6RxahB

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

а

б

Рис. 7.1. Рабочие органы роботов: а) вакуумный захват удерживает лист стекла;
б) человекоподобная роботизированная рука (© Shadow Robot Company 2008)

368 Глава 7 · Кинематика робота-манипулятора

Существует множество различных типов роботов-манипуляторов. Некоторые из них показаны на рис. 7.2. Наиболее распространенным является робот-манипулятор с шестью степенями
свободы (degree of freedom, DoF) , рука которого состоит из
жестких звеньев и шести приводных вращательных сочленений. Робот-манипулятор SCARA (Selective Compliance Assembly
Robot Arm – селективно податливая рука для роботизированной
сборки) – жесткий в вертикальном направлении и податливый
в горизонтальной плоскости – хорошо подходит для выполнения работ в плоскости, таких как сборка электронных плат. Портальный робот две степени свободы движения по подвесным
рельсам и имеет очень большое рабочее пространство. В этих
роботах сочленения соединены последовательно, то есть это
многозвенные манипуляторы с последовательным соединением звеньев, и каждое сочленение должно поддерживать и перемещать все другие сочленения, расположенные между ним
и рабочим органом. В манипуляторе с параллельными звенья-

а

в

б

г

Рис. 7.2. а) Универсальный промышленный манипулятор с 6 степенями свободы (источник:
ABB); б) робот SCARA с 4 степенями свободы, обычно используемый для сборки электроники
(изображение робота Adept Cobra s600 SCARA предоставлено Adept Technology, Inc.);
в) портальный робот; рука движется по подвесному рельсу (изображение предоставлено
Güdel AG Switzerland, Марио Ротенбюлер, https://www.gudel.com); г) манипулятор
с параллельными звеньями, рабочий орган которого приводится в движение шестью
параллельными звеньями (источник: ABB)

Степени свободы
более подробно
рассматриваются
в разделе 2.4.9.

Прямая кинематика 369

Кинематика произошла от греческого
слова κίνημα –
«движение».

ми, в отличие от манипулятора с последовательными звеньями,
все двигатели расположены в основании и соединены механическими связями непосредственно с рабочим органом. Малая
подвижная масса в сочетании с жесткостью, присущей параллельнозвенным структурам, позволяет роботам этого класса
роботов развивать очень высокое ускорение.
В этой главе рассматривается кинематика – раздел механики, изучающий движение тела или системы тел без учета массы или силы. Положение рабочего органа робота зависит от
состояния каждого из его сочленений, что приводит к двум
важным кинематически проблемам. Прямая кинематика, рассматриваемая в разделе 7.1, определяет положение рабочего
органа по конфигурации сочленения. Обратная кинематика, рассматриваемая в разделе 7.2, определяет конфигурацию
сочленения с учетом положения рабочего органа. В разделе 7.3
описываются методы построения плавных траекторий движения рабочего органа. В разделе 7.4 исследуются два сложных
приложения: письмо на плоской поверхности и четырехногий
шагающий робот, ноги которого представляют собой простые
роботизированные руки. В заключение главы в разделе 7.5 обсуждаются некоторые дополнительные темы.

7.1

Прямая кинематика

Роботы, показанные на рис. 7.2а–в, – это манипуляторы, состоящие из последовательности жестких звеньев, соединенных приводными сочленениями. Каждое сочленение управляет относительным положением двух соединяемых им звеньев,
а положение конечного рабочего органа определяется всеми
сочленениями. Последовательность звеньев и сочленений представляет собой кинематическую цепочку или цепочку жестких
тел. Большинство промышленных роботов, подобных тем, что
изображены на рис. 7.2a, имеют шесть сочленений, но все чаще
встречаются роботы с семью сочленениями, что обеспечивает
большую подвижность и большее рабочее пространство, но об
этом мы поговорим в главе 8. Робот YuMi® (рис. 7.3а) имеет две
кинематические цепочки, каждая из которых состоит из семи
сочленений. Робот Atlas™ (рис. 7.3б) имеет четыре цепочки: две
из них – руки, а две – ноги, при этом рабочими органами являются кисти и ступни соответственно.
В робототехнике наиболее распространены поворотные со­
членения, когда одно звено поворачивается относительно другого вокруг оси, закрепленной на них обоих. Реже встречается
призматическое сочленение (его еще называют скользящим,

370 Глава 7 · Кинематика робота-манипулятора

а

б

Рис. 7.3. а) ABB Yumi® имеет две кинематические цепочки и 14 сочленений (источник: ABB);
б) гуманоидный робот Boston Dynamics Atlas™ имеет четыре кинематические цепочки
и 28 сочленений (источник: Boston Dynamics)

или телескопическим), когда одно звено скользит относительно
другого вдоль оси, закрепленной на обоих. Роботы на рис. 7.2б,в
имеют одно и три призматических сочленения соответственно. Возможны и другие типы сочленений, например винтовые
(с одной степенью свободы), цилиндрические (с двумя степенями свободы – одна поворотная и одна призматическая) или
сферические сочленения (с тремя степенями свободы), но они
редко встречаются в робототехнике и не будут здесь обсуж­
даться. ►
Нам интересно понять взаимосвязь между сочленениями,
которыми мы управляем, и положением рабочего органа, выполняющего работу. Это прямая кинематика – преобразование
координат сочленений (или конфигурации робота) в положение рабочего органа. Начнем с описания задачи в разделе 7.1.1
в терминах графа положений, затем в разделе 7.1.2 введем понятие цепочки жестких тел, а в разделе 7.1.3 – понятие цепочки
с несколькими ветвями – дерева жестких тел. В разделе 7.1.4 мы
познакомимся с форматом URDF для представления моделей
роботов и в разделе 7.1.5 введем классическую нотацию Денавита–Хартенберга. В каждом разделе мы сначала рассмотрим
простой 2-мерный случай, а затем 3-мерный.

7.1.1

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

Прямая кинематика на основе графа положений

В этом разделе мы применим наши знания о графах положений
► для решения прямой задачи кинематики в двух- и трехмерном пространствах.

Графы положений
представлены
в разделе 2.1.3.

Прямая кинематика 371

7.1.1.1

Двухмерные (планарные) роботизированные
манипуляторы

Начнем с одной кинематической цепочки, действующей в плос­
кости. Рассмотрим очень простой пример (рис. 7.4a), который
включает одно поворотное сочленение и одно звено и чем-то
напоминает стрелку часов. Граф положений отображает композицию двух относительных положений. Отличие от графов
положений, которые обсуждались в разделе 2.1.3, заключается
в том, что одна стрелка является функцией переменной сочленения и показана красным цветом. Стрелки представляют, по
порядку, поворот на угол q в сочленении, а затем фиксированное перемещение вдоль оси x на расстояние a1. Положение рабочего органа определяется просто:
ξE = ξ r(q) ⊕ ξ tx(a1),

0

где относительное положение ξ записывается как функция с параметром, который может быть либо переменной, например q,
либо константой, например a1. Верхний индекс указывает на
конкретную функцию: ξ r – относительное положение, представляющее собой поворот в плоскости, а ξ tx – относительное положение, представляющее собой перенос в плоскости вдоль оси x.

положение 0

положение E

https://go.sn.pub/
vNyqkq

https://go.sn.pub/
wMC1x3

https://go.sn.pub/
MZ9O9w

положение 0

положение 0

положение E

положение E

Рис. 7.4. Некоторые простые планарные роботизированные манипуляторы и соответствующие
им графы положений. Рабочий орган показан в виде черной сферы. Обобщенные
переменные сочленений – qi . В графе положений ребра могут быть переменными (красные)
или константами (черные) в зависимости от переменных сочленений. Система координат
{0} – это фиксированная система отсчета

372 Глава 7 · Кинематика робота-манипулятора

Используя Toolbox, это положение можно представить в виде
элементарной последовательности преобразований. Эти преобразования создаются путем объединения двух элементарных
преобразований, которые для двухмерного случая создаются
методами класса ET2:
>>> а1 = 1;
>>> е = ET2.R() * ET2.tx(a1);

Оператор * обозначает объединение (композицию), потому
что в Python нет оператора ⊕. Результатом является последовательность элементарных преобразований (elementary transformation sequence, ETS), инкапсулированная в экземпляр ETS2 –
объект, подобный списку:

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

>>> len(e)
2

который содержит два преобразования твердого тела: одно
с переменной и одно с константой. Преобразование с переменной создается методом без аргументов, а с константой соответствует методу с одним аргументом – числом размерностей робота, в данном случае с единицей. При отображении объекта ►
>>> e
R(q) ⊕ tx(1)

как можно заметить, пустые скобки заменяются на q, чтобы
подчеркнуть, что это переменная сочленения, а не константа.
Символ ⊕ напоминает, что это относительное положение, полученное в результате сложения двух элементарных движений.
Для конкретного значения переменной сочленения, скажем
q = π/6 радиан, положение рабочего органа будет определяться
мат­ри­цей
>>> e.fkine(pi / 6)
0.866
-0.5
0.5
0.866
0
0

0.866
0.5
1

представленной объектом SE2 и полученной в результате объ­
единения двух относительных положений в последовательности элементарных преобразований (ETS). Аргумент fkine заменяет совместную переменную в выражении и эквивалентен
>>> SE2.Rot(pi / 6) * SE2.Tx(a1)
0.866
-0.5
0.866
0.5
0.866
0.5
0
0
1

представлению прямой кинематики для нашего простого робота.

Эта красиво
напечатанная
версия
отображается
в ipython. В обычной
оболочке Python
REPL следует
использовать
print(e).

Прямая кинематика 373

Простой и наглядный способ понять, как ведет себя этот
прос­той робот, – вызвать метод
>>> e.teach()

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

Положение рабочего органа
x: 0.678 рыскание: 0.826
y: 0.735
Угол в сочленении

Рис. 7.5. Изображение одношарнирного планарного робота из рис. 7.4а с использованием
метода teach в пакете Toolbox. В панели слева отображаются координаты и ориентация (угол
рыскания в радианах) рабочего органа и ползунок угла поворота сочленения (в градусах).
В панели справа показаны робот и система координат рабочего органа

Рассмотрим теперь робота, показанного на рис. 7.4б, который
имеет два поворотных сочленения q0 и q1. Положение рабочего
органа извлекается из графа положений как
ξE = ξ r(q0) ⊕ ξ tx(a1) ⊕ ξ r(q1) ⊕ ξ tx(a2)

0

и с использованием Toolbox реализуется так:
>>> a1 = 1; a2 = 1;
>>> e = ET2.R() * ET2.tx(a1) * ET2.R() * ET2.tx(a2);

Отобразив объект
>>> e
R(q0) ⊕ tx(1) ⊕ R(q1) ⊕ tx(1)

374 Глава 7 · Кинематика робота-манипулятора

можно заметить, что пустые скобки были заменены переменными последовательных сочленений q0 и q1. Мы можем вычислить это выражение для конкретных углов в градусах
>>> e.fkine(np.deg2rad([30, 40])).printline()
t = 1.21, 1.44; 70°

и получить в результате положение рабочего органа, например
когда q0 = 30° и q1 = 40°. Этот вызов эквивалентен следующему
выражению:
>>> T = SE2.Rot(np.deg2rad(30)) * SE2.Tx(a1)
...
* SE2.Rot(np.deg2rad(40)) * SE2.Tx(a2);
>>> T.printline()
t = 1.21, 1.44; 70°

Объект ETS2 имеет множество методов. Можно найти количест­
во сочленений и элементы ET в последовательности сочленений
>>> e.n
2
>>> e.joints()
[R(q0), R(q1)]

Структура сочленений робота часто обозначается сокращенно буквами R (revolute – поворотный) или P (prismatic – призматический), указывающими количество и типы сочленений. Для
данной кинематической цепочки получаем такой результат:
>>> e.structure
'RR'

сообщающий, что используется последовательность сочленений «поворотное-поворотное». Мы можем также создать ин­
тер­активное представление робота, как в предыдущем примере, так и неинтерактивное
>>> e.plot(np.deg2rad([30, 40]));

Объект ETS2 также можно использовать как список и, например, получить отдельный элемент
>>> e[1]
tx(1)

который является объектом ET2 и тоже имеет ряд методов:
>>> e[1].eta
1
>>> e[1].A()
array([[1, 0, 1],
[0, 1, 0],
[0, 0, 1]])

Передать можно
любой итерируемый тип, например
список, кортеж или
массив NumPy.

Прямая кинематика 375

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

которые возвращают соответственно константу преобразования η, переданную конструктору, в данном случае a1, и соответствующую мат­ри­цу SE(2), которая в этом примере представляет
перемещение на 1 в направлении x.
Этот простой робот-манипулятор, действующий в плоскости, обладает рядом интересных особенностей. Во-первых,
большинство положений рабочего органа можно достичь, используя два разных набора углов поворота в сочленениях. Вовторых, робот может расположить рабочий орган в любой точке
в пределах досягаемости, но не способен обеспечить его произвольную ориентацию.
Конфигурация робота-манипулятора с N сочленениями описывается вектором обобщенных координат, где qj ∈ S1 – угол для
поворотного сочленения или qj ∈ ℝ – длина призматического
сочленения. Мы называем q конфигурацией сочленений, или ко­
ординатами сочленений, а для робота с поворотными сочленениями – углами сочленений.
 Иногда их по ошибке называют положением манипулятора. В этой
книге термин «положение» используется для обозначения ξ – комбинации местоположения (координат) и ориентации тела в пространстве.

Для призматического сочленения
необходимо указать
диапазон перемещения qlim, чтобы
метод teach мог
определить размеры поверхности
построения диаграммы и масштаб
ползунка. Если не
указано иное, то
предполагается,
что поворотные
сочленения имеют
диапазон [–π, π].

Вернемся к обсуждению конфигурации и пространства
задач из раздела 2.4.9. Этот робот имеет 2 степени свободы,
а его конфигурационное пространство определяется как 𝒞 ∈
S1 × S1 и q ∈ 𝒞. Этого достаточно для достижения точек в пространстве задач 𝒯 ⊂ ℝ2, так как dim 𝒞 = dim 𝒯. Однако если
пространство задач включает ориентацию 𝒯 ⊂ ℝ2 × S1, то робот
оказывается малоприводным, потому что dim 𝒞 < dim 𝒯, и может получить доступ только к подмножеству пространства задач, например может достичь нужного местоположения, но
не ориентации.
Наконец, перейдем к роботу на рис. 7.4в, который включает призматическое сочленение и обычно называется полярнокоординатным роботом-манипулятором. Положение рабочего
органа выражается как
ξE = ξ r(q0) ⊕ ξ tx(q1)

0

и в Toolbox реализуется в виде объекта
>>> e = ET2.R() * ET2.tx(qlim=[1,2])
R(q0) ⊕ tx(q1)
>>> e.structure
'RP'

который, как и предыдущие объекты, имеет методы fkine, teach
и plot.

376 Глава 7 · Кинематика робота-манипулятора
Отступление 7.1. Призматические сочленения
Сочленения роботов могут быть не только поворотными (револьверными), но также призматическими
(линейными, скользящими, телескопическими и т. д.).
Робот SCARA (рис. 7.2б) имеет третье призматическое
сочленение, а портальный робот (рис. 7.2в) – три приз­
матических сочленения, обеспечивающих перемещение в направлениях x, y и z.
Стэнфордский манипулятор, показанный здесь, имеет третье призматическое
сочленение. Он был разработан в Стэнфордской лаборатории искусственного интеллекта в 1969 году пионером робототехники Виктором Шейнманом (1942–2016),
основателем VICARM, который впоследствии спроектировал роботизированные
манипуляторы PUMA для Unimation Inc. (см. отступление 7.2). Один экземпляр
можно увидеть в Смитсоновском музее американской истории в Вашингтоне,
округ Колумбия. (Изображение предоставил Усама Хатиб.)

Мы могли бы добавить больше сочленений и использовать
уже знакомый функционал Toolbox для исследования возможностей создаваемых нами роботов. Робот с тремя или более поворотными сочленениями может достичь любой точки в пространстве задач 𝒯 ⊂ ℝ2 × S1, то есть принять любое положение
в плоскости (ограниченной радиусом действия).
Манипулятор с N сочленениями, пронумерованными от 0 до
N - 1, имеет N + 1 звеньев с порядковыми номерами от 0 до N.
Сочленение j соединяет звено j со звеном j - 1 и перемещает их
относительно друг друга. Звено ℓ соединяет сочленение ℓ - 1
с сочленением ℓ. Звено 0 является основанием робота, обычно
неподвижным, а звено N – это последнее звено робота, которое
несет рабочий орган или инструмент.

7.1.1.2

Трехмерные роботизированные манипуляторы

Большинство роботов-манипуляторов в реальном мире имеют
пространство задач 𝒯 ⊂ ℝ3 × (S1)3, которое позволяет произвольно позиционировать и ориентировать рабочий орган в пределах трехмерной рабочей зоны. Для этого требуется робот с конфигурационным пространством dim 𝒞 ≥ dim 𝒯, то есть с шестью
или более сочленениями. Используя трехмерную версию кода
выше, мы можем определить робота, представляющего упрощенную имитацию человеческой руки:
>>> a1 = 1; a2 = 1;
>>> e = ET.Rz() * ET.Ry() \
...
* ET.tz(a1) * ET.Ry() * ET.tz(a2) \
...
* ET.Rz() * ET.Ry() * ET.Rz();

Прямая кинематика 377
Отступление 7.2. Робот PUMA 560
Программируемый универсальный сборочный манипулятор (Programmable Universal Manipulator for Assembly,
PUMA) модели 560 был выпущен в 1978 году и приобрел
огромную популярность. Это был первый современный
промышленный робот, имевший антропоморфную конструкцию, электродвигатели и сферическую кисть. Один
из экземпляров выставлен в Смитсоновском музее американской истории в Вашингтоне, округ Колумбия.
Появление PUMA 560 дало толчок исследованиям в области робототехники в 1980-х, эта модель стала одной из
самых распространенных лабораторных роботов. К настоящему времени она устарела и почти везде заменена более совершенными моделями, но в знак признания
важной роли в робототехнике мы используем ее в нескольких примерах. Для нас
преимущество этого робота заключается в том, что он хорошо изучен и его параметры хорошо известны – его называют «белой крысой» в робототехнике. (Изображение предоставил Усама Хатиб.)

который имеет сферический плечевой сустав с 2 степенями
свободы (строка 2); плечо, локтевой сустав и предплечье (строка 3); и сферический лучезапястный сустав с 3 степенями свободы, представляющий собой последовательность углов Эйлера
ZYZ (строка 4). e – это объект ETS, трехмерная версия ETS2. Он
предлагает те же методы, что и ETS2, использованные в предыдущем разделе.
>>> e.n
6
>>> e.structure
'RRRRRR'

Положение рабочего органа при нулевых углах поворота всех
сочленений:
>>> e.fkine(np.zeros((6,)))
1
0
0

0

Отступление 7.3. Антропоморфная конструкция
Антропоморфными называют конструкции, обладающие человекоподобными характеристиками. Например, робот PUMA 560 имеет размеры и радиус действия,
примерно сопоставимые с размерами и радиусом действия человека. Он имел
в запястье такой же сферический сустав, как у человека.
Робототехники также склонны использовать антропоморфные термины при
описании роботов. Мы используем такие термины, как «туловище», «плечо», «локоть» и «запястье», при описании манипуляторов с последовательным соединением звеньев. Для робота PUMA эти термины соответствуют сочленениям 0, 1, 2
и 3–5 соответственно.

378 Глава 7 · Кинематика робота-манипулятора
0
0
0

7.1.2

1
0
0

0
1
0

0
2
1

Прямая кинематика как цепочка звеньев робота

Основная часть физической структуры робота определяется его
звеньями, которые образуют его форму и внешний вид, а также обладают такими важными свойствами, как масса, инерция
и риск столкновения. На рис. 7.6 показана взаимосвязь между
сочленениями и системами координат звеньев. Система координат звена ℓ обозначается как {ℓ}, а его положение является
функцией координат сочленения {qj, j ∈ [0, ℓ)}. Мы говорим, что
звено ℓ является родительским для звена ℓ + 1 и дочерним для
звена ℓ - 1. Неподвижное основание считается звеном 0 и не
имеет родителя, тогда как рабочий орган не имеет потомка.
Положение рабочего органа, его прямая кинематика, выражается как
ξN = 0ξ1(q0) ⊕ 1ξ2(q1) ⊕ ... ⊕ N-1ξN(qN-1),

(7.1)

ξN = 𝒦(q),

(7.2)

0

где jξj+1(qj) – положение системы координат звена {j + 1} относительно системы координат звена {j}. То же самое можно записать в функциональной форме
0

где 𝒦(·) – функция, характерная для робота и включающая параметры сочленений и звеньев.
сочленение 0
сочленение 1
основание

рабочий орган

система координат звена 1
перед сочленением 1

система координат звена 2
после сочленения 2

Рис. 7.6. Граф положений, отображающий системы координат звеньев. Каждое относительное
положение в этом графе является функцией переменной сочленения. Система координат
{0} – это фиксированное основание, база, а система {E} соответствует рабочему органу

7.1.2.1

Двухмерный (плоский) случай

Снова рассмотрим робота на рис. 7.4б и жестко прикрепим
к каждому звену отдельную систему координат, как показано

Прямая кинематика 379

на рис. 7.7. Положение звена 1 зависит только от q0, а положение
звена 2 – от q0 и q1.

https://go.sn.pub/
uXIHHx

Рис. 7.7. Системы координат, прикрепленные к звеньям робота, изображенного на рис. 7.4б.
Звено 1 показано красным, а звено 2 – синим

На рис. 7.8 показан более подробный граф положений этого робота с выделенными сочленениями и звеньями. Система
координат {ℓ¢} является родительской стороной сочленения, то
есть сочленение представляет преобразование из системы координат {ℓ¢} родительского звена в систему координат {ℓ} потомка. Когда qℓ-1 = 0, эти две системы координат идентичны.

сочленение 0

"link1"

"link2"

сочленение 1

основание

"link3"

система координат звена 1
перед сочленением 1

рабочий орган

система координат звена 2
после сочленения 2

Рис. 7.8. Граф положений робота, изображенного на рис. 7.4б, с выделенными сочленениями
и системами координат звеньев. Серые текстовые поля связывают элементы графа
положений с именованными объектами Link2

Описать робота с точки зрения звеньев можно следующим
образом:
>>>
>>>
>>>
>>>

a1 = 1;
link1 =
link2 =
link3 =

a2 = 1;
Link2(ET2.R(), name="link1");
Link2(ET2.tx(a) * ET2.R(), name="link2", parent=link1);
Link2(ET2.tx(a2), name="link3", parent=link2);

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

380 Глава 7 · Кинематика робота-манипулятора

ссылка на родительское звено и необязательное имя. Преобразование описывается с помощью выражения последовательности преобразований ETS и может включать не более одной
переменной сочленения, которая, если присутствует, должна
быть последней в последовательности. В примере выше также создается система координат для рабочего органа, который
имеет константное преобразование относительно своего родителя.
Передадим этот список объектов звеньев конструктору робота и получим отображение его богатой структуры:
>>> robot = ERobot2([link1, link2, link3], name="my robot")
ERobot2: my robot, 2 joints (RR)
link
link
joint
parent
ETS: parent to link
0
link1
0
BASE
R(q0)
1
link2
1
link1
tx(1) ⊕ R(q1)
2
@link3
link2
tx(1)

Над таблицей указаны тип класса, имя робота, количество
сочленений и их структура. Символ @ обозначает систему координат звена, который также является рабочим органом – у него
нет дочерних звеньев. ► В правом столбце показаны элементарные преобразования, соответствующие звену. Сочленениям
присвоены последовательные номера, начиная с нуля. В целом
эта таблица описывает кинематическую цепочку – последовательность твердых тел и сочленений.
Этот объект подкласса робота имеет множество методов.
Прямая кинематика возвращает экземпляр SE2:
>>> robot.fkine(np.deg2rad([30, 40])).printline()
t = 1.21, 1.44; 70°

представляет положение системы координат link3. Мы можем
создать экземпляр робота с этой конфигурацией:
>>> robot.plot(np.deg2rad([30, 40]));

а также нарисовать конфигурацию последовательности сочле­
нений в виде анимации. Последовательность представлена
массивом, строки которого определяют последовательные конфигурации
>>> q = np.array([np.linspace(0, pi, 100),
...
np.linspace(0, -2 * pi, 100)]).T;
>>> q.shape
(100, 2)
>>> robot.plot(q);

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

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

Прямая кинематика 381
>>> robot[1]
Link2("link2", tx(1) ⊕ R(q), parent="link1")

который в данном случае является звеном 1, или как итератор
по звеньям. Его также можно использовать как словарь
>>> robot["link2"]
Link2("link2", tx(1) ⊕ R(q), parent="link1")

и извлекать ссылки на звенья по их именам. Рабочие органы
робота доступны как
>>> robot.ee_links
[Link2("link3", tx(1), parent="link2")]

И в этом случае в возвращаемом списке имеется только один
рабочий орган.
Объекты звеньев имеют множество своих методов и свойств.
Например, так можно получить ссылку на родительское звено:
>>> link2.parent
Link2("link1", R(q))

а так – на дочернее:
>>> link2.children
[Link2("link3", tx(1), parent="link2")]

В последнем случае возвращается список звеньев. В данном
примере имеется только одно дочернее звено, но у «многоруких» роботов их может быть больше.
Переменная сочленения для этого звена назначена как q1:
>>> link2.jindex
1

Мы можем проверить тип сочленения:
>>> link2.isrevolute
True
>>> link2.isprismatic
False

Звено link3 не будет определено ни как поворотное, ни как
призматическое – это фиксированное сочленение. У сочленений могут быть верхние и нижние пределы, но в данном случае
они не заданы.
>>> print(link2.qlim)
None

Для прямой кинематики важную роль играет метод
>>> link2.A(pi / 6)
0.866
-0.5
1

382 Глава 7 · Кинематика робота-манипулятора
0.5
0

0.866
0

0
1

возвращающий относительное положение системы координат
этого звена по отношению к системе координат родителя или
1
ξ2. Он оценивает ETS звена
>>> link2.ets
tx(1) ⊕ R(q)

для конкретного значения q, переданного в метод A. Прямая
кинематика – это просто комбинация преобразований звена от
основания к инструменту.

7.1.2.2

Трехмерный случай

Для опробования трехмерного случая можно создать модель
робота по аналогичному шаблону, но используя классы Link,
ERobot и ETS. Однако можно пойти более коротким путем. Вопер­вых, можно создать робота непосредственно из выражения
ETS, в данном случае из выражения в разделе 7.1.1.2.
>>> a1 = 1; a2 = 1;
>>> robot6 = ERobot(ET.Rz() * ET.Ry() * ET.tz(a1) * ET.Ry() \
...
* ET.tz(a2) * ET.Rz() * ET.Ry() * ET.Rz())
ERobot: noname, 6 joints (RRRRRR)
link
link
joint
parent
ETS: parent to link
0
link0
0
BASE
Rz(q0)
1
link1
1
link0
Ry(q1)
2
link2
2
link1
tz(1) ⊕ Ry(q2)
3
link3
3
link2
tz(1) ⊕ Rz(q3)
4
link4
4
link3
Ry(q4)
5
@link5
5
link4
Rz(q5)

Конструктор ERobot автоматически разбил выражение ETS на
части и создал объекты звеньев. Теперь можно применить уже
знакомые методы, такие как fkine, plot и teach.
Второй способ – использовать одну из моделей, включенных
в Toolbox. Список моделей можно получить так:
>>> models.list(type="ETS")
class
name
Panda
Panda
Frankie
Frankie
Puma560
Puma560
Planar_Y
Planar-Y
Planar2
Planar2
GenericSeven
Generic Seven

manufacturer
Franka Emika
Franka Emika, Omron
Unimation
Jesse's Imagination

type
ETSS
ETS
ETS
ETS
ETS
ETS

DoF
7
9
6
6
2
7

di
3d
3d
3d
3d
2d
3d

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

Каждая модель наследует класс ERobot. Например, вот как
можно создать экземпляр модели робота Panda:

Прямая кинематика 383
>>> panda = models.ETS.Panda()
ERobot: Panda (by Franka Emika), 7 joints (RRRRRRR)
link
link
joint
parent
ETS: parent to link
0
link0
0
BASE
tz(0.333) ⊕ Rz(q0)
1
link1
1
link0
Rx(–90°) ⊕ Rz(q1)
2
link2
2
link1
Rx(90°) ⊕ tz()0.316) ⊕ Rz(q2)
3
link3
3
link2
tx(0.0825) ⊕ Rx(90°) ⊕ Rz(q3)
4
link4
4
link3
tx(–0.0825) ⊕ Rx(–90°) ⊕ tz(0.3
5
link5
5
link4
Rx(90°) ⊕ Rz(q5)
6
link6
6
link5
tx(0.088) ⊕ Rx(90°) ⊕ tz(0.107)
7
@ee
link6
tx(0.103) ⊕ Rz(–45°)
name
qr
qz

q0
0°
0°

q1
q2
–17.2° 0°
0°
0°

q3
–126°
0°

q4
0°
0°

q5
115°
0°

q6
45°
0°

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

Формат первой таблицы уже знаком вам, а вторая таблица
перечисляет некоторые предопределенные конфигурации со­
членений с именами qr и qz, доступные как свойства объекта
робота. Это просто конфигурации именованных сочленений.
>>> panda.qr
array([ 0, -0.3,

0, -2.2,

0,

2, 0.7854])

Чтобы конфигураДобавить конфигурацию в этот экземпляр робота можно выция отображалась зовом
как атрибут объекта
робота, используйте
>>> panda.addconfiguration("foo", [1, 2, 3, 4, 5, 6, 7])
вызов addconfiguration_attr, но
Все конфигурации, предопределенные или определенные
имейте в виду, что
динамическое до- пользователем, доступны в словаре конфигураций configs:
бавление атрибутов
в объекты может >>> panda.configs["foo"];
запутать систему >>> panda.configs["qz"];
проверки типов
Python в вашей IDE.

Используя конфигурацию сочленений, можно вычислить
прямую кинематику
>>> panda.fkine(panda.qr).printline()
t = 0.484, 0, 0.413; rpy/zyx = 180°, -5.73°, 0°

или нарисовать диаграмму робота с конкретной именованной
конфигурацией
>>> panda.plot(panda.qr);

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

384 Глава 7 · Кинематика робота-манипулятора

стемы координат звена. Вычисление прямой кинематики – это
последовательный процесс, включающий определение положений всех систем координат промежуточных звеньев. Все эти
системы координат можно получить одним вызовом метода
>>> T = panda.fkine_all(panda.qr);
>>> len(T)
9

Рис. 7.9. Трехмерная диаграмма робота Panda. Этот график построен с использованием
стандартной библиотеки PyPlot (Matplotlib). Отрезки изображают связанные системы
координат звеньев, а не физические звенья робота-манипулятора

а

б

Рис. 7.10. а) Трехмерное изображение робота Panda; б) каркасная сетка, описывающая форму
звена 1, точки которой определяются относительно системы координат звена. Положение звена
вычисляется с помощью прямой кинематики, а координаты точек, определяющих форму звена,
могут быть преобразованы в их правильное местоположение в мировой системе координат

Прямая кинематика 385

Положение системы координат звена 1:
>>> T[1].printline()
t = 0, 0, 0.333; rpy/zyx = 0°, 0°, 0°

T[0] – система координат основания, а T[8] – система координат инструмента. Зная положение системы координат звена,
можно преобразовать все точки его сетки и отобразить ее на
экране.
Центр масс (center of mass, CoM) звена определяется вектором координат относительно системы координат звена, а тензор инерции определяется относительно системы координат,
параллельной системе координат звена, но с началом в центре
масс. Эти параметры важны для моделирования динамики робота, о которой мы поговорим в главе 9.

7.1.2.3

Инструменты и основания

Будет полезно расширить формулу (7.1) прямой кинематики,
добавив два дополнительных преобразования
(7.3)
показанных на рис. 7.11. Мы обозначили мировую систему координат символом W, потому что здесь 0 обозначает звено 0 –
основание манипулятора.

основание робота

рабочий орган

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

Рис. 7.11. Кинематическая цепочка, представленная на рис. 7.6, расширенная за счет включения системы координат
основания и преобразования инструмента

Традиционно началом кинематической цепочки является
основание робота, но базовое преобразование ξB позволяет поместить основание робота в произвольное положение в мировой системе координат. В производственной ячейке основание
робота фиксировано и определено относительно ячейки. Если
бы манипулятор был установлен на мобильном роботе, то базовое преобразование могло бы изменяться во времени.
Для кинематических моделей не существует стандарта, который бы определял, где на роботе должна находиться система координат инструмента {N}. Для модели робота URDF это,

386 Глава 7 · Кинематика робота-манипулятора

скорее всего, будет фланец крепления инструмента на физическом конце манипулятора, как показано на рис. 7.12б. В модели Денавита–Хартенберга (раздел 7.1.5) часто используется
центр сферического запястья, физически находящегося внутри
робота. Преобразование инструмента ξT описывает положение
острия инструмента – той части, которая фактически выполняет работу и иногда называется центральной точкой инструмента – относительно системы координат {N}. Принято считать,
что инструмент робота направлен вдоль оси z, как показано на
рис. 2.20. На практике ξT может варьироваться в зависимости от
области применения, например для захвата, отвертки или сварочной горелки, или внутри одной области применения, если
требуется смена инструмента. Она также может состоять из нескольких компонентов, например положения по отношению
к держателю инструмента или специфичного для выбранного
инструмента.

а

б

Рис. 7.12. Робот Panda: а) с захватным устройством, включающим камеру; b) без захвата, с фланцем крепления DIN
ISO 9409-1-A50, к которому можно прикрепить инструменты (изображения предоставлены Дорианом Цаем)

При желании для любого объекта в Toolbox можно задать положение основания или инструмента
>>> panda.base = SE3.Tz(3) * SE3.Rx(pi) # основание закреплено на высоте
# 3 м, манипулятор направлен вниз
>>> panda.tool = SE3.Tz(0.15); # инструмент длиной 150 мм направлен
# вдоль оси z

Для поддержки возможности смены инструментов преобразование инструмента может быть переопределено для конкретного случая.
>>> panda.fkine(panda.qr, tool=SE3.Trans(0.05, 0.02, 0.20)
...
* SE3.Rz(np.deg2rad(45)));

Аргументом во всех случаях является объект SE2 или SE3 в зависимости от класса робота.

Прямая кинематика 387

7.1.3

Многорукие роботы

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

7.1.3.1

Двухмерные (планарные) многорукие
роботизированные манипуляторы

Рассмотрим ключевые моменты на примере планарного робота, показанного на рис. 7.13a, с двумя рабочими органами. Его
граф положений изображен на рис. 7.13б. Вот как с помощью
Toolbox можно представить этот манипулятор в виде списка
объектов звеньев:
>>> robot = ERobot2([
...
Link2(ET2.R(), name="link1"),
...
Link2(ET2.tx(1) * ET2.tx(1.2) * ET2.ty(-0.5) * ET2.R(),
...
name="link2", parent="link1"),
...
Link2(ET2.tx(1), name="ee_1", parent="link2"),
...
Link2(ET2.tx(1) * ET2.tx(0.6) * ET2.ty(0.5) * ET2.R(),
...
name="link3", parent="link1"),
...
Link2(ET2.tx(1), name="ee_2", parent="link3")],
...
name="branched");

Как можно заметить, звенья с именами "link2" и "link3" имеют общего родителя – звено "link1". Звено с именем "link1",
в свою очередь, имеет два дочерних звена.
>>> robot["link1"].children
[Link2("link2", tx(1) ⊕ tx(1.2) ⊕ ty(-0.5) ⊕ R(q), parent="link1",
Link2("link3", tx(1) ⊕ tx(0.6) ⊕ ty(0.5) ⊕ R(q), parent="link1"]

При попытке отобразить объект robot получаем:

>>> robot
ERobot2: branched, 3 joints (RRR), 2 branches
link
link
joint
parent
ETS: parent to link
0
link1
0
BASE
R(q0)
1
link2
1
link0
tx(1) ⊕ tx(1.2) ⊕ ty(-0.5) ⊕ R(q1)
2
@ee_1
link1
tx(1)
3
link3
2
link1
tx(1) ⊕ tx(0.6) ⊕ ty(0.5) ⊕ R(q2)
4
@ee_2
link3
tx(1)

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

388 Глава 7 · Кинематика робота-манипулятора

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

https://go.sn.pub/
eLbuEp

а

рабочий орган 1

основание

"link"

"link2"
"ee_1"

"link3"

"ee_2"

рабочий орган 2

б
Рис. 7.13. Многорукий робот: а) системы координат звеньев; б) граф положений. Серые
текстовые поля соединяют элементы графа положений с именованными объектами Link2

Робот с такой структурой образует дерево жесткого тела, которое можно отобразить в виде графа
>>> robot.showgraph()

Результат этого вызова показан на рис. 7.14. Как и в предыдущих примерах, работу этого робота можно исследовать с использованием интерактивной диаграммы (рис. 7.15):
>>> robot.teach()
>>> robot.plot([0.3, 0.4, -0.6]);

Прямая кинематика 389

Рис. 7.14. Дерево твердого тела в виде графа автоматически генерируется методом
showgraph из пакета GraphViz

4
Положение рабочего органа № 0
x: 3.095

рыскание: -0.349

y: 0.325

2

30.0°

q1

-5 0 . 0 °

q2

75.0°

Y

Угол в сочленениях
q0

многорукий

0

−2

−4

−4

−2

0

X

2

4

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

Для расчета прямой кинематики теперь
Рабочий орган
может быть задан кой рабочий орган использовать, например
ссылкой на объект
звена или строкой
>>> robot.fkine([0.3, 0.4, -0.6], end="ee_2")
с именем.
0.9553
-0.2955
0

0.2955
0.9553
0

нужно указать, ка:

2.336
0.655
1

Аналогично можно получить относительное положение {EE2}
относительно {EE1} (пунктирная серая стрелка на рис. 7.13б).
>>> robot.fkine([0.3,
0.5403
0.8415
-0.8415
0.5403
0
0

0.4, -0.6], end="ee_2", start="ee_1")
-0.6229
0.3132
1

390 Глава 7 · Кинематика робота-манипулятора

7.1.4

Унифицированный формат описания роботов
(URDF)

До сих пор мы создавали деревья жестких тел с использованием
программного кода, но такой подход сильно зависит от используемого языка программирования и вспомогательного программного обеспечения. Для обмена моделями между собой мы
используем унифицированный формат описания роботов (Unified Robot Description Format, URDF) – формат файлов на основе
XML.
Этот формат широко используется исследователями,
и в интернете можно найти множество моделей роботов в этом
формате. Пакет Toolbox поддерживает парсинг файлов URDF
>>> urdf, *_ = ERobot.URDF_read("ur_description/urdf/ur5_joint_\
limited_robot.urdf.xacro")
>>> urdf
[Link ("base_link", SE3(), parent="world", m=4, r=[0, 0, 0],
I=[0.00443, 0.00443, 0.0072, 0, 0, 0], Jm=0, B=0,
Tc=[0, 0], G=0),
Link ("shoulder_link", SE3(0, 0, 0.08916) ⊕ Rz(q)
parent="base_link", glim=[-3.14, 3.14], m=3.7,
r=[0, 0, 0], I=[0.0103, 0.0103, 0.00666, 0, 0, 01],
Jm=0, B=0, Tc=[0, 0], G=0),
Link ("upper_arm_link",
SE3(0, 0.1358, 0; 0°, 90°, -0°) ⊕ Ry(q),
parent="shoulder_link", glim=[-3.14, 3.14], m=8.39,
r=[0, 0, 0.28], I=[0.227, 0.227, 0.0151, 0, 0, 0],
Jm=0, B=0, Tc=[0, 0], G=0),
Link ("forearm_link", SE3(0, -0.1197, 0.425) ⊕ Ry(q),
parent="upper_ arm_link", qlim(-3.14, 3.14],
m=2.27, r=[0, 0, 0.196], I=[0.0312, 0.0312, 0.00409, 0, 0],
Jm=0, B=0, Tc=[0, 0], G=0),
...

и возвращает имя робота и список объектов звеньев, инициализированных параметрами из файла URDF. Инерционные параметры, если они определены в файле, будут включены в объекты звеньев.
Модели URDF часто представляют пальцы захватов как звенья с собственной переменной сочленения, что, по сути, создает многорукий робот. На практике пальцы захвата рассмат­
риваются как часть независимой подсистемы, а не как часть
кинематической цепочки. В крайнем случае можно представить захват фиксированным расстоянием до середины пальцев – центральной точки инструмента – и затем сжимать или
разжимать пальцы относительно этой точки.
Пакет Toolbox включает классы, которые читают файлы URDF
и возвращают модели в виде экземпляров подклассов, насле­
дующих Robot.

Многие файлы
URDF написаны
с использованием
Xacro (файлы с расширением .xacro) –
языка макросов
XML, который
помогает сократить
файлы и сделать их
более удобочитаемыми. Для обработки таких файлов
в Toolbox имеется
специализированный препроцессор.
Если указан относительный путь, то
сначала выполняется поиск в текущей
папке, а если файл
не будет найден, то
поиск продолжится
в пакете данных
Robotics Toolbox,
который устанавливается как часть
Toolbox.

Такие как масса
(m), центр масс (r),
тензор инерции (I),
инерция двигателя
(Jm), трение двигателя (B) и передаточное отношение (G).

Прямая кинематика 391
>>> ur5
ERobot:
link
0
1
2
3
4
5
6
7
8
9
name
qr
qz
qn
ql

= models.URDF.UR5()
UR5 (by Universal Robotics), 6 joints (RRRRRR), 1 gripper...
link
joint
parent
ETS: pare
world
BASE
base_link
world
SE3()
shoulder_link
0
base_link
SE3(0, 0, 0.08916)
upper_arm_link
1
shoulder_link
SE3(0, 0.1358, 0; 0
forearm_link
2
upper_arm_link
SE3(0, -0.1197, 0.4
wrist_1_link
3
forearm_link
SE3(0, 0, 0.3922; 0
wrist_2_link
4
wrist_1_link
SE3(0, 0.093, 0) ⊕
wrist_3_link
5
wrist_2_link
SE3(0, 0, 0.0465)
tool0
wrist_3_link
SE3(0, 0.0823, 0; base
base_link
SE3(0°, -0°, -180°)
q0
180°
0°
-40.4°
0°

q1
0°
0°
20.7°
-90°

q2
0°
0°
-85.6°
90°

q3
0°
0°
65°
0°

q4
90°
0°
-40.4°
90°

q5
0°
0°
0°
0°

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

В первой строке указаны некоторые ключевые характерис­
тики робота, включая название, количество сочленений и их
структуру. Робот описывается как многорукий, хотя представляет собой единую кинематическую цепочку. Это обусловлено
особенностью написания файла URDF, и «руки» (ветви) легко
увидеть на графе
>>> ur5.showgraph()

Модель также имеет один захват:
>>> ur5.grippers
[Gripper("ee_link", connected to wrist_3_link, 0 joints, 1 links)]

Теги в конце строки над первой таблицей, что показана выше,
говорят о присутствии в модели инерционных параметров
(dynamics), трехмерной геометрии звеньев (geometry) и модели
столк­новений (collision). Данные геометрии позволяют создать
реалистичную трехмерную визуализацию робота (рис. 7.16).
Каждое звено описывается файлом сетки , включающим трехмерную форму и, при необходимости, цвет и текстуру поверхности. Если модель робота включает геометрические данные, то

Файлы сеток обычно имеют формат
COLLADA (.dae) или
STL (.stl). Файлы
COLLADA также
содержат данные >>> ur5.plot(ur5.qr);
о цвете и текстуре
поверхности, что отобразит в новой вкладке браузера вполне реалистичное изопозволяет создавать
бражение робота, а не просто каркасную диаграмму. Модели
более реалистичные
визуализации по столкновений – это простые геометрические аппроксимации
сравнению с фай- формы звена, которые можно проверить на пересечение с друлами STL, которые
гими объектами во время моделирования. Подробнее об этом
содержат только
геометрию. рассказывается в разделе 7.5.5.

392 Глава 7 · Кинематика робота-манипулятора

а

б

в

г

Рис. 7.16. Галерея изображений роботов, созданных в Swift – стандартном инструменте
отображения экземпляров ERobot с геометрическими данными: а) ABB YuMi (models.URDF.
YuMi); б) Universal Robotics UR5 (models.URDF.UR5); c) Willow Garage PR2 (models.URDF.PR2);
d) NASA Valkyrie (models.URDF.Valkyrie). Робот YuMi смоделирован с помощью файлов STL,
содержащих только геометрические данные, а остальные роботы смоделированы с помощью
файлов COLLADA (файлы .dae), содержащих геометрию, цвет и текстуру

Инерционные параметры можно вывести в табличной форме:
>>> ur5.dynamics()
j
world
base_link
shoulder_link
upper_arm_link
forearm_link
wrist_1_link
wrist_2_link
wrist_3_link
tool0
base

m
0
4
3.7
8.39
2.27
1.22
1.22
0.188
0
0

r
0,
0,
0,
0,
0,
0,
0,
0,
0,
0,

0, 0
0, 0
0, 0
0, 0.28
0, 0.196
0.093, 0
0, 0.0946
0.065, 0
0, 0
0, 0

I
0, 0, 0, 0, 0, 0
0.00443, 0.00443, 0.0
0.0103, 0.0103, 0.006
0.227, 0.227, 0.0151,
0.0312, 0.0312, 0.004
0.00256, 0.00256, 0.0
0.00256, 0.00256, 0.0
8.47e-05, 8.47e-05, 0
0, 0, 0, 0, 0, 0
0, 0, 0, 0, 0, 0

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

где m – масса звена, r – центр масс звена в его системе координат, I – уникальные элементы тензора инерции симметричного

Прямая кинематика 393
Шесть уникальных
элементов симмет­
ричного тензора
инерции относительно центра масс
звена: Ixx, Iyy, Izz, Ixy,
Iyz, Ixz.

звена, ◄ Jm – момент инерции двигателя, B и Tc – вязкое и кулоновское трение, G – передаточное отношение. Подробнее о динамике роботов мы поговорим в главе 9.
Система координат тензора инерции
Тензор инерции определяется относительно центра масс звена, что
удобно для многих алгоритмов динамики. Файлы URDF описывают инерцию относительно системы координат с началом в центре
масс. Однако это нестандартное решение, и в разных программных
пакетах могут использоваться разные подходы. Например, программа, используемая в версии этой книги для MATLAB, использует тензоры инерции, которые определяются относительно системы
координат звена.

В завершение этого раздела рассмотрим пример, в котором
используются многие концепции, рассмотренные выше. Загрузим модель трехмерного многорукого робота из файла URDF:
>>> yumi = models.URDF.YuMi()
ERobot: yumi (by ABB), 14 joints (RRRRRRRRRRRRRR), 2 grippers,
2 branches, dynamics, geometry, collision
...

Судя по представленной таблице, робот имеет 14 поворотных
сочленений, а также два рабочих органа. Структура звеньев
>>> yumi.showgraph(ets="brief")

показана на рис. 7.17. Робот имеет два захвата
>>> yumi.grippers
[Gripper("r_gripper", connected to gripper_l_base, 2 joints,
3 links),
Gripper("l_gripper", connected to gripper_r_base, 2 joints,
3 links)]

каждый из которых имеет два сочленения и три звена – их можно видеть в дереве твердого тела на рис. 7.17. Номера сочленений, как отмечалось ранее, назначаются в порядке «от основания к концу». Робота можно отобразить во вкладке браузера
вызовом (рис. 7.16а):
>>> yumi.plot(yumi.q1);

Рис. 7.17. Кинематическая структура робота YuMi, описанная в URDF-файле. Две основные ветви представляют
две руки робота, каждая из которых заканчивается парой ветвей, представляющих пальцы захвата. Сочленения
пронумерованы в порядке возрастания, но суставы пальцев пронумерованы относительно захвата

394 Глава 7 · Кинематика робота-манипулятора

В пакете Toolbox имеются классы, определяющие множество
роботов на основе моделей URDF, поставляемых вместе с пакетом. Получить список этих классов можно вызовом
>>> models.list(type="URDF")
class
name
Panda
panda
Frankie
frankie
FrankieOmni
FrankieOmni
UR3
UR3
UR5
UR5
UR10
UR10
Puma560
Puma560
px100
px100
px150
px150
rx150
rx150

manufacturer
Franka Emika
Franka Emika
Custom
Universal Robotics
Universal Robotics
Universal Robotics
Unimation
Interbotix
Interbotix
Interbotix

type
URDF
URDF
URDF
URDF
URDF
URDF
URDF
URDF
URDF
URDF

DoF
7
9
10
6
6
6
6
7
8
8

dim
3d
3d
3d
3d
3d
3d
3d
3d
3d
3d

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

...
Таблица 7.1. Стандартные параметры Денавита–Хартенберга
для робота PUMA 560
θj

dj

aj

αj

σj

q0
q1
q2
q3
q4
q5

0.6718
0
0.1505
0.4318
0
0

0
0.4318
0.0203
0
0
0

90°
0°
-90°
90°
-90°
0°

0
0
0
0
0
0

Одна из самых сложных моделей в Toolbox – робот PR2
(рис. 7.16в). Он имеет мобильное основание, две руки с захватами, множество датчиков с приводом
>>> pr2 = models.URDF.PR2()
ERobot: pr2 (by Willow Garage), 31 joints
(RRRRRRRRRRRRPRRRRRRRRRRRRRRRRRR), 27 branches,
dynamics, geometry, collision
...

и состоит из 31 сочленения, 66 звеньев и 27 ветвей. Его кинематическую структуру можно представить в табличной, графической или визуальной форме:
>>> pr2
>>> pr2.showgraph()
>>> pr2.plot(pr2.qz)

Прямая кинематика 395

7.1.5

Параметры Денавита–Хартенберга

С 1960-х, задолго до появления URDF, модели роботов представлялась в виде простых таблиц чисел, подобной той, что показана в табл. 7.1. Для описания кинематики робота отводилась
одна строка на каждую пару звено–сочленение. Если известны
инерционные параметры звена, их можно добавить в виде дополнительных столбцов.
Эта компактная форма записи известна как нотация Денавита–Хартенберга (Denavit-Hartenberg). Каждая строка таблицы
определяет пространственную связь между системами координат двух последовательных звеньев, как показано на рис. 7.18.
Манипулятор с N сочленениями, пронумерованными от 0 до
N - 1, имеет N + 1 звеньев, пронумерованных от 0 до N. Сочленение j соединяет звено j со звеном j + 1. Следовательно, звено
ℓ соединяет сочленение ℓ - 1 с сочленением ℓ. Звено 0 является
основанием робота, обычно неподвижным, а звено N, последнее звено робота, несет рабочий орган или инструмент.
сочленение j
сочленение j-1

звено j-1

основание

звено j
3
4
инструмент

2

1

Рис. 7.18. Определение стандартных параметров Денавита и Хартенберга. Красный и синий
цвета обозначают все, что связано со звеньями j – 1 и j соответственно. Числа в кружках
обозначают порядок применения элементарных преобразований. xj параллельна zj–1 × zj,
и если эти две оси параллельны, то dj можно выбрать произвольно

Нотация Денавита–Хартенберга с индексацией с нуля
Это описание нотации Денавита–Хартенберга отличается от того,
что приводится в стандартных учебниках, где индексация сочленений и параметров начинается с единицы. В этой книге для индексации сочленений используется нумерация, принятая в языке Python
и начинающаяся с нуля.

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

396 Глава 7 · Кинематика робота-манипулятора

ции Денавита–Хартенберга используется всего четыре парамет­
ра, но есть два ограничения: ось xj пересекает ось zj-1 и перпендикулярна ей. Одно из следствий этих ограничений – иногда
системы координат звеньев располагаются не на физических
звеньях робота. Другое следствие: для назначения систем координат звеньев робот должен быть помещен в определенную
конфигурацию – конфигурацию с нулевым углом, – о которой
рассказывается далее в разделе 7.5.1. В табл. 7.2 приводится
обобщенное описание параметров Денавита–Хартенберга.
Таблица 7.2. Параметры Денавита–Хартенберга: их физический смысл,
обозначение и формальное определение
Угол
сочленения
Смещение
звена
Кручение
звена
Тип
сочленения

θj Угол между осями xj и xj-1 относительно
оси zj-1
dj Расстояние от начала координат {j}
до оси xj+1 вдоль оси zj
αj Угол между осью zj и осью zj+1
относительно оси xj+1
σj σ = R – для поворотного сочленения,
σ = P – для призматического сочленения.
По соглашениям R = 0 и P = 1

Переменная поворотного
сочленения или константа
Переменная призматического
сочленения
Константа
Константа

Система координат {j} прикреплена к дальнему (дистальному) концу звена j. Ось z системы координат {j} совмещена с осью
сочленения j. Преобразование из системы координат звена {j}
в систему координат {j + 1} определяется в терминах элементарных преобразований как
ξj+1 = ξ rz(θj) ⊕ ξ tz(dj) ⊕ ξ tx(aj) ⊕ ξ rx(αj),

j

(7.4)

которую можно разложить как мат­ри­цу SE(3)

(7.5)

Параметры αj и aj постоянны. Для поворотного сочленения θj –
переменная величина и dj – константа, тогда как для призматического сочленения dj – переменная величина, а θj – константа.
В обобщенном виде координаты сочленения определяются как

Поворотное сочленение и звено можно создать так:
>>> link = RevoluteDH(a=1)
RevoluteDH(d=0, a=1, α=0)

Прямая кинематика 397
Отступление 7.4. Денавит и Хартенберг
Жак Денавит и Ричард Хартенберг разработали
многие ключевые концепции кинематики для манипуляторов с последовательным соединением
и опубликовали их в статье (Denavit and Hartenberg
1955), а также в более позднем классическом труде
«Kinematic Synthesis of Linkages» (Hartenberg and
Denavit 1964).
Жак Денавит (1930–2012) родился в Париже, где
получил степень бакалавра, а затем степень магистра и доктора наук в области машиностроения
в Северо-Западном университете, штат Иллинойс.
В 1958 году поступил на кафедру машиностроения
и астронавтики в Северо-Западном университете,
где началось его сотрудничество с Хартенбергом.
Помимо динамики и кинематики, Денавит также интересовался физикой плазмы и кинетикой.
После публикации книги он перешел в Ливерморский университет имени Лоуренса, Калифорния,
где проводил исследования по компьютерному
анализу проблем физики плазмы.
Ричард Хартенберг (1907–1997) родился в Чикаго и получил ученую степень в Висконсинском
университете в Мадисоне. Служил в торговом флоте и два года изучал аэронавтику в Геттингенском
университете у пионера космонавтики Теодора
фон Кармана. Был профессором машиностроения
в Северо-Западном университете, где преподавал
56 лет. Его исследования в области кинематики
привели к возрождению интереса к этой области
в 1960-х, а его усилия помогли создать научную основу для кинематики, которую
можно было использовать в компьютерных приложениях для анализа и проектирования сложных механизмов. Он также много писал об истории машиностроения.

Видоизмененная
форма, модифицированная нотация
Денавита–Хартенберга обсуждается
в разделе 7.5.3
и будет представлена объектом
RevoluteMDH.

В результате получается экземпляр класса RevoluteDH, который наследует универсальный класс DHLink. При отображении
значения объекта выводятся кинематические и динамические
параметры (большинство из которых по умолчанию равны
нулю), а название (Revolute – поворотный) подразумевает тип
сочленения. ◄
Объект DHLink имеет много общих атрибутов и методов с объектом Link, который мы использовали ранее. Например, преобразование (7.5) для q = 0.5 рад является объектом SE3:
>>> link.A(0.5)
0.8776 -0.4794
0.4794
0.8776
0
0
0
0

0
0
1
0

0.8776
0.4794
0
1

398 Глава 7 · Кинематика робота-манипулятора

Прямая кинематика является функцией координат сочленений и представляет собой простую композицию относительного положения каждого звена. В представлении Денавита–Хартенберга звено 0 является основанием робота и обычно
определяется как первое звено d0 = 0, но при желании можно
установить d0 > 0, чтобы обозначить высоту первого сочленения
в мировой системе координат. Для последнего звена (звена N)
параметры dN-1, aN-1 и αN-1 предлагают ограниченную возможность описания положения конца инструмента в системе координат {N}. Однако на практике принято добавлять более общее
преобразование инструмента, как описано в разделе 7.1.2.3.
Описание кинематики многих роботов в нотации Денавита–
Хартенберга можно найти в технических характеристиках производителей и в литературе. Эти описания более компактны,
чем модели URDF, а кроме того, таблиц параметров Денавита–
Хартенберга вполне достаточно для расчета прямой и обратной
кинематики, а также для создания каркасной диаграммы или
анимации, как показано на рис. 7.9. Их также можно использовать для расчета матриц Якоби, которые будут рассматриваться
в главе 8 и вместе с инерционными параметрами могут использоваться для вычисления динамики твердого тела, которая будет рассмотрена в главе 9.
Определение параметров Денавита–Хартенберга для конкретного робота является сложной задачей, однако в пакете
Toolbox имеется множество готовых моделей роботов, описанных в этой нотации, которые можно перечислить командой
>>> models.list(type="DH")
class
name
Panda
panda
Puma560
Puma 560
Stanford
Stanford arm
Ball
ball
Hyper
Hyper10
Coil
Coil10
Cobra600
Cobra600
IRB140
IRB 140
KR5
KR5
Orion5
Orion 5
Planar3
Planar 3 link
Planar2
Planar 2 link
LWR4
LWR-IV
Sawyer
Sawyer

manufacturer
Franka Emika
Unimation
Victor Scheinman

Omron
ABB
KUKA
RAWR Robotics
Kuka
Rethink Robotics

type
DH
DH
DH
DH
DH
DH
DH
DH
DH
DH
DH
DH
DH
DH

DoF
7
6
6
10
10
10
4
6
6
4
3
2
7
6

dims
3d
3d
3d
3d
3d
3d
3d
3d
3d
3d
3d
3d
3d
3d

st
RRR
RRR
RRR
RRR
RRR
RRR
RRR
RRR
RRR
RRR
RRR
RR
RRR
RRR

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

...

Попробуем для примера создать модель популярного промышленного робота на основе нотации Денавита–Хартенберга
>>> irb140 = models.DH.IRB140();

Обратная кинематика 399

Попытка вывести его
>>> irb140
DHRobot: IRB 140 (by
θj
dj
aj
q1 0.352
0.07
q2
0
0.36
q3
0
0
q4
0.38
0
q5
0
0
q6 0.065
0
name
qr
qz
qd

q0
0°
0°
0°

ABB), 6 joints (RRRRRR), dynamics, geometry, ...
αj
q–
q+
-90.0°
-180.0°
180.0°
0.0°
-100.0°
100.0°
-90.0°
-220.0°
60.0°
90.0°
-200.0°
200.0°
-90.0°
-120.0°
120.0°
0.0°
-400.0°
400.0°
q1
-90°
0°
-90.0°

q2
90°
0°
180°

q3
0°
0°
0°

q4
90°
0°
0°

q5
-90°
0°
-90°

отобразит таблицу параметров Денавита–Хартенберга и предельные углы сочленений. Созданный объект – все те же операции, что были представлены ранее. Например, для него можно
вычислить прямую кинематику
>>> irb140.fkine(irb140.qr).printline("rpy/xyz")
t = 0.005, 0, 0.332; rpy/xyz = 0°, -90°, -90°

создать каркасную диаграмму
>>> irb140.plot(irb140.qr);

или интерактивную диаграмму с ползунками
>>> irb140.teach()

Определение робота в нотации Денавита–Хартенберга можно преобразовать в формат ETS:
>>> irb140.ets()
Rz(q0) ⊕ tz(0.352) ⊕ tx(0.07) ⊕ Rx(-90°) ⊕ Rz(gl)
⊕ tx(0.36) ⊕ Rz(q2) ⊕ Rx(-90°) ⊕ Rz(q3) ⊕ tz(0.38)
⊕ Rx(90°) ⊕ Rz(q4) ⊕ Rx(-90°) ⊕ Rz(q5) ⊕ tz(0.065)

7.2

Обратная кинематика

Настоящий практический интерес представляет задача, обратная той, что мы только что обсудили: по заданному положению
рабочего органа ξE определить, какие координаты должны занять сочленения. Например, по известному декартову положению объекта определить соответствующие координаты со­
членений робота. Это – задача обратной кинематики, которая
в функциональной форме записывается как

400 Глава 7 · Кинематика робота-манипулятора

q = 𝒦-1(ξE)

(7.6)

и в общем случае имеет несколько решений, то есть конкретное положение рабочего органа может быть достигнуто при нескольких конфигурациях сочленений.
Для решения задачи обратной кинематики можно использовать два подхода. Первый: аналитическое решение с применением геометрических или алгебраических методов. Однако
с увеличением числа сочленений этот подход становится все
более сложным, а кроме того, для некоторых манипуляторов
решения в аналитическом виде просто не существует. Второй:
использовать итеративное численное решение. В разделе 7.2.1
мы снова используем простой двухмерный случай для иллюст­
рации базовых принципов, а затем в разделе 7.2.2 распространим их на роботизированные манипуляторы, действующие
в трех измерениях.

7.2.1

 вухмерные (планарные) роботизированные
Д
манипуляторы

Попробуем решить задачу обратной кинематики для манипулятора с двумя сочленениями (рис. 7.4б) двумя способами: аналитически и численно.

7.2.1.1

Аналитическое решение

Начнем с символического расчета прямой кинематики и используем SymPy для алгебраических вычислений. Определим
некоторые символические константы, определяющие размеры
робота, а затем создадим ETS (последовательность элементарных преобразований) для робота, показанного на рис. 7.4б, уже
знакомым способом:
>>> import sympy
>>> a1, a2 = sympy.symbols("a1 a2")
>>> e = ET2.R() * ET2.tx(a1) * ET2.R() * ET2.tx(a2);

Далее определим некоторые символические переменные для
представления углов сочленений
>>> q0, q1 = sympy.symbols("q0 q1")

и вычислим прямую кинематику как мат­ри­цу SE(2)
>>> TE = e.fkine([q0, q1])
cos(q0 + q1) -sin(q0 + q1)
sin(q0 + q1) cos(q0 + q1)
0
0

a1*cos(q0) + a2*cos(q0 + q1)
a1*sin(q0) + a2*sin(q0 + q1)
1

Обратная кинематика 401

Это алгебраическое представление прямой кинематики робота – положения рабочего органа как функции переменных
сочленений. Мы будем работать только с координатами рабочего органа:
>>> x_fk, y_fk = TE.t;

Наконец, определим еще две символические переменные
для представления желаемых координат рабочего органа:
>>> x, y = sympy.symbols("x y")

Теперь у нас есть два уравнения
xfk = x, yfk = y
с двумя неизвестными (q0 и q1), но пакет SymPy не может решать такие тригонометрические уравнения напрямую – мы
должны ему немного помочь. Для решения подобных задач
часто бывает полезно возвести оба уравнения в квадрат и сложить их: x 2fk + y 2fk = x2 + y2. Это получившееся уравнение можно
переписать как
x 2fk + y 2fk - x2 - y2 = 0
SymPy представля- и выразить в программном коде :
ет уравнение как
выражение, которое >>> eq1 = (x_fk**2 + y_fk**2 - x**2 - y**2).trigsimp()
неявно равно нулю. a1**2 + 2*a1*a2*cos(q1) + a2**2 - x**2 - y**2

Теперь у нас есть уравнение с одним неизвестным, q1, и мы
можем найти решение для eq1 = 0
>>> q1_sol = sympy.solve(eq1, q1)
[-acos(-(a1**2 + a2**2 - x**2 - y**2)/(2*a1*a2)) + 2*pi,
acos((-a1**2 - a2**2 + x**2 + y**2)/(2*a1*a2))]

Диапазон значений
cos–1 ограничен
квадрантами 1 и 2.

которое представляет собой список. В разделе 7.1.1 мы видели, как две различные конфигурации сочленений могут давать
одинаковое положение рабочего органа, и здесь как раз тот самый случай, когда мы получаем решения, соответствующие положительному или отрицательному значению q1 .
Чтобы найти q0, сначала разложим два уравнения, упомянутых выше:
>>> eq0 = tuple(map(sympy.expand_trig, [x_fk - x, y_fk - y]))
(a1*cos(q0) + a2*(-sin(q0)*sin(q1) + cos(q0)*cos(q1)) - x,
a1*sin(q0) + a2*(sin(q0)*cos(q1) + sin(q1)*cos(q0)) - y)

и решим их для sin(q0) и cos(q0):
>>> q0_sol = sympy.solve(eq0, [sympy.sin(q0), sympy.cos(q0)]);

402 Глава 7 · Кинематика робота-манипулятора
Отступление 7.5. Сферическое запястье
Сферическое запястье – ключевой компонент
Вращение фланца
(сочленение 6)
многих современных роботов-манипуляторов. Запястье имеет три оси вращения, взаСгибание запястья
имно перпендикулярные и пересекающиеся
(сочленение 5)
в одной точке. Это механизм, подобный карданному подвесу, и, как обсуждалось в разделе 2.3.1.2, он обладает сингулярностью.
Для удобства математических вычислений
Вращение запястья
положение рабочего органа робота, его коор(сочленение 4)
динаты и ориентация, определяется центром
запястья. Поскольку оси запястья пересекаются в одной точке, движение запястья
не вызывает поступательного движения, поэтому положение рабочего органа зависит только от первых трех сочленений. Это существенное упрощение позволяет
находить аналитические решения обратной кинематики для шестиосевых промышленных роботов. Произвольная ориентация рабочего органа достигается независимо от его положения с помощью трех запястных сочленений.

В результате получим словарь Python, содержащий синус
и косинус значения q0. Их отношение равно tan(q0).
>>> sympy.atan2(q0_sol[sympy.sin(q0)],
...
q0_sol[sympy.cos(q0)]).simplify()
atan2((a1 * y - a2 * x * sin(q1) + a2 * y * cos(q1)) /
(a1**2 + 2 * a1 * a2 * cos(q1) + a2**2),
(a1 * x + a2 * x * cos(q1) + a2 * y * sin(q1)) /
(a1**2 + 2 * a1 * a2 * cos(q1) + a2**2))

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

7.2.1.2

Численное решение

Задачу обратной кинематики можно представить как задачу
корректировки координат сочленений до тех пор, пока прямая
кинематика не будет соответствовать желаемому положению.
Более формально эта задача называется задачей оптимизации,
потому что ее цель – минимизировать ошибку между решением прямой кинематики и желаемым положением рабочего органа ξE∗:
q∗ = argmin
|| 𝒦  (q) ⊝ ξ ∗E||.
q

SymPy – это
стандартный
инструмент во
вселенной Python,
но он сравнительно
слаб в сложных
тригонометрических
выражениях.

Обратная кинематика 403

Снова воспользуемся роботом на рис. 7.4б
>>> e = ET2.R() * ET2.tx(1) * ET2.R() * ET2.tx(1);

и определим функцию ошибки, основанную на ошибке местоположения рабочего органа, но без учета ориентации
E(q) = ||[𝒦(q)]t - (x∗, y∗)T||,

которую можно реализовать как лямбда-функцию:
>>> pstar = np.array([0.6, 0.7]); # desired position
>>> E = lambda q: np.linalg.norm(e.fkine(q).t - pstar);

Выполнить минимизацию можно с помощью многопарамет­
рической функции из SciPy
>>> sol = optimize.minimize(E, [0, 0]);

аргументами которой являются функция стоимости и начальная оценка координат сочленения. В результате мы получили следующие углы сочленений, минимизирующие функцию
стои­мости:
>>> sol.x
array([ -0.2295,

2.183])

и следующую прямую кинематику:
>>> e.fkine(sol.x).printline()
t = 0.6, 0.7. 112°

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

7.2.2

Трехмерные роботизированные манипуляторы

7.2.2.1

Аналитическое решение

Аналитические решения были разработаны для большинства
распространенных типов шестиосевых промышленных роботов,
определяемых, как правило, в нотации Денавита–Хартенберга.
Необходимым условием существования аналитического решения для шестиосевого робота является наличие сферического
механизма запястья. Некоторые модели роботов, входящие в пакет Toolbox, имеют замкнутую инверсную кинематику, и мы проиллюстрируем это на примере классического робота PUMA 560
>>> puma = models.DH.Puma560();

404 Глава 7 · Кинематика робота-манипулятора

В номинальных координатах сочленений, показанных на
рис. 7.19а:
>>> puma.qn
array([ 0, 0.7854, 3.142, 0, 0.7854, 0])

положение рабочего органа определяется как
>>> T = puma.fkine(puma.qn);
>>> T.printline()
t = 0.596, -0.15, 0.657; rpy/zyx = 0°, 90°, 0°

а

б

в

г

Рис. 7.19. Различные конфигурации робота PUMA 560: а) влево – вверх – без переворота;
б) вправо – вверх – без переворота (номинальная конфигурация qn); в) влево – вниз – без
переворота; г) вправо – вниз – без переворота

PUMA 560 – это шестиосевой роботизированный манипулятор со сферическим запястьем, поэтому для него существует
аналитическое решение задачи обратной кинематики.
>>> sol = puma.ikine_a(T)
IKSolution: q=[2.649, 2.356, 0.09396, -0.609, -0.9743, -2.768],
success=True

Результат содержит информацию о состоянии, указывающую, что решение найдено, и конфигурацию сочленений в виде
массива NumPy. Удивительно, но решение

Обратная кинематика 405
>>> sol.q
array([ 2.649, 2.356, 0.09396, -0.609, -0.9743, -2.768])

сильно отличается от начальных координат сочленений, тем не
менее найденное решение верное, в чем легко убедиться:
>>> puma.fkine(sol.q).printline()
t = 0.596, -0.15, 0.657; rpy/zyx = 0°, 90°, 0°

Эти две различные конфигурации соединений показаны на
рис. 7.19а,б.
Робот PUMA 560 отличается от других роботов тем, что его
плечевой сустав смещен по горизонтали относительно талии,
поэтому в одном решении рука находится слева от талии (как
наша левая рука), в другом – справа (как наша правая рука). Эти
конфигурации называются левосторонней и правосторонней
кинематикой соответственно. В общем случае существует восемь различных групп координат сочленений, которые дают
одно и то же положение рабочего органа. Мы можем форсировать выбор правостороннего решения

Современные роботы обычно не имеют
бокового смещения
плечевого сустава,
тем не менее эти
два решения все
еще существуют,
просто они менее
очевидны. В этом
случае робот поворачивается на 180° >>> sol = puma.ikine_a(T, "r");
лицом назад, затем >>> sol.q
поворачивает плечо array([
0, 0.7854, -3.142, -3.142, -0.7854,
3.142])
через верх, чтобы
переместить рабои получить исходную конфигурацию сочленений, причем
чий орган вперед.
Точнее, локоть
находится выше или
ниже плеча.
Для двупальцевого
захвата переворот
не оказывает
функционального
влияния
на положение
пальцев.

отметьте, что для q2 значения π и -π эквивалентны.
Помимо лево- и правосторонних решений, существуют решения с локтем, направленным вверх или вниз, и с перевернутым или неперевернутым запястьем. Первое запястное со­
членение q3 робота PUMA 560 может поворачиваться в больших
пределах и принимать один из двух углов, различающихся на
π радиан.
На рис. 7.19 показаны четыре различных кинематических
решения. Конфигурация сочленений, возвращаемая функцией
ikine_a, управляется одним или несколькими символами-флагами:
лево- или правостороннее решение
локоть вверх или вниз
запястье перевернуто или нет

"l", "r"
"u", "d"
"f", "n"

которые передаются как необязательный строковый аргумент.
Из-за механических ограничений на углы поворота сочленений и возможных столкновений между звеньями не все восемь
решений физически достижимы. Может также случиться, что ни
одно решение не может быть достигнуто. Например, решение
>>> puma.ikine_a(SE3.Tx(3))
IKSolution: q=None, success=False, reason=Out of reach

406 Глава 7 · Кинематика робота-манипулятора

отсутствует просто потому, что манипулятор недостаточно
длинный, чтобы достичь заданного положения.
Положение также может быть недостижимо из-за сингулярности, когда совмещение осей уменьшает эффективные степени свободы (все та же проблема блокировки карданного подвеса). Одна из особенностей робота PUMA 560 – сингулярность
запястья, когда q4 равно нулю и оси сочленений 3 и 5 совмещаются. В этом случае самое большее, что может сделать функция ikine_a, – ограничить сумму q3 + q5, но их индивидуальные
значения могут быть любыми. Например, рассмотрим конфигурацию

Запястье PUMA – это
последовательность
углов Эйлера ZXZ,
как обсуждалось
в разделе 2.3.1.2.

>>> q = [0, pi/4, pi, 0.1, 0, 0.2];

Решение обратной кинематики
>>> puma.ikine_a(puma.fkine(q), "ru").q
array([
0, 0.7854, -3.142,

0,

0,

0.3])

дает совершенно другие значения для q3 и q5, но в обоих случаях
сумма q3 + q5 = 0.3.

7.2.2.2

Численное решение

Для роботов с другим количеством сочленений, не имеющих
сферического запястья или для которых аналитическое решение отсутствует, необходимо использовать итеративное численное решение. Продолжим пример из предыдущего раздела
и используем метод ikine_LM, чтобы получить численное решение задачи обратной кинематики ►:
>>> T = puma.fkine(puma.qn);
>>> T.printline("rpy/xyz")
t = 0.596, -0.15, 0.657; rpy/xyz = 0°, 90°, 0°
>>> sol = puma.ikine_LM(T, q0=[0, 0, 0, 0, 0, 0])
IKSolution: q=[0, -0.8335, 0.09396, 0, -0.8312, 0],
success=True, iterations=5, searches=1,
residual=4.15e-09

Результат сообщает об успехе и был получен всего после
5 итераций с очень небольшой ошибкой, или невязкой. Однако
конфигурация сочленений отличается от исходной:
>>> puma.qn
array([

0,

0.7854,

3.142,

0,

0.7854,

0])

но обеспечивает правильное положение рабочего органа
>>> puma.fkine(sol.q).printline("rpy/xyz")
t = 0.596, -0.15, 0.657; rpy/xyz = 0°, 90°, 0°

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

Обратная кинематика 407

Нарисовав диаграмму
>>> puma.plot(sol.q);

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

Нулевые значения
не всегда являются
хорошим выбором.
Если значение q0 не
>>> puma.ikine_LM(T, q0=[0, 0, 3, 0, 0, 0])
задано, то функция
выберет случайные IKsolution(q=array([ 0, 0.7854, -3.142, 0, 0.7854, 0]),
success=True, reason=None, iterations=10,
начальные значения
и будет продолжать
residual=2.450e-11)
попытки, пока не
найдет решение.
то можно подтолкнуть оптимизатор к конфигурации «локоть
Аргумент seed
можно использовать вверх». Как и ожидалось, численный подход ikine_LM значительдля инициализации но медленнее аналитического подхода ikine_a. Однако он облагенератора случайдает существенным преимуществом: позволяет работать с маных чисел, чтобы
обеспечить повторя- нипуляторами с сингулярностью и с манипуляторами с числом
емость результатов. сочленений менее или более шести. Принципы, лежащие в ос-

нове ikine_LM, обсуждаются в разделе 8.5.

7.2.3

Малоприводный манипулятор

Малоприводный манипулятор – это манипулятор, имеющий
менее шести сочленений. Типичным примером могут служить
роботы SCARA (рис. 7.2б). Они имеют структуру сочленений
RRPR, оптимизированную для задач планарной сборки, требующих управления положением рабочего органа в трехмерном
пространстве и ориентацией в плоскости xy. Пространство задач
𝒯 ⊂ ℝ3 × S1, а конфигурационное пространство 𝒞 ⊂  (S1)2 × ℝ × S1.
Поскольку размерность 𝒞 < 6, робот ограничен и рабочий орган
робота может принимать не все положения. Загрузим для примера модель робота Omron Cobra 600 SCARA.
>>> cobra = models.DH.Cobra600()
DHRobot: Cobra600 (by Omron), 4 joints (RRPR), dynmics, standard ...
αj
θj
dj
aj
q–
q+
q1
0.387
0.325
0.0°
-50.0°
50.0°
q2
0
0.275
180.0°
-88.0°
88.0°
0.0°
q3
0
0.0°
0.0
0.21
q4
0
0
0.0°
-180.0°
180.0°

408 Глава 7 · Кинематика робота-манипулятора
name
qz
qr

q0
0°
0°

q1
0°
0.0°

q2
0
0

q3
0°
0°

и зададим желаемое положение рабочего органа
>>> TE = SE3.Trans(0.4, -0.3, 0.2)
...
* SE3.RPY(np.deg2rad([30, 0, 170]), order="xyz");

где вектор рабочего органа направлен вниз, но не параллелен
вертикальной оси – он отклонен от вертикали на 10°. Это положение недостижимо для робота SCARA с четырьмя сочленениями. Робот физически не может удовлетворить требования
к ориентации рабочего органа, отличной от вертикальной,
и попытка вычислить решение обратной кинематики
>>> sol = cobra.ikine_LM(TE, seed=0)
IKSolution: q=[-0.111, -1.176, 0.187, -0.7634], success=False,
reason=iteration and search limit reached, iterations=3000,
searches=100, residual=0.0152

терпит неудачу. Мы должны ослабить требования и игнорировать ошибку вращения вокруг осей x и y. Это достигается указанием вектора маски.
>>> sol = cobra.ikine_LM(TE, mask=[1, 1, 1, 0, 0, 1], seed=0)
IKSolution: q=[-0.111, -1.176, 0.187, -0.7634],
success=True, iterations=8, searches=1,
residual=7.82e-10

Элементы вектора маски соответствуют трем перемещениям
и трем ориентациям: tx, ty, tz, rx, ry, rz в системе координат рабочего органа. В этом примере мы указали, что ошибка вращения
вокруг осей x и y должна игнорироваться (заданы нулевые элементы). Полученные углы в сочленениях соответствуют достижимому положению рабочего органа.
>>> cobra.fkine(sol.q).printline("rpy/xyz")
t = 0.4, -0.3, 0.2; rpy/xyz = 30°, 0°, 180°

для которого заданы желаемые величина перемещения и угол
крена, но угол рыскания получился ошибочным, как мы и допус­
кали. Именно это отклонение допускает механизм робота. Мы
также можем графически сравнить желаемые и достижимые
положения.
>>> TE.plot(color="r");
>>> cobra.fkine(sol.q).plot(color="b");

Траектории 409

7.2.4

Многоприводный манипулятор

Многоприводный манипулятор – это манипулятор, имеющий
более шесть сочленений. Как упоминалось выше, теоретически
шести сочленений достаточно для достижения любого желаемого положения в декартовом пространстве задач 𝒯 ⊂ ℝ3 × S3.
На практике такие проблемы, как ограничения сочленений,
столкновения звеньев друг с другом и сингулярность, приводят
к тому, что возможными оказываются не все положения в пределах достижимого пространства робота. Добавление дополнительных сочленений – это один из способов решения данной
проблемы, но результатом этого является бесконечное число
решений. Чтобы найти единственное решение, необходимо
ввести ограничения. Одно из таких ограничений – ограничение
минимальной нормы, которое возвращает решение, в котором
норма вектора координат сочленений имеет наименьшую величину.
Проиллюстрируем сказанное на примере робота Panda
(рис. 7.3а), который имеет семь сочленений. Загрузим модель
>>> panda = models.ETS.Panda();

и зададим желаемое положение рабочего органа
>>> TE = SE3.Trans(0.7, 0.2, 0.1) * SE3.OA((0, 1, 0), (0, 0, -1));

вектор подхода которого направлен вниз. Численным решением задачи обратной кинематики
>>> sol = panda.ikine_LM(TE, seed=0)
IKSolution: q=[0.4436, 1.388, 2.532, 0.4795, -2.528, 1.783,
-2.186], success=True, iterations=37, searches=2,
residual=9.34e-07

является вектор конфигурации сочленений с наименьшей нормой, которая дает желаемое положение рабочего органа, в чем
легко убедиться:
>>> panda.fkine(sol.q).printline("angvec")
t = 0.7, 0.2, 0.1; angvec = (180° | 0, 1, 0)

7.3

Траектории

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

410 Глава 7 · Кинематика робота-манипулятора

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

7.3.1

Движение в пространстве сочленений

Рассмотрим перемещение рабочего органа между двумя положениями
>>> TE1 = SE3.Trans(0.4, -0.2, 0) * SE3.Rx(3);
>>> TE2 = SE3.Trans(0.4, 0.2, 0) * SE3.Rx(1);

описывающими две точки в плоскости xy с различными ориентациями рабочего органа. Конфигурации сочленений в этих
положениях ►:
>>> sol1 = puma.ikine_a(TE1, "ru");
>>> sol2 = puma.ikine_a(TE2, "ru");

Обратите внимание, что мы потребовали рассчитать конфигурацию с локтем, повернутым вправо и поднятым вверх. Кроме того, нам нужно, чтобы движение продолжалось в течение
2 секунд с шагом 20 мс, поэтому создадим массив значений
времени
>>> t = np.arange(0, 2, 0.02);

Траектория в пространстве сочленений формируется путем интерполяции между двумя конфигурациями сочленений.
В комбинации с функцией многоосевого драйвера mtraj можно
использовать функции скалярной интерполяции quintic или
trapezoidal, представленные в разделе 3.3.1:
>>> traj = mtraj(quintic, sol1.q, sol2.q, t);

или
>>> traj = mtraj(trapezoidal, sol1.q, sol2.q, t);

В результате получается экземпляр Trajectory и массив 100×6
с координатами сочленений
>>> traj.q.shape
(100, 6)

в котором каждая строка соответствует временному шагу, а каждый столбец – сочленению. Объект Trajectory также содержит

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

Траектории 411

в свойствах qd и qdd векторы скорости и ускорения сочленений

Она эквивалентна как функцию времени. С этого момента мы будем использовать
вызову функции удобную эквивалентную функцию jtraj :
mtraj с интерполяцией quinitic, но
>>> traj = jtraj(sol1.q, sol2.q, t)
оптимизирована
для многоосевого Trajectory created by jtraj: 100 time steps x 6 axes
случая, а также
Последним аргументом в вызовах mtraj и jtraj может быть
позволяет задавать
начальную и ко- вектор с отметками времени, как здесь, или целое число, опренечную скорости
в дополнительных деляющее количество временных шагов.
Траекторию лучше рассматривать как анимацию
аргументах.

>>> puma.plot(traj.q);

но можем построить график зависимости углов в сочленениях
от времени (рис. 7.20а):
>>> xplot(t, traj.q);
0.6
0.5
0.4
1
q0
q1
q2
q3
q4
q5

0

−1

0.3

Координаты (м)

Координаты сочленения (рад, м)

2

0.2
0.1
0.0

−2

−0.1

0.00

а

x
y
z

−0.2

−3
0.25

0.50

0.75

1.00

Время (с)

1.25

1.50

1.75

б

0.00

0.25

0.50

0.75

1.00

Время (с)

1.25

1.50

0.15

2.00

крен
тангаж
рыскание

3.0

0.20

1.75

2.5

0.10
2.0
RPY angles (rad)

y (м)

0.05
0.00
−0.05
−0.10

1.5

1.0

0.5

−0.15
0.0
−0.20

в

0.2

0.3

0.4

x (м)

0.5

0.6

0.7

г

0.00

0.25

0.50

0.75

1.00

Время (с)

1.25

1.50

1.75

2.00

Рис. 7.20. Движение в пространстве сочленений: а) зависимость координат сочленений от времени; б) декартовы
координаты в зависимости от времени; в) декартовы координаты в плоскости xy; г) углы крена, тангажа и рыскания
в зависимости от времени

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

412 Глава 7 · Кинематика робота-манипулятора

Это легко определить, решив задачу прямой кинематики для
координат сочленений
>>> T = puma.fkine(traj.q);
>>> len(T)
100

В результате получаем экземпляр SE3 со 100 значениями.
Часть этой траектории, описывающая перемещение
>>> p = T.t;
>>> p.shape
(100, 3)

представлена массивом, в котором каждая строка соответствует одному временному шагу, а каждый столбец – одной координате. Этот массив можно отобразить как график зависимости
координат от времени (рис. 7.20б)
>>> xplot(t, Tt, labels="xy z");

Путь рабочего органа в плоскости xy показан на рис. 7.20в.
Как видите, траектория не является прямой линией. Этого
следовало ожидать, потому что мы указали только декартовы
координаты конечных точек. При повороте робота вокруг поясного сочленения во время движения конечный эффектор
естественным образом будет описывать дугу окружности. На
практике это может привести к столкновениям робота с близлежащими объектами, даже если они не находятся на прямой
между начальной и конечной позициями. Также можно отобразить в зависимости от времени ориентацию рабочего органа
в виде углов крена, тангажа и рыскания по осям XYZ
>>> xplot(t, T.rpy("xyz"), labels="roll pitch yaw");

как показано на рис. 7.20г. Обратите внимание, что угол крена
► изменяется от 3 до 1 радиана, как мы и указали. Углы крена
и тангажа соответствуют граничным условиям, но не остаются
постоянными и отклоняются вдоль траектории.

7.3.2

Движение в декартовом пространстве

Во многих применениях требуется организовать прямолинейное перемещение в декартовом пространстве. Такое перемещение реализуется с помощью функции ctraj из пакета Toolbox,
представленной в разделе 3.3.5. Она применяется практически
так же, как функция jtraj
>>> Ts = ctraj(TE1, TE2, t);

Поворот вокруг
оси x рабочего
органа робота
определяется
с использованием
последовательности
углов XYZ (см. раздел 2.3.1.2).

Траектории 413

– принимает начальное и конечное положения, а также вектор
с отметками времени и возвращает траекторию в виде экземпляра SE3. Как и в предыдущем примере с пространством сочленений, построим графики перемещения
>>> xplot(t, Ts.t, labels="x y z");

и изменения ориентации
>>> xplot(t, Ts.rpy("xyz"), labels="roll pitch yaw");

которые показаны на рис. 7.21б,г.

При решении
Как можно заметить на рис. 7.21в, траектория в пространстве
задачи обратной
кинематики с ис- задачи теперь представляет собой прямую линию. Соответствупользованием
численного подхода ющую ей траекторию в пространстве сочленений можно полурешение, найден- чить применением обратной кинематики (рис. 7.21а) :
ное для одной
точки траектории, >>> qc = puma.ikine_a(Ts);
используется как
начальное значение
Хотя в целом рис. 7.21а похож на рис. 7.20а, небольшие разлидля поиска решения
чия
определяют разницу между изогнутой и прямой линиями
для следующей
точки. в пространстве задач.
0.6
0.5
0.4

1
q0
q1
q2
q3
q4
q5

0

−1

0.2
0.1
0.0

−2

−0.1

x
y
z

−0.2

−3
0.00

а

0.3

Координаты (м)

Координаты сочленения (рад, м)

2

0.25

0.50

0.75

1.00

Время (с)

1.25

1.50

1.75

б

0.20

0.00

0.25

0.50

0.75

1.00

Время (с)

1.25

1.50

1.75

2.00

крен
тангаж
рыскание

3.0

0.15
Углы крена, тангажа, рыскания (рад)

2.5
0.10

y (м)

0.05
0.00

−0.05
−0.10

2.0

1.5

1.0

0.5
−0.15
0.0

−0.20

в

0.380

0.385

0.390

0.395

0.400
x (м)

0.405

0.410

0.415

0.420

г

0.00

0.25

0.50

0.75

1.00

Время (с)

1.25

1.50

1.75

2.00

Рис. 7.21. Движение в декартовом пространстве: а) зависимость координат сочленений от времени; б) декартовы
координаты в зависимости от времени; в) декартовы координаты в плоскости xy; г) углы крена, тангажа и рыскания
в зависимости от времени

414 Глава 7 · Кинематика робота-манипулятора

7.3.3

Представление кинематики в виде блок-схемы

Этот пример можно также реализовать в среде блок-схем bdsim.
>>> %run -m jointspace -H

Блок-схема показана на рис. 7.22. Параметры блока Jtraj –
начальные и конечные значения конфигурации сочленений.
Плавно изменяющиеся углы сочленений передаются в блок Armplot, создающий анимацию робота в отдельном окне, и в блок
Fkine. Оба блока имеют параметр – экземпляр подкласса Robot,
в данном случае класса models.DH.Puma560. Декартовы координаты рабочего органа извлекаются блоком translation, который
действует подобно функции transl или свойству .t объекта SE3.
Блок SCOPEXY строит графики одного входного сигнала относительно другого.

Блок Armplot

Блок JTraj

Блок Main

Блок Fkine

Блок
translation

перемещение
в плоскости XZ

Рис. 7.22. Блок-схема модели models/jointspace для представления движения
в пространстве сочленений

7.3.4

Движение через сингулярность

В разделе 7.2.2.1 мы кратко коснулись темы сингулярности
и еще вернемся к ней в главе 8. В следующем примере мы намеренно выберем траекторию, проходящую через сингулярность
запястья робота. Повторим предыдущий пример, но выберем
другие начальное и конечное положения:
>>> TE1 = SE3.Trans(0.5, -0.3, 1.12) * SE3.OA((0, 1, 0), (1, 0, 0));
>>> TE2 = SE3.Trans(0.5, 0.3, 1.12) * SE3.OA((0, 1, 0), (1, 0, 0));

В результате получим движение вдоль y, при этом ось z рабочего органа направлена вдоль оси x мировой системы координат. Найдем декартову траекторию
>>> Ts = ctraj(TE1, TE2, t);

Траектории 415

и преобразуем ее в координаты сочленений
>>> sol = puma.ikine_a(Ts, "lu");

для левосторонней конфигурации с локтем, направленным
вверх, и выведем график (рис. 7.23а):
>>> xplot(t, sol.q, unwrap=True);

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

Мы использовали параметр unwrap, чтобы устранить отвле­
кающие скачки при прохождении углов через ±π.
В момент времени t ≈ 1.3 с наблюдается очень быстрое изменение углов в лучезапястных суставах q3 и q5. В этот момент
q4 близок к нулю, что означает почти полное совмещение осей
поворота запястья q3 и q5 – это еще одна ситуация блокировки
карданного подвеса, или сингулярность, в которой робот теряет
одну степень свободы и теперь фактически является пятиосевым роботом.
Как обсуждалось в разделе 7.2.2.1, такое совмещение осей
озна­чает, что решение возможно только для суммы q3 + q5 и, соответственно, существует бесконечное число решений для q3
и q5, дающих искомую сумму. На рис. 7.23б видно, что численный метод обратной кинематики ikine_LM обрабатывает сингулярность, избегая ненужного движения сочленений. Это является следствием решения с минимальной нормой, которое
вернуло значения q3 и q5, дающие искомую сумму при наименьшей норме. Движение в пространстве сочленений между двумя
положениями (рис. 7.23в) не подвержено этой проблеме, потому что в этом случае не используется обратная кинематика. Однако в этом случае ориентация инструмента вдоль оси x будет
поддерживаться не на всем протяжении траектории, а только
в двух конечных точках.
Ловкость манипулятора определяется легкостью, с какой он
может перемещать или поворачивать рабочий орган относительно любой оси. Распространенной скалярной мерой ловкости является управляемость, которую можно вычислить для
каждой точки траектории (рис. 7.23г).
>>> m = puma.manipulability(sol.q);

Управляемость в точке t = 1.3 с для траектории, вычисленной с использованием аналитической обратной кинематики
(красная кривая), была практически нулевой, что указывает
на значительную потерю ловкости. Следствием является очень
быстрое движение лучезапястного сустава, показанное на
рис. 7.23a, необходимое, чтобы обеспечить перемещение рабочего органа вдоль траектории. Численное решение, напротив,
позволило обеспечить высокую управляемость на протяжении
всей траектории, потому что этот подход неявно минимизирует

416 Глава 7 · Кинематика робота-манипулятора

скорость всех суставов. Управляемость и числовое решение обратной кинематики, реализованное в функции ikine_LM, основаны на мат­ри­це Якоби манипулятора, которая рассматривается
в главе 8.

3
0.5

1
0
−1
−2
−3
−4

а

Координаты сочленения (рад, м)

Координаты сочленения (рад, м)

2

−5
0.00

q0
q1
q2
q3
q4
q5
0.25

0.0

−0.5

q0
q1
q2
q3
q4
q5

−1.0

−1.5
0.50

0.75

1.00

Время (с)

1.25

1.50

1.75

б

0.00

0.25

0.50

0.75

1.00

Время (с)

1.25

1.50

1.75

0.10
2

0

−1

−2

−3

в

Манипулируемость

Координаты сочленения (рад, м)

0.08
1

0.00

q0
q1
q2
q3
q4
q5
0.25

0.06

Аналитическое решение IK
Числовое решение IK

0.04

0.02

0.00
0.50

0.75

1.00

Время (с)

1.25

1.50

1.75

г

0.00

0.25

0.50

0.75

1.00

Время (с)

1.25

1.50

1.75

2.00

Рис. 7.23. Углы в сочленениях для траектории через сингулярность запястья: а) траектория в декартовом
пространстве, вычисленная с использованием аналитической обратной кинематики (ikine_a): б) траектория
в декартовом пространстве, вычисленная с использованием численной обратной кинематики (ikine_LM);
в) движение в пространстве сочленений (jtraj); г) управляемость для двух траекторий в декартовом пространстве

7.3.5

Изменение конфигурации

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

Применение 417

рации в декартовом пространстве. Для этого необходимо использовать движение в пространстве сочленений.
Например, чтобы переместить руку робота из правосторонней в левостороннюю конфигурацию, сначала определим положение конечного исполнительного органа:
>>> TE = SE3.Trans(0.4, 0.2, 0.6) * SE3.Rx(pi);

затем – конфигурацию сочленений для право- и левосторонней
ориентаций с локтем вверх:
>>> sol_r = puma.ikine_a(TE, "ru");
>>> sol_l = puma.ikine_a(TE, "lu");

и, наконец, определим траекторию в пространстве сочленений
этими двумя векторами координат:
>>> traj = jtraj(sol_r.q, sol_l.q, t);

Хотя начальное и конечное положения рабочего органа одинаковы, робот совершает довольно значительные движения в пространстве сочленений, что очень хорошо иллюстрирует анимация.
>>> puma.plot(traj.q);

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

7.4

Применение

7.4.1

Создание надписи на поверхности

Разработан Давайте решим задачу создания траектории, которая позволит
доктором роботу нарисовать букву. В пакете Toolbox имеется перерабоАлленом В. Херши
в Лаборатории танная версия шрифта Hershey
военно-морского вооружения >>> font = rtb_load_jsonfile("data/hershey.json");
в 1967 году. Подробности см. по в форме словаря дескрипторов символов. Символы в этом слоадресу: https:// варе сами являются словарями. Вот, например, как определена
paulbourke.net/
заглавная буква «B»:
dataformats/
hershey.

>>> letter = font["B"]
{'strokes': [[(0.16, 0.12), (0.16, -0.72)],
[(0.16, 0.12), (0.52, 0.12), ... ],
[(0.16, -0.28), (0.52, -0.28), ...
(0.16, -0.72)]],
'width': 0.84, 'top': 0.72, 'bottom': -0.12}

418 Глава 7 · Кинематика робота-манипулятора

Это шрифт переменной ширины, и все символы помещаются
в сетку с единичным размером ячеек. Ширина данного символа
составляет 0.84.
Элемент "strokes" – это список списков кортежей, определяющих координаты x и y точек на плоскости для каждого штриха
символа. Выполним некоторую дополнительную обработку:
>>>
>>>
>>>
>>>
...
...
...
...

lift = 0.1; # высота подъема пера
scale = 0.25;
via = np.empty((0, 3));
for stroke in letter["strokes"]:
xyz = np.array(stroke) * scale # преобразовать штрих в массив nx2
xyz = np.pad(xyz, ((0, 0), (0, 1))) # добавить третий столбец, z=0
via = np.vstack((via, xyz)) # добавить строки в точки via
via = np.vstack((via, np.hstack([xyz[-1,:2], lift]))) # поднять

чтобы создать массив точек via, по одному для каждой строки.
Масштабируем символ умножением на 0.25, чтобы его высота
была около 20 см, и добавляем третий столбец с нулевыми значениями для письма и значениями lift там, где нужно поднять
перо. Затем преобразуем полученный результат в непрерывную траекторию
>>> xyz_traj = mstraj(via, qdmax=[0.5, 0.5, 0.5], q0=[0, 0, lift],
...
dt=0.02, tacc=0.2).q;

где аргументами являются массив промежуточных точек via,
максимальная скорость перемещения вдоль осей x, y и z, начальная точка над символом, интервал дискретизации и время
ускорения. Количество шагов в интерполированном пути равно
>>> len(xyz_traj)
500

и потребуется
>>> len(xyz_traj) * 0.02
10

секунд для его преодоления с интервалом дискретизации 20 мс.
Траекторию (рис. 7.24) можно нарисовать так:
>>> fig = plt.figure(); ax = fig.add_subplot(111, projection="3d");
>>> plt.plot(xyz_traj[:,0], xyz_traj[:,1], xyz_traj[:,2]);

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

Применение 419

Рис. 7.24. Траектория движения
рабочего органа (пера),
рисующего букву «B»
Ориентация
продольной оси
пера в данном
случае не имеет
значения.

может быть произвольной, поэтому выберем выравнивание
по оси y, то есть o = [0, 1, 0]. Символ будет помещен в рабочее
пространство в координаты (0.6, 0, 0), соответственно:
>>> T_pen = SE3.Trans(0.6, 0, 0.7) * SE3.Trans(xyz_traj)
...
* SE3.OA( [0, 1, 0], [0, 0, -1]);

Теперь применим обратную кинематику
>>> puma = models.DH.Puma560();
>>> sol = puma.ikine_a(T_pen, "lu");

чтобы определить координаты сочленений и анимировать их
движение:
Мы не учли силу, >>> puma.plot(sol.q);
с которой робот
Запустите пример командой
давит на бумагу.
Управление этой
силой мы рас- >>> %run -m writing
смотрим в главе 9.
Одно очень простое и понаблюдайте, как робот рисует букву «B», поднимая перо
решение заключается в исполь- при переходе от штриха к штриху. Реализованный нами подход
зовании пружины, довольно общий, и мы могли бы легко изменить размер буквы,
прижимающей перо написать целое слово и даже предложение, нанести надпись на
к бумаге с достаточной силой, чтобы произвольной плоскости или использовать робота с совершенона могла писать. но иной кинематикой.

7.4.2

Четвероногий шагающий робот

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

420 Глава 7 · Кинематика робота-манипулятора

Кинематически нога робота аналогична его руке. Для этого
применения достаточно манипулятора с последовательными
звеньями, соединенными тремя сочленениями, поскольку стопа имеет точечный контакт с землей и ее ориентация не важна. Как всегда, начнем с определения системы координат. Она
показана на рис. 7.25 вместе с ногой робота в конфигурации
с нулевым углом. Мы выбрали систему координат с осью x, направленной вперед, осью z, направленной вверх, и осью y, направленной влево. Размеры ноги:
>>> mm = 0.001; # миллиметры
>>> L1 = -100 * mm;
>>> L2 = -100 * mm;
Перед
z

x

y
Зад

Рис. 7.25. Система координат и оси поворота простой ноги. Нога показана в положении
с нулевым углом

Робот оснащен сферическим тазобедренным суставом с двумя степенями свободы. Первое сочленение обеспечивает движение вперед и назад, то есть поворачивается вокруг оси z.
Второе сочленение отвечает за движение бедра вверх и вниз,
то есть поворачивается вокруг оси x. Колено смещено на расстояние L1 вдоль оси y, а третий шарнир отвечает за движение
колена к телу и от него, то есть поворачивается вокруг оси x.
Наконец, стопа смещена на расстояние L2 в направлении оси z.
В формате ETS это выглядит так:
>>> leg
ERobot:
link
0
1
2
3

= ERobot(ET.Rz() * ET.Rx() * ET.ty(L1) * ET.Rx() * ET.tz(L2))
noname, 3 joints (RRR)
link
joint
parent
ETS: parent to link
link0
0
BASE
Rz(q0)
link1
1
link0
Rx(q1)
link2
2
link2
ty(-0.1) ⊕ Rx(q2)
@link3
tx(-0.1)

Быстрая проверка показывает, что при нулевых углах суставов стопа находится в положении
>>> leg.fkine([0,0,0]).t
array([
0,
-0.1,

-0.1])

Применение 421

как мы и задумывали. Мы можем визуализировать положение
нулевого угла:
>>> leg.plot([0, 0, 0]);

7.4.2.1

Движение одной ноги

Далее мы определим траекторию, по которой будет двигаться рабочий орган ноги – ее стопа. Первое, что необходимо
учесть, – все рабочие органы (все стопы) движутся назад в плос­
кости земли с одинаковой скоростью и без проскальзывания,
продвигая корпус робота вперед. Каждая нога имеет ограниченный диапазон перемещения – она не может двигаться назад слишком долго. В какой-то момент мы должны вернуть ногу
в исходное положение – поднять ступню, переместить ее вперед и снова поставить на землю. Второе соображение исходит
из условия статической устойчивости – в каждый конкретный
момент времени по крайней мере три ноги должны опираться
на землю, поэтому ноги должны переводиться в исходное положение по очереди. Проще говоря, любая нога должна контактировать с землей в течение 75 % времени цикла движения
и в течение 25 % времени цикла перемещаться в исходное положение. Как следствие во время перемещения в исходное положение нога должна двигаться намного быстрее, поскольку ей
приходится преодолеть более длинный путь за меньшее время.
Этот шаблон скоординированного движения ног известен как
походка – в данном случае ползущая, или волновая, походка.
В фазе опоры стопа находится в 50 мм ниже бедра и касается земли. Она перемещается из исходного положения (в 50 мм
перед бедром) на 50 мм назад, толкая корпус робота вперед.
В фазе возврата стопа поднимается до уровня на 20 мм ниже
бедра, выносится вперед и опускается на землю. Полный цикл –
начало фазы опоры, конец фазы опоры, подъем ноги в верхнюю точку, перемещение ноги вперед, опускание ноги и начало
фазы опоры – определяется промежуточными точками
>>> xf = 50; xb = -xf; y = -50; zu = -20; zd = -50;
>>> via = np.array([
...
[xf, y, zd],
...
[xb, y, zd],
...
[xb, y, zu],
...
[xf, y, zu],
...
[xf, y, zd]]) * mm;

где xf и xb – пределы движения ноги вперед (forward) и назад (backward) в направлении оси x (в мм), y – расстояние до стопы от тела
в направлении y, а zu и zd – соответственно высота стопы в направлении z при ее подъеме вверх (up) и опускании вниз (down).

422 Глава 7 · Кинематика робота-манипулятора

Далее произведем дискретизацию многосегментной траектории с частотой 100 Гц:
>>> x = mstraj(via, tsegment=[3, 0.25, 0.5, 0.25], dt=0.01,
...
tacc=0.1).q

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

−20
Фаза опоры

40

−25

Фаза возврата
20

−45

0.0

а

0.5

1.0

1.5
2.0
Время (с)

2.5

3.0

3.5

0

−20

Стопа 0
Стопа 1
Стопа 2
Стопа 3

−40

x (м

м)

−50
40
20
0
−20
−40

Координата x стопы (мм)

−35
−40

z (мм)

−30

4.0

б

0.0

0.5

1.0

1.5

2.0

Время (с)

2.5

3.0

3.5

4.0

Рис. 7.26. а) Траектория движения одной ноги. Напомним, как показано на рис. 7.25, ось z направлена вниз;
б) движение каждой ноги по оси x (смещение по вертикали) для демонстрации походки. Перенос ноги в исходное
положение – это период высокой скорости по оси x

Применим обратную кинематику для определения траекторий углов в сочленениях, необходимых, чтобы стопа следовала
вдоль вычисленной декартовой траектории. Этот робот малоприводный, поэтому используем численную обратную кинематику и устанавливаем маску, учитывающую только перемещение рабочего органа.
>>> sol = leg.ikine_LM(SE3.Trans(x), mask=[1, 1, 1, 0, 0, 0]);

Мы можем посмотреть анимацию движения ноги
>>> leg.plot(sol.q);

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

Применение 423

7.4.2.2

Движение четырех ног

Наш робот имеет ширину и длину.
>>> W = 100 * mm; L = 200 * mm;

Создадим несколько экземпляров ноги, скопировав объект
ноги, созданный ранее, и применив различные базовые преобразования, чтобы прикрепить ноги к разным точкам на теле:
>>> Tf = SE3.Rz(pi);
>>> legs = [
...
ERobot(leg, name="leg0", base=SE3.Trans( L/2,
...
ERobot(leg, name="leg1", base=SE3.Trans(-L/2,
...
ERobot(leg, name="leg2", base=SE3.Trans( L/2,
...
ERobot(leg, name="leg3", base=SE3.Trans(-L/2,

W/2, 0)),
W/2, 0)),
-W/2, 0) * Tf),
-W/2, 0) * Tf)];

В результате получаем список объектов ERobot. Обратите внимание, что ноги 2 и 3 с левой стороны тела повернуты вокруг
оси z, что они оказались направлены в сторону от тела.
Как упоминалось выше, каждая нога должна по очереди возвращаться в исходное положение. Поскольку траектория цик­
лична, мы добиваемся этого, перемещая каждую ногу по траектории со сдвигом фазы, равным четверти от общего времени
цикла. Поскольку полный цикл состоит из 400 точек, траектория
каждой ноги смещается на 100, и мы используем модульную
арифметику для индексации циклов каждой ноги. В результате получается диаграмма походки на рис. 7.26б. Основная часть
программы ходьбы:
>>> for i in range(4000):
...
legs[0].q = gait(qcycle, i, 0, False)
...
legs[1].q = gait(qcycle, i, 100, False)
...
legs[2].q = gait(qcycle, i, 200, True)
...
legs[3].q = gait(qcycle, i, 300, True)
>>> env.step(dt=0.02) # отображение графики

где функция
>>> def gait(cycle, k, phi, flip):
...
k = (k + phi) % cycle.shape[0] # сложение по модулю
...
q = cycle[k, :]
...
if flip:
...
q[0] = -q[0] # для ног с правой стороны
...
return q

возвращает (k+phi)-й элемент q, применяя модульную арифметику, которая рассматривает q как цикл. Аргумент flip меняет
знак движения сочленения 0 для ног с левой стороны робота.

424 Глава 7 · Кинематика робота-манипулятора

Запустите пример командой
>>> %run -m walking

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

нога3

0.05

нога2
нога1

0.00
нога0

−0.05

Z

−0.10

−0.15
0.10
0.05
−0.20

−0.15

−0.10

0.00
−0.05

0.00
X

0.05

Y

−0.05
0.10

0.15

0.20 −0.10

Рис. 7.27. Шагающий робот

7.5

Дополнительные темы

Форма робота, когда все координаты сочленений равны нулю,
совершенно произвольна – на этот счет не существует никаких стандартов или соглашений. Она зависит от особенностей
аппаратного контроллера, управляющего двигателями робота,
и кинематической модели.
 одель Денавита–Хартенберга и модель URDF одного и того же роМ
бота могут иметь разную форму нулевого сочленения, то же верно
и для двух разных моделей URDF одного и того же робота.

Для модели, определяемой:
„„

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

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

Дополнительные темы 425

На самом деле это
реализуется внутри
объекта DHLink.

нения используется процедура, описанная в разделе 7.5.2
и определяющая ETS, которая обеспечит достижение этой
формы;
„„ с использованием файла URDF форма нулевого сочленения
определяется элементами <origin> сочленений, описывающими преобразования жесткого тела для звеньев, которые
они соединяют. Для сочленений с осями, параллельными
осям x, y или z, регулировка параметров крена, тангажа или
рыскания в элементе <origin> считается наиболее простым
способом регулировки формы нулевого сочленения;
„„ с
использованием параметров Денавита–Хартенбурга
форма нулевого сочленения зависит от способа назначения систем координат звеньев. Из-за множества ограничений, накладываемых при этом, форма нулевого сочленения часто оказывается совершенно неочевидной. Класс
DHRobot поддерживает возможность смещения координат
сочленения, что позволяет использовать произвольную
форму для нулевого сочленения. Вектор смещения q0 добавляется к углам сочленения, заданным пользователем
перед вызовом любой кинематической или динамической
функции, например ◄
ξE = 𝒦(q + q0).

(7.7)

Смещение задается путем назначения атрибута offset
объекта DHLink или передачи параметра offset конструктору класса DHLink.

7.5.2

Создание кинематической модели робота

Классический метод определения параметров Денавита–Хартенберга заключается в систематическом назначении системы
координат каждому звену. Системы координат звеньев для робота PUMA 560, определяемые с использованием стандартного
формализма Денавита–Хартенберга, показаны на рис. 7.28а.
На размещение каждой системы координат накладываются
ограничения из-за того, что поворот сочленения должен происходить вокруг оси z, а само звено смещено в направлении x.
Это, в свою очередь, накладывает ограничения на размещение
систем координат основания и рабочего органа и в конечном
итоге определяет форму нулевого угла, обсуждаемую в разделе 7.5.1. Таким образом, определение параметров Денавита–
Хартенберга и систем координат звеньев для совершенно нового механизма оказывается сложнее, чем могло бы быть, даже
для опытного робототехника. Поэтому далее мы представим
альтернативный подход.

426 Глава 7 · Кинематика робота-манипулятора

Мы будем использовать классическую модель робота PUMA
560 как представителя класса полноповоротных шестиосевых
роботов-манипуляторов с 𝒞 = (S1)6. На рис. 7.28б показан вид робота в желаемой конфигурации с нулевыми углами в сочленениях. Его основные размеры имеют следующие значения:
>>> L1 = 0.672; L2 = -0.2337; L3 = 0.4318;
>>> L4 = 0.0203; L5 = 0.0837; L6 = 0.4318;

а

б

Рис. 7.28. Системы координат робота PUMA 560: а) стандартные системы координат
Денавита–Хартенберга и положение с нулевым углом (Corke, 1996b). Обратите внимание, что
индексация систем координат начинается с 1; б) конфигурация с нулевым углом сочленения
для кинематической модели ETS, в которой также отображаются размеры и оси сочленений
(обозначены тройными синими стрелками) (Corke 2007)

Создадим кинематическую модель этого робота, двигаясь
вдоль кинематической цепочки от основания до кончика рабочего органа и попутно записывая элементарные преобразования. Начиная с системы координат {0}, мы поднимаемся вверх
на расстояние L1, выполняем поворот талии вокруг оси z на
угол q0, перемещаемся влево (в направлении y) на расстояние L2,
выполняем поворот плеча вокруг оси y на угол q1, поднимаемся (в направлении z) на расстояние L3, движемся вперед (в направлении x) на расстояние L4, движемся вбок на расстояние L5,
выполняем поворот локтя вокруг оси y на угол q2, поднимаемся
вверх на расстояние L6, выполняем поворот на угол q3 вокруг

Дополнительные темы 427

оси z, затем поворот на угол q4 вокруг оси y и, наконец, поворот
на угол q5 вокруг оси z. Результат нашего путешествия:
>>> e = ET.tz(L1) * ET.Rz() * ET.ty(L2) * ET.Ry() \
...
* ET.tz(L3) * ET.tx(L4) * ET.ty(L5) * ET.Ry() \
...
* ET.tz(L6) * ET.Rz() * ET.Ry() * ET.Rz();

Превратим его в полноценный объект робота:
>>> robot = ERobot(e)
ERobot: noname, 6 joints (RRRRRR)
link
link
joint
parent
0
link0
0
BASE
1
link1
1
link0
2
link2
2
link1
3
link3
3
link2
4
link4
4
link3
5
@link5
5
link4

ETS: parent to link
tz(0.672) ⊕ Rz(q0)
ty(-0.2337) ⊕ Ry(q1)
tz(0.4318) ⊕ tx(0.0203) ⊕ ty(0.83
tz(0.4318) ⊕ Rz(q3)
Ry(q4)
Rz(q5)

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

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

7.5.3

Первое упоминание
встречается в Craig
(1986).

 одифицированные параметры
М
Денавита–Хартенберга

Нотация Денавита–Хартенберга, представленная в разделе 7.1.5, уходит корнями в 1960-е и описана во многих учебниках по робототехнике. Позднее была представлена модифицированная нотация Денавита–Хартенберга, основное отличие
которой состоит в том, что начала систем координат звеньев
размещаются на ближнем (проксимальном), а не на дальнем
(дистальном) конце звена, как показано на рис. 7.29. Эта модифицированная нотация в некоторых отношениях более понятна и в настоящее время используется довольно часто.
 азличия между стандартной и модифицированной
Р
нотациями Денавита–Хартенберга
Наличие двух нотаций Денавита–Хартенберга может сбивать с толку, особенно новичков в кинематике роботов. Алгоритмы кинематики, якобианы и динамика зависят от используемых кинематических соглашений. Знание «параметров Денавита–Хартенберга» для
робота бесполезно, если неизвестно, какая нотация использована
для их обозначения – стандартная или модифицированная.

428 Глава 7 · Кинематика робота-манипулятора
сочленение j-1

сочленение j

звено j
инструмент
основание

звено j+1
2

4

1
3
Рис. 7.29. Определение модифицированных параметров Денавита–Хартенберга. Красный
и синий цвета обозначают все, что относится к звеньям j – 1 и j соответственно. Числа
в кружках обозначают порядок применения элементарных преобразований

В модифицированной нотации Денавита–Хартенберга мат­
ри­ца преобразования звеньев имеет вид
ξj+1 = ξ tx(aj-1) ⊕ ξ rx(αj-1) ⊕ ξ rz(θj) ⊕ ξ tz(dj).

j

(7.8)

Это уравнение содержит те же члены, что и уравнение (7.4),
но в другом порядке – не забывайте, что повороты некоммутативны. В обоих случаях aj – это длина звена, но в стандартной
нотации это смещение между началами систем координат {j}
и {j + 1}, а в модифицированной – между {j + 1} и {j + 2}.
 то описание нотации Денавита–Хартенберга отличается от описаЭ
ния в стандартных учебниках, где индексация сочленений и параметров начинается с единицы. В данной книге используется индексация с нуля, как это принято в языке Python.
Отступление 7.6. О параметрах Денавита–Хартенберга
Если вы собираетесь использовать пакет Toolbox для построения
модели робота на основе таблицы кинематических параметров,
полученной на форуме или из статьи в интернете, то вам следует
узнать, какие соглашения были использованы при ее составлении. Слишком часто этот важный факт упускается из виду, и приходится проводить небольшое расследование.
Важную подсказку можно найти в заголовках столбцов. Если
у всех столбцов одинаковый нижний индекс, то есть θj, dj, aj и αj, то
это стандартная нотация Денавита–Хартенберга. Если половина
нижних индексов отличается, то есть θj, dj, aj-1 и αj-1, то это модифицированная нотация Денавита–Хартенберга.
Вы здорово поможете другим, если в своем коде или в статьях,
публикуемых в интернете, будете четко указывать используемое
кинематическое соглашение.

Дополнительные темы 429

Пакет Toolbox поддерживает обе нотации, вам нужно лишь
указать ее, выбрав соответствующий класс при создании объекта звена, например
>>> L1 = RevoluteMDH(d=1)
RevoluteMDH(d=1, a=0, ˛=0)

После этого все остальное – создание объекта робота, кинематические и динамические функции – работает так же, как
описано ранее.
Эти две нотации можно менять местами, рассматривая преобразование звеньев как последовательность элементарных
поворотов и переносов, как в (7.4) или (7.8). Рассмотрим цепочку преобразований для стандартной нотации Денавита–Хартенберга:

которую можно переформулировать как

где члены, обозначенные как MDHj, имеют форму (7.8).

7.5.4

Произведение экспонент

В главе 2 мы рассмотрели двух- и трехмерные кручения – преобразования твердого тела, определяемые направлением оси
винта, точкой на оси винта и шагом винта. Двухмерное кручение – это вектор S ∈ ℝ3, а трехмерное – вектор S ∈ ℝ6.
В случае робота с одним сочленением, показанного на
рис. 7.4а, винт можно разместить вдоль оси сочленения, чтобы ось координат рабочего органа поворачивалась вокруг оси
винта.
TE(q) = eq[Ŝ] ТЕ(0),
где Ŝ – единичное кручение, представляющее ось винта, а ТЕ(0) –
положение рабочего органа, когда q = 0.
Для робота с двумя сочленениями, показанного на рис. 7.4б,
можно записать
TE(q) = eq̊[Ŝ 0] (eq1[Ŝ 1] ТЕ(0)),
где Ŝ 0 и Ŝ 1 – единичные кручения, представляющие винты, оси
которых совпадают с осями сочленений в конфигурации с нуле-

430 Глава 7 · Кинематика робота-манипулятора

выми углами сочленений q1 = q2 = 0, а ТЕ(0) – положение рабочего
органа в этой конфигурации. Член в скобках аналогичен роботу
с одним сочленением, описанному выше, и первое кручение поворачивает это сочленение и звено вокруг точки Ŝ 0. Используя
Toolbox, мы можем определить длины звеньев и вычислить ТЕ(0):
>>> a1 = 1; a2 = 1;
>>> TE0 = SE2(a1 + a2, 0, 0);

определить два двухмерных кручения в виде SE(2) для этого
примера:
>>> S0 = Twist2.UnitRevolute([0, 0]);
>>> S1 = Twist2.UnitRevolute([a1, 0]);

и применить их к ТЕ(0):
>>> TE = S0.exp(np.deg2rad(30)) * S1.exp(np.deg2rad(40)) * TE0
0.342
-0.9397
1.208
0.9397
0.342
1.44
0
0
1

Прямую кинематику обобщенного робота можно записать
в форме произведения экспоненты (product of exponential, PoE)
как
TE = eq0[Ŝ 0]  eqN-1[Ŝ N-1] ТЕ(0),
где 0TE(0) – это положение конечного рабочего органа, когда все
координаты сочленений равны нулю, и Ŝ j – это единичное кручение для сочленения j, выраженное в мировой системе координат в данной конфигурации. Это также можно записать как
TE = ТЕ(0)(eq0[ Ŝ 0]  eqN-1[ Ŝ N-1]),
E

E

и eqj [ Sj ] – это кручение для сочленения j, выраженное в системе
координат рабочего органа и связанное с кручениями, указанными выше, соотношением EŜ j = Ad(Eξ0)Ŝ j.
Для конкретного робота мы можем вычислить кручения со­
членений
E

>>> irb140 = models.DH.IRB140();
>>> S, TE0 = irb140.twists()
>>> S
0: (0 0 0; 0 0 1)
1: (-0.352 0 0.07; 0 1 0)
2: (-0.352 0 0.43; 0 1 0)
3: (0 0.43 0; 0 0 -1)
4: (0.028 0 0.43; 0 1 0)
5: (0 0.43 0; 0 0 -1)

Преобразование
инструмента и основания фактически
включены в TE(0),
но можно добавить
явное преобразование основания, если
оси винтов определены относительно
основания робота,
а не мировой системы координат, или
использовать сопряженную мат­ри­цу
для преобразования
осей винтов из
системы координат основания
в мировую систему
координат.

Дополнительные темы 431

где S – это экземпляр Twist с шестью элементами, а TE0 – экземпляр SE3. Для конкретной конфигурации сочленения прямая
кинематика задается композицией экспоненциальных скручиваний и TE0.
>>> T = S.exp(irb140.qr).prod() * TE0
0
0
-1
0.005
1
0
0
0
0
-1
0
0.332
0
0
0
1

В данном случае S и irb140.qr содержат по шесть элементов,
поэтому выполняется поэлементное возведение в степень, а затем вычисляется произведение результатов. Для визуализации
кручений сначала отобразим робота в конфигурации с нулевым
углом
>>> irb140.plot(irb140.qz);

и наложим линии направления действий:
>>> lines = S.line()
0: { 0 0 0; 0 0 1}
1: { 0.352 0 -0.07; 0 1 0}
2: { 0.352 0 -0.43; 0 1 0}
3: { 0 -0.43 0; 0 0 -1}
4: { -0.028 0 -0.43; 0 1 0}
5: { 0 -0.43 0; 0 0 -1}
>>> lines.plot("k:")

где lines – экземпляр Line3, представляющий шесть линий
с использованием координат Плюккера, которые показаны на
рис. 7.30.
Toolbox также поддерживает модели роботов, заданные непосредственно в терминах кручений, используя формализм
произведения экспонент. В этом случае, так же как в других
подходах, нужно создать несколько экземпляров PoELink и T0 как
экземпляр SE3. Мы можем напрямую создать модель робота, показанного на рис. 7.4б, в трехмерном пространстве, используя
кручения
>>> link1 = PoERevolute([0, 0, 1], [0, 0, 0]); # Rz через (0,0,0)
>>> link2 = PoERevolute([0, 0, 1], [1, 0, 0]); # Rz через (1,0,0)
>>> TE0 = SE3.Tx(2); # положение рабочего органа, когда q=[0,0]

где в вызов PoELink.Revolute передаются те же аргументы, что
и в вызов Twist.UnitRevolute. Затем объекты звеньев объединяются в объект подкласса Robot обычным способом:
>>> robot = PoERobot([link1, link2], TE0);

432 Глава 7 · Кинематика робота-манипулятора

и вычисляется прямая кинематика:
>>> robot.fkine([0, 0]).printline()
t = 2, 0, 0; rpy/zyx = 0°, 0°, 0°

0.5
0.4
0.3
0.2

Z

0.1
IRB 140

0.0
−0.1
0.3
0.2
0.1
−0.1

0.0

0.0
0.1

0.2

X

−0.1
0.3

0.4

Y

−0.2
0.5

−0.3

Рис. 7.30. Диаграмма, показывающая линии кручения каждого сочленения робота ABB IRB-140

7.5.5

Определение столкновений

Во многих робототехнических приложениях требуется возможность проверки потенциальных столкновений робота с непо­
движными или движущимися объектами в окружающем мире.
Обычно это достигается моделированием каждого объекта одной или несколькими фигурами столкновений, как показано на
рис. 7.31. Эти фигуры могут быть простыми геометрическими
примитивами, такими как прямоугольные призмы, сферы или
цилиндры, пересечения с которыми легко вычислить, или более общие сетки. ►
Для иллюстрации создадим модель робота Panda
>>> panda = models.URDF.Panda();

включающую модели столкновений и область рабочего пространства в форме куба со стороной 1 м и с центром в точке (1.1,
0. 0):
>>> from spatialgeometry import Cuboid
>>> box = Cuboid([1, 1, 1], pose=SE3.Tx(1.1));

Как определено
в файле STL или
COLLADA.

Дополнительные темы 433

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

Проверим столкновение робота в заданной конфигурации
с кубом:
>>> panda.iscollided(panda.qr, box)
False

В данном случае ни одно звено робота не пересекает границ
куба. Если переместить центр куба в (1.0, 0, 0):
>>> box.T = SE3.Tx(1)
>>> panda.iscollided(panda.qr, box)
True

тогда обнаружится столкновение.
Для визуализации можно расширить пример выше и создать
экземпляр робота и куба в среде моделирования Swift:
>>>
>>>
>>>
>>>

# построить робота и получить ссылку на графическую среду
env = panda.plot(panda.qr, backend="swift");
env.add(box); # добавить систему координат в диаграмму
env.step()
# обновить графику

чтобы отобразить результат во вкладке браузера. Присмотревшись, можно заметить, что захват не совсем касается куба, но
модель столкновений, показанная на рис. 7.31, весьма консервативна. Мы можем изменить конфигурацию робота, установив
свойство q, или положение куба, установив его свойство T, а затем обновить изображение:
>>> env.step()

434 Глава 7 · Кинематика робота-манипулятора

7.6

Подведение итогов

В этой главе мы научились определять прямую и обратную
кинематику руки-манипулятора с последовательным соединением звеньев. Прямая кинематика включает суммирование
относительных положений, создаваемых каждым сочленением и звеном, что позволяет определить положение рабочего
органа относительно основания робота. Мы рассмотрели эту
тему с точки зрения графов положений и деревьев твердых тел
как в двух, так и в трех измерениях, а также обсудили другие
способы представления моделей, такие как файлы URDF, параметры Денавита–Хартенберга и произведение экспонент
с кручениями.
Задача обратной кинематики заключается в определении координат сочленений с учетом положения рабочего органа. Эта
задача может иметь несколько решений, если одно и то же положение рабочего органа может быть обеспечено несколькими
конфигурациями сочленений. Для простых роботов или роботов с шестью сочленениями и сферическим запястьем сущест­
вует аналитическое решение задачи обратной кинематики,
обеспечивающее явный контроль над искомым решением.
Для роботов, не имеющих шести сочленений и сферического
запястья, решить задачу обратной кинематики можно итерационным численным методом. В данной главе вы видели применение этого подхода к малоприводному роботу SCARA с четырьмя сочленениями и к многоприводному роботу с семью
звеньями. Мы также кратко коснулись темы сингулярностей,
возникающих из-за совпадения осей сочленений.
Мы узнали, как создавать траектории для плавного перемещения рабочего органа из одного положения в другое. Траектории в пространстве сочленений легко вычисляются, но, как
правило, приводят к нелинейным траекториям в декартовом
пространстве, что может создавать проблемы в некоторых ситуациях. Прямолинейные траектории в декартовом пространстве можно получить, но наличие сингулярностей в рабочей
области может потребовать перемещения сочленений по неоптимальным траекториям.

7.6.1

Дополнительное чтение

Кинематика манипулятора с последовательным расположением звеньев рассматривается во всех стандартных руководствах по робототехнике, таких как «Robotics Handbook» (2016),
Siciliano et al. (2009), Spong et al. (2006) и Paul (1981). Отличным
введением в кинематику роботов может послужить книга Craig

Подведение итогов 435

(2005). В ней используется модифицированная нотация Денавита–Хартенберга и примеры в третьем издании основаны на
более старой версии Robotics Toolbox для MATLAB®. В Lynch
and Park (2017) и Murray et al. (1994) рассматривается подход
экспоненциального произведения. Описание развивающегося
формата URDF (unified robot description format – унифицированный формат описания роботов) – альтернативы нотации
Денавита–Хартенберга – можно найти на странице https://wiki.
ros.org/urdf.
В Siciliano et al. (2009) дается очень четкое описание процесса определения параметров Денавита–Хартенберга для произвольного робота. Альтернативный подход, основанный на символической факторизации последовательностей элементарных
преобразований, подробно описан в Corke (2007). Окончательные значения параметров для робота PUMA 560 описаны в статье Corke and Armstrong-Hélouvry (1995).
Ходьба роботов – обширная область сама по себе, и приведенный здесь пример весьма упрощен. Исследователями были
продемонстрированы машины со сложными походками, такими как бег и галоп, основанными на динамическом, а не статическом равновесии. Хорошее введение в шагающих роботов
вы найдете в «Robotics Handbook» (Siciliano and Khatib 2016,
§ 17). Роботизированные манипуляторы с захватами и манипулирование рабочими инструментами – еще одна большая
тема, которую мы не рассмотрели. Хорошее введение по ней
вы найдете в «Robotics Handbook» (Siciliano and Khatib 2016,
§ 37, 38).
Манипуляторы с параллельными звеньями в этой книге не
рассматриваются. Они обладают дополнительными преимуществами, такими как возможность создания повышенного
усилия и высокая жесткость (потому что приводы образуют
ферменную конструкцию), а также способны развивать очень
высокую скорость перемещения. Для механизмов этого класса
задача обратной кинематики обычно имеет аналитическое решение, но прямая кинематика всегда требует численного решения. Полезные отправные точки в изучении этого класса роботов: «Robotics Handbook» (Siciliano and Khatib 2016, § 18), а также
отдельные разделы в Siciliano et al. (2009) и Merlet (2006).
Аналитическое решение задачи обратной кинематики можно получить алгебраически, записав ряд кинематических соотношений и решив их для углов сочленений, как описано в Paul
(1981). Существуют программные пакеты для автоматического построения прямой и обратной кинематики для заданного
робота, в том числе SYMORO (Khalil and Creusot 1997), который ныне доступен с открытым исходным кодом, и OpenRAVE
(https://openrave.org/), который содержит кинематический решатель IKFast (Diankov 2010).

436 Глава 7 · Кинематика робота-манипулятора

Историческая справка
Первоначальная работа Денавита и Хартенберга – это их статья
1955 года и учебник (Hartenberg and Denavit, 1964). Книга содержит введение в кинематику и ее историю, но в настоящее время
не переиздается, хотя ее можно найти в интернете. Первое полное описание кинематики шестизвенной руки со сферическим
запястьем было дано в Paul and Zhang (1986). В Lipkin (2008) рассматриваются различные формы нотации Денавита–Хартенберга, помимо двух вариантов, представленных в этой главе.

7.6.2

Упражнения

1. Прямая кинематика для планарного робота на рис. 7.4.
а) Для робота с двумя сочленениями используйте метод
teach, чтобы определить два набора углов сочленений, которые поместят рабочий орган в точку (0.5, 0.5).
б) Поэкспериментируйте с тремя различными моделями на
рис. 7.4, используя методы fkine и teach.
в) Попробуйте изменять длину звеньев, создайте звенья,
смещенные вдоль y или вдоль x и y одновременно.
2. Поэкспериментируйте с методом teach для робота PUMA 560.
3. Обратная кинематика для робота с двумя звеньями (раздел 7.2.1).
а) Как изменится решение, если точка окажется вне досягае­
мости?
б) Большинство положений рабочего органа можно достичь,
используя два разных набора углов в сочленениях сочленений. Какие точки можно достичь, используя только
один набор?
в) Найдите решение для заданных x и θ, когда значение y не
ограничено.
4. Сравните решения, сгенерированные функциями ikine_a
и ikine_LM для робота PUMA 560 и разных положений рабочего органа. Есть ли разница в точности? Насколько медленнее
ikine_LM?
5. Для PUMA 560 в конфигурации qn продемонстрируйте переход от конфигурации с локтем вверх к конфигурации с локтем вниз.
6. Для робота PUMA 560 исследуйте ошибки в положении конечного органа, вызванные производственными ошибками.
а) Удлините звено 2 на 0.5 мм. Какова средняя и максимальная погрешность определения компонентов положения рабочего органа для 100 случайных конфигураций суставов?
б) Введите погрешность 0.1° в угол сочленения 2 и повторите анализ выше.

Подведение итогов 437

7. Изучите многоприводные модели роботов DH.Hyper и DH.
Coil. Управляйте ими вручную, используя метод teach, вычисляя прямую и обратную кинематику.
8. Поэкспериментируйте с методом ikine_LM для решения обратной кинематической задачи в случае, когда координаты
сочленения имеют ограничения (механические концевые
упоры). Ограничения сочленения задаются свойством qlim
класса Link.
9. Рисование буквы «B» (раздел 7.4.1).
а) Измените размер буквы.
б) Напишите слово или предложение.
в) Напишите на вертикальной плоскости.
г) Напишите на наклонной плоскости.
д) Замените робота PUMA 560 роботом IRB140.
е) Напишите на сфере. Подсказка: выполните надпись на
касательной плоскости, а затем спроецируйте точки на
поверхность сферы.
ж) Для нанесения надписи не требуется манипулятор с шестью степенями свободы, потому что поворот пера вокруг
своей оси не имеет значения. Удалите последнее звено из
модели робота PUMA 560 и повторите упражнение.
10. Шагающий робот (раздел 7.4.2).
а) Сократите траекторию возврата ноги в исходное положение, уменьшив высоту подъема ступни.
б) Увеличьте длину шага.
в) Определите местоположение центра робота как функцию от времени.
г) Придумайте, как управлять поворотом робота, изменяя
длину шага на одной стороне тела.
д) Измените походку так, чтобы робот двигался боком, как
краб.
е) Добавьте еще одну пару ног. Измените походку так, чтобы одновременно переставлялись две или три ноги.
ж) В текущей симуляции ноги двигаются, но тело не перемещается вперед. Измените симуляцию так, чтобы тело
перемещалось.
11. Рука робота имеет несколько пальцев, каждый из которых
представляет собой небольшой манипулятор из последовательно соединенных звеньев. Создайте модель руки с 2, 3
или 5 пальцами и создайте анимацию движения пальцев.
12. Создайте симуляцию с двумя роботизированными манипуляторами, расположенными рядом друг с другом, рабочие
органы которых держат баскетбольный мяч в диаметрально противоположных точках в горизонтальной плоскости.
Напишите код для перемещения манипуляторов так, чтобы
они вращали мяч вокруг вертикальной оси.

Глава

8

	Скорость манипулятора

Скорость перемещения рабочего органа робота-манипулятора – скорость изменения его положения – известна как про­
странственная скорость и была представлена в разделе 3.1. Эта
скорость в пространстве задач имеет поступательную и вращательную составляющие. В случае перемещения в трехмерном
пространстве она определяется шестимерным вектором и является следствием скорости изменения координат сочленений,
скорости в пространстве сочленений или просто скорости движения сочленений.
В этой главе мы исследуем мат­ри­цу Якоби манипулятора,
которая описывает связь между скоростью сочленений и пространственной скоростью рабочего органа. В разделе 8.1 мы
представим мат­ри­цу Якоби на примере простого планарного
робота-манипулятора, а затем распространим этот принцип на
более общие типы роботов. В разделе 8.2 демонстрируется применение обратного якобиана для создания прямолинейных декартовых траекторий без необходимости решать задачу обратной кинематики. В разделе 8.3 описываются числовые свойства
мат­ри­цы Якоби, помогающие получить представление о ловкости манипулятора – направлениях, в которых он может или
не может двигаться. Также мы поговорим о много- и малоприводности роботов. В разделе 8.4 демонстрируется, как можно
использовать транспонирование якобиана для преобразования
сил и моментов в рабочем органе в силы или моменты в сочленениях. В разделе 8.5 вновь рассматривается задача обратной
кинематики с более подробным обсуждением численного решения, представленного в предыдущей главе, и описанием его
зависимости от мат­ри­цы Якоби. А в разделе 8.6 мы рассмотрим
несколько дополнительных тем.

8.1

Якобиан манипулятора

В предыдущей главе мы обсудили кинематику манипулятора –
взаимосвязь между координатами сочленений и положением
рабочего органа. Теперь мы исследуем дифференциальную кине­
матику манипулятора – взаимосвязь между скоростями изме-

chap8.ipynb

https://go.sn.
pub/3xirai

Якобиан манипулятора 439

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

8.1.1

Якобиан в мировой системе координат

Проиллюстрируем основы на нашем уже знакомом двухмерном примере (рис. 8.1), но на этот раз используем символические, а не числовые параметры
>>> import sympy
>>> a1, a2 = sympy.symbols("a1, a2")
>>> e = ERobot2(ET2.R() * ET2.tx(a1) * ET2.R() * ET2.tx(a2))

и определим символический двухмерный вектор для представления углов сочленений
>>> q = sympy.symbols("q:2")
(q0, q1)

Рис. 8.1. Робот с двумя звеньями и положением рабочего органа P, представленным вектором
координат p = (x, y), и вектором скорости в пространстве задачи v = dp/dt. Это тот же робот,
что и на рис. 7.7

Прямая кинематика определяется как
>>> TE = e.fkine(q);

и по ней можно определить местоположение рабочего органа
p = (x, y) ∈ ℝ2:
>>> p = TE.t
array([a1*cos(q0) + a2*cos(q0 + q1),
a1*sin(q0) + a2*sin(q0 + q1)], dtype=object)

А теперь вычислим производную p относительно конфигурации сочленений q.
Поскольку p и q – это векторы, производная
(8.1)

440 Глава 8 · Скорость манипулятора
Отступление 8.1. Матрица Якоби
Матрица Якоби (якобиан) – это многомерная форма производной векторной
функции от векторного аргумента. Если y = f(x) и x ∈ ℝn, а y ∈ ℝm, то якобиан – это
мат­ри­ца m×n.

Название «якобиан» дано в честь Карла Якоби. Дополнительные подробности
ищите в приложении E.

будет иметь вид мат­ри­цы Якоби (см. приложение E):
>>> J = sympy.Matrix(p).jacobian(q)
Matrix([
[-a1*sin(q0) - a2*sin(q0 + q1), -a2*sin(q0 + q1)],
[ a1*
cos(q0) + a2*cos(q0 + q1), a2*cos(q0 + q1)]])
>>> J.shape
(2, 2)

Обычно мат­ри­ца Якоби обозначается символом J, и в данном
случае она имеет размер 2×2.
Чтобы определить соотношение между скоростью сочленения и скоростью рабочего органа, переформулируем (8.1) следующим образом:
∂p = J(q)∂q
и, разделив на dt, получаем
ṗ = J(q)q̇,
где мат­ри­ца Якоби отображает скорость в пространстве конфигурации сочленений в скорость рабочего органа в пространстве
задач и сама является функцией конфигурации сочленения.
Чтобы получить более обобщенное представление, запишем
прямую кинематику в виде функции, повторив (7.2)
ξ = 𝒦(q),

0

где q ∈ ℝN и N = dim 𝒞 – размерность конфигурационного пространства, то есть количество сочленений робота. Взяв производную, запишем пространственную скорость рабочего органа
v = 0J(q)q̇ ∈ ℝM,

0

(8.2)

Якобиан манипулятора 441

где M = dim 𝒯 – размерность пространства задач. Для типичного
трехмерного случая, когда ξ ∈ ℝ3 × S3, как обсуждалось в разделе 3.1.1, 0v = (υx, υy, υz, ωx, ωy, ωz) ∈ ℝ6 и включает в себя компоненты поступательной и вращательной скоростей. Матрица 0J(q) ∈
ℝ6×N – это якобиан манипулятора, или геометрический якобиан.
Это соотношение иногда называют мгновенной прямой кинема­
тикой.
Матрицу Якоби можно вычислить методом jacob0 для любого
объекта робота в Toolbox. Для робота UR5 в конфигурации, показанной на рис. 8.2, якобиан
>>> ur5 = models.URDF.UR5();
>>> J = ur5.jacob0(ur5.q1)
array([[-0.02685, 0.3304, -0.09465, -0.09465, -0.0823,
0],
[ 0.4745,
0,
0,
0, -0.0823,
0],
[
0, -0.4745, -0.4745, -0.0823,
0, -0.0823],
[
0,
0,
0,
0,
0,
1],
[
0,
1,
1,
1,
0,
0],
[
1,
0,
0,
0,
-1,
0]])

– это мат­ри­ца размером 6×6. Каждая строка соответствует степени свободы в пространстве задач в порядке υx, υy, υz, ωx, ωy и ωz.

фланец
рабочего
органа

Рис. 8.2. Робот UR5 в конфигурации сочленений q1. Ось z рабочего органа направлена вдоль
оси x мировой системы координат

 В некоторых источниках, включая версию этой книги для MATLAB®, и некоторых программных пакетах компоненты вращения
помещаются первыми, то есть строки соответствуют ωx, ωy, ωz, υx,
υy и υz.

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

442 Глава 8 · Скорость манипулятора
Отступление 8.2. Карл Густав Якоб Якоби
Якоби (1804–1851) – прусский математик, получивший степень доктора философии в Берлинском университете в 1825 году. В 1827 году он
был назначен профессором математики в Кенигсбергском университете и занимал эту должность
до 1842 года, когда у него случился нервный срыв
из-за переутомления.
В 1829 году Якоби написал классический трактат об эллиптических функциях, а также описал
производную m функций n переменных, которая
носит его имя. В 1836-м был избран иностранным членом Королевской Шведской академии
наук. Похоронен на кладбище I церкви Святой
Троицы (Dreifaltigkeitskirche) в Берлине.

y и x и поворот вокруг оси z. Движение сочленений 1 и 2 вызывает движение в мировых направлениях x и z и отрицательный
поворот вокруг оси y. Блок 3×3 в правом верхнем углу якобиана
связывает скорость запястья (q̇3, q̇4, q̇5) со скоростью поступательного движения рабочего органа. Эти значения малы из-за
малой длины звеньев в запястном механизме.
На рис. 8.2 показаны оси сочленений в пространстве, что позволяет представить, как рабочий орган перемещается в пространстве при небольшом повороте каждого сочленения. Также
можно воспользоваться методом teach

Для робота со сферическим запястьем
и без инструмента
этот блок будет
содержать нули.

>>> ur5.teach(ur5.q1)

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

8.1.2

Якобиан в системе координат рабочего органа

Якобиан, вычисленный методом jacob0, отображает скорость
сочленения в пространственную скорость рабочего органа
в мировой системе координат. Чтобы получить пространственную скорость в системе координат рабочего органа, можно использовать преобразование (3.5) из мировой системы координат в систему координат рабочего органа
(8.3)
что приводит к новому якобиану, выражающему скорость рабочего органа. ►

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

Якобиан манипулятора 443

В Toolbox этот якобиан вычисляется методом jacobe, например для робота UR5 в положении, описанном выше:
>>> ur5.jacobe(ur5.q1)
array([[ -0.4745,
0,
0,
0,
[ 0.02685, -0.3303, 0.09465, 0.09465,
[
0, 0.4746, 0.4745, 0.0823,
[
0,
-1,
-1,
-1,
[
0,
0,
0,
0,
[
-1,
0,
0,
0,

8.1.3

0.0823,
0.0823,
0,
0,
0,
1,

0],
0],
0.0823],
0],
-1],
0]])

Аналитический якобиан

В (8.2) и (8.3) пространственная скорость выражается через
векторы поступательной и угловой скоростей, однако угловая
скорость – сложное понятие. В некоторых приложениях может
быть полезнее рассматривать скорость вращения через скорости изменения углов крена, тангажа и рыскания, углов Эйлера
или экспоненциальных координат. Аналитические якобианы –
это якобианы, в которых скорость вращения выражена в ином
представлении, отличном от угловой скорости.
Рассмотрим случай с углами XYZ крена-тангажа-рыскания
Γ = (α, β, γ) ∈ ℝ3, для которых мат­ри­ца вращения равна
.
Здесь сокращения cθ и sθ обозначают cos θ и sin θ соответственно. Использовав цепочечное правило и приложив немного усилий, можно записать производную Ṙ, и, вспоминая (3.1)
Ṙ = [ω]×R,
мы можем найти решение для элементов ω в терминах углов
крена-тангажа-рыскания и их скоростей и получить

Это решение можно разложить на множители

444 Глава 8 · Скорость манипулятора

и записать в более краткой форме:
ω = A(Γ)Γ̇.
Матрица A сама по себе является якобианом, отображающим
скорость изменения углов XYZ крена-тангажа-рыскания. Ее
можно вычислить с помощью функции в пакете Toolbox
>>> rotvelxform((0.1, 0.2, 0.3), representation="rpy/xyz")
array([[ 0.1987,
0,
1],
[ -0.2896, 0.9553,
0],
[ 0.9363, 0.2955,
0]])

которая принимает аргументы с углами крена, тангажа и рыс­
кания. При условии что A не является вырожденной (сингулярной), аналитический якобиан имеет вид

Матрица A вырождена, когда cos β = 0 (угол тангажа β = ±π/2).
Это состояние называется репрезентативной сингулярностью.
Аналитический якобиан вычисляется как
>>> ur5.jacob0_analytical(ur5.q1, "rpy/xyz");

Связанный с ним вектор пространственной скорости определен относительно мировой системы координат, а скорость
вращения выражается как скорость изменения углов XYZ крена-тангажа-рыскания.
Аналогичный подход можно применить к углам Эйлера, использовав аргумент "eul" в rotvelxform
и jacob0_analytical.
Другой полезный аналитический якобиан связывает угловую
скорость со скоростью изменения экспоненциальных координат s = υ̂θ ∈ ℝ3
ω = A(s)ṡ,

где

а υ̂ и θ можно определить из мат­ри­цы вращения рабочего органа через логарифм мат­ри­цы и вычислить ее, передав аргумент
"exp" в вызов rotvelxform и jacob0_analytical. Когда θ = 0, получается вырожденная мат­ри­ца, что соответствует повороту на
нулевой угол.

Вызов rotvelxform с параметром
inverse=True
вернет обратную
мат­ри­цу, вычисленную в аналитическом виде, а вызов
с параметром
full=True вернет
блочно-диагональную мат­ри­цу 6×6,
подходящую для
умножения слева
на геометрический
якобиан.

Применение: управление движением с заданной скоростью 445

8.2

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

Мы обсудили, как мат­ри­ца Якоби отображает скорость сочленения в скорость рабочего органа в пространстве задач, однако
бо́льшее практическое значение имеет обратная задача – задача определения скоростей сочленений, необходимых для
достижения определенной скорости рабочего органа в пространстве задач. При условии что мат­ри­ца Якоби квадратная
и невырожденная, мы можем инвертировать (8.2) и записать
q̇ = J-1(q)v.

(8.4)

Это простой и элегантный алгоритм управления движением с заданной скоростью, не требующий решать задачу обратной кинематики. Он использует обратную мат­ри­цу Якоби
для преобразования желаемой скорости в пространстве задач
в скорость сочленения. Для примера предположим, что якобиан – квадратная (6×6) и невырожденная мат­ри­ца, но позже мы
ослабим эти ограничения.
Схема управления движением обычно реализуется в дискретной форме как
q̇∗〈k〉 = J-1(q〈k〉)v∗,

q∗〈k + 1〉 ← q∗〈k〉 + δt q̇∗〈k〉,

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

(8.5)

где δt – интервал дискретизации, а 〈·〉 – временной шаг. Первое
уравнение вычисляет скорость сочленения как функцию текущей конфигурации сочленения и желаемой пространственной скорости рабочего органа v ∗. Второе уравнение вычисляет
желаемые координаты сочленения на следующем временном
шаге, используя прямоугольное интегрирование.
Пример алгоритма в виде блок-схемы показан на рис. 8.3.
Требуемая скорость в пространстве задач составляет 0.05 м/с
в направлении мировой оси y. Входными данными блока Jacobian являются текущие координаты сочленений манипулятора,
а результатом – мат­ри­ца Якоби 6×6. Она обращается и умножается на требуемую скорость в пространстве задач для получения требуемой скорости сочленений, после чего робот моделируется интегратором.
Команда
>>> %run -m RRMC -H

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

446 Глава 8 · Скорость манипулятора

откуда можно извлечь дискретные отметки времени и координаты сочленений:
>>> t = out.clock0.t;
>>> q = out.clock0.x;

qdot

Блок
Jacobian

Блок Inverse

q

Блок Prod

Блок
Dintegrator

Блок Armplot

скорость в пространстве задач

Рис. 8.3. Блок-схема модели RRMC для управления движением рабочего органа с заданной
постоянной скоростью

Траектории первых трех сочленений
>>> xplot(t, q[:, :3], stack=True);

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

Функция xplot – это
вспомогательная
функция из пакета
Toolbox, которая
отображает столбцы
мат­ри­цы в отдельных подграфах.

>>> Tfk = puma.fkine(q);

и построим как функцию времени (см. рис. 8.4б)

:

>>> xplot(out.clock0.t, Tfk.t, stack=True);
0.59630

q0

x

0.4
0.59625

0.2
0.59620
0.0
1

2

3

4

5

0.1

0.81

1

2

3

4

5

1

2

3

4

5

3

4

5

q1

y

0.0

0.80

−0.1

0.79
1

2

3

4

5

×10 −7 + 6.5747500000×10 −1

3.14

8.5

z

q2

3.12

8.0

3.10
7.5
3.08

а

1

2

Время (с)

3

4

5

б

1

2

Время (с)

Рис. 8.4. Движение робота, управляемого моделью на рис. 8.3: а) движение в пространстве сочленений;
б) движение рабочего органа в пространстве задач. Обратите внимание на небольшое, но нежелательное движение
в направлениях x и z

Применение: управление движением с заданной скоростью 447

Движение в пространстве задачи выполняется со скоростью
0.05 м/с вдоль оси y, как мы и указали.
Описанный подход, основанный исключительно на интегрировании, страдает проблемой накопления ошибок, которая наблюдается на рис. 8.4б как небольшое, но нежелательное движение вдоль осей x и z. Якобиан – это градиент очень нелинейной
функции (прямой кинематики), которая является функцией
конфигурации. Основная причина ошибки заключается в том,
что при конечном шаге во времени, по мере перемещения робота, ранее вычисленный якобиан отклоняется от истинного
градиента.
Чтобы реализовать движение по более сложной траектории,
можно задействовать алгоритм с обратной связью, оценивающий разницу между фактическим и желаемым положениями
q̇〈k〉 ← Kp J-1(q〈k〉)Δ(𝒦(q〈k〉), ξ ∗〈k〉),

(8.6)

где Kp – пропорциональный коэффициент усиления, Δ(·) ∈ ℝ6 –
пространственное смещение из (3.11) и желаемое положение
ξ ∗〈k〉 является функцией времени.
Блок-схема модели, реализующей движение с заданной скоростью и перемещением рабочего органа по круговой траектории, показана на рис. 8.5. Инструмент робота описывает окружность радиусом 50 мм в горизонтальной плоскости. Желаемое
положение – это система координат, начало которой движется
по окружности.

qdot

Блок Jacobian

Блок Inverse

Блок Prod

q

Блок
Dintegrator

Блок Armplot

фактическое положение рабочего органа

желаемое
положение
рабочего
органа
Блок Circlepath

заданная
пространственная
скорость
Блок Tr2delta

прямая
кинематика

Kp

Рис. 8.5. Блок-схема модели RRMC2, которая реализует управление с обратной связью движением с заданной
скоростью и круговым движением рабочего органа

Разница (оператор Δ(·)) между желаемым и текущим положениями вычисляется на основе прямой кинематики блоком
tr2delta. Результатом является пространственное смещение,
поступательное движение и вращение, описываемые шестимерным вектором, масштабированным пропорциональным
коэффициентом усиления, необходимым для получения задан-

448 Глава 8 · Скорость манипулятора

ной пространственной скорости. Эта скорость преобразуется
обратной мат­ри­цей Якоби манипулятора в требуемую скорость
сочленения, которое переместит рабочий орган в изменяющееся во времени положение. Запускается модель командой
>>> %run -m RRMC2 -H

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

−0.16

Y

−0.18

−0.20

−0.22

−0.24

0.56

0.58

0.60
X

0.62

0.64

Рис. 8.6. Траектория рабочего органа с использованием управления с обратной связью
движением с заданной скоростью, показанным на рис. 8.5, с круговым движением рабочего
органа. Требуемое круговое движение показано красным пунктирным кругом

8.3

Обусловленность якобиана
и манипулируемость

До настоящего момента предполагалось, что якобиан – квад­
ратная и невырожденная мат­ри­ца, но на практике так бывает не
всегда. Якобиан – это мат­ри­ца dim 𝒯 = dim 𝒞, поэтому для робота,
действующего в пространстве задач 𝒯 ∈ ℝ3 × S3, где dim 𝒯 = 6, квад­
ратный якобиан требует, чтобы робот имел шесть сочленений.

8.3.1

Вырожденные якобианы

Если якобиан – квадратная мат­ри­ца, то конфигурация робота q,
где det(J(q)) = 0, описывается как сингулярная, или вырожденная.
Сингулярности возникают, когда робот достигает максимально-

Обусловленность якобиана и манипулируемость 449

го радиуса действия или когда совпадают оси одного или нескольких сочленений. Это приводит к потере степеней свободы движения и появлению проблемы блокировки карданного подвеса.
Например, робот UR5 в своей нулевой конфигурации имеет
мат­ри­цу Якоби
>>> J = ur5.jacob0(ur5.qz)
array([[ -0.1915, -0.09465, -0.09465, -0.09465, 0.0823,
0],
[ 0.8996,
0,
0,
0, -0.0823,
0],
[
0, -0.8996, -0.4746, -0.0823,
0, -0.0823],
[
0,
0,
0,
0,
0,
0],
[
0,
1,
1,
1,
0,
1],
[
1,
0,
0,
0,
-1,
0]])

которая вырождена
>>> np.linalg.det(J)
0

И, копнув немного глубже, можно увидеть, что ранг якобиана
уменьшился
>>> np.linalg.matrix_rank(J)
5

по сравнению с максимальным числом шесть для мат­ри­цы 6×6.
Дефицит ранга 6 - 5 = 1 означает, что один столбец равен некоторой линейной комбинации других столбцов. Рассматривая
якобиан, мы видим, что столбцы 1, 2 и 3 схожи, и все они вызывают движение в пространстве задач в направлениях x и z
и вращение вокруг оси y.
Проверить взаимозависимость столбцов можно с помощью
функции jsingu:
>>> jsingu(J)
column 3 = - 0.923 column_1 + 1.92 column_2

В данном случае полученный результат говорит о том, что
любая скорость в пространстве задач, обусловленная действием
q̇3, также может быть вызвана некоторой комбинацией q̇1 и q̇2.
Это означает, что сочленение 3 исключено из работы и у робота
есть только 5 эффективных сочленений, что делает движение
в одном из направлений в пространстве задач недоступным.
Даже если робот просто находится близко к сингулярности,
но не в ней, все равно возможны проблемы. Рассмотрим конфигурацию, близкую к сингулярности в точке qz, со всеми сочленениями, имеющими ненулевое значение угла поворота 5°
>>> qns = np.full((6,), np.deg2rad(5))
array([ 0.08727, 0.08727, 0.08727, 0.08727, 0.08727, 0.08727])

450 Глава 8 · Скорость манипулятора

и якобиан
>>> J = ur5.jacob0(qns);

Если использовать алгоритм управления движением с заданной скоростью для перемещения рабочего органа с умеренной
скоростью 0.1 рад/с вокруг оси x:
>>> qd = np.linalg.inv(J) @ [0, 0, 0, 0.1, 0, 0]
array([-0.003797, -2.773,
5.764, -4.088, -0.02945,

1.093])

то в результате многие сочленения будут двигаться с очень высокой скоростью, например скорость сочленения 2 составит почти
1 оборот в секунду, что более чем в 50 раз превышает заданную
скорость вращения рабочего органа. Да, робот не находится
в сингулярности, но определитель якобиана слишком мал:
>>> np.linalg.det(J)
-0.0009943

Чем ближе конфигурация к сингулярности, тем выше будут
скорости движения сочленений – в сингулярности они стремятся к бесконечности. Иначе говоря, число обусловленности
якобиана очень велико:
>>> np.linalg.cond(J)
232.1

что говорит о плохой обусловленности якобиана.
Для некоторых движений, таких как поступательное движение, плохая обусловленность якобиана не является проблемой.
Если бы мы решили переместить инструмент вдоль оси y:
>>> qd = np.linalg.inv(J) @ [0, 0.1, 0, 0, 0, 0]
array([ 0.1269, -0.3555, 0.7214, -0.7412, 0.1226,

0.3768])

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

8.3.2

Эллипсоид скорости и манипулируемость

Было бы полезно иметь некоторое представление о способности робота достигать заданной скорости в пространстве задач.
Для начала рассмотрим множество обобщенных скоростей со­
членений q̇ ∈ ℝN с единичной нормой ||q̇|| = 1, то есть
q̇T q̇ = 1.

Аналогичный
эффект наблюдается
на рис. 7.23.

Обусловленность якобиана и манипулируемость 451

Геометрически, если рассматривать q̇ как точку в N-мерном
пространстве скоростей сочленений, это уравнение описывает
все точки, которые лежат на поверхности гиперсферы. Подставляя (8.4), можно записать
vT(J(q)JT(q))-1v = 1.

(8.7)

Это уравнение точек на поверхности гиперэллипсоида в нечетком 𝒯-мерном пространстве скоростей задач. Каждая точка
представляет возможную скорость рабочего органа, и в идеале
эти скорости одинаково велики во всех возможных направлениях в пространстве – пространство скоростей рабочего органа
изотропно. Это означает, что эллипсоид близок к сферическому, а его радиусы имеют величину одинакового порядка. Однако если один или несколько радиусов оказываются очень малы,
то это означает, что рабочий орган не сможет достичь большой
скорости в направлениях, соответствующих этим малым ра­
диусам.
Для планарного робота-манипулятора, показанного на
рис. 8.1
>>> planar2 = models.ETS.Planar2();

мы можем интерактивно исследовать, как изменяется форма
эллипса скорости в зависимости от конфигурации, используя
метод teach (см. рис. 8.7):
>>> planar2.teach(np.deg2rad([30, 40]), vellipse=True);
3

2
Положение рабочего органа № 0
x: 1.208

рыскание: 1.222

y: 1.44

1

Y

Угол в сочленениях
q0

30.0°

q1

40.0°

без имени

0

−1

−2

−3

−2

−1

0

X

Рис. 8.7. Двухзвенный робот с наложенным эллипсом скоростей

1

2

452 Глава 8 · Скорость манипулятора

Для робота, действующего в трехмерном пространстве задач,
уравнение (8.7) описывает шестимерный эллипсоид, который
мы не можем визуализировать. Однако мы можем выделить
часть якобиана, описывающую поступательную скорость в мировой системе отсчета. Для робота UR5 в конфигурации, показанной на рис. 8.2, получим якобиан
>>> J = ur5.jacob0(ur5.q1);

и три верхние строки, которые соответствуют скорости перемещения в пространстве задач:
>>> Jt = J[:3, :]; # первые 3 строки

из которых можно вычислить и построить эллипсоид скоростей
>>> E = np.linalg.inv(Jt @ Jt.T)
array([[ 9.005, 0.2317,
2.02],
[ 0.2317,
4.317, 0.05197],
[
2.02, 0.05197,
2.608]])
>>> plot_ellipsoid(E);

как показано на рис. 8.8а. В идеальном случае, при наличии
способности развивать одинаковую скорость в любом направлении, это была бы сфера. В данном примере линзовидная форма указывает на то, что рабочий орган не может развивать одинаковую скорость во всех направлениях. Радиусы эллипсоида
в направлениях главных осей определяются соотношением
>>> e, _ = np.linalg.eig(E);
>>> radii = 1 / np.sqrt(e)
array([ 0.3227, 0.7029, 0.4819])

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

J = ur5.jacob0(np.full((6,), np.deg2rad(1)));
Jr = J[3:, :]; # последние 3 строки
E = np.linalg.inv(Jr @ Jr.T);
plot_ellipsoid(E);

показан на рис. 8.8б. Эллипсоид получился практически плос­
ким. Радиусы можно определить так:
>>> e, x = np.linalg.eig(E);
>>> radii = 1 / np.sqrt(e)
array([ 0.03998,
1.414,

2])

причем наименьший радиус соответствует направлению
>>> x[:, 0]
array([ -0.9996, -0.01308, 0.02617])

Построение эллипсоида требует
инвертирования
переданной мат­ри­
цы, которая вычисляется с помощью
обратной мат­ри­цы.
Мы можем избавиться от второй
ненужной операции
обращения мат­ри­
цы, использовав
plot_ellipsoid(Jt
@ Jt.T,
inverted=True). См.
раздел C.1.4, где
приводятся основные сведения об
эллипсах и эллипсоидах.

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

Обусловленность якобиана и манипулируемость 453

которое почти параллельно оси x. Это указывает на невозможность вращения рабочего органа вокруг оси x, которая является потерянной степенью свободы. Оба сочленения, 3 и 5,
обеспечивают вращение вокруг мировой оси y, сочленение 4
обеспечивает вращение вокруг мировой оси z, но ни одно со­
членение не обеспечивает возможность вращения вокруг мировой оси x. В сингулярности эллипсоид будет иметь нулевую
толщину.

https://go.sn.pub/
HoVmEO

https://go.sn.pub/
gpcPbC
а

б

Рис. 8.8. Эллипсоиды скоростей рабочего органа робота UR5 (показан синим цветом):
а) эллипсоид поступательной скорости для положения, показанного на рис. 8.2 (м/с);
б) эллипсоид вращательной скорости для почти сингулярного положения (рад/с). Этот
эллипсоид практически плоский. Черные эллипсы – это тени эллипсоида на плоскостях,
перпендикулярных осям x, y и z. Эти диаграммы были построены с помощью PyVista,
а не Matplotlib

Пакет Toolbox поддерживает сокращенную запись для отоб­
ражения эллипсоида скорости, который в примере выше выглядит следующим образом:
>>> ur5.vellipse(qns, "rot");

Размер и форма эллипсоида описывают, насколько хорошо
манипулятор подготовлен к выполнению определенных движений, а манипулируемость является краткой скалярной мерой
этой подготовленности. Метод manipulability вычисляет меру
манипулируемости, предложенную Есикавой:

Определитель
является произведением собственных
значений, а радиусы
эллипсоида – квад­
ратными корнями которая пропорциональна объему эллипсоида.
собственных значений J(q)JT(q). Эти конфигурации мера манипулируемости равна
темы более подробно рассматриваются >>> ur5.manipulability(ur5.q1)
в разделе C.1.4. 0.06539

(8.8)
В рабочей

454 Глава 8 · Скорость манипулятора

Сравните ее с
>>> ur5.manipulability(ur5.qz)
0

– манипулируемостью в сингулярной конфигурации.
В этих примерах манипулируемость определяется объемом
шестимерного эллипсоида скоростей в пространстве задач, но
часто бывает полезно отдельно рассматривать поступательную
и вращательную манипулируемость. Для этого можно передать
методу параметр axes со значением "trans", "rot" или "both". Как
можно видеть, в сингулярной конфигурации
>>> ur5.manipulability(ur5.qz, axes="both")
(0,105, 2,449e-16)

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

8.3.3

Работа с вырожденным якобианом

В случае когда якобиан – квадратная мат­ри­ца и det(J(q)) = 0, мы
не можем решить уравнение (8.2) непосредственно относительно q̇. Один из способов решения проблемы сингулярности – заменить обратную функцию затухающей обратной функцией
q̇ = (J(q) + λ1)-1v,
где λ – небольшая константа, которая добавляется в диагональ
и устанавливает нижнюю границу определителя. Это вносит
некоторую погрешность в q̇, которая при интегрировании по
времени может привести к значительной ошибке в определении местоположения инструмента, но схема с обратной связью
управления движением с заданной скоростью, такая как (8.6),
минимизирует эту погрешность.
Альтернативой является использование псевдо-, обобщенного или псевдообратного якобиана Мура-Пенроуза
q̇ = J(q)+v,

(8.9)

Обусловленность якобиана и манипулируемость 455
Матричное
выражение типа
b = Ax – это система
скалярных уравнений, которую можно
решить относительно неизвестных x.
В сингулярной
конфигурации некоторые уравнения становятся
эквивалентными,
поэтому уникальных
уравнений оказывается меньше, чем
неизвестных, и мы
имеем дело с недоопределенной
системой, которая
может иметь бесконечное число
решений или не
иметь ни одного.

который представляет собой решение, минимизирующее
||J(q)q̇ - v||, ошибку между желаемой и фактической скоростью
в пространстве задач J(q)q̇, рассчитанной методом наименьших
квадратов. Ее легко вычислить с помощью функции np.linalg.
pinv из пакета NumPy.
Другой подход – удалить из якобиана все столбцы, линейно
зависящие от других столбцов. Это фактически то же самое,
что заблокировать сочленения, соответствующие удаленным
столбцам. В результате получится малоприводная (underactuated) система, о которой мы поговорим в следующем разделе.

8.3.4

Работа с неквадратным якобианом

Якобиан – это мат­ри­ца dim 𝒯 × dim 𝒞, и до сих пор мы предполагали, что якобиан имеет квадратную форму. В случаях когда эта
мат­ри­ца не является квадратной, полезно учесть соотношение
скоростей
v = J(q)q̇

(8.10)

в схематической форме, показанной на рис. 8.9 для общего
трехмерного случая, когда dim 𝒯 = 6. Якобиан в этом случае станет мат­ри­цей 6×N, скорость сочленения – N-мерным вектором
и v – шестимерным вектором.

неконтролируемая степень свободы

малоприводные

заблокированные
сочленения

полноприводные

многоприводные

Рис. 8.9. Якобиан, v и q̇ для различного числа сочленений N. Заштрихованные области
представляют части мат­ри­цы, которые можно удалить для создания квадратной подсистемы,
имеющей решение

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

Случай N < 6 называется малоприводным (underactuated) роботом. Количество сочленений в таком роботе слишком мало
для управления числом степеней свободы в пространстве задач.
Уравнение (8.10) – это переопределенная (overdetermined) система уравнений , поэтому мы можем лишь аппроксимировать
решение, а псевдообратная мат­ри­ца даст решение, минимизирующее ошибку в смысле наименьших квадратов. В качестве
альтернативы можно сделать якобиан квадратной мат­ри­цей,
удалив некоторые строки из v и J, тем самым допустив, что некоторые степени свободы в пространстве задач невозможно
контролировать из-за недостаточного количества сочленений.

456 Глава 8 · Скорость манипулятора

Случай N > 6 называется многоприводным (overactuated), или
избыточным. В многоприводном роботе больше сочленений,
чем фактически необходимо для управления всеми степенями
свободы в пространстве задач. Уравнение (8.10) – это недоопределенная система уравнений, ► и в таких случаях обычно выбирается решение с наименьшей нормой. В качестве альтернативы мы можем сделать якобиан квадратной мат­ри­цей, удалив
некоторые столбцы из J и строки из q̇, фактически заблокировав
соответствующие сочленения.

8.3.4.1

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

Якобиан малоприводного робота

Малоприводный робот имеет N < 6 сочленений, а его якобиан
больше в высоту, чем в ширину. Например, манипулятор с двумя сочленениями в номинальном положении
>>> planar2 = models.ETS.Planar2();
>>> qn = [1, 1];

с 𝒯 = ℝ2 × S1 имеет якобиан
>>> J = planar2.jacob0(qn)
array([[ -1.751, -0.9093],
[ 0.1242, -0.4161],
[
1,
1]])

Если якобиан имеет полный ранг столбцов, ► то мы можем
найти решение, используя псевдообратную мат­ри­цу, обладающую тем же свойством
J+ J = 1,
что и обратная мат­ри­ца. Она определяется как

Матрица с полным
рангом столбцов – это мат­ри­
ца, в которой все
столбцы линейно
независимы, а ранг
мат­ри­цы равен
числу столбцов.

J+ = (JT J)-1 JT
и реализуется функцией np.linalg.pinv. ► Например, если заданная скорость рабочего органа
>>> xd_desired = [0.1, 0.2, 0];

то псевдообратное решение для скорости сочленения:
>>> qd = np.linalg.pinv(J) @ xd_desired
array([ 0.08309, -0.1926])

Скорость рабочего органа:
>>> J @ qd
array([ 0.02967, 0.09047, -0.1095])

Эта псевдообратная
мат­ри­ца называется
левой обратной
мат­ри­цей, потому
что она применяется слева от J.

Обусловленность якобиана и манипулируемость 457

Скорость робота в направлениях x и y близка к заданной,
а скорость вращения вокруг оси z нежелательна. Учитывая, что
робот малоприводный, точное решение отсутствует, и это решение является наилучшим с точки зрения наименьших квад­
ратов – это компромисс. Ошибка в этом случае:
>>> np.linalg.norm(xd_desired - J @ qd)
0.1701

Мы вынуждены признать реальность: у нас всего два сочленения, и мы пытаемся использовать их для управления υx и υy.
Перепишем (8.2) следующим образом:

и, взяв верхнюю часть, первые две строки, запишем

где Jxy – мат­ри­ца 2×2, которую можно обратить, если det(Jxy) ≠ 0:

Скорость сочленения теперь равна
>>> Jxy = J[:2, :];
>>> qd = np.linalg.inv(Jxy) @ xd_desired[:2]
array([ 0.1667, -0.4309])

и приводит к скорости рабочего органа
>>> xd = J @ qd
array([
0.1,

0.2, -0.2642])

которая точно совпадает с заданной по осям x и y. Вращение
вокруг оси z неизбежно, потому что мы явно использовали две
степени свободы для управления перемещением вдоль осей x
и y, а не вращением вокруг оси z. Ошибка решения
>>> np.linalg.norm(xd_desired - J @ qd)
0.2642

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

458 Глава 8 · Скорость манипулятора

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

8.3.4.2

Якобиан многоприводного робота

У многоприводного (overactuated) или избыточного (redundant)
робота число сочленений N > 6, а его якобиан больше в ширину,
чем в высоту. В этом случае мы снова можем применить прием
с псевдообратной мат­ри­цей, но теперь решение содержит дополнительный член
q̇ = J(q)+v + P(q)q̇0.

(8.11)

Первый член – это решение с минимальной нормой из бесконечного числа возможных решений, имеющее наименьшее
значение ||q̇||. Второй член допускает бесконечное число решений, поскольку добавляется произвольная скорость в пространстве q̇0. Матрица
P(q) = 1N×N - J(q)+ JT(q) ∈ ℝN×N

(8.12)

проецирует любую скорость сочленения q̇0 в нуль-прост­ран­
ство J так, чтобы это вызвало нулевое движение рабочего органа.
Мы продемонстрируем это на примере семиосевого робота
Panda из раздела 7.2.4, находящегося в номинальном поло­жении:
>>> panda = models.ETS.Panda();
>>> TE = SE3.Trans(0.5, 0.2, -0.2) * SE3.Ry(pi);
>>> sol = panda.ikine_LMS(TE);

Его якобиан
>>> J = panda.jacob0(sol.q);
>>> J.shape
(6, 7)

– это мат­ри­ца 6×7. Теперь допустим, что мы решили задать желаемую пространственную скорость рабочего органа:
>>> xd_desired = [0.1, 0.2, 0.3, 0, 0, 0];

тогда, используя первый член (8.12), можно вычислить требуемые скорости сочленений:
>>> qd = np.linalg.pinv(J) @ xd_desired
array([ 0.4045, -0.5582, 0.04829,
0.167,
0.3502])

0.2591, -0.6515,

Как видите, все сочленения имеют ненулевую скорость и обес­
печивают заданную скорость движения рабочего органа ►:

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

Обусловленность якобиана и манипулируемость 459
>>> J @ qd
array([
0.1,

0.2,

0.3,

0,

0,

0])

Якобиан в этом случае имеет семь столбцов, а его ранг равен
шести.
>>> np.linalg.matrix_rank(J)
6
См. приложение B.

Следовательно, имеется нуль-пространство
ным вектором, заданным как вектор-столбец
>>> N = linalg.null_space(J);
>>> N.shape
(7, 1)
>>> N.T
array([[ -0.4231, -0.1561, -0.7747, -0.007397,
0.1591]])

с одним базис-

0.4094, 0.06011,

Любая скорость сочленений, являющаяся линейной комбинацией базисных векторов в нуль-пространстве, приведет к нулевому движению рабочего органа. Для этого робота существует только один базисный вектор, и можно легко показать, что
движение сочленения в нуль-пространстве приводит к нулевому
движению рабочего органа.
>>> np.linalg.norm( J @ N[:,0])
3.132e-16

Если базисные векторы нуль-пространства расположены как
столбцы в мат­ри­це N(q) ∈ ℝN×R, где R = N - dim 𝒯, тогда альтернативное выражение для проекционной мат­ри­цы будет иметь
вид
P(q) = N(q)N(q)+.
Движение в нуль-пространстве можно использовать для
управления избыточными роботами, чтобы избежать столк­
новений звеньев с препятствиями (включая другие звенья)
или чтобы координаты сочленений не касались механических
ограничителей. Предположим, что помимо желаемой скорости
в пространстве задач необходимо одновременно увеличить q4,
чтобы отвести руку от некоторого препятствия. Задаем желаемую скорость сочленения
>>> qd_0 = [0, 0, 0, 0, 1, 0, 0];

и проецируем ее в нуль-пространство
>>> qd = N @ np.linalg.pinv(N) @ qd_0
array([ -0.1732, -0.06391, -0.3171, -0.003028,
0.06513])

0.1676, 0.02461,

460 Глава 8 · Скорость манипулятора

Здесь добавлено масштабирование, но скорость этого сочле­
нения, или ее масштабированная версия, увеличит q4 без изменения положения конечного органа. Другие сочленения тоже
сдвинутся – это компенсирующее движение обеспечит неизменность положения рабочего органа
>>> np.linalg.norm(J @ qd)
1.129e-16

Робот-змея с высокой избыточностью, подобный показанному на рис. 8.10, будет иметь нуль-пространство с 20 - 6 = 14 измерениями. Это можно использовать для точного управления
формой манипулятора независимо от положения рабочего органа. Это может быть очень важно при работе в ограниченном
пространстве.

Рис. 8.10. Робот-змея с манипулятором, имеющим 20 степеней свободы: радиус действия
2.5 м, диаметр 90 мм и грузоподъемность 25 кг (изображение предоставлено OC Robotics)

Отношения сил 461

8.4

Отношения сил

В разделе 3.2.2 мы ввели понятие динамического винта (wrench)
w = (fx, f y, fz, mx, my, mz) ∈ ℝ6 – вектора сил и моментов. Динамический винт рабочего органа связан с моментами или силами
в сочленении посредством мат­ри­цы Якоби манипулятора.

8.4.1

Преобразование динамических винтов
в пространство сочленений

Динамический винт 0wE, приложенный к рабочему органу и определенный в мировой системе координат, преобразуется в прост­
ранство сочленений транспонированием якобиана манипулятора
Q = 0J T(q) 0wE

(8.13)

в обобщенный вектор сил, приложенных к сочленению Q ∈ ℝN,
элементами которого являются крутящий момент или сила для
поворотных или призматических сочленений соответственно.
Если динамический винт определен в системе координат рабочего органа, то используется преобразование
Q = E J T(q) EwE.

(8.14)

Для робота UR5 в конфигурации, показанной на рис. 8.2, сила
20 Н, приложенная в направлении мировой оси y, толкнет рабочий орган вбок, что приведет к возникновению моментов в со­
членениях
>>> tau = ur5.jacob0(ur5.q1).T @ [0, 20, 0, 0, 0, 0]
array([ 9.491,
0,
0,
0, -1.646,

0])

и к значительному крутящему моменту в талии робота из-за
эффекта рычага и к ее повороту. Сила 20 Н, приложенная в направлении оси x, оттянет рабочий орган от основания, что приведет к крутящему моменту в талии робота
>>> tau = ur5.jacob0(ur5.q1).T @ [20, 0, 0, 0, 0, 0]
array([ -0.537,
6.607, -1.893, -1.893, -1.646,

0])

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

462 Глава 8 · Скорость манипулятора

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

8.4.2

Эллипсоиды сил

В разделе 8.3.2 мы ввели понятия эллипса и эллипсоида скоростей, описывающих направления, в которых рабочий орган
может двигаться наиболее свободно. Аналогичный анализ можно провести для динамического винта рабочего органа. Начнем
с множества обобщенных сил в сочленениях, имеющего единичную норму ||Q|| = 1:
QTQ = 1.
Подставляя (8.13), можно записать
w T(J(q)JT(q))w = 1.
Это уравнение точек w на поверхности шестимерного гипер­
эллипсоида в пространстве динамического винта. Для планарного манипулятора, показанного на рис. 8.1, можно интерактивно исследовать изменение формы эллипса силы в зависимости
от конфигурации, использовав метод teach:
>>> planar2.teach(np.deg2rad([30, 40]), fellipse=True);

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

Численная обратная кинематика 463

8.5

Численная обратная кинематика

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

текущая
конфигурация
сочленений

желаемое положение
рабочего органа
пружина
текущее положение
рабочего органа

Рис. 8.11. Схема численного метода решения задачи обратной кинематики, показывающая
текущую и желаемую конфигурации манипулятора. На схему наложен граф положений,
показывающий текущее положение рабочего органа ξE, желаемое положение рабочего
∗
органа ξE и необходимое изменение положения

Наложенный граф положения показывает, что ошибка между
фактическим ξE и желаемым ξE∗ положениями составляет величину ξ Δ, которую можно описать пространственным смещением, как обсуждалось в разделе 3.1.5:
Δ = Δ(ξE, ξE∗) = (t, υ̂θ) ∈ ℝ6,

E

где текущее положение вычисляется с использованием прямой
кинематики ξE = 𝒦(q).
Представьте, что между двумя положениями рабочего органа
существует своеобразная пружина, которая тянет (и скручивает) рабочий орган робота в направлении желаемого положения
с усилием, пропорциональным пространственному смещению
w = γ EΔ,

Е

(8.15)

которое преобразуется в обобщенные силы в сочленениях
Q = E J T(q) Ew

464 Глава 8 · Скорость манипулятора

c использованием транспонированного якобиана (8.14). Предполагается, что у этого виртуального робота нет двигателей
в сочленениях, а есть только вязкие демпферы, поэтому скорость сочленения будет пропорциональна этим силам в сочленениях
q̇ = Q /B,
где B – коэффициент демпфирования соединения (предполагается, что все демпферы одинаковы). Собрав все вместе и объединив константы α = γ/B, мы можем записать:
q̇ = αJT(q)Δ(𝒦(q), ξЕ∗).

Это скорость сочленения, которая будет направлять решение
прямой кинематики к желаемому положению рабочего органа.
Уравнение можно решить численно, выполняя итерации
δq〈k〉 = α JT(q〈k〉)Δ(𝒦(q), ξЕ∗),
q〈k+1〉 ← q〈k〉 + δq〈k〉,

(8.16)

пока норма обновления ||δq〈k〉|| не станет достаточно малой, где
α > 0 – специально подобранная константа. Поскольку решение
основано на транспонировании, а не на обращении якобиана,
алгоритм может применяться даже в случаях, когда якобиан неквадратный или вырожденный. Однако на практике этот алгоритм медленно сходится и очень чувствителен к выбору α.
Эту задачу можно также сформулировать как задачу наименьших квадратов в мировой системе координат и минимизировать скалярную стоимость
e = EΔT M EΔ,
где M = diag(m) ∈ ℝ6×6 и m – вектор маски, введенный в разделе 7.2.3. Обновление в этом случае выражается как
δq〈k〉 = (JT(q〈k〉)MJ(q〈k〉))-1 JT(q〈k〉)M Δ(𝒦(q〈k〉), ξЕ∗)

и обеспечивает более быструю сходимость, но может вести себя
некорректно вблизи сингулярностей. Мы исправим эту проблему, введя константу затухания λ:
δq〈k〉 = (JT(q〈k〉)MJ(q〈k〉) + λ1N×N)-1 JT(q〈k〉)M Δ(𝒦(q〈k〉), ξЕ∗),

которая гарантирует, что обращаемый член никогда не превратится в вырожденную мат­ри­цу.
Эффективный способ выбора λ – проверить, уменьшает ли итерация ошибку, то есть выполняется ли условие ||δq〈k〉|| < ||δq〈k – 1〉||.

Дополнительные темы 465

Если ошибка уменьшается, то можно уменьшить λ, чтобы ускорить сходимость. Если ошибка увеличивается, то следует вернуться к предыдущей оценке q〈k〉 и увеличить λ. Эта схема адаптивного
изменения коэффициента затухания лежит в основе известного
алгоритма оптимизации Левенберга–Марквардта.
Этот алгоритм реализован в методе ikine_LM и хорошо зарекомендовал себя на практике. Как и все алгоритмы оптимизации,
он требует разумной начальной оценки q〈k〉, которую можно
явно задать с помощью аргумента q0. Если аргумент q0 не задан,
то выбираются случайные значения конфигурации сочленений
и попытки повторяются до тех пор, пока оптимизация не сойдется. Этот аспект контролируется параметром slimit.
Важный вывод заключается в том, что для вычисления обратной кинематики нужен лишь итерационный алгоритм, как
обсуждалось выше, прямая кинематика и якобиан. Их легко вычислить для последовательного манипулятора с произвольным
числом сочленений.

8.6

Дополнительные темы

8.6.1

Якобиан манипулируемости

Иногда бывает полезно знать, как изменится манипулируемость в зависимости от конфигурации сочленений и что якобиан манипулируемости имеет элементы dm/dqi. Например:
>>> panda = models.ETS.Panda()
>>> panda.jacobm(panda.qr).T
array([[ 0, -0.002627,
0, 0.04064,

0, -0.02734,

0]])

указывает на тесную связь между q3 и манипулируемостью –
увеличение q3 увеличивает манипулируемость, а увеличение
q5 – уменьшает. Этот вектор можно интерпретировать как скорость в пространстве сочленений и спроецировать в нуль-про­
стран­ство как q̇0 в (8.11). Это изменит конфигурацию сочленения
таким образом, чтобы при этом увеличился объем эллипсоида
скорости, а положение рабочего органа осталось неизменным.

8.6.2

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

В разделе 7.5.4 мы вычисляли прямую кинематику как произведение экспонент на основе винтов, представляющих оси

466 Глава 8 · Скорость манипулятора

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

описывающей скорость манипулятора в мировой системе координат. Якобиан имеет очень элегантное представление, его
легко построить столбец за столбцом, и он связывает скорость
в системе координат рабочего органа со скоростью сочленений.
–
E–
J = Ad(Eξ0) 0J ,
где Ad(·) – сопряженная мат­ри­ца, представленная в разделе 3.1.3.
–
 кобианы J , отмеченные чертой сверху, представляют скорость раЯ
бочего органа как угловую, а не пространственную скорость. Различия описаны в разделе 3.1.3.

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

Это преобразование реализуется методом jacob0 класса PoERobot.

8.6.3

 анипулируемость, масштабирование
М
и единицы измерения

Значения элементов мат­ри­цы Якоби зависят от выбора физических единиц, используемых для описания длин звеньев и координат сочленений. То же относится к определителю Якоби
и мере манипулируемости. При одинаковой структуре сочленений робот меньшего размера будет иметь меньшую манипулируемость, чем робот большего размера.
Мера манипулируемости Есикавы (8.8) – это объем эллипсоида скорости, но она не оценивает изотропию скорости. Эта
мера даст одно и то же значение как для большого и плоского,
так и для меньшего и более сферического эллипсоида. Исследователями было предложено множество других мер, лучше описывающих изотропию, например отношение максимального
и минимального сингулярных значений.

Подведение итогов 467

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

8.7

Подведение итогов

Якобианы – важная концепция в робототехнике, связывающая
скорость в одном пространстве со скоростью в другом. Мы уже
встречали якобианы в главе 6 и будем использовать их позже
для нужд компьютерного зрения и управления.
В этой главе мы познакомились с якобианом, который описывает связь между скоростью изменения координат сочленений
и пространственной скоростью рабочего органа, выраженной
либо в мировой системе координат, либо в системе координат
рабочего органа. Мы показали, как можно использовать обратный якобиан для преобразования желаемой декартовой скорости в скорость сочленений и построения траекторий в пространстве задач для мало- и многоприводных роботов. Мы показали,
что движения в нуль-пространстве можно использовать для
перемещения сочленений многоприводного робота, не влияя
на положение его рабочего органа. Числовые свойства якобиана
позволяют также оценить манипулируемость – способность манипулятора двигаться в различных направлениях пространства
задач, которую мы визуализировали в виде эллипсоида скоростей. В случае сингулярности, когда между столбцами якобиана
имеет место линейная зависимость, робот не сможет двигаться
в определенных направлениях. Мы также создали якобианы для
отображения угловой скорости в угловые скорости крена, тангажа и рыскания или углы Эйлера, которые были использованы
для формирования аналитической мат­ри­цы якобиана.
Эллипсоид сил описывает силы, которые рабочий орган может прилагать в различных направлениях. Для преобразования
крутящих моментов, приложенных к рабочему органу, в кру-

468 Глава 8 · Скорость манипулятора

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

8.7.1

Дополнительное чтение

Якобиан манипулятора рассматривается практически во всех
стандартных учебниках по робототехнике, таких как «Robotics
Handbook» (Siciliano и Khatib, 2016), Lynch and Park (2017), Siciliano et al. (2009), Spong et al. (2006), Craig (2005) и Paul (1981). Отличное обсуждение манипулируемости и эллипсоидов скорости
представлено в работе Siciliano et al. (2009), а наиболее распространенная мера манипулируемости предложена в Yoshi­kawa
(1984). В Patel et al. (2015) представлен всесторонний обзор мер,
характеризующих робота, включая меры манипулируемости
и изотропии скорости. Вычисление якобиана манипулятора на
основе параметров Денавита–Хартенберга, реализованное в пакете Toolbox, впервые было описано в Paul and Shimano (1978).
Схема управления движением с заданной скоростью была
предложена в Whitney (1969). Расширения, такие как управление на основе псевдообратного якобиана, рассматриваются
в Klein and Huang (1983), а методы наименьших квадратов с затуханием – в Deo and Walker (1995).

8.7.2

Упражнения

1. Для простого манипулятора с двумя звеньями (раздел 8.1.1)
вычислите определитель символически и выясните, при каких условиях он равен нулю. Что это означает физически?
2. Добавьте к роботу UR5 инструмент длиной 10 см в направлении
оси z рабочего органа. Как это изменит блок 3×3 вверху справа
в якобиане? А если инструмент будет иметь длину 30 см?
3. Выведите аналитический якобиан для углов Эйлера.
4. Управление движением с заданной скоростью (раздел 8.2).
а) Поэкспериментируйте с различными требованиями к поступательной и вращательной скоростям в декартовой
системе координат и их комбинациями.
б) Дополните блок-схему на рис. 8.3 и впишите в нее определитель мат­ри­цы Якоби.
в) Движение робота, представленное графиками на рис. 8.4,
моделируется в течение 5 с. Увеличьте время моделирования до 10 с и объясните происшедшие изменения.

Подведение итогов 469

5.

6.

7.

8.

г) Установите начальное положение и направление движения, имитирующие положение в разделе 7.3.4. Что случится, когда робот достигнет сингулярности?
д) Замените блок обращения якобиана на рис. 8.3 функцией
np.linalg.pinv из пакета Numpy.
е) Замените блок обращения якобиана на рис. 8.3 затухающей функцией наименьших квадратов и исследуйте влия­
ние различных значений коэффициента затухания.
ж) Замените блок обращения якобиана на рис. 8.3 функцией
np.linalg.lstsq из пакета Numpy.
Примените интерактивный графический инструмент teach
к роботу PUMA 560 в его уникальной конфигурации. Отрегулируйте сочленения 3 и 5 и посмотрите, как они приводят
к одинаковому движению рабочего органа.
Эллипсоиды скорости и силы для двухзвенного манипулятора (разделы 8.3.2 и 8.4.2). Ответьте на следующие вопросы
(при желании можете использовать метод teach).
а) Какая конфигурация обеспечивает наилучшую манипулируемость?
б) Какая конфигурация лучше всего подходит для броска
мяча вдоль положительного направления оси x?
в) Какая конфигурация лучше всего подходит для переноски тяжелого груза, если сила тяжести действует в направлении оси –y?
г) Постройте эллипс скоростей (по осям x и y) двухзвенного
манипулятора на сетке местоположений рабочего органа
в его рабочем пространстве. Центр каждого эллипсоида
должен находиться в соответствующей точке местоположения рабочего органа.
Эллипсоиды скорости и силы для манипулятора UR5.
а) Найдите конфигурацию, в которой манипулируемость
больше, чем в q1.
б) Найдите конфигурацию, которая максимизирует манипулируемость.
в) Используйте метод teach с параметрами vellipse и fellipse и исследуйте эллипсоиды в интерактивном режиме.
г) Повторите то же самое для избыточного робота, такого
как Panda.
Модель models.ETS.XYPanda() описывает робота с девятью
сочленениями (PPRRRRRRR), включающего основание xy
(PP), к которому крепится манипулятор Panda (RRRRRRR).
а) Вычислите нуль-пространство и покажите, что комбинации его базисных векторов вызывают нулевое движение
рабочего органа. Попробуйте физически интерпретировать эти векторы.

470 Глава 8 · Скорость манипулятора

б) Вычислите траекторию рабочего органа в пространстве
задач и используйте численную обратную кинематику
для определения координат сочленений. Проанализируйте, как распределяется движение между основанием
и манипулятором робота.
в) Удерживая рабочий орган в постоянном положении,
исследуйте управление в нуль-пространстве. Задайте
скорость для подвижного основания и посмотрите, как
конфигурация руки ее учитывает.
г) Разработайте контроллер нуль-пространства, который
удерживает последние сочленения руки робота в середине их рабочего диапазона, используя первые два
сочленения для позиционирования основания руки. Измените его так, чтобы максимально увеличить манипулируемость робота XYPanda.
д) Представьте, что манипулятор Panda установлен на неголономном роботе. Создайте контроллер, который
будет генерировать соответствующие сигналы, управляющие движением и скоростью мобильного робота
(сложная задача).
е) Для произвольного положения и конечной пространственной скорости необходимо перемещать шесть сочленений и блокировать два. Напишите алгоритм, который
бы определял, какие два сочленения следует заблоки­
ровать.
9. Докажите или продемонстрируйте численно эквивалентность двух уравнений проекции в нуль-пространство: 1 J(q)+JT(q) и N(q)N(q)+.
10. Модель models.DH.Hyper3d(20) – это робот с 20 сочленениями,
перемещающийся в трехмерном пространстве.
а) Изучите возможности этого робота.
б) Вычислите траекторию рабочего органа в пространстве
задач, которая описывает окружность на земле, и используйте численное решение обратной кинематики
для определения координат сочленений.
в) Добавьте стратегию управления нуль-пространством,
которая удерживает все углы сочленений близкими
к нулю во время движения.
г) Определите целевое положение рабочего органа на
земле, которого робот должен достичь после прохождения через отверстие в стене. Попробуйте определить конфигурацию сочленений, которая позволит это
сделать. Можно ли сделать то же самое для отверстий
в двух стенах?
11. Напишите код, вычисляющий якобиан объекта робота с использованием кручений, как описано в разделе 8.6.2, и сравните результаты.

Подведение итогов 471

12. Пусть в рабочем пространстве робота UR5 есть вертикальная плоскость. Разбейте плоскость на ячейки по сетке с шагом 2 см и для каждой ячейки определите ее достижимость
для робота. Если ячейка достижима, то определите для нее
манипулируемость первыми тремя сочленениями манипулятора. Нарисуйте тепловую карту манипулируемости
робота в плоскости. Перемещайте плоскость относительно
робота и наблюдайте, как меняется тепловая карта.
13. а)	Создайте версию модели PUMA 560 в нотации Денавита–Хартенберга, указав все линейные размеры в дюймах вместо метров. Как изменилась манипулируемость
при той же конфигурации сочленений, по сравнению
с метрической версией?
б)	Создайте версию модели PUMA 560 в нотации Денавита–Хартенберга, уменьшив все линейные размеры в десять раз. Как изменилась манипулируемость при той же
конфигурации сочленений?
в)	Создайте версию модели PUMA 560 в нотации Денавита–Хартенберга, увеличив все линейные размеры в десять раз. Как изменилась манипулируемость при той же
конфигурации сочленений?

Глава

9

	Динамика и управление

Эта глава раскрывает тему динамики многозвенного манипулятора и управления им. Сначала мы рассмотрим управление
отдельными сочленениями, которое тесно связано с особенностями управления скоростью колес и винтов мобильных роботов. В разделе 9.1 описываются ключевые элементы системы
управления сочленениями робота, которая обеспечивает движение одного сочленения по заданной траектории, а также рассматриваются практические проблемы, такие как трение, гравитационная нагрузка и переменная инерция.
Движение рабочего органа является суммой движения всех
звеньев, а сами звенья в конечном итоге приводятся в движение силами и моментами, создаваемыми сочленяющими приводами. Каждое звено манипулятора поддерживается силой
реакции и моментом предыдущего звена и подвержено воздействию собственного веса, а также сил и моментов со стороны
звеньев, которые оно поддерживает. В разделе 9.2 представлены уравнения движения жесткого тела – набор связанных нелинейных дифференциальных уравнений, которые описывают
моменты в сочленениях, необходимые для достижения определенного состояния манипулятора.
Эти уравнения можно разложить на члены, описывающие
инерцию, гравитационную нагрузку и гироскопическую связь,
дающие представление о том, какое воздействие на другие
сочленения оказывает движение одного сочленения и как эти
члены меняются в зависимости от конфигурации и полезной
нагрузки. В разделе 9.3 будет представлена прямая динамика
движения манипулятора, то есть изменение его конфигурации
с течением времени в ответ на приложенные силы и крутящие
моменты, обусловленные приводами сочленений, нагрузкой на
рабочий орган и гравитацией.
В разделе 9.4 представлены системы управления, явно учитывающие динамику жесткого тела, что приводит к улучшению управления местоположением. Во многих применениях
требуется управление силой и местоположением, поэтому
в разделе 9.5 будет представлено введение в динамику рабочего пространства, где динамика робота абстрагируется до
точечной массы, а динамика и управление выражаются в терминах рабочего пространства. В разделе 9.6 представлены два

chap9.ipynb

https://go.sn.pub/
zpWGvB

Независимое управление сочленениями 473

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

9.1

Независимое управление сочленениями

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

9.1.1

Приводы

Большинство современных роботов приводятся в движение
роторными электродвигателями. В крупных промышленных
роботах обычно используются бесщеточные серводвигатели, а в небольших лабораторных или любительских роботах –
щеточные двигатели постоянного тока или шаговые двигатели. Может также использоваться пневматический привод
с цилинд­рами или диафрагмами, хотя и нечасто, преимущество
которого состоит в том, что он позволяет генерировать движение, сохраняя при этом естественную податливость. В случаях,
когда требуется создать очень большое усилие, обычно используется электрогидравлический привод – гидравлический привод с электроуправляемыми гидравлическими клапанами. Он
применяется в манипуляторах для манипулирования очень
большими грузами, используемых в горнодобывающей промышленности, лесном хозяйстве или строительстве, а также
в высокодинамичных гуманоидах, таких как робот Atlas, показанный на рис. 7.3б.

474 Глава 9 · Динамика и управление
Отступление 9.1. Сервомеханизм
Сервомеханизм, или сервопривод, – это автоматическое устройство, предназначенное
для перемещения механизма в нужное местоположение и использующее обратную
связь об ошибке между желаемым и фактическим местоположениями. Возможно,
первым таким устройством была рулевая
машина корабля (изображение предоставлено WikiCommons), разработанная Джоном
Макфарлейном Греем для парохода «ГрейтИстерн», построенного инженером Изамбардом Кингдомом Брюнелем в 1866 го­ду. Слово «серво» происходит от латинского корня
«servus», означающего «раб», и впервые было
использовано в 1868 году Дж. Дж. Л. Фарко –
«Le Servomoteur» – для описания гидравлических и паровых двигателей, которые используются для управления судами.
Погрешность местоположения измеряется
датчиком, а затем усиливается для передачи на двигатель, который генерирует усилие
для перемещения устройства, уменьшая погрешность. Развитие сервосистем было стимулировано Второй мировой войной, когда появились электрические сервосистемы управления огнем, использовавшие электродвигатели и электромеханические
усилители. Позднее в сервоусилителях использовались электронные лампы, а в последнее время – твердотельные усилители мощности (электронные регуляторы скорости). В настоящее время сервомеханизмы широко применяются для позиционирования головок чтения/записи в оптических и магнитных дисководах, объективов
в камерах с автофокусировкой, в игрушках с дистанционным управлением, в антеннах слежения за спутниками, в автоматических станках и в сочленениях роботов.
В контексте управления, основанного на зрении, мы будем использовать термин «визуальное сервоуправление».

Основные элементы системы электропривода показаны на рис. 9.1. Электродвигатели могут управляться как силой тока, так и напряжением. ► Здесь предполагается, что управление
производится силой тока, когда контроллер
двигателя выдает ток
im = Kau,
пропорциональный приложенному управляющему напряжению u, где Ka – крутизна усилителя, измеряемая в единицах AV-1. Крутящий
момент, создаваемый двигателем, пропорцио­
нален току

Регулирование силы тока осуществляется
с помощью электронного источника постоянного тока, который представляет собой
источник переменного напряжения с обратной связью по текущему току двигателя. Источник переменного напряжения чаще всего
реализуется с помощью схемы с широтноимпульсной модуляцией (ШИМ). Альтернативный вариант – управление напряжением,
но он требует при проектировании системы
управления учитывать электрическую
динамику двигателя, обусловленную его
сопротивлением и индуктивностью, а также
обратную ЭДС (напряжение, создаваемое
вращающимся двигателем, которое противодействует приложенному напряжению).

Независимое управление сочленениями 475

τm = Kmim,
Эта константа
зависит от силы постоянных магнитов
и количества витков
провода в катушках
двигателя.

(9.1)

где Km – постоянная крутящего момента двигателя, измеряемая в единицах Н·м/А. Этот крутящий момент ускоряет вращение двигателя – его якоря, или ротора, – имеющего момент
инерции Jm и скорость вращения ωm. Движению противодействует момент трения τf, который состоит из многих компонентов.

усилитель двигатель
мощности

энкодер

инерция
мотора

трение
мотора

Рис. 9.1. Ключевые компоненты привода сочленения манипулятора. Напряжение
управляет током im, протекающим через двигатель, который создает крутящий момент τm,
увеличивающий момент инерции вращения Jm и противодействующий силе трения τf .
Энкодер измеряет скорость и угол поворота

9.1.2

Трение

Любой вращающийся механизм, будь то двигатель или редуктор, подвержен воздействию трения – силы или крутящего момента, противодействующего движению. Результирующий крутящий момент двигателя равен
τ¢ = τm - τf,
где τf – момент трения, который является функцией скорости
τf = Bωm + τC,

(9.2)

где наклон B > 0 – коэффициент вязкого трения, а смещение –
кулоновское трение, которое обычно моделируется как
(9.3)
В общем случае коэффициенты трения зависят от направления вращения, и эта асимметрия более выражена для кулоновского трения, чем для вязкого.
Общий момент трения как функция скорости вращения показан на рис. 9.2. На очень низких скоростях преобладает трение Штрибека. Приложенный крутящий момент должен превы-

476 Глава 9 · Динамика и управление

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

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

режим низкой скорости
момент
трения

трение
Штрибека

вязкое
трение

кулоновское
трение

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

Отступление 9.2. Шарль-Огюстен де Кулон
Кулон (1736–1806) – французский физик, родившийся в Ангулеме в богатой семье. Изучал математику
в Коллеже Четырех Наций у Пьера Шарля Монье,
а затем в военной школе в Мезьере. Восемь лет провел на Мартинике, участвуя в строительстве форта
Бурбон, где заразился тропической лихорадкой.
Позднее работал на верфях в Рошфоре, которые
использовал в качестве лаборатории для своих
экспериментов по статическому и динамическому трению скользящих поверхностей. Его работа
«Теория простых машин» получила Гран-при Академии наук в 1781 году. Последующие исследования ученого были посвящены электромагнетизму
и электростатике. Он наиболее известен своей формулой расчета электростатических сил, названной в его честь, как и единица измерения заряда в системе СИ.
После революции участвовал в разработке новой системы мер и весов.

Независимое управление сочленениями 477

9.1.3

Масса звена

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

ин

ер

усилитель двигатель
мощности

энкодер

инерция
мотора

трение
мотора

ци

ян

агр

узк

и

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

На рис. 9.4 показан простой манипулятор с двумя поворотными сочленениями, работающий в вертикальной плоскости.
Сочленение 0 поворачивает красное звено относительно неподвижной рамы основания. Предполагается, что масса красного звена сосредоточена в его центре масс (center of mass, CoM),
поэтому дополнительный момент инерции звена составит m1r12.
Двигатель также будет испытывать инерцию синего звена, зависящую от значения q1: момент инерции манипулятора максимален, когда манипулятор выпрямлен, и уменьшается при
его сгибании.

Рис. 9.4. Двухзвенный манипулятор, аналогичный изображенному на рис. 7.7. Двигается
в вертикальной плоскости, сила тяжести направлена вниз. Показаны системы координат
звеньев и соответствующие размеры. Центр масс (CoM) каждого звена обозначен значком
и смещен на расстояние в направлении оси x, соответствующей системе координат {i}

478 Глава 9 · Динамика и управление

Гравитация действует на центр масс ► красного звена, создавая крутящий момент на двигателе сочленения 0, пропорциональный cos q0. Сила тяжести, действующая на центр масс
синего звена, тоже создает крутящий момент на двигателе со­
членения 0, и этот момент более выражен, потому что он действует на большем расстоянии от двигателя, то есть эффект ры­
чага в этом случае больше.
Эти эффекты очевидны даже при беглом взгляде на рис. 9.4,
но в реальности ситуация намного сложнее. Как будет обсуждаться в следующем разделе, мы можем использовать Toolbox ► для определения крутящего момента, действующего на
каждое сочленение, как функции местоположения, скорости
и ускорения сочленений. Для начала определим модель робота,
как это делалось в главе 7, но на этот раз длины звеньев определим как символические значения. Инерционные параметры
каждого звена, массу и местоположение центра масс, а также
силу тяжести тоже определим символически:
>>> import sympy
>>> a1, a2, r1, r2, m1, m2, g = sympy.symbols("a1 a2 r1 r2 m1 m2 g")
>>> link1 = Link(ET.Ry(flip=True), m=m1, r=[r1, 0, 0], name="link0")
Link("link0", Ry(-q), isflip=1, m=m1, r=[r1, 0, 0],
I=[0, 0, 0, 0, 0, 0], Jm=0, B=0, Tc=[0, 0], G=0)
>>> link2 = Link(ET.tx(a1) * ET.Ry(flip=True), m=m2, r=[r2, 0, 0],
name="link1")
Link("link1", tx(a1) ˚ Ry(-q), isflip=1, m=m2, r=[r2, 0, 0],
I=[0, 0, 0, 0, 0, 0], Jm=0, B=0, Tc=[0, 0], G=0)
>>> robot = ERobot([link1, link2])
ERobot2: noname, 2 joints (RR), dynamics
link
link
joint
parent
ETS: parent to link
0
link1
0
BASE
Ry(-q0)
1
@link1
1
link0
tx(a1) ⊕ Ry(-q1)

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

Также называется
центром тяжести.

Для этого потребуется пакет SymPy.

На рис. 9.4 ось y направлена от наблюдателя (в глубину
книжной страницы),
>>> robot.dynamics()
а это означает, что
j
m
r
I
Jm
B
Tc
q0 и q1 – это поворот
вокруг оси –y. Это
link0
m1
r1, 0, 0
0, 0, 0, 0, 0, 0
0
0
0, 0
отрицательное враlink1
m2
r2, 0, 0
0, 0, 0, 0, 0, 0
0
0
0, 0
щение сочленения
* Этот метод выводит слишком широкую таблицу, не умещающуюся по ширине задается с помощью
книжной страницы.
параметра flip
метода ETS.ry().

Далее определим некоторые символические кортежи, задающие угол, скорость и ускорение сочленения:
>>> q = sympy.symbols("q:2")
(q0, q1)
>>> qd = sympy.symbols("qd:2")
(qd0, qd1)

Независимое управление сочленениями 479
>>> qdd = sympy.symbols("qdd:2")
(qdd0, qdd1)

Крутящие моменты в сочленениях определяются как функция от q, q̇ и q̇˙:
>>> tau = robot.rne(q, qd, qdd, gravity=[0, 0, g], symbolic=True);

Она возвращает двухмерный (по количеству сочленений)
символический вектор с элементами, содержащими удивительно большое количество членов, которые можно представить,
как показано ниже:
τ0 = m00(q1)q̇˙0 + m01(q1)q̇˙1 + c1(q1)q̇0q̇1 + c2(q1)q̇12 + g(q0, q1) ,
возмущение

m00(q1) = m r + m2(a + r + 2a1r2 cos q1),
2
1 1

2
1

2
2

m01(q1) = m2r2(r2 + a1 cos q1),
c1(q1) = 2a1m2r2 sin q1,
c2(q1) = a1m2r2 sin q1,
g(q0, q1) = (m1r1 + a1m2)g cos q0 + m2r2 cos(q0 + q1)g,

(9.4)

где g – ускорение свободного падения, направленное вниз. Это
второй закон Ньютона, в котором инерция зависит от q1 и ряда
возмущений. Как уже обсуждалось, момент силы тяжести g(·)
зависит от q0 и q1. Но самое, пожалуй, удивительное заключается в том, что момент силы, приложенный к сочленению 0,
зависит от скорости и ускорения сочленения 1, но об этом мы
поговорим в разделе 9.2.
Подводя итог, можно сказать, что эффект движения сочленений в последовательности механических звеньев нетривиален.
Каждое сочленение получает крутящий момент, зависящий от
положения, скорости и ускорения всех остальных сочленений,
и вычислить его для манипулятора с большим количеством со­
членений становится довольно сложно.

9.1.4

Редуктор

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

480 Глава 9 · Динамика и управление

монические передачи привносят еще и пульсации крутящего
момента. Высокопроизводительные роботы, например применяемые в высокоскоростной сборке электроники, используют дорогостоящие двигатели с высоким моментом и прямым
приводом или очень низким передаточным отношением, достигаемым с помощью тросов или тонких металлических лент
вместо шестерен.
На рис. 9.5 показана полная схема привода типичного сочленения манипулятора. Для редуктора G:1 крутящий момент на
звене равен произведению G на крутящий момент двигателя.
Величины, измеренные на звене в системе отсчета l, соотносятся с величинами, измеренными на двигателе в системе отсчета m, как показано в табл. 9.1. Инерция нагрузки уменьшается
в G 2 раз, а возмущающий момент – в G раз. ►
сторона сторона
двигателя нагрузки

редуктор
усилитель двигатель энкодер
мощности

инерция
мотора

трение
мотора

инерция
нагрузки

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

Таблица 9.1. Соотношение между крутящими моментами на нагрузке (l·)
и двигателе (m·) в зависимости от передаточного числа редуктора
l
l

J = G2 mJ
B = G2 mB

τ = G mτ
τC = G mτC

ω = mω/G
ω̇ = mω̇/G

l

l

l

l

Двигатель испытывает две составляющие инерции. Первая
обусловлена вращающейся частью самого двигателя, его ротора, и обозначается как mJm. Это постоянная величина, являющаяся внутренней характеристикой двигателя, которая приводится производителем в техническом описании двигателя.
Вторая составляющая – переменный момент инерции нагрузки
l
Jl, определяемый как сумма моментов инерции ведомого звена
и всех остальных звеньев, прикрепленных к нему. Для сочленения j это элемент mjj мат­ри­цы инерции, зависящей от конфигурации, которая будет представлена в разделе 9.2.2.

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

Независимое управление сочленениями 481

9.1.5

Моделирование сочленения робота

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

щетка

редуктор
выходной вал

Рис. 9.6. Схема интегрированного приводного блока, включающего двигатель, энкодер
и редуктор (изображение предоставлено maxon precision motors, inc.)

Собрав уравнения, представленные выше, можно записать
баланс крутящего момента на валу двигателя относительно
двигателя как
(9.5)
где B¢, τ¢C и J¢ – эффективное полное вязкое трение, кулоновское
трение и инерция, обусловленные двигателем, редуктором,
подшипниками и нагрузкой
Кулоновское трение, часто весьма
существенное,
можно не игнорировать, а заменить
дополнительным
эквивалентным
вязким трением
B¢ = Bm + Bl /G2 + τ¢C /ω
в рабочей точке.

(9.6)
Чтобы проанализировать динамику (9.5), необходимо сначала линеаризовать ее, для чего достаточно установить все аддитивные константы равными нулю:
J¢ω̇ + B¢ω = Km Kau

482 Глава 9 · Динамика и управление

– и затем применить преобразование Лапласа
sJ¢ Ω(s) + B¢Ω(s) = Km KaU(s),
где Ω(s) и U(s) – это преобразование Лапласа сигналов во временной области ω(t) и u(t) соответственно. Это можно представить в виде линейной передаточной функции
(9.7)
связывающей скорость двигателя с управляющим сигналом.
У нее один полюс, механический, в точке s = -B¢/J¢.
Мы будем использовать данные для первого сочленения робота PUMA 560, потому что его параметры хорошо известны
и приведены в табл. 9.2. У нас нет данных о трении в звене, поэтому предположим, что B¢ = Bm. Зависимость момента инерции M11 от конфигурации, испытываемого двигателем первого
сочленения, показана на рис. 9.16в. Как видите, он изменяется
в широких пределах – от 3.66 до 5.21 кг·м². Используя среднее
значение момента инерции, которое составляет 4.43 кг·м², получаем эффективный момент инерции

и мы видим, что инерция звена на редукторе со стороны двигателя сравнима с инерцией самого двигателя.
Таблица 9.2. Параметры двигателя и привода первого (плечевого)
сочленения в PUMA 560 (Corke 1996b)
Постоянная крутящего момента двигателя
Инерция двигателя
Вязкое трение двигателя + соединения
Кулоновское трение двигателя + соединения
Передаточное соотношение
Максимальный крутящий момент
Максимальная скорость

Km
Jm
Bm
τС+
τСG
τmax
q̇max

0.228
200×10-6
817×10-6
0.126
-0.0709
107.815
0.900
165

Н·м/А
кг·м2
Н·м·с/рад
Н·м
Н·м
Н·м
рад/с

Пакет Toolbox позволяет сгенерировать линеаризованную
динамическую модель
>>> puma = models.DH.Puma560(); # загрузить модель с динамическими
# параметрами
>>> tf = puma.jointdynamics(puma.qn);

Независимое управление сочленениями 483

Это список кортежей, содержащих полиномы числителя
и знаменателя, которые представляют линейную непрерывную модель с постоянными параметрами, вычисленными для
конкретной конфигурации. Для рассматриваемой здесь модели
передаточная функция имеет параметры
>>> tf[1]
((1,), (0.0005797, 0.000817))

Например, как это
реализовано в пакете python-control.

Она аналогична функции (9.7), за исключением того, что не
учитывает Km и Ka, поскольку эти электрические параметры не
хранятся в объекте Robot. Такую модель можно использовать
для анализа и проектирования систем управления. ◄

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

ω_err

Контур управления скоростью

Для управления положением двигателя часто используется
дополнительный внутренний контур управления. Внешний
контур отвечает за поддержание местоположения сочленения
и определение его скорости, чтобы минимизировать погрешность позиционирования. Внутренний контур – контур управления скоростью – отвечает за поддержание скорости сочленения, заданной внешним контуром. Управление скоростью
двигателя важно для всех типов роботов, а не только для манипуляторов. Например, управление скоростью вращения колес
мобильного робота, винтов квадрокоптера, как обсуждалось
в главе 4, или гребного винта подводного робота. Сначала рассмотрим работу внутреннего контура управления скоростью,
а затем перейдем к внешнему.
Блок-схема контура управления скоростью показана на
рис. 9.7. Входной сигнал усилителя мощности двигателя формируется на основе рассогласования между требуемой и фактической скоростями. Сатуратор моделирует конечный максимальный крутящий момент, который может развить двигатель.
Для тестирования этого регулятора скорости можно использоПИ-регулятор
ω

ток
двигателя

Входной
порт
ω∗
τd
τff

ошибка
скорости

Kp

ПИ-регулятор

ток

Km

ограничение
крутящего
момента

суммарный
крутящий
момент

якорь
двигателя

двигатель
интеграл
ошибки

Ki

прямая связь по крутящему моменту
прямая связь
по току

возмущающий
момент

крутящий
момент
двигателя

ω
ω_err
τ
integral

Kff

Выходной порт

Рис. 9.7. Блок-схема модели models/vloop контура управления скоростью сочленений робота

484 Глава 9 · Динамика и управление

вать тестовую схему, показанную на рис. 9.8, которая обеспечивает требуемую трапециевидную скорость.

ω

время

профиль скорости

ошибка ω
контур управления
скоростью

возмущающий
момент

прямая связь
по крутящему моменту

крутящий момент
двигателя

интегральный
член

Рис. 9.8. Блок-схема модели models/vloop-test для тестирования контура управления
скоростью

Рассмотрим сначала случай пропорционального управления,
когда Ki = 0 и
u∗ = Kυ(q̇∗ - q̇).

(9.8)

Запуск симуляции регулятора скорости с его тестовым набором
>>> %run -m vloop_test -H

дает возможность поэкспериментировать, а поэкспериментировав, можно обнаружить, что коэффициент усиления Kυ = 0.6
дает вполне удовлетворительные характеристики, как показано на рис. 9.9. Наблюдается небольшое перерегулирование на
разрыве, но меньший коэффициент усиления приводит к увеличению погрешности в скорости, а больший – к колебаниям:
при проектировании систем управления постоянно приходится идти на какие-то компромиссы.
На графике также можно наблюдать очень незначительную
постоянную ошибку – фактическая скорость всегда меньше требуемой. С точки зрения классической системы управления, этот
контур скорости относится к системам типа 0, поскольку не содержит интегратора. Для таких систем характерно демонстри-

Параметр -H
предотвращает
блокировку до
закрытия всех окон
моделирования.

Независимое управление сочленениями 485

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

50

требуемая

40
ω (рад/с)

требуемая

фактическая

фактическая

50.5

30
50.0

20
10

49.5

0.0

0.1

0.2

0.3

0.4

0.5

0.6

Крутящий момент (Н·м)

0.2

а

49.0
48.5

0.1

48.0

0.0

47.5

−0.1
−0.2

ω (рад/с)

0

0.0

0.1

0.2

0.3

Время (с)

0.4

0.5

0.6

47.0
0.290

б

0.295

0.300

0.305

0.310

0.315

0.320

0.325

0.330

Рис. 9.9. Реакция контура управления скоростью на попытку получить трапециевидный график изменения скорости:
а) график скорости и крутящего момента двигателя; б) обратная связь при отслеживании скорости

Теперь исследуем влияние изменения момента инерции на
отклик обратной связи. Используя (9.6) и данные на рис. 9.16в,
находим, что минимальный и максимальный моменты инерции
сочленения на двигателе составляют 515×10-6 и 648×10-6 кг·м²
соответственно. На рис. 9.10 показана ошибка отслеживания
скорости с использованием выбранных выше коэффициентов
усиления для различных значений инерции. Видно, что ошибка уменьшается медленнее при больших значениях момента
инерции. В случае когда момент инерции изменяется существеннее, коэффициент усиления следует выбирать таким, чтобы он обеспечивал удовлетворительные характеристики обратной связи в обоих крайних значениях.
На рис. 9.15а показано, что момент силы тяжести в сочленении изменяется примерно от -40 до 40 Н·м. Теперь добавим
возмущающий момент, равный половине максимального значения, 20 Н·м, приложенный к редуктору на стороне нагрузки.
Для этого установим ненулевое значение в блоке возмущающего момента на рис. 9.8 и повторим моделирование. Результаты, представленные на рис. 9.11, указывают на значительное
ухудшение характеристик управления – динамическая погрешность увеличилась до более чем 2 рад/с. Первопричина та
же, что и в случае с небольшой ошибкой, которую мы видели на
рис. 9.9, – система типа 0 демонстрирует конечную ошибку при
постоянном входном сигнале или постоянном возмущении.

486 Глава 9 · Динамика и управление
диапазон требований

51.0

нет инерции звена
мин. инерция звена
средняя инерция звена
макс. инерция звена

50.5
50.0

ω (рад/с)

49.5
49.0
48.5
48.0
47.5
47.0
0.290

0.295

0.300

0.305

Время (с)

0.310

0.315

0.320

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

50

фактическая

40

ω (рад/с)

30

20

10

0
0.0

0.1

0.2

0.3

Время (с)

0.4

0.5

0.6

Рис. 9.11. Реакция контура управления скоростью на попытку получить трапециевидный
график изменения скорости с возмущающей силой тяжести 20 Н·м и средней инерцией звена

Существует три основных способа борьбы с этой ошибкой.
Первый и самый простой – увеличить коэффициент усиления.
Это уменьшит динамическую погрешность, но ухудшит стабильность системы и увеличит риск перерегулирования.
Второй способ обычно используется в промышленных элект­
роприводах и заключается в добавлении интегрального члена,
в результате чего система превращается в систему типа 1, которая
имеет нулевую ошибку при постоянном входном сигнале или постоянном возмущении. Давайте преобразуем (9.8) в пропорционально-интегральный (proportional-integral, PI) регулятор:

Независимое управление сочленениями 487
Отступление 9.3. Ограничения двигателя
Электродвигатели имеют ограниченные крутящий момент и скорость. Максимальный крутящий момент определяется максимальным током, который может
обеспечить усилитель мощности. Двигатель также имеет максимальный номинальный ток, превышение которого может привести к повреждению из-за перегрева или размагничивания постоянных магнитов, что необратимо снизит постоянный крутящий момент Km. С увеличением скорости увеличивается и трение,
и максимальная скорость равна ωmax = τmax /B.
Произведение крутящего момента двигателя на скорость – это выходная механическая мощность, которая тоже имеет верхнюю границу. Двигатели способны
выдерживать некоторую перегрузку, пиковую мощность и пиковый крутящий момент в течение коротких промежутков времени, но длительно может выдаваться
только мощность, которая значительно ниже пиковой.

На блок-схеме (рис. 9.7) это достигается установкой Ki в значение больше нуля. Поэкспериментировав, мы нашли коэффициенты усиления Kυ = 1 и Ki = 10, обеспечивающие неплохие результаты. Соответствующие характеристики показаны
на рис. 9.12. Состояние интегратора изменяется со временем,
компенсируя возмущение, и ошибка начинает стремиться
к нулю. На практике возмущение изменяется со временем,
и способность интегратора реагировать на это изменение зависит от значения коэффициента усиления интегральной составляющей Ki. В действительности на сочленение влияют
и другие возмущения, например кулоновское трение и крутящие моменты, обусловленные взаимодействием скорости
и ускорения. Регулятор должен быть настроен так, чтобы все
эти возмущения оказывали минимальное влияние на характеристики отслеживания.
требуемая
фактическая

ω (рад/с)

40

20

0
0.0

0.1

0.2

0.0

0.1

0.2

0.3

0.4

0.5

0.6

0.3

0.4

0.5

0.6

Интегральное значение

2.0
1.5
1.0
0.5
0.0

Время (с)

Рис. 9.12. Реакция контура управления скоростью на попытку получить трапециевидный
график изменения скорости с возмущающей силой тяжести 20 Н·м и пропорциональноинтегральным управлением

488 Глава 9 · Динамика и управление

Как это обычно для инженерии, в этой ситуации не обошлось
без компромиссов: интегральный член может увеличить риск
перерегулирования, поэтому увеличение Ki обычно требует
некоторого компенсирующего уменьшения Kυ. Если привод
сочленения работает на пределе своих возможностей, например достигает предела крутящего момента, то динамическая
погрешность будет расти со временем, потому что ускорение
двигателя будет меньше требуемого. Интеграл этой увеличивающейся погрешности будет расти и может привести к состоянию, известному как интегральное насыщение. Когда со­
членение наконец достигает своего назначения, накопленный
интеграл продолжает приводить в движение двигатель до его
уменьшения, что приведет к перерегулированию. Для борьбы
с этим эффектом применяются различные стратегии, такие как
ограничение максимального значения интегратора или разрешение интегрального воздействия, только когда двигатель близок к уставке (заданному значению регулируемой величины).
Вышеупомянутые подходы в совокупности называются подав­
лением возмущений, потому что они направлены на снижение
влияния неизвестных возмущений.
В контексте робототехники возмущение силы тяжести фактически известно. В разделе 9.1.3 мы показали, что крутящий
момент, возникающий под действием силы тяжести и действующий на сочленения, является функцией углов сочленений,
а в роботе эти углы всегда известны. Зная крутящий момент
и постоянную момента двигателя, можно добавить компенсирующий сигнал в выходной сигнал PI-регулятора. ► Стратегия,
основанная на прогнозировании возмущения и его подавлении, известна как управление моментом с упреждением и является альтернативой подавлению возмущений. Мы можем
опробовать этот подход, настроив в блоке прямой связи по крутящему моменту (рис. 9.8) значение, равное или близкое к значению возмущения.
Отступление 9.4. Обратная ЭДС
Вращающийся двигатель действует как генератор и вырабатывает напряжение Vb, называемое обратной ЭДС, противодействующей току, протекающему через двигатель. Обратная ЭДС пропорциональна скорости двигателя Vb = Kmω, где Km – постоянная
момента двигателя, единицей измерения которой также может
быть V·s/rad. Когда это напряжение равно максимально возможному напряжению усилителя мощности, ток перестает поступать
в двигатель, и крутящий момент падает до нуля – то есть достигается верхняя граница скорости двигателя. С увеличением скорости уменьшение тока уменьшает доступный крутящий момент,
и это, по-видимому, является дополнительным источником
демпфирования.

Даже если сила
тяжести известна
неточно, этот прием
все равно поможет
уменьшить величину
возмущения.

Независимое управление сочленениями 489

9.1.7
Другой распространенный подход –
использование
пропорциональноинтегрально-дифференциального
(proportional-integral-derivative, PID)
регулятора, но
можно показать,
что коэффициент
усиления D этого
регулятора связан
с коэффициентом
усиления P внутреннего контура управления скоростью.

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

Контур управления положением

Внешний контур отвечает за сохранение положения, и в нем
мы используем пропорциональный регулятор, вычисляющий
требуемую скорость двигателя на основе ошибки между требуемым q∗(t) и фактическим q# положениями:
q̇∗ = Kp(q∗(t) - q#),

(9.9)

который является входным сигналом для контура скорости. Во
многих случаях, особенно при движении по траектории, мы
также знаем желаемую скорость q̇∗(t) и можем использовать ее
в качестве сигнала прямой связи. В таком случае управление
положением принимает вид
q̇∗ = Kp(q∗(t) - q#) + Kff q̇∗(t),

(9.10)

где Kff будет иметь значение в диапазоне от нуля (без прямой
связи) до единицы (максимальная прямая связь).
Блок-схема модели контура управления положением показана на рис. 9.13a, а требуемые положение и скорость, q∗(t) и q̇∗(t),
являются гладкими функциями времени, вычисляемыми генератором траекторий, который перемещается от 0 до 0.5 рад
за 1 с. Местоположение сочленения определяется интегрированием скорости сочленения, полученной из контура управления
скоростью двигателя, и масштабируется обратным передаточным отношением редуктора. Расхождение между положением
двигателя и заданным местоположением звена определяет требуемую скорость для внутреннего контура.
Запустим тестовую схему, показанную на рис. 9.13б:
>>> %run -m ploop_test -H

Ее работа настраивается путем регулировки четырех коэффициентов усиления: Kp, Kυ, Ki, Kff. Цель – добиться хорошей
точности совпадения фактической скорости с требуемой. На
рис. 9.14a показаны результаты, полученные для Kp = 40, Kυ = 0.6,
Ki = 0 и Kff = 0. Как можно заметить, итоговая ошибка равна нулю,
но есть некоторая ошибка на участке траектории, где положение двигателя отстает от требуемого. Ошибка между кривыми
требуемого и фактического положений обусловлена ошибкой
скорости, накопленной во внутреннем контуре, которая измеряется в углах.
Контур управления положением, как и контур управления
скоростью, основан на классической отрицательной обратной
связи. Нулевая ошибка положения при отслеживании линейного изменения означает нулевую требуемую скорость для внут­
реннего контура, что, по сути, является противоречием. Эта

490 Глава 9 · Динамика и управление

ошибка положения
Входной
порт

Kp

требуемая
скорость

Редуктор

θ∗
ω∗
τ_d

Интегратор

Выходной
порт
θ
θ_err

Контур управления
скоростью

прямая связь по
крутящему моменту

Kff
прямая связь по скорости

а

q∗, q
прямая связь по скорости

трапециевидная
траектория
Контур управления
положением
ошибка q
возмущающий момент

б

0

Рис. 9.13. Блок-схема модели управления положением: а) модель управления models/ploop; б) тестовое окружение
модели models/ploop_test

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

1.0

требуемое

1.0

фактическое

0.8

0.6

0.6
q (рад)

q (рад)

0.8

фактическое

0.4

0.4

0.2

0.2

0.0

0.0

0.2
0.4
0.6
0.8
1.0
0.2
0.4
0.6
0.8
1.0
а 0.0
б 0.0
Рис. 9.14. Контур управления положением, обеспечивающий следование по трапецевидной траектории: а) только
пропорциональное управление; б) пропорциональное управление плюс упреждающая коррекция по величине
требуемой скорости

Независимое управление сочленениями 491

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

9.1.8

Подведение итогов

Наиболее распространенный подход к управлению сочленениями робота основан на использовании вложенного контура
управления, который применяет пропорциональный или пропорционально-интегральный закон для формирования крутящего момента, чтобы добиться точного соответствия фактической и требуемой скоростей. Внешний контур использует
пропорциональный закон управления для формирования требуемой скорости, при которой фактическое положение точно
соответствует заданному. На работу контура управления скоростью влияют возмущающие моменты, вызванные силой тяжести и другими динамическими связями, а также изменения
параметров управляемого объекта. Эти возмущающие моменты приводят к ошибкам отслеживания положения. Редукторная
передача снижает величину возмущающих моментов на 1/G,
а изменение инерции и трения на 1/G 2, но за это приходится
платить повышенной стоимостью, весом, увеличением трения
и механического шума.
Характеристики контура управления скоростью можно улучшить, добавив интегральный член управления или организовав
прямую связь для передачи возмущающего момента, который
в значительной степени предсказуем. Характеристики контура
положения тоже можно улучшить добавлением прямой связи
для передачи желаемой скорости сочленения. На практике системы управления используют в управлении как прямые, так
и обратные связи. Прямая связь используется для передачи
сигналов, которые можно вычислить, в данном случае скорость
сочленения, а в предыдущем случае – момент силы тяжести.
Управление с обратной связью компенсирует все оставшиеся
источники ошибок, включая изменение инерции при изменении конфигурации манипулятора и полезной нагрузки, трения
с течением времени, а также всех возмущающих моментов изза связи скорости и ускорения. В целом использование прямой
связи позволяет уменьшить коэффициент усиления обратной
связи, потому что в этом случае большая часть управляющего
сигнала поступает через прямую связь.

492 Глава 9 · Динамика и управление

9.2

Уравнения движения твердого тела

Рассмотрим двигатель, приводящий в действие сочленение
j ∈ {0, ..., N - 1} многозвенного манипулятора. Как показано на
рис. 7.18, сочленение j соединяет звено j со звеном j + 1. Двигатель создает крутящий момент, вызывающий ускорение вращения внешнего звена (звена j + 1), а также реактивный момент на
внутреннем звене (звене j). Внешние звенья от j + 1 до N давят
на звено j своим весом под действием силы тяжести, а вращающиеся звенья также оказывают друг на друга эффекты, обусловленные гироскопическими силами. Инерция, испытываемая
двигателем, зависит от конфигурации внешних звеньев.
Ситуация в отдельном звене довольно сложна, но для серии
звеньев результат можно записать элегантно и кратко в виде
набора связанных дифференциальных уравнений в матричной
форме:
Q = M(q)q̇˙ + C(q, q̇)q̇ + f(q̇) + g(q) + JT(q)w,

(9.11)

где q, q̇, q̇˙ ∈ ℝN – вектор обобщенных координат, скоростей
и ускорений сочленения соответственно, M ∈ ℝN×N – мат­ри­
ца инерции пространства сочленений, C ∈ ℝN×N – мат­ри­ца Кориолиса и центростремительной связи, f ∈ ℝN – сила трения,
g ∈ ℝN – нагрузка, обусловленная силой тяжести, w ∈ ℝ6 – нагрузка, приложенная к рабочему органу, J(q) ∈ ℝ6×N – якобиан
манипулятора, а Q ∈ ℝN – вектор обобщенных сил привода. Это
уравнение описывает динамику жесткого тела манипулятора
и известно как обратная динамика: учитывая конфигурацию,
скорость и ускорение, оно вычисляет требуемые силы или крутящие моменты в сочленениях.
Эти уравнения можно вывести, используя любой метод классической динамики, такой как второй закон Ньютона и уравнение движения Эйлера, обсуждаемые в разделе 3.2.1, или метод
Лагранжа, основанный на энергии. Очень эффективный способ
вычисления (9.11) – рекурсивный алгоритм Ньютона–Эйлера,
который, начиная вычисления с основания и продвигаясь наружу, по скорости и ускорению каждого сочленения вычисляет
скорость и ускорение каждого звена. Затем, двигаясь от рабочего органа обратно к основанию, вычисляет силы и моменты,
действующие на каждое звено, и, соответственно, крутящие
Рекурсивный алгоритм Ньютона–
моменты в сочленениях.
Эйлера имеет сложность O(N), и его можно записать в функ­
циональной форме как
Q = 𝒟-1(q, q̇, q̇˙).

(9.12)

Рекурсивная форма
обратной динамики
не вычисляет явно
мат­ри­цы M, C и g,
которые используются в (9.11).
Однако для расчета
этих матриц можно
применить рекурсивный алгоритм
Ньютона–Эйлера,
а также использовать функции
inertia и coriolis
из пакета Toolbox,
реализующие
алгоритм под названием «Method 1»,
описанный в Walker
and Orin (1982).
Несмотря на то что
рекурсивные формы
позволяют довольно
эффективно вычислять обратную
динамику, вычисление коэффициентов
отдельных динамических членов
(M, C и g) в (9.11)
является довольно
затратным – O(N3)
для манипулятора
с N-осями.

Уравнения движения твердого тела 493
Не все модели
манипуляторов,
реализованные
в Toolbox, имеют
динамические
параметры. Об их
существовании или
отсутствии можно
судить по столбцу
dynamics в выводе
команды models.
list(). Также можно воспользоваться
командой models.
list(keywords=
("dynamics",)),
которая выведет
список моделей,
обладающих динамическими пара­
метрами. В качестве
примера в этой
главе используется
робот PUMA 560,
имеющий полную
динамическую
модель, которая
включает инерцию
и трение.

В пакете Toolbox эти вычисления реализованы в виде метода
rne класса Robot. Рассмотрим робота PUMA 560
>>> puma = models.DH.Puma560();

в номинальной конфигурации, при нулевой скорости и ускорении в сочленении. Для достижения этого состояния требуемые
обобщенные силы в сочленении, или, в данном случае, крутящие моменты, должны быть
>>> zero = np.zeros((6,));
>>> Q = puma.rne(puma.qn, zero, zero)
array([
0,
31.64,
6.035,

0, 0.02825,

0])

Поскольку робот не движется (мы указали q̇ = q̇˙ = 0), эти моменты должны быть направлены против силы тяжести, чтобы
удерживать робота на месте. Это можно подтвердить, вычислив моменты, необходимые в отсутствие силы тяжести:
>>> Q = puma.rne(puma.qn, zero, zero, gravity=[0, 0, 0])
array([
0,
0,
0,
0,
0,

0])

для чего достаточно просто переопределить вектор силы тяжести по умолчанию.
Как и большинство методов в Toolbox, rne может работать по
траектории
>>> traj = jtraj(puma.qz, puma.qr, 10);
>>> Q = puma.rne(traj.q, traj.qd, traj.qdd);

В этом случае он возвращает
>>> Q.shape (10, 6)

мат­ри­цу размером 10×6, каждая строка которой представляет
обобщенную силу, необходимую для достижения соответствующей точки traj.q на траектории. Вот, например, моменты в со­
членениях, соответствующие пятому временному шагу:
>>> Q[5, :]
array([ -1.681,

52.44,

-14.38,

0, 0.003379,

0])

Теперь рассмотрим случай, когда робот движется. Он мгно­
венно принимает номинальную конфигурацию, но сочленение
0 движется со скоростью 1 рад/с, а ускорение всех сочленений
равно нулю. Тогда в отсутствие силы тяжести требуемые моменты в сочленениях
>>> puma.rne(puma.qn, [1, 0, 0, 0, 0, 0], zero, gravity=[0, 0, 0])
array([30.53, 0.628, -0.3607, -0.0003056, 0, 0])

не равны нулю. Крутящий момент в сочленении 0 необходим
для преодоления трения, которое всегда препятствует движе-

494 Глава 9 · Динамика и управление

нию. Самое интересное, что к сочленениям 1, 2 и 3 тоже должен
прилагаться ненулевой крутящий момент, чтобы противостоять гироскопическим моментам, которые оказывают на них
движение сочленения 0.
Элементы M, C, f и g являются комплексными функциями кинематических и инерционных параметров звеньев. Каждое звено имеет десять независимых инерционных параметров: массу
звена mi ∈ ℝ, смещение центра масс ri ∈ ℝ3 относительно начала системы координат звена и шесть вторых моментов в виде
тензора инерции звена относительно центра масс, но с учетом
направления осей системы координат звена {j}. Динамические
параметры звена робота можно просмотреть так:
>>> print(puma[1].dyn())
m
=
17
r
=
-0.36
0.006
0.23
|
0.13
0
0 |
I
= |
0
0.52
0 |
|
0
0
0.54 |
Jm =
0.0002
B
= 0.00082
Tc =
0.13(+)
-0.071(-)
G
= 1.1e+02
qlim =
-1.9 to
1.9

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

9.2.1

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

Член силы тяжести

Q = M(q)q̇˙ + C(q, q̇)q̇ + f(q̇) + g(q) + JT(q)w
Подробное обсуждение мы начнем с члена силы тяжести, потому что он, как правило, является доминирующим в (9.11)
и присутствует, даже когда робот неподвижен. В некоторых
роботах имеются противовесы или пружины для уменьшения крутящего момента, вызванного силой тяжести. Это позволяет использовать менее мощные и менее дорогостоящие
двигатели.
В предыдущем разделе мы вычисляли нагрузку, обусловленную действием силы тяжести, с помощью метода rne, установив

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

Уравнения движения твердого тела 495

при этом скорость и ускорение в сочленении равными нулю.
Более удобный подход – использовать метод gravload.
>>> Q = puma.gravload(puma.qn)
array([
0,
31.64,
6.035,

0, 0.02825,

0])

Ньютоновское ускоОбъект Robot содержит вектор ускорения свободного падения
рение свободного по умолчанию, который инициализируется номинальным знападения, обсуждавшееся в разде- чением для Земли ◄:
ле 3.4.2.1, направленное вертикально >>> puma.gravity
0,
0,
-9.81])
вниз. Это значение array([
можно изменить,
передав аргумент
Для эксперимента можно подставить ускорение свободного
gravity в конструкпадения
на Луне
тор Robot.

>>> puma.gravity /= 6

что приводит к уменьшению крутящих моментов в сочленениях
>>> puma.gravload(puma.qn)
array([
0,
5.273,

1.006,

0, 0.004709,

0])

или перевернуть нашего лунного робота вверх ногами
>>> puma.base = SE3.Rx(pi);
>>> puma.gravload(puma.qn)
array([
0, -5.273, -1.006,

0, -0.004709,

0])

и убедиться, что моменты сил изменили знак. Прежде чем продолжить, вернем робота на Землю и поставим его на ноги:
>>> puma = models.DH.Puma560();
Отступление 9.5. Жозеф-Луи Лагранж
Лагранж (1736–1813) – итальянец по происхождению
(Джузеппе Лодовико Лагранджиа) – французский математик и астроном. Внес значительный вклад в анализ, тео­рию чисел, классическую и небесную механику.
В 1766 го­ду сменил Эйлера на посту директора кафедры
математики в Прусском университете Берлинской академии наук. Там он проработал более двадцати лет, написал
множество работ и получил несколько премий Французской академии наук. Его трактат по аналитической механике «Mécanique Analytique», впервые опубликованный
в 1788 году, содержал наиболее полное изложение классической механики со времен Ньютона и заложил основу для развития математической физики в XIX веке.
В 1787 году Лагранж стал членом Французской академии, первым профессором
анализа в Политехнической школе, способствовал переходу Франции на десятичную систему счисления и в 1808 году был награжден орденом Почетного легиона
и титулом графа Империи. Похоронен в Пантеоне в Париже.

496 Глава 9 · Динамика и управление

Крутящий момент, действующий на сочленение и обусловленный силой тяжести, во многом зависит от конфигурации
робота. Интуитивно понятно, что крутящий момент в плечевом
сочленении должен быть значительно больше, когда рука вытянута горизонтально:
>>> Q = puma.gravload(puma.qs)
array([
0,
46.01,
8.772,

0, 0.02825,

0])

чем когда рука направлена прямо вверх:
>>> Q = puma.gravload(puma.qr)
array([
0, -0.7752, 0.2489,

0,

0,

0])

В первой конфигурации момент силы тяжести на локте тоже
очень велик, потому что он должен поддерживать предплечье
и запястье. Давайте посмотрим, как меняется гравитационная
нагрузка в сочленениях 2 и 3 в зависимости от конфигурации
сочленений:
>>>
>>>
...
>>>
>>>
...
...
...
...
>>>

N = 100;
Q1, Q2 = np.meshgrid(np.linspace(-pi, pi, N),
np.linspace(-pi, pi, N));
G1, G2 = np.zeros((N,N)), np.zeros((N,N));
for i in range(N):
for j in range(N):
g = puma.gravload(np.array([0, Q1[i,j], Q2[i,j], 0, 0, 0]))
G1[i, j] = g[1] # нагрузка на плечо
G2[i, j] = g[2] # нагрузка на локоть
plt.axes(projection="3d").plot_surface(Q1, Q2, G1);

8
6
4
2
0
−2
−4
−6
−8

0

g1 (Н·м)

40
20

−20
−40

1

−2
2

−3

1
−2

0
−1

−1

q1 (р 0
ад)

1

−2
2

−3

3
3
а
б
Рис. 9.15. Изменение гравитационной нагрузки в зависимости от конфигурации
манипулятора: а) нагрузка на плечо, g2(q1, q2); б) нагрузка на локоть g3(q1, q2)

2

(р
ад
)

−3

2

q1 (р 0
ад)

(р
ад
)

0
−1

−1

2

−2

q

−3

2

q

1

3

3

g2 (Н·м)

Результаты показаны на рис. 9.15. Крутящий момент силы
тяжести на сочленении 1 меняется в пределах ±40 Н·м, а на со­
членении 2 – в пределах ±10 Н·м. Анализ подобного типа очень
важен при проектировании роботов для определения необходимого крутящего момента двигателей.

Уравнения движения твердого тела 497

9.2.2

Матрица инерции

Q = M(q)q̇˙ + C(q, q̇)q̇ + f(q̇) + g(q) + JT(q)w
Диагональные элементы этой мат­ри­цы
включают инерцию
якоря двигателя,
умноженную на G2.

Матрица инерции пространства сочленений, иногда называемая мат­ри­цей масс, симметрична и положительно определена.
Она является функцией конфигурации манипулятора ◄:
>>> M = puma.inertia(puma.qn)
array([[ 3.659, -0.4044, 0.1006, -0.002517,
0,
0],
[ -0.4044,
4.414, 0.3509,
0, 0.00236,
0],
[ 0.1006, 0.3509, 0.9378,
0, 0.00148,
0],
[-0.002517,
0,
0, 0.1925,
0, 2.828e-05],
[
0, 0.00236, 0.00148,
0,
0.1713,
0],
[
0,
0,
0, 2.828e-05,
0, 0.1941]])

а ее диагональные элементы mjj описывают инерцию, испыты­
ваемую сочленением j, то есть Qj = mjj q̇˙j. Обратите внимание,
что первые два диагональных элемента, соответствующих поясничному и плечевым сочленениям робота, имеют бо́льшую
величину, потому что движение этих сочленений включает вращение тяжелых звеньев верхней и нижней рук. Недиагональные члены mij = mji, i ≠ j являются произведениями инерции, которые связывают ускорение сочленения j с обобщенной силой,
действующей на сочленение i.
Давайте рассмотрим некоторые элементы мат­ри­цы инерции
и как они изменяются в зависимости от конфигурации робота.
>>>
>>>
...
>>>
>>>
>>>
...
...
...
...
...
>>>

N = 100;
Q1, Q2 = np.meshgrid(np.linspace(-pi, pi, N),
np.linspace(-pi, pi, N));
M00, M01 = np.zeros((N,N)), np.zeros((N,N))
M11 = np.zeros((N,N))
for i in range(N):
for j in range(N):
M = puma.inertia(np.array([0, Q1[i,j], Q2[i,j], 0, 0, 0]))
M00[i, j] = M[0, 0]
M01[i, j] = M[0, 1]
M11[i, j] = M[1, 1]
plt.axes(projection="3d").plot_surface(Q1, Q2, M00);

В результатах, показанных на рис. 9.16, можно видеть значительные колебания значения m11:
>>> M00.max() / M00.min()
2.156

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

498 Глава 9 · Динамика и управление

5.0

0.8
0.6
0.4
0.2
0.0
−0.2
−0.4
−0.6
−0.8

4.5
m00 (кг·м2)

3.0

m01 (кг·м2)

4.0
3.5

2.5

а

2

3

)

1

−1
2

−2
3

)
ад

0
0

q1 (р
ад

б

−3

−1

(р

)

−2

3

2

−2

−3

2

q

1

−1

ад

q1 (р 0
ад)

1

(р

0

−1

3

2

−2

2

q

−3

1

−3

5.2
5.0
4.8

m11 (кг·м2)

4.6
4.4
4.2
4.0
3.8
3.6

в

−3

−2

−1

0

q2 (рад)

1

2

3

Рис. 9.16. Изменение элементов мат­ри­цы инерции в зависимости от конфигурации манипулятора:
а) инерция сочленения 0 как функция углов сочленений 1 и 2: m00(q1, q2); б) произведение
инерции m01(q1, q2); в) инерция сочленения 1 как функция угла сочленения 2: m11(q2)

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

9.2.3

Трение

Q = M(q)q̇˙ + C(q, q̇)q̇ + f(q̇) + g(q) + JT(q)w
Трение, как правило, является следующей по значимости силой
в сочленениях после силы тяжести. ► Если робот движется со
скоростью 1 рад/с в сочленении 0, то крутящий момент из-за
вязкости и кулоновского трения составит
>>> puma.friction([1, 0, 0, 0, 0, 0])
array([ -30.53,
0,
0,
0,
0,

0])

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

Сила трения в со­
членениях робота
PUMA 560 варьируется от 10 до 47 %
от максимального
крутящего момента
двигателя (Corke
1996b).

Уравнения движения твердого тела 499

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

9.2.4

Матрица Кориолиса и центростремительной связи

Q = M(q)q̇˙ + C(q, q̇)q̇ + f(q̇) + g(q) + JT(q)w
Матрица C является функцией координат и скорости сочленения, а элемент cij связывает скорость сочленения j с обобщенной
силой, действующей на сочленение i. Эта связь обусловлена гироскопическими эффектами: центростремительные моменты
пропорциональны q̇j2, а моменты Кориолиса пропорциональны q̇i q̇j.
Например, в номинальной конфигурации с локтевым суставом, движущимся со скоростью 1 рад/с:
>>> qd = [0, 0, 1, 0, 0, 0];

мат­ри­ца Кориолиса – это
>>> C = puma.coriolis(puma.qn, qd)
array([[ 0.3607, -0.09566, -0.09566, 0.0005445, -0.0004321, 2e-05],
[
0, 0.3858, 0.3858,
0, -4.134e-05,
0],
[
0,
0,
0,
0, -0.0009207,
0],
[-3.584e-05,
0,
0,
0,
0, -1.414e-05],
[
0, 0.0009207, 0.0009207,
0,
0,
0],
[ 2e-05,
0,
0, 1.414e-05,
0,
0]])

Элемент c12 определяет значительную связь между скоростью
сочленения 2 и крутящим моментом в сочленении 1 – скорость
вращения локтя, создающая момент силы, действующий на
плечо. Элементы этой мат­ри­цы определяют связи между скоростью и силой, действующей на сочленение, и измеряются в тех
же единицах, что и вязкое трение или демпфирование, однако
знак может быть как положительным, так и отрицательным.
Крутящие моменты в сочленении, вызванные движением только этого одного сочленения, равны
>>> C @ qd
array([-0.09566,

9.2.5

0.3858, 0, 0, 0.0009207, 0])

Влияние полезной нагрузки

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

500 Глава 9 · Динамика и управление

всеми двигателями сочленений, что снижает ускорение и динамические характеристики. Второй заключается в том, что масса
создает силу тяжести, которую должны выдерживать все сочленения. В худшем случае увеличенный крутящий момент, обусловленный силой тяжести, может превысить номинал одного или
нескольких двигателей. Однако даже если номинал не превышен,
остается меньше крутящего момента, доступного для ускорения,
что, в свою очередь, снижает динамические характеристики.
Чтобы количественно оценить эти эффекты, сделаем снимок
мат­ри­цы гравитационного момента и инерции робота:
>>> G = puma.gravload(puma.qn);
>>> M = puma.inertia(puma.qn);

а затем подвесим к рабочему органу PUMA 560 точечную массу
2.5 кг, соответствующую его максимально допустимой полезной нагрузке.
>>> puma.payload(2.5, [0, 0, 0.1]);

Центр масс полезной нагрузки не может находиться в цент­
ре системы координат запястья, то есть внутри механизма за­
пястья, поэтому мы сместили его на 100 мм в направлении оси z
системы координат запястья.
Инерция в номинальной конфигурации теперь равна
>>> M_loaded = puma.inertia(puma.qn);

и отношение к случаю отсутствия полезной нагрузки равно
>>> M_loaded / np.where(abs(M)
array([[ 1.336,
nan,
[
nan,
1.267,
[ 2.149,
2.919,
[
nan,
nan,
[
nan,
74.01,
[
nan,
nan,

< 1e-6, np.nan, M)
2.149,
nan,
nan,
2.919,
nan,
74.01,
1.66,
nan,
66.41,
nan,
1.065,
nan,
66.41,
nan,
1.145,
nan,
1,
nan,

nan],
nan],
nan],
1],
nan],
1]])

Как видите, диагональные элементы увеличились. Например, инерция локтевого сочленения увеличилась на 66 %, что
уменьшило его максимальное ускорение почти на 40 %. Инерция сочленения 5 не изменилась, потому что эта добавленная
масса расположена на оси вращения данного сочленения. Некоторые недиагональные члены увеличились значительно,
особенно в строке и столбце 4. Это указывает, что движение лучезапястного сочленения, приводящее к качанию смещенной
массы, создает большие силы реакции, которые ощущаются
всеми остальными сочленениями.
Гравитационная нагрузка в локте и запястье тоже существенно увеличилась.

Некоторые элементы М имеют очень
малые значения, что
привело бы к аномальным соотношениям, поэтому мы
присваиваем этим
элементам значение
nan, чтобы избежать
этого.

Уравнения движения твердого тела 501
>>> puma.gravload(puma.qn) / np.where(abs(G) < 1e-6, np.nan, G)
array([
nan,
1.522,
2.542,
nan,
86.81,
nan])

Прежде чем продолжить, обнулим массу полезной нагрузки
робота.
>>> puma.payload(0)

9.2.6

Воздействие на основание

Движение манипулятора накладывает силовой винт (оказывает
винтовую нагрузку) на основание своим весом, а также силами
реакции и моментами. Этот винт можно получить в необязательном выходном аргументе метода rne, например:
>>> Q, wb = puma.rne(puma.qn, zero, zero, base_wrench=True);

К основанию необходимо приложить винт
>>> wb
array([

0,

0,

229.2,

-48.27,

-31.09,

0])

чтобы сохранить его в равновесии. Вертикальная сила 230 Н
противодействует силе тяжести робота, направленной вниз.
>>> sum([link.m for link in puma]) * puma.gravity[2]
-229.2

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

9.2.7

Динамическая манипулируемость

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

502 Глава 9 · Динамика и управление

QTQ = 1.
Из (9.11), игнорируя силу тяжести и предполагая q̇ = 0, можно
записать
Q = M(q)q̇˙.
Дифференцируя (8.2) и предполагая q̇ = 0, запишем
v̇ = J(q)q̇˙,
где v̇ – пространственное ускорение – вектор, включающий поступательное и угловое ускорения. Объединяя их, запишем ►
v̇T(J(q)M-1(q)M-T(q)JT(q))-1v̇ = 1.

Если мат­ри­ца
не квадратная,
то используем
псевдообратную.

Это уравнение точек v̇ на поверхности гиперэллипсоида
в пространстве ускорений задачи.
Если рассматривать только поступательное ускорение, то
можно взять поступательную часть якобиана
>>> Jt = puma.jacob0(puma.qn, half="trans"); # первые 3 строки

и на ее основе вычислить и построить эллипсоид поступательного ускорения
>>> M = puma.inertia(puma.qn);
>>> E = (Jt @ np.linalg.inv(M) @ np.linalg.inv(M).T @ Jt.T);
>>> plot_ellipsoid(E);

как показано на рис. 9.17. Большая ось эллипсоида – это направление, в котором манипулятор имеет максимальное ускорение
в данной конфигурации. Радиусы эллипсоида – квадратные
корни собственных значений
>>> e, _ = np.linalg.eig(E)
>>> radii = 1 / np.sqrt(e)
array([ 2.219,
9.582,

5.905])

а направление максимального ускорения определяется наибольшим собственным вектором. Отношение минимального
и максимального радиусов
>>> radii.min() / radii.max()
0.2315

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

Измерения
шестимерного
эллипсоида имеют
разные единицы
измерения: м/с2
и рад/с2, что
затрудняет
сравнение всех
шести радиусов.

Прямая динамика 503

https://go.sn.pub/
IfXBCw

Рис. 9.17. Эллипсоид поступательного ускорения робота PUMA 560 в номинальной
конфигурации. Черные эллипсы – это тени эллипсоида на плоскостях, перпендикулярных
осям x, y и z. Эта диаграмма была создана с помощью PyVista, а не Matplotlib

Другая распространенная скалярная мера манипулируе­
мости учитывает инерцию и отношение минимального и максимального собственных значений обобщенной мат­ри­цы
инерции
Mx = J-T(q)M(q)J(q)-1,
так что m ∈ [0, 1], где 1 означает равномерность ускорения во
всех направлениях. В данном примере эта мера имеет величину
>>> puma.manipulability(puma.qn, method="asada")
0,004389

9.3

Прямая динамика

Для определения движения манипулятора под воздействием
сил и моментов, приложенных к его сочленениям, потребуется
решить задачу прямой, или интегральной, динамики. Преобразуя уравнения движения (9.11), получаем ускорение сочленения
q̇˙ = M-1(q)(Q - C(q, q̇)q̇ - f(q̇) - g(q) - JT(q)w),

(9.13)

где M(q) является положительно определенной функцией
и, следовательно, всегда обратимой. Эта функция вычисляется
методом accel класса Robot

504 Глава 9 · Динамика и управление
>>> qdd = puma.accel(puma.q, puma.qd, Q)

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

сочленение
с нулевыми
крутящими
моментами

Блок Armplot
прямая динамика

Рис. 9.18. Блок-схема модели models/zerotorque для манипулятора PUMA 560 с нулевыми
моментами в сочленениях. Эта модель исключает кулоновское трение для упрощения
численного интегрирования

Простейшей демонстрацией динамики движения вперед
служит блок-схема модели, показанная на рис. 9.18, имеющей
сочленения с нулевыми моментами. Модель, которая выполняет интегрирование (9.13) по времени, запускается командой
>>> %run -m zerotorque -H

Поскольку в сочленениях нет моментов, противодействующих гравитации и удерживающих манипулятор в вертикальном
положении, то он разрушается под собственным весом. Верхняя часть манипулятора падает и качается вперед и назад, как
и нижняя часть, в то время как поясничное сочленение вращается под воздействием силы Кориолиса. Движение будет медленно затухать по мере рассеивания энергии вязким трением.
Чтобы более детально увидеть, как развивается ситуация,
можно построить график зависимости от времени углов первых
трех сочленений, полученных в ходе моделирования (рис. 9.19).
>>> xplot(out.t, out.q[:3, :])

Тот же результат можно получить, используя Toolbox на­
прямую:
>>> torque_func = lambda t, q, qd: np.zeros((6,))
>>> traj = puma.nofriction().fdyn(T=5, q0=puma.qr, Q=torque_func)
>>> xplot(traj.t, traj.q)

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

Компенсация динамики твердого тела 505

0.0

q (рад)

−0.5

−1.0

−1.5

−2.0

q0
q1
q2
1

2

Время (с)

3

4

5

Рис. 9.19. Изменение угла сочленения для робота PUMA 560 с нулевым кулоновским трением,
разрушающегося под действием силы тяжести из начальной конфигурации с нулевыми
углами сочленений

функция фактических и желаемых координат и угловых скоростей сочленений. Но об этом мы поговорим в следующем разделе.
Кулоновское трение и прямая динамика
Кулоновское трение привносит сильную нелинейность и может
осложнять использование методов численного интегрирования
при решении задачи прямой динамики. Обычно это проявляется
в очень большом времени интегрирования. Решения с фиксированным шагом могли бы оказаться более оптимальными, но они
пока не реализованы.
Обходное решение – исключить проблемное кулоновское трение из
модели, но это ухудшает точность результатов. Модель манипулятора PUMA 560 (models.DH.Puma560) имеет ненулевые параметры вязкости и кулоновского трения для каждого сочленения. Кулоновское
трение можно убрать, как показано ниже:
>>> puma_nf = puma.nofriction();
Вызов nofriction() возвращает копию объекта робота, подобную
оригиналу, за исключением отсутствия кулоновского трения. Метод позволяет исключить кулоновское и/или вязкое трение, но по
умолчанию удаляет только кулоновское трение.

9.4

Компенсация динамики твердого тела

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

506 Глава 9 · Динамика и управление

званного силой тяжести. Изменение инерции и другие динамические силы не рассматривались явно и были оставлены на
усмотрение регулятора обратной связи, который должен был
обрабатывать их как возмущение. Однако инерционные и другие возмущающие моменты можно вычислить по формуле
(9.11), зная углы поворота, скорости и ускорения сочленений,
а также инерционные параметры звеньев. Эти моменты можно включить в закон управления, используя один из двух подходов, на основе моделирования: управление с прямой связью
и управление с вычисляемым моментом. Конструктивные различия показаны на рис. 9.20 и 9.21.

9.4.1

Управление с прямой связью

Регулятор крутящего момента с прямой связью, показанный на
рис. 9.20, реализует следующее уравнение:

(9.14)
Член прямой связи представляет расчетную силу в сочленении, необходимую для достижения желаемых параметров движения манипулятора: местоположения q∗, q̇∗ и ускорения q̇˙∗.
Член обратной связи генерирует силы в сочленении, связанные
с динамической погрешностью, вызванной такими ошибками,
как неопределенность инерциальных параметров, немоделированные силы или внешние возмущения.
Kp и Kv – это диагональные мат­ри­цы коэффициентов усиления (или демпфирования) местоположения и скорости соответственно. M̂, Ĉ, fˆ и ĝ – оценки мат­ри­цы массы, мат­ри­цы кориолисовой связи, силы трения и силы тяжести.
На блок-схеме крутящий момент прямой связи вычисляется
блоком обратной динамики и суммируется с крутящим моментом обратной связи, вычисленным на основе ошибки местоположения и скорости. Требуемые углы сочленений, скорость
и ускорение определяются с помощью полиномов пятой степени блоком траектории сочленений, параметрами которого
являются начальный и конечный углы сочленений. Поскольку
конфигурация робота изменяется относительно медленно, крутящий момент прямой связью можно оценить на большем интервале Tff, чем ошибки контуров обратной связи Tfb.

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

Компенсация динамики твердого тела 507

обратная связь

робот

ошибка
местоположения

Kp

ошибка скорости

Kv

обратная связь
по моменту

Tfb

траектория
в пространстве
сочленений

суммарный
момент

прямая динамика

Анимация
манипулятора

управление с прямой связью

прямая связь по крутящему моменту

Блок Main

Tff

обратная
динамика

Рис. 9.20. Блок-схема модели models/feedforward для робота PUMA 560, поддерживающей управление крутящим
моментом с прямой связью

После запуска симуляции
>>> %run -m feedforward -H

результаты с временной последовательностью сохранятся в переменной out.
Член прямой связи линеаризует нелинейную динамику относительно рабочей точки (q∗, q̇∗, q̇˙∗). Если линеаризация идеальна, то есть если M̂ = М, Ĉ = С, fˆ = f и ĝ = g, то динамику ошибки
e = q∗ - q можно получить, объединив (9.11) и (9.14):
M(q∗)ė˙ + Kυė + Kpe = 0.

(9.15)

При правильно выбранных Kp и Kυ ошибка будет стремиться
к нулю, но совместные ошибки связаны из-за недиагональности мат­ри­цы M, и их динамика зависит от конфигурации манипулятора.

9.4.2

Регулятор с вычисляемым крутящим моментом

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

508 Глава 9 · Динамика и управление

Q = 𝒟ˆ -1(q∗, q̇∗, q̇˙∗ + Kυ(q̇∗ - q̇) + Kp(q∗ - q))

= M̂(q){q̇˙∗ + Kυ(q̇∗ - q̇) + Kp(q∗ - q) + Ĉ(q∗, q̇∗)q̇∗
+ fˆ(q̇∗) + ĝ(q∗)},

(9.16)

где 𝒟ˆ -1 – расчетное усилие в сочленении, необходимое для достижения желаемых параметров движения манипулятора. На
практике обратная ситуация не идеальна, и будут наблюдаться
динамические отклонения в местоположении и скорости. Они
влияют на корректирующее ускорение посредством матриц Kp
и Kυ с коэффициентами усиления (или демпфирования) местоположения и скорости соответственно, которые обычно являются диагональными. M̂, Ĉ, fˆ и ĝ – оценки мат­ри­цы массы, мат­
ри­цы кориолисовой связи, силы трения и силы тяжести.

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

робот

траектория
в пространстве
сочленений

ошибка
местоположения

ошибка скорости

Kp

Kv

блок Sum

обратная
динамика

Tfb

необходимо
ускорение
для уменьшения
динамического
отклонения

прямая динамика

Анимация
манипулятора

qd
Блок Main

q

Рис. 9.21. Блок-схема модели models/computed-torque для робота PUMA 560, поддерживающая управление
с вычисляемым крутящим моментом

Требуемые углы и скорости сочленений генерируются с использованием полиномов пятой степени блоком траектории
сочленений, параметрами которого являются начальный и конечный углы сочленений.
В этом случае обратную динамику необходимо оценивать на
каждом интервале сервопривода, хотя мат­ри­цы коэффициентов M̂, Ĉ и ĝ можно оценивать с меньшей скоростью, поскольку
конфигурация робота изменяется относительно медленно.
После запуска симуляции
>>> %run -m computed-torque -H

результаты с временной последовательностью сохранятся в переменной out.
Если линеаризация идеальна, то есть если M̂ = М, Ĉ = С, fˆ = f
и ĝ = g, то динамику ошибки e = q∗ - q можно получить, объединив (9.11) и (9.16):
ė˙ + Kυė + Kpe = 0.

(9.17)

Динамика пространства задач и управление 509

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

9.5

Динамика пространства задач и управление

До сих пор наше внимание было сосредоточено на точном
управлении сочленениями робота, что, конечно, важно для промышленного применения, где скорость и точность позицио­
нирования имеют решающее значение. Однако времена меняются, и ныне уже требуется, чтобы роботы могли безопасно
выполнять повседневные задачи, такие как открытие дверей
или ящиков, уборка поверхностей или обмен предметами
с людьми. Люди легко выполняют эти задачи, но за этой кажущейся простотой скрываются сложные взаимодействия сил.
Когда мы открываем дверь, наша рука прикладывает к ней силу,
ускоряя ее движение, а дверь прикладывает силу реакции, которая направляет нашу руку по дуге окружности. Приложение сил
и реа­гирование на силы реакции критически важны для многих
реальных приложений.
При исследовании этого класса задач может быть полезно
рассматривать их с точки зрения рабочего органа, как показано на рис. 9.22. Здесь я абстрагировался от конкретной модели робота и рассматриваю рабочий орган как точечную массу
в трехмерном пространстве. На массу может оказываться воздействие со стороны окружающей среды, например человека,
взаимодействующего с роботом, или со стороны реактивных
сил, возникающих при выполнении какой-либо контактной за-

Рис. 9.22. Рабочий орган изображен
как масса в форме черной сферы, и нас
интересует, как он движется в ответ на
приложенные силы и крутящие моменты.
w – это общий крутящий момент,
прикладываемый окружающей средой
и приводами сочленений, а ẋ и ẋ˙ – это
результирующие скорость и ускорение
в пространстве задачи (изображение
робота предоставлено Franka Emika)

510 Глава 9 · Динамика и управление

дачи. Приводы робота тоже оказывают реактивное воздействие
на массу. В ответ на все эти воздействия масса движется с некоторой скоростью и ускорением.
Начнем разбирательство со всеми этими воздействиями
с соотношения между скоростями в пространстве сочленений
и в пространстве задач, заданного в (8.2).
v = J(q)q̇ ∈ ℝM,

(9.18)

v̇ = J(q)q̇˙ + J̇(q)q̇,

(9.19)

где J(q) ∈ ℝM×N – якобиан манипулятора, N – количество сочленений, а M = dim 𝒯 – размерность пространства задач. Производ­
ная по времени равна
где v̇ – пространственное ускорение, представляющее собой
вектор, содержащий поступательное и угловое ускорения, а q̇˙ –
ускорение сочленения.
J̇(q) – производная якобиана по времени, или скорость изменения элементов мат­ри­цы Якоби, которая вычисляется методом jacob0_dot. ► Ее можно развернуть так:

где H(q) ∈ ℝN×M×N – гессиан манипулятора, который является тензором ранга 3, который можно вычислить с помощью метода
hessian0. ►
Если J(q) – квадратная мат­ри­ца, то (9.19) можно переписать
как
q̇˙ = J-1(q)(v̇ - J̇(q)q̇).

(9.20)

Подставляя (9.20) и обратную величину (9.18) в (9.11), можно
переписать динамику твердого тела в пространстве задач как
Mx(q)ẋ˙ + Cx(q, q̇)ẋ + fx(q̇) + gx(q) = w,

(9.21)

где x ∈ ℝM – положение в пространстве задач (вектор), а w ∈ ℝM –
общий крутящий динамический винт, приложенный к исполнительному органу. Эта формулировка также называется дина­
микой рабочего пространства. Динамика манипулятора абстрагируется в массу, представленную черной сферой на рис. 9.22.
Однако поскольку масса связана с манипулятором, она наследу­
ет его динамику. Например, если отпустить сферу, то она упадет, но не прямо вниз.
Члены Mx, Cx, fx, gx представляют, соответственно, мат­ри­цу
инерции в пространстве задач, мат­ри­цу кориолисовой и цент­
ростремительной связи, силу трения и силу тяжести, которые

Ее можно получить
непосредственно
путем символического дифференцирования элементов
мат­ри­цы Якоби,
выраженных как
функции координат
сочленений.
Произведение
тензора на
вектор является
произведением
тензора mode1, или сверткой,
H(q) ×1 q̇, которое
вычисляется
по формуле
∑j,k hi,j,k q̇, i = 0,
..., N – 1 или
np.tensordot(H,
qd, (0,0)).

Динамика пространства задач и управление 511

для неизбыточного робота связаны с соответствующими членами в (9.11) соотношением
Mx(q) = Ja-T(q)M(q)Ja-1(q) ∈ ℝM×M

Cx(q, q̇) = (Ja-T(q)C(q, q̇) - Mx(q)J̇a(q))Ja-1(q) ∈ ℝM×M

fx(q̇) = Ja-T(q)f(q̇) ∈ ℝM

gx(q) = Ja-T(q)g(q) ∈ ℝM,

Вращательная
составляющая
пространственной
скорости – это
угловая скорость ω,
как отмечалось
в разделе 3.1.1,
тогда как в данном
случае это Γ.

(9.22)

где Ja(q) – аналитический якобиан, введенный в разделе 8.1.3.
Распространенное векторное представление положения в пространстве задач: x = (p, Γ) ∈ ℝ6, где p ∈ ℝ3 – местоположение
рабочего органа, а Γ ∈ ℝ3 – его ориентация, представленная
углами Эйлера, углами крена-тангажа-рыскания или экспоненциальными координатами. Производные просто обозначаются
как ẋ = (ṗ, Γ̇ ) и ẋ˙ = (ṗ˙, Γ̇˙). Использование этого представления
означает, что ẋ не является пространственной скоростью, как
это было в главах 3 и 8, и потому мы должны использовать соответствующий аналитический якобиан Ja ∈ ℝM×N.
Для избыточного робота, где M ≠ N, мы используем уравнения
Mx(q) = (Ja(q)M-1(q)JaT(q))-1 ∈ ℝM×M

–
–
Cx(q, q̇) = (JaT(q)C(q, q̇) - Mx(q)J̇a(q))Ja(q) ∈ ℝM×M
–
fx(q̇) = JaT(q)f(q̇) ∈ ℝM

–
gx(q) = JaT(q)g(q) ∈ ℝM,

(9.23)

–
где Ja(q) = M-1(q)JaT(q)Mx(q) ∈ ℝN×M – динамически согласованная
величина, обратная Ja(q) – обобщенной обратной величины,
минимизирующей мгновенную кинетическую энергию манипулятора. Эти величины вычисляются методами inertia_x,
coriolis_x, friction_x и gravload_x, которые аналогичны похожим
методам, используемым в пространстве сочленений: inertia,
coriolis, friction и gravload.
Воздействие, приложенное к массе на рис. 9.22, имеет две
компоненты
w = we + wc,

(9.24)

где we – это воздействие, оказываемое окружающей средой,
например человеком, толкающим рабочий орган, или сила реакции, возникающая при выполнении какой-либо контактной
задачи. wc – это управление, воздействие, которое применяется
к рабочему органу через управление крутящими моментами,
прикладываемыми к сочленениям.

512 Глава 9 · Динамика и управление

Начнем обсуждение управления в пространстве задач с применения простого регулятора
wc = ĝx(q),

(9.25)

где ĝx – наилучшая оценка силы тяжести в операционном пространстве. Подставляя это выражение в (9.24) и (9.21), получаем динамику рабочего органа
Mx(q)ẋ˙ + Cx(q, q̇)ẋ + fx(q̇) = we,
где отсутствует гравитационный член, то есть рабочий орган
считается невесомым. При нулевом приложенном усилии, то
есть we = 0, если робот не движется, q̇ = 0, рабочий орган не будет
ускоряться. Если приложить к рабочему органу ненулевое усилие we, то он ускорится, и мы почувствуем инерцию. Мы также
почувствуем некоторый эффект, например затухание, обусловленный членом ẋ.
Для реализации этого регулятора управляющее воздействие
(динамический винт) из (9.25) отображается в обобщенные
силы в сочленениях с использованием аналитического яко­
биана ►
Q c = JaT(q)wc,

(9.26)

чтобы отобразить его в крутящие моменты или требуемые силы
для приводов соединений.
Демпфирование, или вязкое трение, – это сила, пропорцио­
нальная скорости и препятствующая движению. На первый
взгляд, коэффициент Cx(q, q̇) выглядит как коэффициент вязкого трения, но вообще это сложная нелинейная функция конфигурации сочленения и произведения скоростей в пространстве сочленений. Для робота в его номинальной конфигурации,
когда рабочий орган движется с постоянной скоростью 0,1 м/с
в направлении оси y:
>>> xd = [0, 0.1, 0, 0, 0, 0];
>>> qd = np.linalg.inv(puma.jacob0_analytical(puma.qn, "eul")) @ xd;
>>> Cx = puma.coriolis_x(puma.qn, qd, representation="eul");

рабочий орган будет испытывать воздействие
>>> Cx @ xd
array([ 0.226, -0.02948, -0.1431, 0.006606, -0.0001601, 0.003991])

препятствующее движению в направлении оси y, подобно вязкому трению, а также толкающее рабочий орган в направлениях осей x и -z. ► Член скорости является крайне нелинейным,
и между осями пространства задач существует связь, и, как следствие, это не будет ощущаться как настоящее вязкое трение.

Это предполагает,
что нам точно
известны инерционные параметры
робота: массы
звеньев, центры
масс звеньев. Точные динамические
модели роботов
встречаются редко,
и лишь небольшое
подмножество
моделей в Toolbox
имеет динамические параметры.
Наиболее точная
динамическая
модель – models.
DH.Puma560.

Воздействие,
упоминаемое
здесь, отличается от
описанного в разделах 3.2.2 и 8.4.1.
Влияние силы тяжести, вычисленное
по формуле (9.22),
является функцией
аналитического
якобиана, который,
в свою очередь, зависит от выбранного
представления
для x. Поэтому,
чтобы преобразовать динамический
винт в пространство
сочленений, необходимо использовать
аналитический
якобиан, а не гео­
метрический, как
описано в разделе 8.4.1.
В этом примере
для представления
ориентации в пространстве задач
были выбраны
углы Эйлера, потому что при таком
представлении
ориентация не является сингулярной.
Она сингулярна для
углов крена-тангажа-рыскания.

Динамика пространства задач и управление 513

Как и в случае с силой тяжести, эту связь со скоростью, а также трение можно эффективно устранить, используя регулятор
wc = Ĉx(q, q̇)ẋ + ĝ x(q) + fˆx(q̇),
после чего динамика рабочего органа получает вид
Mx(q)ẋ˙ = we.
Однако в этом многомерном случае мат­ри­ца инерции не является диагональной.
>>> Mx =
array([[
[
[
[
[
[

puma.inertia_x(puma.qn, representation="eul")
17.21, -2.754, -9.623,
0, 0.2795,
0],
-2.754,
12.1,
1.246, -0.968, -0.07034, -0.3254],
-9.623,
1.246,
13.24,
0, 0.2795,
0],
0, -0.968,
0,
0.579,
0, 0.1941],
0.2795, -0.07034, 0.2795,
0, 0.1713,
0],
0, -0.3254,
0, 0.1941,
0, 0.1941]])

Здесь все еще имеется перекрестная связь, но на этот раз –
перекрестная связь ускорения. Например, сила 10 Н в направлении оси x
>>> np.linalg.inv(Mx) @ [10, 0, 0, 0, 0, 0]
array([ 1.167, 0.1774, 0.9012, 0.2962,

-3.304, 0.001352])

приведет к ускорению в направлениях x и z, а также к значительному ускорению вращения вокруг оси y.
Для следования по траектории x∗(t) выберем регулятор
wc = M̂x(q)(ẋ˙∗ + Kp(x∗ - x) + Kυ(ẋ∗ - ẋ)) + Ĉ x(q, q̇)ẋ
+ fˆx(q̇) + ĝx(q),

(9.27)

который учитывает отклонение фактического положения
в пространстве задач от желаемого. Если оценки инерционных
членов пространства задач точны, то динамика ошибки в пространстве задач e = x* - x будет равна
ė˙ + Kυė + Kpe = -we
Это операционное
пространство эквивалентно управлению с вычисляемым
крутящим моментом, о котором
рассказывалось
в разделе 9.4.2.

и стремиться к нулю при хорошо выбранных значениях Kp > 0
и Kv > 0, если внешнее воздействие we = 0. Нелинейная динамика твердого тела была линеаризована, поэтому данный подход
к управлению называется линеаризацией с обратной связью.
Этот регулятор отображает смещение x∗ - x в силу wc и, следовательно, действует как механическое сопротивление. Такой тип
управления роботом называется импедансным управлением.
Мы можем сделать еще шаг и фактически указать желаемую
инерцию M∗ динамики с обратной связью, используя регулятор

514 Глава 9 · Динамика и управление

wc = M̂x(q)Mx∗-1(Mx∗ ẋ˙∗ + Kp(x∗ - x) + Kυ(ẋ∗ - ẋ))
+ Ĉ x(q, q̇)ẋ + fˆx(q̇) + ĝx(q).

(9.28)

В предположении точной линеаризации динамика ошибки
становится равной
Mx∗ė˙ + Kυė + Kpe = -Mx∗Mx-1(q)we.

Этот метод иногда называют формовкой инерции (inertia shaping). Теперь можно описать динамику рабочего органа в пространстве задач с точки зрения кажущейся инерции,
демпфирования и жесткости. Однако внешнее динамическое
воздействие связано с подчеркнутым членом, который не обязательно является диагональной мат­ри­цей, и это ведет к нежелательному движению в ответ на воздействие.
Чтобы исправить ситуацию, необходимо измерить внешнее
воздействие, для чего можно использовать один из обычных
подходов. Можно измерить ток двигателя сочленения и связать
его с крутящим моментом сочленения по формуле (9.1), но на
практике трение и другие эффекты маскируют интересующий
нас сигнал. Более сложные роботы имеют датчики крутящего
момента, встроенные в приводы сочленений, например как на
рис. 9.23. Крутящие моменты сочленений связаны с крутящим
моментом рабочего органа посредством транспонирования
якобиана согласно (8.13). Альтернативный подход заключается
в использовании запястья, чувствительного к силе/крутящему
моменту, например показанного на рис. 9.24. Если выбрать регулятор, который также учитывает измеренное внешнее воздействие we#
wc = M̂x(q)Mx∗-1(Kp(x∗ - x) + Kυ(ẋ∗ - ẋ)) + Ĉ x(q, q̇)ẋ
+ fˆx(q̇) + ĝx(q) + (1 - M̂x(q)Mx∗-1)we#,

(9.29)

тогда динамика ошибки будет выражаться как
Mx∗ė˙ + Kυė + Kpe = -we.

Теперь робот действует как шестимерная пружина, и если
приложить к рабочему органу ненулевое усилие, то можно
ощутить динамику физической системы «масса–пружина–
амор­тизатор». Поступательное и вращательное демпфирование, а также жесткость определяются мат­ри­ца­ми Kυ и Kp соответственно.
До сих пор мы предполагали, что сочленениям робота можно приказать приложить определенную силу или крутящий момент, вычисляемые по формуле (9.26), и использовать конкретный закон управления. Некоторые роботы способны на это, но
большинство современных роботов управляются по координа-

Обычно Kυ и Kp –
диагональные мат­
ри­цы, что обеспечивает поведение
робота как физически реализуемой
системы пружина–
масса–амортизатор.
Диагональные
элементы этих
матриц измеряются
в Н·с/м или
Н·с·м/рад и Н/м или
Н·м/рад соответственно.

Динамика пространства задач и управление 515
Интерфейсная плата
с контроллером мотора

Плата
датчика

Поперечный
роликовый подшипник

Плата
АЦП (для
измерения
крутящего
момента)
Контактное
кольцо

Полый вал
с тензо­
датчиками

Инкремен­
тальный
энкодер
Шариковые
подшипники

Бесщеточный
мотор
постоянного
тока (BLDC)

Гармонический
привод

Абсолютный
энкодер

Рис. 9.23. Интегрированный блок датчика-привода-контроллера, способный измерять
положение и крутящий момент сочленения и управлять ими (изображение предоставлено
Тамимом Асфуром и Технологическим институтом Карлсруэ)

силомоментный датчик

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

516 Глава 9 · Динамика и управление

там, что отражает наследие манипуляторов, предназначенных
для выполнения задач, требующих точного позиционирования.
Управление крутящим моментом манипуляторов – нетривиальная задача, потому что требует учитывать трение, а также такие
сложные явления, как изменение крутящего момента двигателя
и редуктора в зависимости от пульсаций. Высокоточное управление крутящим моментом сочленений требует применения
сложных приводов, таких как на рис. 9.23, которые могут измерять крутящий момент для компенсации этих отклонений.
Стандартный робот с координатным управлением не способен распознавать контакт с окружающей средой, и силы взаимодействия при контакте будут расти очень быстро, что может
привести к повреждению робота или окружающих его предметов. Чтобы такой робот мог выполнять задачи, соответствующие требованиям, необходимо измерить критерий воздействия окружающей среды we# и использовать его для изменения
положения рабочего органа. Используя датчик силы / крутящего момента, показанный на рис. 9.24, можно выбрать регулятор
Δx = Ks-1(we∗ - we#) - K v ẋ,

чтобы определять изменение положения робота для достижения желаемого усилия. Матрица жесткости робота – Ks, а мат­ри­
ца демпфирования – K v > 0. Этот регулятор отображает воздействие we# в смещение Δx и потому действует как механическая
податливость – этот тип управления роботом так и называется:
управление податливостью. На практике такое управление требует очень высокой частоты дискретизации и малой задержки
между измерениями крутящего момента и командами коррекции положения, отправляемыми на приводы сочленений.

9.6

Применение

9.6.1

Управление в операционном пространстве

Представьте робота, которому поручено протирать стол. Высота стола неизвестна, а его поверхность лишь приблизительно
горизонтальна. Рабочий орган робота направлен вниз. Для выполнения этой задачи необходимо перемещать рабочий орган
по траектории в плоскости xy, обеспечивая охват всей поверхности и удерживая чистящий инструмент в неизменной ориентации относительно оси z. Одновременно необходимо поддерживать постоянное усилие в направлении z, чтобы прижимать
чистящий инструмент к столу, и постоянный крутящий момент
по осям x и y, чтобы поддерживать рабочий орган в положении,

Применение 517

параллельном поверхности стола. Первая группа осей должна
управляться положением, а вторая – усилием.
Регулятор управления в операционном пространстве позволяет управлять координатами или силой для каждой степени
свободы в пространстве, определяемом осями системы координат задачи. Управляющее воздействие определяется как
wc = M̂ x(q)(Ωp(ẋ˙ + Kp(x∗ - x) + Kυ(ẋ∗ - ẋ) - Ωf Kυf ẋ)
+ Ωp(Kf (w∗ - w) + w∗)
+ Ĉ x(q, q̇)ẋ + ĝx(q),
где Ki – это коэффициенты усиления. Матрицы спецификации
задачи:

– = 1 - Σ, R – ориентация системы координат задачи. Согде Σ
ответственно, мат­ри­цы спецификации координат и поворота
выглядят так:

где элементы σi или ρi равны 1, если степень свободы в пространстве задачи управляется координатами или углом, и 0,
если она управляется силой или моментом. Спецификация задачи – система координат задачи и мат­ри­цы спецификации –
может изменяться в любой момент в процессе движения.
Блок-схема управления моделью для упрощенного сценария чистки стола показана на рис. 9.25 и может быть запущена
коман­дой
>>> %run -m opspace -H

В данной симуляции система координат пространства задачи параллельна системе координат рабочего органа. Движение
контролируется положением в направлениях x и y, а также относительно осей x, y и z этой системы координат: робот перемещается из исходного положения в рабочее, используя 5 из
6 степеней свободы пространства задачи.
Движение управляется силой в направлении оси z с заданным значением -5 Н в мировой системе координат. Для этого регулятор перемещает рабочий орган вниз до касания поверхности в точке z = zt, жесткость которой моделируется как
100 Н/м. Результаты на рис. 9.26 показывают, что координаты x

518 Глава 9 · Динамика и управление

измеренная сила / крутящий момент

матрица инерции
в пространстве задачи

управление силой
Mx

ошибка силы

w∗

Блок Prod

Omega_f

Kvf

Kf

ошибка силы + EE

Блок Sum 4

Omega_f

прямая связь по силе

робот

w_c

Блок Prod 1
x∗

ошибка x

Kp

xd∗

ошибка xd

Kv

ошибка
положения +
скорости

Omega_p

прямая
динамика

силомоментный
датчик

управление положением
сила / крутящий
момент

x

Блок Main

Рис. 9.25. Блок-схема модели models/opspace, используемой для управления в операционном пространстве робота
PUMA 560, как описано в (Khatib 1987)

положение рабочего органа (м)

0.8
0.6
0.4
0.2
x
y
z
zt

0.0
−0.2

0

Fz (H)

−2
−4
−6
0.00

0.25

0.50

0.75

1.00

Время (с)

1.25

1.50

1.75

2.00

Рис. 9.26. Результаты работы регулятора рабочего пространства для степеней свободы
поступательного движения. Рабочий орган перемещается в требуемое местоположение по
осям x и y, одновременно перемещаясь в направлении –z до соприкосновения с рабочей
поверхностью на высоте zt, где есть возможность приложить заданное усилие –5 Н

и y сходятся к цели (0.8, 0.2), тогда как координата z уменьшается до тех пор, пока рабочий орган не коснется поверхности
стола. Регулятор способен одновременно соблюдать ограничения по местоположению и усилию. Рабочий орган находится

Применение 519

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

9.6.2

Или робот
не очень точен.

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

Приводы переменной жесткости

Для высокоскоростных роботов эластичность звеньев и сочле­
нений становится существенным динамическим фактором,
влияющим на точность следования по траектории. Эластичность обычно обусловлена такими элементами трансмиссии,
как волновой редуктор, обладающий собственной упругостью,
упругость вала двигателя или спиральных муфт, а также продольная упругость зубчатого ремня или тросового привода.
Наличие некоторой эластичности между двигателем и нагрузкой имеет свои преимущества. Представьте робота, выполняющего задачу, которая включает захват объекта со стола,
высота которого неизвестна. Простая стратегия – опустить
захват вниз до соприкосновения со столом, свести пальцы захвата и затем поднять его. Однако в момент контакта на робота будет воздействовать большая неравномерная сила, которая
может повредить объект или робота. Это особенно проблематично для роботов с большой инерцией, которые быстро движутся, – кинетическая энергия должна рассеиваться мгновенно. В этом случае поможет эластичный элемент (например,
пружина) между двигателем и сочленением. В момент контакта
пружина начнет сжиматься, кинетическая энергия преобразуется в потенциальную энергию пружины, и система управления
роботом успеет среагировать и остановить или реверсировать
двигатели. Мы изменили задачу с разрушительного жесткого
удара на мягкий. Помимо амортизации, деформация пружины
позволит определить силу, прилагаемую роботом. Эта возможность особенно полезна для роботов, тесно взаимодействующих с людьми, так как она снижает опасность робота в случае
столкновения, а пружина – это простая технология, которая не
может выйти из строя. Для роботов, которым для выполнения
своей задачи необходимо прилагать усилие, это более простое
решение, чем контроллер операционного пространства, представленный в разделе 9.6.1. К сожалению, управление положением в этом случае усложняется, потому что между двигателем
и нагрузкой находится упругий элемент.
Рассмотрим одномерный случай, показанный на рис. 9.27,
где двигатель представлен массой m1, к которой приложена
управляемая сила u. Он соединен через линейный упругий
элемент или пружину с грузом массой m2. Если приложить положительную силу к массе m1, то она переместится вправо и сожмет пружину, что, в свою очередь, приведет к возникнове-

520 Глава 9 · Динамика и управление

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

Рис. 9.27. Приводы переменной жесткости. Две массы представляют мотор и нагрузку
и соединены упругим элементом или пружиной. Мы используем управляющую силу,
приложенную к m1, для косвенного управления местоположением m2. fs – сила упругости
пружины, а fе – сила воздействия окружающей среды

В робототехнике такая система, встроенная в сочленение робота, известна как привод переменной жесткости (Series-Elastic
Actuator, SEA). Блок-схема модели SEA показана на рис. 9.28.
Она пытается переместить груз m2 в положение x2∗ = 1. Модель
вычисляет управляющую силу с помощью регулятора с обратной связью по положению и скорости двигателя и груза. Запус­
тить симуляцию этой системы можно так:
>>> %run -m SEA -H
регулятор с обратной связью по состоянию

масса двигателя

место­
поло­
жение
двига­
теля

пружина

x2∗
Блок Step

x∗

ошибка
состояния

коэффициент
усиления
LQR

Блок Clip

суммарная
сила
воздействия
на m1

1/m1

x1d

x1

смещение
пружины

Ks

Сила
пружины

вектор состояния (x1, x1d, x2, x2d)

состояние

масса груза
местоположение нагрузки

1/m2

x2d

Блок Prod

Местопо­
ложение
нагрузки

x2

Блок Main
0 при столкновении
1 в противном случае
столкновение?
fs – сила пружины

Рис. 9.28. Блок-схема модели models/SEA привода переменной жесткости, сталкивающегося с препятствием

Результаты показаны на рис. 9.29. В первом случае препятствие отсутствует и регулятор достигает своей цели с минимальным перерегулированием, но обратите внимание на сложный профиль силы, приложенной к m1. Во втором случае масса
груза останавливается на отметке x2 = 0.8, и сила упругости изменяется, чтобы компенсировать это.

Подведение итогов 521
2.5

x2
u
Fs

2.0

2.5

1.5

1.5

1.0

1.0

0.5

0.5

0.0

0.0

−0.5

−0.5

−1.0

−1.0

−1.5

−1.5

−2.0

а

0

1

2

Время (с)

3

4

5

x2
u
Fs

2.0

−2.0

б

0

1

2

Время (с)

3

4

5

Рис. 9.29. Реакция привода переменной жесткости на единичный шаг воздействия при t = 1 с, показывающая
положение груза (m), силу (Н), прикладываемую двигателем, и силу (Н), прикладываемую пружиной: а) движение
к точке x2∗ = 1 без столкновения; б) движение к точке x2∗ = 1 с препятствием в точке x2 = 0.8, которая достигается
в момент времени t ≈ 2.3

Зная x1 и x2, а также жесткость пружины Ks, можно вычислить
силу внешней среды fе, приложенную к m2. Привод также является датчиком, и некоторые из этих принципов используются
в приводе, показанном на рис. 9.23.
Более сложная для анализа проблема – эластичность звеньев.
Большинство роботов имеют очень жесткие звенья, чтобы избежать этой проблемы, но для роботов, действующих с экстремальными ускорениями, может потребоваться учитывать ее.

9.7

Подведение итогов

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

522 Глава 9 · Динамика и управление

влияет на другие сочленения, что важно для проектирования
систем управления. Затем мы обсудили прямую динамику, которая описывает изменение конфигурации со временем под
действием сил и крутящих моментов, прикладываемых к со­
членениям приводами и внешними силами, такими как сила
тяжести. Мы расширили концепцию управления с прямой
связью до полного управления на основе модели, используя
управление с прямой связью по крутящему моменту и управление с вычисляемым крутящим моментом. Затем мы переформулировали динамику твердого тела в пространстве задач
и спроектировали регулятор для управления положением, в котором можно задать динамические характеристики рабочего
органа как систему пружина–груз–демпфер. Наконец, мы обсудили два применения: управление в рабочем пространстве, где
можно управлять положением или силой различных степеней
свободы в пространстве задач, и приводы переменной жесткости, где эластичный элемент между двигателем и звеном манипулятора обеспечивает управление силой и безопасность при
работе с человеком.

9.7.1

Дополнительное чтение

Инженерное проектирование систем управления двигателями
рассматривается в учебниках по мехатронике, таких как Bolton
(2015). Динамика многозвенных манипуляторов подробно
описывается во всех стандартных учебниках по робототехнике,
таких как Paul (1981), Spong et al. (2006), Siciliano et al. (2009)
и «Robotics Handbook» (Siciliano and Khatib 2016). Эффективный
рекурсивный метод Ньютона–Эйлера, ныне широко используемый, является результатом многочисленных исследований
начала 1980-х и описан в работе Hollerbach (1982). Уравнения
движения можно вывести с помощью ряда методов, включая
методы Лагранжа (на основе энергии), Ньютона–Эйлера, Даламбера (Fu et al. 1987, Lee et al. 1983) или метод Кейна (Kane and
Levinson 1983). Вычислительная стоимость методов Лагранжа
(Uicker 1965; Kahn 1969) огромна, O(N 4), что делало невозможным его использование на компьютерах той эпохи и требовало
множества упрощений и приближений. В Orin et al. (1979) был
предложен альтернативный подход, основанный на уравнениях Ньютона–Эйлера движения твердого тела, применяемых
к каждому звену. Затем Армстронг в Armstrong (1979) показал,
как можно применить рекурсию, что позволило уменьшить
сложность до O(N ). В Luh et al. (1980) была предложена рекурсивная формулировка уравнений Ньютона–Эйлера с линейными и угловыми скоростями в системах координат звена, кото-

Подведение итогов 523

рая дала тысячекратное сокращение времени, необходимого
для выполнения вычислений, и сделала возможным реализовать эти вычисления в реальном времени. Холлербах в Hollerbach (1980) показал, как можно применить рекурсивный подход
к методу Лагранжа и сделать его всего лишь в 3 раза медленнее
рекурсивной формы Ньютона–Эйлера, а Сильвер в Silver (1982)
показал эквивалентность рекурсивной формы Лагранжа и формы Ньютона–Эйлера и что разница в эффективности обусловлена представлением угловой скорости.
Рекурсивный алгоритм Ньютона–Эйлера обычно вычисляется с использованием векторного представления поступательных и угловых скоростей и ускорений, но это громоздкий
и неудобный способ. Пространственная векторная нотация
Фезерстоуна (Featherstone 1987, 2010a, 2010b) гораздо проще
и компактнее и поддерживается в пакете Toolbox классами пространственной математики SpatialVelocity, SpatialAcceleration,
SpatialForce, SpatialMomentum и SpatialInertia.
Прямая динамика (раздел 9.3) требует больших вычислительных затрат. В Toolbox реализован метод O(N 3), предложенный
в Walker and Orin (1982). Метод сочлененного тела из Featherstone (1987) имеет сложность O(N), но при N < 9 обходится дороже, чем метод Уокера и Орина.
Для любого анализа динамики робота критически важно
знать инерционные параметры (по десять на звено), а также параметры двигателя. Corke and Armstrong-Hélouvry (1994, 1995)
опубликовали метаисследование параметров PUMA 560 и представили согласованную оценку инерционных и двигательных
параметров этого манипулятора. Некоторые из этих данных
были получены путем разборки робота и тщательного измерения и взвешивания его компонентов. Инерцию компонентов
можно оценить по массе и размерам, предположив распределение масс, или измерить с помощью бифилярного маятника,
как описано в Armstrong et al. (1986).
Также параметры можно оценить, измеряя крутящие моменты в сочленениях или силу и момент реакции основания при
движении робота. Среди ранних работ в этой области можно
назвать Mayeda et al.(1990), Izaguirre and Paul (1985), Khalil and
Dombre (2002), а наиболее примечательной из последних пуб­
ликаций является Siciliano and Khatib (2016, § 6). Определить
базовые инерциальные параметры роботов можно путем анализа некоторых интересных траекторий их движения (Gautier
and Khalil 1992, Armstrong 1989). Трение является важной динамической характеристикой и подробно описано в диссертации
Armstrong (1988). В Armstrong-Hélouvry et al. (1994) представлено очень доступное и подробное исследование моделирования и управления трением. Параметры двигателя можно узнать
непосредственно из паспорта производителя или определить

524 Глава 9 · Динамика и управление

экспериментально, не снимая двигатель с робота, как описано
в Corke (1996a). Параметры, используемые в модели PUMA 560
в пакете Toolbox, являются наилучшими оценками, полученными из работ Corke and Armstrong-Hélouvry (1995) и Corke (1996a).
Обсуждение вопросов управления было довольно кратким,
и в нем особое внимание уделялось преимуществам упреждающего управления. Методы управления сочленениями робота
подробно описаны в работах Spong et al. (2006), Craig (2005) и Siciliano et al. (2009) и обобщены в Siciliano and Khatib (2016, § 8).
В Siciliano et al. (2009) приводится хорошее обсуждение приводов и датчиков, как и в довольно старой книге Klafter et al.
(1989). Управление роботами с гибкими сочленениями обсуждается в работе Spong et al. (2006). Адаптивное управление может
использоваться для адаптации к изменяющимся во времени
инерционным параметрам, и по этой теме существует обширная литература, но наиболее хорошими ранними источниками
являются книга Craig (1987) и статьи Craig et al. (1987), Spong
(1989), Middleton and Goodwin (1988) и Ortega and Spong (1989).
Структура управления в операционном пространстве была
предложена в работе Khatib (1987), и пример в разделе 9.6.1 основан на этой работе с некоторыми небольшими изменениями
в обозначениях. В последнее время наблюдается значительный
интерес к приводам переменной жесткости, положение и жесткость которых можно контролировать независимо, подобно нашим собственным мышцам. Хороший сборник статей по этой
технологии можно найти в специальном выпуске Vanderborght
et al. (2008).
Динамическая манипулируемость обсуждается в работах
Spong et al. (2006) и Siciliano et al. (2009). Мера Асады, используе­
мая в Toolbox, описана в работе Asada (1983).

Исторические и интересные факты
Второй закон Ньютона описан в его главном труде «Principia
Naturalis» («Математические начала натуральной философии»),
написанном на латыни и вышедшем в 1687 году. Перевод этого труда на английский язык доступен по адресу https://www.
archive.org/details/newtonspmathema00newtrich. Его труды по
другим темам, включая расшифровки его записных книжек,
можно найти на сайте http://www.newtonproject.sussex.ac.uk.

9.7.2

Упражнения

1. Независимое управление сочленениями (раздел 9.1.8).
а) Исследуйте различные значения Kv и Ki, а также требуемые форму и амплитуду командного сигнала.

Подведение итогов 525

2.
3.

4.

5.
6.

7.

б) Выполните анализ устойчивости с помощью корневого
годографа и определите максимально допустимый коэффициент усиления для пропорционального варианта.
Повторите эти действия для варианта с PI-регулятором.
в) Учтите, что двигатель управляется источником напряжения, а не тока и что его импеданс составляет 1 мГн
и 1.6 Ом. Измените значение vloop соответствующим образом. Расширьте модель, включив в нее влияние обратной ЭДС.
г) Увеличьте требуемую скорость движения, чтобы момент
двигателя достиг насыщения. При интегральном регулировании вы увидите явление, известное как интегральное
возбуждение, – посмотрите, что происходит с состоянием
интегратора во время движения. Для борьбы с этим явлением применяются различные стратегии, например
ограничение максимального значения интегратора или
разрешение интегрального действия только при приближении двигателя к заданному значению. Поэкспериментируйте с некоторыми из них.
д) Создайте модель робота PUMA, в которой каждое сочле­
нение управляется через ploop. Параметры различных
двигателей PUMA описаны в работе Corke and ArmstrongHélouvry (1995).
Постоянная момента двигателя измеряется в Н·м/А и равна
постоянной обратной ЭДС, измеряемой в В·с/рад. Покажите,
что эти единицы эквивалентны.
Простой двухзвенный робот-манипулятор (рис. 9.4).
а) Постройте график силы тяжести как функции углов обоих
суставов. Предположите, что m1 = 0.45 кг, m2 = 0.35 кг, r1 =
8 см и r2 = 8 см.
б) Постройте график инерции сочленения 1 как функции q2.
Для вычисления инерции звена предположите, что звено
можно представить в виде точечной массы, расположенной в центре масс.
Запустите код, который генерирует рис. 9.15, чтобы вычислить гравитационную нагрузку на сочленения 2 и 3 как
функцию конфигурации. Добавьте полезную нагрузку и повторите.
Запустите код, который генерирует рис. 9.16, и покажите, что
инерции сочленений 1 и 2 различаются в зависимости от полезной нагрузки.
Постройте кривую, показанную на рис. 9.16в. Добавьте полезную нагрузку и сравните результаты. Во сколько раз
изменяется инерция в диапазоне углов поворота сочле­
нения?
Почему мат­ри­ца инерции манипулятора симметрична?

526 Глава 9 · Динамика и управление

8. Робот прикладывает усилие к основанию во время движения (раздел 9.2.6). Предположите, что робот стоит на
горизонтальном столе без трения (например, на большой
воздушной шайбе). Создайте имитационную модель, учитывающую динамику манипулятора робота и динамику
скольжения по столу. Покажите, что движение манипулятора приводит к поступательному и вращательному движению робота. Попробуйте разработать движение манипулятора, которое переместит основание робота из одной точки
в другую и остановит его.
9. Наложите эллипсоид динамической манипулируемости на
изображение робота. Сравните его с эллипсоидом сил из
раздела 8.4.2.
10. Управление на основе модели (раздел 9.4).
а) Вычислите и отобразите отклонение сочленения для
случаев управления с прямой связью по крутящему моменту и с вычисляемым крутящим моментом. Поэкспериментируйте с различными движениями, параметрами управления и частотой дискретизации Tfb.
б) Уменьшите частоту вычисления крутящего момента для
прямой связи и посмотрите, как это повлияет на величину отклонения.
в) На практике динамическая модель робота точно неизвестна, мы можем только рассчитывать на нашу наилучшую оценку динамики жесткого тела. Это можно
смоделировать с помощью метода perturb (см. онлайндокументацию), который возвращает объект робота
с инерционными параметрами, измененными на указанный процент. Измените модель на блок-схеме так,
чтобы блок обратной динамики использовал модель робота с параметрами, измененными на 10 %, то есть чтобы обратная динамика вычислялась для динамической
модели, несколько отличающейся от модели управляемого робота, и тем самым можно было бы продемонстрировать влияние ошибки модели на характеристики
управления. Исследуйте влияние ошибки как для случая
прямой связи по крутящему моменту, так и для случая
вычисляемого крутящего момента.
г) Расширьте пример управления в операционном пространстве, включив датчик, измеряющий все силы и моменты, прилагаемые роботом к наклонной поверхности
стола. Перемещайте рабочий орган робота по окружности в плоскости xy, прилагая постоянную силу, направленную вниз. Рабочий орган должен двигаться вверх
и вниз, описывая окружность. Покажите, как контроллер
позволяет рабочему органу робота приспосабливаться
к поверхности с неизвестной высотой и ориентацией.

Подведение итогов 527

11. Управление в операционном пространстве (раздел 9.6.1).
а) Увеличьте жесткость поверхности стола и посмотрите,
как это скажется на высоте рабочего органа в стационарном состоянии.
б) Исследуйте влияние изменения различных коэффициентов усиления.
в) Добавьте более сложную траекторию для движения
в плоскости xy, охватывающую всю рабочую зону.
г) Измените функцию силомоментного датчика для моделирования стола, наклоненного в направлении оси x
мировой системы координат.
д) То же, но стол наклонен в мировых направлениях x и y.
е) Поэкспериментируйте с различными представлениями
положений в пространстве задач, например углами крена-тангажа-рыскания или экспоненциальными координатами.
ж) Добавьте соответствие осям x и y, чтобы плоская чистящая губка для стола соответствовала нормали к поверхности.
з) Используйте модель другого робота, например Panda,
и ответьте на вопросы выше еще раз.
12. Приводы с переменной жесткостью (раздел 9.6.2).
а) Поэкспериментируйте с различными значениями жесткости упругого элемента и параметрами управления.
Постарайтесь сократить время установки в требуемое
положение.
б) Измените модель так, чтобы манипулятор робота касался объекта на неизвестном расстоянии и прикладывал
к нему силу 5 Н.
в) Постройте график функции частотной характеристики
X2(s)/X1(s) для различных значений Ks, m1 и m2.
г) Смоделируйте эффект столкновения груза с препятствием, добавив шаг к силе пружины.

Часть IV	Компьютерное
зрение
Глава 10
Глава 11
Глава 12
Глава 13
Глава 14

Свет и цвет
Изображения и их обработка
Извлечение признаков изображения
Формирование изображения
Использование нескольких изображений

Как обсуждалось в главе 1, почти у всех видов животных есть глаза, и наш собственный опыт показывает, что глаза – эффективные и бесценные сенсоры для
распознавания, навигации, обхода препятствий и манипулирования предметами. Зрение для роботов – роботизированное зрение – реализуется с использованием видеокамер, имитирующих функции глаз, и алгоритмов, имитирующих работу мозга.
Мы начинаем главу 10 с обсуждения света и, в частности, цвета, потому что
это очень важная характеристика мира, который мы воспринимаем. Хотя мы
изучаем цвет в детском саду, это сложная тема, которую часто плохо понимают. Получение изображений и основные алгоритмы обработки изображений
рассматриваются в главе 11; на них основаны алгоритмы извлечения признаков, обсуждаемые в главе 12. Извлечение признаков – это задача сокращения
данных, извлечения сущности сцены из огромного количества пикселей. Например, чтобы определить координаты круглого красного объекта, нам необходимо каким-то образом получить всего четыре байта из миллионов байтов,
составляющих изображение. Чтобы решить эту задачу, мы должны ответить на
множество важных промежуточных вопросов, таких как «что такое красный
цвет?», «как отличить красные пиксели от некрасных?», «как описать форму
красных пикселей?», «что, если красных объектов несколько?» и т. д.
В главе 13 мы поговорим о том, как изображение мира формируется на сенсоре и преобразуется в цифровое изображение. Мы рассмотрим множество
различных камер, а также обсудим ограничения использования только одной
камеры для наблюдения за миром. Биология снова показывает нам решение –
наличие нескольких глаз. Как оказывается, в природе многие виды живых существ имеют по нескольку глаз. Это подводит нас к рассмотрению использования множественных видов мира, с одной движущейся камеры или нескольких
камер, наблюдающих за сценой с разных точек зрения. Эта тема обсуждается
в главе 14, и она особенно важна для понимания трехмерной структуры мира.
Все это подготавливает почву для описания того, как можно использовать зрение для управления манипуляторами и мобильными роботами, что станет темой следующей, заключительной части книги.

Глава

10

	 Свет и цвет
природе свет создает цвет, а на картинке цвет создает
« Всвет.
– Ганс Хоффман

chap10.ipynb

https://go.sn.pub/
gZ5KyJ

В древности считалось, что глаз излучает конусообразный зрительный поток, который соприкасается с видимыми объектами
мира, создавая у наблюдателя ощущение, подобное осязанию,
но на расстоянии, – это теория экстрамиссии. Сегодня мы считаем, что часть света, падающего на сцену от источника, отражается в глаза наблюдателя, чтобы создать представление
об этой сцене. Свет, достигающий глаза или камеры, является
функцией от количества света, падающего на сцену, и свойства
материала сцены, называемого отражательной способностью.
Эта глава посвящена самому свету и нашему восприятию
света с точки зрения яркости и цвета. Раздел 10.1 описывает
свет с точки зрения электромагнитного излучения и смесей
света в виде непрерывных спектров. Раздел 10.2 представляет
собой краткое введение в колориметрию – науку о восприятии
цвета, трехцветном восприятии цвета человеком и о том, как
цвета могут быть представлены в различных цветовых пространствах. Раздел 10.3 охватывает ряд дополнительных тем,
таких как постоянство цвета, гамма-коррекция и баланс белого. В разделе 10.4 приведены два рабочих примера приложений,
предназначенных для различения цветных объектов на изображении и удаления теней с изображения.

10.1

Спектральное представление света

Примерно в 1670 г. сэр Исаак Ньютон обнаружил, что белый
свет представляет собой смесь разных цветов. Теперь мы знаем, что каждый из этих цветов соответствует одной частоте или
длине волны электромагнитного излучения. Мы воспринимаем
длины волн от 400 до 700 нм как разные цвета, как показано на
рис. 10.1.
В большинстве случаев свет, который мы наблюдаем, представляет собой смесь волн различной длины и может быть

532 Глава 10 · Свет и цвет

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

Длина волны (нм)

Рис. 10.1. Спектр видимых цветов как функция длины волны в нанометрах. Видимый
диапазон зависит от условий просмотра и человека, но обычно принимается равным
400–700 нм. Длины волн более 700 нм называются инфракрасными, а менее 400 нм –
ультрафиолетовыми

Наиболее распространенным источником света является
лампа накаливания, которая работает по принципу излучения
света от горячего тела, такого как Солнце или нить накала традиционной лампочки. В физике это явление моделируется как
излучающее черное тело (blackbody radiator), или планковский
источник. Излучаемая мощность как функция длины волны λ
определяется формулой излучения Планка:
(10.1)
где T – абсолютная температура (K) источника, h – постоянная
Планка, k – постоянная Больцмана и c – скорость света. ► Это
мощность, излучаемая на стерадиан ► на единицу площади на
единицу длины волны.
Мы можем построить спектры излучения абсолютно черного
тела при разных температурах. Сначала определим диапазон
длин волн: ►
>>> nm = 1e-9;
>>> lmbda = np.linspace(300, 1_000, 100) * nm;
Отступление 10.1. Спектр света
Во время эпидемии чумы 1665–1666 гг. Исаак
Ньютон разработал свою теорию света и цвета. Он показал, что призма может разложить
белый свет на спектр цветов, а линза и вторая
призма могут преобразовать многоцветный
спектр в белый свет (набросок Ньютона слева).
Ньютон обнаружил еще один важный факт:
цвет света не меняется при отражении от разных объектов, из чего он сделал вывод, что
цвет является внутренним свойством света, а не объекта.

c = 2.998×108 м/с.
h = 6.626×10–34 Дж·с.
k = 1.381×10–23 Дж/К.
Телесный угол измеряется в стерадианах
(ср), полная сфера
равна 4π ср.
Я использовал для
переменной имя
lmbda, потому что
lambda является
зарезервированным
словом в Python.

Спектральное представление света 533

в данном случае от 300 до 1000 нм, а затем вычислим спектры
абсолютно черного тела:
>>> for T in np.arange(3_000, 6_001, 1_000):
... plt.plot(lmbda / nm, blackbody(lmbda, T))

как показано на рис. 10.2а. Мы можем видеть, что с ростом температуры максимальное количество мощности увеличивается,
а длина волны, на которой возникает пик, уменьшается. Общее
количество излучаемой мощности (на единицу площади) представляет собой площадь под кривой абсолютно черного тела
и определяется законом Стефана–Больцмана:

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

Длина волны пика уменьшается по мере повышения температуры, и мы можем наблюдать это явление, нагревая объект.
Как показано в табл. 10.1, он начинает светиться слабо красным
при температуре около 800 К и переходит от оранжевого и желтого к белому по мере повышения температуры.
Вольфрамовая нить лампы накаливания имеет температуру
2600 К и раскаляется добела. Солнце имеет температуру поверхности 5778 К. Спектры этих источников

Нормализованная E(λ)

E(λ) (Wsr–1m–2m–1)

>>> lamp = blackbody(lmbda, 2_600);
>>> sun = blackbody(lmbda, 5_778);
>>> plt.plot(lmbda / nm, np.c_[lamp / np.max(lamp),
...
sun / np.max(sun)]);

Вольфрамовая нить лампы (2600K)
Солнце (5778K)
Чувствительность глаза

Длина волны (нм)
Длина волны (нм)
а
б
Рис. 10.2. Спектры черного тела: а) спектры излучения черного тела для температур от 3000 до 6000 K; б) излучение
черного тела для температуры Солнца (5778 K), вольфрамовой нити лампы (2600 K) и кривая чувствительности
человеческого глаза – все значения нормализованы к единице для наглядности

534 Глава 10 · Свет и цвет
Отступление 10.2. Инфракрасное излучение
Инфракрасное излучение было открыто в 1800 г.
Уильямом Гершелем (1738–1822), британским
астрономом немецкого происхождения. Он
был придворным астрономом Георга III; построил серию больших телескопов; со своей
сестрой Кэролайн провел первый обзор неба,
обнаружив двойные звезды, туманности и планету Уран, и изучал спектры звезд. Используя
призму и термометры для измерения количест­
ва тепла в различных цветах солнечного света,
он заметил, что температура увеличивалась от
синего до красного и еще больше повышалась
за пределами красного, где не было видимого
света (изображение из рукописи Гершеля 1800 г.).
Отступление 10.3. Лампа накаливания
Сэр Хемфри Дэви продемонстрировал первую
электрическую лампу накаливания с использованием платиновой нити в 1802 г. Затем
в 1850 г. сэр Джозеф Свон продемонстрировал
свои первые электрические лампочки с использованием нити накаливания из углеро­
дис­той бумаги. Однако только после появления в 1865 г. мощных вакуумных насосов такие
лампы начали служить достаточно долго, чтобы найти массовое применение. Свон запатентовал карбонизированную хлопковую нить
в 1878 г. и карбонизированную целлюлозную
нить в 1881 г. Его лампы стали широко применяться после 1880 г., а театр «Савой» в Лондоне был полностью освещен электричеством
в 1881 г. В США Томас Эдисон не начинал исследования ламп накаливания до 1878 г., но
в следующем году он запатентовал долговечную карбонизированную бамбуковую нить
и смог начать их массовое производство. Компании Свона и Эдисона объединились в 1883 г.
Впоследствии лампа накаливания стала доминирующим источником света на
планете, но в настоящее время от нее отказываются из-за низкой энергоэффективности. (Фото Дугласа Брэкетта, Inv., Edisonian.com.)
Таблица 10.1. Цвет нагретого объекта в зависимости от температуры
Начальное красное каление
Темно-красное каление
Ярко-красное каление
Желтовато-красное каление
Начальное белое каление
Белое каление

770–820 К
920–1020 К
1120–1220 К
1320–1420 К
1520–1620 К
1720–1820 К

Спектральное представление света 535

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

10.1.1

Поглощение

Спектр Солнца на поверхности Земли измерен и сведен в таб­
лицу:
>>> sun_ground = loadspectrum(lmbda, "solar");
>>> plt.plot(lmbda / nm, sun_ground);

График спектральной интенсивности показан на рис. 10.3а.
Он заметно отличается от графика излучения идеального черного тела при аналогичной температуре, поскольку некоторые
длины волн поглощаются атмосферой больше, чем другие.
В ходе эволюции пиковая чувствительность нашего глаза максимально приблизилась к пику спектра солнечного света, прошедшего через атмосферу.
Коэффициент пропускания T является обратной величиной
коэффициента поглощения и представляет собой долю прошедшего света в зависимости от длины волны и пройденного
расстояния. Он описывается законом Бера:
T = 10-Ad,

(10.2)

где A – коэффициент поглощения в единицах м-1, который является функцией длины волны, а d – длина оптического пути.
Спектр поглощения A(λ) для воды мы можем получить из таб­
личных данных
>>> lmbda = np.linspace(300, 1_000, 100) * nm;
>>> A = loadspectrum(lmbda, "water");

а пропускание через 5 м воды равно:
>>> d = 5;
>>> T = 10 ** (-A * d);
>>> plt.plot(lmbda / nm, T);

Результат изображен на рис. 10.3. Мы видим, что красный свет
сильно ослаблен, что делает объект более синим. Дифференциальное поглощение длин волн является серьезной проб­лемой
при съемке под водой, и мы вернемся к этой теме в разделе 10.3.4.

536 Глава 10 · Свет и цвет

E(λ) (W ср–1м–2м–1) ×

Солнце на пов. земли
Солнце (абс. черн. тело)
Чувствительность глаза

а

Длина волны (нм)

б

Длина волны (нм)

Рис. 10.3. а) Модифицированный солнечный спектр на уровне земли (синяя линия). Провалы в солнечном спектре
соответствуют различным полосам поглощения воды. СО2 поглощает излучение в инфракрасной области, а озон
О3 сильно поглощает в ультрафиолетовой области. Спектр черного тела с температурой Солнца показан синим
пунктиром, а чувствительность человеческого глаза показана красным; б) прохождение света через 5 м воды. Более
длинные волны (красные) были сильно ослаблены

10.1.2

Отражение

Поверхности отражают падающий свет. Отражение может быть
зеркальным (например, блики от воды) или ламбертовым (рассеянное отражение от матовой поверхности). Доля отраженного
света R(λ) ∈ [0, 1] представляет собой отражательную способность, коэффициент отражения (альбедо) поверхности и зависит
от длины волны. Белая бумага, например, имеет отражательную
способность около 70 %. Спектры отражения многих материалов
были измерены и сведены в таблицы. Рассмотрим, к примеру,
отражательную способность красного кирпича:
>>> lmbda = np.linspace(100, 10_000, 100) * nm;
>>> R = loadspectrum(lmbda, "redbrick");
>>> plt.plot(lmbda / (1_000 * nm), R);

График на рис. 10.4 показывает, что он отражает красный
свет больше, чем синий.

10.1.3

Яркость

Если говорить о свете, отраженном от поверхности, его яркость
имеет спектр, определяемый выражением
L(λ) = E(λ)R(λ) W м-2,

(10.3)

где E – падающее освещение, а R – коэффициент отражения. Освещенность от прямого излучения Солнца в видимой области
равна

http://speclib.
jpl.nasa.gov/,
выветрившийся
красный кирпич
(0412UUUBRK).

Спектральное представление света 537
>>> lmbda = np.arange(400, 701) * nm;
>>> E = loadspectrum(lmbda, "solar");

на уровне земли. Отражательная способность кирпича:
>> R = loadspectrum(lmbda, 'redbrick');

а свет, отраженный от кирпича:
>>> L = E * R;
>>> plt.plot(lmbda / nm, L);

что показано на рис. 10.5. Именно этот спектр интерпретируется нашими глазами как красный цвет.

а

Длина волны (мкм)

б

Длина волны (нм)

L(λ) (W ср–1м–2м–1) × 108

Рис. 10.4. Отражение света от выветренного красного строительного кирпича (данные ASTER, Baldridge et al.
2009): а) полный диапазон измерений составляет от 300 нм в видимом диапазоне до 10 000 нм (в инфракрасном
диапазоне); б) крупный план видимой части спектра

Длина волны (нм)

Рис. 10.5. Яркость выветренного красного кирпича дома при солнечном освещении на уровне
земли, по данным рис. 10.3а и 10.4б

538 Глава 10 · Свет и цвет

10.2

Цвет

« Цвет – это совокупное название всех ощущений, возникающих

в результате деятельности сетчатки глаза и связанных с ней
нервных механизмов, причем эта активность почти во всех
случаях у нормального человека является специфической реак­
цией на лучистую энергию с определенной длиной волны и ин­
тенсивностью.
– Т. Л. Троланд,
отчет комитета Оптического общества Америки
по колориметрии 1920–1921 гг.

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

10.2.1

Человеческий глаз

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

10 мкм

Рис. 10.6. Цветная микрофотография палочек (белые объекты) и колбочек (желтые объекты)
в человеческом глазу, сделанная сканирующим электронным микроскопом. Диаметр клеток
находится в пределах 0.5–4 мкм. Клетки содержат различные типы светочувствительных
белков – опсинов. Удивительно, но палочки и колбочки находятся не на поверхности сетчатки,
а за ее поверхностью, которая представляет собой сеть нервов и кровеносных сосудов

Цвет 539

Люди, как и большинство приматов, являются трихроматами
и имеют три типа колбочек, которые реагируют на разные части
спектра. Они условно называются длинными (L, long), средними (M, medium) и короткими (S, short) в зависимости от длины
волны их пикового отклика, или, чаще, красными, зелеными
и синими. Большинство других млекопитающих являются ди­
хроматами, то есть имеют всего два типа колбочек. Многие птицы и рыбы являются тетрахроматами и обладают колбочками,
чувствительными к ультрафиолетовому свету. А стрекозы имеют до тридцати различных типов колбочек.
Спектральная реакция палочек и колбочек была тщательно
изучена, и реакция клеток колбочек человека может быть загружена из готовой таблицы при помощи кода
>>> cones = loadspectrum(lmbda, "cones");
>>> plt.plot(lmbda / nm, cones);
Разные спектральные характеристики
обусловлены разными фотопсинами
в колбочках.

где cones имеет три столбца, соответствующих откликам кол­
бочек L, M и S, и каждая строка соответствует длине волны
в lmbda. Спектральный отклик колбочек L(λ), M(λ) и S(λ) показан
на рис. 10.7а. ◄
красные (L) колбочки

Нормализованный отклик колбочек L(λ)

зеленые (M) колбочки

а

Фотопическая яркость (лм/Вт)

синие (S) колбочки

Длина волны (нм)

б

Длина волны (нм)

Рис. 10.7. а) Спектральная чувствительность колбочек глаза человека (нормализованная); б) кривая светимости
для обычного человека-наблюдателя. Пик отклика составляет 683 лм/Вт при 555 нм (зеленый)

Сетчатка человеческого глаза имеет центральную, или фовеальную, область диаметром всего 0.6 мм, имеет поле зрения 5°
и содержит большую часть из 6 млн колбочек: 65 % воспринимают красный цвет, 33 % – зеленый и только 2 % – синий. Мы
бессознательно сканируем нашей «зрительной ямкой высокого разрешения» весь видимый мир вокруг себя, чтобы создать
крупномасштабный мысленный образ окружающей обстановки. Кроме того, по сетчатке распределено 120 млн палочек, которые также чувствительны к движению.

540 Глава 10 · Свет и цвет
Отступление 10.4. Опсины
Опсины – это молекулы фоторецепторов
в основе зрительных систем всех животных. Они принадлежат к классу рецепторов, связанных G-белком (GPCR), и состоят из семи спиралей, проходящих через
клеточную мембрану. Они меняют форму
в ответ на определенные молекулы вне
клетки и инициируют каскад химических
сигнальных событий внутри клетки, что
приводит к изменению состояния клетки. Опсины содержат хромофор – светочувствительную молекулу, называемую
ретиналем и получаемую из витамина А,
тело клетки
которая проходит через опсин. Когда сетчатка поглощает фотон, молекула ретиналя меняет свою форму, тем самым деформирует опсин и активирует сигнальный
путь клетки. Основой любого зрения является случайная генетическая мутация
700 млн лет назад, которая сделала химический чувствительный рецептор светочувствительным. В животном мире существует множество вариантов опсинов:
наши палочки содержат родопсин, а колбочки – фотопсины. Американский биохимик Джордж Уолд (1906–1997) получил Нобелевскую премию по медицине 1967 г.
за открытие сетчатки и характеристику спектрального поглощения фотопсинов.
(Изображение d-пирана из «Википедии», хромофор указан стрелкой.)

Отступление 10.5. Трехкомпонентная теория
Трехкомпонентная теория цветового зрения предполагает, что наши глаза имеют
три дискретных типа рецепторов, которые при стимуляции производят ощущения красного, зеленого и синего цветов,
и что все цветовые ощущения представляют собой «психологические смеси» этих
основных цветов. Впервые она была предложена английским ученым Томасом Янгом (1773–1829) в 1802 г., но не получила
большого признания. Позже ее отстаивали Гельмгольц и Максвелл. На рисунке
показано, как смешиваются лучи красного, зеленого и синего светов. Гельмгольц
(1821–1894) был плодовитым немецким
врачом и физиком. Он изобрел офтальмоскоп для исследования сетчатки в 1851 г., а в 1856 г. опубликовал Handbuch der
physiologi­schen Optik («Справочник по физиологической оптике»), в котором содержались тео­рии и экспериментальные данные, касающиеся восприятия глубины,
цветового зрения и восприятия движения. Максвелл (1831–1879) был шотландским ученым и в основном известен как автор уравнений электромагнитного
поля, но также изучал цветовосприятие, дальтонизм и теорию цвета. Его статья
1860 г. «О теории цветового зрения» получила медаль Румфорда, а в 1861 г. он продемонстрировал цветную фотографию в лекции Королевского института.

R

G

B

Цвет 541
Отступление 10.6. Теория противоположных цветов
Теория противоположных цветов утверждает, что цвета воспринимаются по двум осям: «красный–зеленый» и «синий–желтый».
Одна подсказка исходит от цветных остаточных изображений –
если долго смотреть на красный квадрат, а затем перевести
взгляд на белую поверхность, то мы ощутим зеленое остаточное
изображение. Еще одна подсказка исходит из языка – мы комбинируем слова для описания оттенков, например красноватосинего, но никогда не описываем красновато-зеленый или синевато-желтый. Эта теория была впервые выдвинута немецким
писателем Иоганном Вольфгангом фон Гете (1749–1832) в его
«Теории цвета» 1810 г., но позже нашла сильного сторонника
в лице Карла Эвальда Геринга (1834–1918), немецкого физиолога,
который также изучал бинокулярное зрение и движения глаз. Он
был сторонником теории противоположных цветов, а не трехкомпонентной теории и вел острые дебаты с Гельмгольцем по
этой теме. На самом деле справедливы обе теории. В наших глазах есть три типа цветочувствительных клеток, но начальная обработка в ганглиозном слое сетчатки, по-видимому, преобразует
эти сигналы в представление противоположных цветов.

Отступление 10.7. Радиометрические и фотометрические величины
При обсуждении света используются два совершенно разных набора единиц: радиометрические и фотометрические. Радиомет­
рические единицы используются в разделе 10.1 и основаны на
таких величинах, как мощность, которые выражаются в привычных единицах СИ, таких как ватты.
Фотометрические единицы являются аналогами радиометрических единиц, но учитывают зрительные ощущения наблюдателя. Световая мощность, или световой поток, – это воспринимаемая мощность источника света, измеряемая в люменах (лм),
а не в ваттах. Монохроматический источник света мощностью
1 Вт на длине волны 555 нм, соответствующей пиковому отклику
человеческого глаза, по определению излучает световой поток
683 лм. Напротив, источник света мощностью 1 Вт на длине волны 800 нм излучает световой поток 0 лм – он вообще не вызывает
визуальных ощущений.
Однако лампа накаливания мощностью 1 Вт создает воспринимаемое визуальное ощущение менее 15 лм или светоотдачу
15 лм/Вт. Люминесцентные лампы достигают эффективности до
100 лм/Вт, а белые светодиоды – до 150 лм/Вт.

10.2.1.1 Воспринимаемая яркость
Яркость, которую мы связываем с определенной длиной волны, задается функцией светимости (luminosity, фототопическая
яркость). Для нашего дневного (фотопического) зрения яркость

542 Глава 10 · Свет и цвет

как функция длины волны была экспериментально измерена,
сведена в таблицу и легла в основу стандарта CIE 1931 г., который описывает восприятие среднего человека-наблюдателя.
В Toolbox имеется функция фотопической яркости
>>> human = luminos(lmbda);
>>> plt.plot(lmbda / nm, human);

Ее график показан на рис. 10.7б. Возьмем два источника света, излучающих одинаковую мощность (в ваттах), но один из
них имеет длину волны 550 нм (зеленый), а другой – 450 нм
(синий). Воспринимаемая яркость этих двух источников света
совершенно различна, и синий свет выглядит на 3.8 % ярче зеленого:
>>> luminos(450 * nm) / luminos(550 * nm)
0.0382

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

10.2.2

Сенсор в цифровой камере

Сенсор в цифровой камере подобен сетчатке, но вместо палочек и колбочек на кремниевом чипе находится упорядоченный
массив светочувствительных фотоячеек (или пикселей). Каждая
фотоячейка имеет площадь порядка 1–10 мкм и выдает сигнал,
пропорциональный интенсивности света, падающего на ее поверхность. ►
В цветной камере фотоячейки прикрыты цветными фильт­
рами, как показано на рис. 10.8, которые пропускают красный,
зеленый или синий цвет на нужные фотоячейки. Спектральный
отклик фильтров является функциональным эквивалентом отклика колбочек M(λ), показанного на рис. 10.7а. Очень распро-

Это фотопический
отклик светоадаптированного глаза
с использованием
колбочковых фоторецепторных клеток.
Адаптированный
к темноте, или скотопический, отклик
с использованием
монохроматических
стержневых фоторецепторных клеток
глаза отличается
и достигает максимума при длине
волны около 510 нм.
Светодиод на инфракрасном пульте
дистанционного
управления в большинстве цифровых
камер выглядит как
источник яркого
света – попробуйте
посветить в камеру мобильного
телефона пультом
от телевизора. Некоторые камеры для
охранных систем
обеспечивают
инфракрасную подсветку сцены для
скрытого наблюдения в ночное время.
Имейте в виду,
что некоторые
камеры оснащены
инфракрасными
фильтрами, чтобы
предотвратить насыщение (засветку)
сенсора инфракрасным излучением.
Обычно фотоячейки
напрямую соответствуют пикселям
в полученном изображении.

Рис. 10.8. Структура фильтра Байера. Серые блоки – это массив светочувствительных
кремниевых фотоячеек, над которыми находится массив красных, зеленых и синих фильтров.
Изобретен Брайсом Байером из Eastman Kodak, патент США № 3971065

Цвет 543
Более дорогие камеры типа «3 CCD»
могут выполнять
независимые измерения для каждого
пикселя, поскольку
свет разделяется
набором призм,
фильтруется и подается на отдельную
мат­ри­цу CCD для
каждого основного
цвета. Файлы необработанных изображений цифровой
камеры (raw-файлы)
содержат фактические значения
фотоячеек после
фильтра Байера.

страненной схемой размещения цветных фильтров является
шаблон Байера – периодический шаблон 2×2, состоящий из двух
зеленых фильтров, одного красного и одного синего.
Каждая фотоячейка являет собой аналог колбочки, чувствительной только к одному цвету. Следствием такого размещения
является невозможность проведения независимых измерений яркости красного, зеленого и синего в каждом пикселе,
но ее можно оценить. Например, количество красного в синечувствительном пикселе получается путем интерполяции соседних пикселей, прошедших фильтрацию красного. Файлы
изображений RAW, получаемых цифровой камерой, содержат
фактические выходные данные фотоячеек, отфильтрованные
по Байеру.
Массивы фильтров 3×3 или 4×4 позволяют создавать множество интересных конструкций камер. Использование более
трех различных цветовых фильтров позволяет создавать мультиспектральные камеры с лучшим цветовым разрешением, ряд
фильтров нейтральной цветовой плотности (серых) приводит
к созданию камеры с расширенным динамическим диапазоном, или эти различные фильтры можно смешивать, чтобы
получить камеру с лучшим динамическим диапазоном и цветовым разрешением. Этот подход называется смешанным набором пикселей.

10.2.3

Измерение цвета

Путь света, попадающего в глаз, показан на рис. 10.9а. Спектр
яркости L(λ) является функцией источника света и коэффициента отражения объекта, как показано в уравнении (10.3). Отклик каждого из трех типов колбочек будет следующим:

(10.4)
где Mr(λ), Mg(λ) и Mb(λ) – спектральные характеристики красных,
зеленых и синих колбочек соответственно, как показано на
рис. 10.7а. Отклик представляет собой трехкомпонентный вектор (ρ, γ, β), который в колориметрии известен как тристимулус
(tristimulus).

Освещенность
Яркость

Отражение

а

Яркость

Отклик

Отклик колбочек

Источник света

Освещенность

544 Глава 10 · Свет и цвет

б

Длина волны (нм)

Рис. 10.9. Принцип тристимулуса: а) путь света от источника к глазу; б) внутри глаза присутствует три фильтра,
и общий выход этих фильтров, области, показанные сплошным цветом, представляют собой значение тристимулуса

Для красного кирпича интегралы соответствуют площадям
областей сплошного цвета на рис. 10.9б. Мы можем вычислить
тристимулус, аппроксимируя интегралы уравнения (10.4) как
сумму площадей прямоугольников с шагом dλ = 1 нм:
>>> np.sum(np.c_[L, L, L] * cones * nm, axis=0)
array([ 16.36,
10.07,
2.822])

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

Отступление 10.8. Экспонометры, освещенность и яркость
Фотографический экспонометр измеряет световой поток в лм/м2, или люксах (лк).
Сила света I точечного источника света представляет собой световой поток на
единицу телесного угла, измеряемый в лм/ср, или канделах (кд). Освещенность Е,
падающая нормально на поверхность, равна
E = 1/d 2 лк,
где d – расстояние между источником и поверхностью. Наружная освещенность
в яркий солнечный день составляет примерно 10 000 лк. Уровни освещения в офисе обычно составляют около 1000 лк, а лунный свет дает освещенность около 0.1 лк.
Яркость или светимость ламбертовой поверхности равна:
Ls = Ei cos θ нт.
Единицей измерения является кд/м2, или нит (нт), где Ei – падающая освещенность под углом θ к нормали к поверхности.

Цвет 545
Отступление 10.9. Цветовая слепота
Цветовая слепота, или дальтонизм, – это неспособность различать некоторые цвета, которые
могут различать другие. Протанопия, дейтеранопия, тританопия обозначают отсутствие колбочек L, M и S соответственно. Более распространенными состояниями являются протаномалия,
дейтераномалия и тританомалия, при которых
мутируют пигменты колбочек и изменяется пиковая частота отклика. Чаще всего это генетическое заболевание, поскольку красный и зеленый
фотопсины закодированы в Х-хромосоме. Наиболее распространенной формой (встречающейся у 6 % мужчин, включая автора) является дейтераномалия, при которой реакция М-колбочек
смещена в сторону красного конца спектра, что
приводит к снижению чувствительности к зеленому цвету и плохому различению оттенков красного, оранжевого, желтого цвета
и зеленой области спектра.
Английский ученый Джон Дальтон (1766–1844) не отличал алый от зеленого,
а розовый от голубого. Он предположил, что стекловидное тело в его глазах было
окрашено в синий цвет, и приказал проверить его глаза после его смерти. Исследование установило, что стекловидное тело было совершенно чистым, но ДНК,
недавно извлеченная из его сохранившегося глаза, показала, что он был дейтеранопом. Цветовую слепоту в обиходе часто называют дальтонизмом.

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

бора представления является то, что один и тот же визуальный
стимул будет производиться многими различными спектрами, которые называются метамерами. Более важным является
следствие – произвольный визуальный стимул может быть сгенерирован комбинацией всего трех монохроматических стимулов. Это три основных цвета, о которых мы знаем с детства.
Не существует уникального набора основных цветов – подойдут любые три, если ни один из них не может быть сопоставлен
комбинацией других. CIE определил набор монохроматических
основных цветов, а их длины волн приведены в табл. 10.2.
Таблица 10.2. Основные цвета CIE 1976 (Commission Internationale
de L’Éclairage 1987) представляют собой спектральные цвета,
соответствующие эмиссионным линиям в парах ртути
λ, нм

10.2.4

красный
700.0

зеленый
546.1

синий
435.8

Воспроизведение цветов

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

546 Глава 10 · Свет и цвет
Отступление 10.10. Что такое «основные цвета»?
Понятие основных цветов очень старо, но их количество (от двух до шести) и их
цвет были предметом многочисленных споров. Большая часть путаницы возникла
из-за использования аддитивных основных цветов (красный, зеленый и синий),
которые используются при смешивании света, и субтрактивных основных цветов (голубой, пурпурный, желтый), используемых при смешивании красок или
чернил. Также обсуждался вопрос о том, являются ли черный и белый основными
цветами.

цветов в каждом пикселе. Основные цвета в электронно-лучевой трубке (cathode ray tube, CRT) создаются путем возбуждения люминофоров на задней стороне экрана с помощью
управляемого электронного луча. В жидкокристаллическом
дисплее (liquid crystal display, LCD) цвета получаются путем
цветофильтрации и ослабления белого света, излучаемого задней подсветкой, а OLED-дисплей содержит набор красных, зеленых и синих светодиодов в каждом пикселе. Важная задача
состоит в том, чтобы определить, какая часть каждого первичного сигнала требуется для соответствия заданному трехкомпонентному сигналу.
Начнем с рассмотрения монохроматического компонента
с длиной волны λS, который определяется как

Реакция колбочек на этот компонент определяется уравнением (10.4), но поскольку L(λ) является импульсом, мы можем
отбросить интеграл и получить три компонента:
ρ = LλMr(λS),
γ = LλMq(λS),
β = LλMb(λS).

(10.5)

Рассмотрим следующие три монохроматических первичных
источника света, обозначенных R, G и B, с длинами волн λr, λg
и λb и интенсивностью R, G и B соответственно. Три цветовых
компонента этого источника света будут следующими:
ρ = RMr(λr) + RMr(λq) + RMr(λb),
γ = RMq(λr) + RMq(λq) + RMq(λb),
β = RMb(λr) + RMb(λq) + RMb(λb).

(10.6)

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

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

Цвет 547

два тристимулуса должны быть равными. Приравняем уравнение (10.5) и уравнение (10.6) и компактно запишем в матричной форме как

а затем решим для необходимого количества основных цветов
(10.7)

Этот тристимулус имеет спектр, состоящий из трех импульсов (по одному на первичный цвет), но выглядит точно так же,
как исходный цвет непрерывного спектра, – это основа три­
хроматического баланса. Матрица 3×3 постоянна, но зависит от
спектрального отклика колбочек на выбранные основные цвета
(λr, λg, λb).
Правая часть уравнения (10.7) – это просто функция от λS, которую можно записать в еще более компактной форме

(10.8)

–
где r– (λ), g–(λ), b (λ) обозначают функции цветового баланса. Эти
функции были эмпирически определены у подопытных людей
и сведены в таблицу. Их можно загрузить с помощью функции
cmfrgb:
>>> cmf = cmfrgb(lmbda);
>>> plt.plot(lmbda / nm, cmf);

Полученный график показан на рис. 10.10а. Каждая кривая
показывает, какая часть соответствующего основного компонента требуется для визуального совпадения с монохроматическим светом с длиной волны λ.
Например, чтобы создать ощущение света с длиной волны
500 нм (зеленого цвета), нам понадобятся следующие компоненты:
>>> green = cmfrgb(500 * nm)
array([-0.07137, 0.08536, 0.04776])

548 Глава 10 · Свет и цвет
Отступление 10.11. Эксперименты по балансу цветов
Эксперименты по балансу цветов выполняются с использованием источника света,
состоящего из трех регулируемых ламп, соответствующих основным цветам, интенсивность которых можно регулировать индивидуально. Свет смешивается, рассеивается и сравнивается с некоторым тестовым цветом. Основные цвета, три лампы,
обозначаются R, G и B, а их интенсивность – R, G и B соответственно. Интенсивности
свечения трех ламп регулируются человеком-оператором до тех пор, пока они не
будут соответствовать тестовому цвету. Совпадение обозначается выражением
C ≡ RR + GG + BB,
которое читается так: визуальный стимул C (тестовый цвет) совпадает или выглядит так же, как смесь трех основных цветов с яркостью R, G и B. Обозначение RR
можно рассматривать как лампу R с интенсивностью R.
Эксперименты показывают, что сопоставление цветов подчиняется алгебраическим правилам аддитивности и линейности, известным как законы Грассмана.
Например, два световых стимула С1 и С2
C1 ≡ R1R + G1G + B1B
C2 ≡ R2R + G2G + B2B
при смешивании будут давать следующий баланс:

Функция цветового баланса

С1 + С2 ≡ (R1 + R2)R + (G1 + G2)G + (B1 + B2)B.

а

Длина волны (нм)

б

Рис. 10.10. а) Функции цветового баланса CIE 1931 г. для стандартного наблюдателя на основе поля зрения 2°;
б) геометрия цветового баланса как функция длины волны, ее проекция на плоскость –
r g– показана красным цветом

Удивительно, но для этого требуется значительное отрица­
тельное количество красного основного цвета, и это проблематично, потому что источник света не может иметь отрицательную яркость.
Проблему можно решить, добавив немного белого света (R =
G = B = w, см. раздел 10.2.8), чтобы все три компонента были положительными. Например:
>>> w = -np.min(green)
0.07137

Цвет 549
>>> feasible_green = green + w
array([
0, 0.1567, 0.1191])

Сравнив этот цвет с желаемым зеленым цветом 500 нм, вы
наверняка скажете, что сгенерированный цвет имеет правильный оттенок, но он не такой насыщенный. Насыщенность обозначает чистоту цвета. Цвета спектра полностью насыщены, но
становятся менее насыщенными (более пастельными) по мере
добавления большего количества белого. В данном случае мы
подмешали компонент светло-серого (7 %) цвета.
Это приводит нас к очень важному моменту в отношении
цветопередачи: невозможно воспроизвести все существующие
цвета, используя только три основных цвета. Это интуитивно
понятно, поскольку натуральный цвет представлен как бесконечномерная спектральная функция, а трехкомпонентный
вектор может только аппроксимировать ее. Для более полного
понимания нам нужно рассмотреть цветовые пространства.
Функция cmfrgb в пакете Toolbox также может вычислять трехкомпонентное представление CIE для произвольного спектра.
Спектр яркости красного кирпича, освещенного солнечным
светом на уровне земли, был рассчитан ранее в разделе 10.1.3,
а трехкомпонентная форма выглядит так:
>>> RGB_brick = cmfrgb(lmbda, L)
array([ 0.01554, 0.006651, 0.003096])

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

10.2.5

Цветовое пространство

Трехкомпонентное представление описывает не только яркость, но и цвет. Например, если удвоить количество красного,
зеленого и синего, мы воспримем результат как примерно тот
же цвет, но отметим, что он получился более ярким.
Относительные трехкомпонентные значения получаются
путем нормализации абсолютных значений
(10.9)
что дает нам координаты цветности r, g и b, инвариантные к общей яркости. По определению r + g + b = 1, поэтому одна координата избыточна, и обычно учитываются только r и g. Поскольку
влияние интенсивности было устранено, двухмерная величина
(r, g) представляет цвет или, точнее, цветность.

550 Глава 10 · Свет и цвет

Мы можем нанести геометрическое место (локус) спектральных цветов – цветов радуги – на диаграмму цветности, используя вариант функций цветового баланса
>>> rg = lambda2rg(np.linspace(400, 700, 100) * nm);
>>> plt.plot(rg[:, 0], rg[:, 1]);

в результате чего получим подковообразную кривую, показанную на рис. 10.11. Это масштабированная версия красной кривой на рис. 10.10б. Основные красные и синие цвета соединены
прямым отрезком, называемым границей фиолетового цвета,
и представляют собой геометрическое место насыщенных фио­
летовых цветов.
Функция lambda2rg в пакете Toolbox вычисляет функцию цветового баланса (рис. 10.10) для заданной длины волны, а затем
преобразует трехцветное значение в цветовые координаты, используя уравнение (10.9). График, представленный на рис. 10.11,
с обозначенными длинами волн, получен следующим образом:
>>> plot_spectral_locus("rg")

Рис. 10.11. Спектральный локус цветности на плоскости rg. Монохроматические стимулы
лежат на локусе, а рядом с ними подписаны длины волн в нм. Все возможные цвета лежат
на этом локусе или внутри него. Стандартные основные цвета CIE отмечены большими
кружками, а желтый треугольник определяет палитру цветов, которые можно представить
этими основными цветами

Цветность основных цветов CIE, перечисленных в табл. 10.2,
представлена в столбцах
>>> primaries = lambda2rg(cie_primaries()).T
array([[
1, 0.001084, -0.0003924],
[
0, 0.9987, 0.0002112]])

Цвет 551

и ее можно отобразить на диаграмме в виде кружков
>>> plot_point(primaries, "o");

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

как показано на рис. 10.11.
Закон центра тяжести Грассмана гласит, что смесь двух цветов лежит вдоль линии между этими двумя цветами на плоскости цветности. Смесь N цветов лежит в области, ограниченной
этими цветами. Если принимать во внимание рис. 10.11, это
имеет важные последствия. Во-первых, поскольку все цветовые стимулы представляют собой комбинации спектральных
стимулов, все реальные цветовые стимулы должны лежать на
спектральном локусе или внутри него. Во-вторых, любые цвета,
которые мы получаем путем смешивания трех основных цветов, могут лежать только внутри треугольника, ограниченного
основными цветами – цветовой палитрой. По рис. 10.11 видно,
что основные цвета CIE определяют лишь небольшое подмножество всех возможных цветов – внутри желтого треугольника.
Очень многие реальные цвета не могут быть созданы с использованием этих основных цветов, в частности цвета радуги, лежащие в спектральном локусе от 460 до 545 нм. На самом деле
независимо от расположения основных цветов не все возможные цвета могут быть воспроизведены. ◄ С точки зрения геометрии внутри гаммы нет трех точек, образующих треугольник,
включающий всю гамму. В-третьих, мы наблюдаем, что большая часть локуса требует отрицательного количества красного
основного цвета и не может быть представлена.

Отступление 10.12. Колориметрические стандарты
Колориметрия – сложная тема, и для нее очень важно иметь стандарты. Ведущую
роль в этой области играют две организации – CIE и ITU.
Международная комиссия по освещению (Commission Internationale de l’Eclai­ra­
ge, CIE) основана в 1913 г. и является независимой некоммерческой организацией,
занимающейся международным сотрудничеством и обменом информацией по всем
вопросам, касающимся науки и искусства света, цвета и зрения, а также технологий
изображения. Восьмая сессия CIE была проведена в Кембридже, Великобритания,
в 1931 г. На ней было принято международное соглашение по колориметрическим
спецификациям и формализовано цветовое пространство XYZ. CIE признана ISO
в качестве международного органа по стандартизации. Более подробную информацию и наборы данных CIE вы найдете на сайте http://www.cie.co.at.
Международный союз электросвязи (International Telecommunication Union, ITU,
МСЭ) является агентством Организации Объединенных Наций и создан для стандартизации и регулирования международной радиосвязи и телекоммуникаций.
Он был основан как Международный телеграфный союз в Париже 17 мая 1865 г.
Международный консультативный комитет по радио, или МККР (Comité Consultatif International des Radiocommunications, CCIR), в 1992 г. стал Бюро радиосвязи,
или ITU-R. Он публикует стандарты и рекомендации, относящиеся к колоримет­
рии, в своей серии BT (broadcasting service of television, вещательная служба телевидения). Более подробную информацию вы найдете на сайте http://www.itu.int.

552 Глава 10 · Свет и цвет

Мы много времени проводим, глядя на экраны, и эта теоретическая невозможность отобразить все реальные цвета вызывает удивление и недоумение. Чтобы проиллюстрировать,
как решается эта проблема, снова рассмотрим задачу из раздела 10.2.4, касающуюся отображения зеленого цвета с длиной волны 500 нм – зеленого, который мы видим в радуге. На
рис. 10.12 показана цветность спектрального зеленого цвета
>>> green_cc = lambda2rg(500 * nm)
array([ -1.156, 1.382])
>>> plot_point(green_cc, "x");

отмеченного на диаграмме крестиком. Белый цвет по определению R = G = B = 1, и его цветность
>>> white_cc = tristim2cc([1, 1, 1])
array([ 0.3333, 0.3333])
>>> plot_point(white_cc, "o");

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

Рис. 10.12. Диаграмма цветности, показывающая цветовую палитру для нестандартных
основных цветов при 600, 555 и 450 нм

Ранее мы говорили, что в палитре нет трех точек, образующих треугольник, включающий всю палитру. Поэтому в 1931 г.
CIE предложила систему воображаемых нефизических основных

Цвет 553
Яркость здесь
имеет другое
значение, чем то,
что определено
в разделе 10.1.3,
и может считаться
синонимом
освещенности.

цветов X, Y и Z, которые полностью охватывают спектральный
локус. X и Z имеют нулевую яркость – она полностью зависит
от Y. Таким образом, всем реальным цветам может соответствовать положительное количество этих трех основных цветов. Соответствующие трехцветные значения обозначаются
как (x, y, z).
Функции цветового баланса XYZ, определенные CIE:

Единицы выбраны
так, что для соот>>> cmf = cmfxyz(lmbda);
ветствия белому
стимулу равной >>> plt.plot(lmbda / nm, cmf);
энергии требуется
равное количество графически представлены на рис. 10.13а. Графики показывают
основных цветов. количество каждого основного цвета CIE XYZ, которое требует-

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

(10.10)
Напомню, что x + y + z = 1, поэтому достаточно двух парамет­
ров – по соглашению диаграмму цветности строят как функцию y от x. Спектральный локус можно построить так же, как
и раньше:
>>> xy = lambda2xy(lmbda);
>>> plt.plot(xy[0], xy[1], "ko");

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

Функция цветового баланса

>>> plot_chromaticity_diagram("xy")

Длина волны (нм)
а
б
Рис. 10.13. а) Функции цветового баланса для стандартного наблюдателя, основанные на воображаемых основных
цветах X, Y (интенсивность) и Z, сведены в таблицу CIE; б) цвета на xy-хроматической плоскости

554 Глава 10 · Свет и цвет

как показано на рис. 10.13б. ► Эти координаты являются стан­
дартным способом представления цвета для графики, печати
и других целей. Например, координаты цветности зеленого
пика (550 нм) равны
>>> lambda2xy(550 * nm)
array([ 0.3016, 0.6923])

а координаты цветности стандартного источника света с вольфрамовой нитью при 2600 К равны
>>> lamp = blackbody(lmbda, 2_600);
>>> lambda2xy(lmbda, lamp)
array([ 0.4677, 0.4127])

10.2.6

Названия цветов

Цветовые координаты обеспечивают количественный способ
описания и сравнения цветов, однако людям удобнее использовать названия цветов. Многие компьютерные операционные
системы содержат базу данных или файл,
сопоставляющий
понятные человеку названия цветов с соответствующими им
трехкомпонентными значениями (R, G, B). В Toolbox используется база данных цветов из Matplotlib, в которой определено
более тысячи цветов. Например, вот как можно получить трехкомпонентное определение оранжевого цвета:

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

В пакете X11
файл называется
rgb.txt. Пакет
Toolbox реализует
эту возможность
с помощью
Matplotlib.

Отступление 10.13. Цвета
Цвета важны для человека, и в английском
языке имеется более 4000 слов, связанных
с цветом. У древних греков были только слова, обозначающие черный, белый, красный
и желтовато-зеленый цвета. Во всех языках
есть слова, обозначающие черный и белый,
и слово «красный» является следующим по
частоте названием цвета в языке, за которым следуют желтый, зеленый, синий и т. д.
Мы также связываем цвета с эмоциями, например красный – гнев, а синий – грусть, но
в разных культурах ассоциации различаются.
В Азии оранжевый обычно является положительным цветом, тогда как на Западе это цвет
бульдозеров и опасностей на дороге. Химия и технологии за последние 700 лет подарили нам широчайший выбор новых красителей, но с этим выбором возникает
путаница в отношении названий цветов – люди не всегда согласны с лингвистическим тегом, который присвоен определенному цвету. (Облако слов от tagxedo.com
с использованием данных Steinvall, 2002.)

Цвет 555
>>> name2color("orange")
array([ 1, 0.6471, 0])

со значениями, приведенными к интервалу [0, 1]. Мы также можем запросить xy-координаты цветности:
>>> bs = name2color("orange", "xy")
array([ 0.4548, 0.4771])

На рис. 10.13б можно заметить, что эта точка находится
в желто-красной части цветового пространства. В Python цвет
также можно задать с помощью регулярного выражения
>>> name2color(".*coral.*")
['xkcd:coral pink', 'xkcd:dark coral', 'xkcd:coral', 'coral',
'lightcoral']

которое вернет соответствующие названия цветов.
Аналогично можно решить обратную задачу: для трехцветного значения
>>> color2name([0.45, 0.48], "xy")
'xkcd:orange yellow'

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

10.2.7

Другие цветовые и хроматические пространства

Цветовое пространство – это трехмерное пространство, содержащее все возможные трехцветные значения – все цвета и все
уровни яркости. С точки зрения систем координат существует бесконечное число вариантов декартовой системы отсчета,
с помощью которых можно определять цвета. Мы уже обсуждали два разных декартовых цветовых пространства: RGB и XYZ.
Однако мы могли бы также использовать полярные, сферические или гибридные системы координат.
Двухмерные пространства цветности rg или xy не учитывают
яркость – мы нормализовали ее в уравнениях (10.9) и (10.10).
Яркость обозначается буквой Y и, согласно определению из Рекомендации МСЭ 709,
Y 709 = 0.2126R + 0.7152G + 0.0722B,

(10.11)

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

556 Глава 10 · Свет и цвет

Людям более свойственно рассматривать цветность с точки
зрения двух характеристик: оттенка и насыщенности. Оттенок
(hue) – это доминирующий цвет, ближайший спектральный
цвет, а насыщенность (saturation) обозначает отсутствие примеси белого. Стимулы на спектральном локусе полностью насыщены, а те, что ближе к его центроиду, менее насыщены. Геометрический смысл понятий оттенка и насыщенности показан
на рис. 10.14.

спектральный цвет

цвет
щ
насы
ь

ост

енн

оттенок

белый
красный

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

Цветовые пространства, которые мы обсуждали, не имеют
простой интерпретации с точки зрения оттенка и насыщенности, поэтому были предложены альтернативные цветовые
пространства. Двумя наиболее известными являются HSV и CIE
L*C*h. В обозначениях цветового пространства H – это оттенок
(hue), S – это насыщенность (saturation), которая также известна
как C, или цветность (chroma). Измерение интенсивности называется либо V (value, величина), либо L (lightness, светлость),
но они вычисляются совершенно по-разному. ►
Для преобразований между различными цветовыми пространствами можно использовать функцию colorspace_convert.
Например, оттенок, насыщенность и интенсивность для каждого из чистых красных, зеленых и синих значений трехкомпонентной системы RGB ► равны
>>> colorspace_convert([1, 0, 0], "RGB", "HSV")
array([ 0, 1, 1])
>>> colorspace_convert([0, 1, 0], "RGB", "HSV")
array([ 120, 1, 1])

L* – нелинейная
функция относительной яркости,
аппроксимирующая
нелинейную реакцию человеческого
глаза. Величина
определяется как
V = 1/2(min(R, G, B) +
max(R, G, B)).
Эта функция
предполагает, что
значения RGB закодированы с помощью гамма-кодирования, как сказано
в разделе 10.3.6.
Конкретные числовые значения,
выбранные здесь,
не зависят от гаммакодирования.

Цвет 557
>>> colorspace_convert([0, 0, 1], "RGB", "HSV")
array([ 240, 1, 1])

В каждом случае насыщенность равна 1, цвета чистые,
а интенсивность равна 1. Как показано на рис. 10.14, оттенок
представлен в виде угла в диапазоне [0, 360)° с красным в 0°,
который увеличивается при проходе по спектральным цветам
в направлении уменьшения длины волны (оранжевый, желтый,
зеленый, синий, фиолетовый).
Если уменьшить количество зеленого компонента:
>>> colorspace_convert([0, 0.5, 0], "RGB", "HSV")
array([ 120, 1, 0.5])
В случае очень то интенсивность упадет, но оттенок и насыщенность остануттемных цветов ся неизменными. Для средне-серого
погрешности вычислений с малыми >>> colorspace_convert([0.4, 0.4, 0.4], "RGB", "HSV")
чиселами приводят
0, 0.4])
к неточным коор- array([ 0,
динатам оттенка
и насыщенности. насыщенность равна нулю, это всего лишь вариант белого, а от-

тенок не имеет значения, так как нет цвета. Если добавить зеленый к серому
>>> colorspace_convert(np.array([0, 0.5, 0])
...
+ np.array([0.4, 0.4, 0.4]), "RGB", "HSV")
array([ 120, 0.5556, 0.9])

то получится зеленый оттенок и среднее значение насыщенности.
Функцию colorspace также можно применить к цветному
изобра­жению (эта тема обсуждается в следующей главе)
>>> flowers = Image.Read("flowers4.png")
Image: 640 x 426 (uint8), R:G:B [.../images/flowers4.png]
Более подробно об
этом рассказывается в следующей
главе.

Это изображение показано на рис. 10.15а. Это фотография разноцветных цветов и зелени на заднем плане. Изображение flo­
wers имеет три измерения, где третье – это цветовая плоскость,
на которой выбраны красные, зеленые или синие пиксели. ◄
Преобразовать изображение в значения оттенка, насыщенности и яркости можно следующим образом:
>>> hsv = flowers.colorspace("HSV")
Image: 640 x 426 (uint8), H:S:V

– и в результате получается еще одна трехмерная мат­ри­ца, но
на этот раз цветовые плоскости представляют оттенок, насыщенность и яркость. Мы можем отобразить эти плоскости
>>> hsv.plane("H").disp();
>>> hsv.plane("S").disp();
>>> hsv.plane("V").disp();

558 Глава 10 · Свет и цвет

в виде изображений, которые показаны на рис. 10.15б,в,г соответственно. На представлении в виде оттенков темный цвет
соответствует красному, а яркий белый – фиолетовому. Красные цветы выглядят как очень маленький угол оттенка (темный) и как очень большой угол, близкий к 360°. Желтые цветы
и зеленый фон можно рассматривать как различные значения
оттенка. Изображение в виде насыщенности показывает, что
красные и желтые цветы имеют высокую насыщенность, а зеленые листья и стебли – менее насыщенные. Белые цветы имеют очень низкую насыщенность, так как по определению белый
цвет содержит много белого и мало цвета.

а

б

в

г

д

е

Рис. 10.15. Пример цветной сцены: а) исходное цветное изображение; б) оттенок изображения; в) насыщенность
изображения. Обратите внимание, что белые цветы имеют низкую насыщенность (они кажутся темными); г) карта
интенсивности или монохромное изображение; д) изображение a* (от зеленого до красного); е) изображение b*
(от синего до желтого)

Недостаток многих цветовых пространств заключается
в том, что воспринимаемая человеческим глазом цветовая разница между двумя точками не связана напрямую с евклидовым расстоянием между ними. В некоторых частях цветового
пространства две удаленные точки могут казаться совершенно
одинаковыми, тогда как в другой области две близкие – совершенно разными. Это привело к развитию перцептивно однородных цветовых пространств, таких как пространства CIE
L*u*v* (CIELUV) и L*a*b*. ►
Функция colorspace может выполнять преобразования между
13 различными цветовыми пространствами, включая L*a*b*,
L*u*v*, YUV и YCBCR. Преобразование этого изображения в цветовое пространство L*a*b* выполняется по тому же принципу:
>>> Lab = flowers.colorspace("L*a*b*")
Image: 640 x 426 (uint8), L*:a*:b*

L*u*v* – это обновленная в 1976 году
версия унифицированного цветового
пространства (UCS)
CIE Luv 1960 года.
L*a*b* – это стандартизация цветового
пространства CIE
1976 года, которое
было определено в Hunter Lab
в 1948 году.

Цвет 559
Относительно что снова дает изображение с тремя измерениями. Цветность
белого источника кодируется в плоскостях a* и b*:
света, который эта
функция рассмат­
>>> Lab.plane("a*").disp();
ривает как CIE D65
с Y = 1, цвета a*b* >>> Lab.plane("b*").disp();
не инвариантны
к общей яркости. и они показаны на рис. 10.15д и е соответственно. L*a*b* – оп-

позитное цветовое пространство, где a* охватывает цвета от зеленого (черного) до красного (белого), а b* – от синего (черного)
до желтого (белого), с белым в начале координат, где a* = b * = 0.

10.2.8

Преобразование между различными
основными цветами

Стандарты CIE были определены в 1931 г., задолго до появления цветного телевидения в 1950-х гг. Основные цвета CIE
в табл. 10.2 основаны на линиях излучения ртутной лампы,
которые хорошо повторяются и подходят для лабораторного
использования. В ранних телевизионных приемниках использовались ЭЛТ-мониторы, где основные цвета генерировались
люминофорами, излучающими свет при бомбардировке элект­
ронами. Используемые люминофоры и их оттенки свечения
менялись со временем в погоне за более яркими дисплеями.
Международное соглашение, рекомендация МСЭ 709, определяет основные частоты спектра для телевидения высокой четкости (ТВЧ), и они перечислены в табл. 10.3.
Таблица 10.3. xyz-цветность стандартных основных и белых цветов.
Основные цвета CIE из табл. 10.2 и более поздняя рекомендация ITU 709,
определенные для HDTV. D65 – это белый цвет излучателя абсолютно
черного тела при температуре 6500 К, а E – белый цвет равной энергии
x
y
z

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

RCIE
0.7347
0.2653
0.0000

GCIE
0.2738
0.7174
0.0088

BCIE
0.1666
0.0089
0.8245

R709
0.640
0.330
0.030

G709
0.300
0.600
0.100

B709
0.150
0.060
0.790

D65
0.3127
0.3290
0.3582

E
0.3333
0.3333
0.3333

Отсюда вытекает задача преобразования трехкомпонентных значений из одного набора основных цветов в другой. Допустим, мы хотим отобразить изображение, представленное
в трехкомпонентной схеме CIE, на экране, соответствующем
стандарту ITU 709. Используя обозначения, введенные ранее,
определим два набора основных цветов: P1, P2, P3 с трехкомпонентными значениями (S1, S2, S3) и P¢1, P¢2, P¢3 с трехкомпонентными значениями (S¢1, S¢2, S¢3). Мы всегда можем выразить один
набор основных цветов как линейную комбинацию других:

560 Глава 10 · Свет и цвет

(10.12)
И поскольку два трехкомпонентных цвета совпадают, то мы
можем записать:
(10.13)
Подставляя (10.12), приравнивая цветовые тройки и транспонируя, получаем
(10.14)
– просто линейное преобразование трехцветных значений.
Рассмотрим конкретную задачу преобразования основных
цветов CIE в трехкомпонентные значения XYZ. Из табл. 10.3 мы
знаем соответствие между основными цветами CIE и основными цветами XYZ:
>>> C = np.array([[0.7347, 0.2738, 0.1666],
...
[0.2653, 0.7174, 0.0088],
...
[0,
0.0089, 0.8245]]);

что соответствует первым трем столбцам табл. 10.3. Таким образом, мы приходим к преобразованию

Напомню, что яркость полностью зависит от основного цвета Y. Обычно применяется ограничение, согласно которому
единичные значения R, G, B дают единичную яркость Y и белый
цвет с заданной цветностью. Мы выберем белый цвет D65, цветность которого указана в табл. 10.3 и который будем обозначать
как (x w, y w, z w). Теперь можно записать уравнение

Цвет 561

где левая часть имеет Y = 1. Кроме того, мы ввели диагональную
мат­ри­цу J, которая масштабирует яркость основных цветов. Мы
можем решить уравнение и найти элементы J:

Подставляя реальные значения, получаем:
>>> white = np.array([0.3127, 0.3290, 0.3582]);
>>> J = np.linalg.inv(C) @ white / white[1]
array([ 0.5609,
1.17,
1.308])
>>> C @ np.diag(J)
array([[ 0.4121, 0.3205, 0.2179],
[ 0.1488, 0.8397, 0.01151],
[
0, 0.01042,
1.078]])

Средняя строка этой мат­ри­цы представляет собой коэффициенты яркости
Y = 0.1488R + 0.8395G + 0.0116B,
аналогично уравнению (10.11). Небольшое отклонение связано
с использованием разных основных цветов – в данном случае
CIE вместо Rec. 709 в случае уравнения (10.11).
Трехкомпонентное значение RGB красного кирпича было
вычислено ранее, и мы можем определить его тройку компонентов XYZ:
>>> XYZ_brick = (C @ np.diag(J) @ RGB_brick).T
array([0.009212, 0.007933, 0.003408])

которую преобразуем в координаты цветности согласно уравнению (10.10):
>>> tri = tristim2cc(XYZ_brick)
array([ 0.4482,
0.386])

Возвращаясь к рис. 3.13б, мы видим, что эта ху-цветность лежит в красной области и называется
>>> color2name(tri, "xy")
'xkcd:red brown'

red brown (красно-коричневый), что вполне соответствует объекту «выветренный красный кирпич».

562 Глава 10 · Свет и цвет

10.2.9

Что такое белый цвет?

В предыдущем разделе мы коснулись темы белого цвета. Он необычен тем, что это одновременно и отсутствие цвета, и сумма всех цветов. Одним из определений белого цвета является
стандартный дневной свет, который наблюдается от полуденного солнца в Западной/Северной Европе и занесен в таблицу
CIE как источник света D65. Он может быть близко аппроксимирован излучением абсолютно черного тела при температуре
6500 K
>>> d65 = blackbody(lmbda, 6_500);
>>> lambda2xy(lmbda, d65)
array([ 0.3136, 0.3243])

которое, как мы видим, близко к цветности D65, приведенной
в табл. 10.3.
Другое определение основано на том, что белый свет представляет собой равную смесь всех спектральных цветов. Это
представление однородного спектра
>>> ee = np.ones(lmbda.shape);

известное также как стимул равной энергии, имеющее цветность
>>> lambda2xy(lmbda, ee)
array([ 0.3334, 0.334])

что близко к определенному значению (1/3, 1/3).

10.3

Дополнительные темы

Цвет – это обширная и сложная тема, и в этом разделе мы кратко
раскроем несколько оставшихся важных аспектов. Распространенным способом описания спектра источника света является
цветовая температура, а влияние цвета освещения на видимый
цвет объекта представляет собой проблему постоянства цвета,
которая очень актуальна для робота, использующего цветовые
сигналы в среде с естественным освещением. Одним из способов решить эту проблему является корректировка баланса белого. Другим источником изменения цвета в таких средах, как
вода, является поглощение определенных длин волн. Большинство камер на самом деле реализуют нелинейную зависимость,
называемую гамма-коррекцией, между фактической яркостью
сцены и выходными значениями трехцветного изображения.
Наконец, мы рассмотрим более реалистичную модель отраже-

Дополнительные темы 563

ния света от поверхности, которая имеет как зеркальную, так
и диффузную составляющую, у каждой из которых свои спект­
ральные характеристики.

10.3.1

Цветовая температура

Фотографы часто упоминают цветовую температуру источника света – температуру черного тела, спектр которого, согласно
уравнению (10.1), наиболее похож на источник света. Цветовая
температура для ряда распространенных условий освещения
указана в табл. 10.4. Освещение с низкой цветовой температурой мы описываем как теплое – оно кажется нам красно-оранжевым. Свет с высокой цветовой температурой более жесткий –
он кажется нам ярко-белым, возможно, с оттенком синего.
Таблица 10.4. Цветовые температуры некоторых распространенных
источников света
Источник света
Свеча
Рассветное/закатное небо
Обычная лампа накаливания 40 Вт
Обычная лампа накаливания 100 В
Галогенная лампа 100 Вт
Прямой солнечный свет
Пасмурное небо
Стандартный дневной свет (солнце + голубое небо)
Белесоватое небо
Чистое голубое небо

10.3.2
Мы адаптируем
наше восприятие
цвета таким образом, чтобы интеграл,
или среднее, по
всей сцене был серым. Этот механизм
хорошо работает
в диапазоне цветовой температуры
5000–6500 K.

Цветовая температура, К
1900
2000
2600
2850
3200
5800
6000–7000
6500
8000
10 000–30 000

Постоянство цвета

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

564 Глава 10 · Свет и цвет
Отступление 10.14. Яркость сцены
Яркость сцены является произведением освещенности и отражательной способности, но ключ
к пониманию сцены – отражательная способность,
поскольку ее можно использовать в качестве косвенного показателя типа материала. Освещенность может различаться по интенсивности и цвету в сцене, что усложняет понимание изображения.
К сожалению, разделение яркости на освещенность и отражательную способность является некорректной постановкой задачи, но человеческий
мозг делает это весьма неплохо, хотя и не всегда правильно, как иллюстрирует иллюзия справа – квадраты, обозначенные A и B, имеют одинаковый уровень серого.
Американский изобретатель и основатель корпорации Polaroid Эдвард Лэнд
(1909–1991) предложил теорию ретинекса (retinex = retina (сетчатка) + cortex
(кора)), чтобы объяснить, как зрительная система человека отделяет отражение
от яркости. (Изображение с иллюзией тени на шахматной доске предоставлено
Эдвардом Адельсоном, http://persci.mit.edu/gallery.)

яркость и видимый цвет объекта. Для иллюстрации этой проб­
лемы вернемся к красному кирпичу
>>> lmbda = np.arange(400, 701) * nm;
>>> R = loadspectrum(lmbda, "redbrick");

при двух разных условиях освещения Солнцем на уровне земли
>>> sun = loadspectrum(lmbda, "solar");

и лампой накаливания
>>> lamp = blackbody(lmbda, 2_600);

и вычислим xy-цветность для каждого случая:
>>> xy_sun = lambda2xy(lmbda, sun * R)
array([ 0.4764, 0.3787])
>>> xy_lamp = lambda2xy(lmbda, lamp * R)
array([ 0.5721, 0.3879])

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

10.3.3

Баланс белого

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

Дополнительные темы 565

Солнце
Лампа накаливания
Под водой

Рис. 10.16. Цветность красного кирпича при разном освещении

синий фильтр, чтобы ослабить красную часть спектра и компенсировать смещение баланса белого. Мы можем получить
аналогичную функцию, выбрав мат­ри­цу J

Обычно принимают JG = 1, а JR и JB
корректируют.

для регулировки усиления цветовых каналов. Например, усиление JB компенсирует недостаток синего цвета при освещении
вольфрамовой нитью накала. Это процесс балансировки белого – обеспечение совпадения с цветностью объектов, которые,
как нам известно, являются белыми (или серыми).
Некоторые камеры позволяют пользователю устанавливать
цветовую температуру освещения через меню, как правило,
с заранее настроенными параметрами для ламп накаливания,
люминесцентных ламп, дневного света и вспышки, которые
заданы различными предустановленными значениями J. При
ручной балансировке белого камеру направляют на серый или
белый объект и нажимают кнопку запуска настройки. Камера
регулирует усиление своего канала J так, чтобы были получены
равные значения трехцветного изображения R¢ = G¢ = B¢, что, как
мы помним, дает желаемую цветность белого. Для цветов, отличных от белого, эти исправления вносят некоторое цветовое
искажение, но тем не менее изображение выглядит приемлемо
для человеческого глаза. Часто применяется автоматическая
балансировка белого, основанная на эвристических методах
оценки цветовой температуры источника света, но ее можно
обмануть сценами с преобладанием определенного цвета.

566 Глава 10 · Свет и цвет

Наиболее практичным решением является использование
трехцветных значений трех объектов с известной цветностью
в сцене. Это позволяет напрямую вычислить мат­ри­цу C в уравнении (10.14), отображая значения трехцветного стимула от
датчика в координаты XYZ, которые являются абсолютным (независимым от освещения) представлением отражательной способности поверхности. Отсюда можно также оценить цветность
освещения. Этот подход используется в панорамной камере
марсохода, где калибровочная цель, показанная на рис. 10.17,
периодически появляется перед камерой для обновления баланса белого при изменении марсианского освещения.

Рис. 10.17. Калибровочная мишень, используемая для камеры PanCam марсохода. Области
известного коэффициента отражения и цветности (красный, желтый, зеленый, синий
и оттенки серого) используются для установки баланса белого камеры. Центральный
стержень имеет очень низкую отражательную способность и также служит солнечными
часами. В лучших традициях солнечных часов на них нанесен девиз «Два мира – одно
солнце» (фото предоставлено NASA/JPL/Cornell/Jim Bell)

10.3.4

Изменение цвета из-за поглощения

Последний и весьма экстремальный пример – искажение цвета,
возникающее под водой. Допустим, наш робот пытается найти
док-станцию, обозначенную цветными мишенями. Как упоминалось в разделе 10.1.1, вода действует как фильтр, который
поглощает красный свет сильнее, чем синий. В случае объекта,
находящегося под водой, эта фильтрация влияет как на освещение, падающее на объект, так и на отраженный свет по пути
к камере. Рассмотрим снова красный кирпич
>>> lmbda = np.arange(400, 701) * nm;
>>> R = loadspectrum(lmbda, "redbrick");

Дополнительные темы 567

который погружен на 1 м под воду, а камера расположена на
расстоянии 1 м от кирпича. Освещенность поверхности воды
такая же, как у солнечного света на уровне земли:
>>> sun = loadspectrum(lmbda, "solar");

Спектр поглощения воды
>>> A = loadspectrum(lmbda, "water");

а полная длина оптического пути через воду равна
>>> d = 2;

Передача T определяется законом Бера (10.2)
>>> T = 10.0 ** (-d * A);

и результирующая яркость кирпича будет равна
>>> L = sun * R * T;

что показано на рис. 10.18. Мы видим, что более длинные волны, красные, сильно ослаблены. Видимый цвет кирпича:
>>> xy_water = lambda2xy(lmbda, L)
array([ 0.3742, 0.3823])

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

Яркость L(λ)

Под водой
На воздухе

Длина волны (нм)

Рис. 10.18. Спектр яркости красного кирпича, оказавшегося под водой.
Спектр без поглощения водой показан красным

568 Глава 10 · Свет и цвет
Отступление 10.15. Ламбертово отражение
Незеркальная, или матовая, поверхность является диффузным отражателем, и количест­
во света, отраженного под определенным
углом от нормали к поверхности, пропорцио­
нально косинусу угла отражения θr. Это явление называется ламбертовым отражением
в честь швейцарского математика и физика
Иоган­на Генриха Ламберта (1728–1777). Следствием данного явления является то, что объект имеет одинаковую видимую яркость при
всех углах обзора. Ярким примером этого является луна, которая выглядит как диск равномерной яркости, несмотря на то что это сфера
с изогнутой в сторону от нас поверхностью;
см. также зеркальное отражение в разделе 13.10.

10.3.5

Двухцветное отражение

Простая модель отражения, представленная в разделе 10.1.3,
подходит для объектов с матовой поверхностью (например,
бумага, необработанное дерево), но если поверхность частично блестящая, то свет, отраженный от объекта, будет иметь две
составляющие. Это модель дихроматического отражения, показанная на рис. 10.19а. Одним компонентом является свет источника, зеркально отраженный от поверхности без изменения
спектра, – это граничное, или френелевское, отражение. Другой
компонент – это свет, который взаимодействует с поверхностью:
проникает, рассеивается, подвергается селективному спектральному поглощению и переизлучается во всех направлениях в соответствии с ламбертовской моделью отражения. Соотношение
этих двух компонентов зависит от материала и геометрии источника света, положения наблюдателя и нормали к поверхности.
Хороший пример двухцветного отражения можно увидеть на
рис. 10.19б. Оба помидора выглядят красными из-за рассеивания света, когда свет взаимодействует с поверхностью плода.
Однако у каждого фрукта есть область зеркального отражения,
которая кажется белой, т. е. имеет цвет источника света, а не
поверхности фрукта.
Реальный мир еще более сложен из-за взаимоотражений.
Например, зеленый свет, отраженный от листьев, попадет на
красные плоды и рассеется. Часть этого света будет снова отражаться от зеленых листьев и т. д. – близлежащие объекты
сложным образом влияют на цвет друг друга. Для достижения
фотореалистичных результатов в компьютерной графике все
эти эффекты необходимо моделировать на основе детального

Дополнительные темы 569

нормаль
к поверхности
ие

па

ламбертово
отражение

да

ющ

ие

чи

т

ео

но

ь
ал
рк

лу

н
же

ра

зе

широкий
отраженный луч

а

внутреннее рассеяние

б

Рис. 10.19. Двухцветное отражение: а) часть падающего света подвергается зеркальному отражению от
поверхности (белая стрелка), в то время как другая часть проникающего через нее света рассеивается, фильтруется
и переизлучается во всех направлениях в соответствии с ламбертовской моделью отражения (красная стрелка);
б) отражение от зеркальной поверхности отчетливо видно в некрасных светлых областях на двух помидорах,
это отражения потолочных светильников (снимок любезно предоставлен проектом Distributed Robot Garden,
Массачусетский технологический институт)

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

10.3.6
У некоторых камер
есть возможность
выбрать гамму 1
или 0.45 (= 1/2.2).
Гамма-кодирование
и декодирование
часто называют
гамма-компрессией
и гамма-декомпрессией соответственно, поскольку операция кодирования
сжимает диапазон
сигнала, а декодирования – распаковывает его.

Гамма

Когда-то ЭЛТ-мониторы были широко распространены, яркость, создаваемая на экране дисплея, была нелинейно связана
с управляющим напряжением V в соответствии с формулой
L = V γ,

(10.15)

где γ ≈ 2.2. Чтобы устранить эффект нелинейности, первые видеокамеры применяли обратную нелинейность V = L1/γ к выходному сигналу, в результате чего видеоканал был линейным от
начала до конца. Оба преобразования обычно называют гам­
ма-коррекцией, хотя более точно операция на стороне камеры
представляет собой гамма-кодирование, а операция на стороне
дисплея – гамма-декодирование.
ЖК-дисплеи имеют более сильную нелинейность, чем ЭЛТ,
но в дисплее применяется коррекция, чтобы он соответствовал
стандартному поведению γ = 2.2 устаревшей ЭЛТ.
Чтобы показать эффект гаммы дисплея, создадим простой
тестовый код:

Компьютеры
Macintosh являются
исключением, и до
MacOS 10.6 использовалось значение
γ = 1.8, что делало
цвета более яркими
и сочными. >>> wedge = np.linspace(0, 1, 11).reshape(1,-1);

>>> Image(wedge).disp();

570 Глава 10 · Свет и цвет

Результат показан на рис. 10.20 и похож на градационный
оптический клин, который применяют фотографы. На экране компьютера он будет выглядеть иначе, чем напечатанный
в книге. Скорее всего, будет наблюдаться большое изменение
яркости между вторым и третьим блоками – эффект нелинейности гамма-декодирования (10.15) на дисплее компьютера.

Рис. 10.20. Клин линейной интенсивности

Если применить гамма-кодирование
>>> Image(wedge ** (1 / 2.2)).disp();

изменения интенсивности будут выглядеть более линейными
и ближе к тому, что напечатано в книге.
Цветовые пространства и гамма
Координаты цветности уравнений (10.9) и (10.10) вычисляются как
отношения трехкомпонентных значений, которые линейно связаны с яркостью сцены. Нелинейность, применяемая к выходу камеры, должна быть скорректирована путем гамма-декодирования
перед любыми колориметрическими операциями. В пакете Toolbox
эту операцию выполняет функция gamma_decode. Гамма-декодирование также может быть выполнено, когда изображение загружается
с использованием параметра gamma функции Image.Read().

В настоящее время большинство цифровых камер ► кодируют изображения в формате sRGB (стандарт IEC 61966-2-1), в котором используются базовые цвета ITU 709 и функция гаммакодирования
(10.16)
которая применяет линейную функцию для малых значений
и степенной закон для больших. Общая гамма составляет примерно 2.2.
Цветовые пространства видео: YUV и YCBCR
Важным свойством цветовых пространств, таких как HSV или xyY,
является инвариантность координат цветности к изменениям интенсивности. Многие цифровые видеокамеры обеспечивают вывод
в формате YUV или YCBCR, имеющий компонент яркости Y и два
других компонента цветности, которые являются цветоразностными сигналами – такими, что U, CB ∝ B¢ − Y¢ и V, CR ∝ R¢ − Y¢, где R¢,

Заголовок файла
JPEG (формат файла
JFIF) имеет тег Color
Space, в котором
можно установить одно из двух
значений: sRGB или
Uncalibrated (не
калибровано), если
гамма или цветовая
модель неизвестна
(см. окончание раздела 11.1.1).

Применение: цветное изображение 571
B¢ – гамма-кодированные трехкомпонентные значения, а Y¢ – гамма-кодированная интенсивность. Гамма-нелинейность означает,
что UV или CBCR не будут постоянными при изменении общего
уровня освещения.
Трехкомпонентные значения, поступающие с камеры, необходимо
сначала преобразовать в линейные трехкомпонентные значения,
применив соответствующее гамма-декодирование, а затем вычислив цветность. Простого пути нет.

10.4

Применение: цветное изображение

10.4.1

Сравнение цветовых пространств

В этом разделе мы объединим многие концепции и инструменты, представленные в данной главе, и сравним координаты
цветности цветных квадратов (первые три ряда) диаграммы
GretagMacbeth ColorChecker, показанной на рис. 10.21, используя цветовые пространства xy и L*a*b*. Для начала вычислим
цветность, используя информацию о спектральном коэффициенте отражения для каждого квадрата, полученную с помощью
Toolbox:
>>> lmbda = np.linspace(400, 701, 100) * nm;
>>> macbeth = loadspectrum(lmbda, "macbeth");

Рис. 10.21. Плашка ColorChecker® фирмы Gretag Macbeth представляет собой набор
из 24 печатных цветных квадратов (пронумерованных слева направо и сверху вниз), которые
включают различные оттенки серого и цвета, а также спектральные модели кожи, неба,
листвы и т. д. Спектральные данные квадратов включены в пакет Toolbox

Всего будет 24 столбца, по одному на квадрат тестовой диаграммы. Загружаем спектр относительной мощности стандартного белого источника света D65:

572 Глава 10 · Свет и цвет
>>> d65 = loadspectrum(lmbda, "D65") * 3e9;

и масштабируем его до яркости, сравнимой с солнечным светом, как показано на рис. 10.3а. Тогда для каждого несерого
квад­рата с 0-го по 17-й
>>> XYZ = np.empty((18, 3));
>>> Lab = np.empty((18, 3));
>>> for i in range(18):
... L = macbeth[:,i] * d65;
... RGB = np.maximum(cmfrgb(lmbda, L), 0);
... XYZ[i,:] = colorspace_convert(RGB, "rgb", "xyz");
... Lab[i,:] = colorspace_convert(RGB, "rgb", "L*a*b*");

вычисляем спектр яркости (строка 4), используем функции балансировки цветов CIE для определения трехкомпонентного
отклика глаза и устанавливаем пределы гаммы (строка 5). Полученный спектр преобразуется в цветовое пространство XYZ
(строка 6) и цветовое пространство L*a*b* (строка 7). Далее преобразуем XYZ в цветовые координаты и извлечем значения a*b*:
>>> xy = tristim2cc(XYZ);
>>> ab = Lab[:, 1:];
>>> xy.shape, ab.shape
((18, 2), (18, 2))

В результате получаются две мат­ри­цы 18×2 с одной строкой
на цветной квадрат. Наконец, мы наносим эти точки на соответствующие цветные плоскости и получаем результаты, показанные на рис. 10.22. На полученных диаграммах видно, например, что квадрат 15 ближе к 9 и дальше от 7 на плоскости
a*b*. Цветовое пространство L*a*b* было разработано таким
образом, чтобы евклидово расстояние между точками было
пропорционально разности цветов, воспринимаемой людьми.
Если мы используем алгоритмы для различения объектов по
цвету, тогда L*a*b* будет предпочтительнее, чем RGB или XYZ.

10.4.2

Удаление теней

Как показано на рис. 10.23а, для системы технического зрения робота, работающего вне помещения, тени представляют
серьезную проблему. Из-за теней поверхности одного и того
же типа выглядят совершенно по-разному, и роботу, который
пытается использовать зрение, чтобы понять сцену и спланировать, куда двигаться, это доставляет большие проблемы. Еще
более проблематичным является то, что этот эффект не является постоянным, он меняется в зависимости от времени суток
и состояния облачности. На рис. 10.23б были удалены эффекты

Применение: цветное изображение 573

а

б

v (пиксели)

в
г
Рис. 10.22. Диаграммы цветности плашки Color Checker: а) ху-пространство; б) увеличенный участок xyпространства; в) a*b*-пространство; г) увеличенный участок a*b*-пространства

u (пиксели)
а
б
Рис. 10.23. Тени создают смешанные эффекты в изображениях: а) вид на парк с сильными тенями; б) изображение
без теней, в котором переменчивость освещения почти полностью устранена (Corke et al. 2013)

574 Глава 10 · Свет и цвет

затенения, и теперь четко видны различные типы местности –
трава и гравий.
Для правильного удаления теней важно понимать, что яркие
части сцены освещаются непосредственно солнцем, а более затененные области – небом. И солнце, и небо можно смоделировать как излучатели абсолютно черного тела с цветовыми температурами, указанными в табл. 10.4. Таким образом, у теней
есть две определяющие характеристики: они темные и имеют
легкий голубой оттенок.
Мы моделируем камеру, используя уравнение (10.4), но спект­
ральную характеристику цветовых датчиков камеры смоделируем как функции Дирака M(λ) = δ(λ − λx), что позволяет исключить интегралы:
R = E(λR)R(λR)MR(λR);
G = E(λG)R(λG)MG(λG);
B = E(λB)R(λB)MB(λB).
Для каждого пикселя мы вычисляем цветовые координаты
r = R/G и b = B/G, которые инвариантны к изменению величины
освещенности:

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

Это функция цветовой температуры T и различных констант:
физических констант c, h и k; длины волны отклика датчика λx
и величины Mx(λx), а также свойств материала R(λx). Логарифмируя, получаем очень простую форму:
(10.17)
И, повторяя процесс для синей цветности, мы можем написать:
(10.18)

Применение: цветное изображение 575

Каждому цветовому пикселю (R, G, B) ∈ ℝ3 можно сопоставить
точку (log r, log b) ∈ ℝ2, и при изменении цветовой температуры
все точки будут двигаться вдоль линий с наклоном c¢2 /c2. Следовательно, проекция на ортогональное направление, линия с наклоном c2 /c¢2, дает одномерную величину
s = -c2 log r + c¢2 log b,
которая не зависит от цветовой температуры источника света.
Эту величину можно вычислить для каждого пикселя изображения:
>>> im = Image.Read("parks.png", gamma="sRGB", dtype="float")
Image: 3264 x 2448 (float32), R:G:B [.../images/parks.png]
>>> s = shadow_invariant(im.image, 0.7);
>>> Image(s).disp(interpolation="none", badcolor="red");

где второй аргумент в вызове shadow_invariant – это наклон прямой в радианах, а первый – изображение. Функция принимает
и возвращает изображение в виде массива NumPy. Результат
показан на рис. 10.23б. Пиксели имеют значения шкалы серого,
которые представляют собой сложную функцию отражательной
способности материала и свойств сенсора камеры. Аргументы
метода disp помогают выделить мелкие детали изображения
и отметить красным цветом пиксели, при вычислении цветности которых произошла ошибка деления на ноль.
Для достижения этого результата мы сделали некоторые приближения и ряд достаточно сильных допущений: камера дает
линейный отклик на яркость сцены в трехцветных значениях RGB, цветовые каналы камеры имеют неперекрывающийся
спектральный отклик, сцена освещена светом источника абсолютно черного тела. Первое предположение означает, что нам
нужно использовать камеру с γ = 1 или применить гамма-декодирование к изображению, прежде чем продолжить. Второе далеко не так, особенно для красного и зеленого каналов цветной
камеры, но на практике метод работает хорошо. Самый большой
эффект заключается в том, что точки перемещаются по линии
с наклоном, отличным от c¢2 /c2, но мы можем оценить наклон эмпирически, рассмотрев набор затененных и незатененных пикселей, соответствующих одному и тому же материа­лу в сцене
>>> theta = esttheta(im)

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

576 Глава 10 · Свет и цвет

10.5

Подведение итогов

Вы узнали, что видимый свет представляет собой электромагнитное излучение со смесью длин волн, с непрерывным
спектром, который изменяется за счет отражения и поглощения. Спектр вызывает отклик глаза, который мы интерпретируем как цвет. Для человека этот отклик представляет собой
трехмерный вектор, представляющий собой выходные сигналы трех различных типов колбочек в нашем глазу. Цифровая
цветная камера функционально эквивалентна глазу. Трехкомпонентный цвет можно рассматривать как одномерную координату яркости и двухмерную координату цветности, что позволяет отображать цвета на плоскости. Спектральные цвета
образуют локус на этой плоскости, и все реальные цвета лежат
внутри этого локуса. Любые три основных цвета образуют на
этой плоскости треугольник, который является палитрой этих
основных цветов. Любой цвет внутри треугольника может быть
сопоставлен соответствующей смесью этих основных цветов.
Никакой набор основных цветов не может определить палит­
ру, содержащую все цвета. Альтернативный набор воображаемых основных цветов, система CIE XYZ, содержит все реальОтступление 10.16. За пределами человеческого зрения
Бытовые камеры функционально эквивалентны человеческому глазу и чувствительны к видимому спектру. Камеры с более сложным устройством не имеют этого
ограничения.
Инфракрасные камеры
чувствительны к инфракрасному излучению, и CIE определяет несколько инфракрасных диапазонов:
 IR-A (700–1400 нм), ближний инфракрасный (near infrared, NIR);
 IR-B (1400–3000 нм), коротковолновый инфракрасный (short-wavelength

infrared, SWIR);
 IR-С (3000 нм – 1000 мкм), который делится на поддиапазоны:
 3000–8000 нм, средневолновый (medium-wavelength, MWIR);
 8000–15 000 нм, длинноволновый (long-wavelength, LWIR). Камеры LWIR также
называют тепловизионными, или термографическими.
Ультрафиолетовые камеры
обычно работают в ближнем ультрафиолетовом диапазоне (NUV, 200–380 нм)
и используются в промышленных приложениях, таких как обнаружение коронного разряда в высоковольтных элект­рических системах.
Гиперспектральные камеры
имеют более трех классов фоторецепторов, они измеряют входящий спектр во
многих точках, обычно от инфракрасного до ульт­рафиолетового, и с десятками
или даже сотнями спектральных диапазонов. Гиперспектральные камеры используются для таких приложений, как аэрофотосъемка, классификация землепользования и определение минерального состава горных пород.

Подведение итогов 577

ные цвета и является стандартным способом описания цветов.
К трехкомпонентным значениям можно применить линейные
преобразования для перехода к другим наборам основных
цветов. Нелинейные преобразования можно использовать для
описания трехкомпонентных значений с точки зрения качеств,
ориентированных на человеческое восприятие, таких как оттенок и насыщенность. Мы также обсудили определение белого цвета, цветовую температуру, постоянство цвета, проблему
балансировки белого, нелинейный отклик устройств отображения и то, как это влияет на обычное представление изображений и видео.
Мы узнали, что воспринимаемые нами цвета и яркость зависят от источника света и свойств поверхности объекта. Несмот­
ря на то что люди вполне способны «учитывать» изменения
освещения, это остается серьезной проблемой для роботизированных систем технического зрения. Глава заканчивается примером удаления тени на цветном изображении вне помещения.

10.5.1

Дополнительное чтение

На первый взгляд, цвет – это простое понятие, которое мы изучаем в детском саду, но по мере углубления обнаруживается, что
это увлекательная и сложная тема, о которой написано огромное количество литературы. В этой главе мы только начали поверхностное изучение фотометрии и колориметрии. Фотомет­
рия – это часть науки о радиометрии, связанная с измерением
видимого света. Это сложная задача для инженеров и программистов, поскольку в ней используются необычные единицы
измерения, такие как люмен, стерадиан, нит, кандела и люкс.
Одним из источников сложности является то, что такие слова,
как «интенсивность» и «яркость» (а иногда и «освещенность»),
являются синонимами в повседневной речи, но имеют очень
специфическое значение в фотометрии. Колориметрия – это
наука о восприятии цвета, а также большая и сложная область,
поскольку человеческое восприятие цвета зависит от индивидуального наблюдателя, окружающего освещения и даже поля
зрения. Однако колориметрия имеет решающее значение при
проектировании камер, компьютерных дисплеев, видеооборудования и принтеров. Всеобъемлющая онлайн-информация
о компьютерном зрении доступна по адресу http://home­pages.
inf.ed.ac.uk/rbf/CVonline, а материал этой главы посвящен разделу «Физика изображения».
В учебниках по компьютерному зрению Gonzalez and Woods
(2018) и Forsyth and Ponce (2012) обсуждаются цвет и цветовые
пространства. В последнем также обсуждаются эффекты затенения и взаимоотражения. Книга Gevers et al. (2012) пред-

578 Глава 10 · Свет и цвет

ставляет собой серьезное введение в теорию цветового зрения
и подробно описывает модель дихроматического отражения.
Она также охватывает алгоритмы компьютерного зрения, которые решают проблемы постоянства цвета. Теория ретинекса описана в работе Land and McCann (1971). Другие ресурсы,
связанные с постоянством цветовосприятия, можно найти на
http://colorconstancy.com.
К легко читаемым и исчерпывающим книгам по науке о цвете относятся учебники Berns (2019), Koenderink (2010), Hunt
(1987) и Benson (1986), последний из которых раскрывает тему
с телевизионной или инженерной точки зрения. Hunter and
Harold (1987) используют более доступный язык и охватывают
также другие аспекты внешнего вида, такие как блеск и глянец.
Стандарт CIE (Commission Internationale de l’Éclairage 1987)
является истиной в последней инстанции, но, как и любой стандарт, весьма труден для чтения. Работа CIE продолжается, и его
стандарты периодически обновляются на сайте https://www.
cie.co.at/. Функции цветового баланса были впервые представлены в виде таблицы в 1931 г. и пересмотрены в 1964 г.

Общее развитие
В Crone (1999) рассматривается история теорий человеческого
зрения и цвета. Как работает зрительная система человека, от
глаза до восприятия, описано в двух очень легко читаемых книгах: Stone (2012) и Gregory (1997). В Land and Nilsson (2002) описывается устройство глаз животных и оптимизация для разных
видов таких характеристик, как острота зрения, поле зрения
и способность работать в условиях низкой освещенности.

10.5.2

Источники данных

Toolbox содержит ряд файлов данных, описывающих различные спектры, которые сведены в табл. 10.5. Каждый файл содержит в первом столбце длину волны в метрах. Файлы имеют
разные диапазоны длин волн и интервалы, но вспомогательная
функция loadspectrum интерполирует данные в указанный пользователем диапазон и интервал выборки.
Несколько интернет-сайтов содержат спектральные данные
в табличном формате, ссылка на которые есть на веб-сайте
книги. Сюда входят данные об отражательной способности для
более чем 2000 материалов, предоставленных спектральной
онлайн-библиотекой НАСА ASTER 2.0 (Baldridge et al., 2009) по
адресу http://speclib.jpl.nasa.gov и спектральной базой данных
Лаборатории исследования цвета Университета Восточной
Финляндии на сайте http://uef.fi/en/spectral. Данные о реакции

Подведение итогов 579

колбочек и функциях цветового баланса CIE доступны в Исследовательской лаборатории цвета и зрения Университетского
колледжа Лондона по адресу http://cvrl.org. Данные CIE также
доступны в интернете по адресу http://cie.co.at.
Таблица 10.5. Toolbox содержит данные о различных спектрах. Значения
относительной светимости лежат в интервале [0, 1], а относительное
спектральное распределение мощности (spectral power distribution, SPD)
нормировано к значению 1.0 на 550 нм. Эти файлы можно загрузить
с помощью функции loadspectrum Toolbox
Имя
файла
cones
bb2

Единицы
измерения
Отн. яркость
Отн. яркость

photopic
scotopic
redbrick
macbeth

Отн. яркость
Отн. яркость
Отр. способность
Отр. способность

solar
water
D65

Вт/(м2·м)
1 м-1
Отн. SPD

10.5.3

Описание
Спектральный отклик колбочек человека
Спектральный отклик сенсора Sony ICX 204AK,
применяемого в камере Point Grey BumbleBee
Фотопический отклик CIE 1924
Скотопический отклик CIE 1951
Отраженный спектр выветренного красного кирпича
Отражательная способность плашки Gretag-Macbeth
Color Checker (24 квадрата, рис. 10.21)
Спектр солнечного света на уровне земли
Спектр светопоглощения воды
Источник света по стандарту CIA D65

Упражнения

1. Представьте, что вы излучатель черного тела! Постройте
спектр своего излучения. Какова пиковая частота вашего
излучения? Какая это часть спектра? Сенсор какого типа вы
посоветуете использовать, чтобы обнаружить ваше излучение?
2. Рассмотрим сенсор, который измеряет количество излучаемой мощности P1 и P2 на длинах волн λ1 и λ2 соответственно.
Напишите уравнение для вычисления температуры Т черного тела через эти величины.
3. Используя закон Стефана–Больцмана, рассчитайте мощность, излучаемую на квадратный метр поверхности Солнца. Вычислите полную выходную мощность Солнца.
4. С помощью численного интегрирования вычислите мощность, излучаемую в видимом диапазоне 400–700 нм на квад­
ратный метр поверхности Солнца.
5. Почему пиковая яркость определяется как 683 лм/Вт?
6. Используя типичную наружную освещенность из раздела 10.2.3, определите силу света Солнца.
7. Из падающей мощности излучения солнечного света на
уровне земли определите процентные доли инфракрасного,
видимого и ультрафиолетового света.

580 Глава 10 · Свет и цвет

8. Используя численное интегрирование, вычислите мощность, излучаемую в видимом диапазоне 400−700 нм /м2
для вольфрамовой лампы при температуре 2600 К. Какова
эта доля от общей излучаемой мощности?
9. Центральная ямка глаза человека имеет поле зрения 5°. Какому диаметру круга это соответствует для вертикальной
поверхности на расстоянии 2 м?
10. Постройте и сравните фотопический и скотопический
спектральные отклики глаза человека.
a) Сравните кривые отклика колбочек человека и каналов
RGB цветной камеры. Используйте файлы данных cones.
dat и bb2.dat.
11. Можете ли вы найти метамер для красного кирпича?
12. Докажите закон центра тяжести Грассмана, упомянутый
в разделе 10.2.4.
13. На цветовой плоскости xy начертите геометрическое место
(локус) излучения абсолютно черного тела с температурами в диапазоне 1000–10 000 К.
14. Нанесите основные цвета XYZ на rg-плоскость.
15. Для рис. 10.12 определите цветность допустимого зеленого
цвета.
16. Определите значения трехкомпонентного цветового сигнала для красного кирпича, используя базовые цвета стандарта 709.
17. Сфотографируйте белый предмет, освещенный лампой накаливания. Определите среднее трехцветное значение RGB
и вычислите xy-цветность. Насколько она далека от белого?
Определите мат­ри­цу цветового баланса J, чтобы скорректировать цветность. Что такое цветность освещения?
18. Как называется цвет красного кирпича под водой?
19. Вспомните мишень, изображенную на рис. 3.17, с тремя
цветными пятнами известной цветности. По их наблюдаемой цветности определяют преобразование наблюдаемых
трехцветных значений в Rec. 709. Что такое цветность освещения?
20. Пусть некоторый объект находится под водой на глубине
d метров и наблюдается через m-метровую толщу воды,
а поверхность воды освещается солнечным светом. Можно
ли по наблюдаемой цветности определить истинную цветность цели? Насколько чувствительна эта оценка к неправильным оценкам m и d? Если бы вы знали истинную цветность цели, могли бы вы определить расстояние до нее?
21. Возможно ли, чтобы два разных цвета выглядели одинаково при определенном освещении? Можете ли вы предложить пример цветов и освещения, которые реализуют это
явление?

Подведение итогов 581

22. Используйте одно из ваших собственных изображений
и подход из раздела 10.4.1. Сможете ли вы различить разные предметы на изображении?
23. Покажите аналитически или численно, что масштабирование трехкомпонентного значения не влияет на цветность.
Что произойдет, если цветность вычисляется на основе
трехкомпонентных гамма-кодированных значений?
24. Создайте интерактивный инструмент с ползунковыми регуляторами для R, G и B, которые изменяют цвет отображаемого фрагмента. Теперь измените этот инструмент так,
чтобы ползунки регулировали компоненты X, Y и Z или x, y
и Y.
25. Создайте цветное изображение и определите, как оно будет
выглядеть под водой на глубине 1, 5 и 10 м.
26. Определите названия цветов в таблице цветов Gretag-Macbeth.
27. Нарисуйте графики изменения компонентов функции цветового баланса, показанные на рис. 10.10, в виде трехмерной кривой. Поверните ее, чтобы увидеть локус, как показано на рис. 11.11.
28. Какую температуру должно иметь абсолютно черное тело,
чтобы его пик излучения соответствовал пику чувствительности синих колбочек глаза человека? Остаются ли металлы в твердом состоянии при такой температуре?
29. Изучите алгоритм Retinex, реализуйте его и поэкспериментируйте с ним.

Глава

11

	 Изображения
и их обработка

Обработка изображений – это вычислительный процесс, направленный на преобразование одного или нескольких входных
изображений в выходное. Обработка изображений часто используется для улучшения изображения и последующего просмотра
или интерпретации человеком, например для повышения контрастности. С другой стороны – и это важнее с точки зрения робототехники, – это основа процесса извлечения признаков, который будет рассмотрен более подробно в следующей главе.
Изображение – это прямоугольный массив элементов (пикселей), поэтому для представления изображения в рабочей области мы будем использовать массив NumPy. Это позволит нам
применить мощный и эффективный арсенал матричных операций в NumPy.
Мы начинаем в разделе 11.1 с описания загрузки изображений в сеанс Python из таких источников, как файлы (изображения и видеоролики), камеры и интернет. Далее, в разделе 11.2,
мы введем понятие гистограмм изображения, предоставляющих полезную информацию о распределении значений пикселей. Затем обсудим различные классы алгоритмов обработки
изображений. Эти алгоритмы работают попиксельно с одним
изображением, парой изображений или с локальными группами пикселей в изображении, и мы называем их монадическими,
диадическими и пространственными операциями соответственно. Монадические и диадические операции рассматриваются
в разделах 11.3 и 11.4. Пространственные операторы описаны
в разделе 11.5 и включают такие операции, как сглаживание,
обнаружение краев и сопоставление с шаблоном. С ними тесно
связан метод фильтрации по форме, или математическая морфология, описанный в разделе 11.6. Наконец, в разделе 11.7 мы
обсудим операции изменения формы, такие как обрезка, сжатие, расширение, а также более сложные операции, такие как
поворот и деформация изображения.
Роботы всегда будут получать несовершенные изображения
мира из-за шума, теней, отражений и неравномерного освещения. В этой главе мы обсудим некоторые фундаментальные

chap11.ipynb

https://go.sn.pub/
XuZbLo

Получение изображения 583

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

11.1

Получение изображения

В настоящее время цифровые изображения используются повсеместно, поскольку камеры встроены в наши цифровые
устройства, а создание и распространение изображений почти
ничего не стоит.
У каждого из нас есть постоянно растущие личные коллекции
и доступ к огромным онлайн-коллекциям цифровых изображений, таким как Google Images, Picasa или Flickr. У нас также есть
доступ к потоковым изображениям с чужих камер – десятки
тысяч веб-камер по всему миру снимают изображения и транслируют их в интернет, – а также изображения вида Земли из
космоса, Луны и Марса.

11.1.1

Изображения из файлов

Изображения Начнем с изображений, хранящихся в файлах и распростра­
и другие данные няемых вместе с пакетом Toolbox,
но вы с неменьшим успевходят в состав
хом
можете
использовать
свои
личные
изображения. Для напакета mvtb-images,
который объявлен чала импортируем изображение в сеанс Python с помощью
как зависимость функции iread
в пакете Machine
Vision Toolbox.
>>> street, _ = iread("street.png");
Функция iread сначала пытается прочитать изображение которая возвращает массив NumPy
в текущей папке,
а затем в коллекции >>> street
изображений, по- array([[193, 193, 191, ...,
8, 7, 6],
ставляемых вместе
[189, 190, 191, ..., 7, 7, 8],
с Toolbox.

Гамма-кодирование
и гамма-декодирование обсуждаются
в разделе 10.3.6.
Используйте параметр gamma в вызове
iread, чтобы выполнить гамма-декодирование и получить
значения пикселей,
пропорциональные
яркости сцены.

[190,
...,
[ 50,
[ 53,
[ 53,

189, 190, ...,

8,

8,

7],

51, 52, ..., 30, 28, 29],
54, 53, ..., 28, 28, 29],
56, 54, ..., 27, 27, 28]], dtype=uint8)

содержащий множество маленьких целых чисел типа uint8 –
беззнаковых целых чисел в интервале [0, 255]. Элементы изображения называются значениями пикселей, или значениями
серого, и представляют собой гамма-кодированную яркость
этой точки в исходной сцене. Для этого 8-битного изображения
значения пикселей варьируются от 0 (самый темный) до 255
(самый яркий). Изображение показано на рис. 11.1а.

584 Глава 11 · Изображения и их обработка

Массив имеет два измерения
>>> street.shape
(851, 1280)

и содержит 851 строку и 1280 столбцов. Обычно мы описываем
размеры изображения в формате (ширина × высота), так что мы
имеем изображение размером 1280×851 пикселей.
Например, пиксель с координатами (200, 400) имеет значение
>>> street[400, 200]
24

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

Мы легко можем отобразить этот массив как изображение,
используя Matplotlib:
>>> idisp(street);

Получившееся изображение показано на рис. 11.1. Горизонтальная ось изображения подписана как u и направлена слева
направо. Вертикальная ось подписана как v и направлена вертикально вниз. Верхний левый пиксель имеет координаты (0, 0).
Панель инструментов в нижней части окна позволяет увеличивать масштаб изображения, а также панорамировать и прокручивать его. Координаты и значение пикселя под курсором
отображаются справа в панели инструментов.
Изображение было прочитано из файла street.png, который
имеет формат переносимой сетевой графики (portable network
graphics, PNG), обеспечивающий сжатие без потерь
и получивший широкое распространение в интернете.
Это конкретное изображение не имеет цветов, оно представлено в оттенках серого, т. е. монохроматическое. Но мы с неменьшей легкостью можем загрузить и цветное изображение:
>>> flowers, _ = iread("flowers8.png")

Как и в предыдущем примере, элементы массива
>>> flowers.dtype
dtype('uint8')

«Без потерь»
означает, что сжатое
изображение после
распаковки будет
точно таким же, как
исходное изображение.

Получение изображения 585

Рис. 11.1. Черно-белое изображение. Окно просмотра изображений в Toolbox, которое можно
вывести вызовом функции, выводит idisp, или метод disp класса Image. Панель инструментов
внизу позволяет масштабировать, панорамировать и прокручивать изображение.
Значение уровня серого для пикселя, находящегося на кончике курсора, и его координаты
отображаются справа в панели инструментов

Отступление 11.1. Форматы файлов изображений
Существует очень большое количество форматов
файлов изображений, которые подробно каталогизированы на http://en.wikipedia.org/wiki/Image_
file_formats. Наиболее популярным является JPEG,
использующийся в цифровых камерах и веб-ка­ме­
рах. TIFF распространен во многих компьютерных
системах и часто используется для сканеров. PNG
и GIF широко применяются в интернете. Внутренний формат этих файлов сложен, но су­щест­вует
большое количество высококачественного программного обеспечения с открытым исходным кодом на различных языках для чтения и записи таких файлов. Toolbox может читать многие из этих
форматов файлов изображений.
Намного более простой набор форматов, широко используемых в системах
Linux, – это PBM, PGM и PPM (обычно PNM), которые представляют изображения
без сжатия и иногда в виде текстового файла ASCII. Множество инструментов с открытым исходным кодом, таких как ImageMagick, обеспечивают преобразование
форматов и манипулирование изображениями в Linux, MacOS X и Windows (рисунок справа получен от https://tagxedo.com/).

586 Глава 11 · Изображения и их обработка

имеют значения типа uint8, и загруженное изображение можно
отобразить с помощью Matplotlib
>>> idisp(flowers);

как показано на рис. 11.2. Здесь тоже есть панель инструментов,
позволяющая увеличивать изображение, а также панорамировать и прокручивать его. Координаты и цветовой тон пикселя
под указателем мыши отображаются в панели инструментов
справа.
Массив имеет три измерения
>>> flowers.shape
(426, 640, 3)

и содержит 426 строк и 640 столбцов. Третье измерение массива, показанное на рис. 11.3, называется индексом цветовой
плоскости. Например:
>>> idisp(flowers[:, :, 0]);

Рис. 11.2. Цветное изображение. Окно просмотра изображений в Toolbox, которое можно
вывести вызовом функции, выводит idisp, или метод disp класса Image. Панель инструментов
внизу позволяет масштабировать, панорамировать и прокручивать изображение. Значения
цветовых каналов пикселя, находящегося на кончике указателя мыши (в порядке «красный»,
«зеленый», «синий»), и его координаты отображаются в панели инструментов справа.
В данном случае пиксель находится на красном лепестке и поэтому имеет большую красную
составляющую цвета

Получение изображения 587
Отступление 11.2. JPEG
Формат JPEG основан на алгоритме сжатия с потерями для уменьшения размера
файла. В отличие от обычного сжатия файлов (например, zip, rar и т. д.), распакованный образ не совпадает с исходным, что позволяет использовать гораздо более высокие уровни сжатия. Сжатие JPEG использует ограничения человеческого
глаза и отбрасывает незаметную информацию, такую как очень небольшие изменения цвета (которые воспринимаются менее точно, чем небольшие изменения
яркости) и мелкая текс­тура. Очень важно помнить, что JPEG предназначен для
сжатия изображений, которые будут просматривать люди. Потеря цветовых деталей и тонкой текстуры может стать проблемой для компьютерных алгоритмов,
анализирующих изображения.
JPEG был создан, чтобы качественно сжимать естественные сцены, но он хуже
справляется с надписями и штриховыми рисунками, в которых много элементов
с высокой пространственной частотой. Величину потерь можно варьировать, регулируя степень сжатия, что позволяет найти компромисс между качеством изображения и размером файла. JPEG можно использовать как для цветных изображений, так и для оттенков серого.
То, что обычно называют файлом JPEG, часто с расширением .jpg или .jpeg,
правильнее называть файлом JPEG EXIF (Exchangeable Image File Format). EXIF –
это формат файла, который тоже содержит изображение, сжатое в формате JPEG
как метаданные. Формат файла EXIF является стандартом для метаданных, связанных с камерой, таких как настройки камеры, время, местоположение и т. д.
Эти метаданные можно получить с помощью метода metadata объекта Image или
утилиты командной строки, такой как exiftool (https://exiftool.org/). Подробную
информацию о формате можно найти на веб-сайте независимой группы JPEG
https://jpegclub.org/.

строка

H
высота

столбец

W

ширина

синяя плоскость
зеленая плоскость
красная плоскость

Рис. 11.3. Цветное изображение, представленное в виде структуры с тремя измерениями:
строкой, столбцом и цветовой плоскостью

588 Глава 11 · Изображения и их обработка

отобразит красную плоскость как черно-белое изображение,
представляющее величину красного компонента в каждом пикселе. Индексы 1 и 2 соответствуют зеленой или синей плоскости
соответственно.
Этот массив можно рассматривать как двумерный массив
одномерных трехкомпонентных значений RGB, каждое из которых представлено трехмерным вектором. Например, пиксель
с координатами (318, 276)
>>> pix = flowers[276, 318, :]
array([ 77, 109, 218], dtype=uint8)

имеет трехкомпонентное значение (77, 109, 218). Этот пиксель
имеет большое значение в синем (третьем) компоненте и соответствует синему цветку на изображении.
Функция iread имеет множество параметров. Аргумент
mono=True возвращает черно-белое изображение, по умолчанию
вычисляемое в соответствии со стандартом ITU Rec. 709, который определен формулой (10.11). Аргумент gamma="sRGB" выполняет гамма-декодирование и возвращает линейное изоб­
ражение, в котором уровни серого, или значения каналов цвета,
пропорциональны яркости исходной сцены. Функция также
может принимать URL вместо имени файла, что позволяет загрузить изображение из интернета.
Чтобы упростить работу с изображениями, в этой и последующих главах мы введем класс для представления изображения. Он будет хранить дополнительную информацию об изображении, такую как имя файла и имена цветовых плоскостей,
предлагать методы для обработки изображений, поддерживать
перегруженные операторы и скрывать низкоуровневые детали,
затрудняющие понимание концепций. Мы проиллюстрируем
использование класса, повторив примеры, приведенные выше.
Вот как можно создать экземпляр объекта Image для черно-белого изображения:
>>> street = Image.Read("street.png")
Image: 1280 x 851 (uint8) [.../images/street.png]

Метод класса Read принимает те же необязательные аргументы, что и iread. Объект во многом ведет себя подобно массиву
NumPy:
>>> street.shape
(851, 1280)

Отобразить хранимое изображение можно с помощью метода disp
>>> street.disp();

Получение изображения 589

который ведет себя точно так же, как функция idisp, и принимает те же необязательные аргументы.
Минимальные и максимальные значения пикселей можно
получить вызовом методов min() и max()
>>> street.min()
0
>>> street.max()
255

а сводную статистику значений пикселей – вызовом stats()
>>> street.stats()
range=0 - 255, mean=92.431, sdev=70.340

Внутренний массив NumPy доступен как свойство
>>> img = street.image;
>>> type(img)
numpy.ndarray

а так можно получить пиксель с координатами (200, 400):
>>> street.image[400, 200]
24

К массиву NumPy можно применить операцию среза
>>> subimage = street.image[100:200, 200:300];
>>> type(subimage)
numpy.ndarray

которая в данном примере вернет фрагмент 100×100 исходного
массива. Фрагмент можно извлечь и более непосредственно:
>>> subimage = street[100:200, 200:300];
>>> type(subimage)
machinevisiontoolbox.classes.Image

но имейте в виду, что применение операции среза к объекту
Ima­ge возвращает другой объект Image, представляющий фрагмент изображения размером 100×100.
Для цветного изображения
>>> flowers = Image.Read("flowers8.png")
Image: 640 x 426 (uint8), R:G:B [.../images/flowers8.png]

будет создан экземпляр Image c дополнительной информацией
о цветовых плоскостях. Диапазон значений пикселей в каждой
плоскости можно отобразить так:
>>> flowers.stats()
R: range=0 - 255, mean=83.346, sdev=74.197

590 Глава 11 · Изображения и их обработка
G: range=0 - 255, mean=77.923, sdev=42.822
B: range=0 - 255, mean=72.085, sdev=61.307

Через свойство image все так же доступен внутренний массив
NumPy. Вот, например, трехцветное значение пикселя в точке
(318, 276):
>>> flowers.image[276, 318, :]
array([ 77, 109, 218], dtype=uint8)

Красная цветовая плоскость доступна как двумерный массив
NumPy
>>> flowers.image[:, :, 0];

В качестве альтернативы можно записать
>>> flowers[:, :, 0].disp();

где срез объекта Image возвращает объект Image, а не массив
NumPy. Это можно также записать так:
>>> flowers.plane(0).disp();

или указать плоскость по имени
>>> flowers.plane("R").disp();

либо
>>> flowers.red().disp();

Методы red, green и blue гарантированно возвращают указанную цветовую плоскость, если она присутствует, независимо от
порядка их следования. Если указать несколько имен цветовых
плоскостей, то будет создано новое многоплоскостное изображение, например
>>> flowers.plane("BRG")
Image: 640 x 426 (uint8), B:R:G

вернет изображение с тремя плоскостями в следующем порядке: синяя, красная и зеленая.
Пакет Toolbox позволяет работать с цветовыми плоскостями
и их названиями в произвольном порядке. Например, результатом преобразования цветового пространства будет многоплос­
костное изображение, в котором плоскости получат имена по
компонентам цветового пространства. Например, изображение
в формате «оттенок–насыщенность» будет иметь плоскости "H",
"S" и "V". Имена цветовых плоскостей также можно разделять
двоеточиями, например:
>>> flowers.plane("B:G:R").disp();

Получение изображения 591

что особенно удобно при работе с длинными именами, такими
как "a*" или "b*".
OpenCV использует порядок цветов BGR
Цветовая шкала «красный–зеленый–синий» (RGB) сегодня используется практически повсеместно, но OpenCV использует шкалу
«синий–зеленый–красный» (BGR). Toolbox старается максимально
придерживаться RGB-последовательности.

Многие форматы файлов изображений также содержат обширные метаданные – «данные о данных» в файле. Файлы JPEG,
созданные большинством цифровых камер, содержат особенно
полные метаданные, которые можно извлечь в виде словаря:
>>> church = Image.Read("church.jpg");
>>> church.metadata()
{'Make': 'Panasonic',
'Model': 'DMC-FZ30',
'Software': 'Ver.1.0 ',
'Orientation': 1,
'DateTime': '2009:08:22 15:50:06',
'CompressedBitsPerPixel': 4.0,
'MaxApertureValue': 3.0,
'LightSource': 0,
'Flash': 16,
'FocalLength': 7.4,
...}

Теги EXIF стандартизированы, поэтому для любой камеры
фокусное расстояние всегда будет указано как значение с ключом FocalLength.

11.1.2

Изображения из последовательностей файлов

Для доступа к последовательности файлов можно создать итератор
>>> images = ImageCollection("seq/*.png");
>>> len(images)
9

который в данном случае загружает девять изображений в формате PNG из папки seq. Получить нужное изображение можно
с помощью операции среза
>>> images[3]
Image: 512 x 512 (uint8), id=3 [.../seq/im.004.png]

Есть возможность перебирать изображения:
>>> for image in images:
... image.disp() # выполнить некоторую операцию с изображением

592 Глава 11 · Изображения и их обработка

Доступ к отдельным файлам изображений в последовательности в zip-файле можно получить без предварительной распаковки файла:
>>> images = ZipArchive("bridge-l.zip", "*.pgm");
>>> len(images)
251

11.1.3

Изображения со встроенной камеры

Большинство современных портативных компьютеров оснащаются встроенной камерой для видеоконференций. К компьютерам без встроенной камеры легко подключить внешнюю
камеру через соединение USB. Способ доступа к камере зависит
от операционной системы, но Toolbox реализует универсальный интерфейс, идентифицирующий камеры последовательными целыми числами, начиная с нуля. Вот как подключиться
к камере с номером 0:

За кулисами
используется
OpenCV.

>>> camera = VideoCamera(0)
VideoCamera(0) 1920 x 1080 @ 29fps

Эта функция возвращает экземпляр объекта VideoCamera
и включает индикатор записи камеры. Размер изображения
и частота кадров являются свойствами этого объекта, а конструктор принимает те же дополнительные аргументы, что
и iread и Image.Read. Камера выключается вызовом
>>> camera.release()

Изображение извлекается методом grab
>>> image = camera.grab()
Image: 1920 x 1080 (uint8), R:G:B, id=0

который ожидает, пока следующий кадр не станет доступным,
а затем возвращает экземпляр Image. Объект также является
итератором, поэтому последовательные кадры можно получить так:
>>> for image in camera:
... # обработка изображения

11.1.4

Изображения из видеофайла

Видеофайл содержит последовательность сжатых изображений, закодированных в формате MPEG4 или AVI. Toolbox может

Поскольку кадры
генерируются со
скоростью R в секунду, то ожидание
в наихудшем
случае равномерно
распределяется
в интервале [0, 1/R).
Незахваченные
изображения не
накапливаются, они
отбрасываются.

Получение изображения 593
Отступление 11.3. Фотоны и значения пикселей
Внутри камеры происходит много разных интересных вещей. Фотоячейки – светочувствительфотоны
ные элементы сенсорной микросхемы, расположенные в плотном массиве. Каждая фотоячейка
обычно имеет квадратную форму со стороной
от 1 до 10 мкм. Количество фотонов, падающих
фотоэлектроны
на фотоячейку за фиксированный интервал
времени, подчиняется распределению Пуассозарядовый
на. Среднее число фотонов и дисперсия пропорколодец
циональны яркости – эта дисперсия проявляеттепловой
электрон
ся как дробовой шум в значении пикселя. Часть
этих фотонов преобразуется в электроны – это
накопленные
квантовая эффективность сенсора, – и они наэлектроны
капливаются в зарядовом колодце (charge well)
фотоячейки. Количество захваченных фотонов
пропорционально площади поверхности, но не
вся фотоячейка чувствительна к свету из-за наличия транзисторов и других элементов – доля чувствительной площади фотоячейки называется коэффициентом заполнения. Для КМОП-сенсоров она может
быть менее 50 %, но этот параметр можно улучшить, изготовив микролинзы над
каждой фотоячейкой.
Зарядовый колодец также хорошо аккумулирует термически генерируемые
электроны – темновой ток, который пропорционален температуре и является источником шума, поэтому камеры, работающие при экстремально слабом освещении, дополнительно охлаждаются. Другим источником шума является неравномерность пикселей из-за того, что соседние пиксели имеют разное усиление или
смещение, поэтому однородное освещение дает пиксели с разными значения­
ми, что является аддитивным шумом. Зарядовый колодец имеет ограниченную
емкость, и при чрезмерном освещении избыточные электроны могут перетечь
в соседние колодцы, что приведет к засветке
и обесцвечиванию изображения.
Накопленный заряд, состоящий из тепловых и фотоэлектронов, считывается в конце
интервала экспозиции. В недорогих КМОПсенсорах зарядовые колодцы последовательно
подключаются через коммутатор к одному или
нескольким аналого-цифровым преобразователям на кристалле. Это так называемый механизм скользящего затвора, который при съемке
высокоскоростного относительного движения
приводит к разрывам движения, или эффекту
желе, как показано на рисунке. Более дорогие
датчики CMOS и CCD имеют глобальный затвор – они делают моментальный промежуточный снимок заряда в буфере, который затем последовательно оцифровывается.
Экспозиция сенсора описывается выражением
лк·с,

594 Глава 11 · Изображения и их обработка
где L – яркость сцены (в нитах), T – время экспозиции, N – это f-число (обратный
диаметр диафрагмы), а q ≈ 0.7 – функция фокусного расстояния, качества объектива и виньетирования. Время экспозиции T имеет верхнюю границу, равную обратной частоте кадров. Чтобы избежать размытия при движении, требуется короткое
время экспозиции, что приводит к более темным и шумным изображениям.
Целочисленное значение пикселя равно
x = kH,
где k – усиление, связанное с настройкамиa ISO камеры. Для получения изображения sRGB (см. раздел 10.3.6) со средним значением 118b требуемая экспозиция
составляет
лк·с,
где SSOS – это рейтинг ISO, стандартная выходная чувствительность (standard output sensitivity, SOS) цифровой камеры. Чем выше значение ISO, тем выше яркость
изображения за счет большего усиления измеренного заряда, но также усиливаются различные источники шума, что приводит к увеличению шума изображения
в виде зернистости.
В фотографии настройки камеры, управляющие яркостью изображения, могут
быть объединены в значение экспозиции (exposure value, EV)
,
и все комбинации f-числа и скорости затвора с одинаковым значением EV дают
одинаковую экспозицию. Это позволяет найти компромисс между апертурой
(глубина резкости) и временем экспозиции (размытие в движении). В большинстве недорогих камер диафрагма фиксирована, и камера управляет экспозицией
с помощью T, вместо того чтобы использовать дорогую и медленную механическую диафрагму. Разница в 1 EV – это двукратное изменение экспозиции, которое
фотографы называют остановкой (stop), или ступенью. Увеличение EV приводит
к более темному изображению – большинство цифровых зеркальных камер позволяют вам вручную регулировать EV относительно того, что определил экспонометр камеры.
 Которые обратно совместимы с историческими шкалами (ASA, DIN, ISO), разработанными для отражения чувствительности химических пленок для фотоаппаратов – большее число отражало более чувствительную, или «быструю», пленку.
b
 Насыщенность 18 %, средний серый цвет, 8-битные пиксели с гаммой 2.2.
a

читать отдельные кадры из многих популярных форматов видеофайлов, например вызов
>>> video = VideoFile("traffic_sequence.mp4")
VideoFile(traffic_sequence.mp4) 704 x 576, 350 frames @ 30fps

вернет объект VideoFile, полиморфный классу VideoCamera, описанному выше. Видеоролик, полученный в этом примере, содержит 350 кадров и снят с частотой 30 кадров в секунду.

Получение изображения 595
Отступление 11.4. Динамический диапазон сенсора
Динамический диапазон сенсора – это отношение его наибольшего значения
к наименьшему. Для изображений полезно использовать log2 этого отношения,
что делает его эквивалентным значениям экспозиции. Каждая фотоячейка содержит зарядовый колодец, в котором в течение периода экспонирования накапливаются генерируемые фотонами электроны. Зарядовый колодец имеет конечную
емкость до насыщения фотоячейки, и это определяет максимальное значение.
Минимальное число электронов не равно нулю, а является конечным числом термически генерируемых электронов.
8-битное изображение имеет динамический диапазон около 8, 10-битная камера высокого класса имеет диапазон 10, а фотопленка может обеспечить диапазон
10–12, но она довольно нелинейна.
В определенном состоянии адаптации человеческий глаз имеет динамический
диапазон в 10 ступеней, но общий диапазон адаптации составляет впечатляющие
20 ступеней. Это достигается за счет использования радужной оболочки и более
медленной (десятки минут) химической адаптации чувствительности палочек.
Темновая адаптация к слабому освещению медленная, тогда как адаптация от
темноты к яркому происходит быстрее, но иногда болезненна.

Отступление 11.5. Форматы видеофайлов
Подобно файлам изображений, видеофайлы имеют
большое количество различных форматов. Наиболее распространенными форматами являются MPEG
и AVI. Важно четко понимать разницу между форматом файла (контейнера) и типом сжатия (кодеком),
используемым для изображений в файле. Файл MP4 –
это контейнер MPEG-4, использующий кодек H.264.
Видеокодеки сжимают отдельные кадры, а также используют избыточную информацию между последовательными кадрами.
Файлы форматов MPEG и AVI можно преобразовать
в последовательность кадров в виде отдельных файлов с помощью таких инструментов, как FFmpeg и convert из пакета ImageMagick.
Затем отдельные кадры можно по отдельности загрузить в Python для обработки
посредством Image.read. Класс VideoFile в Toolbox предоставляет более удобный
способ чтения кадров непосредственно из распространенных форматов фильмов
без необходимости сначала преобразовывать фильмы в набор отдельных кадров
(изображение карты слов получено от tagxedo.com).
Это форма массива
Размер каждого
NumPy, содержащего изображе- >>> video.shape
ние с размерами (576, 704, 3)
(height, width,
ncolors).

кадра в ролике ◄

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

596 Глава 11 · Изображения и их обработка
>>> for frame in video:
... frame.disp(reuse=True) # отобразит рамку с теми же осями

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

11.1.5

Изображения из интернета

Термин «веб-камера» стал обозначать любую локальную камеру, подключенную к компьютеру через порт USB, но здесь мы
используем его для обозначения камеры, подключенной к ин­
тернету через веб-сервер, предоставляющий изображения по
запросу. По всему миру установлены десятки тысяч таких вебкамер, направленных на различные сцены, от обыденных до захватывающих. Получив URL веб-камеры ►, мы можем скачать
изображение с камеры в любой точке мира.
Например, мы можем подключиться к камере в Дартмутском
колледже в Нью-Гемпшире:

Веб-камеры
поддерживают
различные параметры, которые
можно встроить
в URL, и для них не
сущест­вует стандарта. Этот пример
демонстрирует один
из распространенных форматов URL,
используемых многими веб-камерами.

>>> dartmouth = WebCam("https://webcam.dartmouth.edu/webcam/image.jpg");

которая возвращает объект WebCam, полиморфный к классам VideoCamera и VideoFile, описанным выше.
Следующее изображение можно получить путем последовательных итераций или вызовом метода grab:
>>> dartmouth.grab().disp();

Полученное в этом примере цветное изображение показано на рис. 11.4. Веб-камеры настраиваются их владельцем на
периодическую съемку изображений, от одного раза в секунду
Отступление 11.6. Соотношение сторон
Соотношение сторон – это отношение ширины изображения к его высоте. Оно
значительно различается в зависимости от различных технологий обработки изображений и дисплеев. Для 35-мм пленки это соотношение равно 3:2 (1.5), что соответствует отпечатку 4×6 дюймов (1.5). Существуют и другие соотношения сторон:
5×7 дюймов (1.4) и 8×10 дюймов (1.15), которые требуют обрезки вертикальных
краев изображения для его размещения на стандартном мониторе.
В телевизорах и ранних компьютерных мониторах использовался формат 4:3
(1.33), например распространенный формат VGA 640×480. Формат HDTV остановился на соотношении 16:9 (1.78). Современные цифровые зеркальные камеры
обычно используют соотношение 1.81, что близко к соотношению для HDTV. В кинотеатрах предпочтение отдается очень широким изображениям с соотношением
сторон 1.85 или даже 2.39. Формат CinemaScope был разработан компанией 20th
Century Fox на основе работ Анри Кретьена в 1920-х. Анаморфотный объектив на
камере сжимает широкое изображение до стандартного соотношения сторон в камере, а на проекторе процесс происходит в обратном порядке.

Получение изображения 597

v (пиксели)

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

u (пиксели)
Рис. 11.4. Изображение с веб-камеры Дартмутского университета, которая направлена
на главную лужайку колледжа

11.1.6

Снимки из космоса

Чтобы получить
ключ API, необходимо зарегистри- Вы можете получить доступ к спутниковым снимкам и дорожроваться в Google ным картам любой точки планеты из сеанса Python. Сначала
Maps Platform,
создайте экземпляр объекта EarthView:
ввести данные
кредитной карты
и согласиться со- >>> world = EarthView();
блюдать условия исДля этого вызова необходимо иметь ключ Google API. Его
пользования Google.
Компания Google можно передать конструктору в аргументе key="ВАШ_КЛЮЧ" или
позволяет загружать поместить в переменную окружения GOOGLE_KEY.
Например,
десятки тысяч бесвот
как
можно
получить
спутниковый
снимок
моего
универ­
платных изображений в месяц. ситета:

>>> world.grab(-27.475722, 153.0285, 17).disp();
Уровень масштабиСнимок показан на рис. 11.5а, а аргументами являются ширования – это целое рота, долгота и уровень масштабирования.
число. Для нулевого
Вместо вида со спутника можно выбрать вид дорожной карты
значения возвращается вид, охватывающий всю Землю. >>> world.grab(-27.475722,153.0285, 15, type="map").disp();
Каждое увеличение
на единицу удва- который показывает обширную картографическую информаивает разрешение цию, такую как названия дорог и мест. Более простое представв направлениях
x и y. ление дает команда

>>> world.grab(-27.475722,153.0285, 15, type="roads").disp();

598 Глава 11 · Изображения и их обработка

v (пиксели)

v (пиксели)

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

u (пиксели)
u (пиксели)
а
б
Рис. 11.5. Вид с воздуха на Брисбен, Австралия: а) цветной аэрофотоснимок; б) бинарное
изображение – транспортная сетка, где белые пиксели – это дороги, по которым можно
проехать (изображения предоставлены Google, Airbus, Maxar Technologies)

11.1.7

Изображения из кода

При отладке алгоритма бывает полезно начать с идеального
и простого изображения, прежде чем переходить к более сложным изображениям реального мира. Изображение можно создать с помощью любого графического редактора, экспортировать его в файл, а затем импортировать в Python.
Также можно создать изображение непосредственно с помощью кода на Python. Класс Image содержит ряд методов, генерирующих различные простые шаблоны, включая линии, сетки
точек или квадратов, градиенты интенсивности и синусоиды интенсивности. Вот примеры методов класса, создающих
изобра­жения с использованием определенных шаблонов:
>>>
>>>
>>>
>>>

image
image
image
image

=
=
=
=

Image.Ramp(cycles=2, size=500, dir="x");
Image.Sin(cycles=5, size=500, dir="y");
Image.Squares(number=5, size=500);
Image.Circles(number=2, size=500);

Результат показан на рис. 11.6а. Аргумент size – это размер
создаваемого изображения, в данном случае все изображения
имеют размеры 512×512 пикселей, а остальные аргументы зависят от типа запрошенного шаблона.

v (пиксели)

Получение изображения 599

u (пиксели)
а
б
Рис. 11.6 Изображения из кода: а) некоторые тестовые шаблоны, сгенерированные Toolbox;
б) простое изображение, созданное из графических примитивов

Объект Image имеет
Мы также можем создать изображение из простых графичеметоды draw_xx, поских
примитивов. Сначала создадим пустой холст, содержащий
хожие на функции
plot_xx из Spatial только черные пиксели типа uint8 (значение пикселя равно
Math Toolbox. Понулю):
следние используют
Matplotlib для отображения данных >>> canvas = Image.Zeros(1000, 1000, dtype="uint8")
в экранных осях, Image: 1000 x 1000 (uint8)
а первые – в массиве NumPy. а затем создадим два квадрата ◄

>>> canvas.draw_box(lt=(100, 100), wh=(150, 150), color=100,
...
thickness=-1);
>>> canvas.draw_box(lt=(300, 300), wh=(80, 80), color=150,
...
thickness=-1);
В данном случае
Аргументами метода draw_box() являются координаты лево«верх» соответству- го верхнего угла,
ширина и высота, значение для пикселей
ет самой нижней
и
толщина
рамки,
где
значение -1 означает, что рамка покрыкоординате v, потому что на изображе- вает весь прямоугольник. Аналогично нарисуем круг.
ниях v увеличивается в направлении >>> canvas.draw_circle((600, 600), 120, color=200, thickness=-1)
сверху вниз.

с центром в точке (600, 600), радиусом 120 пикселей и залитый
серым цветом со значением 200. Наконец, нарисуем отрезок
прямой
>>> canvas.draw_line((100, 100), (800, 800), color=250, thickness=8)

от точки (100, 100) до точки (800, 800) толщиной 8 пикселей
и всем этим пикселям присвоим значение 200. Результат
>>> canvas.disp();

показан на рис. 11.6б. Как видите, формы имеют разную яркость, и отрезок с кругом демонстрируют эффекты квантова-

600 Глава 11 · Изображения и их обработка

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

В компьютерной
графике принято применять
сглаживание,
Обратите внимание, что все эти функции принимают координаты, для чего следует
выраженные в нотации (u, v), а не в нотации столбцов-строк NumPy. передать параметр
antialias=True.
Верхний левый пиксель имеет координаты (0, 0).
В таком случае
пикселям на краях
и пикселям, смежным с краем, будут
присвоены дробные
значения серого, что
создаст впечатление
более гладкого края.

11.2

Гистограммы изображений

Распределение значений пикселей предоставляет полезную
информацию о качестве изображения и композиции сцены.
Для сцены (рис. 11.9а)
>>> church = Image.Read("church.png", mono=True)
Image: 1280 x 851 (uint8) [.../images/church.png]

мы можем извлечь некоторые простые статистики с помощью
методов Image:
>>> church.min()
5
>>> church.max()
238
>>> church.mean()
132.9
>>> church.median()
143
>>> church.std()
57.44

или вывести однострочную сводку:
>>> church.stats()
range=5 - 238, mean=132.906, sdev=57.445

Полное распределение значений пикселей можно увидеть на
гистограмме
>>> h = church.hist()
histogram with 256 bins: xrange 0.0 - 255.0, yrange 0.0 - 35830.0
>>> h.plot();

показанной на рис. 11.7а. Горизонтальная ось – значение пикселя, вертикальная ось – частота значения пикселя, то есть количество раз, когда значение пикселя встречается на изображении.
Метод hist объекта Image возвращает объект Histogram, который имеет ряд методов, включая построение графика. Как
показывает гистограмма на рис. 11.7a, значения серого (гори-

Гистограммы изображений 601

зонтальная ось) охватывают диапазон от 5 до 238, что близко
к полному диапазону возможных значений. Если бы изображение было недоэкспонировано, область гистограммы была бы
сдвинута влево. Если бы изображение было переэкспонировано, гистограмма была бы сдвинута вправо, и многие пиксели
имели бы максимальное значение. Нормализованная кумулятивная гистограмма
>>> h.plot("ncdf", color="blue")

частота пикселей

накопленное количество пикселей

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

а

уровень серого

б

исходное
нормализованное

уровень серого

Рис. 11.7. Изображение церкви: а) гистограмма; б) кумулятивная гистограмма до и после нормализации

Для этого изображения распределение значений пикселей
далеко не равномерно, и мы видим, что есть три значимых
пика. Однако если присмотреться повнимательнее, мы увидим
множество очень незначительных пиков. Форма пика зависит
от масштаба, в котором мы рассматриваем данные. Можно получить гистограмму как пару векторов. Метод peaks автоматически определит положение пиков
>>> x = h.peaks();
>>> x.shape
(45,)

В этом случае было обнаружено 45 пиков, большинство из которых весьма незначительны. Значимые пики не только больше,
чем их непосредственные соседи, они больше, чем все другие
значения поблизости – теперь проблема состоит в том, чтобы
определить, что мы подразумеваем под соседними пиками. Например, пики, превышающие все остальные значения в пределах ±25 пикселей в горизонтальном направлении

602 Глава 11 · Изображения и их обработка
>>> x = h.peaks(scale=25)
array([
213,
154,

40])

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

11.3

Монадические операции

Монадические (monadic, одноместные) операции обработки
изоб­ражений схематично показаны на рис. 11.8. Результатом
Y является изображение того же размера W×H, что и входное
изоб­ражение X, и каждый выходной пиксель является функцией соответствующего входного пикселя.
yu,v = f(xu,v),

∀(u, v) ∈ X.

Один полезный класс монадических функций изменяет тип
данных пикселей. Например, чтобы перейти от uint8 (целочисленные значения пикселей в диапазоне [0, 255]) к значениям
с плавающей точкой в диапазоне [0, 1], можно использовать
метод to объекта Image
>>> church_float = church.to("float")
Image: 1280 x 851 (float64)
>>> church.max()
238
>>> church_float.max()
0.9333

и обратный переход:
>>> church_float.to("uint8")
Image: 1280 x 851 (uint8)

Монадические операции 603

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

Значения с плавающей точкой могут быть либо 4-байтовыми, типа float32, либо 8-байтовыми, типа float64. Тип можно
указать при загрузке изображения
>>> street_float = Image.Read("street.png", dtype="float")
Image: 1280 x 851 (float32) [.../images/street.png]

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

604 Глава 11 · Изображения и их обработка
>>> gray = flowers.mono()
Image: 640 x 426 (uint8)

Обратная операция
>>> color = gray.colorize()
Image: 640 x 426 (uint8), R:G:B

возвращает трехмерное цветное изображение, где каждая цветовая плоскость равна gray – при отображении оно по-прежнему
выглядит как монохромное изображение. Мы можем создать
цветное изображение, где красная плоскость равна входному
изображению
>>> color = gray.colorize((1, 0, 0))
Image: 640 x 426 (uint8), R:G:B

Результат представляет собой окрашенную в красный цвет
версию исходного изображения, где каждый выходной цветной
пиксель представляет собой трехцветный сигнал (1, 0, 0), масштабированный соответствующим значением серого.
Очень распространенной монадической операцией является установка порога. Эта логическая монадическая операция
разделяет пиксели на два класса в зависимости от их интенсивности:
>>> bright = (church >= 180)
Image: 1280 x 851 (bool)

В результате получается логическое изображение, где все
пиксели имеют только два значения, True или False. При отображении
>>> bright.disp();

как показано на рис. 11.9б, значение True соответствует белым
пикселям – пикселям изображения в интервале [180, 255], а значение False – черным пикселям. Такие изображения называются бинарными.
Глядя на гистограмму изображения на рис. 11.7а, мы видим,
что значение серого 180 находится посередине между вторым
и третьим пиками, что является хорошим приближением к оптимальной стратегии разделения пикселей, принадлежащих
этим двум популяциям.
Многие монадические операции связаны с изменением распределения уровней серого в изображении. Иногда изображение охватывает не весь диапазон доступных уровней серого,
например оно недоэкспонировано или переэкспонировано. Мы
можем применить линейное отображение к значениям шкалы
серого и гарантировать охват значениями пикселей всего диа-

Монадические операции 605
Гистограмма такого пазона ◄, который составляет либо [0, 1], либо [0, 255], в зависиизображения будет мости от класса изображения, например
иметь пробелы.
Если M – максимально возможное >>> church.stretch().stats()
значение пикселя, range=0.0 - 1.0, mean=0.549, sdev=0.247
а N < M – максимальное значение
Более сложной версией является нормализация или выравв изображении, то
растянутое изо- нивание гистограммы
бражение будет
иметь не более N >>> im = church.normhist();
уникальных значений пикселя, т. е.
Результат, показанный на рис. 11.9в, подчеркивает текстурзначения M – N не
могут встречаться. ные детали стен и неба, которые ранее имели очень незначиа

б

в

г

д

е

Рис. 11.9. Некоторые монадические операции с изображением: а) исходное изображение; б) после установки
порогового значения; в) нормализация гистограммы; г) гамма-коррекция; д) увеличение яркости; е) постеризация.
Вставка на каждом изображении представляет собой график, показывающий сопоставление уровня серого
изображения по горизонтальной оси с выходным значением по вертикальной оси

606 Глава 11 · Изображения и их обработка

тельную вариацию уровня серого. Пиксели изображения были
сопоставлены с помощью инверсной кумулятивной гистограммы (синяя линия на рис. 11.7б). Значения исходных пикселей
масштабируются в интервале [0, 1], и для этого значения на
вертикальной оси кумулятивной гистограммы мы считываем
соответствующее значение на горизонтальной оси. Кумулятивное распределение полученного изображения (красная линия
на рис. 11.7б) теперь линейно – все значения серого встречаются с одинаковой вероятностью.
 Улучшение изображения не добавляет дополнительной
информации в него
Такие операции, как isstretch и inormhist, могут улучшить изображение с точки зрения человека, но важно помнить, что в изображение не добавляет никакой новой информации. Последующие этапы
обработки изображения не приведут к улучшениям.

Как обсуждалось в разделе 10.3.6, выходной сигнал камеры
обычно подвергается гамма-кодированию, так что значение
пикселя представляет собой нелинейную функцию Lγ яркости,
воспринимаемой фотоячейкой. Такие изображения можно
гамма-декодировать с помощью нелинейной монадической
операции
>>> im = church.gamma_decode(2.2);

которая возводит каждый пиксель в указанную степень, как показано на рис. 11.9г, или
>>> im = church.gamma_decode("sRGB");

для декодирования изображений со стандартной гамма-кодировкой sRGB.
Класс Image поддерживает все арифметические операторы
Python, такие как скалярное умножение и деление, сложение
и унарное отрицание. Например:
>>> (church // 64).disp();

создает эффект поп-арт-постеризации, уменьшая количество
уровней серого, как показано на рис. 11.9е. Целочисленное деление дает изображение со значениями пикселей в диапазоне
[0, 3] и поэтому всего четыре разных оттенка серого.
Класс Image имеет методы для реализации множества распространенных математических функций, таких как abs или sqrt,
которые применяются к каждому пикселю изображения. Мы
можем применить любую функцию, которая принимает и возвращает скалярное значение.
>>> church.apply(lambda x: x // 64, vectorize=True).disp()

Гамма-коррекция
в данном случае
применялась
дважды: один раз
с помощью функции
gamma_decode и один
раз с помощью
устройства отображения. Из-за этого
результирующее
изображение выглядит неестественно
контрастным.

Диадические операции 607

и в этом случае функция будет вызвана
>>> church.npixels
1089280

раз, что потребует много времени. Более эффективный подход,
при условии что функция может принимать и возвращать массив NumPy
>>> church.apply(lambda x: x // 64).disp();

– передать изображение как массив NumPy в функцию и преобразовать полученный массив NumPy в новый экземпляр
Image.
Если пиксели изображения имеют тип uint8, то можно пропус­
тить каждый пиксель через таблицу поиска (lookup table, LUT)
из 256 элементов. Например:
>>> lut = [x // 64 for x in range(256)]; # создать таблицу поиска
>>> church.LUT(lut).d isp();

что позволит произвольно отображать значения пикселей: если
значение пикселя равно i, результатом будет lut[i]. OpenCV
очень эффективно реализует это на внутреннем уровне.

11.4

Диадические операции

Диадические (dyadic, двухместные) операции схематично показаны на рис. 11.10. Два входных изображения дают одно выходное изображение, и все три изображения имеют одинаковый
размер. Каждый выходной пиксель является функцией соответствующих пикселей в двух входных изображениях:
yu,v = f(x1:u,v, x2:u,v),

входное изображение

∀(u, v) ∈ X1, X2.

входное изображение

выходное изображение

Рис. 11.10. Диадические операции обработки изображений. Каждый выходной пиксель является функцией двух
соответствующих входных пикселей (показаны красным)

608 Глава 11 · Изображения и их обработка

Примерами полезных диадических операций могут служить
бинарные арифметические операторы, такие как сложение, вычитание, поэлементное умножение, или диадические функции,
встроенные в массивы NumPy, такие как max, min, atan2.
Объекты Image в Toolbox поддерживают перегруженные операторы Python
>>> church / 2

# новое изображение, все значения пикселей
# уменьшены вдвое
Image: 1280 x 851 (float32)
>>> church + 20
# новое изображение, все значения пикселей
# увеличены на 20
Image: 1280 x 851 (uint8)
>>> church - church # новое изображение, все значения пикселей равны 0
Image: 1280 x 851 (uint8)

Также к соответствующим парам пикселей можно применять
произвольные функции
>>> church.apply2(church, lambda x, y: x - y, vectorize=True).disp()

Это неэффективный способ вычисления разности значений
пикселей между изображением церкви и самим собой, потому
что функция будет вызвана свыше миллиона раз. Более эффективный подход – воспользоваться тем фактом, что исходное
изображение хранится в виде массива NumPy, поэтому
>>> church.apply2(church, lambda x, y: x - y).disp();

передаст массив изображений в лямбда-функцию и преобразует полученный массив NumPy в новый экземпляр Image.
При выполнении арифметических операций со значениями
типа uint8 в NumPy важно помнить некоторые важные тонкости. Продемонстрируем их на примере двух значений типа uint8
>>> а = нп.uint8(100)
100
>>> b = np.uint8(200)
200

и некоторых простых арифметических действий
>>> a + b
RuntimeWarning: overflow encountered in ubyte_scalars
44
>>> a - b
RuntimeWarning: overflow encountered in ubyte_scalars
156

NumPy выполняет модульную арифметику и все результаты
вычисляет по модулю 256, то есть при переполнении значения

Диадические операции 609

циклически переносятся от 255 к 0 или наоборот. Вот яркий
пример:
>>> -а
156

когда отрицание положительного целого числа дает положительное целое число без вывода предупреждения. Обычно эта
проблема решается преобразованием пикселей изображений
в целые числа со знаком большего размера или в значения
с плавающей точкой с помощью .to("int16") либо .to("float32")
соответственно.
При делении
>>> а / б
0.5

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

11.4.1

Применения

Проиллюстрируем диадические операции на двух примерах.

11.4.1.1 Применение: хромакей
Первым примером послужит хромакей (chroma keying) – метод,
обычно используемый на телевидении для наложения изображения человека на какой-либо фон, например диктора, рассказывающего прогноз погоды, на карту погоды. Объект снимается на синем или зеленом фоне, что позволяет легко, используя
только значения пикселей, отличить фон от объекта.
Итак, загрузим изображение предмета, снятого перед зеленым экраном (рис. 11.11а).
>>> foreground = Image.Read("greenscreen.png", dtype="float")
Image: 1024 x 768 (float32), R:G:B [.../images/greenscreen.png]

Вычислим координаты цветности, используя (10.9)
>>> cc = foreground.gamma_decode("sRGB").tristim2cc()
Image: 1024 x 768 (float32), r:g

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

610 Глава 11 · Изображения и их обработка

леного цветов соответственно. В данном случае для различения фоновых пикселей достаточно плоскости g изображения.
Его гистограмма
>>> cc.plane("g").hist().plot()

показанная на рис. 11.11б, указывает на большую совокупность
пикселей около 0.55, которая является фоном, и другую совокупность, принадлежащую объекту. Можно смело сказать, что
к предмету относится любой пиксель, для которого g < 0.45,
и создать маску изображения
>>> mask = cc.plane("g") < 0.45;
>>> mask.disp();

где пиксель является истинным (равным единице и отображается белым цветом), если он является частью объекта, как показано на рис. 11.11в.
Получаем изображение предмета без фона:
>>> (foreground * mask).disp();

как показано на рис. 11.11г. Умножение на логическое изображение, например mask, выполняет функцию попиксельного
переключения: результатом является значение пикселя переднего плана, если умножить на True, и ноль в противном случае.
В данном примере передний план имеет три плоскости, поэтому маска применяется к каждой из них, в результате чего получается изображение с тремя плоскостями.
Далее загружаем нужное фоновое изображение.
>>> background = Image.Read("road.png", dtype="float")\
...
.samesize(foreground)
Image: 1024 x 768 (float32), R:G:B

выполняем масштабирование и обрезку, чтобы оно было того
же размера, что и исходное изображение. Затем вычисляется
фон с вырезом для объекта
>>> (background * ~mask).disp();

как показано на рис. 11.11д. Оператор ~ инвертирует логическое изображение, меняя местами значения True и False. Наконец, добавим объект без фона к фону без объекта, чтобы получить объект, наложенный на фон
>>> composite = foreground * mask + background * ~mask;
>>> composite.disp();

как показано на рис. 11.11е. Этот метод, конечно, не сработает,
если объект содержит цвета, совпадающие с цветом фона. ►По-

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

цветность (g)

u (пиксели)

г

u (пиксели)

v (пиксели)

v (пиксели)

в

б

v (пиксели)

u (пиксели)

v (пиксели)

а

частота

v (пиксели)

Диадические операции 611

u (пиксели)
u (пиксели)
д
е
Рис. 11.11. Как работает хромакей: а) объект на зеленом фоне; б) гистограмма значений цветности зеленого;
в) вычисленное изображение маски, где true означает белый цвет; г) маска объекта; д) фон с наложенной маской
объекта; е) объект, наложенный на фоновую сцену (фото любезно предоставлено Фионой Корк)

следний шаг можно также выполнить вызовом метода choose,
осуществляющего попиксельное переключение
>>> background.choose(foreground, mask).disp();

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

612 Глава 11 · Изображения и их обработка

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

11.4.1.2 Применение: обнаружение движения
В следующем примере мы обработаем последовательность
изображений и оценим фон при наличии в сцене нескольких
движущихся объектов. Мы будем использовать рекурсивный
алгоритм, который обновляет предполагаемое фоновое изображение B̂ на каждом временном шаге на основе предыдущей
оценки и текущего изображения:
B̂〈k + 1〉 ← B̂〈k〉 + c(X〈k〉 - B̂〈k〉),
где k – шаг по времени, а c(·) – монадическая функция насыщения изображения

Для демонстрации откроем фильм, показывающий движение транспорта через перекресток:
>>> video = VideoFile("traffic_sequence.mp4", mono=True,
... dtype="float")
VideoFile(traffic_sequence.mp4) 704 x 576, 350 frames @ 30fps

и запустим основной цикл по кадрам:
>>> sigma = 0.02;
>>> background = None
>>> for im in video:
... if background is None:

Диадические операции 613
...
...
...
...
...

background = im # first frame only
else:
d = im - background
background += d.clip(-sigma, sigma)
background.disp(reuse=True, fps=video.fps)

v (пиксели)

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

u (пиксели)

б

v (пиксели)

а

в

u (пиксели)

Рис. 11.12. Пример обнаружения дорожного движения в кадре 200: а) текущее изображение; б) предполагаемое
фоновое изображение; в) разница между текущим и оценочным фоновым изображением, где белый цвет равен
нулю, красный и синий являются отрицательными и положительными значениями соответственно, а величина
обозначается интенсивностью цвета

614 Глава 11 · Изображения и их обработка

11.5

Пространственные операции

Пространственные операции схематично показаны на рис. 11.13.
Каждый пиксель выходного изображения является функцией
всех пикселей в области, окружающей соответствующий пиксель входного изображения:
yu,v = f(𝒲u,v),

∀(u, v) ∈ X,

где 𝒲u,v обозначает окно (закрашенная красным область на
рис. 11.13) – обычно это квадратная область размером w×w.
Число w обычно выбирается нечетным, для чего соответствующие методы в Toolbox принимают полуширину h ∈ ℕ такую,
что w = 2h + 1.

входное изображение

выходное изображение

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

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

11.5.1

Линейная пространственная фильтрация

Очень важным линейным пространственным оператором является корреляция:

Пространственные операции 615

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

где K – ядро, массив w×w,
элементы которого называются
коэффициентами фильтра. Для каждого выходного пикселя соответствующее окно пикселей входного изображения 𝒲 поэлементно умножается на ядро K. Центр окна и ядра определяются
координатами (0, 0) и i, j ∈ −h, ..., h ⊂ ℤ × ℤ. Эту операцию можно
рассматривать как взвешенную сумму пикселей в окне, где веса
определяются элементами ядра K. Корреляция часто записывается в операторной форме как
Y = K ⊗ X.

Тесно связанной операцией является свертка
(11.2)
где K – ядро свертки, массив w×w. Обратите внимание, что
в первом члене уравнения изменился знак индексов i и j по
сравнению с (11.1). Свертка часто записывается в операторной
форме как
Y = K ∗ X.

Как вы увидите далее, свертка – это рабочая лошадка обработки изображений, и для выполнения таких функций, как
сглаживание, вычисление градиента или обнаружение краев,
можно выбрать подходящее ядро K. В Toolbox свертка выполняется с помощью функции convolve:
>>> O = I.convolve(K);

Отступление 11.7. Корреляция или свертка?
Эти два термина часто используются слишком вольно и имеют
схожие, хотя и разные определения. Свертка – это пространственный эквивалент умножения в частотной области, а ядро
свертки – это импульсная характеристика фильтра в частотной
области. У свертки также есть много полезных математических
свойств, описанных в отступлении 11.8.
Разница в индексации между уравнениями 11.1 и 11.2 эквивалентна отражению ядра по горизонтали и вертикали вокруг
центральной точки. Многие ядра симметричны, и в этом случае
корреляция и свертка дают один и тот же результат. Однако обнаружение границ всегда основано на несимметричных ядрах,
поэтому мы должны позаботиться о применении свертки. Мы
будем использовать корреляцию только для сопоставления шаб­
лонов в разделе 11.5.2.

616 Глава 11 · Изображения и их обработка
Отступление 11.8. Свойства свертки
Свертка подчиняется знакомым правилам алгебры, она:
 коммутативная:
A ∗ B = B ∗ A;
 ассоциативная:
A ∗ B ∗ C = (A ∗ B) ∗ C = A ∗ (B ∗ C);
 распределительная (применяется суперпозиция):
A ∗ (B + C) = A ∗ B + A ∗ C;
 линейная:
A ∗ (αB) = α(A ∗ B)
– и инвариантна к сдвигу – пространственный эквивалент временной инвариантности при обработке одномерных сигналов,
т. е. результат операции, одинаков везде на изображении.

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

11.5.1.1 Сглаживание
Рассмотрим ядро свертки в виде квадратной мат­ри­цы 21×21,
содержащей одинаковые элементы:
>>> K = Image.Constant(21, 21, value=1/21**2);
>>> K.shape
(21, 21)

Матрица имеет единичный объем, т. е. сумма ее элементов
равна единице. Результатом свертки изображения с этим ядром
будет изображение, в котором каждый выходной пиксель является средним значением пикселей в соответствующей окрестности 21×21 во входном изображении. Как и следовало ожидать,
это усреднение
>>> mona = Image.Read("monalisa.png", mono=True, dtype="float")
Image: 677 x 700 (float32) [.../images/monalisa.png]
>>> mona.convolve(K).disp();

приводит к сглаживанию, размытию или расфокусировке, как
показано на рис. 11.14б.
Ядро также можно создать с помощью метода класса Kernel
>>> K = Kernel.Box(h=10);

который возвращает ядро, обычно называемое блочным фильт­
ром, в виде массива NumPy. Мы указали полуширину ядра
h = 10, а ширина будет равна w = 2h + 1, то есть гарантированно
нечетным числом.

Расфокусировка
включает в себя
ядро, которое
представляет собой
двухмерный паттерн Эйри или функцию sinc. Функция
Гаусса аналогична
по форме, но всегда
положительна, тогда
как паттерн Эйри
имеет отрицательные кольца с малой
амплитудой.

u (пиксели)

б

u (пиксели)

v (пиксели)

а

v (пиксели)

v (пиксели)

Пространственные операции 617

в

u (пиксели)

Рис. 11.14. Сглаживание: а) исходное изображение; б) сглаживание ядром усреднения 21×21;
в) сглаживание гауссовым ядром G(σ = 5) размером 31×31
Подробности
см. в приложении G.

Более подходящим ядром для сглаживания является двухмерная функция Гаусса
(11.3)
которая симметрична относительно начала координат, а объем
под кривой равен единице. Пиксели взвешиваются в зависимости от их расстояния от центра. Разброс гауссовой кривой определяется параметром стандартного отклонения σ. Применение
этого ядра к изображению
>>> K = Kernel.Gauss(sigma=5);
>>> mona.convolve(K).disp();

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

618 Глава 11 · Изображения и их обработка
>>> K.shape
(31, 31)

Сглаживания легко добиться с помощью метода smooth класса
Image:
>>> mona.smooth(sigma=5).disp();

Размытие – это нелогичная операция обработки, поскольку
мы обычно прилагаем много усилий, чтобы получить чистое
и четкое изображение. Намеренное ухудшение изображения
кажется на первый взгляд несколько безрассудным. Однако, как
вы увидите позже, сглаживание по Гауссу оказывается чрезвычайно полезным.
Ядро само по себе является мат­ри­цей, поэтому мы можем
отобразить его как изображение
>>> idisp(K);

как показано на рис. 11.15а. Мы видим большое значение в цент­
ре ядра, плавно спадающее во всех направлениях. Мы также можем отобразить ядро как поверхность
>>> span = np.arange(-15, 15 + 1);
>>> X, Y = np.meshgrid(span, span);
>>> plt.subplot(projection="3d").plot_surface(X, Y, K);

а

б

в

г
д
е
Рис. 11.15. Галерея часто используемых ядер свертки. h = 15, σ = 5: а) гауссиан как интенсивность; б) гауссиан;
в) цилиндр (r = 8); г) производная гауссиана (DoG); д) лапласиан гауссиана (LoG); е) разность гауссианов (DiffG)

Пространственные операции 619
Отступление 11.9. Насколько широк должен быть гауссиан?
При выборе гауссового ядра необходимо учитывать стандартное отклонение,
обычно определяемое задачей, и размеры ядра K, содержащего дискретную
функцию Гаусса. Время вычислений пропорционально w2, поэтому в идеале окно
должно быть не больше, чем нужно. Гауссиан монотонно убывает во всех направлениях, но никогда не достигает нуля. Поэтому мы выбираем полуширину
окна h так, чтобы значение гауссиана было меньше некоторого порога вне окна
свертки w×w.
2
На краю окна, на расстоянии h от центра, значение гауссиана будет e-h2 /2σ . При
−2
σ = 1 и h = 2 гауссиан будет иметь ширину e ≈ 0.14, при h = 3 ширина будет e−4.5
≈ 0.01, а при h = 4 будет e−8 ≈ 3.4×10−4. Если h не указано, Toolbox выбирает h = 3σ.
Для σ = 1 это соответствует окну с размерами 7 × 7, которое содержит все значения
гауссиана, превышающие 1 % от пикового значения.

Отступление 11.10. Разделимые фильтры
Некоторые ядра являются разделимыми, то есть тот же результат можно получить,
свернув каждую строку с одномерным ядром, а затем каждый столбец с другим
одномерным ядром. Общее число операций в этом случае сокращается до 2wN 2,
что лучше в w/2 раза. Фильтр разделим, если ядро представляет собой мат­ри­цу
ранга 1. Например, простой блочный фильтр
>>> K=Kernel.Box(h=1)
array([[ 0.1111, 0.1111, 0.1111],
[ 0.1111, 0.1111, 0.1111],
[ 0.1111, 0.1111, 0.1111]])
>>> np.linalg.matrix_rank(K)
1
разделим. Одномерные ядра получаются путем сингулярного разложения
>>> U, s, Vh = np.linalg.svd(K, full_matrices=True)
>>> Kh = s[0] * U[:, 0] # одномерное горизонтальное ядро
array([ -0.1925, -0.1925, -0.1925])
>>> Kv = Vh[0, :]
# одномерное вертикальное ядро
array([ -0.5774, -0.5774, -0.5774])
Для перекрестной проверки можно свернуть два одномерных ядра, используя
внешнее произведение, чтобы сформировать эквивалентное двумерное ядро
>>> np.outer(Kh, Kv)
array([[ 0.1111, 0.1111,
[ 0.1111, 0.1111,
[ 0.1111, 0.1111,

0.1111],
0.1111],
0.1111]])

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

620 Глава 11 · Изображения и их обработка

как показано на рис. 11.15б. Грубым приближением к гауссиану является цилиндрическое ядро, которое представляет собой
цилиндр с вертикальными сторонами, а не плавное изменение
амплитуды
>>> K = Kernel.Circle(radius=8, h=15);

как показано на рис. 11.15в. Аргументы задают радиус 8 пикселей в пределах окна половинной ширины h = 15.
Отступление 11.11. Свойства гауссиана
Функция Гаусса G(·) обладает некоторыми особыми свойствами.
Свертка двух гауссианов – это еще один гауссиан:

Для случая, когда σ1 = σ2 = σ:

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

11.5.1.2 Экстраполяция за границы
Затруднения со всеми пространственными операциями возникают, когда окно находится близко к краю входного изображения, как показано на рис. 11.16. В этом случае выходной пиксель
является функцией окна, которое охватывает пиксели за преде­
лами входного изображения – эти пиксели не имеют определенного значения.
Есть несколько распространенных способов экстраполяции значений пикселей за пределы границ изображения, выбор которых определяется необязательным аргументом border
метода convolve. Во-первых, можно предположить, что пиксели за пределами изображения имеют определенное значение.
Во-первых, можно выбрать ноль, но в таком случае происходит
затемнение выходных пикселей на расстоянии h от границы.
Во-вторых, можно предположить, что последний пиксель повторяется, и повторяется за границей изображения. В-третьих,
можно считать, что граница подобна зеркалу, и изображение
отражается от нее – это поведение по умолчанию, реализованное методом convolve.

Пространственные операции 621

Другой вариант – считать результат недействительным, когда
окно пересекает границу изображения. Недопустимые выходные пиксели показаны штриховкой на рис. 11.16. В результате
получается выходное изображение размером (W - 2h) × (H - 2h),
которое немного меньше входного изображения. Это поведение можно задать, передав в convolve параметр border="valid".

входное изображение

выходное изображение

Рис. 11.16. В случае когда окно 𝒲 выходит за край входного изображения, выходной пиксель
в точке (u, v) не определен. Заштрихованные пиксели в выходном изображении – это все те
пиксели, для которых выходное значение функции не определено

11.5.1.3 Определение края
Часто нас интересуют края объектов в сцене. Возьмем изображение
>>> castle = Image.Read("castle.png", mono=True, dtype="float")
Image: 1280 x 960 (float32) [.../images/castle.png]

показанное на рис. 11.17а. Весьма полезно было бы рассмотреть
значения пикселей вдоль одномерного профиля, проходящего
через все изображение. Горизонтальный профиль изображения
при v = 360
>>> profile = castle.image[360, :];
>>> profile.shape
(1280,)

является вектором, который можно нарисовать
>>> plt.plot(profile);

относительно горизонтальной координаты u на рис. 11.17б. Отчетливые высокие шипы соответствуют белым буквам и другим
обозначениям на знаке. При более внимательном рассмотрении одного из пиков на рис. 11.17в мы видим профиль интенсивности поперек вертикального стержня буквы Т. Интенсивность фона ≈ 0.3, а интенсивность яркого света ≈ 0.9, но эти

v (пиксели)

Уровень серого

622 Глава 11 · Изображения и их обработка

в

б

u (пиксели)

Производная уровня серого

u (пиксели)

Уровень серого

а

u (пиксели)

г

u (пиксели)

Рис. 11.17. Профиль интенсивности на краях: а) исходное изображение; б) профиль уровня серого вдоль
горизонтальной линии v = 360; в) крупный план всплеска при u ≈ 580; г) производная от c (изображение из набора
данных ICDAR 2005 OCR; Lucas, 2005)

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

Ее можно вычислить с помощью функции diff из NumPy
>>> plt.plot(np.diff(profile));

и построить график, как показано на рис. 11.17г. Сигнал в целом
нулевой с четкими ненулевыми откликами на краях объекта,
в данном случае на краях стержня буквы Т.
Производную в точке u также можно записать как симмет­
ричную разность первого порядка:

Пространственные операции 623

где члены упорядочены так, чтобы соответствовать увеличению координаты u пикселя в направлении слева направо. Это
эквивалентно скалярному произведению, или корреляции с одномерным ядром
Kcorr = (-½ 0 ½),
или свертке с ядром
K = (½ 0

-½).

Это отличие обусловлено разницей в индексации между
(11.1) и (11.2). Свертка изображения с этим ядром
>>> K = [0.5, 0, -0.5];
>>> castle.convolve(K).disp(colormap="signed");

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

дает результат, очень похожий на показанный на рис. 11.18а, на
котором отчетливо видны вертикальные края – высокие горизонтальные градиенты. Поскольку это ядро имеет значения со
знаком, результат свертки тоже будет со знаком, т. е. градиент
в пикселе может быть положительным или отрицательным.
Для вычисления горизонтального градиента было предложено множество ядер свертки. Популярным выбором является
ядро Собеля ◄
>>> Du = Kernel.Sobel()
array([[ 0.125,
0,
[
0.25,
0,
[ 0.125,
0,

-0.125],
-0.25],
-0.125]])

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

Фильтры могут
реагировать на края
под любым произвольным углом. >>> castle.convolve(Du).disp(colormap="signed");
Само ядро Собеля
можно рассматри- генерирует горизонтальное градиентное изображение, покавать как изображение и вращать с по- занное на рис. 11.18а, которое выделяет вертикальные края.
мощью Image(Du). Вертикальный градиент вычисляется с использованием трансrotate(). Для понирования ядра
получения угловой
точности обычно >>> castle.convolve(Du.T).disp(colormap="signed");
требуется большее
ядро, например сгекак видим на рис. 11.18б.
нерированное (11.4) и выделяет горизонтальные ребра,
функцией kdgauss. Обозначения, используемые для градиентов, значительно раз-

v (пиксели)

v (пиксели)

624 Глава 11 · Изображения и их обработка

а

б

u (пиксели)

г

u (пиксели)

v (пиксели)

v (пиксели)

u (пиксели)

в

u (пиксели)

Рис. 11.18. Краевой градиент: а) градиент в направлении u, вычислен с применением одномерного ядра;
б) градиент в направлении v, вычислен с применением ядра Собеля; в) производная величины градиента;
г) производная гауссова направления. Синим цветом показаны положительные градиенты, красным –
отрицательные, белым – нулевые

личаются в литературе. Чаще всего горизонтальный и вертикальный градиенты обозначают соответственно как ∂X/∂u, ∂X/∂v;
∇u X, ∇v X или Xu, Xv. В операторной форме это записывается так:
Xu = Du ∗ X,

Xv = DvT ∗ X,

где D – производное ядро, такое как ядро Собеля.
Взятие производной сигнала усиливает высокочастотный
шум, а все изображения имеют шум, как было сказано в отступлении 11.3. На уровне пикселей шум представляет собой
стационарный случайный процесс – значения не коррелируют
между пикселями. Однако интересующие нас признаки, такие
как края, имеют коррелированные изменения значения пикселя в большем пространственном масштабе, как показано на
рис. 11.17в. Мы можем уменьшить влияние шума, сгладив изображение, прежде чем брать производную
Xu = Du ∗ (G(σ) ∗ X).

Пространственные операции 625

Вместо свертки изображения с помощью гауссовой кривой,
а затем производной мы используем свойство ассоциативности свертки, чтобы записать:
Xu = Du ∗ (G(σ) ∗ X) = (Du ∗ (G(σ)) ∗ X).

Мы получаем гладкий градиент путем свертки изображения
с использованием производной гауссиана (derivative of the Gaus­
sian, DoG), которую можно получить численно, свернув ядра Собеля и гауссианы
>>>
>>>
>>>
(9,

from scipy.signal import convolve2d
Gu = convolve2d(Du, Kernel.Gauss(sigma=1));
Gu.shape
9)

или аналитически, взяв производную в направлении u от уравнения Гаусса (11.3) и получив уравнение
(11.4)
которое вычисляется методом класса Kernel.DGauss и показано
на рис. 11.15г.
Стандартное отклонение σ управляет масштабом обнаруженных краев. При большом σ, что предполагает повышенное
сглаживание, края из-за тонкой текстуры будут ослаблены,

Отступление 11.12. Карл Фридрих Гаусс
Гаусс (1777–1855) был немецким математиком, внесшим крупный вклад в такие области, как теория чисел,
дифференциальная геометрия, магнетизм, астрономия
и оптика. Он был вундеркиндом, родился в Брауншвейге, Германия, и был единственным сыном необразованных родителей. В возрасте трех лет он исправил в уме
финансовую ошибку, допущенную его отцом, а в подростковом возрасте сделал свои первые математические открытия. Гаусс был перфекционистом и трудолюбивым, но не плодовитым писателем. Он отказывался
публиковать что-либо, что не считал полным и не выдерживающим критики. Существует мнение, что математика могла бы продвинуться вперед на 50 лет, если бы он вовремя опубликовал все свои открытия. Согласно легенде, Гаусса прервали посреди работы
и сказали, что его жена умирает, на что он ответил: «Скажите ей подождать,
пока я закончу».
Любопытно, но нормальное распределение, или функция Гаусса, не является
одним из его достижений. Впервые нормальное распределение было открыто де
Муавром в 1733 г., а затем Лапласом в 1778 г. В честь Гаусса названа единица СИ
для плотности магнитного потока.

626 Глава 11 · Изображения и их обработка

останутся только края крупных элементов. Эта способность
находить ребра в разных пространственных масштабах важна
и лежит в основе концепции масштабного пространства, которую мы обсудим в разделе 12.3.2. Другая интерпретация этого
оператора – пространственный полосовой фильтр (bandpass filter), поскольку он представляет собой каскад фильтра низких
частот (сглаживающий) с фильтром высоких частот (дифференцирующий).
Вычисление горизонтальной и вертикальной составляющих
градиента в каждом пикселе
>>> Xu = castle.convolve(Kernel.DGauss(sigma=2));
>>> Xv = castle.convolve(Kernel.DGauss(sigma=2).T);

позволяет нам вычислить величину градиента в каждом пикселе
>>> m = (Xu ** 2 + Xv ** 2).sqrt()
Image: 1280 x 960 (float32)

Это изображение с усиленными краями на рис. 11.18в показывает края очень отчетливо. Направление градиента в каждом
пикселе равно
>>> th = Xv.apply2(Xu, np.arctan2); # arctan2(Xv, Xu)

и лучше всего видно на разреженном динамическом графике
>>> plt.quiver(castle.uspan(20), castle.vspan(20),
... Iu.image[::20, ::20], Iv.image[::20, ::20], scale=10);

как показано на рис. 11.18г. График краевых направлений намного более зашумлен, чем график амплитуды. Там, где градиент края сильный, на границе знака или краях букв, направление перпендикулярно краю, но мелкомасштабная текстура
наподобие кирпичной кладки выглядит как почти случайное
направление края.
Градиентные изображения можно легко вычислить с по­
мощью метода:
>>> Xu, Xv = castle.gradients(Kernel.DGauss(sigma=2))

где аргумент переопределяет ядро Собеля по умолчанию.
Хорошо известным и очень эффективным детектором краев
является оператор края Кэнни. Он использует величину и направление края, которые мы только что вычислили, и выполняет два дополнительных шага. Во-первых, это подавление нелокальных максимумов. Рассмотрим изображение величины
градиента на рис. 11.18в как трехмерную поверхность, высота
которой пропорциональна яркости, как показано на рис. 11.19.
Мы видим серию холмов и гребней, и нам нужно найти пиксе-

Пространственные операции 627

на края

Величи
Рис. 11.19. Крупный план величины
градиента вокруг буквы T в виде
трехмерной поверхности

ли, лежащие вдоль линий гребней. Исследуя значения пикселей
в локальной окрестности, перпендикулярной направлению края,
т. е. в направлении градиента края, можно найти максимальное
значение и установить все остальные пиксели равными нулю.
Результатом является набор ненулевых пикселей, соответствующих пикам и линиям хребта. Второй шаг – пороговое значение гистерезиса. Для каждого ненулевого пикселя, превышающего верхний порог, создается цепочка из соседних пикселей,
превышающих нижний порог. Любые другие пиксели устанавливаются равными нулю.
Применить оператор Кэнни к нашему примерному изображению очень просто:
>>> edges = castle.canny()
Image: 1280 x 960 (uint8)

Этот вызов возвращает изображение, края которого отмечены ненулевыми значениями интенсивности, соответствующими величине градиента в этом пикселе, как показано на
рис. 11.20a. Мы видим, что края намного тоньше, чем для величины производной оператора Гаусса, показанной на рис. 11.20б.
При помощи дополнительных аргументов можно настроить параметры порога гистерезиса.
До сих пор мы рассматривали ребро как точку с высоким градиентом, а подавление нелокальных максимумов использовалось для поиска максимального значения в локальных окрестностях. Альтернативный способ найти точку максимального
градиента – вычислить вторую производную и определить, где
она равна нулю. Оператор Лапласа

v (пиксели)

v (пиксели)

628 Глава 11 · Изображения и их обработка

u (пиксели)
u (пиксели)
а
б
Рис. 11.20. Сравнение двух пограничных операторов: а) оператор Кэнни с параметрами по
умолчанию; б) величина производной ядра Гаусса (σ = 2). Оператор |DoG| требует меньше
вычислений, чем оператор Кэнни, но генерирует более толстые края. Для обоих случаев
результаты показаны инвертированными, белый цвет равен нулю

(11.5)
представляет собой сумму второй пространственной производной в горизонтальном и вертикальном направлениях. Это
изотропный фильтр, который одинаково реагирует на края
в любом направлении.
Для дискретного изображения его можно вычислить путем
свертки с ядром Лапласа:
>>> L = Kernel.Laplace()
array([[ 0, 1, 0],
[ 1, -4, 1],
[ 0, 1, 0]])

Отступление 11.13. Пьер-Симон Лаплас
Лаплас (1749–1827) был французским математиком
и астрономом, который объединил теории математической астрономии в своем пятитомном Mécanique
Céleste («Небесная механика»). Когда он был подростком, его математические способности впечатлили
Даламбера, который помог ему получить профессорскую должность. На вопрос Наполеона, почему он не
упомянул Бога в своей книге по астрономии, он якобы сказал: «Je n’avais pas besoin de cette hypothèse-là»
(«Мне не нужна эта гипотеза»). Он стал графом Империи в 1806 г., а впоследствии маркизом.
Его именем названы оператор Лапласа (дифференциальный оператор второго порядка) и преобразование Лапласа.

Пространственные операции 629

Вторая производная даже более чувствительна к шуму, чем
первая, и поэтому тоже часто используется в сочетании со сглаживанием изображения по Гауссу
(11.6)
которое мы объединяем в лапласиан ядра Гаусса (LoG), где L –
это ядро Лапласа, рассмотренное выше. Аналитически это можно записать так:
(11.7)
(11.8)
Это так называемый оператор Марра–Хилдрета, или ядро
«мексиканской шляпы», показанное на рис. 11.15д.
Применим это ядро к нашему изображению:
>>> lap = castle.convolve(Kernel.LoG(sigma=2));

Результат показан на рис. 11.21a,б. Максимальный градиент
возникает, когда вторая производная равна нулю, но значимый
край представляет собой переход через ноль от сильного положительного значения (синий) к сильному отрицательному
(красный). Рассмотрим увеличенный вид лапласиана буквы T,
показанный на рис. 11.21б. Сгенерируем горизонтальное сечение стержня буквы Т при v = 360
>>> profile = lap.image[360, 570:601];
>>> plt.plot(np.arange(570, 601), profile, "-o");

которое показано на рис. 11.21в. Мы видим, что нулевые значения второй производной лежат между пикселями. Детектор
пересечения нуля выбирает пиксели, соседние с точками перехода через ноль
>>> zc = lap.zerocross();

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

u (пиксели)

в

б

u (пиксели)

v (пиксели)

|Лапласиан| в v = 360

а

v (пиксели)

v (пиксели)

630 Глава 11 · Изображения и их обработка

u (пиксели)

г

u (пиксели)

Рис. 11.21. а) Лапласиан гауссиана; б) крупный план части (a) вокруг буквы T, где синий и красный цвета обозначают
положительные и отрицательные значения соответственно; в) горизонтальное поперечное сечение LoG через
стержень T; г) крупный план выхода детектора перехода через ноль в букве Т

Отступление 11.14. Разность гауссианов
Лапласиан гауссиана можно аппроксимировать разностью двух гауссовых функций:

где σ1 > σ2 и обычно σ1 = 1.6σ2. Разность вычисляется методом класса Kernel.DoG
и показана на рис. 11.14е.
Это приближение полезно при работе с последовательностями масштабного
пространства, которые будут обсуждаться в разделе 12.3.2. Рассмотрим последовательность изображений X〈k+1〉 = G(σ) ⊗ X〈k〉, т. е. изображения сглажены по нарастающей. В таком случае разница между любыми двумя изображениями в последовательности эквивалентна значению функции DoG( 2σ, σ), применяемой
к исходному изображению.

Пространственные операции 631
 Края изображений не обязательно совпадают
с краями объектов
Фундаментальное ограничение всех подходов к обнаружению
краев заключается в том, что края интенсивности не обязательно
очерчивают границы объектов. Объект может иметь плохой контраст с фоном, что приводит к слабым граничным краям. И наоборот, на объекте может быть полоса, которая не является его краем.
Тени нередко имеют очень резкие края, но не являются реальными
объектами. Текстура объекта часто дает сильный выходной сигнал
детектора краев не только в точках на его границе, как, например,
в случае с кирпичами на рис. 11.17б.

11.5.2

Сравнение с шаблоном

До сих пор в нашем обсуждении мы использовали ядра, которые представляют математические функции, такие как гауссиан и его производная и ее лапласиан. Мы также рассмотрели
ядро свертки как мат­ри­цу, как изображение и как трехмерную
поверхность (рис. 11.15). В этом разделе мы будем считать, что
ядро – это изображение или часть изображения – шаблон, сопоставляя с которым, мы ставим цель найти части входного изображения, больше всего похожие на него.
Схема сравнения с шаблоном показана на рис. 11.22. Каждый
пиксель в выходном изображении определяется как
yu,v = s(𝒲u,v, T),

∀(u, v) ∈ X,

где 𝒲u,v – окно w×w с центром в точке (u, v) во входном изображении, используемое как для свертки, так и для корреляции,
а T – шаблон w×w пикселей, который мы ищем, где w – нечетное
число.

входное изображение

выходное изображение

Рис. 11.22. Сопоставление с шаблоном. Результатом определения сходства s(·) между
областью 𝒲, заштрихованной красным, и шаблоном T является пиксель (показан красным)

632 Глава 11 · Изображения и их обработка

Функция s(A, B) является скалярной мерой, описывающей сходство двух одинаковых по размеру изображений A
и B. Перечень распространенных мер сходства
приведен
в табл. 11.1. Наиболее очевидные меры вычисляются просто
как попиксельная разность A - B с последующим суммированием абсолютных значений этих разностей (sum of the absolute
differences, SAD) или их квадратов (sum of the squared differences, SSD). Эти меры равны нулю, если изображения идентичны, и увеличиваются при непохожести. Сложно сказать, какое
значение меры лучше всего отражает плохое совпадение, но
для определения наилучшего совпадения можно использовать
ранжирование мер подобия.
Таблица 11.1. Меры подобия для двух областей изображения, A и B,
с одинаковыми размерами. Префикс Z указывает, что мера учитывает
смещение нуля или разницу в среднем значении двух изображений (Banks
– –
and Corke 2001). A и B – средние значения областей A и B изображения
соответственно. Имена методов класса Image в последнем столбце
Сумма абсолютных разностей
SAD
s = Σ(u,v)∈A,B|au,v - bu,v|
–
–
ZSAD
s = Σ(u,v)∈A,B|(au,v - A) - (bu,v - B)|

sad
zsad

Сумма квадратов разностей
SSD
s = Σ(u,v)∈A,B(au,v - bu,v)2
–
–
ZSSD
s = Σ(u,v)∈A,B((au,v - A) - (bu,v - B))2

ssd
zssd

Перекрестная корреляция
Σ(u,v)∈A,B au,v bu,v
NCC
s=
Σ(u,v)∈A a2u,v · Σ(u,v)∈B b2u,v
Σ(u,v)∈A,B(au,v - A–) - (bu,v - –B)
ZNCC
s=
–
–
Σ(u,v)∈A (au,v - A)2 · Σ(u,v)∈B (bu,v - B)2

ncc
zncc

Более сложные меры, такие как нормализованная взаимная
корреляция, дают оценку в интервале [-1, +1], где +1 соответствует идентичным областям. На практике хорошим совпадением считается значение больше 0.8. Нормализованная взаимная корреляция требует больших вычислительных ресурсов,
поскольку использует операции умножения, деления и извлечения квадратного корня. Нужно отметить, что результат может
быть неопределенным, если знаменатель равен нулю, что происходит, если элементы A или B идентичны.
Для иллюстрации используем в качестве шаблона фрагмент
51×51 с глазом Моны Лизы
>>> mona = Image.Read("monalisa.png", mono=True, dtype="float");
>>> A = mona.roi([170, 220, 245, 295]);

и оценим три распространенных показателя. Если A ≡ B, то легко показать, что SAD = SSD = 0 и NCC = 1, что указывает на идеальное совпадение:

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

Пространственные операции 633
>>>
>>>
0.0
>>>
0.0
>>>
1.0

B = A
A.sad(B)
A.ssd(B)
A.ncc(B)

Теперь рассмотрим случай, когда два изображения относятся
к одной и той же сцене, но одно изображение темнее другого –
изменилось освещение или экспозиция камеры. В этом случае
B = αA, и теперь
>>> B = 0.9 * A
>>> A.sad(B)
24.428635
>>> A.ssd(B)
0.2672252

эти меры указывают на степень несходства. Однако нормированная взаимная корреляция
>>> A.ncc(B)
1.0

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

инвариантна к изменению интенсивности.
Далее допустим, что значения пикселей имеют смещение
такое, что B = A + β, и мы находим, что
>>> B = A + 0.1
>>> A.sad(B)
260.1
>>> A.ssd(B)
26.010002
>>> A.ncc(B)
0.9817292

все меры теперь указывают на степень несходства. Проблемное
смещение можно устранить, сначала вычитая из каждого элемента A и B их средние значения:
>>> A.zsad(B)
2.5782734e-05
>>> A.zssd(B)
2.8106684e-13
>>> A.zncc(B)
1.0

и теперь все эти меры указывают на идеальное совпадение.
Префикс z обозначает разновидности мер подобия, описанных выше, которые инвариантны к смещению интенсивности.
Только мера ZNCC

634 Глава 11 · Изображения и их обработка
>>> B = 0.9 * A + 0.1
>>> A.zncc(B)
1.0000001

инвариантна как к изменению усиления, так и к изменению
смещения.
Все эти методы потерпят неудачу, если изображения имеют даже небольшое изменение угла поворота или
масштаба.
Отступление 11.15. Дэвид Марр
Марр (1945–1980) был британским нейробиологом и психологом,
который объединил достижения психологии, искусственного интеллекта и нейрофизиологии в новую дисциплину вычислительной нейронауки. Он изучал математику в Тринити-колледже
в Кембридже и защитил докторскую диссертацию по физиологии, посвященную моделированию функции мозжечка. Его достижения были опубликованы в трех журнальных статьях в период с 1969 по 1971 г. и легли в основу теории функционирования
мозга млекопитающих, большая часть которой остается актуальной и сегодня. В 1973 г. он был приглашенным научным сотрудником в Лаборатории искусственного интеллекта Массачусетского технологического института, а позже стал профессором
кафедры психологии. Его внимание переключилось на изучение
зрения и, в частности, так называемой ранней зрительной системы. Он предложил три уровня системного анализа, применимых
к биологическим и инженерным системам: вычислительный (что
делает система), алгоритмический (как система делает то, что она
делает, и какие представления при этом использует) и реализа­
ционный (как система физически реализована). Он также ввел
термин «первичный набросок», который обозначает низкоуровневые признаки, которые можно извлечь из сцены.
Умер от лейкемии в возрасте 35 лет, и вскоре после его смерти
была опубликована книга «Vision: A computational investigation
into the human representation and processing of visual information»
(Marr, 2010).

Обратите внимание,
что полученный
результат > 1, хотя
он должен лежать
в интервале [–1, 1].
Это обусловлено накоплением ошибки
в тысячах арифметических операций,
выполняемых для
вычисления данного
значения.

11.5.2.1 Применение: задача поиска Уолли
Рассмотрим задачу из известной детской книги «Где Уолли?»
или «Где Вальдо?» – было бы интересно попытаться найти лицо
Уолли в толпе:
>>> crowd = Image.Read("wheres-wally.png", mono=True, dtype="float")
Image: 640 x 640 (float32) [.../images/wheres-wally.png]
>>> crowd.disp();

К счастью, мы приблизительно знаем, как он выглядит,
а шаб­лон

Из-за законов об
авторском праве
я не могу привести
в книге рассмат­
риваемые далее
изображения, однако они включены
в пакет Toolbox.

Пространственные операции 635
>>> T = Image.Read("wally.png", mono=True, dtype="float")
Image: 21 x 25 (float32) [.../images/wally.png]
>>> T.disp();

был извлечен из другого изображения и масштабирован так,
чтобы голова была примерно такой же ширины, как и другие
головы в сцене толпы (около 21 пикселя в ширину).
Сходство нашего шаблона T с каждым возможным расположением окна вычисляется по формуле
>>> sim = crowd.similarity(T, "zncc")
Image: 620 x 616 (float32)

с использованием меры сходства ZNCC. Результат
>>> sim.disp(colormap="signed", colorbar=True);

показан на рис. 11.23, где цвет пикселя указывает на сходство
ZNCC в соответствии со шкалой справа. Мы можем видеть несколько точек высокого сходства (темно-синие), которые являются кандидатами на позиции Уолли. Пиковые значения по отношению к локальному окну 3×3 равны:
>>> maxima, location = sim.peak2d(scale=2, npeaks=5)
>>> maxima
array([ 0.5258,
0.523, 0.5222, 0.5032, 0.5023],
dtype=float32)

v (пиксели)

в порядке убывания. Первый аргумент задает полуширину окна
h = 2, в котором допустимый пик должен быть больше всех своих
соседей, а второй аргумент – количество возвращаемых пиков.
Наибольшее значение 0.5258 – это сходство наиболее сильного
найденного совпадения.

u (пиксели)

Рис. 11.23. Изображение сходства S с отмеченными пятью лучшими кандидатами
на совпадение с Уолли. Полоса справа – это шкала подобия

636 Глава 11 · Изображения и их обработка

Эти совпадения обнаруживаются в координатах (u, v), заданных вторым возвращаемым значением
>>> location
array([[250, 8, 590, 397, 289],
[364, 596, 568, 417, 551]])

И мы можем выделить эти точки на сцене
>>> crowd.disp();
>>> plot_circle(centre=location, radius=20, color="k");
>>> plot_point(location, color="none", marker="none", text=" #{}");

используя последовательно кружки с номерами. Лучшее совпадение (261, 377) на самом деле является правильным ответом – мы нашли Уолли! Интересно взглянуть на других предпочтительных кандидатов. Номера два и три в нижней части
изображения – это люди в бейсболках, которые очень похожи
друг на друга.
На основании этого примера можно отметить несколько
важных моментов. Изображения имеют довольно низкое разрешение, а шаблон всего 21×25 – очень грубое подобие Уолли.
Совпадение слабое – всего 0.5258 по сравнению с максимально возможным значением 1.0, и есть несколько сопутствующих
факторов. Показатель сходства не является инвариантным по
отношению к масштабу, т. е. при изменении относительного
масштаба (увеличения) показатель сходства довольно быстро
падает. На практике допустимо расхождение масштаба между
T и 𝒲 на 10–20 %. Для этого примера шаблон был масштабирован лишь приблизительно. Во-вторых, не все Уолли одинаковы.
Уолли в шаблоне смотрит вперед, но Уолли, которого мы нашли
на изображении, смотрит влево. Другая проблема – квадратный
шаблон обычно включает не только интересующий объект, но
и пиксели фона. По мере перемещения объекта фоновые пиксели могут меняться, что приводит к снижению оценки сходства. Это известная проблема смешанных пикселей, которая
обсуждается в следующем разделе. В идеале шаблон должен как
можно точнее описывать интересующий объект. На практике
возникает еще одна проблема из-за искажения перспективы.
Квадратный участок пикселей в центре изображения будет казаться трапецеидальным на краю изображения и, таким образом, будет хуже совпадать с квадратным шаблоном.
Распространенная проблема с сопоставлением шаблонов
заключается в том, что могут возникать ложные совпадения.
В приведенном выше примере второй кандидат имел показатель сходства всего на 0.5 % ниже, чем первый, пятый кандидат
был только на 5 % ниже. На практике, прежде чем принять сходство за совпадение, применяется ряд правил: сходство должно

Пространственные операции 637

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

11.5.2.2 Непараметрические локальные преобразования

Для 32-битного
целого числа uint32
это ограничивает
окно до размера
5×5, если только
не используется
разреженное
отображение
(Humenberger et al.
2009). 64-битное
целое число uint64
допускает окно размером 7×7.

Непараметрические меры сходства более устойчивы к проб­
леме смешанных пикселей, и перед сравнением можно применить локальное преобразование к изображению и шаблону.
Двумя распространенными представителями этого класса являются последовательное (census transform) и ранговое (rank
transform) преобразования.
Последовательное преобразование отображает значения
пикселей из локальной области в целое число, рассматриваемое
как битовая строка – каждый бит соответствует одному пикселю в области, как показано на рис. 11.24. Если пиксель больше
цент­рального пикселя, его соответствующий бит устанавливается в единицу, в противном случае он равен нулю. В случае
окна w×w строка будет иметь длину w2 − 1 бит. Две строки
битов сравниваются с использованием расстояния Хэмминга,
которое представляет собой количество различающихся битов. Его можно вычислить, просто подсчитав количество установленных битов после операции исключающего ИЛИ между
двумя битовыми строками. Благодаря этому требуется очень
мало арифметических операций по сравнению с более традиционными методами – без квадратных корней или деления, –
и такие алгоритмы поддаются реализации в оборудовании
специального назначения или ПЛИС. Другое преимущество
заключается в том, что интенсивность считается относительно
центрального пикселя окна, что делает метод инвариантным

638 Глава 11 · Изображения и их обработка

к общим изменениям интенсивности или постепенным градиентам интенсивности.

Рис. 11.24. Пример последовательного и рангового преобразования для окна 3×3. Пиксели
помечаются красным или синим цветом, если они меньше или больше либо равны
центральному пикселю соответственно. Эти логические значения затем упаковываются
в двоичное слово в указанном направлении, от младшего бита к старшему. Значение
последовательного преобразования – 101011012, или десятичное число 173. Значение
рангового преобразования – это сумма всех битов, которая в данном случае равна 5

Ранговое преобразование сопоставляет значения пикселей
в локальной области со скаляром, который представляет собой
количество элементов в области, превышающих значение цент­
рального пикселя. Этот показатель характеризует сущность области, окружающей центральный пиксель, и, как и последовательное преобразование, инвариантен к общим изменениям
интенсивности, поскольку основан на локальных относительных значениях шкалы серого.
Эти преобразования реализуются методами censusxform
и rankxform класса Image. Их можно использовать в качестве этапа
предварительной обработки, применяемой к каждому из изображений перед использованием простой классической меры
подобия, такой как SAD.

11.5.3

Нелинейные операции

Другой класс пространственных операций основан на нелинейных пиксельных функциях внутри окна. Например, функция
>>> out = mona.window(np.var, h=3);

вычислит дисперсию пикселей в каждом окне 7×7. Аргументы
задают половинный размер окна и функцию, в данном случае
функцию NumPy для вычисления дисперсии. Функция вызывается с векторным аргументом 49×1, содержащим пиксели

Пространственные операции 639

в окне, расположенные в виде вектора-столбца, и возвращаемое значение функции становится соответствующим значением выходного пикселя. Эта операция действует как детектор
границ, так как она имеет низкое значение для однородных областей независимо от их яркости. Однако это требует больших
вычислительных ресурсов, поскольку функция вызывается более 470 000 раз.
Ранговые фильтры сортируют пиксели в окне по значению
и возвращают указанный элемент из отсортированного списка.
Максимальное значение в окне 5×5 для каждого пикселя – это
пиксель с наивысшим рангом в окне
>>> mx = mona.rank(rank=0, h=2);

где аргументами являются ранг и полуширина окна. Медиана
по окну 5×5 является 12-й по рангу
>>> med = mona.rank(rank=12, h=2);

и полезна в качестве фильтра для удаления шумов импульсного типа, а для изображения Моны Лизы это значительно уменьшает влияние мелких трещинок на поверхности картины. Более впечатляющая демонстрация заключается в добавлении
значительного импульсного шума к копии изображения Моны
Лизы:
>>> spotty = mona.copy()
Image: 677 x 700 (float32) [.../images/monalisa.png]
>>> pixels = spotty.view1d(); # Создать одномерное представление NumPy
>>> npix = mona.npixels
# общее количество пикселей
473900
>>> # choose 10,000 unique pixels
>>> k = np.random.choice(npix, 10_000, replace=True);
>>> pixels[k[:5_000]] = 0 # обнулить половину из них
>>> pixels[k[5_000:]] = 1 # другой половине присвоить значений 1
>>> spotty.disp();

как показано на рис. 4.25а. Мы установили 5000 случайных пикселей равными нулю, а еще 5000 случайных пикселей – равными максимальному значению. Этот тип шума часто называют
импульсным шумом, или шумом «соли и перца». А теперь применяем медианный фильтр 3×3
>>> spotty.rank(rank=4, h=1).disp();

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

v (пиксели)

v (пиксели)

640 Глава 11 · Изображения и их обработка

u (пиксели)
u (пиксели)
а
б
Рис. 11.25. Медианный фильтр очищает изображение от импульсного шума: а) изображение,
искаженное шумом; б) результат применения медианного фильтра

Но окно можно задать не только его половинной шириной, но
также в виде логического двумерного массива, который иног­да
называют контуром:
>>> M = np.full((3, 3), True);
>>> M[1, 1] = False
>>> M
array([[ True, True, True],
[ True, False, True],
[ True, True, True]])

в данном примере тоже с размерами 3×3
>>> max_neighbors = mona.rank(rank=0, footprint=M);

Возвращаемое значение – это первое по рангу (максимальное)
подмножество пикселей из окна, соответствующего ненулевым
элементам M. В данном случае M задает восемь соседних пикселей,
но не центральный пиксель. Мы можем использовать функцию
>>> (mona > max_neighbors).disp();

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

11.6

Математическая морфология

Математическая морфология – это класс нелинейных пространственных операторов, схематично показанных на рис. 11.26.

Именно так работает
метод peak2d.

Математическая морфология 641

Каждый пиксель в выходной мат­ри­це является функцией подмножества пикселей в области, окружающей соответствующий
пиксель входного изображения:
yu,v = f(𝒲u,v, S),

∀(u, v) ∈ X,

(11.9)

где S – структурирующий элемент, произвольное маленькое
бинарное или логическое изображение с нечетными длинами сторон. Он подобен рассмотренному выше ядру свертки,
за исключением того, что теперь определяет, к каким пикселям
в окне 𝒲u,v применяется функция f(·), – он задает подмножество
пикселей в окне. Выбранные пиксели, для которых соответствующие значения структурирующего элемента отличны от нуля
или равны True, показаны красным цветом на рис. 11.26. Математическая морфология, как следует из ее названия, занимается формой объектов на изображении.

входное изображение

выходное изображение

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

Проще всего объяснить концепцию структурирующего элемента на простом примере. В данном случае это синтетическое
бинарное изображение, созданное скриптом:
>>> im = Image.Read("eg-morph1.png")
Image: 50 x 40 (uint8) [.../images/eg-morph1.png]
>>> im.disp();

которое показано повторно слева на рис. 11.27. Структурирующий элемент показан красным цветом в конце каждой строки.
Если мы рассмотрим самую верхнюю строку, структурирующий
элемент представляет собой квадрат 5×5
>>> S1 = np.ones((5, 5));

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

642 Глава 11 · Изображения и их обработка
>>> e1 = im.morph(S1, op="min")
Image: 50 x 40 (uint8)

а результат показан во втором столбце на рис. 11.27. Для каждого пикселя входного изображения берется минимум всех пикселей в окне 5×5. Если какой-либо из этих пикселей равен нулю, то
результирующий пиксель будет равен нулю. Мы можем увидеть
анимированный процесс
>>> morphdemo(im, S, op="min")
Выходное изображение

после эрозии

затем дилатация

СЭ

Рис. 11.27. Пример математической морфологии. Пиксели равны либо 0 (черный), либо 1
(белый). Каждый столбец соответствует обработке с использованием структурирующего
элемента, показанного в конце красным цветом

Результат впечатляет: два объекта полностью исчезли, а два
квадрата разделились и стали меньше. Два исчезнувших объекта не соответствовали форме структурирующего элемента.
Здесь возникает связь с морфологией или формой – в выходном изображении будут присутствовать только те формы, которые могут содержать структурирующий элемент.
Структурирующий элемент может принимать любую форму: круг, кольцо, пятиконечную звезду, отрезок линии длиной
20 пикселей под углом 30° к горизонтали или силуэт уточки. Математическая морфология позволяет создавать очень мощные
фильтры на основе форм. Во второй строке показаны результаты для большего структурирующего элемента 7×7, применение которого привело к полному устранению малого квадрата
и дальнейшему уменьшению большого квадрата. В третьей
строке показаны результаты применения структурирующего
элемента, представленного отрезком горизонтальной линии
шириной 14 пикселей, и единственными оставшимися фигурами являются длинные горизонтальные линии.

Математическая морфология 643

Полуширина
структурирующего
элемента.

Операцию, которую мы только что выполнили, часто называют эрозией, поскольку большие объекты размываются и становятся меньше – в этом случае структурирующий элемент 5×5
заставил срезать два пикселя по периметру каждого объекта.
Маленький квадрат первоначально размером 5×5 теперь имеет
размер только 1×1. Если бы мы повторили операцию, маленький квадрат полностью исчез бы, а большой уменьшился бы
еще больше.
Обратная операция – дилатация (dilation, расширение) – делает объекты больше. Применим дилатацию к результатам, показанным во втором столбце на рис. 11.27
>>> d1 = e1.morph(S1, op="max")
Image: 50 x 40 (uint8)

Результаты дилатации показаны в третьем столбце. Для каждого пикселя входного изображения берется максимум всех
пикселей в окне 5×5. Если любой из этих соседей равен единице, результирующий пиксель будет тоже равен единице. В этом
случае мы видим, что два квадрата вернулись к своим первоначальным размерам, но большой квадрат потерял свои выступающие линии.
Морфологические операции часто записываются в операторной форме. Эрозия описывается выражением
Y = X ⊖ S,

где в уравнении (11.9) функция f(·) = min(·), а дилатация
Y = X ⨁ S,

где в уравнении (11.9) функция f(·) = max(·). Эти операции также известны как вычитание и сложение Минковского соответственно.
Эрозия и дилатация связаны уравнением
–
X ⨁ S = X ⊝ S¢,

где черта обозначает логическое дополнение значений пикселей, а штрих – отражение относительно центрального пикселя.
По сути, это означает, что эрозия белых пикселей аналогична
дилатации темных пикселей, и наоборот. Для морфологических
операций справедливы равенства:
(X ⨁ S1) ⨁ S2 = X ⨁ (S1 ⨁ S2),

(X ⊖ S1) ⊖ S2 = X ⊖ (S1 ⨁ S2),

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

644 Глава 11 · Изображения и их обработка

применению одного более крупного структурирующего элемента, но первое в вычислительном отношении дешевле. ► Сокращенные функции
>>> out = im.erode(S);
>>> out = im.dilate(S);

можно использовать вместо низкоуровневого метода morph.
Последовательность операций эрозии, а затем дилатации
известна как открытие, поскольку она открывает промежутки.
В операторной форме это записывается так:
X ∘ S = (X ⊖ S) ⨁ S.

Открытие не только выделило определенные формы, но
и очистило изображение: квадраты были разделены, а выступы на большом квадрате удалены, так как они не соответствуют
форме структурирующего элемента.
На рис. 11.28 мы выполняем операции в обратном порядке:
дилатация, затем эрозия. В первом ряду элементы не утеряны,
они сначала выросли, затем сжались, а у большого квадрата
остались выступы. Отверстие было заполнено, так как оно не
соответствует форме структурирующего элемента. Во втором
ряду более крупный структурирующий элемент заставил два
квадрата соединиться вместе. Эта последовательность операций называется закрытием, поскольку она закрывает промежутки и записывается в операторной форме как
X • S = (X ⨁ S) ⊖ S.
Выходное изображение

после эрозии

затем дилатация

СЭ

Рис. 11.28. Пример математической морфологии. Пиксели либо 0 (черный), либо 1 (белый).
Каждая строка соответствует обработке с использованием структурирующего элемента,
показанного в конце красным

Например, квад­
ратный структурирующий элемент
3×3, примененный
дважды, эквивалентен квадратному
структурирующему
элементу 5×5.
Первый использует
2 × (3 × 3 × N 2) =
18N 2 операций, тогда как последний
5 × 5 × N2 =
25N 2 операций.

Математическая морфология 645

Эти имена имеют
смысл, если учесть,
что происходит
с белыми объектами
на черном фоне.
Для черных объектов операции выполняют обратную
функцию.

Обратите внимание, что в нижней строке два отрезка остались
прикрепленными к краю, это связано с поведением по умолчанию при обработке пикселей края. Как и в случае свертки с помощью функции convolve, эти методы позволяют управлять обработкой случаев, когда окно выходит за пределы изоб­ражения.
Открытие и закрытие реализуются методами open и close
соответственно. В отличие от эрозии и дилатации, повторное
применение открывания или закрывания бесполезно, поскольку эти операции идемпотентны:
(X ∘ S) ∘ S = X ∘ S;
(X • S) • S = X • S.

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

11.6.1

Удаление шума

Обычно морфологическое открытие используется для удаления
шума на изображении. Зашумленное бинарное изображение
>>> objects = Image.Read("segmentation.png")
Image: 640 x 480 (uint8) [.../images/segmentation.png]
>>> objects.disp();

показанное на рис. 11.29а, получено в результате неудачной
операции пороговой обработки. Нам нужно удалить белые пиксели, которые не принадлежат объектам, и заполнить пропуски
в четырех белых прямоугольных объектах.
Выберем симметричный круговой структурирующий элемент
с радиусом 3
>>> S_circle = Kernel.Circle(3)
array([[ 0, 0, 0, 1, 0,
[ 0, 1, 1, 1, 1,
[ 0, 1, 1, 1, 1,
[ 1, 1, 1, 1, 1,
[ 0, 1, 1, 1, 1,
[ 0, 1, 1, 1, 1,
[ 0, 0, 0, 1, 0,

0,
1,
1,
1,
1,
1,
0,

0],
0],
0],
1],
0],
0],
0]])

и применим операцию закрытия, чтобы заполнить отверстия
в объектах:
>>> closed = objects.close(S_circle);

в

б

u (пиксели)

v (пиксели)

u (пиксели)

v (пиксели)

а

v (пиксели)

v (пиксели)

646 Глава 11 · Изображения и их обработка

u (пиксели)

г

u (пиксели)

Рис. 11.29. Морфологическая очистка: а) исходное изображение; б) оригинал после открытия; в) закрытие
и открытие; г) открытие и закрытие. Структурирующий элемент – это круг с радиусом 3

Результат показан на рис. 11.29б. Отверстия были заполнены,
но шумовые пиксели превратились в маленькие кружочки, а некоторые слиплись. Мы устраняем их с помощью операции открытия
>>> clean = closed.open(S_circle);

В результате получаем (рис. 11.29в) очищенное изображение. Если применить операции в обратном порядке – сначала
открытие, затем закрытие
>>> objects.open(S_circle).close(S_circle).disp();

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

11.6.2

Обнаружение границы

Преобразование цилиндра (top-hat transform) использует морфологические операции для обнаружения краев объектов. Про-

Математическая морфология 647

должая приведенный выше пример и применяя к изображению
функцию clean, как показано на рис. 11.29в, мы вычисляем его
эрозию с помощью кругового структурирующего элемента
>>> eroded = clean.erode(S_circle)
Image: 640 x 480 (uint8)

Объекты на этом изображении немного меньше, так как
структурирующий элемент срезал один пиксель снаружи каждого объекта. Вычитание изображения после эрозии из ори­
гинала
>>> edge = clean - eroded
Image: 640 x 480 (uint8)
>>> edge.disp();

v (пиксели)

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

Рис. 11.30. Обнаружение
границы путем морфологической
обработки

11.6.3

u (пиксели)

Преобразование «попадание или промах»

Преобразование «попадание или промах» (hit or miss) – это
морфологическая операция, определяемая двумя структурными элементами
–
Y = (X ⊖ S1) ∩ (X ⊖ S2)

и представляющая собой логическое «И» двоичного изображения и его дополнения, размытого двумя различными структурными элементами. Она сохраняет пиксели, где единицы в окне
соответствуют S1, а нули – S2.
Два структурных элемента можно объединить в интервал
S1–S2 со значениями 1 (соответствующий пиксель должен быть

648 Глава 11 · Изображения и их обработка

единицей), -1 (соответствующий пиксель должен быть нулем)
или 0 (не имеет значения), как показано на рис. 11.31a. Для
получения результата, равного единице, интервал должен соответствовать пикселям исходного изображения, как показано на рис. 11.31б. При наличии несоответствия, как видим на
рис. 11.31в, результат будет равен нулю. Преобразование реализуется методом hitormiss, например
>>> out = image.hitormiss(S);

где S – интервал.

а
б
в
Рис. 11.31. Преобразование «попадание или промах»: а) интервал имеет значения 1 (должен
быть единицей, синий), –1 (должен быть нулем, красный) или 0 (не имеет значения, зеленый);
б) пример попадания; в) пример промаха – базовый шаблон пикселей не соответствует
наложенному интервалу, несовместимые пиксели обозначены крестиками

Преобразование «попадание или промах» можно применять
итеративно с последовательностью структурирующих элементов для выполнения сложных операций, таких как скелетирование и обнаружение линейных признаков. Скелет объектов
вычисляется при помощи функции
>>> skeleton = clean.thin();

Результат показан на рис. 11.32а. Линии имеют ширину
в один пиксель и являются краями обобщенной диаграммы
Вороного – они очерчивают наборы пикселей в соответствии
с границей формы, к которой они ближе всего. Затем можно
найти конечные точки скелета
>>> ends = skeleton.endpoint()
Image: 640 x 480 (bool)

а также тройные точки
>>> joins = skeleton.triplepoint();

т. е. точки, в которых соединяются три линии. Они показаны на
рис. 11.32б и в соответственно.

u (пиксели)

б

u (пиксели)

v (пиксели)

а

v (пиксели)

v (пиксели)

Математическая морфология 649

в

u (пиксели)

Рис. 11.32. Операции преобразования «попадание или промах»: а) скелетирование; б) пиксели, соответствующие
конечным точкам, показаны темно-серым и обведены кружками; в) пиксели, соответствующие тройным
соединениям, показаны черным и обведены кружками

11.6.4

Преобразование расстояния

Использование преобразования расстояния для решения задачи
планирования пути робота было представлено в разделе 5.4.1.
На основе сетки занятости программа вычисляла расстояние от
каждой свободной ячейки до целевой точки. Преобразование
расстояния, обсуждаемое здесь, работает с бинарным изобра­
жением, а выходное значение, соответствующее каждому нулевому пикселю во входном изображении, представляет собой
расстояние до ближайшего ненулевого пикселя.
Рассмотрим задачу подбора модели по форме изображения.
Создадим контур повернутого квадрата

Для планирования
пути в разделе 5.4.1
мы использовали
метод медленного
итерационного
волнового фронта.
Здесь мы используем функцию
distanceTransform
из OpenCV. >>> im = Image.Squares(1, size=256).rotate(0.3).canny()

Image: 256 x 256 (uint8)

который показан на рис. 11.33a, а затем вычислим преобразование расстояния

650 Глава 11 · Изображения и их обработка
>>> dx = im.distance_transform(norm="L2")
Image: 256 x 256 (float32)

а

Евклидово расстояние (пиксели)

v (пиксели)

v (пиксели)

как показано на рис. 11.33б. Значение пикселя – это его расстояние от ближайшего ненулевого пикселя входного изображения.
Начальная оценка квадрата показана красной линией. Выходное значение преобразования расстояния в любой точке
этого красного квадрата указывает, насколько далеко оно находится от ближайшей точки исходного квадрата. Просуммировав преобразование расстояния для каждой точки на красном
квадрате или хотя бы только для вершин, мы получим общую
меру расстояния, которая будет равна нулю, только когда квад­
рат нашей модели наложится на исходный квадрат. Общее расстояние – это функция стоимости, которую можно минимизировать, используя процедуру оптимизации, подбирающую
положение, ориентацию и размер квадрата. Если рассматривать преобразование расстояния как трехмерную поверхность
на рис. 11.33в, наша задача аналогична броску растяжимого

б

u (пиксели)

u (пиксели)

е

Евклидово расстояни
(пиксели)

v (п
ли)
иксе
ксели)

u (пи
в
Рис. 11.33. Преобразование расстояния: а) входное бинарное изображение; б) преобразованное входное
изображение с наложенным квадратом модели; в) преобразование расстояния как поверхность

Изменение формы 651
Отступление 11.16. Преобразование расстояния
В преобразовании расстояния бинарного изображения значение в каждом пикселе равно расстоянию от этого пикселя до
1.00
ближайшего ненулевого пикселя во входном изображении. Метрикой расстояния
f(X )
обычно является либо евклидово (норма L2),
либо манхэттенское расстояние (норма L1).
–3.00
–3.00
–3.00
Оно равно нулю для пикселей, которые не
равны нулю во входном изображении. Это
преобразование тесно связано со знаковой
y
x
функцией расстояния, значение которой
3.00 3.00
в любой точке равно расстоянию от этой
точки до ближайшей границы фигуры и является положительным внутри фигуры
и отрицательным вне фигуры. На рисунке показана функция расстояния со знаком
для единичного круга, и она имеет нулевое значение, обозначенное красной плос­
костью на границе объекта. Если мы считаем, что форма определяется ее преобразованием расстояния со знаком, то ее нулевой контур определяет границу формы.

квадратного контура в долину преобразования расстояния. Обратите внимание, что преобразование расстояния необходимо
вычислить только один раз, и во время подбора модели функция стоимости представляет собой просто поиск вычисленного
расстояния. Это пример сопоставления граней, а полный пример с оптимизацией приведен в examples/chamfer_match.py.

11.7

Изменение формы

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

11.7.1

Обрезка

Самым простым изменением формы является выбор прямо­
угольной области изображения, что является знакомой операцией обрезки. Возьмем изображение
>>> mona.disp();

показанное на рис. 11.34a, на котором мы интерактивно указываем область наблюдения (region of interest, ROI):
>>> eyes, roi = mona.roi();
>>> eyes
Image: 137 x 41 (float32)

652 Глава 11 · Изображения и их обработка

щелкнув и перетащив рамку выбора на изображении. В данном
случае мы выделили глаза, а углы выделенной области можно
при желании вывести на печать
>>> roi
array([239, 375, 172, 212])

v (пиксели)

v (пиксели)

где элементы массива – это координаты верхнего левого и нижнего правого углов. Функцию можно использовать неинтерактивно, указав ROI.
В данном случае выделенная область охватывает улыбку
Моны Лизы, как показано на рис. 11.34б.

а

u (пиксели)

б

u (пиксели)

Рис. 11.34. Пример выбора области наблюдения или обрезки изображения: а) исходное изображение; б) выбранная
область

11.7.2

Изменение размера изображения

Часто приходится уменьшать размеры изображения, например
чтобы сократить время обработки или объем занимаемой памяти. Продемонстрируем эту операцию на примере изображения с высоким разрешением
>>> roof = Image.Read("roof.png", mono=True)
Image: 2009 x 1668 (uint8) [.../images/roof.png]

которое показано на рис. 11.35а. Простейшим средством
уменьшения размера изображения является подвыборка или
прореживание, при котором выбирается каждый m-й пиксель
в направлении u и v, где m ∈ ℕ – коэффициент подвыборки. Например, при m = 2 изображение N×N становится изображением
N/2×N/2, которое содержит одну четверть от количества пикселей исходного изображения.

v (пиксели)

v (пиксели)

Изменение формы 653

а

б

u (пиксели)

v (пиксели)

v (пиксели)

u (пиксели)

в

u (пиксели)

г

u (пиксели)

Рис. 11.35. Пример масштабирования изображения: а) исходное изображение; б) подвыборка с m = 7, обратите
внимание на масштабирование по оси; в) подвыборка с m = 7 после сглаживания; г) изображение c восстановлено
до исходного размера путем репликации пикселей

Для примера мы уменьшим размер изображения в семь раз
в каждом направлении:
Это пример >>> roof[::7, ::7].disp();
муарового узора.
Любой реализуемый фильтр низких
частот имеет конечную характеристику
выше его частоты
среза. На практике
частота среза выбирается настолько
ниже теоретической, чтобы отклик
фильтра на частоте
Найквиста был
достаточно малым.
Как правило, для
подвыборки с m = 2
используется гауссиан с σ = m/2.

Эта операция выбирает каждую седьмую строку и столбец.
Результат показан на рис. 11.35б, где можно наблюдать несколько ярко выраженных изогнутых линий на крыше, которых не
было на исходном изображении. Это артефакты процесса выборки. Подвыборка снижает частоту пространственного разрешения изображения, что может привести к пространственному
наложению высокочастотных компонентов из-за текстуры или
резких краев. Чтобы убедиться, что выполняется требование
теоремы выборки Шеннона–Найквиста, необходимо применить сглаживающий пространственный фильтр низких частот
и тем самым уменьшить пространственную полосу пропускания изображения перед операцией подвыборки.
Это еще
одно применение сглаживания, и ядро Гаусса является вполне
подходящим фильтром низких частот для этой цели:

654 Глава 11 · Изображения и их обработка
>>> roof.smooth(sigma=3)[::7, ::7].disp();

а результаты для m = 7 показаны на рис. 11.35в. Обратите внимание, что артефакты в виде изогнутых линий больше не наблюдаются. Мы можем записать это более кратко как
>>> smaller = roof.scale(1/7, sigma=3)
Image: 287 x 238 (uint8)

Обратная операция – это репликация пикселей, при которой
каждый входной пиксель реплицируется как плитка размера
m×m в выходном изображении
>>> smaller.replicate(7).disp();

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

11.7.3

Пирамиды изображений

Важной концепцией компьютерного зрения, к которой мы вернемся в следующей главе, является масштабируемое пространство. Метод pyramid класса Image возвращает пирамидальную декомпозицию входного изображения
>>> pyramid = mona.pyramid()
[Image: 677 x 700 (float32),
Image: 339 x 350 (float32),
Image: 170 x 175 (float32),
Image: 85 x 88 (float32),
Image: 43 x 44 (float32),
Image: 22 x 22 (float32),
Image: 11 x 11 (float32),
Image: 6 x 6 (float32),
Image: 3 x 3 (float32),
Image: 2 x 2 (float32),
Image: 1 x 1 (float32)]
>>> len(pyramid)
11

как список изображений с последовательно более низким разрешением. Обратите внимание, что последний элемент – версия с разрешением 1×1 – представляет собой один темно-серый
пиксель! Эти изображения вставляются в составное изображение, показанное на рис. 11.36.

v (пиксели)

Изменение формы 655

u (пиксели)
Рис. 11.36. Пирамида изображений, последовательность изображений, каждое из которых
составляет половину (по длине стороны) разрешения от предыдущего

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

11.7.4

Деформация изображения

Деформация изображения – это преобразование координат,
а не значений пикселей. Деформацию можно использовать для
увеличения или уменьшения размера изображения, поворота
изображения или применения совершенно произвольных изменений формы. Координаты пикселя в новом представлении
(u, v) выражаются как функции
u = fu(u¢, v¢),

υ = f v(u¢, v¢)

(11.10)

от координат (u¢, v¢) в исходном представлении.
Рассмотрим простой пример, показанный на рис. 11.37а, когда изображение уменьшено в размере в 4 раза в обоих направлениях и смещено так, что его начало координат (его верхний
левый угол) сдвинуто в точку (100, 200). Мы можем выразить
это кратко:
u¢ = u/4 + 100,

v¢ = v/4 + 200.

(11.11)

б

u (пиксели)

u (пиксели)

v (пиксели)

а

v (пиксели)

v (пиксели)

656 Глава 11 · Изображения и их обработка

u (пиксели)

Рис. 11.37. Деформация изображений: а) масштабирование и сдвиг с помощью warp;
б) масштабирование и сдвиг с помощью affine_warp; в) поворот на 30° вокруг центра
с использованием affine_warp. Пикселям, отображаемым красным цветом, метод affine_warp
присвоил значение nan – они не интерполировались из каких-либо пикселей изображения

Эту формулу можно привести к виду (11.10) как
u = 4(u¢ - 100),

v¢ = 4(v¢ - 200).

(11.12)

Сначала создадим пару координатных матриц
>>> Up, Vp = Image.meshgrid(width=500, height=500)

которые определяют координаты (u¢, v¢) пикселей в выходном
изображении 500×500. Эти мат­ри­цы таковы, что Up[v,u] = u
и Vp[v,u] = v. Соответствующие координаты во входном изображении задаются формулой (11.12):
>>> U = 4 * (Up - 100);
>>> V = 4 * (Vp - 200);

Эти мат­ри­цы отображают (u¢, v¢) ↦ (u, v) координаты выходного изображения в координаты входного изображения. Например, пиксель выходного изображения с координатами (200,
300) получается из

Изменение формы 657
>>> p = (300, 200); # (v, u)
>>> (U[p], V[p])
(400, 400)

во входном изображении, а пиксель выходного изображения
с координатами (200, 100) получается из
>>> p = (100, 200); # (v, u)
>>> (U[p], V[p])
(400, -400)

который находится за границами входного изображения. Теперь можно деформировать входное изображение
>>> mona.warp(U, V).disp();

Результат показан на рис. 11.37a. Большая часть изображения
черная, потому что этим пикселям не соответствуют входные
пиксели – они соответствуют точкам за пределами изображения.
Напомню, что форма U и V определяет форму выходного
изобра­жения, а их элементы описывают отображение (u¢, v¢) ↦
(u, v). Этот обобщенный подход к деформации можно использовать для создания сложных нелинейных отображений, например для восстановления изображений, как показано на
рис. 13.9.
Часто между изображениями существует аффинное отношение – плоскостной перенос, поворот и масштабирование. Для
приведенного выше примера аффинное преобразование можно записать в однородной форме как

то есть как произведение диагональной масштабирующей мат­
ри­цы и мат­ри­цы SE(2), описывающей движение плоского твердого тела

которую можно вычислить как
>>> M = np.diag([0.25, 0.25, 1]) * SE2(100, 200)
array([[
0.25,
0,
100],
[
0,
0.25,
200],
[
0,
0,
1]])

658 Глава 11 · Изображения и их обработка

Результатом является массив NumPy 3×3
ляет аффинную деформацию

который опреде-

>>> out = mona.affine_warp(M, bgcolor=np.nan);
>>> out.disp(badcolor="r");

Результат показан на рис. 11.37б. Мы также указали, что выходные пиксели, для которых нет соответствующих входных
пикселей, имеют значение nan, а параметр метода disp предписывает отображать их красным цветом. По умолчанию выходное изображение имеет тот же размер, что и входное.
За кулисами эти операции скрывают некоторые тонкие особенности. Во-первых, хотя координаты (u¢, v¢) в выходном изображении являются целыми числами, координаты входного
изображения (u, v) не обязательно будут целыми числами. Значения пикселей приходится интерполировать
по соседним
пикселям во входном изображении. Во-вторых, не все пиксели
в выходном изображении имеют соответствующие пиксели во
входном изображении, как показано на рис. 11.38 – они отображаются красным цветом. В случае значительной деформации может оказаться, что многие соседние выходные пиксели
отображаются в один и тот же входной пиксель, что приводит
к появлению эффекта пикселизации, или ступенчатости, в выходном изображении.

входное изображение

выходное изображение

Рис. 11.38. Обозначение координат для деформации изображения. Пиксель (u¢, v¢) в выходном
изображении получается из пикселя (u, v) во входном изображении, как показано стрелкой.
Деформированное изображение не обязательно является многоугольным или полностью
содержится в выходном изображении

Теперь попробуем сделать что-нибудь более существенное
и повернем изображение на 30°, чтобы получить выходное изображение того же размера, что и входное. Мы могли бы снова использовать мат­ри­цу SE(2), но, как обсуждалось в разделе 2.2.2.4, поворот вокруг произвольной точки можно описать
более кратко, использовав единичное кручение
>>> S = Twist2.UnitRevolute(mona.centre)
(350 -338.5; 1)

Обычная мат­ри­ца
3×3, умноженная
на мат­ри­цу SE(2),
не входит в группу
SE(2), поэтому возвращается общий
массив NumPy.
Аффинное преобразование – это
мат­ри­ца 2×3, поэтому affine_warp
использует только
первые две строки
переданной мат­ри­
цы 3×3.
Есть возможность
выбора разных режимов интерполяции. По умолчанию
используется билинейная интерполяция, когда пиксель
с координатами
(u + δu, v + δv)
является линейной
комбинацией пикселей (u, v), (u + 1, v),
(u, v + 1) и (u + 1,
v + 1), где u, v ∈
ℕ0 и δu, δv ∈ [0, 1).
Функция интерполяции действует
как слабый фильтр
сглаживания, но
для очень большого
уменьшения масштаба изображение
следует сначала
сгладить с помощью
гауссовского ядра.

Подведение итогов 659
Как объект SE2,
который affine_
warp преобразует
в массив NumPy.

которое можно возвести в степень, чтобы получить мат­ри­цу
преобразования SE(2) ◄
>>> M = S.exp(pi / 6)
0.866
-0.5
220.4
0.5
0.866
-122.4
0
0
1
>>> out = mona.affine_warp(M, bgcolor=np.nan)
Image: 700 x 677 (float32)

Результат показан на рис. 11.37б. Обратите внимание на направление вращения – наше определение осей x и y (параллельных осям u и v соответственно) таково, что ось z определяется
как направленная внутрь страницы, что делает вращение по
часовой стрелке положительным углом. Также обратите внимание, что углы исходного изображения были потеряны, они
оказались за пределами выходного изображения.
Аффинная деформация используется методом scale для изменения масштаба изображения, а методом rotate – для поворота. Пример выше также можно реализовать как
>>> twisted_mona = mona.rotate(пи/6);

Аффинное преобразование может выполнять любую комбинацию переноса, поворота, масштабирования, сдвига и отражения исходного изображения, но не может моделировать
изменение, вызванное изменением точки зрения. Некоторые
изменения точки зрения влекут простое выполнение переноса, масштабирования или поворота, но чаще они приводят
к перспективному укорочению, которое невозможно корректно
представить аффинным преобразованием. Перспективное преобразование выполняет общее изменение точки зрения, описываемое мат­ри­цей 3×3 (см. раздел 14.8.1).

11.8

Подведение итогов

В этой главе вы узнали, как получать изображения из различных источников, таких как файлы изображений, видеофайлы,
видеокамеры и интернет, и загружать их в сеанс Python. Получив изображения, мы можем обращаться с ними как с двух- или
трехмерными массивами и удобно ими манипулировать с помощью NumPy. Элементами матриц изображения могут быть
целые числа, числа с плавающей точкой или логические значения. Затем мы познакомились с классом Image, который инкапсулирует массив NumPy, скрывая множество низкоуровневых
деталей, что упростило исследование важных концепций и по-

660 Глава 11 · Изображения и их обработка

требовало для этого минимальный объем кода. Класс Image обладает множеством методов, обеспечивающих согласованный
интерфейс к базовым алгоритмам, реализованным с помощью
NumPy, SciPy или OpenCV. Подробности можно узнать из исходного кода.
Далее было представлено множество операций обработки
изображений с применением класса Image. Операции с одним
изображением: унарные арифметические операции, преобразование типов, различные преобразования цвета и растяжение
уровня серого; нелинейные операции, такие как нормализация
гистограммы и гамма-кодирование или декодирование, и логические операции, такие как отсечение по пороговому значению. Также были показаны операции с парами изображений,
такие как фильтрация зеленого, вычисление фона и обнаружение движущихся объектов.
Самый большой и разнообразный класс операций – пространственные операторы. Мы обсудили свертки, которые можно использовать для сглаживания изображения и обнаружения
краев. Линейные операции определяются мат­ри­цей ядра, которую можно выбрать для выполнения таких функций, как сглаживание изображения (для уменьшения эффекта зашумления
или в качестве низкочастотного сглаживающего фильтра перед
прореживанием) или для обнаружения краев. Нелинейные
пространственные операции применяются для сопоставления
шаблонов, вычисления ранговой статистики (включая медианный фильтр, который устраняет импульсный шум) и математической морфологии, которая фильтрует изображение на
основе формы и может использоваться для очистки бинарных
изображений. Преобразование «попадание или промах» может
итеративно применяться для выполнения таких функций, как
скелетирование.
Наконец, мы обсудили операции по изменению формы, такие как области внимания, изменение масштаба, а также проб­
лемы, которые могут возникнуть из-за ступенчатости и общего искажения изображения при перемещении, повороте или
устранении оптических искажений изображения. Все эти методы обработки изображений лежат в основе алгоритмов выделения признаков, которые мы обсудим в следующей главе.

11.8.1

Дополнительное чтение

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

Подведение итогов 661

частотной области, а также сжатие изображений, можно найти в Nixon and Aguado (2019), Gonzalez and Woods (2018), Davies
(2017), Klette (2014) и Szeliski (2022). Книгу Szeliski (2022) можно
бесплатно получить для личного пользования по адресу https://
szeliski.org/Book. В Nixon and Aguado (2019) можно найти примеры для Python и MATLAB, в Gonzalez and Woods (2018) – примеры для MATLAB, а в Davies (2017) – примеры на Python и C++.
Онлайн-информация о компьютерном зрении доступна через CVonline по адресу http://homepages.inf.ed.ac.uk/rbf/CVonline, а материал этой главы рассматривается в разделе Ima­
ge Transformations and Filters («Преобразование изображений
и фильтры»).
Обнаружение краев – это часть обработки изображений, но
по ней имеется огромное количество литературы. В Forsyth and
Ponce (2012) вы найдете исчерпывающее введение в обнаружение границ и полезное обсуждение ограничений обнаружения
границ. В Nixon and Aguado (2019) также рассматривают подходы фазовой конгруэнтности к обнаружению границ и сравнивают различные детекторы границ. Ядро Собеля для обнаружения краев было описано в неопубликованной статье 1968 г.
Стэнфордской лаборатории искусственного интеллекта Ирвина
Собеля и Джерома Фельдмана A 3×3 Isotropic Gradient Operator
for Image Processing. Детектор краев Кэнни был первоначально
описан в статье Canny (1983, 1987).
Непараметрические меры сходства изображений стали популярными в 1990-х гг. благодаря ряду ключевых статей, таких
как Zabih and Woodfill (1994), Banks and Corke (2001), Bhat and
Nayar (2002). Применение к системам обработки изображений
в реальном времени высокоскоростной логики, такой как FPGA,
изуча­лось несколькими группами (Corke et al., 1999; Woodfill
and Von Herzen, 1997).
Математическая морфология – еще одна очень большая те­
ма, и мы коснулись только поверхности, а важные методы, такие как морфология оттенков серого и переходные границы,
вообще не были рассмотрены. В упомянутых выше общих книгах по обработке изображений есть полезные обсуждения этой
темы. Большинство специализированных книг в этой области
уже устарели, тем не менее в Soille (2003) приводятся теоретические основы, работа Shih (2009) может послужить хорошим
введением, а книга Dougherty and Latufo (2003) предлагает более практический подход к обучению.
Подход к компьютерному зрению, описанный в данной книге, часто называют восходящей обработкой. Эта глава была посвящена низкоуровневым методам, представляющим операции
с пикселями. Следующая глава посвящена высокоуровневым методам, в которых наборы пикселей группируются, а затем описываются для представления объектов на сцене.

662 Глава 11 · Изображения и их обработка

11.8.2

Источники изображений

Все изображения, используемые в этой части книги, поставляются вместе с пакетом mvtb-data, который устанавливается как
зависимость Machine Vision Toolbox for Python.

11.8.3

Программные инструменты

Существует множество высококачественных программных инструментов с открытым исходным кодом для обработки изоб­
ражений. Пакет Toolbox, используемый в этой книге, широко
использует OpenCV (http://opencv.org) – зрелый проект программного обеспечения компьютерного зрения с открытым
исходным кодом, содержащий тысячи алгоритмов, интерфейсы
для C++, C, Python и Java и работающий в Windows, Linux, Mac
OS, iOS и Android. В настоящее время издано несколько книг об
OpenCV, но я рекомендую книгу Kaehler and Bradski (2016) – это
второе издание популярной книги, которая являет собой хорошее введение в компьютерное зрение в целом.

11.8.4

Упражнения

1. Поэкспериментируйте с функциями iread и idisp, предназначенными для работы с черно-белыми и цветными изображениями, а также с эквивалентными операциями Image.Read
и Image.disp. Исследуйте значения пикселей на изображении,
а также кнопки масштабирования, линии и гистограммы.
2. Используйте метод roi, чтобы извлечь улыбку Моны Лизы.
3. Попробуйте получить несколько кадров с камеры на вашем
компьютере или из видеофайла и отобразить их.
4. Напишите цикл захвата кадров с камеры и их отображения.
Перед отображением добавьте в изображение эффекты, такие как «негатив», фильтрация по пороговому значению, постеризация, ложные цвета, фильтрация границ и т. д.
5. Сравните гистограммы изображений в градациях серого, которые недоэкспонированы, правильно экспонированы и переэкспонированы. В случае цветных изображений сравните
гистограммы цветовых каналов RGB для сцен с разными доминирующими цветами. Комбинируйте захват изображения
в реальном времени с вычислением и отображением гистограммы.
6. Создайте две копии черно-белого изображения в NumPy-мас­
сивах A и B. Напишите код, определяющий время, необходимое для вычисления разницы между A и B, используя со-

Подведение итогов 663

7.

8.

9.

10.

кращенную запись A–B, поддерживаемую в NumPy, или два
вложенных цикла for. Используйте пакет Python timeit для
измерения времени.
Для сцены с яркостью 800 нит и камерой с ISO 1000, q = 0.7
и f-числом 2.2 вычислите время экспозиции, необходимое,
чтобы средний уровень серого 8-битного изображения равнялся 150.
Выполните обработку спутниковых снимков (раздел 11.1.6):
a) получите карту дорог вашего района и используйте ее,
чтобы найти путь между двумя точками, задействовав
планировщики движения робота, описанные в разделе 5.4;
б) для изображений, возвращаемых классом EarthView, напишите функцию преобразования координат пикселя
в широту и долготу; используйте параметр coordformat
в вызове Image.disp, чтобы широта и долгота пикселя
отображались на панели инструментов окна;
в) загрузите данные GPS-трека со своего телефона и наложите их на изображение со спутника.
Обнаружение движения:
a) измените пример обработки дорожного движения (раздел 11.4) и выделите движущиеся транспортные средства; изучите влияние параметра σ;
б) напишите цикл, который определяет фон, используя
кадры с вашей камеры. Что происходит при наличии
перемещающихся или остающихся на месте объектов?
Исследуйте эффект изменения параметра σ;
в) объедините концепции обнаружения движения и хроматической рирпроекции (хромакей), чтобы поместить
пиксели с камеры, где есть движение, в сцену пустыни.
Свертка:
а) сравните результаты сглаживания с использованием
равномерного ядра 21×21 и ядра Гаусса. Наблюдается ли
артефакт «звона» в первом случае?
б) почему мы выбираем сглаживающее ядро, сумма элементов которого равна единице?
в) сравните качество применения ядра простого горизонтального градиента K = (−0.5 0 0.5) с ядром Собеля;
г) исследуйте фильтрацию с помощью ядра Гаусса для различных значений σ и размера ядра;
д) создайте ядро 31×31 для обнаружения линий под уг­
лом 60°;
е) выведите аналитически производную гауссиана в направлении x (уравнение 11.4);
ж) выведите аналитически лапласиан гауссиана (уравнение 11.8);
з) выведите аналитически разность гауссианов;

664 Глава 11 · Изображения и их обработка

11.
12.

13.
14.
15.
16.

и) покажите разницу между разностью гауссианов и производной гауссиана.
Покажите аналитически, как ошибки шкалы интенсивности влияют на меры подобия SSD и NCC.
Выполните сопоставление шаблона с изображением Моны
Лизы; сначала преобразуйте его в оттенки серого:
a) используйте метод roi, чтобы выбрать один из глаз
Моны Лизы в качестве шаблона. Шаблон должен иметь
нечетные размеры;
б) используйте метод similarity для вычисления подобия
изображений. Какое совпадение лучше всего и где оно
встречается? Каково сходство с другим глазом? Где находится второе наилучшее совпадение и какова его
оценка сходства?
в) измените интенсивность изображения Моны Лизы и исследуйте влияние на пиковое сходство;
г) добавьте смещение к интенсивности изображения
Моны Лизы и исследуйте влияние на пиковое сходство;
д) повторите шаги (в) и (г) для различных показателей
сходства, таких как SAD, SSD, рейтинг и последовательность;
е) масштабируйте размер шаблона с помощью различных
коэффициентов (используйте метод scale) в диапазоне
от 0.5 до 2.0 с шагом 0.05 и исследуйте влияние на пиковое сходство. Постройте график зависимости пикового
сходства от масштаба;
ж) повторите (е) для поворота шаблона на угол в диапазоне
от -0.2 до 0.2 рад с шагом 0.05.
Выполните пример подвыборки в разделе 11.7.2 и изучите артефакты сглаживания вокруг острых краев и обычную
текстуру черепицы.
Напишите функцию для создания рис. 11.36 из вывода pyramid.
Деформируйте изображение в полярных координатах (r, θ)
относительно центра изображения, где горизонтальная
ось – это r, а вертикальная – θ.
Создайте функцию деформации, которая имитирует ваше
любимое зеркало в комнате смеха.

Глава

12

	 Извлечение признаков
изображения

chap12.ipynb

https://go.sn.pub/
HWpVq3

В предыдущей главе мы обсудили получение и обработку изображений. Вы узнали, что изображения – это просто большие
массивы значений пикселей, но с точки зрения роботов изображения содержат слишком много данных и мало информации. Робот должен уметь отвечать на содержательные вопросы, например: в каком положении находится объект? что это
за объект? как быстро он движется? как быстро движется сам
робот? и т. д. Ответами на такие вопросы являются измерения,
полученные из изображения, которые принято называть признаками изображения. Признаки – это суть сцены и исходный
материал, который нужен для управления роботом.
Операции обработки изображений из предыдущей главы
работали с одним или несколькими входными изображениями и возвращали другое изображение. В отличие от этого, процедура извлечения признаков работает с одним изображением и возвращает один или несколько признаков изображения.
Признаки обычно представляют собой скаляры (например,
площадь или соотношение сторон) или короткие векторы (например, координаты объекта или параметры линии). Извлечение признаков изображения – необходимый первый шаг в использовании данных изображения для управления роботом.
Это этап выделения уплотненной информации, снижающий
скорость передачи данных с 106–108 байт/с на выходе камеры
до порядка десятков признаков на кадр, которые можно использовать в качестве входных данных для системы управления роботом.
В этой главе мы рассмотрим признаки и способы их извлечения из изображений. Опираясь на методы обработки изображений из предыдущей главы, мы обсудим три класса объектов:
области, линии и опорные точки. В разделе 12.1 обсуждаются
признаки областей, которые представляют собой смежные
группы пикселей, однородные по некоторому свойству. Например, это может быть набор пикселей, представляющих красный
объект на фоне другого цвета. В разделе 12.2 обсуждаются признаки линий, описывающие прямые линии в мире робота. Пря-

666 Глава 12 · Извлечение признаков изображения

мые линии отчетливы и очень часто встречаются в искусственных средах, например на краях дверных проемов, зданий или
дорог. Последним источником признаков являются опорные
точки, которые обсуждаются в разделе 12.3. Это особенно ха­
рактерные точки сцены, которые можно надежно обнаружить
в разных проекциях одной и той же сцены и которые имеют
важное значение для дискуссии в главе 14.
В разделе 12.4 будут представлены два применения: оптическое распознавание символов и поиск изображений. В последнем случае решается задача поиска изображения в существующем наборе, наиболее похожего на образец. Такой поиск может
использоваться роботом, чтобы определить, посещал ли он ранее определенное место или видел ли уже этот объект.
 ельзя забывать, что признаки изображения представляют собой
Н
сводку информации, представленной в пикселях, составляющих
изображение, и что отображение мира в пиксели сопряжено со значительными потерями информации. Обычно мы боремся с этой потерей информации, делая предположения, основанные на знаниях
об окружающей среде, но наша система всегда будет настолько хороша, насколько хороши наши предположения. Например, мы можем использовать признаки изображения для описания положения
и формы группы красных пикселей, соответствующих красному
объекту. Однако признак размера, обычно количество пикселей,
ничего не говорит о размере красного объекта в реальном мире –
нам нужна дополнительная информация, такая как расстояние
между камерой и объектом и фокусное расстояние камеры. Мы
также должны предположить, что объект не закрыт частично – это
сделало бы наблюдаемый размер меньше, чем истинный размер.
Далее нужно предположить, что освещенность такова, что цветность света, отраженного от предмета, соответствует его свойствам.
На изображении также могут быть обнаружены черты, не соответствующие физическому предмету, – декоративные рисунки, четкие
края тени или отражения в окне.

12.1

Получение признаков из области

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

г

u (пиксели)

v (пиксели)

u (пиксели)

v (пиксели)

в

u (пиксели)

v (пиксели)

u (пиксели)

v (пиксели)

б

u (пиксели)

v (пиксели)

u (пиксели)

v (пиксели)

а

v (пиксели)

v (пиксели)

Получение признаков из области 667

u (пиксели)

u (пиксели)

Рис. 12.1. Примеры классификации пикселей. Слева – входное изображение, справа – результат классификации.
Классификация зависит от задачи, и пиксели классифицируются в C категорий, представленных разными цветами.
Предметы внимания: а) отдельные буквы на знаке (C = 2); б) красные помидоры (C = 2); в) локально однородные
области пикселей (C = 27); г) семантическая классификация с помощью глубокой сети, отображающая пиксели фона,
автомобиля, собаки и человека (C = 21)

668 Глава 12 · Извлечение признаков изображения

изменении освещения сцены или точки обзора. Сегментацию
изображения обычно принято разделять на три подзадачи.
Классификация пикселей – процесс, применяемый к каждому пикселю с целью отнести его к одному из C классов c = 0,
..., С − 1. Обычно мы используем случай C = 2, который принято
называть бинарной классификацией, или бинаризацией. Некоторые примеры классификации показаны на рис. 12.1а–б. Пиксели были классифицированы как объекты (c = 1) или необъекты
(c = 0), которые отображаются как белые или черные пиксели
соответственно. Классификация всегда зависит от задачи – например, объект соответствует ярким, желтым, красным или
движущимся пикселям. Рисунок 12.1в–г иллюстрирует много­
уровневую классификацию, где C > 2, а класс пикселя отражается в его цвете.
Основное предположение в примерах на рис. 12.1 – области
гомогенны (однородны) по некоторым характеристикам, таким
как яркость, цвет или текстура. Подобные предположения всег­
да зависят от задачи. Но мы понимаем, что в реальной жизни пиксели могут быть неправильно классифицированы и нам
придется это учитывать на следующих этапах.
Представление экземпляра объекта, в котором соседние
пиксели одного класса соединяются в пространственные множества S0, ..., Sm-1. Например, на рис. 12.1б представлены два
множества, каждое из которых представляет отдельный экземпляр помидора. Множества могут быть представлены путем
присвоения метки множества каждому пикселю или списком
координат пикселей, который определяет граница связного
множества. Иногда это называют сегментацией экземпляров.
Описание экземпляра объекта, когда множества Si опи­
сываются с помощью компактных скалярных или векторных
признаков, таких как размер, местоположение и форма. В некоторых применениях существует дополнительный этап – семан­
тическая классификация, – в процессе которой классам пикселей
или множествам присваивается значимая для задачи метка, например «кошка», «собака», «чашка кофе» и т. д.
Эти темы более подробно рассматриваются в следующих
трех подразделах.

12.1.1

Классификация пикселей

Класс пикселей, монохромных или цветных, представлен целым числом c = 0, ..., C - 1, где C – количество классов. Во многих примерах мы будем использовать бинарную классификацию всего с двумя классами, соответствующими необъекту
и объекту или фону и переднему плану.

Получение признаков из области 669

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

в котором решение основано просто на значении пикселя xu,v.
Этот подход называется пороговой бинаризацией (thresholding),
а t называется порогом.
Пороговая бинаризация очень проста в реализации. Возьмем
для примера изображение
>>> castle = Image.Read("castle.png", dtype="float");

которое показано на рис. 12.2а. Бинаризованное изображение

v (пиксели)

v (пиксели)

>>> (castle >= 0.7).disp();

а

u (пиксели)

б

u (пиксели)

в

v (пиксели)

частота пикселей

0.7
порог Оцу

уровень серого

г

u (пиксели)

Рис. 12.2. Бинарная классификация: а) исходное изображение (получено из коллекции ICDAR; Lucas 2005);
б) результат бинарной классификации с порогом 0.7; в) гистограмма значений пикселей в оттенках серого;
г) результат бинарной классификации с порогом Оцу

670 Глава 12 · Извлечение признаков изображения

представлено на рис. 12.2б. Пиксели были довольно точно классифицированы на соответствие белой краске. Эта классификация основана на, казалось бы, разумном предположении, что
объекты, окрашенные белой краской, ярче всего остального на
изображении.
Данный подход широко использовался на заре компьютерного зрения, когда компьютеры были слабыми – было проще
придумать мир белых объектов и темного фона, чем реализовать более сложную классификацию. Многие современные промышленные системы визуального контроля до сих пор используют этот простой подход, поскольку он позволяет применять
скромные встроенные компьютеры – он очень хорошо работает, если объекты находятся на конвейерной ленте подходящего
контрастного цвета или образуют силуэт на станции контроля.
В реальной разнородной среде нам обычно приходится трудиться немного усерднее, чтобы извлечь полезную классификацию на уровне серого. Важный и сложный вопрос: как получено
пороговое значение 0.7? Разумеется, методом проб и ошибок!
Метод ithresh класса Image
>>> castle.ithresh()

отображает изображение и ползунок порога, который можно
перемещать до получения удовлетворительного результата.
Однако если изображение было получено в день с другим освещением, то потребуется другое пороговое значение.
Более принципиальный подход, чем метод проб и ошибок, –
анализ гистограммы изображения
>>> castle.hist().plot();

как показано на рис. 12.2в. Гистограмма имеет два четко выраженных пика, т. е. это бимодальное распределение, которое
соответствует двум популяциям пикселей. Меньший пик около
0.9 соответствует ярким пикселям и имеет довольно небольшой
диапазон изменения значений. Более широкий и высокий пик
около 0.3 соответствует пикселям более темного фона знака
и кирпичей и имеет гораздо большую изменчивость яркости.
Чтобы разделить два класса пикселей, необходимо найти
разделяющую границу – порог, лежащий в долине между пиками. В этом отношении выбор t = 0.7 является удачным. Поскольку долина очень широка, у нас фактически есть довольно
широкий диапазон выбора для порога; например, t = 0.75 тоже
будет хорошо работать.
Оптимальный порог можно вычислить с помощью метода
Оцу
>>> t = castle.otsu()
0.6039216

Получение признаков из области 671

который разделяет изображение на два класса пикселей таким
образом, чтобы минимизировать дисперсию значений внутри
каждого класса и максимизировать дисперсию значений между классами – при условии, что гистограмма имеет только два
пика. К сожалению, как мы скоро увидим, реальный мир редко
бывает таким простым.
Рассмотрим другое изображение из той же сцены, на котором есть пятно засветки
>>> castle2 = Image.Read("castle2.png", dtype="float");

как показано на рис. 12.3а. Результат применения метода Оцу
>>> t = castle2.otsu()
0.59607846

v (пиксели)

v (пиксели)

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

а

u (пиксели)

б

u (пиксели)

в

v (пиксели)

частота пикселей

0.7
порог Оцу

уровень серого

г

u (пиксели)

Рис. 12.3. Пример бинарной сегментации: а) изображение в градациях серого с выделением интенсивности;
б) результат классификации с пороговым значением Оцу 0.59; в) гистограмма с порогами; г) классификация
с пороговым значением 0.79

672 Глава 12 · Извлечение признаков изображения

серого теперь перекрываются, и, к сожалению, не существует
единого порога, который может их разделить. Результат применения порогового значения 0.79, показанный на рис. 12.3г,
успешно классифицирует символы, находящиеся под подсвеченными участками, но остальные символы теряются.
Поскольку ни один порог не подходит для всего изображения, мы можем выбрать порог для каждого пикселя. Такой порог часто называют адаптивным порогом. Обычно порог имеет
вид
tu,v = μ(𝒲u,v) - kσ(𝒲u,v),
где 𝒲u,v – локальное окно вокруг пикселя с координатами
(u, v), а μ(·) и σ(·) – среднее значение и стандартное отклонение соответственно. Результат классификации изображения на
рис. 12.3a
>>> castle2.adaptive_threshold(h=15).disp();

v (пиксели)

с адаптивным порогом показан на рис. 12.4. Все пиксели, принадлежащие буквам, были правильно классифицированы, но,
по сравнению с рис. 12.3в, имеется много ложных срабатываний – фоновых пикселей, классифицированных как объекты.
Позже в этом разделе мы обсудим методы устранения таких
ложных срабатываний. Обратите внимание, что процесс классификации больше не является функцией только входного
пикселя, теперь это сложная функция пикселя и его соседей.
Нам больше не нужно выбирать t, но теперь важно выбрать параметр k и размер окна, и опять же это обычно процесс проб
и ошибок, который можно заставить хорошо работать для конкретной сцены.

u (пиксели)

Рис. 12.4. Результат классификации
с адаптивным порогом

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

12.1.1.2 Классификация цветных изображений
Цвет – мощный параметр для сегментации, но робототехники
склонны избегать его использования из-за проблем с непостоянством цвета, обсуждавшихся в разделе 10.3.2. Далее мы рассмотрим два примера, в которых используются цветные изображения. Первый – довольно примитивная навигационная
цель для эксперимента по посадке БПЛА в помещении
>>> targets = Image.Read("yellowtargets.png", dtype="float",
...
gamma="sRGB");

– показан на рис. 12.5а, а второй – из проекта MIT Robotic Garden
>>> garden = Image.Read("tomato_124.png", dtype="float",
...
gamma="sRGB");

– показан на рис. 12.6а. Наша цель – определить центроиды
желтых мишеней и красных помидоров соответственно. Начальные этапы обработки одинаковы для каждого изображения, но мы подробно рассмотрим процесс для изображения
желтых мишеней, показанных на рис. 12.5.
Первый шаг – устранить влияние общего уровня освещенности сцены путем преобразования цветного изображения
в координаты цветности, которые были представлены в разделе 10.2.5:
>>> ab = targets.colorspace("L*a*b*").plane("a*:b*")
Image: 640 x 480 (float32), a*:b*

Это изображение состоит всего из двух плоскостей – значе­
ний цветности a*b*. Мы выбрали цветность a*b* из раздела 10.2.7, поскольку евклидово расстояние в этом пространстве
соответствует человеческому восприятию разницы между цветами.
Цветовая плоскость b* охватывает диапазон цветов от синего
до желтого с белым в середине
>>> ab.plane("b*").disp();

u (пиксели)

б

u (пиксели)

v (пиксели)

а

v (пиксели)

v (пиксели)

674 Глава 12 · Извлечение признаков изображения

u (пиксели)

г

v (пиксели)

в

д

u (пиксели)

Рис. 12.5. Пример изображения цветной мишени: а) исходное изображение; б) цветовая плоскость b*;
в) классификация пикселей (C = 2), показанная синтетическим цветом; г) центры кластеров в a*b*-пространстве
цветности; д) все пиксели класса c = 0

и показана на рис. 12.5б. Это изображение в оттенках серого,
и применение порогового значения 70 обеспечит хорошую сегментацию.
Беспороговый подход заключается в том, чтобы рассматривать пиксели как набор точек со значениями (a*, b*) ∈ ℝ2, которые можно кластеризовать в наборы с похожей цветностью.
Для поиска кластеров мы будем использовать алгоритм k средних, но нам нужно указать количество кластеров. Воспользу-

Получение признаков из области 675

емся нашим знанием, что данная сцена содержит, по сути, два
элемента разного цвета: желтые цели и все остальное, по сути,
серое: пол, металлическая крышка водостока и тени. Пиксели
кластеризуются в два класса цветности (C = 2)
>>> targets_labels, targets_centroids, resid = \
... ab.kmeans_color(k=2, seed=0)

и мы можем отобразить классификацию пикселей
>>> targets_labels.disp(colormap="jet", colorbar=True);

как изображение в псевдоцветах (рис. 12.5в). Пиксели на этом
изображении имеют значения c = {0, 1}, указывающие, к какому
классу относится соответствующий входной пиксел. Мы видим,
что желтые мишени четко относятся к классу c = 0, который отображается синим цветом.
Функция также возвращает a*b*-цветность центроидов клас­
теров
>>> targets_centroids
array([[ 14.71,
1.106],
[ 73.63, -6.511]], dtype=float32)

по одному столбцу на кластер. Мы можем построить центроиды
этих кластеров на плоскости цветности a*b*
>>> plot_chromaticity_diagram(colorspace="a*b*");
>>> plot_point(targets_centroids, marker="*", text="{}");

как показано на рис. 12.5г. Видно, что кластер 0 наиболее близок к желтому. Вот названия цветов, связанных с центроидами
кластеров:
>>> [color2name(c, "a*b*") for c in targets_centroids.T]

Одним из вариантов ['xkcd:squash', 'ghostwhite']
является запуск
k средних несколько
которые соответствуют желтому и серому.
раз и выбор цент­
Остаток представляет собой сумму расстояний каждой точки
ров кластеров, для
которых невязка от назначенного ей центроида кластера и указывает на комявляется наименьпактность кластеров. Мы можем выполнить нормализацию по
шей. Если число k
заранее неизвестно, количеству пикселей
то можно перебрать
диапазон значений >>> resid / ab.npixels
и выбрать k, при ко- 59.11
тором невязка будет
наименьшей. Для
Поскольку алгоритм использует случайную инициализацию,
получения повторямы
будем получать разные кластеры и классификацию при
ющихся результатов
можно явно задать каждом запуске и, следовательно, разные невязки. ◄
начальное значение
Кластеризация методом k средних требует больших вычисслучайного числа
лительных
ресурсов и поэтому не очень хорошо подходит для
с помощью аргумента seed. приложений реального времени. Однако мы можем разделить

676 Глава 12 · Извлечение признаков изображения

процесс на этап обучения и этап классификации. На этапе
обуче­ния несколько примеров изображений будут объединены
и переданы в kmeans_color, которые будут определять центры
кластеров для каждого класса. Впоследствии мы можем довольно экономно относить пиксели к ближайшему кластеру:
>>> labels = ab.kmeans_color(centroids=targets_centroids)
Image: 640 x 480 (uint8)

Можно выбрать пиксели, принадлежащие классу 0:
>>> objects = (labels == 0)
Image: 640 x 480 (bool)

и получить логическое изображение, которое затем отобразить
>>> objects.disp();

как показано на рис. 12.5д. Все пиксели True отображаются белым цветом и соответствуют желтым мишеням на исходном
изображении.
Применим схожую процедуру к изображению сада на
рис. 12.6а. Цветовая плоскость a* представлена в оттенках серого на рис. 12.6б, и, применив пороговое значение 35, мы могли
бы получить хорошую сегментацию. Мы также могли бы снова
использовать кластеризацию, но на этот раз с тремя кластерами
(C = 3), основываясь на наших знаниях о том, что сцена содержит красные помидоры, зеленые листья и темный фон:
>>> ab = garden.colorspace("L*a*b*").plane("a*:b*")
Image: 318 x 238 (float32), a*:b*
>>> garden_labels, garden_centroids, resid \
... = ab.kmeans_color(k=3, seed=0);
>>> garden_centroids
array([[ -19.46, -1.375, 39.86],
[ 31.04,
2.673, 24.09]], dtype=float32)

Классы пикселей показаны на синтетическом цветном
изобра­жении рис. 12.6в. Пиксели, соответствующие помидору,
были присвоены классу c = 2, которые (по совпадению) отображаются белым цветом. Центры кластеров отмечены на цветовой плоскости a*b* на рис. 12.6г. Названия цветов, связанных
с центрами кластеров:
>>> [color2name(c, "a*b*") for c in garden_centroids.T]
['xkcd:military green', 'xkcd:grey', 'xkcd:dark salmon']

Можно выбрать только красные пиксели
>>> tomatoes = (garden_labels == 2);

и получить логическое изображение, показанное на рис. 12.6д.

u (пиксели)

б

u (пиксели)

v (пиксели)

а

v (пиксели)

v (пиксели)

Получение признаков из области 677

д

г

v (пиксели)

u (пиксели)

v (пиксели)

в

u (пиксели)

е

u (пиксели)

Рис. 12.6. Пример изображения сада: а) исходное изображение (предоставлено проектом Distributed Robot Garden,
Массачусетский технологический институт); б) цветовая плоскость a*; в) классификация пикселей (C = 3), показанная
синтетическим цветом; г) центры кластеров в цветовом пространстве a*b*; д) все пиксели класса c = 2; е) после
морфологического закрытия круговым структурирующим элементом (с радиусом 15)

Эта сегментация далека от совершенства. Оба помидора
имеют отверстия из-за белого пятна зеркального отражения,
обсуждавшегося в разделе 10.3.5. Несколько пикселей в левом
нижнем углу, наоборот, ошибочно классифицированы как помидор. Мы можем улучшить результат, применив операцию
морфологического закрытия с большим круглым ядром, которое соответствует форме помидора
>>> tomatoes_binary = tomatoes.close(Kernel.Circle(radius=15));
>>> tomatoes_binary.disp();

678 Глава 12 · Извлечение признаков изображения

Результат показан на рис. 12.6е. Операция закрытия улучшила форму помидора, но с нежелательным побочным эффектом
в виде увеличения группы ошибочно классифицированных
пикселей в нижнем левом углу. Тем не менее это изображение
содержит работающую классификацию пикселей на два класса:
помидоры и не помидоры.
Изображение сада иллюстрирует два распространенных артефакта изображения реального мира: зеркальное отражение
и окклюзию (перекрытие). Поверхность помидора достаточно
блестящая и ориентирована так, что камера видит отражение
комнатного света – эти пиксели белые, а не красные. Верхний помидор также частично закрыт листьями и ветвями. В за-

Обратите внимание,
что они имеют ту
же цветность, что
и черный фон, пиксели класса 1, расположенные близко
к белой точке на
плоскости a∗b∗.

Отступление 12.1. Кластеризация методом k средних
Метод k средних – это итерационный алгоритм для группировки n-мерных точек
в k пространственных кластеров. Каждый кластер определяется центральной точкой, являющейся n-мерным вектором ci ∈ ℝn, i = 0, ..., k - 1. В каждой итерации все
точки присваиваются ближайшему центру кластера, основываясь на евклидовом
расстоянии, и затем каждый центр кластера обновляется, чтобы быть средним
значением всех точек, назначенных кластеру.
Для демонстрации выберем 500 случайных двухмерных точек
>>> data = np.random.rand(500, 2);
где data – мат­ри­ца 500×2 с одной точкой на строку. Мы сгруппируем эти данные
в три набора:
>>> from scipy.cluster.vq import kmeans2
>>> centroids, labels = kmeans2(data, k=3)
где labels – вектор из 500 элементов, которые определяют класс соответствующей
строки мат­ри­цы data. Далее, centroids – это мат­ри­ца 3×2, строки которой определяют центр каждого двухмерного кластера.
Нарисуем точки в каждом кластере
разными цветами:
>>> for i in range(3):
... plot_point(data[labels==i, :].T,
... color="rgb"[i], marker=".",
... markersize=10)
– и становится видно, что точки разделены вполне обоснованно. Центроиды кластеров наложены на изображение в виде черных точек.
Алгоритм k средних требует начальной оценки центра каждого клас­
тера, что обычно достигается с помощью случайных чисел (подробности
см. в документации SciPy). В результате алгоритм может возвращать разные результаты при каждом запуске.

Получение признаков из области 679

Цвет интересующего объекта известен
(в этих примерах
желтый и красный
соответственно). Мы
могли бы использовать функцию name2color, чтобы найти
его, а затем выбрать
кластер с ближайшим центром.

висимости от того, как работает приложение, это может быть
проб­лемой, а может и не быть. Поскольку до помидора все равно нельзя добраться с того направления, в котором был сделан
снимок, из-за мешающих листьев, на самом деле может быть
уместнее не классифицировать его как помидор.
В этих примерах получена работающая классификация пикселей изображения на объекты и необъекты. Полученные группы белых пикселей часто называют блобами (от англ. слова
blob, пятно, и одновременно «binary large object», большой бинарный объект). Интересно отметить, что мы не указали порог
или какое-либо определение цвета объекта, но нам нужно было
указать количество классов и определить, какой из этих класНам также пришлось
сов соответствует нужным объектам.
выбирать последовательность шагов обработки изображения
и параметры каждого из этих шагов, например радиус структурирующего элемента. Классификация пикселей – сложная задача, но мы можем получить неплохие результаты, используя
априорное знание задачи, хороший набор приемов обработки
изображений и опыт.
Отступление 12.2. Зеркальные блики
Зеркальные блики на изображениях являются отражением ярких
источников света и могут усложнить сегментацию, как показано
на рис. 12.3 и 12.6д.
Как обсуждается в разделе 13.10, свет, отраженный большинством реальных объектов, имеет две составляющие: зеркальное
отражение от поверхности, которое не изменяет спектр света;
и диффузное отражение тела, которое фильтрует отраженный
свет.
Есть несколько способов уменьшить проблему зеркальных
бликов. Во-первых, переместить или удалить проблемный источник света либо переместить камеру. Во-вторых, использовать источник рассеянного света рядом с камерой, например кольцевой
осветитель, который надевается на объектив камеры. В-третьих,
можно ослабить зеркальное отражение с помощью поляризационного фильтра, поскольку свет, зеркально отраженный от ди­
электрической поверхности, будет поляризован.

12.1.1.3 Семантическая классификация

Это основа совершенно новой отрасли по маркировке
данных.

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

680 Глава 12 · Извлечение признаков изображения

зультат прогнозирования для набора обучающих изображений
максимально соответствовал данным, маркированным человеком. Обучение может занять от нескольких часов до нескольких
дней на графическом процессоре (GPU), а результатом становится модель нейронной сети с миллионами параметров.
Использование обученной нейронной сети для создания набора выходных данных, соответствующих входному изображению, называется выводом, или прогнозом. Процесс прогнозирования выполняется относительно быстро, обычно за доли
секунды, и не требует использования графического процессора.
Нейронные сети обладают способностью к обобщению – распознаванию объекта, даже если они никогда ранее не встречали
точно такое же изображение. Они не идеальны, но могут показывать весьма впечатляющие результаты.
Подробное описание архитектуры нейронных сетей и процедуры их обучения выходит за рамки этой книги, однако
существуют уже обученные сети, которые можно применить
к изображению. Семантическая классификация заключается
в маркировке пикселей типом объекта, которому они принадлежат, например человек, автомобиль, мотоцикл и т. д. В следующих примерах мы используем пакет PyTorch и сверточную
сеть FCN-ResNet из него, предварительно обученную на наборе
данных PASCAL VOC, ► содержащем 20 классов объектов, включая фон, человека, велосипед, машину, кошку, собаку и овцу.
Первым шагом загрузим изображение для сегментирования
>>> scene = Image.Read("image3.jpg")
Image: 640 x 438 (uint8), R:G:B [.../images/image3.jpg]
>>> scene.disp();

Visual Object Classes
(классы визуальных объектов)
см. http://host.
robots.ox.ac.uk/
pascal/VOC.

показанное на рис. 12.7а. Это сложная сцена, и все изученные
нами к данному моменту подходы не в состоянии более или
менее осмысленно сегментировать ее. Предполагая, что пакет
PyTorch установлен, импортируем необходимые модули:
>>> import torch
>>> import torchvision as tv

затем нормализуем изображение, чтобы оно имело такое же
распределение значений пикселей, как и обучающие изображения, а потом преобразуем его в тензор PyTorch ►
>>> transform = tv.transforms.Compose([
...
tv.transforms.ToTensor(),
...
tv.transforms.Normalize(mean=[0.485, 0.456, 0.406],
...
std=[0.229, 0.224, 0.225])]);
>>> in_tensor = transform(scene.image);

Далее загрузим сеть FCN-ResNet, настроим ее для прогнозирования и передадим ей изображение ►

Тензор PyTorch –
это многомерный
NumPy-подобный
массив с поддержкой операций на
графическом процессоре.

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

в

б

u (пиксели)

v (пиксели)

u (пиксели)

v (пиксели)

а

v (пиксели)

v (пиксели)

Получение признаков из области 681

u (пиксели)

г

u (пиксели)

Рис. 12.7. Семантическая сегментация с использованием ResNet-50: а) исходная сцена; б) классификация пикселей;
в) бинарное изображение, показывающее пиксели, принадлежащие классу «человек»; г) входные пиксели,
принадлежащие классу «человек» (изображение включено как часть https://github.com/OlafenwaMoses/
ImageAI)

>>> model = tv.models.segmentation.fcn_resnet50(pretrained=True)\
...
.eval();
>>> outputs = model(torch.stack([in_tensor]));

и затем преобразуем полученный тензор в экземпляр Image
>>> labels = Image(torch.argmax(outputs["out"].squeeze(),
...
dim=0).detach().cpu().numpy());
>>> labels.disp(colormap="viridis", ncolors=20, colorbar=True);

Результат показан на рис. 12.7б. Класс «человек» в PASCAL
В Pascal VOC
имеется 20 классов VOC обозначается порядковым значением 15,
и мы можем
изображений,
выбрать
и
замаскировать
все
пиксели,
соответствующие
людям
пронумерованных
начиная с нуля: фон,
самолет, велоси- >>> (labels == 15).disp();
пед, птица, лодка, >>> scene.choose("white", labels != 15).disp();
бутылка, автобус,
машина, кот, стул, как показано на рис. 12.7в и г соответственно.
корова, обеденный
стол, собака, лошадь,
мотоцикл, человек,
растение в горшке,
овца, диван, поезд,
телевизор, монитор.

12.1.2

Представление экземпляра объекта

В предыдущем разделе мы брали черно-белые или цветные
изобра­жения и обрабатывали их для создания бинарных изобра­

682 Глава 12 · Извлечение признаков изображения

u (пиксели)

б

u (пиксели)

v (пиксели)

а

v (пиксели)

v (пиксели)

жений, содержащих одну или несколько областей белых пикселей. Взглянув на бинарное изображение на рис. 12.8a, мы увидим четыре белых объекта, но изображение представляет собой
всего лишь массив черных и белых пикселей, то есть понятие
«объекта» в нем отсутствует. Говоря об объектах, мы подразумеваем набор пикселей одного класса, соседствующих или связан­
ных друг с другом. Более формально мы могли бы сказать, что
объект – это пространственно-непрерывная область пикселей
одного класса. Объекты в бинарных сценах обычно называют
блобами, пятнами, областями или связанными компонентами.
В этом разделе мы рассмотрим задачу распределения пикселей
по пространственным множествам S0, ..., Sm-1, которые соответствуют человеческому представлению об объектах, тут m – количество объектов в сцене.
Если рассматривать классы объектов, то на рис. 12.8а представлены только два типа объектов: «буква S» и «акула», но имеется три экземпляра класса «акула» и один экземпляр объекта
«буква S». Термины «объект» и «экземпляр» часто используются
как взаимозаменяемые.

в

u (пиксели)

Рис. 12.8. Пример маркировки изображения: а) бинарное изображение; б) маркированное изображение;
в) все пиксели с меткой 3

Получение признаков из области 683

12.1.2.1 Создание бинарных блобов
Рассмотрим бинарное изображение
>>> sharks = Image.Read("sharks.png");
>>> sharks.disp();

показанное на рис. 12.8а. Свяжем пиксели с пространственными множествами
>>> labels, m = sharks.labels_binary()
>>> m
5

и в результате получим пять блобов. Результат маркировки
пикселей в labels представляет собой изображение того же размера, что и бинарное изображение.
>>> labels.disp(colorbar=True);

как показано на рис. 12.8б. Каждая связанная область имеет
уникальную метку и, следовательно, уникальный цвет. Процесс
назначения меток часто называют анализом связности, маркировкой блобов или раскраской блобов. Каждый пиксель на
изоб­ражении labels имеет целочисленное значение в диапазоне
[0, 5], которое является меткой пространственно непрерывного
множества, или блоба, которому принадлежит соответствующий
входной пиксель. Глядя на значения меток на рисунке или интер­
активно исследуя значения пикселей на отображаемом изображении, можно заметить, что фон имеет метку 0, а правая акула –
метку 3. Впервые мы смогли количественно оценить структуру
изображения: имеется пять объектов, один из которых – фон.
Теперь не составит труда получить изображение, содержащее
только конкретный блоб. Чтобы выбрать все пиксели, принадлежащие нужной акуле, создадим логическое изображение
>>> right_shark = (labels == 3);
>>> right_shark.disp();
Восьмисторонняя связность
может привести
к неожиданным
результатам. Например, черно-белая
шахматная доска
будет иметь только
две области; все белые квадраты – это
одна область, а все
черные квадраты –
другая.

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

12.1.2.2 М
 аксимально устойчивые экстремальные области
(MSER)
Результаты, представленные на рис. 12.3, оказались разочаровывающими, потому что ни один порог не смог классифици-

684 Глава 12 · Извлечение признаков изображения

ровать все пиксели символа. Однако, используя разные пороги,
можно правильно классифицировать все символы. Фактически
каждая область символа правильно сегментируется некоторым
диапазоном порогов, и было бы желательно иметь возможность
объединить области, классифицированные в диапазоне всех
порогов. Алгоритм максимально устойчивой экстремальной
области (Maximally Stable Extremal Regions), или MSER, делает
именно это. Он позволяет выполнить маркировку областей на
черно-белом изображении всего за один шаг
>>> labels, m = castle2.labels_MSER()

И для этого изображения было обнаружено
>>> m
394

множеств.

Изображение labels

>>> labels.disp(colormap="viridis_r", ncolors=m);

v (пиксели)

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

u (пиксели)

12.1.2.3 Графовая сегментация
Рассмотрим более сложную сцену
>>> grain = Image.Read("58060.png")
Image: 481 x 321 (uint8), R:G:B [.../images/58060.png]
>>> grain.disp();

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

Рис. 12.9. Сегментация
с использованием максимально
устойчивых экстремальных
областей (MSER). Выделенные
области имеют уникальную
цветовую кодировку. Некоторые
нетекстовые области слабо
видны на левом и правом краях

Получение признаков из области 685

На сайте сегмен­
тации Беркли
http://www.eecs.
berkeley.edu/
Research/
Projects/CS/
vision/bsds размещены эти изображения, а также ряд
различных сегментаций, созданных
человеком.

показанную на рис. 12.10а. Гештальт-принцип эмерджентности
гласит, что мы идентифицируем объекты как целое, а не как набор частей – мы скорее видим чашу с зерном, чем выводим чашу
с зерном, узнавая ее отдельные компоненты. Однако когда дело
доходит до подробной попиксельной сегментации, начинается
субъективизация – разные люди выполняют сегментацию поразному, основываясь на суждениях о том, что важно. Например, следует ли сегментировать цветные полосы на ткани? Если
сегменты представляют объекты реального мира, то гештальтпредставление будет заключаться в том, что ткань должна быть
только одним сегментом. Однако полосы существуют, для их
создания были предприняты определенные усилия, поэтому,
возможно, их следует выделить. Вот почему сегментация является сложной задачей – люди не могут договориться о том, что
правильно. А от компьютерного алгоритма мы и подавно не можем ожидать суждений подобного рода.
Графовая сегментация маркирует области на цветном изображении всего за один шаг:
>>> labels, m = grain.labels_graphseg()
>>> m
27

и в нашем случае находит 27 областей. Изображение labels
>>> labels.disp(colormap="viridis_r", ncolors=m);

v (пиксели)

v (пиксели)

показано на рис. 12.10б. Этап классификации пикселей интег­
рирован в этап представления.
Графовая сегментация представляет изображение в виде графа (см. приложение I), где каждый пиксель является вершиной
и имеет 8 ребер, соединяющих его с соседними пикселями. Вес
каждого ребра – это неотрицательная мера несходства между
двумя пикселями – абсолютное значение разницы в цвете. Алгоритм начинается с того, что назначает каждую вершину свое­

u (пиксели)
u (пиксели)
а
б
Рис. 12.10. Пример сложной сегментации: а) исходное цветное изображение (получено из набора данных
сегментации Berkeley segmentation Dataset; Martin et al. 2001); б) сегментация на основе графа

686 Глава 12 · Извлечение признаков изображения

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

12.1.3

Описание экземпляра объекта

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

12.1.3.1 Площадь
Размер, или площадь, блоба определяется простым количест­
вом пикселей. Для бинарного изображения можно просто суммировать количество пикселей
>>> right_shark.sum()
7728

поскольку пиксели объекта имеют значение True (1), а пиксели
фона – значение False (0).

12.1.3.2 Ограничивающие рамки
Полезной мерой размера и формы блоба является ограничивающая рамка – наименьший прямоугольник со сторонами,
параллельными осям u и v, который окружает область. Координаты [u, v] всех ненулевых (объектных) пикселей являются соответствующими элементами:
>>> u, v = right_shark.nonzero()

где u и v – одномерные массивы одинакового размера:
>>> u.shape
(7728,)

Получение признаков из области 687

Границы области:
>>>
442
>>>
580
>>>
124
>>>
234

umin = u.min()
umax = u.max()
vmin = v.min()
vmax = v.max()

Эти границы определяют прямоугольник, который мы можем наложить на изображение
>>> plot_box(lrbt=[umin, umax, vmin, vmax], color="g");

v (пиксели)

v (пиксели)

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

u (пиксели)
u (пиксели)
а
б
Рис. 12.11. Одна акула (блоб 3), выделенная из бинарного изображения на рис. 12.8а:
а) ограничивающий прямоугольник (зеленый), центроид (синий) и эквивалентный эллипс
(циановый) для одной акулы; б) увеличенное изображение

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

688 Глава 12 · Извлечение признаков изображения

где (p + q) – порядок момента. Нулевой момент p = q = 0 вычисляется из выражения
(12.2)
а для бинарного изображения, где фоновые пиксели равны ну­
лю, это просто количество ненулевых (белых) пикселей – площадь области.
Моменты рассчитываются с помощью метода Image.mpq. Для
одиночной акулы из нашего примера нулевой момент равен:
>>> m00 = right_shark.mpq(0, 0)
7728

что является площадью области в пикселях.
Моментам можно дать физическую интерпретацию, рассмат­
ривая функцию изображения как распределение массы. Представьте, что акула представляет собой тонкую пластину, где
каждый пиксель имеет одну единицу площади и одну единицу
массы. Общая масса области равна m00, а центр масс (центроид)
области равен:
(12.3)
где m10 и m01 – моменты первого порядка. Для нашего примера
центроид целевой области имеет координаты:
>>> uc = right_shark.mpq(1, 0) / m00
502.5
>>> vc = right_shark.mpq(0, 1) / m00
183.7

Мы можем отобразить его:
>>> plot_point((uc, vc), ["bo", "bx"]);

как показано на рис. 12.11.
Центральные моменты μpq вычисляются относительно цент­
роида
(12.4)

и инвариантны к местоположению области. Они связаны с моментами mpq соотношением

Получение признаков из области 689

(12.5)
и вычисляются методом Image.upq.
Снова воспользуемся аналогией с тонкой пластиной. Инерция области относительно осей, параллельных осям u и v и пересекающихся в центре тяжести области, задается симметричной мат­ри­цей
(12.6)
Центральные вторые моменты μ20, μ02 – это моменты инерции, μ11 – произведение инерции. Произведение инерции отлично от нуля, если форма несимметрична относительно осей
uv области.
Эквивалентный эллипс – это эллипс, который имеет тот же
тензор инерции, что и область. Для нашего примера:
>>> u20 = right_shark.upq(2, 0); u02 = right_shark.upq(0, 2);
>>> u11 = right_shark.upq(1, 1);
>>> J = np.array([[u20, u11], [u11, u02]])
array([[7.83e+06, -2.917e+06],
[-2.917e+06, 4.733e+06]])

и мы можем наложить эквивалентный эллипс на область:
>>> plot_ellipse(4 * J / m00, centre=(uc, vc), inverted=True,
...
color="blue");

Результат показан на рис. 12.11.
Собственные значения и собственные векторы J связаны
с радиусами эллипса и ориентацией его большой и малой осей
(см. раздел C.1.4). Для этого примера собственные значения
>>> lmbda, x = np.linalg.eig(J)

NumPy не гаран>>> lmbda
тирует возврат
собственных array([9.584e+06, 2.979e+06])
значений в какомто определенном являются главными моментами инерции области. Максимальпорядке. Функция ный и минимальный радиусы эквивалентного эллипса равны
np.linalg.eigh
при применении
к симметричной
(12.7)
мат­ри­це возвращает собственные
значения в порядке
возрастания. соответственно, где λ2 ≥ λ1. На Python это можно выразить так :

690 Глава 12 · Извлечение признаков изображения
>>> a = 2 * np.sqrt(lmbda.max() / m00)
70.43
>>> b = 2 * np.sqrt(lmbda.min() / m00)
39.27

в единицах пикселей. Эти длины характерны для данной конк­
ретной формы и инвариантны к вращению. Соотношение сторон региона
>>> b / a
0.5575

– это скаляр, приближенно характеризующий форму и инвариантный к местоположению, масштабу и вращению.
Собственные векторы J – это главные оси эллипса, т. е. направления его большой и малой осей. Большая, или главная,
ось – это собственный вектор υ, соответствующий максимальному собственному значению. Для нашего примера собственные векторы – это столбцы
>>> x
array([[ 0.857, 0.5153],
[ -0.5153, 0.857]])

и соответствующий собственный вектор равен
>>> i = np.argmax(lmbda) # get index of largest eigenvalue
0
>>> v = x[:, i]
array([ 0.857, -0.5153])

Угол этого вектора относительно горизонтальной оси равен:
,
и для нашего примера он имеет величину
>>> np.rad2deg(np.arctan2(v[1], v[0]))
-31.02

градусов, что указывает на то, что большая ось эквивалентного
эллипса находится примерно на 30° выше горизонтали. ►
Подведем краткий итог. Мы создали изображение, содержащее пространственно-непрерывный набор пикселей, соответствующих одному из объектов в сцене, который мы выделили из
исходного цветного изображения. Мы определили его площадь,
построили описывающий прямоугольник, нашли его положение (местоположение центра тяжести), его ориентацию и его
форму (соотношение сторон). Некоторые параметры зависят
только от формы области и не зависят от положения и ориентации объекта или инвариантны к ним. Соотношение сторон –

Согласно рис. C.2b,
угол увеличивается
по часовой стрелке
от горизонтали,
поскольку ось
y изоб­ражения
направлена вниз,
а ось z – вглубь
(от читателя).

Получение признаков из области 691

это очень грубая мера формы, и дополнительные меры, или инварианты, приведены в табл. 12.1, которая будет представлена
далее в этом разделе.
Таблица 12.1. Признаки области и их инвариантность к движению
камеры: перемещение, вращение вокруг центра тяжести объекта
и масштабирование
Площадь
Центроид
Ориентация θ
Отношение сторон
Округлость
Моменты

Перемещение
ü
û
ü
ü
ü
ü

Вращение
ü
ü
û
ü
ü
ü

Масштабирование
û
ü
ü
ü
ü
ü

12.1.3.4 Описательные параметры блобов
В предыдущих разделах мы преодолели длительный процесс
выделения одного блоба в сцене и вычисления некоторых его
описательных параметров. Однако есть более простой способ
получить ту же информацию для всех блобов в сцене – применить метод blobs к бинарному изображению
>>> blobs = sharks.blobs();

который возвращает объект Blobs, содержащий описательные
параметры множества блобов в табличной форме
>>> blobs
id
parent
0
-1
1
-1
2
-1
3
-1

centroid
245.6, 425.9
502.3, 183.9
82.9, 159.6
297.0, 180.0

area
1.85e+04
7.51e+03
7.52e+03
1.44e+04

touch
False
False
False
False

perim
811.2
514.8
513.4
1235.4

circul
0.392
0.396
0.399
0.132

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

где id – это метка области. Объект ведет себя как список в Python, а его длина
>>> len(blobs)
4

указывает, что он описывает 4 блоба. Его можно разбить на срезы или выполнить итерации, например
>>> blobs[3]
id
parent
3
-1

centroid
297.0, 180.0

area
1.44e+04

touch
False

perim
1235.4

circul
0.132

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

692 Глава 12 · Извлечение признаков изображения

Каждый блоб имеет множество атрибутов, доступных как
свойства
>>> blobs[3].area
1.436e+04
>>> blobs[3].umin
214
>>> blobs[3].aspect
0.7013
>>> blobs[3].centroid
(297, 180)

Свойство touch имеет значение True, если блоб касается границы изображения. Моменты определены в виде именованного кортежа:
>>> blobs[3].moments.m00 # момент p=q=0
1.436e+04
>>> blobs[3].moments.mu11 # центральный момент p=q=1
3.187e+06
>>> blobs[3].moments.nu03 # нормализованный центральный момент p=0, q=3
-0.004655

В данном случае blobs[3] – это экземпляр Blobs, содержащий
только один объект, но если обратиться к свойствам экземпляра
Blobs, содержащего несколько блобов, то результатом будет массив с атрибутом для всех блобов. Например:
>>> blobs.area
array([1.847e+04, 7510, 7523, 1.436e+04])

– это одномерный массив с площадями всех блобов.
Объект также имеет методы, например
>>> blobs[3].plot_box(color="red")

нарисует красную ограничивающую рамку вокруг blob[3], а
>>> blobs[:2].plot_box(color="red")

нарисует красную рамку вокруг первых двух блобов. Вот как
можно добавить центроиды и ограничивающие рамки ко всем
блобам:
>>> blobs.plot_centroid(marker="+", color="blue")
>>> blobs.plot_box(color="red")

Результат показан на рис. 12.12а.
Мы можем извлечь интересующую нас область с определенным фрагментом и повернуть ее так, чтобы ее главная ось была
горизонтальной:
>>> sharks.roi(blobs[1].bbox).rotate(blobs[1].orientation).disp();

Получение признаков из области 693

v (пиксели)

v (пиксели)

Результат показан на рис. 12.12б.

u (пиксели)

u (пиксели)
а
б
Рис. 12.12. а) Графические аннотации для блобов, показанных на рис. 12.8, с центроидами,
ограничивающими рамками и метками; б) блоб 1, извлеченный из изображения
и повернутый так, что его главная ось стала горизонтальной

Объект Blobs также можно разбить на логические массивы:
>>> blobs[blobs.area > 10_000]
id
parent
centroid
0
-1
245.6, 425.9
3
-1
297.0, 180.0

area
1.85e+04
1.44e+04

touch
False
False

perim
811.2
1235.4

circul
0.392
0.132

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

В данном случае будут выбраны блобы с размером более
10 000 пикселей. Также существует метод фильтрации, и для
изображения помидора
>>> tomato_blobs = tomatoes_binary.blobs()
id
parent
centroid
area
0
-1
8.9, 235.1
39
1
-1
90.7, 206.5
2.56e+03
2
-1
130.2, 126.9
1.14e+03

touch
True
False
False

perim
37.9
201.5
144.5

circul
0.380
0.880
0.763

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

мы могли бы установить границы допустимой площади:
>>> tomato_blobs.filter(area=(1_000, 5_000))
id
parent
centroid
area
1
-1
90.7, 206.5
2.56e+03
2
-1
130.2, 126.9
1.14e+03

touch
False
False

perim
201.5
144.5

circul
0.880
0.763

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

694 Глава 12 · Извлечение признаков изображения

чтобы исключить мелкие блобы, образованные шумом, выбрав
только блобы с площадью между 1000 и 5000 пикселей. Также
есть возможность фильтровать по соотношению сторон и касанию границы. Более подробная информация представлена
в онлайн-документации. Вот как можно исключить блобы, касающиеся границы:
>>> tomato_blobs.filter(touch=False)
id
parent
centroid
area
1
-1
90.7, 206.5
2.56e+03
2
-1
130.2, 126.9
1.14e+03

touch
False
False

perim
201.5
144.5

circul
0.880
0.763

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

Правила фильтрации можно объединять, например:
>>> tomato_blobs.filter(area=[1000, 5000], touch=False, color=1)
id
parent
centroid
area
touch
perim
1
-1
90.7, 206.5
2.56e+03
False
201.5
2
-1
130.2, 126.9
1.14e+03
False
144.5

circul
0.880
0.763

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

В результате будут возвращены блобы, соответствующие
всем правилам. Метод sort сортирует блобы по таким критериям, как площадь, соотношение сторон, длина периметра и форма окружности.

12.1.3.5 Иерархия блобов
Рассмотрим бинарное изображение
>>> multiblobs = Image.Read("multiblobs.png");
>>> multiblobs.disp();

которое показано на рис. 12.13а. Как мы уже делали это ранее,
выполним маркировку областей:
>>> labels, m = multiblobs.labels_binary()
>>> m
6
>>> multiblobs.disp();

как показано на рис. 12.13б. Здесь мы видим белые блобы
и черный фон, а черные блобы на фоне белых воспринимаются как дыры. Эти дыры, в свою очередь, могут содержать белые
блобы, которые также могут содержать дыры. Проще говоря,
существует иерархия черных и белых блобов. Анализ блобов,
представленный в разделе 12.1.3.4, включает информацию об
иерархии:

Получение признаков из области 695
>>> blobs = multiblobs.blobs()
id
parent
centroid
0
-1
907.9, 735.1
1
0
1025.0, 813.7
2
1
938.1, 855.2
3
1
988.1, 697.2
4
0
846.0, 511.7
5
-1
291.7, 377.8
6
5
312.7, 472.1
7
5
241.9, 245.0
8
-1
1228.0, 254.3
9
8
1225.2, 220.0

area
1.94e+05
1.06e+05
1.72e+04
1.21e+04
1.75e+04
1.7e+04
1.75e04
1.7e+04
8.14e+04
1.75e+04

touch
False
False
False
False
False
False
False
False
False
False

perim
2219.0
1386.9
489.7
411.5
495.9
1711.6
494.5
495.9
1214.2
495.9

circ
0.5
0.7
1.0
0.9
0.9
0.8
1.0
0.9
0.7
0.9

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

Второй столбец – это идентификатор родительского, то есть
вмещающего, блоба. Блоб с идентификатором -1 – это фон, содержащий все белые блобы, и для него не вычисляются никакие
параметры. Дополнительные свойства блоба включают:

u (пиксели)

б

u (пиксели)

v (пиксели)

а

v (пиксели)

v (пиксели)

>>> blobs[1].children
[2, 3]
>>> blobs[1].parent
0

в

u (пиксели)

Рис. 12.13. Пример маркировки изображения: а) бинарное изображение; б) маркированное изображение;
в) маркированное изображение с областями в иерархическом представлении

696 Глава 12 · Извлечение признаков изображения

Мы можем превратить эти иерархические данные в изображение, отражающее иерархию:
>>> blobs.label_image().disp();

как показано на рис. 12.13в, где внутренние черные области
теперь имеют отдельные метки. Всего имеется 11 уникальных
меток: 10 перечислены в таблице выше и один фоновый блоб
с идентификатором -1. Значения меток представляют указанный выше идентификатор плюс единица.
Иерархию блобов можно также отобразить в виде графа
>>> blobs.dotfile(show=True);

как показано на рис. 12.14.
–1

0

1

2

4

5

6

8

7

9

Рис. 12.14. Иерархия блобов
в виде дерева. Узлы подписаны
идентификаторами блобов, где –1
соответствует фону

3

12.1.3.6 Информация о форме из моментов
Чтобы распознавать определенные объекты, нам нужны показатели формы, инвариантные к повороту и масштабу изображения. Вернемся вновь к изображению акулы:
>>> blobs = sharks.blobs()
id
parent centroid
0
-1
245.6, 425.9
1
-1
502.3, 183.9
2
-1
82.9, 159.6
3
-1
297.0, 180.0

area
1.85e+04
7.51e+03
7.52e+03
1.44e+04

touch
False
False
False
False

perim
811.2
514.8
513.4
1235.4

circul
0.392
0.396
0.399
0.132

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

>>> blobs.aspect
array([ 0.5609, 0.5619,

0.562, 0.7013])

Как видите, сцена содержит две разные фигуры: три блоба
с соотношением сторон около 0.56 и один с соотношением около 0.70.

Получение признаков из области 697

Соотношение сторон не имеет особого значения, и более точное описание формы можно получить из соотношений моментов, инвариантных к местоположению, ориентации и масштабу. Для этой сцены семь инвариантов момента Hu для блобов
>>> blobs.humoments
[array([ 0.21, 0.01199, 0.001997,
6.149e-05, 1.245e-07]),
array([
0.21, 0.01193, 0.002015,
6.115e-05, 1.291e-07]),
array([ 0.2097, 0.01189, 0.001979,
5.989e-05, 1.199e-07]),
array([ 0.4705, 0.02568, 0.000624,
-1.041e-06, -1.232e-10])]
На практике дискретная природа
пикселей означает,
что инвариантность
будет лишь приблизительной.

0.0005748, 6.032e-07,
0.0005745, 6.045e-07,
0.0005618, 5.801e-07,
1.115e-05, 9.217e-10,

можно считать цифровыми подписями блобов. Они явно указывают на сходство блобов 0, 1 и 2, несмотря на их разное местоположение, ориентацию и масштаб. Они также указывают на отличие формы блоба 3. Этот описатель формы можно
рассматривать как точку в ℝ7, а сходство с другими формами
можно определить с точки зрения евклидова расстояния в пространстве описателей. Если бы мы знали, что в сцене всего два
объекта разной формы, то могли бы применить кластеризацию
методом k средних.
Отступление 12.3. Инварианты моментов
Нормированные моменты
(12.8)
инвариантны к перемещению и масштабированию и вычисляются из центральных моментов с помощью методов класса Image.
Моменты третьего порядка позволяют создавать параметры,
которые инвариантны к переносу, масштабу и ориентации в пределах плоскости. Один из таких наборов моментов определяется
формулой Hu (1962)
φ1 = ν20 + ν02
φ2 = (ν20 - ν02)2 + 4ν112
φ3 = (ν30 - 3ν12)2 + (3ν21 - ν03)2
φ4 = (ν30 + ν12)2 + (ν21 + ν03)2
φ5 = (ν30 - 3ν12)(ν30 + ν12)[(ν30 + ν12)2 - 3(ν21 + ν03)2]
+ (3ν21 - ν03)(ν21 + ν03)[3(ν30 + ν12)2 - (ν21 + ν03)2]
φ6 = (ν20 - ν02)[(ν30 + ν12)2 - (ν21 + ν03)2] + 4ν11(ν30 + ν12)(ν21 + ν03)
φ7 = ( 3ν21 - ν03)(ν30 + ν12)[(ν30 + ν12)2 - 3(ν21 + ν03)2]
+ (3ν12 - ν30)(ν21 + ν03)[3(ν30 + ν12)2 - (ν21 + ν03)2]
и вычисляется методом humoments класса Image и класса Blobs.

698 Глава 12 · Извлечение признаков изображения

12.1.3.7 Информация о форме из периметра
Форма области кратко описывается ее границей или пикселями
периметра, иногда называемыми краями. На рис. 12.15 показаны три распространенных способа представления перимет­
ра области, каждый из которых дает немного отличающуюся
оценку длины периметра. Цепной код (chain code) – это список
самых удаленных пикселей области, центры которых связаны
короткими отрезками. В цепном коде с четырьмя соседями последовательные пиксели должны быть смежными, а сегменты
периметра имеют ориентацию k × 90°, где k ∈ {0, 1, 2, 3}. В цепном коде с восемью соседями или цепном коде Фримена сегменты периметра имеют ориентацию k × 45°, где k ∈ {0, ⋯, 7}.
Код зазоров (crack code) располагает сегменты в зазорах между
пикселями на краю области и пикселями вне области. Они имеют ориентацию k × 90°, где k ∈ {0, 1, 2, 3}.

а

б

в

Рис. 12.15. Представления периметра с пикселями области показаны серым цветом, сегменты периметра – синим,
а центры граничных пикселей отмечены красными точками: а) цепной код с четырьмя направлениями; б) цепной
код Фримена с восемью направлениями; в) код зазоров. Длина периметра для этого примера составляет,
соответственно, 14, 12.2 и 18 пикселей

Периметр может быть закодирован как список координат
пикселей (ui, vi) или очень компактно как битовая строка, использующая всего 2 или 3 бита для представления k для каждого
сегмента. Эти различные представления эквивалентны, и любое представление может быть преобразовано в другое.
 братите внимание, что для цепных кодов граница проходит по
О
пути, который в среднем находится на половине пикселя внутри истинной границы, и поэтому длина периметра занижается. Ошибка
наиболее значительна для небольших областей.

Объект блоба содержит информацию о периметре, ►который
включает список точек периметра. В данном случае имеется
>>> blobs[1].perimeter.shape
(2, 435)

435 точек периметра. Первые пять точек периметра блоба 1

Получается с помощью OpenCVфункции findContours, которая
по умолчанию
возвращает цепной
код для 8 соседей.

Получение признаков из области 699
>>> blobs[1].perimeter[:, :5]
array([[559, 558, 558, 558, 557],
[124, 125, 126, 127, 128]], dtype=int32)

представлены в виде двухмерного массива, в котором каждый
столбец соответствует точке периметра. Периметр можно наложить на текущую диаграмму
>>> blobs[1].plot_perimeter(color="orange")

в виде оранжевой линии. Методы построения диаграмм также
можно применить ко всем блобам:
>>> sharks.disp();
>>> blobs.plot_perimeter(color="orange")
>>> blobs.plot_centroid()

v (пиксели)

как показано на рис. 12.16.

Рис. 12.16. Границы (оранжевые)
и центроиды четырех блобов

u (пиксели)

Длина периметра с учетом угла поворота каждого сегмента
периметра равна
>>> p = blobs[1].perimeter_length
514.8

Отсюда можно вычислить меру округлости – часто используемую и интуитивно понятную характеристику формы. Она
определяется как
(12.9)
где ρ – длина периметра области. Округлость имеет максимальное значение ρ = 1 для круга, ρ = π/4 для квадрата и ноль для

700 Глава 12 · Извлечение признаков изображения

бесконечно длинной линии. Округлость также инвариантна
к перемещению, вращению и масштабированию. Для изображений акул, показанных выше:
>>> blobs.circularity
array([ 0.3923, 0.3962,

0.399,

0.1315])

мы снова видим, что блоб 3 имеет форму, отличающуюся от
других блобов. Его мера округлости меньше из-за того, что фактически он представляет собой длинную линию.
Каждый объект имеет одну внешнюю границу, которая может включать часть границы изображения, если объект касается
границы. Объект с отверстиями имеет одну внутреннюю границу для каждого отверстия, но Toolbox возвращает только внешнюю границу – внутренние границы можно найти как внешние
границы отверстий, которые являются дочерними областями.
Поскольку внешняя граница содержит всю необходимую информацию о форме области, то, предположив, что область не
имеет отверстий, можно вычислить моменты, исходя только из
границы. Это можно продемонстрировать, сначала преобразовав периметр в многоугольный блоб, а затем вычислив момент
нулевого порядка.

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

>>> p = Polygon2(blobs[1].perimeter).moment(0, 0)
-7510

Тот же результат можно получить подсчетом всех пикселей.
Один из способов анализа богатой информации о форме, закодированной в периметре, состоит в том, чтобы вычислить
расстояние и угол до каждой точки периметра по отношению
к центроиду объекта. Этот показатель вычисляется как
>>> r, th = blobs[1].polar();
>>> plt.plot(r, "r", th, "b");

Результат показан на рис. 12.17а. Показатели вычисляются
для 400 точек (по умолчанию), равномерно распределенных по
всему периметру объекта. Сигнатуры радиуса и угла описывают
форму объекта. Сигнатура угла инвариантна к масштабу объекта, в то время как амплитуда сигнатуры радиуса зависит от размера объекта. Сигнатуры радиуса всех четырех блобов можно
сравнить с помощью следующего кода:
>>> for blob in blobs:
... r, theta = blob.polar()
... plt.plot(r / r.max());

Результат показан на рис. 12.17б. Он нормализован по сумме,
чтобы убрать эффект масштаба объекта. Сигнатуры являются

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

а

нормализованное расстояние по периметру

нормализованный радиус

угол (в радианах)

нормализованный радиус

Получение признаков из области 701

б

нормализованное расстояние по периметру

Рис. 12.17. Сопоставление сигнатур радиуса: a) радиус и угловая сигнатура для блоба 1 (верхняя правая акула на
рис. 12.16); б) нормализованные сигнатуры радиусов для всех блобов

функцией нормализованного расстояния по периметру. Все
они начинаются с самого верхнего левого пикселя на границе
объекта. Различные объекты одной и той же формы имеют одинаковые сигнатуры, которые могут быть сдвинуты по горизонтали. К тому же сигнатуры закольцованы – первая и последняя
точки в горизонтальном направлении соседствуют.
Вот как можно сравнить профиль блоба 1 с профилями всех
фигур при всех возможных взаимных сдвигах по горизонтали:
>>> similarity, _ = blobs.polarmatch(1)
>>> similarity
[0.9936, 1, 0.9856, 0.6434]

Полученный результат указывает, что форма блоба 1 практически точно соответствует самой себе и форме блобов 0 и 2,
но не блоба 3. Метод polarmatch вычисляет одномерную нормализованную взаимную корреляцию (см. табл. 12.1) для каждого
возможного поворота одной сигнатуры по отношению к другой
и возвращает наибольшее значение.
Существует множество вариантов описанного подхода.
К сигнатуре можно применить преобразование Фурье и более
кратко описать ее с помощью нескольких первых коэффициентов Фурье. Можно вычислить кривизну границы и выделить
углы или разбить границу на прямые линии и дуги. Метод peri­
meter_approx, например, позволяет аппроксимировать периметр
последовательностью отрезков прямых.

12.1.4

Обнаружение объектов с использованием
глубокого обучения

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

702 Глава 12 · Извлечение признаков изображения

и ограничивающую рамку. Сети выполняют все описанные выше
этапы: классификацию пикселей (семантическую сегментацию),
различение разных объектов (сегментация экземпляров) и описание с использованием ограничивающих рамок. Проиллюстрируем все это на примере применения Faster R-CNN – сверточной
нейронной сети, обученной на наборе данных COCO. Этот набор данных включает изображения объектов 80 классов, в том
числе человека, велосипед, машину, кошку, собаку, овцу, яблоко, пиццу, воздушного змея, зубную щетку, фрисби и т. д.
Загрузим то же изображение, что и в разделе 12.1.1.3:
>>> scene = Image.Read("image3.jpg")
Image: 640 x 438 (uint8), R:G:B [.../images/image3.jpg]
>>> scene.disp();

которое показано на рис. 12.18а. Предполагая, что библиотека
PyTorch установлена, сначала преобразуем изображение в тензор PyTorch:
>>>
>>>
>>>
>>>

import torch
import torchvision as tv
transform = tv.transforms.ToTensor();
in_tensor = transform(scene.image);

Затем загрузим сеть Faster R-CNN, настроим ее для прогнозирования (вывода) и передадим ей на вход изображение
>>> model = tv.models.detection\
...
.fasterrcnn_resnet50_fpn(pretrained=True).eval();
>>> outputs = model(torch.stack([in_tensor]));

а полученные результаты преобразуем в списки Python:
>>>
>>>
>>>
>>>
>>>
>>>

# список оценок достоверности
scores = outputs[0]["scores"].detach().numpy();
# список названий классов в виде строк
labels = outputs[0]["labels"].detach().numpy();
# список рамок в виде массива ([x1, y1, x2, y2])
boxes = outputs[0]["boxes"].detach().numpy();

который описывает
>>> len(scores)
42

объекта, часть из которых имеют очень низкую достоверность.
Вот как можно показать только достоверные прогнозы и соответствующие метки:
>>> classname_dict = {1: "человек", 2: "велосипед", 3: "машина",
...
4: "мотоцикл", 18: "собака"};
>>> for score, label, box in zip(scores, labels, boxes):

Common Objects in
Context – обычные
объекты в контексте,
см. https://cocodataset.org.

Получение признаков из области 703
...
...
...

if score > 0.5: # только надежные прогнозы
plot_labelbox(classname_dict[label], lbrt=box, filled=True,
alpha=0.3, color="yellow", linewidth=2);

Результат показан на рис. 12.18б. Здесь определяется словарь
classname_dict для отображения подмножества целочисленных
меток классов в соответствующие названия классов COCO.

v (пиксели)

v (пиксели)

человек

человек
человек

мотоцикл
человек

человек
мотоцикл

велосипед
собака

u (пиксели)

u (пиксели)

Рис. 12.18. Глубокая нейронная сеть для обнаружения объектов в сложной реальной сцене

12.1.5

Итоги

Мы обсудили процесс преобразования входного изображения – черно-белого или цветного – в краткие описания областей внут­ри сцены. Характерные признаки области зависят от
природы и решаемой задачи. Для робота-сборщика помидоров
это будут красные круги, для приземляющегося БПЛА это могут
быть желтые мишени на земле.
Описанный процесс представляет собой классический подход к приложениям машинного зрения «от частного к общему»
и делится на следующие ключевые этапы:
1)	классификация пикселей по критерию конкретного применения, например краснота, желтизна или движение.
Каждому пикселю присваивается класс c;
2)	группировка смежных пикселей одного класса в множества, при этом каждому пикселю присваивается метка S,
обозначающая множество, которому он принадлежит;
3)	описание множеств посредством признаков, полученных
из их пространственной протяженности, моментов, эквивалентного эллипса и границы.
Эти шаги представляют собой переход от низкого уровня
представления к высокому. Операции низкого уровня работают
с отдельными пикселями, тогда как операции высокого уровня
связаны с более абстрактными понятиями, такими как размер

704 Глава 12 · Извлечение признаков изображения

и форма. Алгоритмы MSER и graphcuts достаточно мощны, поскольку они объединяют шаги 1 и 2 и учитывают области пикселей и локализованные различия для создания сегментации.
Важно отметить, что ни один из этих шагов не обязан быть
идеальным. Возможно, на первом этапе будет несколько ложных
срабатываний – отдельные пиксели, ошибочно классифицированные как объекты, которые мы можем устранить с помощью
морфологических операций или отбросить после анализа связности на основании их небольшого размера. Первый шаг также может иметь ложноотрицательные результаты; например,
зеркальное отражение и окклюзия могут привести к тому, что
некоторые пиксели объекта будут неправильно отброшены как
посторонние. В этом случае необходимо применить определенную эвристику, например морфологическую обработку, чтобы
заполнить пробелы в блобе. Другой вариант – заново сегментировать сцену, т. е. увеличить количество областей и использовать некоторые специфичные знания предметной области
для объединения смежных областей. Допустим, область белого
зеркального отражения может быть объединена с окружающими областями для создания области, соответствующей целому
помидору.
В некоторых случаях можно тщательно подобрать положение
камеры и освещение для получения изображения высокого качества, но роботу, работающему в изменчивом реальном мире,
такая роскошь недоступна. Робот должен извлечь из изображения как можно больше полезной информации и двигаться
дальше.
Знание предметной области всегда является мощным инструментом. Например, если мы знаем, что сцена содержит помидоры и листья, и наблюдаем большую красную область, которая
не является круглой, мы используем наше знание предметной
области, чтобы сделать вывод, что это круглый плод, который
частично закрыт листьями. Поэтому можно приказать роботу
искать другой плод, который не закрыт, а затем переместиться
в новое место, откуда лучше видны остальные плоды.
Эти методы подходят для относительно простых сцен и не
требуют продолжительных вычислений. Однако в последнее
время эти методы уступили место методам, основанным на глубоком обучении, предлагающем мощное и практичное решение
сложных задач сегментирования изображений. Сеть за один шаг
преобразует цветное изображение в пиксельную классификацию, применяя понятные человеку семантические метки, такие
как «человек» или «велосипед». В настоящее время сущест­вует
множество инструментов с открытым исходным кодом, и теперь обучить сеть для решения конкретных задач довольно
прос­то. Однако обучение требует большого объема маркированных данных и значительного объема вычислений.

Получение признаков из линий 705

12.2

Получение признаков из линий

Линии – это отчетливые визуальные элементы, которые особенно распространены в искусственных средах, например края
дорог, зданий и дверных проемов. В разделе 11.5.1.3 мы обсудили возможность использования градиентов интенсивности
изображения для определения краев, и в этом разделе речь
пойдет о подгонке сегментов линий к таким краям.
Для иллюстрации основного принципа построим очень прос­
тую сцену
>>> points5 = Image.Read("5points.png", dtype="float");

а

u (пиксели)

б

Голоса

v (пиксели)

ρ (пиксели)

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

θ (радианы)

Рис. 12.19. Основы преобразования Хафа: а) пять точек, определяющих шесть линий; б) массив накопителей Хафа.
По горизонтальной оси отложен угол θ ∈ S 1, поэтому мы можем представить себе граф, обернутый вокруг цилиндра
(левая и правая стороны соединены). Знак ρ тоже меняется в месте соединения, поэтому пересечения кривых на
левом и правом ребрах эквивалентны

Было бы удобно описывать каждую линию с помощью минимального количества параметров, но стандартное уравнение v = mu + c не подходит для случая вертикальных линий, где
m = ∞. Вместо этого принято представлять линии с помощью
параметрической пары (ρ, θ), как показано на рис. 12.20:
u cos θ + v sin θ = ρ,

(12.10)

706 Глава 12 · Извлечение признаков изображения

где θ ∈ [0, π) – угол между горизонтальной осью и нормалью
к прямой, а ρ ∈ [−ρmin, ρmax] – расстояние по перпендикуляру между началом координат и прямой, как показано на рис. 12.20. Линию 2 можно было бы описать как θ2 > π и ρ2 > 0, но вместо этого
сохраним θ2 ∈ [0, π) и сделаем ρ2 < 0. ► Горизонтальная прямая
имеет θ = π/2, а вертикальная – θ = 0. Поэтому любую прямую
можно рассматривать как точку (θ, ρ) в двухмерном пространстве всех возможных прямых.

e
lin

1

lin

e

2

Рис. 12.20. Пример параметрической пары (θ, ρ) для двух отрезков. θ ∈ [0, π) – между
горизонтальной осью и нормалью к прямой. Положительные значения показаны синим
цветом, отрицательные – красным. Линия 2 имеет отрицательное значение ρ

Голосовать за бесконечное число линий, проходящих через
каждую точку, нецелесообразно, поэтому мы рассматриваем
конечное множество линий. Параметрическое θρ-пространство
квантуется, и для подсчета голосов используется соответствующий массив A размерностью Nθ ×Nρ – так называемый массив на­
копителей (accumulator array). Для входного изображения W×H:
ρmax = -ρmax = max(W, H).
Массив A содержит Nρ элементов, покрывающих интервал
ρ ∈ [−ρmax, ρmax], и Nθ элементов, покрывающих интервал θ ∈ [0, π).
Индексами массива являются целые числа (i, j) ⊂ ℕ02 такие, что
i = 0, ..., Nθ – 1 ↦ θ ∈ [0, π),

j = 0, ..., Nρ - 1 ↦ ρ ∈ [−ρmax, ρmax).

Краевая точка (u, v) голосует за все линии, удовлетворяющие
уравнению (12.10). Для каждого i вычисляется соответствующее
значение θ с использованием линейного отображения, как показано выше, затем вычисляется ρ в соответствии с уравнением

Это соглашение
принято OpenCV.

Получение признаков из линий 707

ρ = u cos θ + v sin θ
и отображается в соответствующее целое число j. В заключение
увеличивается значение ai,j для фиксации голоса. Каждая граничная точка добавляет голос к Nθ элементам A, лежащим вдоль
синусоидальной кривой.
В итоге элементы A, набравшие наибольшее количество
голосов, соответствуют доминирующим линиям сцены. Для
примера на рис. 12.19а результирующий массив накопителей
показан на рис. 12.19б. Большая часть массива содержит ноль
голосов (желтый), а темные кривые представляют собой следы
одиночных голосов, соответствующих каждой из пяти входных
точек. Эти кривые пересекаются в точках, которые соответствуют линиям более чем с одним голосом. Мы видим четыре точки, где две кривые пересекаются, что дает нам ячейки с двумя
голосами, и они соответствуют линиям, соединяющим четыре
внешние точки на рис. 12.19а. Горизонтальная ось представляет угол θ ∈ S1, поэтому левый и правый концы соединяются, а ρ
меняет знак – точки пересечения кривых в левой и правой частях массива эквивалентны. Существуют также два места, где
пересекаются три кривые, что дает ячейки с тремя голосами,
и они соответствуют диагональным линиям, проходящим через
среднюю точку рис. 12.19а. Этот метод называется преобразованием Хафа (Hough transform).
Рассмотрим более сложный пример сплошного квадрата, повернутого против часовой стрелки на 0.3 рад:
>>> square = Image.Squares(number=1, size=256, fg=128).rotate(0.3)
Image: 256 x 256 (uint8)

Вычислим краевые точки
В Toolbox для вы>>> edges = square.canny();
числения преобразования Хафа
используется которые показаны на рис. 12.21а. Преобразование Хафа вычисOpenCV-функция ляется как
HoughLines, но
она не возвращает >>> h = edges.Hough();
массив накопителей.
Метод plot_accumulator восстанав- и возвращает экземпляр класса Hough. Он содержит двухмерливает этот массив, ный массив накопителей голосов, который можно визуализииспользуя ресур- ровать :
соемкий процесс
вычисления преоб- >>> h.plot_accumulator()
разования Хафа для
всех возможных пороговых значений. как показано на рис. 12.21б, в. Четыре самых темных пятна соЭто полезно для ответствуют доминирующим границам на входном изображеобучения, но крайне нии. Видно, что многие другие возможные линии тоже полунеэффективно
для практического чили небольшое количество голосов. Гистограмма количества
примене­ния. голосов доступна в свойстве votes

v (пиксели)

708 Глава 12 · Извлечение признаков изображения

Голоса

Количество голосов выше порога

б

u (пиксели)

ρ (пиксели)

а

количество голосов
порог

г

θ (радианы)

Порог

v (пиксели)

в

д

u (пиксели)

Рис. 12.21. Преобразование Хафа для повернутого квадрата: а) изображение края; б) накопитель Хафа; в) крупный
план накопителя Хафа; г) распределение голосов выше порога; д) искомые линии, наложенные на исходное
изображение

>>> plt.plot(h.votes);
>>> plt.yscale("log");

и представлена на рис. 12.21г. Обратите внимание, что хотя
объект имеет только четыре стороны, в массиве накопителей
гораздо больше четырех пиков, но большинство из них – небольшие.

Получение признаков из линий 709

Выбирая линии с 60 и более голосами, мы получаем набор
доминирующих линий
>>> lines
array([[
[
[
[
[
[
[
[

= h.lines(60)
1.274,
224],
1.274,
95],
2.845,
-149],
1.257,
97],
1.257,
226],
2.845,
-20],
2.827,
-147],
2.827,
-18]], dtype=float32)

в виде двухмерного массива параметров линий, где каждая
строка – это (θ, ρ). Мы видим, что существуют пары схожих параметров линий, и это связано с эффектами квантования.
На рис. 12.21в показаны две ячейки аккумулятора со значениями выше порога, в непосредственной близости от того, что
должно быть одним пиком.
Обнаруженные линии можно спроецировать на исходное
изображение.

Подавление нелокальных максимумов могло бы
пригодиться в этом
случае, но лежащая
в основе OpenCVфункция HoughLines
не поддерживает
>>> square.disp();
такой возможности.

>>> h.plot_lines(lines);

Результат показан на рис. 12.21д. Мы ясно видим несколько
линий, параллельных каждой стороне квадрата.
Выбор порогового значения критически важен для качества
результата. Слишком высокое значение приведет к пропуску
линий, а слишком низкое – к появлению большого количества
ложных линий.
Вероятностное преобразование Хафа – более эффективная
версия, которая обрабатывает только подмножество точек на
и возвращает отрезки, а не прямые. Отрезок
изображении
имеет минимальную длину и может содержать зазоры, при условии что длина зазора не превышает порогового значения.
Для реального изображения, показанного на рис. 12.22:

Алгоритм использует случайные числа,
поэтому на каждом
запуске могут
получаться разные
результаты, если не
задавать начальное
значение для генератора случайных
чисел. >>> church = Image.Read("church.png", mono=True)
Если зазор превышает пороговое
значение, то возвращаются два отрезка,
при условии что их
длина соответствует
критерию минимальной длины.

Image: 1280 x 851 (uint8) [.../images/church.png]
>>> edges = church.canny()
Image: 1280 x 851 (uint8)
>>> h = edges.Hough();
>>> lines = h.lines_p(100, minlinelength=200, maxlinegap=5, seed=0);

можно построить линии со 100 или более голосами, которые соответствуют критериям длины линии и зазора, наложенных на
изображение.
>>> church.disp();
>>> h.plot_lines_p(lines, "r--")

v (пиксели)

710 Глава 12 · Извлечение признаков изображения

u (пиксели)

Рис. 12.22. Вероятностное
преобразование Хафа
реального изображения

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

12.2.1

Итоги

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

12.3

Получение признаков из точек

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

Получение признаков из точек 711

изображении, называемые опорными (point of interest), выступающими, ключевыми или угловыми точками. Сначала мы
обсудим некоторые классические методы нахождения опорных
точек, а затем рассмотрим более современные масштабно-инвариантные методы.

12.3.1

Классические детекторы углов

В разделе 11.5.1.3 было сказано, что точка на линии имеет сильный градиент в направлении, нормальном к линии. Однако
градиент вдоль линии низкий, т. е. пиксель будет очень похож
на своих соседей по линии. Опорная точка отличается тем, что
обладает высоким градиентом в ортогональных направлениях.
Это может быть один пиксель, интенсивность которого значительно отличается от интенсивности всех его соседей, или это
может быть буквально пиксель в углу объекта. Поскольку опорные точки существенно отличаются от их окружения, вероятность того, что они будут надежно обнаружены при разных видах одной и той же сцены, гораздо выше. Следовательно, они
играют ключевую роль в многоракурсных методах, таких как
стереозрение и оценка движения, которые мы обсудим в главе 14.
Самым ранним детектором опорных (угловых) точек был
оператор Моравека (детектор углов Моравека), выбирающий
точки сцены, которые полезны для отслеживания. Он основан
на интуитивном предположении, что если небольшой фрагмент изображения 𝒲 должен однозначно присутствовать на
другом изображении, он будет сильно отличаться от фрагмента
того же размера в любом соседнем месте. На рис. 12.23 показано
содержимое красного окна для каждой строки при смещении
по горизонтали и вертикали. Первая строка относится к области изображения, где все пиксели имеют одинаковое значение,
и мы видим, что все смещенные окна идентичны. Вторая строка
относится к случаю вертикального края, и мы видим, что смещенные окна имеют некоторую изменчивость, но есть только
три уникальных окна. Третья и четвертая строки показывают
случай изолированной точки и угла соответственно – каждое из
смещенных окон имеет уникальное значение. Очевидно, что
наиболее различимыми являются изолированная точка и угловая точка.
Моравек определил сходство между областью с центром
в (u, v) и соседней областью, смещенной на (δu, δv), как
(12.11)

712 Глава 12 · Извлечение признаков изображения

Рис. 12.23. Принцип работы детектора Моравека. Слева от каждой строки находится область
изображения с окном 3×3, обозначенным красным. Справа показано содержимое этого
красного окна для различных смещений, как видно в нижней части рисунка (δu, δv)

где 𝒲 – некоторая локальная область изображения X, обычно
квадратное окно w × w, где w – нечетное число. Это мера сходства SSD из табл. 12.1, которую мы обсуждали ранее. Сходство
оценивается для перемещений по восьми сторонам света ►
(δu, δv) ∈ 𝒟, а минимальное значение является мерой значи­мости
(12.12)

N, NE, E, … W, NW
или i, j ∈ {–1, 0, 1}.

которая велика, только если все смещенные участки отличаются от исходного участка. Функция CM(·) вычисляется для каждого пикселя изображения, и опорные точки располагаются
там, где высоко значение CM. Основной недостаток детектора
Моравека – он неизотропен, поскольку исследует изменение
изображения (в основном градиентное) в ограниченном числе
направлений. Следовательно, детектор может давать большое
выходное значение для точки на линии, что нежелательно.
Мы можем обобщить этот подход, определив сходство как
взвешенную сумму квадратов различий между областью изображения и смещенной областью:

где W – весовая мат­ри­ца, например двухмерный гауссиан, выделяющая точки ближе к центру окна 𝒲. Подчеркнутый член
можно аппроксимировать усеченным рядом Тейлора

См. приложение E.

Получение признаков из точек 713

где Xu и Xv – горизонтальный и вертикальный градиенты изоб­
ражения соответственно. Теперь мы можем написать:

Это выражение можно более компактно записать в квадратичной форме как

где

Если предположить, что весовая мат­ри­ца является ядром Гаусса W = G(σI) и мы заменяем суммирование сверткой, то
Градиентные
изображения Xu и Xv
обычно рассчитываются с использованием производной
метода ядра Гаусса
(раздел 11.5.1.3)
с параметром
сглаживания σD . Xu2,
2
Xv и Xu Xv рассчитываются поэлементным возведением в квадрат или
умножением.
В отступлении 11.2
было сказано, что
сжатие изображения с потерями,
такое как JPEG,
удаляет высокочастотные детали
из изображения,
но именно такие
детали определяют угол. В идеале
детекторы углов
следует применять
к изображениям,
которые не подвергались сжатию
и распаковке.

(12.13)
A представляет собой симметричную мат­ри­цу 2×2, называемую тензором структуры, мат­ри­цей автокорреляции или
мат­ри­цей второго момента. Она отражает структуру интенсивности локальной окрестности, а ее собственные значения
обеспечивают вращательно-инвариантное описание окрестности. Элементы мат­ри­цы A вычисляют из градиентов изображения, возводят в квадрат или умножают, а затем сглаживают
с использованием весовой мат­ри­цы. Последнее действие снижает шум и повышает стабильность и надежность детектора.
Опорная точка (u, v) – это такая точка, относительно которой значение s(u, v : · ) велико для всех направлений вектора
(δu, δv). То есть в каком бы направлении мы ни двигали окно,
оно быстро становится непохожим на исходную область. Если
мы рассматриваем исходное изображение X как поверхность,
собственные значения A являются главной кривизной поверхности в этой точке. Если оба собственных значения малы,
то поверхность плоская, т. е. область изображения имеет приблизительно постоянную локальную интенсивность. Если одно
собственное значение высокое, а другое низкое, то поверхность
имеет форму гребня, что указывает на ребро. Если оба собственных значения высокие, поверхность имеет острый пик,
который мы считаем углом. ◄

714 Глава 12 · Извлечение признаков изображения

Детектор Ши–Томаси (иногда называют детектором Канаде–
Томаси) рассматривает силу угла как минимальное собственное
значение:
CST(u, v) = min(λ1, λ2),

(12.14)

где λi – собственные значения A. Точки на изображении, для
которых эта мера высока, называются «хорошими признаками для отслеживания». Детектор Харриса ► основан на том же
принципе, но определяет силу угла как
CH(u, v) = det(A) - k tr(A).

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

(12.15)

Здесь большое значение также представляет сильный, отчетливый угол. Поскольку det(A) = λ1λ2 и tr(A) = λ1 + λ2, детектор Харриса реагирует, когда оба собственных значения велики, и элегантно избегает вычисления собственных значений A, которое
требует несколько более высоких вычислительных затрат.
Обычно используемое значение k равно 0.04. Еще один вариант – детектор Нобля:
(12.16)
который арифметически прост, но потенциально подвержен
сингулярности.
Обычно сила угла вычисляется для каждого пикселя, и в результате получается изображение силы угла. Затем применяется нелокальное подавление максимумов, чтобы сохранить
только те значения, которые больше, чем их непосредственные
соседи. Список таких точек сортируется по убыванию значения
силы угла. Затем можно применить пороговую фильтрацию,
чтобы принимать только углы с силой выше определенного порога, или выше определенной доли самого сильного угла, или
просто взять N самых сильных углов.
Toolbox предоставляет угловой детектор Харриса, который
мы продемонстрируем на реальном изображении:
>>> view1 = Image.Read("building2-1.png", mono=True);
>>> view1.disp();

Вычислим признаки Харриса:
>>> harris1 = view1.Harris(nfeat=500)
Harris features, 500 points

Результат представляет собой экземпляр объекта HarrisFeature, являющегося подклассом BaseFeature2D. Объект ведет себя
подобно спискам Python

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

Получение признаков из точек 715
>>> len(harris1)
500

и поддерживает итерации и операции получения срезов
>>> harris1[0]
Harris feature: (597.0, 648.0), strength=1.64, id=-1
В этой реализации
Эти значения отражают основные свойства признаков. Кажкоординаты являют- дый признак имеет такие свойства, как местоположение
ся целыми числами.
Другие детекторы и сила:
углов определяют
положение угла >>> harris1[0].p
597],
с точностью до array([[
субпикселя.
[
648]])

>>> harris1[0].strength
1.636

Свойство id наследуется от id изображения, откуда получены
признаки. Для источников изображений VideoFile, ImageCollection и ZipArchive идентификатор id изображения принимает последовательные целочисленные значения, а Image.Read устанавливает его равным -1.
Также можно создавать выражения, такие как
>>> harris1[:5].p
array([[
597,
587,
576,
[
648,
658,
636,
>>> harris1[:5].strength
[1.636, 1.532, 1.514, 1.487, 1.348]

810,
580,

149],
490]])

представляющие позиции и силу первых пяти признаков в виде
двух- и одномерных массивов соответственно.
Отступление 12.4. Определитель гессиана
Другой подход к определению кривизны изображения заключается в использовании определителя гессиана (determinant of the
Hessian, DoH). Гессиан H – это мат­ри­ца градиентов второго порядка в точке

где Xuu = ∂2X/∂u2, Xvv = ∂2X/∂v2 и Xuv = ∂2X2/∂u∂v. Определитель det(H)
имеет большую величину, когда уровень серого изменяется в двух
направлениях. Однако вторые производные усиливают шум изображения даже больше, чем первые производные, и изображение
должно быть предварительно сглажено.

Углы можно накладывать на изображение в виде желтых
крес­тиков:

716 Глава 12 · Извлечение признаков изображения
>>> view1.disp(darken=True);
>>> harris1.plot();

как показано на рис. 12.24а. Параметр darken в вызове disp
уменьшает яркость изображения, чтобы сделать наложенные
угловые маркеры более заметными. Крупный план фрагмента изображения показан на рис. 12.24б, и там мы видим, что
элементы действительно часто расположены на углах объектов.
Чтобы показать только часть угловых элементов, можно, к примеру, выбрать лишь каждый пятый элемент
>>> harris1[::5].plot()

или подмножество из 20 равномерно расположенных углов

u (пиксели)

в

б

u (пиксели)

v (пиксели)

v (пиксели)

а

v (пиксели)

v (пиксели)

>>> harris1.subset(20).plot()

u (пиксели)

г

u (пиксели)

Рис. 12.24. Применение детектора углов Харриса к двум видам одного и того же здания: а) первый вид;
б) увеличенный фрагмент первого вида; в) второй вид; г) увеличенный фрагмент второго вида. Обратите внимание,
что довольно много обнаруженных углов связано с одними и теми же объектами мира в двух видах

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

Получение признаков из точек 717

увеличить расстояние, используемое для подавления нелокальных максимумов:
>>> harris1 = view1.Harris(nfeat=500, scale=15)
Harris features, 500 points

указав минимальное расстояние между углами в 15 пикселей
(по умолчанию выбирается расстояние 7 пикселей).
Силу угла Харриса для всех пикселей можно вычислить и отобразить:
>>> view1.Harris_corner_strength().disp();

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

v (пиксели)

сила угла

)
ели
икс

v (п
)
иксели

u (п
u (пиксели)
а
б
Рис. 12.25. Сила угла по Харрису: а) увеличенный фрагмент значений силы угла,
представленных в виде изображения (синий – положительные, красный – отрицательные);
б) увеличенное изображение значений силы угла, отображаемых в виде поверхности

Интегральная гистограмма силы 500 обнаруженных углов
показана на рис. 12.26. Самый сильный угол имеет CH ≈ 1.64, но
большинство из них намного слабее, только 10 % углов превышают половину этого значения.
Возьмем изображение того же здания, снятое с другого ракурса:
>>> view2 = Image.Read("building2-2.png", mono=True);

Посмотрим, какие углы были обнаружены:
>>> harris2 = view2.Harris(nfeat=250);

718 Глава 12 · Извлечение признаков изображения
>>> view2.disp(darken=True);
>>> harris2.plot();

Интегральное количество признаков

Результаты показаны на рис. 12.24в,г. Для многих полезных
приложений роботизированного зрения, таких как отслеживание, создание мозаики и стереозрение, которые мы обсудим
в главе 14, важно, чтобы угловые объекты обнаруживались в одних и тех же точках мира независимо от различий в освещении
или изменений поворота и масштаба между двумя видами. По
рис. 12.24 мы видим, что многие (хотя не все) признаки действительно привязаны к одному и тому же мировому признаку
в обоих представлениях.

Сила угла

Рис. 12.26. Интегральная
гистограмма силы углов

Детектор Харриса вычисляется из градиентов изображения
и, следовательно, устойчив к изменениям освещения, а собст­
венные значения структурного тензора A инвариантны к вращению. Однако детектор не инвариантен к изменениям масштаба. По мере увеличения изображения градиенты вокруг
угловых точек становятся меньше – одинаковое изменение интенсивности распространяется на большее количество пикселей. Это уменьшает крутизну градиента и, следовательно, силу
углов. В следующем разделе обсуждается решение этой проблемы с помощью угловых детекторов, не зависящих от масштаба.

12.3.2

У гловые детекторы в масштабируемом
пространстве

Угловой детектор Харриса, представленный в предыдущем разделе, очень хорошо работает на практике со статичными изоб­

Получение признаков из точек 719

ражениями, но плохо реагирует на изменения масштаба. К сожалению, изменение масштаба из-за изменения расстояния от
камеры до сцены или оптического приближения/отдаления является обычным явлением во многих реальных приложениях.
Мы также заметили, что детектор Харриса сильно реагирует на
мелкие текстуры, такие как листья деревьев на рис. 12.24, хотя
намного полезнее возможность обнаруживать признаки, связанные с крупномасштабной структурой сцены, такой как окна
и балконы.
Рисунок 12.27 иллюстрирует фундаментальный принцип
обнаружения пространственных признаков. Сначала загрузим
синтетическое изображение
>>> foursquares = Image.Read("scale-space.png", dtype="float");

которое показано на рис. 12.27а. Изображение содержит четыре
квадрата разного размера: 5×5, 9×9, 17×17 и 33×33. Последовательность масштабных представлений вычисляется путем применения ядра Гаусса с увеличением σ, из-за чего области становятся все более размытыми и меньшие участки постепенно
исчезают из поля зрения. На каждом шагу последовательности
сглаженное по Гауссу изображение свертывается с использованием ядра Лапласа (12.5), что дает сильные отрицательные отклики на эти яркие блобы. ◄
С помощью Toolbox вычислим последовательность масштабного пространства

На самом деле мы
вычисляем разность
аппроксимации
гауссиана и лапласиана гауссиана,
как показано на >>> G, L, s = foursquares.scalespace(60, sigma=2);
рис. 12.30.

где входными аргументами являются количество шагов масштаба для вычисления и σ ядра Гаусса, которое должно применяться на каждом последующем шаге. G – список изображений,
содержащий исходное изображение и изображения со все возрастающими уровнями сглаживания, L – список изображений,
являющихся лапласианами соответствующих сглаженных изображений, а s – соответствующий масштаб. Например, шестое
изображение в последовательности лапласианов гауссианов
(LoG) можно отобразить как
>>> L[5].disp(colormap="signed");

как показано на рис. 12.27б, и узнать масштаб:
>>> s[5]
4.5

На рис. 12.27в–д показан лапласиан гауссиана в трех других
точках последовательности масштабного пространства.

u (пиксели)

д

u (пиксели)

г

u (пиксели)

|LoG|

v (пиксели)

в

б

v (пиксели)

u (пиксели)

v (пиксели)

а

v (пиксели)

v (пиксели)

720 Глава 12 · Извлечение признаков изображения

u (пиксели)

е

Масштаб

Рис. 12.27. Пример масштабного пространства: а) синтетическое изображение X с блоками размером 5×5,
9×9, 17×17 и 33×33; б–д) нормированный лапласиан гауссиана σ2L∗G(σ)∗X для возрастающих значений
масштаба, значение σ указано внизу слева. Используется синтетический цвет: красный – отрицательный, синий –
положительный; е) величина лапласиана в центре каждого квадрата (обозначена знаком «+») в зависимости от σ

Получение признаков из точек 721

Рассмотрим теперь значение лапласиана гауссиана в центре
наименьшего квадрата как функцию масштаба
>>> plt.plot(s[:-1], [-Ls.image[63, 63] for Ls in L]);

график которой показан синей кривой на рис. 12.27е. Аналогично можно построить график зависимости отклика в центре других квадратов от масштаба. Каждая кривая имеет хорошо выраженный пик, и масштаб, связанный с пиком, пропорционален
размеру области – характеристическому масштабу области.
Если сложить двухмерные изображения из последовательности L в трехмерный объем, то характерная точка масштабного пространства – это любой пиксель, являющийся локальным
трехмерным максимумом. Это элемент, который больше, чем
его 26 соседей во всех трех измерениях, – его пространственные
соседи в текущем масштабе и в масштабе выше и ниже. Такие
точки обнаруживаются функцией findpeaks3d:
>>> features
...
array([[
[
[
[

= findpeaks3d(np.stack([np.abs(Lk.image) for Lk in L],
axis=2), npeaks=4)
63,
63,
1,
1.962],
127,
63,
2,
1.716],
63,
127,
11,
1.514],
127,
127,
43,
1.46]])

которая возвращает двухмерный массив, содержащий по одной
строке на каждый максимум, где каждая строка содержит v, u,
индекс масштабирования k и силу. Мы можем наложить обнаруженные признаки на исходное изображение.
>>> foursquares.disp();
>>> for v, u, i, _ in features:
... plt.plot(u, v, 'k+')
... scale = s[int(i)]
... plot_circle(radius=scale * np.sqrt(2), centre=(u, v),
...
color="y")

Результат показан на рис. 12.28. Масштаб, связанный с признаком, легко визуализировать с помощью кругов, имеющих радиус, равный масштабу признака. Результат этой визуализации
также показан на рис. 12.28. Мы видим, что обнаруженные признаки расположены в центре каждого объекта и что масштаб
признака связан с размером объекта. Область внутри круга называется опорной областью (support region) объекта.
Возьмем для примера реальное изображение
>>> mona = Image.Read("monalisa.png", dtype="float");

v (пиксели)

722 Глава 12 · Извлечение признаков изображения

u (пиксели)
Рис. 12.28. Синтетическое изображение с наложенным центром признака и индикатором
масштаба

Вычислим масштабное пространство за восемь больших шагов с σ = 8
>>> G, L, _ = mona.scalespace(8, sigma=8);

которые мы можем сгладить и отобразить:
>>> Image.Hstack(G).disp();
>>> Image.Hstack(L).disp();

v (пиксели)

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

v (пиксели)

u (пиксели)

u (пиксели)

Рис. 12.29. Последовательность в масштабном пространстве для σ = 2; вверху – гауссова последовательность,
внизу – лапласиан гауссовой последовательности

Получение признаков из точек 723

Для ускорения вычислений алгоритм
SIFT (Lowe, 2004)
использует пирамиды изображений, показанные
на рис. 12.35.
Уровни пирамид
называются октавами, поскольку
последовательные
изображения
масштабируются
в два раза по обоим
измерениям. SIFT
добавляет дополнительную октаву, размер которой вдвое
больше исходного
изображения. Внут­
ри каждой октавы
выполняется серия
операций гауссовского размытия, или
масштабирования.
Число октав и масштабов является
параметрами SIFT,
но обычно устанавливается равным 4
и 5 соответственно.

Свертка исходного изображения с ядром Гаусса с увеличением σ приводит к увеличению размера ядра и, следовательно,
объема вычислений на каждом шаге масштабирования. Как вы
уже знаете, свертка двух гауссианов является еще одним более
широким гауссианом. Вместо свертки нашего исходного изображения с более широкими гауссианами можно многократно
применить один и тот же гауссиан к предыдущему результату.
Вы также знаете, что ядро LoG аппроксимируется разностью
двух гауссианов. Используя свойства свертки, можно записать
(G(σ1) - G(σ2)) ∗ X = G(σ1) ∗ X - G(σ2) ∗ X,

где σ1 > σ2. Разность операторов Гаусса, примененных к изображению, эквивалентна разнице между изображениями при двух
разных уровнях сглаживания. Если мы выполним сглаживание
последовательным применением гауссианов, то получим последовательность изображений с нарастающим уровнем сглаживания. Таким образом, разница между шагами в последовательности является приближением к лапласиану гауссиана. На
рис. 12.30 это показано в виде диаграммы.

12.3.2.1 П
 олучение признаков из точек
в масштабном пространстве
Рассмотренные выше концепции масштабного пространства
лежат в основе ряда популярных детекторов признаков, которые находят характерные точки в изображении и определяют
их масштаб, а также ориентацию. Масштабно-инвариантное
преобразование признаков (Scale-Invariant Feature Transform,
SIFT) основано на максимумах разности гауссовой последовательности.
Гауссова последовательность

–

+

–

+

–

+

Лапласиан гауссиана (LoG)

Рис. 12.30. Схема расчета гауссиана и лапласиана гауссовой пространственно-масштабной последовательности

724 Глава 12 · Извлечение признаков изображения

Для иллюстрации вычислим признаки SIFT для изображения
здания, которое использовалось ранее:
>>> sift1 = view1.SIFT(nfeat=200)
SIFT features, 200 points

В результате мы получаем экземпляр объекта SIFTFeature, являющегося подклассом BaseFeature2D, который ведет себя подобно объекту HarrisCorner, представленному выше. Например, вот
первый признак:
>>> sift1[0]
SIFT feature: (2.3, 285.4), strength=0.05, scale=2.1, orient=148.5°,
id=-1

Каждый признак имеет координату (с точностью до субпикселя), масштаб и ориентацию, которые определяются доминирующим направлением кромки в опорной области.
Это изображение содержит более 10 000 признаков, но, как
мы делали ранее с угловыми признаками Харриса, мы запросили 200 самых сильных и нанесли их на график:
>>> view1.disp(darken=True);
>>> sift1.plot(filled=True, color="y", hand=True, alpha=0.3)

v (пиксели)

Результат показан на рис. 12.31. Метод plot рисует круг вокруг
местоположения признака с радиусом, который соответствует
масштабу – размеру опорной области. Параметр hand рисует радиальную линию, указывающую ориентацию признака SIFT –
направление доминирующего градиента.

u (пиксели)
Рис. 12.31. Признаки SIFT, показывающие опорную область (масштаб) и ориентацию
в виде радиальной синей линии. Некоторые признаки кажутся имеющими две ориентации,
но на самом деле это два признака с разной ориентацией в одном и том же месте

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

Применения 725

Масштаб признаков широко варьируется, а гистограмма
>>> plt.hist(sift1.scale, bins=100);

Количество элементов

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

Рис. 12.32. Гистограмма
масштабов признаков
показана с логарифмической
вертикальной шкалой

Масштаб

Алгоритм SIFT – это больше, чем просто масштабно-инвариантный детектор признаков, он также вычисляет очень устойчивый дескриптор. Дескриптор представляет собой вектор d ∈ ℝ128,
кодирующий градиент изображения в подобластях опорной
области так, что не зависит от яркости, масштаба и поворота.
Это позволяет однозначно сопоставлять дескрипторы признаков с дескриптором той же точки мира на другом изображении, даже если их масштаб и ориентация сильно различаются.
Разница в положении, масштабе и ориентации совпадающих
признаков дает некоторое представление об относительном
движении камеры между двумя кадрами. Сопоставление признаков между сценами имеет решающее значение для решения
задач, которые мы рассмотрим в главе 14.

12.4

Применения

12.4.1

Распознавание символов

Особенно важным классом объектов являются символы. Наш
мир наполнен информативным текстом в виде знаков и надпи­

726 Глава 12 · Извлечение признаков изображения

сей, которые предоставляют информацию о названиях мест
и направлениях движения. Мы обрабатываем большую часть
визуальных символов бессознательно, но этот богатый источник информации в значительной степени недоступен для роботов.
В этом примере мы будем использовать пакет с открытым
исходным кодом Tesseract. Чтобы последовать за примерами,
вам необходимо установить выполняемый файл и интерфейс
для Python pytessearct. Начнем с изображения знака, показанного на рис. 12.33а, и применим OCR ко всему изображению:
>>> import pytesseract as tess
>>> penguins = Image.Read("penguins.png");
>>> ocr = tess.image_to_data(penguins.image < 100,
...
output_type=tess.Output.DICT);

Изображение сначала было преобразовано в бинарное представление, чтобы получить темный текст на белом фоне – это
способствует более высокому качеству работы Tesseract. Результат был запрошен в формате словаря, который соответствует
словарю списков. Далее отфильтруем результаты по уровню
достоверности и по наличию в строке символов, отличных от
пробелов:
>>> for confidence, text in zip(ocr["conf"], ocr["text"]):
... if text.strip() != "" and float(confidence) > 0:
...
print(confidence, text)
96 Table
95 Mountain
95 National
96 Park
95 Warning
...
7 a
44 =
33 mae

Текст на знаке прочитан правильно, за исключением некоторых графических символов, которые интерпретированы неверно. Мы можем определить степень уверенности распознавания
текста
>>> for i, (text, confidence) in enumerate(zip(ocr["text"],
...
ocr["conf"])):
... if text.strip() != "" and float(confidence) > 50:
...
plot_labelbox(text,
...
lb=(ocr["left"][i], ocr["top"][i]),
...
wh=(ocr["width"][i], ocr["height"][i]),
...
color="y", filled=True, alpha=0.2)

и получить результаты, показанные на рис. 12.33.

pytesseract
возвращает координату «top» (верхнего
края) изображения,
которая является
минимальной
координатой v, или
«bottom» (нижнего
края) для неперевернутой оси v.

v (пиксели)

v (пиксели)

Применения 727

u (пиксели)

u (пиксели)

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

12.4.2

Поиск изображений

При наличии набора изображений {Xj, j = 0, ..., N - 1} и нового
изображения X¢ задача поиска изображений состоит в определении такого j, при котором Xj максимально похоже на X¢. Это
сложная задача, если учесть, что немалое влияние на выбор может оказать изменение ракурса и экспозиции. Меры сходства
на уровне пикселей, такие как сумма абсолютных разностей
(SSD) или нормализованная взаимная корреляция с нулевым
средним (ZNCC), представленные в разделе 11.5.2, не подходят
для этой задачи, потому что даже совсем небольшое изменение
ракурса приведет к практически нулевому сходству.
Поиск изображений может помочь роботу определить, посещал ли он ранее определенное место или видел ли уже некоторый объект. Если предыдущие изображения имеют какие-либо
связанные с ним семантические данные, такие как название
объекта или название места, то тогда можно будет сделать вывод, что эти семантические данные применимы и к новому изображению. Например, если новое изображение совпадает с существующим изображением с семантическим тегом «кухня»,
то это означает, что робот видит ту же сцену и, следовательно,
находится на кухне или рядом с ней.
Конкретный метод, который мы рассмотрим, обычно называют «мешком слов» (bag of words), и он полезен для многих применений в сфере робототехники и основан на методах,
с которыми мы уже сталкивались, таких как точечные признаки
SIFT и кластеризация методом k средних.
Начнем с загрузки набора из двадцати изображений.
>>> images = ImageCollection("campus/*.png", mono=True);

Это итератор изображений, который мы используем для вычисления и объединения SIFT-признаков всех изображений.

728 Глава 12 · Извлечение признаков изображения
>>> features = [];
>>> for image in images:
... features += image.SIFT()
>>> features.sort(by="scale", inplace=True);

По сути, он является экземпляром объекта SIFTFeature с
>>> len(features)
42194

признаками, отсортированными по убыванию масштаба. Числовые параметры первых десяти объектов можно представить
в табличной форме.
>>>
#
0
1
2
3
4
5
6
7
8
9

features[:10].table()
centroid
strength
350.0, 175.1
0.0309
465.5, 273.1
0.0367
499.7, 264.2
0.026
291.5, 90.7
0.0368
521.8, 286.6
0.0221
521.8, 286.6
0.0221
132.2, 111.8
0.0335
132.2, 111.8
0.0335
209.1, 220.2
0.0272
96.6, 274.9
0.0422

scale
140
104
103
97.8
94.6
94.6
93.4
93.4
90.5
89.3

orient
204°
268°
137°
348°
77.1°
333°
277°
106°
253°
279°

id
4
7
15
5
3
3
11
11
6
0

Здесь мы видим те же свойства SIFT-признаков, что обсуждались выше, такие как центроид, сила, масштаб и ориентация.
Свойство id указывает на изображение в коллекции, в котором
обнаружен этот признак.
Чтобы понять, какие части представляют эти признаки, мы
можем отобразить их опорные области. Это квадратная область изображения, центрированная по центроиду и повернутая в соответствии с ориентацией признака, с длиной стороны,
пропорциональной масштабу объекта. Эта область исходного
изображения преобразуется в миниатюру размером 50×50 пикселей. Давайте отобразим сетку опорных областей для первых
400 признаков SIFT:
>>> supports = [];
>>> for feature in features[:400]:
... supports.append(feature.support(images))
>>> Image.Tile(supports, columns=20).disp(plain=True);

Результат показан на рис. 12.34. Здесь можно видеть фрагменты зданий и деревьев разного масштаба – от массивных
портиков до небольших окон. Рассмотрим элемент 108, обведенный кружком на рис. 12.34, который представляет довольно
типичную разновидность окна:

Применения 729
>>> feature = features[108]
SIFT feature: (243.9, 200.7), strength=0.03, scale=41.8,
orient=207.9°, id=8

Рис. 12.34. Опорные области для первых 400 признаков SIFT, отсортированные по убыванию
масштаба. Кружком обведена область признака 108

Мы можем отобразить это изображение и наложить на него
признак
>>> images[feature.id].disp();
>>> feature.plot(filled=True, color="y", hand=True, alpha=0.5)

как показано на рис. 12.35а, вместе с крупным планом опорной
области (рис. 12.35б).
Главная идея, лежащая в основе метода «мешка слов», заключается в том, что многие из этих признаков будут описывать визуально похожие элементы сцены, которые, например,
можно видеть на рис. 12.34. Если рассматривать каждый описатель признака SIFT как точку в 128-мерном пространстве, то
похожие дескрипторы образуют кластеры, и эта задача решается методом k средних. Попытка найти 2000 кластеров признаков

730 Глава 12 · Извлечение признаков изображения
>>> bag = BagOfWords(features, 2_000, seed=0)
BagOfWords: 42194 features from 20 images, 2000 words, 0 stop words

а

v (пиксели)

v (пиксели)

возвращает объект BagOfWords, содержащий исходные признаки, центр каждого кластера и различную другую информацию.
Каждый кластер называется визуальным словом и описывается
128-мерным SIFT-дескриптором. Множество всех визуальных
слов (в данном случае 2000) образует визуальный словарь. Подобно тому, как документ можно рассматривать как множество
слов, взятых из некоторого словаря, каждое изображение можно рассматривать как коллекцию (или мешок) визуальных слов,
взятых из визуального словаря.

u (пиксели)

б

u (пиксели)

Рис. 12.35. а) Изображение 8, содержащее 108-й визуальный элемент SIFT, отмеченный зеленым кругом,
показывающим масштаб, и радиальной линией, показывающей ориентацию; б) квадратная опорная область имеет
ту же площадь, что и круг, а горизонтальная ось параллельна направлению ориентации и была масштабирована до
50×50 пикселей

На этапе кластеризации каждому признаку SIFT присваивается индекс визуального слова. Для конкретного признака, показанного выше:
>>> w = bag.word(108)
965

кластеризация методом k средних присвоила этот признак визуальному слову 965, то есть это экземпляр визуального слова
965. Во всем наборе изображений это конкретное визуальное
слово встречается
>>> bag.occurrence(w)
32

раза и присутствует в каждом из изображений
>>> bag.contains(w)
array([ 1, 2, 5, 6, 7, 8, 10, 11, 12, 13, 14, 16, 17, 18, 19])

Применения 731

Мы можем отобразить некоторые из экземпляров слова 965
>>> bag.exemplars(w, images).disp();

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

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

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

Частота встречаемости визуальных слов определяется как
>>> word, freq = bag.wordfreq();

где word – вектор, содержащий все уникальные слова, а freq –
соответствующие им частоты. Визуальные слова встречаются
с совершенно разной частотой, и мы можем видеть
>>> np.max(freq)
704
>>> np.median(freq)
14

что максимальное значение значительно превышает медианное. Разница становится еще более очевидной, если отобразить
значения частоты в порядке убывания:
>>> plt.bar(word, -np.sort(-freq), width=1);
Поисковые системы
игнорируют стопслова, такие как «a»,
«and», «the» и т. д.

как показано на рис. 12.37. Очень часто встречающиеся слова
имеют меньшее значение или не обладают достаточной способностью различать изображения и аналогичны стоп-словам,
известным в сфере поиска текстовых документов.
Давайте будем считать стоп-словами 50 наиболее часто
встречающихся слов и удалим их из мешка слов:
>>> bag = BagOfWords(features, 2_000, nstopwords=50, seed=0)
Removing 7197 features (17.1%) associated with 50 most frequent words
BagOfWords: 34997 features from 20 images, 1950 words, 50 stop words

В результате у нас останется около 35 000 признаков SIFT.
Этот метод выполняет перемаркировку, так что метки слов теперь находятся в интервале [0, 1950].

Количество вхождений

732 Глава 12 · Извлечение признаков изображения

Визуальное слово

Рис. 12.37. Гистограмма частоты встречаемости каждого слова (сортированная). Обратите
внимание на небольшое количество особенно часто встречающихся слов

Наш визуальный словарь содержит k = 1950 визуальных слов.
Применим прием, заимствованный из области поиска текстовых документов, и опишем каждое изображение взвешенным
вектором частоты слов
vi = (t0, ..., tK-1)T ∈ ℝk,

элементы которого описывают частоту встречаемости соответствующих визуальных слов в изображении:
(12.17)

где j – метка визуального слова, N – общее количество изображений в базе данных, Nj – количество изображений, содержащих
слово j, ni – количество слов в изображении i, а nij – количество
раз, когда слово j встречается в изображении i. Обратная частота документа (inverse document frequency, IDF) – это весовой коэффициент, который снижает значимость слов, общих для всех
изображений, и, следовательно, уменьшает их способность отличать изображения. Вот как можно получить взвешенные векторы частот слов для изображения 10:
>>> v10 = bag.wwfv(10);
>>> v10.shape
(1950,1)

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

Это очень большой
вектор, но он содержит менее 1 %
элементов исходного изображения. Это
очень лаконичное
описание.

Применения 733

и реализуется методом similarity. Значение, равное единице,
соответствует максимальному сходству. Определение сходства
изображения 10 со всеми другими изображениями в пакете осуществляется просто:
>>> sim_10 = bag.similarity(v10);

В результате получается массив с 20 элементами, содержащими значения сходства между вектором частоты слов для
изображения 10 и остальными. Отсортируем его по убыванию
сходства.
>>> k = np.argsort(-sim_10)
array([10, 11, 8, 18, 17, 4, 7, 15, 16, 9, 6, ...])

Как видите, изображение 10 наиболее похоже на изображение 10, как и ожидалось, а затем в порядке убывания сходства
следуют изображения 11, 8, 18 и т. д. Они показаны на рис. 12.38
вместе с оценками сходства, и мы видим, что фактически алгоритм обнаружил разные виды одного и того же здания.

изображение № 11, сходство 0.44

v (пиксели)

v (пиксели)

изображение № 10, сходство 1.00

u (пиксели)

u (пиксели)
изображение № 18, сходство 0.32

v (пиксели)

v (пиксели)

изображение № 9, сходство 0.34

u (пиксели)

u (пиксели)

Рис. 12.38. Поиск изображения. Роль образца играет изображение 10, и при поиске были найдены изображения 11,
8 и 18, показанные здесь в порядке убывания меры сходства

734 Глава 12 · Извлечение признаков изображения

А теперь представьте, что у нас появилось несколько новых
изображений, после того как наш робот вышел в мир, и мы хотим сравнить их с изображениями мест, которые он посещал
ранее. Действия в целом аналогичны предыдущему случаю. Загрузим пять новых изображений:
>>> query = ImageCollection("campus/holdout/*.png", mono=True);

На этот раз нам не нужно выполнять кластеризацию, поэтому просто сопоставим признаки, обнаруженные в новых изображениях, уже вычисленному набору визуальных слов, то есть
определим ближайшее визуальное слово для каждого из новых
признаков изображения. Это реализуется просто:
>>> S = bag.similarity(query);

Этот вызов получает признаки SIFT для каждого входного
изображения, находит ближайшее визуальное слово, удаляет
согласно (12.17)
стоп-слова, вычисляет вектор частоты слов
и вычисляет расстояние до векторов частот слов для всех изображений в мешке. В итоге получается массив 5×20, где элемент
S[i, j] содержит меру сходства между новым изображением i
и существующим изображением j. Результат лучше всего интерпретировать визуально (см. рис. 12.39):

Сходство

Индекс
изображения
в запросе

>>> Image(S).disp(colorbar=True);

Индекс изображения в мешке
Рис. 12.39. Матрица сходства для 20 изображений, где светлые цвета соответствуют сильному
сходству. Элемент (i, j) определяет сходство между изображением i в мешке и изображением j
в запросе

Наилучшим соответствием для каждого нового изображения
является максимальное значение в каждой строке:
>>> np.argmax(S, axis=1)
array([ 0, 10, 17, 17, 17])

Также можно было бы использовать метод retrieve:
>>> bag.retrieve(query[0])
(0, 0.5752)
>>> bag.retrieve(query[1])
(10, 0.6631)

который принимает искомое изображение и возвращает кортеж с индексом наиболее точно соответствующего изображе-

Для вычисления
весовых коэффициентов IDF требуется статистика
визуальных слов
из существующего
набора.

Подведение итогов 735

ния в мешке и степень его сходства. Новому изображению 0
наиболее точно соответствует изображение 0 в исходной последовательности, а новому изображению 1 – изображение 10.
Два новых изображения и наиболее близкие к ним существующие изображения показаны на рис. 12.40. Первое изображение
имеет низкую степень схожести, но является приемлемым результатом: найденное изображение содержит то же здание, что
и изображение в запросе.

а

найденное изображение № 0, сходство 0.58

v (пиксели)

v (пиксели)

изображение в запросе № 0

u (пиксели)

б

в

найденное изображение № 10, сходство 0.67

v (пиксели)

v (пиксели)

изображение в запросе № 1

u (пиксели)

u (пиксели)

г

u (пиксели)

Рис. 12.40. Поиск по вновь полученным изображениям. Новым изображениям в запросах (a) и (в) наиболее точно
соответствуют изображения из базы данных (б) и (г) соответственно

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

12.5

Подведение итогов

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

736 Глава 12 · Извлечение признаков изображения

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

12.5.1

Дополнительное чтение

В этой главе представлен классический восходящий подход
к извлечению признаков, начиная с пикселей и продвигаясь
к понятиям более высокого уровня, таким как области и линии.
В Prince (2012) и Szeliski (2022) предлагается хорошее введение
в высокоуровневое зрение с использованием вероятностных
методов, которые можно применять к таким задачам, как распознавание объектов, например распознавание лиц и поиск
изображений. Книгу Szeliski (2022) можно бесплатно скачать
для личного использования по адресу: https://szeliski.org/Book.
В последние несколько лет компьютерное зрение, особенно
распознавание объектов, претерпело революцию благодаря использованию глубоких сверточных нейронных сетей. Они продемонстрировали очень высокий уровень точности в обнаружении и распознавании объектов на сложном фоне, даже при
изменении ракурса и освещения. Ресурсы для обучения с применением PyTorch доступны на https://pytorch.org/.

Признаки области
Сегментация изображений на основе областей и анализ блобов
являются классическими методами, описанными во многих
книгах и статьях. В Gonzalez and Woods (2018) и Szeliski (2022)
подробно рассматриваются методы, представленные в этой главе, в частности пороговые и граничные дескрипторы. В Davies
(2017) and Klette (2014) описываются фильтрация по пороговому

Подведение итогов 737

значению, анализ связности, анализ формы и преобразование
Хафа с его вариантами. Алгоритм Оцу для определения порога
представлен в статье Otsu (1975), а алгоритм Ниблэка для адаптивного определения порога представлен в статье Niblack (1985).
Книга Nixon and Aguado (2012) расширяет материал, рассмотренный в этой главе, и знакомит с такими методами, как деформируемые шаблоны и дескрипторы границ. Цепной код Фримена
был впервые описан в работе Freeman (1974). В Flusser (2000) показано, что семь моментов, предложенных в Hu (1962), на самом
деле не являются независимыми, поскольку ϕ3 = (ϕ52 + ϕ72) / ϕ43.
В дополнение к однородности областей, основанной на интенсивности и цвете, также возможно охарактеризовать текс­
туру областей – пространственный образец интенсивности
пикселей, статистика которого описана в Gonzalez and Woods
(2018). Затем области можно сегментировать в соответствии
с текстурой – например, выделить гладкую дорогу или текстурированную траву.
Кластеризация данных – важная тема машинного обучения
(Bishop, 2006). В этой главе мы использовали простую реализацию k средних, которая далека от современной методики
кластеризации и требует, чтобы количество кластеров было известно заранее. Более продвинутые алгоритмы кластеризации
являются иерархическими и используют для ускорения поиска
соседних точек структуры данных, такие как kd-деревья. Инициализация центров кластера также имеет решающее значение
для производительности. В Szeliski (2022) представлены более
общие методы кластеризации, а также методы компьютерного зрения на основе графов. Алгоритм рассечения графов для
сегментации описан в Felzenszwalb and Huttenlocher (2004). Алгоритм максимально робастной экстремальной области (MSER)
описан в Matas et al. (2004). Набор данных Berkeley Segmentation Dataset, доступный по адресу http://www.eecs.berkeley.edu/
Research/Projects/CS/vision/bsds, содержит множество сложных
изображений реального мира, каждое из которых имеет несколько сегментаций, выполненных человеком.
Ранняя работа по использованию распознавания текста для
робототехники описана в Posner et al. (2010), а в Lam et al. (2015)
разбирается применение OCR с целью анализа планов этажей
зданий для навигации роботов. Основная проблема OCR реальных сцен состоит в том, чтобы определить, какие части сцены содержат текст и должны быть переданы в механизм OCR.
Мощным детектором текста является преобразование ширины штриха, описанное в Li et al. (2014). OCR-модуль Tesseract
с открытым исходным кодом был разработан в Hewlett Packard
в конце 1980-х, открыт в 2005-м и впоследствии спонсировался
Google. Он доступен по адресу https://github.com/tesseract-ocr
и описан в статье его первоначального разработчика (2007).

738 Глава 12 · Извлечение признаков изображения

Признаки линии
Преобразование Хафа было впервые описано в патенте США
3069654 «Метод и средства распознавания сложных образов»
Пола Хафа, а его история обсуждается в статье Hart (2009). Первоначальное приложение было предназначено для анализа фотографий пузырьковой камеры и использовало проблематичную параметризацию линий на основе пересечения и наклона.
Известная в настоящее время параметризация (θ, ρ) была впервые описана в Duda and Hart (1972) как «обобщенное преобразование Хафа». Статья доступна по адресу http://www.ai.sri.
com/pubs/files/tn036-duda71.pdf. Преобразование Хафа описано в таких учебниках, как Szeliski (2022), Gonzalez and Woods
(2018), Davies (2017) и Klette (2014). В последнем есть хорошее
обсуждение подгонки формы в целом и вычислений, которые
являются устойчивыми по отношению к точкам выбросов. Базовое преобразование Хафа было расширено во многих аспектах, и по нему имеется обширная литература. Полезный обзор
преобразования и его вариантов представлен в Leavers (1993).
Преобразование может быть распространено на другие формы
(Ballard, 1981), такие как круги фиксированного размера, где
голоса отдаются за координаты центра круга. Для кругов неизвестного размера требуется трехмерный массив голосования
с учетом центра и радиуса круга.

Признаки точки
Литература по операторам опорных точек восходит к ранним
работам Moravec (1980), Förstner and Gülch (1987) и Förstner
(1994). Угловой детектор Харриса (Harris and Stephens, 1988)
стал очень популярным в роботизированном зрении в конце
1980-х, поскольку он мог работать в режиме реального времени
на компьютерах того времени, а его признаки были достаточно
стабильными от кадра к кадру (Tissainayagam and Suter, 2004).
Детектор Нобля описан в Noble (1988). Исследования Shi and
Tomasi (1994) и Tomasi and Kanade (1991) привели к созданию
детектора Ши–Томаси и трекера Канаде–Лукаса–Томаси (KLT).
Хорошими обзорами относительной производительности многих детекторов углов являются работы Deriche and Giraudon
(1993) и Mikolajczyk and Schmid (2004).
Концепции масштабного пространства давно известны
в компьютерном зрении. Доступные введения на эту тему можно найти в Koenderink (1984), Lindeberg (1993) и ter Haar Rome­
ny (1996). Понятие масштабного пространства применялось
к классическим угловым детекторам для создания гибридных
детекторов, таких как масштабный детектор Харриса (Mikolaj­
czyk and Schmid, 2004). Важным достижением в области детекторов пространственных признаков стало масштабно-инвариантное преобразование признаков (SIFT), представленное

Подведение итогов 739

в начале 2000-х в Lowe (2004) и значительно улучшившее работу таких приложений, как отслеживание и распознавание объектов. Необычно, и возможно – к сожалению, алгоритм запатентован и не мог быть использован в этой книге. Природа не
терпит пустоты, и вскоре была разработана эффективная альтернатива (SURF) (Bay et al. 2008). Патент на SIFT истек в марте 2020 года. Детекторы SIFT и SURF дают разные результаты,
и они сравниваются в работе Bauer et al. (2007).
Кроме вышеупомянутых, существуют и продолжают появляться многие другие детекторы опорных точек и признаков.
Алгоритм FAST (Rosten et al., 2010) имеет очень низкие требования к вычислительным ресурсам и высокую воспроизводимость, а программные ресурсы C и MATLAB доступны по адресу
http://www.edwardrosten.com/work/fast.html. CenSurE (Agrawal
et al. 2008) предлагает более высокую производительность,
чем SIFT, SURF и FAST, при меньших затратах. Алгоритм BRIEF,
предложенный в Calonder et al. (2010), не является детектором
признаков, но представляет собой недорогой и компактный дескриптор признаков, требующий всего 256 бит вместо 128 чисел
с плавающей точкой на признак. К другим дескрипторам признаков относятся гистограмма ориентированных градиентов
(histogram of oriented gradients, HOG), ориентированный FAST
и повернутый BRIEF (ORB), бинарная робастная инвариантно
масштабируемая ключевая точка (binary robust invariant scaleable keypoint, BRISK), быстрый алгоритм ключевой точки сетчатки (fast retina keypoint, FREAK), агрегированные характерис­
тики канала (aggregate channel features, ACF), вектор локально
агрегированных дескрипторов (vector of locally aggregated descriptors, VLAD), случайные «листы папоротника» и многиемногие другие. Эти дескрипторы признаков часто описываются
как спроектированные, то есть разработанные людьми с пониманием проблемы. Развивающейся альтернативой является
применение методов машинного обучения, и это привело к появлению таких дескрипторов, как LIFT (Yi et al., 2016) и GRIEF
(Krajník et al., 2017).
Локальные признаки имеют много преимуществ и довольно
стабильны от кадра к кадру, но в сценариях наружного применения расположение признаков и их дескрипторы значительно
различаются в зависимости от условий освещения (например,
Valgren and Lilienthal, 2010). Очевидные примеры – ночь и день,
но даже в течение нескольких часов дескрипторы значительно
меняются. С течением времени внешний вид сцены может резко
меняться: деревья с листьями или без них; земля, покрытая травой или снегом, влажная или сухая. Поиск способов надежного
распознавания элементов сцены, несмотря на их меняющийся
внешний вид, является областью исследований надежного распознавания, которая представлена в статье Lowry et al. (2015).

740 Глава 12 · Извлечение признаков изображения

Метод «мешка слов» для поиска изображений был впервые
предложен в Sivic and Zisserman (2003) и с тех пор использовался многими другими исследователями. Заметным расширением для робототехнических приложений является алгоритм
FABMAP (Cummins and Newman, 2008), который явно учитывает
совместную вероятность появления признаков, связывает вероятность с соответствием изображения и доступен в OpenCV.

12.5.2

Упражнения

1. Классификация областей в градациях серого.
а) Поэкспериментируйте со значениями параметра ithresh
на изображениях castle.png и castle2.png.
б) Поэкспериментируйте с алгоритмом адаптивного порога
и его параметрами.
в) Разработайте алгоритм, который находит буквы в сегментации MSER, представленной на рис. 12.9.
г) Исследуйте параметры метода MSER.
д) Примените функцию labels_graphseg к изображению castle2.png. Изучите назначение параметров и настройте их
для повышения качества.
е) Загрузите изображение adelson.png и попытайтесь сегментировать буквы A и B.
2. Классификация областей на цветных изображениях.
a) Измените количество кластеров k в примерах классификации по цвету. Существует ли лучшее значение?
б) Запустите алгоритм k средних несколько раз и определите, насколько различаются итоговые кластеры.
в) Напишите функцию, определяющую, какой из кластеров
представляет цели, т. е. желтый или красный.
г) Примените функцию labels_graphseg к изображению посадочной мишени и сада. Как она работает? Изучите назначение параметров и настройте их для улучшения качества результатов.
д) Поэкспериментируйте с параметрами морфологической
«очистки», используемой для изображений посадочной
мишени и сада.
е) Напишите код, который перебирает изображения, снятые
с камеры вашего компьютера, применяет классификацию и показывает результат. Критерием классификации
может служить пороговое значение оттенков серого или
цветовая кластеризация в заранее изученный набор цветовых кластеров (см. kmeans_color, centroid= ...).
3. Работа с блобами. Создайте изображение объекта с несколькими отверстиями. Вы можете нарисовать его сами и экс-

Подведение итогов 741

портировать из программы для рисования или сформировать его программно.
а) Определите внешние, внутренние и общие границы объекта.
б) Поместите небольшие предметы в отверстия в предметах. Напишите код для отображения топологической
иерар­хии блобов в сцене.
в) Создайте блоб, центр масс которого находится за пределами блоба.
г) Для одной и той же формы в разных масштабах постройте график изменения округлости в зависимости от масштаба. Объясните форму этой кривой.
д) Создайте квадратный объект и постройте предполагаемый и истинный периметр как функцию длины стороны
квадрата. Что происходит, когда площадь мала?
е) Создайте изображение простой сцены с несколькими
объектами различной формы. Используйте признаки, инвариантные к форме (соотношение сторон, округлость),
для создания простого классификатора формы. Насколько хорошо он работает? Повторите решение, используя
функции момента Ху.
ж) Повторите пример сопоставления границ с некоторыми
созданными вами объектами. Измените код, чтобы создать график зависимости угла край–сегмент (k) от θ, и повторите пример сопоставления границ.
з) Другой часто используемой функцией, не поддерживае­
мой Toolbox, является совмещенный прямоугольник (alig­
ned rectangle). Это наименьший прямоугольник, стороны
которого выровнены с осями эквивалентного эллипса
и который полностью охватывает блоб. Соотношение
сторон этого прямоугольника и отношение площади блоба к площади прямоугольника являются свойствами, не
зависящими от масштаба и поворота. Напишите код для
вычисления этого прямоугольника, наложения прямо­
угольника на изображение и вычисления двух признаков.
и) Напишите код для отслеживания периметра блоба.
4. Поэкспериментируйте с функцией OCR.
а) Сделайте снимок и попробуйте прочитать текст на нем.
Как точность зависит от размера текста, контрастности
и ориентации?
б) Как различаются результаты для сцены с большим количеством фона и области интереса, состоящей из одного
слова? Как найти ограничивающие рамки для слов на
изображении?
5. Преобразование Хафа.
а) Поэкспериментируйте с использованием краевого оператора Собеля вместо оператора Кэнни.

742 Глава 12 · Извлечение признаков изображения

б) Примените преобразование Хафа к одному из ваших собственных изображений.
в) Напишите код, который перебирает изображения, снятые
камерой вашего компьютера, находит две доминирующие линии и накладывает их на изображение.
г) Напишите реализацию преобразования Хафа с нуля.
6. Детекторы углов.
а) Поэкспериментируйте с детектором Харриса, изменяя
параметры k, σD и σI.
б) Сравните характеристики угловых детекторов Харриса,
Нобля и Ши–Томаси.
в) Реализуйте детектор Моравека и сравните его с детектором Харриса.
г) Создайте сглаженную вторую производную Xuu, Xvv и Xuv.
7. Поэкспериментируйте с применением глубокого обучения
для классификации пикселей и обнаружения объектов.
а) Попробуйте другие предварительно обученные сети.
б) Измерьте время прогнозирования (учтите, что для получения первого прогноза может потребоваться гораздо
больше времени, чем для последующих).
в) Создайте цикл в реальном времени, который принимает
входные данные с камеры, применяет детектор объектов
и отображает результат.
8. Поэкспериментируйте с алгоритмом «мешок слов».
а) Изучите различные опорные области различных визуальных слов, используя метод образцов.
б) Изучите влияние изменения количества стоп-слов.
в) Исследуйте влияние изменения объема словарного запаса. Попробуйте 1000, 1500, 2500, 3000 и 5000.
г) Попробуйте использовать разные наборы данных. Сделайте несколько снимков окрестностей вашего дома или
района или воспользуйтесь одним из множества крупных
наборов данных, доступных в интернете, например набором данных Oxford Robot Car (Maddern et al., 2016).
д) Изучите прием RootSIFT, описанный в Arandjelović and
Zisserman (2012).
е) Проведите исследование, используя признаки SURF вмес­
то SIFT.
ж) Проведите исследование с использованием углового детектора SIFT с признаками BRISK или FREAK.

Глава

13

	 Формирование
изображения

chap13.ipynb

https://go.sn.pub/
bD6sdh

В этой главе мы обсудим детали формирования и фиксации
изображений, о которых мы так много говорили в предыдущих
главах. Давно известно, что простое маленькое отверстие способно создать идеальное перевернутое изображение на стене
затемненной комнаты. У некоторых морских моллюсков, например у наутилуса, есть глаза-камеры с точечным отверстием. У всех позвоночных имеется хрусталик, формирующий
перевернутое изображение на сетчатке, где располагаются
светочувствительные клетки – палочки и колбочки, показанные ранее на рис. 10.6. Цифровая камера устроена по тому же
принципу: стеклянная или пластиковая линза формирует изображение на поверхности массива светочувствительных полупроводниковых устройств для преобразования света в цифровое изображение.
Процесс формирования изображения в глазу или в камере
включает проекцию трехмерного мира на двухмерную поверхность. Информация о глубине теряется, и мы больше не можем
определить по изображению, представляет ли оно большой
объект, расположенный далеко, или меньший, но расположенный ближе. Это преобразование из трехмерного представления
в двухмерное называется перспективной проекцией и обсуждается в разделе 13.1. Раздел 13.2 знакомит с вопросами калибровки камеры – оценкой параметров преобразования перспективы.
В разделах с 13.3 по 13.5 будут представлены альтернативные
типы камер, способных снимать широкоугольные, панорамные
или светопольные изображения. В разделе 13.6 будут описаны
два примера практического применения: использование искусственных маркеров в сценах для определения положения
объектов и использование плоской гомографии для проведения измерений на основе плоских изображений. Раздел 13.7
знакомит с некоторыми продвинутыми концепциями, такими
как проецирование прямых и конических сечений, а также с неперспективными камерами.

744 Глава 13 · Формирование изображения

13.1

Перспективная камера

13.1.1

Перспективная проекция

Небольшое отверстие в стене затемненной комнаты будет отбрасывать на противоположную стену тусклое перевернутое
изображение внешнего мира – так устроена простейшая проективная камера с точечным отверстием (pinhole), известная
также как камера-обскура. Такая камера дает очень тусклое
изображение, поскольку, как показано на рис. 13.1а, лишь малая
часть света, исходящего от объекта, участвует в создании изображения. Камера-обскура не имеет регулировки фокуса – все
объекты находятся в фокусе независимо от расстояния.
Ключом к получению более ярких изображений является использование объектива, который собирает свет от объекта на
большей площади и направляет его на плоскость изображения,
как показано на рис. 13.1. Выпуклая линза может формировать
изображение точно так же, как точечное отверстие, а базовый
геометрический принцип формирования изображения при помощи тонкой линзы показан на рис. 13.2. Положительная ось z –
это оптическая ось камеры.
Координата z объекта и его изображение относительно цент­
ра линзы связаны уравнением тонкой линзы:
(13.1)
где zo – расстояние до объекта, zi – расстояние до изображения,
f – фокусное расстояние линзы. ► Если объект расположен на
расстоянии zo > f, перевернутое изображение формируется на
плоскости изображения при zi < −f.

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

Отступление 13.1. Камера-обскура
В V в. до н. э. философ Мози из Древнего Китая упомянул об эффекте перевернутого изображения, формирующегося через точечное
отверстие. Камера-обскура – это затемненная комната, в которой свет, проникающий
через маленькое отверстие, отбрасывает на
стену неясное перевернутое изображение
мира. Эти камеры были популярными туристическими достопримечательностями
в викторианские времена, особенно в Британии, и многие из них функционируют до
сих пор. (Изображение взято из коллекции
Drawing with Optical Instruments на http://vision.mpiwg-berlin.mpg.de/elib.)

Перспективная камера 745

объект

объект

отверстие

линза
изображение

изображение
изображение

а
б
Рис. 13.1. Сравнение светосборной способности: а) камера-обскура; б) объектив с линзой

луч

от

то

чеч

но

го

объект

от

вер

ст

ия

эквивалент
точечного
отверстия

перевернутое
изображение

идеальная
тонкая
линза

плоскость изображения

точки фокуса

Рис. 13.2. Геометрия формирования изображения с помощью тонкой выпуклой линзы,
показанной в поперечном сечении. Линза имеет две фокусные точки на расстоянии f
по обеим сторонам. По соглашению оптическая ось камеры – это ось z

В фотоаппарате плоскость изображения совпадает с чувствительной поверхностью оптического сенсора, поэтому кольцо
фокусировки камеры перемещает объектив вдоль оптической
оси на расстояние zi от плоскости изображения – для бесконечно удаленного объекта zi = f. Недостатком использования объектива является необходимость фокусировки. Наш собственный
глаз имеет единственную выпуклую линзу, состоящую из прозрачных белков-кристаллинов, а фокусировка достигается за
счет мышц, которые изменяют ее форму, – этот процесс называется аккомодация. Объектив камеры высокого качества представляет собой составное устройство, состоящее из нескольких
стеклянных или пластиковых линз.
В компьютерном зрении обычно используется модель визуа­
лизации в центральной перспективе, показанная на рис. 13.3.

746 Глава 13 · Формирование изображения

Лучи от точки P сходятся в начале системы координат камеры
{C}, падают на плоскость изображения, расположенную в точке p на расстоянии z = f. Ось z пересекает плоскость изображения в главной точке (principal point), которая является началом системы координат двухмерного изображения. Используя
принцип подобия треугольников, можно показать, что точка
с мировыми координатами P = (X, Y, Z) проецируется в точку
изображения, представленную координатным вектором p =
(x, y), согласно формуле
(13.2)
которая определяет координаты плоскости изображения. В случае когда f = 1, координаты называются нормализованными,
или каноническими, координатами плоскости изображения.
Уравнение (13.2) задает проективное преобразование, или, точнее, перспективную проекцию.

ый

нн

ио

кц

ое

пр
ч

лу

опт

иче

ска

яо

сь

ная
главчка
то

ть

скос

пло

б

изо

ия

ен
раж

начало
координат
камеры

Рис. 13.3. Модель центральной проекции. Плоскость изображения находится на расстоянии f
перед началом координат камеры, и на ней формируется неперевернутое изображение.
Система координат камеры – правосторонняя с осью z, определяющей центр поля зрения

Это отображение из трехмерного мира в двухмерное изображение имеет следствия, которые можно видеть на рис. 13.4, –
параллельные прямые сходятся, а окружности становятся эллипсами. Более формально можно сказать, что преобразование
из наблюдаемого мира в плоскость изображения имеет следующие свойства.
Оно выполняет отображение из трехмерного пространства
в двухмерную плоскость изображения: 𝒫  : ℝ3 ↦ ℝ2.
„„ Прямые линии мира проецируются на прямые линии на
плоскости изображения.
„„

Перспективная камера 747

а

б

Рис. 13.4. Эффект трансформации перспективы: а) параллельные прямые сходятся;
б) окружности становятся эллипсами

Параллельные прямые в мире проецируются на прямые,
которые пересекаются в точке схождения, как показано на
рис. 13.4а. В рисовании этот эффект известен как ракурс.
Исключением являются фронто-параллельные линии –
лежащие в плоскости, параллельной плоскости изображения, – которые всегда остаются параллельными.
„„ Коники
наблюдаемого мира проецируются на коники на
плоскости изображения. Например, круг проецируется как
круг или эллипс, как показано на рис. 13.4б.
„„ Размер (площадь) фигуры не сохраняется и зависит от расстояния.
„„ Отображение не является взаимно однозначным, и не существует единственного обратного отображения. То есть,
зная (x, y), мы не можем однозначно определить (X, Y, Z).
Все, что можно сказать, – это то, что мировая точка лежит
„„

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

Отступление 13.2. Апертура объектива
Так называемое f-число (диафрагменное число),
обычно указанное на корпусе объектива, представляет собой безразмерную величину F = f/d, где d –
диаметр линзы (часто обозначается символом ϕ на
оправе линзы). Это число обратно пропорционально
светосиле объектива. Чтобы уменьшить коли­чест­
во света, падающего на плоскость изображения,
эффективный диаметр уменьшают за счет механической апертуры или диафрагмы, что увеличивает
f-число. Освещенность на плоскости изображения
обратно пропорциональна F2, так как зависит от площади сбора света. Чтобы
уменьшить освещенность в два раза, f-число должно быть увеличено в 2 раз, или
«на один стоп». Градуировка f-числа увеличивается в 2 раз на каждый «стоп».
f-число обычно записывают в виде f/1.4 для F = 1.4.

748 Глава 13 · Формирование изображения
Отступление 13.3. Фокус и глубина резкости
В идеале группа световых лучей из точки сцены встречается в точке изображения.
При несовершенном фокусе лучи вместо этого образуют пятно конечного размера, называемое кругом нерезкости (circle of confusion), которое является функцией рассеяния точки оптической системы. Принято считать, что если размер круга
нерезкости примерно равен пикселю, то изображение приемлемо сфокусировано.
Камера-обскура не имеет управляемой фокусировки и всегда создает сфокусированное изображение объектов независимо от их расстояния. Объектив не
обладает этим свойством – кольцо фокусировки изменяет расстояние между
объективом и плоскостью изображения и должно быть отрегулировано так, чтобы интересующий объект был приемлемо сфокусирован. Фотографы используют понятие глубины резкости, представляющей собой диапазон расстояний до
объектов, для которых формируются приемлемо сфокусированные изображения.
Глубина резкости высока при малой диафрагме, когда объектив больше похож на
точечное отверстие, но это означает меньшее количество света и более шумные
изображения или более длительное время выдержки и размытие движения. Это
дилемма, с которой постоянно сталкивается фотограф!

где-то вдоль красного проекционного луча, показанного на рис. 13.3. Это важная тема, к которой мы вернемся
в главе 14.
„„ Преобразование неконформное – не сохраняет форму, так
как внутренние углы не сохраняются. Перенос, вращение
и масштабирование являются примерами конформных
преобразований.

13.1.2

Моделирование перспективной камеры

Мы можем записать координаты точки плоскости изображения
в однородной форме p̃ = (x̃, ỹ, z̃), где
x̃ = f X,

ỹ = fY,

z̃ = f Z,

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

(13.3)

Неоднородные координаты плоскости изображения записываются так:

Кратко об однородных координатах рассказывается
в разделе C.2.

Перспективная камера 749

Если мы запишем мировую координату также в однородной
форме CP̃ = (X, Y, Z, 1)T ∈ ℙ3, то перспективную проекцию можно
будет записать в линейной форме как
(13.4)
или
p̃ = C CP̃,

(13.5)

где C – мат­ри­ца 3×4, известная как мат­ри­ца камеры, а CP̃ – координата точки относительно системы координат камеры {C}.
Матрицу камеры можно разложить на множители

где мат­ри­ца Π является мат­ри­цей проекции, которая отображает трехмерное изображение в двухмерное.
Используя Toolbox, можно создать модель камеры центральной перспективы, например с линзой 15 мм
>>> camera = CentralCamera(f=0.015);

Этот вызов возвращает экземпляр класса CentralCamera, наследующего класс CameraBase. По умолчанию камера находится в начале мировой системы координат, а ее оптическая ось
указывает в направлении оси z мира, как показано на рис. 13.3.
Определяем мировую точку
>>> P = [0.3, 0.4, 3.0];

в метрах и получаем соответствующие координаты плоскости
изображения:
>>> camera.project_point(P)

При проецировании array([[ 0.0015],
нескольких точек
[ 0.002]])
мировые точки
являются столбцаТочка на плоскости изображения находится на расстоянии
ми в массиве 3×N,
а проецируемые (1.5, 2.0) мм по отношению к главной точке.
Это очень неточки – соответствубольшое
смещение,
но
оно
соизмеримо
с
размером
типичного
ющими столбцами
в массиве 2×N. сенсора изображения.

750 Глава 13 · Формирование изображения

В общем случае камера будет иметь произвольное положение
ξC относительно системы мировых координат, как показано на
рис. 13.5. Положение точки относительно камеры:
P = Cξ0 • 0P,

C

(13.6)

или если использовать однородные координаты:
P = (0TC)-1 0P,

C

где 0TC ∈ SE(3).

Рис. 13.5. Мировая система координат и система координат камер, а также мировая точка P
и ее отображение в каждой системе координат

Мы можем легко продемонстрировать это, переместив нашу
камеру на 0.5 м влево:
>>> camera.project_point(P, pose=SE3.Tx(-0.5))
array([[ 0.004],
[ 0.002]])

где положение камеры ξC передается как объект SE3. Мы видим,
что координата x увеличилась с 1.5 мм до 4.0 мм, т. е. точка
изображения сместилась в противоположном направлении –
вправо.

13.1.3

Дискретная плоскость изображения

В цифровой камере плоскость изображения представляет собой сетку размером W×H светочувствительных элементов,
называемых фотоячейками, которые непосредственно соответствуют элементам изображения (или пикселям), как показано на рис. 13.6. Координаты пикселя представляют собой
двухмерный вектор (u, v) неотрицательных целых чисел, и по

Перспективная камера 751

соглашению начало координат находится в верхнем левом углу
плоскости изображения. Пиксели имеют одинаковый размер
и равномерно расположены на сетке, поэтому координата пикселя (u, v) связана с координатой (x, y) плоскости изображения
соотношением
(13.7)

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

где ρw и ρh – ширина и высота каждого пикселя соответственно,
а (u0, v0) – главная точка – пиксельная координата точки, в которой оптическая ось пересекает плоскость изображения относительно нового начала координат. В матричной форме однородное соотношение между координатами плоскости изображения
и пиксельными координатами выглядит следующим образом:

где s = 0, но иногда записывается с ненулевым значением для
учета перекоса, т. е. оси u и v неортогональны. Теперь (13.4)
можно записать в терминах пиксельных координат:
(13.8)

кц

ое

пр
нн

ио
ый
ч

лу

опт

иче

ска

яо

сь

ная
глав а
к
ч
о
т
сть

ско
пло

бр

изо

я

ни
аже

начало
координат
камеры

Рис. 13.6. Модель центральной проекции, представляющая плоскость изображения
и дискретные пиксели

752 Глава 13 · Формирование изображения

где p̃ = (ũ, ṽ, w̃) – однородная координата мировой точки P в пиксельных координатах. Произведение первых двух членов – это
внутренняя мат­ри­ца камеры K с размерами 3×3. Неоднородные
координаты пикселей плоскости изображения равны υ:
(13.9)
Например, если пиксели представляют собой квадрат со стороной 10 мкм, а массив пикселей имеет размер 1280×1024 пикселя с главной точкой в координатах плоскости изображения
(640, 512), следующий код
>>> camera = CentralCamera(f=0.015, rho=10e-6, name="mycamera",
...
imagesize=[1280, 1024], pp=[640, 512])
Name: mycamera [CentralCamera]
pixel size: 1e-05 x 1e-05
image size: 1280 x 1024
pose: t = 0, 0, 0; rpy/yxz = 0°, 0°, 0°
principal pt: [
640
512]
focal length: [ 0.015
0.015]

выводит параметры модели камеры, включая положение камеры. ► Соответствующие неоднородные координаты плоскости
изображения ранее определенной мировой точки равны
>>> camera.project_point(P)
array([[
790],
[
712]])

Мы отобразили
другой порядок
вращения по осям
крена-тангажа-рыс­
кания YXZ. Учитывая
способ, которым
мы определили оси
камеры, ориентация
камеры по отношению к мировой
системе координат
представляет собой
рыскание вокруг
вертикальной оси
или оси y, за которым следует тангаж
вокруг оси x и, наконец, крен вокруг
оптической оси z.

Отступление 13.4. Оптический сенсор
Светочувствительные элементы в чипе камеры – фотоячейки – обычно имеют квадратную форму с длиной стороны в диапазоне
1–10 мкм. Профессиональные камеры имеют
большие фотоячейки для повышения светочувствительности, тогда как камеры мобильных телефонов имеют маленькие сенсоры
и, следовательно, маленькие менее чувствительные ячейки. Отношение количества пикселей по горизонтали к вертикали совпадает
с отношением сторон и обычно составляет
4:3 или 16:9 (см. отступление 11.6). Размер датчика измеряется по диагонали массива и обычно выражается в дюймах, например 1/3, 1/4 или 1/2 дюйма. Однако активная чувствительная область чипа имеет диагональ, которая обычно составляет
около 2/3 заявленного размера.

Перспективная камера 753

13.1.4

Матрица камеры

Объединив уравнения (13.6) и (13.8), мы можем записать проекцию камеры в общем виде как

а подставляя C T0 вместо (0TC)-1, получаем

(13.10)
Члены f/ρw и f/ρh
представляют
собой фокусное
расстояние,
выраженное
в единицах
пикселей.

где все члены свернуты в мат­ри­цу камеры C. Она представляет собой гомогенное преобразование 3×4, которое выполняет
масштабирование, перемещение и перспективную проекцию.
Ее также называют проекционной мат­ри­цей, или калибровочной мат­ри­цей, камеры. Эту мат­ри­цу можно разложить на собственную мат­ри­цу камеры K размером 3×3 и мат­ри­цу 3×4.
 ажно отметить, что внешние параметры вращения и перемещеВ
ния описывают мировую систему координат относительно камеры.

Проекция также может быть записана в функциональной
форме как
p = 𝒫(P, K, ξC),

(13.11)

где P – вектор координат точки P в мировой системе координат.
K представляет собой мат­ри­цу параметров камеры и хранит
внутренние характеристики камеры и сенсора, такие как f, ρw,
ρh, u0 и v0. ξC представляет собой положение камеры и содержит
как минимум шесть внешних параметров, которые описывают
перемещение и ориентацию камеры в SE(3).
Существует 5 внутренних и 6 внешних параметров (итого
11 независимых параметров) для описания камеры. Матрица
камеры состоит из 12 элементов, поэтому одна степень свободы – общий коэффициент масштабирования – не ограничена
и может быть выбрана произвольно. Обычно мат­ри­ца нормализована так, что нижний правый элемент равен единице.

754 Глава 13 · Формирование изображения
Отступление 13.5. Неоднозначность перспективной проекции
Перспективной проекции присуща фундаментальная неоднозначность – мы не
можем отличить большой удаленный объект от меньшего более близкого объекта.
Уравнение (13.10) можно переписать в виде
p̃ = CP̃ = C(H-1H)P̃ = (CH-1)(HP̃) = C¢P̃ ¢,
где H ∈ ℝ3×3 – произвольная невырожденная мат­ри­ца размера. Это означает, что
бесконечное число комбинаций камеры C¢ и координат мировой точки P̃ ¢ дадут
одну и ту же проекцию p̃ на плоскость изображения.
В этом и заключается главная трудность определения трехмерных мировых
координат по двухмерным проекционным координатам. Ее можно преодолеть,
только обладая информацией о камере или трехмерном объекте.

Матрица внутренних параметров K для текущей камеры:
>>> camera.K
array([[
1500,
[
0,
[
0,

0,
1500,
0,

640],
512],
1]])

а мат­ри­ца C камеры:
>>> camera.C()
array([[
1500,
[
0,
[
0,

0,
1500,
0,

640,
512,
1,

0],
0],
0]])

и зависит от положения камеры. ◄ Если положение не указано,
то по умолчанию используется положение объекта камеры.
Поле зрения камеры зависит от ее фокусного расстояния f.
Широкоугольный объектив имеет маленькое фокусное расстоя­
ние, телеобъектив имеет большое фокусное расстояние, а зумобъектив – регулируемое фокусное расстояние. Поле зрения
можно определить по геометрической схеме на рис. 13.6. В горизонтальном направлении половинный угол обзора равен

где W – количество пикселей в горизонтальном направлении.
Отсюда мы можем написать:
(13.12)
Отметим, что поле зрения также является функцией размеров чипа камеры, которые записываются как Wρw×Hρh. Поле
зрения вычисляется методом fov объекта камеры

Поэтому C
является методом,
а не свойством.

Перспективная камера 755
>>> np.rad2deg(camera.fov())
array([ 46.21,
37.69])

в градусах по горизонтали и вертикали соответственно.
Отступление 13.6. Поле зрения объектива
Поле зрения объектива представляет собой незамкнутую прямо­
угольную усеченную пирамиду, образующую углы θh и θv в горизонтальной и вертикальной плоскостях соответственно. Нормальный
объектив имеет поле зрения около 50°, тогда как широкоугольный
объектив имеет поле зрения > 60°. За пределами 110° трудно создать линзу, поддерживающую перспективную проекцию, поэтому
широкоугольные линзы обычно являются неперспективными.
Для очень широкоугольных объективов чаще описывают поле
зрения как телесный угол, который измеряется в стерадианах (ср).
Это площадь поля зрения, спроецированного на поверхность единичной сферы. Полусферическое поле зрения равно 2π ср, а полное
сферическое поле равно 4π ср. Если аппроксимировать поле зрения
камеры конусом с углом при вершине θ, то соответствующий телес­
ный угол равен 2π(1 − cos θ/2) ср. Камера с полем зрения больше
полной полусферы называется всенаправленной, или панорамной.

Объект камеры поддерживает возможность проверки видимости точек
>>> P = np.column_stack([[0, 0, 10], [10, 10, 10]])
array([[ 0, 10],
[ 0, 10],
[10, 10]])
>>> p, visible = camera.project_point(P, visibility=True)
>>> visible
array([ True, False])

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

13.1.5

Точки проекции

Класс CentralCamera способен проецировать несколько точек или
линий на плоскость изображения. С помощью Toolbox создадим сетку точек 3×3 в плоскости xy с общей длиной стороны
0.2 м и центром в точке (0, 0, 1):
>>> P = mkgrid(n=3, side=0.2, pose=SE3.Tz(1.0));
>>> P.shape
(3, 9)

756 Глава 13 · Формирование изображения

В результате получаем мат­ри­цу 3×9 с одним столбцом на точку сетки, где каждый столбец содержит координаты в порядке
X, Y, Z. Вот первые четыре столбца:
>>> P[:, :4]
array([[
-0.1,
[
-0.1,
[
1,

-0.1,
0,
1,

-0.1,
0.1,
1,

0],
-0.1],
1]])

По умолчанию mkgrid создает сетку в плоскости xy с центром
в начале координат. Необязательный последний аргумент – это
мат­ри­ца SE(3), которая преобразует точки и позволяет произвольно располагать и ориентировать плоскость.
Координаты вершин в плоскости изображения:
>>> camera.project_point(P)
array([[ 490, 490, 490, 640, 640, 640, 790, 790, 790],
[ 362, 512, 662, 362, 512, 662, 362, 512, 662]])

которые можно вывести на график
>>> camera.plot_point(P);

и получить вид виртуальной камеры (рис. 13.7а). Положение камеры
>>> T_camera = SE3.Trans(-1, 0, 0.5) * SE3.Ry(0.9);

дает косой вид на плоскость
>>> camera.clf() # clear the virtual image plane
>>> camera.plot_point(P, pose=T_camera);

v (пиксели)

v (пиксели)

показанный на рис. 13.7б. Здесь ясно виден эффект перспективной
проекции, которая исказила форму квадрата – верхний и нижний
края, которые являются параллельными линиями, бы­ли спроецированы на линии, которые сходятся в точке схождения.

u (пиксели)
а
б
Рис. 13.7. Два вида плоской сетки точек: а) вид спереди; б) вид сбоку

u (пиксели)

Перспективная камера 757
Слово «идеальТочка схождения линий может быть определена по проекная» в данном ции ее идеальной линии. Верхняя и нижняя линии сетки паралконтексте означает
скорее особый, чем лельны мировой оси x или вектору (1, 0, 0). Соответствующая
совершенный. Это идеальная прямая
имеет однородные координаты (1, 0, 0, 0)
однородная линия и существует на бесконечности благодаря последнему нулевому
(см. раздел C.2.1.1),
третий элемент элементу. Точка схождения – это проекция упомянутого вектора
которой равен
нулю, фактически >>> camera.project_point([1, 0, 0, 0], pose=T_camera)
представляющая array([[
1830],
собой бесконечно
[
512]])
удаленную линию.
См. также идеальт. е. точка с координатами (1803, 512) справа от плоскости видиную точку.

мого изображения.

Если в вызов
Метод plot_point может дополнительно возвращать коордиproject_pointis
наты
плоскости изображения
передается четырехмерный вектор,
то предполагается, >>> p = camera.plot_point(P, pose=T_camera)
1014,
что он содержит array([[ 887.8, 887.8, 887.8, 955.2, 955.2, 955.2,
координаты в ℙ3.
1014, 1014],

[ 364.3,
512, 659.7, 374.9,
512, 639.9]])

Это не совсем верно
для КМОП-сенсоров,
где транзисторы
уменьшают светочувствительную
область на коэффициент заполнения – долю площади
каждой фотоячейки,
чувствительную
к свету.

512, 649.1, 384.1,

В этом случае координаты плоскости изображения имеют
дробную составляющую. Это означает, что точка не проецируется на центр пикселя. Однако пиксель реагирует на свет одинаково по площади своей поверхности, поэтому дискретную
координату пикселя можно получить путем округления.
Трехмерный объект в виде куба можно определить и спроецировать аналогичным образом. Вершины куба со стороной
0.2 м и центром в точке (0, 0, 1) можно определить так:
>>> cube = mkcube(0.2, pose=SE3.Tz(1));
>>> cube.shape
(3, 8)

В результате получается мат­ри­ца 3×8 с одним столбцом на
вершину. Точки плоскости изображения могут быть построены,
как и раньше, с помощью команды
>>> camera.clf() # очистка виртуальной плоскости изображения
>>> camera.plot_point(cube);

В качестве альтернативы мы можем создать реберное представление куба
Ребра задаются >>> X, Y, Z = mkcube(0.2, pose=SE3.Tz(1), edge=True)
в том же формате >>> X.shape
трехмерной сетки, (2, 5)
который используется методом
определяющих
plot_wireframe из в виде трех двухмерных массивов,
Matplotlib. и отобразить его:

12 ребер,

758 Глава 13 · Формирование изображения
>>> camera.plot_wireframe(X, Y, Z)

как показано на рис. 13.8, вместе с косым видом, созданным
перемещением камеры:

v (пиксели)

v (пиксели)

>>> camera.clf() # clear the virtual image plane
>>> camera.plot_wireframe(X, Y, Z, pose=T_camera);

u (пиксели)
а
б
Рис. 13.8. Представление куба отрезками линий: а) вид спереди; б) вид сбоку

u (пиксели)

Последовательные вызовы plot_wireframe перерисовывают
точки или сегменты линий и обеспечивают простейшую анимацию. Короткий фрагмент кода
>>> X, Y, Z = mkcube(0.2, edge=True)
>>> for theta in np.linspace(0, 2 * pi, 100):
... T_cube = SE3.Tz(1.5)
...
* SE3.RPY(theta * np.array([1.1, 1.2, 1.3]))
... camera.clf()
... camera.plot_wireframe(X, Y, Z, objpose=T_cube)
... plt.pause(0.1)

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

13.1.6

Дисторсия объектива

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

v (пиксели)

v (пиксели)

Перспективная камера 759

u (пиксели)
u (пиксели)
а
б
Рис. 13.9. Дисторсия объектива: а) искаженное изображение, кривизна верхнего ряда квадратов заметно выражена;
б) неискаженное изображение. Это калибровочное изображение № 12, включенное в пакет Toolbox под именем
«calibration/left12.jpg»

ем (13.3). Геометрическая дисторсия (рис. 13.9а), как правило,
является наиболее проблематичным эффектом, с которым мы
сталкиваемся в роботизированных приложениях, и состоит из
двух компонентов: радиального и тангенциального.
Радиальная дисторсия приводит к смещению точек изображения по радиальным линиям от главной точки. Радиальная
ошибка хорошо аппроксимируется полиномом
δr = k1r 3 + k2r 5 + k3r 7 + ,

(13.13)

где r – расстояние точки изображения от главной точки. Радиальная дисторсия имеет два возможных проявления: бочкообразная или подушкообразная дисторсия. Бочкообразная
дисторсия возникает, когда увеличение снижается по мере
удаления от главной точки, что приводит к искривлению прямых линий у края изображения наружу. Подушкообразная дисторсия возникает, когда увеличение нарастает с расстоянием
от главной точки и вызывает изгиб прямых линий у края изоб­
ражения внутрь. Несоосность оптических элементов линзы
приводит к тангенциальной дисторсии, перпендикулярной
к радиусам, но она, как правило, менее значительна, чем радиальная.
Точка (ud, vd) на искаженном изображении связана с истинными координатами (u, v) соотношением
ud = u + δu,

vd = v + δv,

(13.14)

где смещение
(13.15)

760 Глава 13 · Формирование изображения

Этот вектор смещения можно построить для различных значений (u, v), как показано на рис. 13.12б.
На практике для описания радиальной дисторсии достаточно трех коэффициентов, а модель дисторсии определяется
параметрами (k1, k2, k3, p1, p2), которые рассматриваются как
дополнительные внутренние параметры. Дисторсию можно
смоделировать классом CentralCamera с использованием параметра distortion, например:
>>> camera = CentralCamera(f=0.015, rho=10e-6,
... imagesize=[1280, 1024], pp=[512, 512],
... distortion=[k1, k2, k3, p1, p2])

Отступление 13.7. Понимание геометрии зрения
Человечеству потребовалось много времени, чтобы
понять природу света, цвета и человеческого зрения.
У древних греков было две научные школы. Теория
эмиссии, поддержанная Евклидом и Птолемеем, утверждала, что зрение работает за счет того, что глаз испускает лучи света, которые взаимодействуют с миром
примерно так же, как осязание. Теория интромиссии,
поддерживаемая Аристотелем и его последователями,
предполагала, что физические формы входят в глаз от
объекта.
Евклид Александрийский (325–265), возможно,
правильно понял геометрические принципы формирования изображения, но его лучи исходили от глаза,
а не от объекта. Клавдий Птолемей (100–170 гг.) написал «Оптику» и обсуждал вопросы отражения, преломления и цвета, но сегодня остался лишь плохой
арабский перевод его работы. Арабский философ Хасан ибн аль-Хайтам (он же Альхазен, 965–1040) написал семитомный трактат «Китаб аль-Маназир» («Книга
оптики») около 1020 г. Он объединил математические лучи Евклида, медицинские
знания Галена и теорию интромиссии Аристотеля. Он писал, что «из каждой точки
каждого цветного тела, освещенного каким-либо светом, исходит свет и цвет по
каждой прямой линии, которую можно провести из этой точки». Он понимал явление преломления лучей, но полагал, что хрусталик глаза, а не сетчатка, получает
изображение – как и многие ранние мыслители, он боролся с идеей перевернутого
изображения на сетчатке. Латинский перевод его работы оказал большое влияние
на более поздних европейских ученых.
Только в 1604 г. геометрическая оптика и человеческое зрение объединились,
когда немецкий астроном и математик Иоганн Кеплер (1571–1630) опубликовал
Astronomiae Pars Optica («Оптическая часть астрономии»). Он был первым, кто понял, что изображения переворачиваются и проецируются хрусталиком глаза на
сетчатку, а изображение затем корректируется «в полостях мозга». (Изображение
из Astronomiae Pars Optica, Иоганн Кеплер, 1604 г.)

Калибровка камеры 761

13.2
Согласно стандарту
ANSI PH3.13-1958
«Маркировка фокусного расстояния
объективов».
При изменении фокуса линза смещается по оптической
оси. В некоторых
конструкциях изменение фокуса приводит к повороту
объектива, поэтому
если он не идеально
симметричен, это
приведет к смещению искажений
относительно плос­
кости изображения.
Изменение диафрагмы изменяет
части линзы, через
которые проходят
световые лучи,
и, следовательно,
искажение, которое
они несут.

Калибровка камеры

Модель проекции камеры (13.10) имеет ряд параметров, которые на практике неизвестны. Как правило, главная точка находится не в центре массива фотоячеек. Точность фокусного
расстояния объектива составляет всего 4 % и соответствует
заявленному, только если объектив сфокусирован на бесконечность. Также общеизвестно, что внутренние параметры изменяются, если объектив отсоединяется и снова прикрепляется
или регулируется по фокусу или апертуре. Единственными
внутренними параметрами, которые возможно получить априори, являются размеры фотоячейки ρw и ρh, известные из технической документации производителя сенсора. Внешние параметры и положение камеры оставляют открытым вопрос о том,
где именно находится центральная точка камеры.
Калибровка камеры – это процесс определения внутренних
и внешних параметров камеры по отношению к мировой системе координат. Методы калибровки основаны на наборах
мировых точек, относительные координаты которых известны
и чьи соответствующие координаты в плоскости изображения
также известны.
Далее мы рассмотрим два подхода. Первый прост для понимания, но не учитывает искажения объектива. Второй, основанный на шахматной доске, используется многими современными инструментами калибровки камер и позволяет оценить
искажения объектива.

13.2.1

Метод с использованием трехмерной мишени

Разложив (13.10) и подставив его в (13.9), положим p̃ = (u, v, 1)
и перепишем как
c0,0X + c0,1Y + c0,2Z + c0,3 - c2,0uX - c2,1uY - c2,2uZ - c2,3u = 0,
c1,0X + c1,1Y + c1,2Z + c1,3 - c2,0vX - c2,1vY - c2,2vZ - c2,3v = 0,

(13.16)

где ci,j – элементы неизвестной мат­ри­цы камеры С, (X, Y, Z) –
координаты точки на калибровочной мишени и (u, v) – соответствующие координаты плоскости изображения.
Для калибровки требуется трехмерная мишень, изображенная на рис. 13.10. Положение центра каждого маркера (Xi, Yi,
Zi), i = 0, ..., N - 1 относительно системы координат мишени {T}
должно быть известно, но сама {T} неизвестна. Во время калиб­
ровки выполняется захват изображения и определяются соответствующие координаты плоскости изображения (ui, vi). Как
отмечалось в разделе 13.1.4, мы можем положить c2,3 = 1 и про-

762 Глава 13 · Формирование изображения

суммировать два уравнения (13.16) для каждого из N маркеров,
чтобы сформировать матричное уравнение

(13.17)

которое можно решить для элементов мат­ри­цы камеры (c0,0 ...
c2,2). Этот подход известен как прямое линейное преобразование. Уравнение (13.17) имеет 11 неизвестных, и для его решения
требуется N ≥ 6. Часто используется более 6 точек, что приводит
к переопределенной системе уравнений, которая решается методом наименьших квадратов.

Рис. 13.10. Трехмерная калибровочная
мишень, представляющая систему
координат {T}. В качестве калибровочных
точек принимаются центроиды
окружностей. Обратите внимание, что
калибровочные точки расположены
в трех плоскостях (фото предоставлено
Фабьеном Шпиндлером)

Если точки компланарны, то левая мат­ри­ца уравнения (13.17)
становится мат­ри­цей неполного ранга. Вот почему калибровочная мишень должна быть трехмерной, обычно это массив точек или квадратов в двух или трех плоскостях, как показано на
рис. 13.10.
Проиллюстрируем калибровку на примере. Калибровочной
мишенью служит куб, маркеры являются его вершинами, а его
система координат {T} параллельна граням куба с началом
в центре куба. Координаты маркеров относительно {T}:
>>> P = mkcube(0.2);

Калибровка камеры 763
Отступление 13.8. Где находится центр камеры?
Составной объектив имеет множество основных точек, включая фокальные точки,
узловые точки, главные точки и плоскости, входной и выходной зрачки. Входной
зрачок (entrance pupil) – это точка на оптической оси системы составных линз, которая является ее центром перспективы, или точкой отсутствия параллакса. Мы
можем рассматривать эту точку как виртуальное точечное отверстие. Вращение
камеры и объектива вокруг этой точки не изменит относительную геометрию мишеней на разных расстояниях на перспективном изображении.
Вращение вокруг входного зрачка важно в панорамной фотографии, чтобы избежать ошибок параллакса в окончательной сшитой панораме. Ряд веб-страниц
посвящен обсуждению методов определения положения этой точки. Некоторые
сайты даже приводят таблицу положения входного зрачка для популярных объективов. В большинстве этих онлайн-публикаций авторы ошибочно называют эту
точку узловой точкой (nodal point), хотя приведенные методы на самом деле определяют входной зрачок.
В зависимости от конструкции объектива входной зрачок может находиться
позади, внутри или перед системой линз.

Теперь калибровочная мишень находится в некотором «неизвестном положении» CξT по отношению к камере, которое мы
принимаем следующим:
>>> T_unknown = SE3.Trans(0.1, 0.2, 1.5) * SE3.RPY(0.1, 0.2, 0.3);

Далее создаем перспективную камеру, параметры которой
мы попытаемся оценить:
>>> camera_unknown = CentralCamera(f=0.015, rho=10e-6,
imagesize=[1280, 1024], noise=0.05, seed=0)
Name: perspective [CentralCamera]
pixel size: 1e-05 x 1e-05
image size: 1280 x 1024
pose: t = 0, 0, 0; rpy/yxz = 0°, 0°, 0°
principal pt: [
640
512]
focal length: [
0.015
0.015]

Мы также указали, что к координатам (u, v) добавляется гауссовский шум с нулевым средним значением и σ = 0.05 для моделирования шума камеры и ошибок в алгоритмах компьютерного зрения. Координаты калибровочной мишени в плоскости
изображения в ее «неизвестном» положении:
>>> p = camera_unknown.project_point(P, objpose=T_unknown);

Теперь, используя только модель объекта P и наблюдаемые
признаки изображения p, вычислим мат­ри­цу камеры
>>> C, resid = CentralCamera.points2C(P, p)
>>> C
array([[ 852.8, -233.1,
633.6,
740],

764 Глава 13 · Формирование изображения
[
223,
990,
[ -0.131, 0.06549,

294.8,
0.6488,

712],
1]])

как функцию внутренних и внешних параметров, согласно
(13.10). Извлечение этих параметров из элементов мат­ри­цы
рассматривается в разделе 13.2.3.
Максимальная невязка, т. е. наихудшая ошибка между проекцией мировой точки с использованием мат­ри­цы камеры С
и фактическим положением плоскости изображения, состав­
ляет
>>> resid
0.03767

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

13.2.2

Калибровка с использованием шахматной доски

Многие популярные инструменты калибровки камер с учетом
дисторсии объектива используют плоскую шахматную мишень,
которую легко напечатать. Сначала делается несколько снимков мишени, обычно от десяти до двадцати, с разных расстояний и в разных ориентациях, как показано на рис. 13.11.
В состав пакета Toolbox включен набор калибровочных изображений из OpenCV. Их можно получить, как показано ниже:
>>> images = ImageCollection("calibration/*.jpg");
>>> len(images)
14

и затем передать в процедуру калибровки
>>> K, distortion, frames = CentralCamera.images2C(images,
...
gridshape=(7,6), squaresize=25e-3)

вместе с информацией о форме сетки угловых точек (рис. 13.11)
и размерах каждого квадрата.
Внутренняя мат­ри­ца
>>> K
array([[
[
[

534.1,
0,
0,

0,
534.1,
0,

341.5],
232.9],
1]])

Размеры квадратов на этой доске
неизвестны, но
предполагается,
что они равны
25×25 мм. Ошибки
в этом определении повлияют на
расчетное фокусное
расстояние.

v (пиксели)

v (пиксели)

Калибровка камеры 765

u (пиксели)

v (пиксели)

v (пиксели)

u (пиксели)

u (пиксели)

u (пиксели)

Рис. 13.11. Некоторые кадры из примера калибровки в OpenCV, показывающие автоматически обнаруженные
угловые точки, которые образуют сетку 6×7

показывает, что главная точка находится в координатах (341.5,
232.9), тогда как центр изображения находится в координатах
>>> images[0].centre
(320, 240)

Возвращаемое значение frames – это список объектов, содержащих результаты для каждого успешно обработанного входного изображения, в данном случае только для 11 из 14. Каждый
объект имеет свойство image, описывающее соответствующее
входное изображение, как показано на рис. 13.11, и свойство
pose, описывающее положение камеры относительно шахматной доски в виде объекта SE3. Мы можем визуализировать предполагаемое положение камеры для каждого входного изображения:
>>> for frame in frames:
... CentralCamera.plot(pose=frame.pose, scale=0.05)

как показано на рис. 13.12а.
Оценочная дисторсия объектива возвращается в виде одномерного массива distortion, который содержит параметры

766 Глава 13 · Формирование изображения

(13.15) в следующем порядке: (k1, k2, p1, p2, k3) – обратите внимание, что k3 следует не по порядку:
0.1077, 0.00131, -3.109e-05, 0.04348])

v (пиксели)

>>> distortion
array([ -0.293,

а

б

u (пиксели)

Рис. 13.12. Результаты калибровки на основе изображений шахматной доски: а) предполагаемое положение
камеры относительно мишени (показана синим) для каждого калибровочного изображения; б) предполагаемая
функция радиального искажения, показывающая смещение (-δu, -δv) от искаженной координаты к истинной
(масштаб преувеличен), контурные линии постоянной величины искажения и главную точку

13.2.2.1 Коррекция дисторсии объектива
Распространенная проблема – устранение эффекта дисторсии
объектива и создание изображения, видимого через идеальный
объектив. Для демонстрации способа ее решения воспользуемся моделью дисторсии из раздела 13.1.6 и деформации изображения из раздела 11.7.4.
Сначала извлечем предполагаемые параметры камеры из
предыдущего раздела:
>>> u0 = K[0, 2]; v0 = K[1, 2]; fpix_u = K[0, 0]; fpix_v = K[1,1];
>>> k1, k2, p1, p2, k3 = distortion;

Выберем неискаженное изображение того же размера, что
и искаженное, и получим мат­ри­цы координат
>>> U, V = images[12].meshgrid()

Далее, используя уравнение (13.7), преобразуем координаты
пикселей в координаты плоскости изображения :
>>> u = (U - u0) / fpix_u;
>>> v = (V - v0) / fpix_v;

Радиальное расстояние пикселей от главной точки равно
>>> r = np.sqrt(u**2 + v**2);

В метрах относительно главной
точки камеры.

Калибровка камеры 767

а ошибки координат сетчатки из-за искажения, определяемые
уравнением (13.15):
>>> delta_u = u
...
+
>>> delta_v = v
...
+

* (k1*r**2
p2*(r**2 +
* (k1*r**2
p1*(r**2 +

+ k2*r**4 + k3*r**6) + p1*u*v
2*u**2);
+ k2*r**4 + k3*r**6)
2*v**2) + p2*u*v;

Теперь получим искаженные координаты сетчатки
>>> ud = u + delta_u; vd = v + delta_v;

преобразуем их обратно в пиксельные координаты в искаженном изображении
>>> Ud = ud * fpix_u + u0;
>>> Vd = vd * fpix_v + v0;

и применим получившееся искажение:
>>> undistorted = images[12].warp(Ud, Vd)
Image: 640 x 480 (uint8)

Результат показан на рис. 13.9б. Изменение получилось незначительным, но его хорошо видно на краях и в углах изображения, где значение r максимально.
Поле векторов искажения-коррекции (-δu, -δv) можно отобразить в виде диаграммы векторного поля
>>> plt.clf() # очистить диаграмму
>>> plt.quiver(Ud[::50, ::50], Vd[::50, ::50], -delta_u[::50, ::50],
...
-delta_v[::50, ::50]);

с величиной искажения в виде контурных линий
>>> magnitude = np.sqrt(delta_u**2 + delta_v**2);
>>> plt.contour(U, V, magnitude);

Получившийся результат показан на рис. 13.12б.

13.2.3

Разложение калибровочной мат­ри­цы камеры

Элементы мат­ри­цы камеры являются функциями внутренних
и внешних параметров. Однако при наличии мат­ри­цы камеры
большинство значений параметров можно восстановить.
Нуль-пространство C – это начало мировой системы координат в кадре камеры. Используем данные из примера выше:
>>> o = linalg.null_space(C);
>>> o.T
array([[ 0.08068, -0.1712, -0.8136,

0.5497]])

768 Глава 13 · Формирование изображения

Это представление в однородных координатах можно преобразовать в декартову форму
>>> h2e(o).T
array([[ 0.1468, -0.3114,

-1.48]])

что близко к истинному значению
>>> T_unknown.inv().t
array([ 0.1464, -0.3105,

-1.477])

Чтобы восстановить ориентацию, а также внутренние параметры, можно разложить ранее найденную мат­ри­цу камеры
>>> est = CentralCamera.decomposeC(C)
Name: invC [CentralCamera]
pixel size: 1.0 x 0.999937
pose: t = 0.147, -0.311, -1.48;
rpy/yxz = -16.1°, -5.65°, -11.4°
principal pt: [ 642.1
512.9]
focal length: [
1504
1504]

и получить объект CentralCamera с его параметрами, имеющими
значения, которые приводят к той же самой мат­ри­це камеры.
Сразу отметим, что фокусное расстояние очень велико по сравнению с истинным фокусным расстоянием нашего объектива,
которое составляло 0.015 м, и что размеры пикселей очень велики. Из уравнения (13.10) мы видим, что фокусное расстояние
и размер пикселя всегда появляются вместе как коэффициенты
f/ρw и f/ρh. ► Метод decompose установил ρw = 1, но отношения найденных параметров
>>> est.f / est.rho[0]
array([
1504,
1504])

очень близки к соотношению для истинных параметров камеры:
>>> camera.f / camera.rho[0]
array([
1500,
1500])

Небольшая ошибка в найденных значениях параметров связана с зашумленными значениями координат плоскости изоб­
ражения, которые мы использовали в процессе калибровки.
Положение оцениваемой камеры рассматривается относительно калибровочной системы {T} и, следовательно, равно TξˆC.
Истинное положение цели относительно камеры равно CξT. Если
наша оценка точна, то CξT ⊕ TξˆC будет равна ∅. Ранее мы присвои­
ли переменной T_unknown значение CξT, и для нашего примера
разница между истинным и предполагаемым положениями камеры довольно мала:

Эти величины
измеряются в пикселях, поскольку ρ
измеряется
в м/пиксель. В литературе довольно
часто считается, что
ρ = 1, а фокусное
расстояние указывается в пикселях.
Если пиксели
не квадратные, то
для горизонтального и вертикального
направлений должны использоваться
разные фокусные
расстояния – fu и fv
соответственно.

Калибровка камеры 769
Отступление 13.9. Свойства мат­ри­цы камеры
Матрица камеры C ⊂ ℝ3×4 имеет определенную структуру и обладает некоторыми важными свойствами:

можно разбить C = (M | c3) на невырожденную мат­ри­цу M ⊂
ℝ3×3 и вектор c3 ∈ ℝ3, где c3 = −Mc, а c – начало координат в системе координат камеры. Мы можем восстановить его как
c = −M−1c3;
 нуль-пространство C равно c̃;
 пиксель с координатой p соответствует лучу в пространстве,
параллельному вектору M−1p̃;
 мат­ри­ца M = K С R0 является произведением внутренних характеристик камеры и инверсной ориентации камеры. Мы можем
––
–
выполнить RQ-разложение M = RQ, где R – верхнетреугольная
–
мат­ри­ца (которая есть K) и ортогональная мат­ри­ца Q (которая
есть C R0);
 нижний ряд C определяет главную плоскость, которая параллельна плоскости изображения и содержит начало системы
координат камеры;
 если строки M являются векторами mi, i ∈ [0, 2], то:
– m2T – вектор, нормальный к главной плоскости и параллельный оптической оси, а Mm2T – главная точка в однородной
форме;
– если камера имеет нулевой перекос, т. е. k0,1 = 0, то
(m0 × m2) · (m1 × m2) = 0;
– если камера имеет квадратные пиксели, т. е. ρu = ρv = ρ, то
||m0 × m2|| = ||m1 × m2|| = f/ρ.
 ее

>>> (T_unknown * est.pose).printline(orient="camera")
t = -0.000106, -0.000605, -0.00303;
rpy/yxz = -0.0159°, -0.0615°, 0.0872°

Положение камеры оценивается с погрешностью менее 5 мм
по смещению и в долях градуса по ориентации.
Этот сценарий можно показать, изобразив калибровочные
маркеры в виде маленьких красных сфер:
>>> plotvol3([-0.9, 0.9, -0.9, 0.9, -1.5, 0.3]);
>>> plot_sphere(0.03, P, color="r");
>>> SE3().plot(frame="T", color="b", length=0.3);

построить систему {T}, установленную в начало мировой системы координат, и наложить на диаграмму предполагаемое положение камеры:
>>> est.plot(scale=0.3, color="black", frame=True);

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

770 Глава 13 · Формирование изображения

Рис. 13.13. Калибровочные целевые точки и расчетное положение камеры относительно
системы {T}, которая, как предполагается, находится в начале мировой системы координат

13.2.4

 пределение положения откалиброванной
О
камеры

Задача вычисления положения состоит в том, чтобы определить
положение CξT системы координат тела объекта {B} относительно откалиброванной камеры. Геометрия мишени известна, т. е.
известно положение ряда точек (Xi, Yi, Zi), i = 0, ..., N - 1 на мишени относительно {B}. Также известны внутренние параметры
камеры. Выполняется захват изображения, и соответствующие
координаты плоскости изображения (ui, vi) определяются с использованием алгоритмов компьютерного зрения.
Вычисление положения с использованием (ui, vi), (Xi, Yi, Zi)
и внутренних параметров камеры называется задачей n-то­чеч­
ной перспективы (Perspective-n-Point), или PnP для краткости.
Это более простая задача, чем калибровка камеры и декомпозиция, поскольку для вычислений требуется меньше пара­метров.
Для иллюстрации создадим откалиброванную камеру с известными параметрами
>>> camera_calib = CentralCamera.Default(noise=0.1, seed=0);

и добавим к координатам проецируемой плоскости изображения гауссовский шум с нулевым средним значением и σ = 0.1.
Объект, положение которого мы хотим определить, представляет собой куб со стороной 0.2 м, а начало системы координат
{B} находится в центре куба и ее оси перпендикулярны граням
куба. «Моделью» объекта являются координаты его вершин относительно системы координат {B}
>>> P = mkcube(0.2);

то есть массив 3×8.

Широкоугольные камеры 771

Объект находится в произвольном и неизвестном положении
ξT относительно камеры:

C

>>> T_unknown = SE3.Trans(0.1, 0.2, 1.5) * SE3.RPY(0.1, 0.2, 0.3);

Координаты точек объекта в плоскости изображения при его
неизвестном положении равны:
>>> p = camera_calib.project_point(P, objpose=T_unknown);

где точки объекта преобразуются перед проекцией на плос­
кость изображения.
Теперь, используя только модель объекта P, наблюдаемые характеристики изображения p и откалиброванную камеру came­
ra_calib, вычислим относительное положение CξT объекта:
>>>
t =
>>>
t =

T_est = camera_calib.estpose(P, p).printline()
0.1, 0.2, 1.5; rpy/zyx = 5.77°, 11.5°, 17.2°
T_unknown.printline()
0.1, 0.2, 1.5; rpy/zyx = 5.73°, 11.5°, 17.2°

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

13.3

Широкоугольные камеры

Мы подробно обсудили перспективные изображения, поскольку они соответствуют модели наших собственных глаз
и почти всех камер, с которыми мы сталкиваемся. Однако перспективное изображение принуждает нас к фундаментально
ограниченному полю зрения. Уравнение тонкой линзы (13.1)
является сингулярным для точек с Z = f, что ограничивает поле
зрения не более чем одним полушарием – реальные линзы
достигают гораздо меньшего. По мере уменьшения фокусного
расстояния устранять радиальные искажения становится все
труднее, и в конце концов достигается предел, выше которого практически невозможно изготовить линзу. Единственный
выход из этой ситуации – отказаться от ограничения перспективного изображения. В разделе 13.3.1 будет рассмотрена

772 Глава 13 · Формирование изображения

геометрия формирования изображения с помощью широкоугольных объективов.
Альтернативой преломляющей оптике является использование отражающей поверхности для формирования изображения,
как показано на рис. 13.14. Ньютоновские телескопы основаны
на отражении от вогнутых зеркал, а не на преломлении линзами. Зеркала не дают цветной окантовки на изображении, и их
легче масштабировать до больших размеров, чем линзы. Природа тоже использует отражающую оптику: у рыб-призраков
и некоторых морских гребешков (рис. 1.8) глаза основаны на
отражателях, образованных из кристаллов гуанина. В разделе 13.3.2 описана геометрия формирования изображения при
комбинации линз и зеркал.

Рис. 13.14. Формирование изображений путем отражения от искривленной поверхности
(Cloud Gate, Чикаго, Аниш Капур, 2006). Обратите внимание, что прямые линии стали кривыми

13.3.1

Камера с объективом «рыбий глаз»

Изображение, полученное через объектив типа «рыбий глаз»,
показано на рис. 13.15. Как видите, прямые линии в мировой
системе координат искривлены, а поле зрения деформировано
и преобразовано в окружность на плоскости изображения. Формирование изображения моделируется с использованием обозначений, показанных на рис. 13.16, где камера расположена
в начале мировой системы координат O, а ее оптическая ось –
это ось z. Мировая точка P представлена в сферических коорди-

Широкоугольные камеры 773

Рис. 13.15. Изображение, полученное с помощью объектива типа «рыбий глаз». Обратите
внимание, что прямые линии проецируются как кривые. Также обратите внимание, что поле
зрения отображается в круговую область на плоскости изображения

опт

иче

ска

яо

сь

начало
системы
координат
камеры

ско

пло

ния

аже

обр

из
сть

Рис. 13.16. Формирование изображения в камере с объективом «рыбий глаз».
Мировая точка P представлена в сферических координатах (R, θ, ϕ) относительно начала
координат камеры

774 Глава 13 · Формирование изображения

натах (R, θ, ϕ), где θ – угол наружу от оптической оси, а ϕ – угол
поворота вокруг оптической оси. Мы можем написать:

На плоскости изображения камеры изобразим проекцию p
в полярных координатах (r, ϕ) относительно главной точки, где
r = r(θ). Декартовы координаты плоскости изображения:
u = r(θ)cos ϕ,

v = r(θ)sin ϕ,

а точный характер функции r(θ) зависит от разновидности объектива «рыбий глаз». Некоторые распространенные проекционные модели перечислены в табл. 13.1, и все они имеют параметр масштабирования k.
Таблица 13.1. Проекционные модели с объективом «рыбий глаз»
Сопоставление
Равноугольное (equiangular)
Стереографическое (stereographic)
Равнотельное (equisolid)
Полиномиальное (polinimial)

Уравнение
r = kθ
r = k tan(θ/2)
r = k sin(θ/2)
r = k1θ + k2θ2 + …

Используя Toolbox, мы можем создать модель камеры типа
«рыбий глаз»
>>> camera = FishEyeCamera(
...
projection="equiangular",
...
rho=10e-6,
...
imagesize=[1280, 1024]
...
);

которая возвращает экземпляр объекта FishEyeCamera, являющийся подклассом объекта CameraBase и полиморфный классу
CentralCamera, рассмотренному ранее. Если значение k не указано, как в этом примере, то оно вычисляется так, что полусферическое поле зрения проецируется в максимальный круг на
плоскости изображения. Как и в случае с перспективными камерами, такие параметры, как главная точка и размеры в пикселях, как правило, неизвестны и должны оцениваться с по­
мощью процедуры калибровки.
Создадим скелетную модель куба со стороной 0.2 м
>>> X, Y, Z = mkcube(side=1, centre=[1, 1, 0.8], edge=True);

и спроецируем ее на плоскость изображения камеры «рыбий
глаз»:
>>> camera.plot_wireframe(X, Y, Z, color="k");

Широкоугольные камеры 775

Результат показан на рис. 13.17. Как видите, прямые линии
мира уже не являются прямыми линиями на изображении.

v (пиксели)

рыбий глаз

Рис. 13.17. Куб,
спроецированный с помощью
класса FishEyeCamera.
Прямые ребра куба являются
отрезками кривых на
плоскости изображения

u (пиксели)

Существуют широкоугольные объективы с углом обзора
180° и даже 190°, однако они имеют некоторые практические
недостатки. Во-первых, пространственное разрешение ниже,
поскольку пиксели камеры распределены по более широкому
полю зрения. Мы также видим на рис. 13.15, что поле зрения
представляет собой круглую область, а это означает, что почти 25 % прямоугольной плоскости изображения теряется. Вовторых, снимки, сделанные на открытом воздухе, с большей
вероятностью будут захватывать яркое небо, поэтому камера
автоматически уменьшит экспозицию, и, как следствие, некоторые части сцены, не относящиеся к небу, будут сильно недоэкспонированы (затемнены).

13.3.2
От греческих слов,
обозначающих
кривое зеркало
(катоптрика) и линзу
(диоптрика).

Катадиоптрическая камера

Катадиоптрическая система получения изображений содержит
как отражающие, так и преломляющие элементы – зеркало
и линзу, как показано на рис. 13.18a. Пример катадиоптрического изображения см. на рис. 13.18б.
Геометрическая схема на рис. 13.19 довольно сложна. Луч проходит из точки P к фокусу зеркала в точке O, которая является началом системы координат камеры. Луч имеет угол возвышения

776 Глава 13 · Формирование изображения

вверх от оптической оси и пересекает зеркало в точке М. Отраженный луч образует угол ψ по отношению к оптической оси,
который является функцией угла падающего луча, т. е. ψ(θ). Соотношение между θ и ψ определяется касательной к зеркалу
в точке M и зависит от формы зеркала. Для катадиоптрической
съемки используются зеркала различных форм – сферические,
параболические, эллиптические и гиперболические. В общем
случае функция ψ(θ) нелинейна, но интересным классом зеркал
являются равноугольные зеркала, для которых
θ = αψ.

а

б

Рис. 13.18. Катадиоптрическая камера: а) система катадиоптрического изображения,
включающая обычную перспективную камеру, которая смотрит в зеркало вверх;
б) катадиоптрическое изображение. Обратите внимание на темное пятно в центре, которое
обусловлено опорой, удерживающей зеркало над объективом. Пол – в центре изображения,
а потолок – на краю (фотографии Майкла Милфорда)

Отраженный луч попадает в объектив камеры под углом ψ
к оптической оси, и, исходя из геометрии объектива, можно записать
r = λ tan ψ,
что является расстоянием от главной точки. Полярные координаты точки плоскости изображения равны p = (r, ϕ), а соответствующие декартовы координаты
u = r cos ϕ,

v = r sin ϕ,

где ϕ – азимутальный угол в горизонтальной плоскости

Широкоугольные камеры 777

оптическая ось

зеркало

начало системы
координат камеры

объектив

плоскость изображения

Рис. 13.19. Формирование катадиоптрического изображения. Луч из точки P под
углом места θ и азимутом ϕ в сторону O отражается от поверхности зеркала в точке M
и проецируется линзой на плоскость изображения в точке p

На рис. 13.19 мы предположили, что все лучи проходят через одну фокальную точку, или точку наблюдения, – в данном
случае O. Эта схема называется центральной системой, и полученное катадиоптрическое изображение может быть правильно преобразовано в перспективное. Равноугольное зеркало не
удовлетворяет этому ограничению и, следовательно, представляет собой нецентральную систему – фокусная точка меняется
в зависимости от угла падающего луча и лежит вдоль короткой
траектории внутри зеркала, известной как каустическая линия
(caustic). Конические, сферические и равноугольные зеркала
нецентральны. На практике изменение точки зрения очень
мало по сравнению с мировым масштабом, и многие такие зеркала хорошо аппроксимируются центральной моделью.
Toolbox предоставляет модель катадиоптрических камер.
Например, мы можем создать равноугольную катадиоптрическую камеру:
>>> camera = CatadioptricCamera(
...
projection="equiangular",
...
rho=10e-6,
...
imagesize=[1280, 1024],
...
maxangle=pi/4
...
);

778 Глава 13 · Формирование изображения
Отступление 13.10. Зеркальное отражение
Зеркальное отражение происходит, как следует
из названия, при падении света на зеркальную
поверхность. Входящие лучи отражаются так, что
угол падения равен углу отражения, или θr = θi.
Этим зеркальное отражение отличается от диффузного, или ламбертовского, отражения, когда
падающие лучи рассеиваются в диапазоне углов.
Speculum в переводе с латыни означает зеркало, а классический металл для изготовления
зеркал (2/3 меди, 1/3 олова) представляет собой
сплав, который хорошо полируется. Его использовали Ньютон и Гершель для изогнутых зеркал
в своих телескопах-рефлекторах (48-дюймовое
металлическое зеркало из 40-футового телескопа
Гершеля, изготовленное в 1789 г., сейчас находится в Британском музее науки, фото Майка Пила
((mikepeel.net), лицензия CC-BY-SA).

Этот код возвращает экземпляр объекта CatadioptricCamera,
который является подклассом объекта CameraBase и полиморфен
классу CentralCamera, рассмотренному ранее. Параметр maxangle
задает максимальный угол места θ, из которого определяются
параметры α и f, так что наибольший угол места соответствует окружности, которая максимально совпадает с плоскостью
изображения. Параметры можно задавать по отдельности с использованием параметров alpha и focal. Также поддерживаются модели параболической и сферической проекции, и каждый
тип камеры имеет различные параметры, как описано в онлайн-документации.
Создадим модель куба на основе ребер
>>> X, Y, Z = mkcube(1, centre=[1, 1, 0.8], edge=True)

которую спроецируем на плоскость изображения:
>>> camera.plot_wireframe(X, Y, Z, color="k");

Результат показан на рис. 13.20.
Преимуществом катадиоптрических камер является обзор
на 360° по азимуту, но они также имеют некоторые практиче-

Широкоугольные камеры 779

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

v (пиксели)

панорамная (катадиоптрическая) камера

u (пиксели)
Рис. 13.20. Куб, спроецированный с помощью равноугольной катадиоптрической камеры

13.3.3

Сферическая камера

Объективы типа «рыбий глаз» и катадиоптрические системы,
которые только что обсуждались, направляют световые лучи из
большого поля зрения на плоскость изображения. В конечном
счете двухмерная плоскость изображения является ограничивающим фактором, и лучше вместо нее рассматривать сферу
изображения, как показано на рис. 13.21.
Мировая точка P проецируется лучом в начало единичной
сферы. Проекция – это точка p, в которой луч пересекает поверхность сферы. Если мы примем p = (x, y, z), то
,

(13.18)

и R = X 2 + Y 2 + Z 2 – радиальное расстояние до мировой точки.
Поверхность сферы описывается уравнением x 2 + y 2 + z 2 = 1,
поэтому одна из трех декартовых координат избыточна. Ми-

780 Глава 13 · Формирование изображения

нимальное двухпараметрическое представление точки на поверхности сферы (ϕ, θ) включает в себя угол дополнительной
широты (colatitude), измеренный вниз от северного полюса
θ = sin-1r,

θ ∈ [0, π],

(13.19)

где r = x 2 + y 2 азимутальный угол (или долгота)
(13.20)

линия долготы

линия
широты

э кв

ат

ор

Рис. 13.21. Формирование сферического изображения. Мировая точка P отображается
в p на поверхности единичной сферы и представлена углами дополнительной широты θ
и долготы ϕ

Полярные и декартовы координаты точки p связаны отношением
x = sin θ cos ϕ,

y = sin θ sin ϕ,

z = cos θ.

(13.21)

С помощью Toolbox мы можем создать модель сферической
камеры
>>> camera = SphericalCamera()
Name: spherical [SphericalCamera]
pixel size: 1.0 x 1.0
pose: t = 0, 0, 0; rpy/yxz = 0°, 0°, 0°

которая возвращает экземпляр объекта SphericalCamera, являющийся подклассом объекта CameraBase и полиморфный классу
CentralCamera, обсуждавшемуся ранее.

Унифицированная модель формирования изображений 781

Как и ранее, можно создать скелетную модель куба
>>> X, Y, Z = mkcube(1, centre=[2, 3, 1], edge=True)

и спроецировать его на сферу
>>> camera.plot_wireframe(X, Y, Z, color="k");

Дополнительная широта θ (рад)

Результат показан на рис. 13.22. Для облегчения визуализации поверхность сферического изображения развернута в прямоугольник – линии долготы и широты отображаются как вертикальные и горизонтальные линии соответственно. Верхний
и нижний края соответствуют северному и южному полюсам
соответственно.
сферическая

Долгота φ (рад)
Рис. 13.22. Куб, спроецированный сферической камерой. Плоскость сферического
изображения представлена в декартовых координатах

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

13.4

Унифицированная модель формирования
изображений

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

782 Глава 13 · Формирование изображения

было бы снято камерой другого типа. Например, учитывая проекцию объектива «рыбий глаз», мы создадим соответствующую
проекцию для сферической или перспективной камеры. Унифицированная модель формирования изображений обеспечивает мощную основу для рассмотрения очень разных типов
камер, таких как стандартная перспектива, катадиоптрические
и многие разновидности объективов типа «рыбий глаз».
Унифицированная модель формирования изображений
представляет собой двухэтапный процесс, схематически показанный на рис. 13.23. Первым шагом является сферическая проекция мировой точки P на поверхность единичной сферы p¢, как
обсуждалось в предыдущем разделе и описывается уравнениями (13.18) и (13.19). Точка обзора O – это центр сферы, который
находится на расстоянии m от плоскости изображения по ее
нормали к оси z. Единая точка обзора подразумевает наличие
центральной камеры.

эк

ор

p

в

ат

главная точка

плоскость изображения

Рис. 13.23. Унифицированная модель формирования изображений. Точка0P проецируется
в точку p¢ на поверхности единичной сферы с помощью точки наблюдения O, а затем
перепроецируется в точку p на плоскости изображения с помощью точки наблюдения F

На втором этапе точка p¢ перепроецируется на плоскость
изображения p с использованием точки обзора F, которая находится на расстоянии ℓ по оси z выше точки O. Полярные координаты точки плоскости изображения равны p = (r, ϕ), где
(13.22)

Унифицированная модель формирования изображений 783

Унифицированная модель формирования изображения имеет только два параметра: m и ℓ, и они зависят от типа камеры,
как указано в табл. 13.2. У перспективной камеры точка обзора F совпадает с O, а геометрия становится такой же, как у цент­
ральной перспективной модели, показанной на рис. 13.3.
Таблица 13.2. Унифицированные параметры модели изображения ℓ и m
в зависимости от типа камеры. ε – конический эксцентриситет, а 4p –
фокальный параметр
Система
Песпективная
Стереографическая
«Рыбий глаз»
Катадиоптрическая (эллиптическая, 0 < ε < 1)
Катадиоптрическая (параболическая, ε = 1)
Катадиоптрическая (гиперболическая, ε > 1)

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

ℓ
0
1
>1
2ε
1 + ε2
1
2ε
1 + ε2

m
f
f
f
2ε(2p - 1)
1 + ε2
2p - 1
2ε(2p - 1)
1 + ε2

У катадиоптрических камер с коническими зеркалами фокальная точка F лежит между центром сферы и северным полюсом, т. е. 0 < ℓ < 1. Эта проекционная модель несколько проще гео­метрии катадиоптрической камеры, показанной на
рис. 13.19. Изображение параметризуется коническим эксцент­
риситетом ε и фокальным параметром 4p.
Проекция с F на северном полюсе называется стереографической проекцией и используется во многих областях для прое­
цирования поверхности сферы на плоскость. Многие линзы
типа «рыбий глаз» очень хорошо аппроксимируются F над северным полюсом.

13.4.1

С опоставление широкоугольных изображений
со сферой

Мы можем использовать унифицированную модель формирования изображений в обратном порядке. Рассмотрим изображение, полученное широкоугольной камерой, например с объективом «рыбий глаз», показанное на рис. 13.24а. Если мы знаем
местоположение F, то можем спроецировать каждую точку изображения на сферу, чтобы создать сферическое изображение,
даже в отсутствие сферической камеры.
Чтобы получить это обратное отображение, нужно знать некоторые параметры камеры, снявшей изображение. Общей
чертой изображений, снятых с помощью объектива «рыбий
глаз» или катадиоптрической камеры, является то, что внешняя
граница изображения представляет собой круг. Этот круг мож-

784 Глава 13 · Формирование изображения

но найти и довольно точно определить его центр – это главная
точка. На рис. 13.24a показан вариант процедуры калибровки
камеры, описанной в разделе 13.2.2, в котором используются
соответствующие точки мира и плоскости изображения из планарной калибровочной мишени. Эта конкретная камера имеет
поле зрения 190°, и ее калибровочные параметры оцениваются
следующим образом:

а

θ (рад)

v (пиксели)

>>> u0 = 528.1214; v0 = 384.0784; l = 2.7899; m = 996.4617;

u (пиксели)

б

φ (рад)

Рис. 13.24. Изображение плоской калибровочной мишени «рыбий глаз»: а) изображение типа «рыбий глаз»
(любезно предоставлено Питером Хансеном); б) изображение преобразовано в координаты (ϕ, θ)

Проиллюстрируем это с помощью изображения, показанного
на рис. 13.24а:
>>> fisheye = Image.Read("fisheye_target.png", dtype="float",
...
mono=True)
Image: 1024 x 768 (float32) [.../images/fisheye_target.png]

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

n = 500;
phi_range = np.linspace(-pi, pi, n); # долгота
theta_range = np.linspace(0, pi, n); # дополнительная широта
Phi, Theta = np.meshgrid(phi_range, theta_range);

охватывает всю сферу с долготой от -π до +π радиан и дополнительной широтой от 0 до π радиан за 500 шагов в каждом направлении.
Для преобразования изображения «рыбий глаз» в сферу необходимо преобразовать координаты точки в выходном сферическом изображении в координаты точки во входном изображении. Эта функция является вторым шагом унифицированной
модели построения изображений (13.22), который мы реализуем как

Унифицированная модель формирования изображений 785
>>> r = (l + m) * np.sin(Theta) / (l - np.cos(Theta));

из которых получаются соответствующие декартовы координаты во входном изображении
>>> U = r * np.cos(Phi) + u0;
>>> V = r * np.sin(Phi) + v0;

Используя деформацию изображения из раздела 11.7.4, преобразуем изображение «рыбий глаз» в сферическое:
>>> spherical = fisheye.warp(U, V, domain=(phi_range, theta_range))
Image: 500 x 500 (float32)

где третий аргумент – это область определения сферического
изображения, содержащая вектор, который охватывает горизонтальную и вертикальную оси. Результат
>>> spherical.disp(axes=(r"$\phi$", r"$\theta$"));

показан на рис. 13.24б. Изображение кажется отраженным относительно экватора. Дело в том, что отображение точки на
плоскости изображения в сферу имеет двойное значение – поскольку F находится над северным полюсом, луч пересекает
сферу дважды. Верхний и нижний ряды этого изображения соответствуют главной точке, а темная полоса над экватором соответствует круглому внешнему краю входного изображения.
Изображение сильно искажено, но эта система координат
очень удобна для наложения текстуры на сферу
>>> ax = plotvol3();
>>> plot_sphere(radius=1, ax=ax, filled=True, resolution=n,
... facecolors=spherical.colorize().A, cstride=1, rstride=1);

Результат показан на рис. 13.25. Используя панель инструментов Matplotlib, можно вращать сферу и смотреть на изображение с разных точек зрения.

Рис. 13.25. Изображение «рыбий
глаз» отображено в единичную
сферу. Хорошо видно плоскую
клетчатую мишень, лежащую на
столе, потолочный светильник,
дверь и доску для заметок

786 Глава 13 · Формирование изображения

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

13.4.2

 реобразование сферического изображения
П
в перспективное

Допустим, у нас есть сферическое изображение, и нужно реконструировать перспективный вид в определенном направлении.
Мы можем рассматривать эту операцию как взгляд изнутри
сферы на небольшой участок поверхности, который близок
к плоскому и приближается к виду камеры в перспективе. Это
второй шаг унифицированной модели визуализации, где F находится в центре сферы, и в этом случае рис. 13.23 становится
похожим на рис. 13.3. Оптической осью перспективной камеры
является отрицательная ось z сферы.
В этом примере мы будем использовать сферическое изображение, созданное в предыдущем разделе. Нам нужно создать
перспективное изображение размером 1000×1000 пикселей
и с полем зрения 45°. Поле зрения можно записать через ширину изображения W и унифицированный параметр изображения m как

Для угла обзора 45° требуется
>>> W = 1000;
>>> m = W / 2 / np.tan(np.deg2rad(45 / 2))
1207

а для перспективной проекции необходимо
>>> l = 0;

Нам также нужно, чтобы главная точка находилась в центре
изображения:
>>> u0, v0 = W / 2, W / 2;

Область выходного изображения будет следующей:
>>> U, V = Image.meshgrid(width=W, height=W);

Полярная координата (r, ϕ) каждой точки выходного изображения равна

Унифицированная модель формирования изображений 787
>>> U0 = U - u0; V0 = V - v0;
>>> r = np.sqrt(U0**2 + V0**2);
>>> phi = np.arctan2(V0, U0);

и соответствующие сферические координаты (ϕ, θ) равны
>>> Phi = phi;
>>> Theta = pi - np.arctan(r / m);

Теперь перейдем от сферических координат к плоскости
перс­пективного изображения:
>>> spherical.warp(Phi, Theta).disp();

Результат преобразования показан на рис. 13.26а. Это вид
перспективной камеры в центре сферы, смотрящей вниз через
южный полюс. Мы видим, что линии на клетчатой калибровочной мишени теперь прямые, как и следовало ожидать от перспективного изображения.
Конечно, мы не ограничиваемся только взглядом вдоль отрицательной оси z сферы. На рис. 13.25 можно видеть некоторые другие элементы комнаты, такие как дверь, белая доска
для заметок и несколько потолочных светильников. Мы можем
направить нашу виртуальную перспективную камеру в их направлении, сначала повернув сферическое изображение:

v (пиксели)

v (пиксели)

Из одного широко­
угольного изоб­
ражения можно
создать перспективный вид в любом
направлении, не
используя механи- >>> spherical2 = spherical.rotate_spherical(SO3.Ry(0.9)
ческий механизм
* SO3.Rz(-1.5))
поворота/накло- ...
на, – это просто вы- Image: 500 x 500 (float32)
числение. На самом
деле несколько так что теперь отрицательная ось z указывает на дальнюю степользователей мо- ну. Повторяя процесс преобразования, мы получаем результат,
гут одновременно
смотреть в разных показанный на рис. 13.26б, в котором можно ясно видеть дверь
направлениях. и доску.

u (пиксели)
u (пиксели)
а
б
Рис. 13.26. Перспективная проекция сферического изображения рис. 13.25 с полем зрения
45°: а) обратите внимание, что линии на клетчатой мишени теперь прямые: б) перспективный
вид на дверь и доску для заметок

788 Глава 13 · Формирование изображения

Исходное широкоугольное изображение содержит много
деталей, хотя их трудно разглядеть из-за искажений. После сопоставления изображения со сферой мы можем создать виртуальный перспективный вид камеры (где прямые линии мира
действительно прямые) вдоль любой линии обзора. Это возможно только в том случае, если исходное изображение было
снято центральной камерой с одной точкой обзора. Теоретически мы не можем создать перспективное изображение из нецентрального широкоугольного изображения, но на практике,
если каустика мала, ошибки параллакса, вносимые в перспективное изображение, будут незначительными.

13.5

Новые типы камер

13.5.1

Многокамерные массивы

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

а

б

в
Рис. 13.27. Всенаправленный массив камер: а) пять перспективных камер обеспечивают панораму 360°
с полем зрения по вертикали 72° (камера Occam Vision Group): б) массив панорамных камер использует шесть
перспективных камер, чтобы обеспечить 90 % сферического поля зрения; в) бесшовное панорамное изображение
(3760×480 пикселей), выводимое камерой (a) (фотографии (a) и (в) Эдварда Пепперелла; изображение (б)
предоставлено Point Grey Research)

Новые типы камер 789

зование этого массива для съемки панорамы на 360° показано
на рис. 13.27в. Массив на рис. 13.27б использует шесть камер,
чтобы получить почти сферическое поле зрения.
Эти массивы камер не являются центральными камерами,
поскольку лучи света сходятся в фокусных точках отдельных
камер, а не в центре сборки камер. Это может создать проблемы при отображении объектов на небольшом расстоянии, но
при обычном использовании расстояние между фокусными
точками камеры (каустика) мало по сравнению с расстояниями
в сцене. Однако разные точки зрения камер дают реальное преимущество, когда дело доходит до съемки по технологии светового поля.

13.5.2

Мы могли бы
добавить дополнительные измерения
для представления
поляризации света.

Слово «пленоптический» происходит
от латинского слова
«plenus», означающего «полный».
Прямые в трехмерном пространстве
имеют четыре
параметра, см.
линии Плюккера
в разделе C.1.2.2.

Камеры светового поля

Как было сказано в начале этой главы, традиционная перспективная камера – аналоговая или цифровая – фиксирует представление сцены, используя два измерения пленки или сенсора.
Полученное изображение можно рассматривать как двухмерную функцию ℒ(X, Y ), которая описывает свет, излучаемый
трехмерной сценой. Функция является скалярной ℒ(X, Y ) ∈ ℝ
для монохромного изображения и векторной ℒ(X, Y ) ∈ ℝ3 для
трехцветного. ◄
Камера-обскура на рис. 13.1 пропускает через апертуру лишь
очень небольшое количество световых лучей, однако пространство заполнено бесчисленными световыми лучами, дающими
более богатое и полное описание мира. Объектив направляет
несколько лучей из точки мира к фотоячейке, увеличивая яркость, но информация о направлении этих лучей теряется, поскольку фотоячейка реагирует на все световые лучи одинаково,
независимо от их угла к поверхности.
Это подробное геометрическое распределение света называется пленоптической функцией.
Яркость на самом деле
является функцией положения и направления в трехмерном
пространстве, например ℒ(X, Y, Z, θ, ϕ).
Линии в трехмерном пространстве имеют только четыре параметра, поэтому
пленоптическую функцию можно записать как ℒ(s, t, u, v) с использованием двухплоскостной параметризации, показанной
на рис. 13.28a, которую можно представить как массив камер,
каждая из которых имеет двумерную плоскость изображения.
Изображение обычной камеры дает лишь двухмерный срез
полной пленоптической функции.
Хотя идеи, лежащие в основе светового поля, известны уже
несколько десятилетий, технология съемки световых полей стала широко доступной только в последние годы. Первые камеры
светового поля (пленоптические камеры) представляли собой

790 Глава 13 · Формирование изображения

а

б

Рис. 13.28. а) Луч света Φ проходит через плоскость изображения в точке (u, v) и через центр камеры в точке (s, t ).
Это похоже на модель центральной проекции, показанную на рис. 13.3. Любой луч можно описать двумя точками,
в данном случае (u, v) и (s, t ); б) путь световых лучей от объекта через линзу главного объектива и мат­ри­цу линз
к мат­ри­це пикселей (изображения предоставлены Дональдом Дансеро)

массивы обычных камер, расположенных в плоскости, как показано на рис. 13.29, или на сфере, окружающей сцену, но они
были большими, сложными и дорогими в изготовлении. Совсем
недавно на рынке появились недорогие и компактные камеры
светового поля, основанные на массивах микролинз. Главным
преимуществом потребительских камер светового поля обычно считают возможность перефокусировать изображение после
съемки, но у изображения, полученного по технологии светового поля, есть много других достоинств, включая синтез новых
видов, 3D-реконструкцию, получение изображения при слабом
освещении и способность видеть сквозь частицы, затемняющие предметы.
Массив микролинз представляет собой регулярную сетку
крошечных линз, обычно состоящую из сотен тысяч линз, размещенную на расстоянии в доли миллиметра над поверхностью мат­ри­цы фотоячеек камеры. Основная линза объектива
фокусирует изображение на поверхности массива микролинз,
как показано на рис. 13.28б. Микролинза направляет падающий
свет на один из небольших участков пикселей, допустим 8×8,
в соответствии с его направлением. Результирующее изображение фиксирует информацию как о происхождении луча (линза),
так и о его направлении (конкретный пиксель под линзой). Напротив, в стандартной перспективной камере все лучи, независимо от направления, вносят свой вклад в значение пикселя.
Пиксели камеры светового поля иногда называют ракселями
(raxel), а разрешение этих камер обычно выражается в мегалучах (megaray).
Необработанное изображение с массива датчиков выглядит,
как показано на рис. 13.30а, но его можно декодировать в четырехмерное световое поле, как представлено на рис. 13.30б,
и использовать для отображения новых видов.

Новые типы камер 791

Рис. 13.29. Массив камер 8×12, описанный в Wilburn et al. (2005 г.) (фото предоставлено
Марком Левоем, Стэнфордский университет)

а

б

Рис. 13.30. а) Крупный план изображения, сформированного на мат­ри­це сенсора с помощью
мат­ри­цы линз. Линзы имеют круглую форму, но расположены по шестиугольной сетке
и проецируют шестиугольные изображения благодаря шестиугольной диафрагме, которая
предотвращает перекрытие изображений, полученных с помощью линз; б) массив
изображений, полученных из светового поля для разных точек зрения камеры (изображения
предоставлены Дональдом Дансеро)

792 Глава 13 · Формирование изображения

13.6

Применения

13.6.1

Координатные метки

На рис. 13.31 показана сцена с рядом искусственных меток, которые часто называют координатными метками. В робототехнике широко используются две разновидности координатных
меток: метки ArUco, изначально разработанные для приложений дополненной реальности, и AprilTags.
Метки имеют высококонтрастные узоры, что позволяет легко
обнаружить их в сцене, а OpenCV имеет функции для определения их положения относительно камеры. Расположение квад­
ратов на изображении метки задает ее ориентацию, а также
кодирует некоторые данные, которые можно использовать для
уникальной идентификации метки. Если известны внутренние
характеристики камеры и размеры метки, то можно оценить ее
положение относительно камеры.
Обе разновидности меток объединены в семейства. Например, семейство ArUco 4×4_ имеет сетку 4×4 из черных или белых
квадратов. Метки из этого семейства содержат в общей сложности 16 бит данных, но для надежного обнаружения метки и определения ее ориентации используется только подмножество
из 216 возможных кодов. Семейство 4×4_50 использует лишь
50 уникальных кодов, тогда как семейство 4×4_250 – 250 кодов.
Сцена с метками ArUco включена в состав пакета Toolbox
>>> scene = Image.Read("lab-scene.png", rgb=False)
Image: 1920 x 1280 (uint8), B:G:R [.../images/lab-scene.png]
>>> scene.disp();

и показана на рис. 13.31. Это та же сцена, что изображена на
рис. 2.6, с той лишь разницей, что здесь присутствуют метки
ArUco.
Внутренние параметры камеры были определены с помощью
небольшого детективного расследования ► и позволяют создать модель откалиброванной камеры
>>> camera = CentralCamera(f=3045, imagesize=scene.shape,
...
pp=(2016, 1512), rho=1.4e-6);

соответствующую камере, с использованием которой был сделан снимок.
Затем мы используем метод fidicual изображения
>>> markers = scene.fiducial(dict="4x4_50", K=camera.K, side=67e-3);

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

Снято на камеру
Sony ILCE-7RM3,
фокусное расстояние взято из
метаданных EXIF
в файле изображения, а данные
о сенсоре – из технического описания
камеры.

v (пиксели)

Применения 793

u (пиксели)
Рис. 13.31. Лабораторная сцена с несколькими метками Aruco 4×4_50. Это та же сцена, что
и на рис. 2.6, но с метками

>>> markers[2]
id=5: t = 0.182, 0.0068, 1.14e+06; rpy/zyx = 145°, 29.5°, -3.62°

представляющих идентификаторы меток и их положение относительно камеры в виде атрибутов id и pose соответственно.
Координаты углов метки на изображении
>>> markers[2].corners
array([[
947,
1048,
[
922,
921,

1031,
1025,

927],
1024]], dtype=float32)

Базовая функция соответствуют метке внизу в центре изображения, закрепленOpenCV, которая ной в лапах плюшевого мишки. Объект Fiducial также имеет мерисует оси на изображении, требует, тод для отображения осей координат ◄
чтобы изображение
имело порядок >>> for marker in markers:
marker.draw(scene, length=0.10, thick=20)
цветов BGR. В дан- ...
ном примере это
Результат показан на рис. 13.31.
достигается с помощью параметра
rgb=False в вызове
метода Image.Read.

13.6.2

Планарная гомография

Рассмотрим систему координат, прикрепленную к плоской поверхности так, что ось z перпендикулярна плоскости, а оси x и y
лежат в ней. Понаблюдаем за меткой с помощью откалиброванной камеры и вспомним соотношение (13.10)

794 Глава 13 · Формирование изображения

для всех точек плоскости Z = 0, поэтому мы можем исключить
Z из уравнения

В результате возникает обратимая линейная связь между однородными координатными векторами в мировой плоскости
и плоскости изображения. Эта связь называется гомографией,
планарной гомографией или проективной гомографией, а мат­ри­
ца H ∈ ℝ3×3 является невырожденной.
Определим центральную перспективную камеру, которая
находится высоко и смотрит наклонно вниз:
>>> T_camera = SE3.Tz(8) * SE3.Rx(-2.8);
>>> camera = CentralCamera.Default(f=0.012, pose=T_camera);

на горизонтальную плоскость, как показано на рис. 13.32. Фигура на плоскости определяется набором двумерных координат
>>> P = np.column_stack([[-1, 1], [-1, 2], [2, 2], [2, 1]])
array([[-1, -1, 2, 2],
[ 1, 2, 2, 1]])

где каждый столбец – это координаты точки (X, Y).
Мы можем спроецировать точки с плоскости земли на плос­
кость изображения привычным способом, используя объект
CentralCamera, предварительно добавив Z = 0 для каждой точки
мира:
>>> camera.project_point(np.vstack([P, np.zeros((4,))]))
array([[ 347.6,
353.8,
792.4,
804.8],
[ 764.9,
616.3,
616.3,
764.9]])

Гомография вычисляется из мат­ри­цы камеры путем удаления второго столбца
>>> H = np.delete(camera.C(), 2, axis=1)
array([[
1200,
167.5,
3769],
[
0, -963.2,
6985],
[
0,
0.335,
7.538]])

Применения 795

и мы можем использовать эту особенность для преобразования
координат точек из плоскости земли непосредственно в плос­
кость изображения
>>> homtrans(H, P)
array([[ 347.6,
[ 764.9,

353.8,
616.3,

792.4,
616.3,

804.8],
764.9]])

Функция homtrans выполняет соответствующие преобразования в однородные координаты и обратно и возвращает те же
результаты, которые можно было бы получить с помощью перс­
пективной проекции с использованием объекта камеры.
Поскольку H обратима, мы можем выполнить обратное преобразование. Камера имеет плоскость изображения 1000×1000,
поэтому координаты ее углов:
>>> p = np.column_stack([[0, 0], [0, 1000], [1000, 1000], [1000, 0]])
array([[ 0,
0, 1000, 1000],
[ 0, 1000, 1000,
0]])

Им соответствуют точки на плоскости земли
>>> Pi = homtrans(np.linalg.inv(H), p)
array([[ -4.153, -3.081,
3.081,
[ 7.252, -0.426, -0.426,

4.153],
7.252]])

которые показаны синим цветом на рис. 13.32. Рисунок был
создан следующим кодом:
>>> camera.plot(scale=2, color="black");
>>> plot_sphere(radius=0.1, centre=np.vstack((P, np.zeros((4,)))),
...
color="red");
>>> plot_sphere(radius=0.1, centre=np.vstack((Pi, np.zeros((4,)))),
...
color="blue");
Обзорная камера

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

796 Глава 13 · Формирование изображения

13.7

Дополнительные темы

13.7.1

Проецирование произвольных линий
и поверхностей в трехмерном пространстве

В разделе 13.1 мы проецировали на плоскость изображения
отдельные точки трехмерного пространства, а также отрезки прямых, просто проецируя и соединяя их конечные точки
на плоскости изображения. Как спроецировать произвольную
трехмерную линию? Для этого нужно заранее решить, как представить такую линию. Есть много разных вариантов, которые
обсуждаются в разделе С.2.2.3. Одним из полезных способов
запи­си параметров являются координаты Плюккера – шестимерный вектор, во многом похожий на кручения.
Мы можем легко создать прямую линию Плюккера с помощью Toolbox. Линия, проходящая через (0, 0, 1) и (1, 1, 1), создается при помощи кода
>>> L = Line3.Join((0, 0, 1), (1, 1, 1))
{ -1 1 0; -1 -1 0}

который возвращает объект Line3, представленный в виде шес­
тимерного вектора с двумя компонентами: вектором момента
и вектором направления. Используя эти параметры, можно указать линию с помощью точки и направления или пересечения
двух плоскостей. Направление линии Плюккера – это вектор
>>> L.w
array([ -1, -1, 0])

Объект Line3 имеет методы для построения графиков, а также определения пересечения с плоскостями или другими объектами Line3. Существует много представлений линии Плюккера, включая шестимерный вектор, использованный выше,
минимальный четырехмерный вектор и кососимметричную
мат­ри­цу 4×4,
вычисляемую с использованием метода skew.
Последняя используется для проецирования линии согласно
уравнению
ℓ = ⋁×(C[L]×CT) ∈ ℝ3,

где C ∈ ℝ3×4 – мат­ри­ца камеры, и в результате получается двухмерная линия, выраженная в однородных координатах. Наблюдение за этой линией с камерой по умолчанию
>>> camera = CentralCamera.Default();
>>> l = camera.project_line(L)
array([[
1],

Кососимметричные
мат­ри­цы 2×2 и 3×3
обсуждались в главе 2. Кососиммет­
ричные мат­ри­цы
4×4 имеют шесть
уникальных элементов.

Дополнительные темы 797
[
[

-1],
0]])

дает диагональную линию, пересекающую плоскость изображения. Ее можно построить, используя plot_homline или на плос­
кости виртуального изображения камеры с помощью
>>> camera.plot_line2(l)

или за один шаг
>>> camera.plot_line3(L)

Квадрики (quadric, квадратичная поверхность) образуют богатое семейство трехмерных поверхностей. Существует 17 стандартных типов таких поверхностей, включая сферы, эллипсоиды, гиперболоиды, параболоиды, цилиндры и конусы, описываемые в общем виде как
x̃ TQx̃ = 0,
где Q ∈ ℝ4×4 – симметричная мат­ри­ца. Контур квадрики проецируется на плоскость изображения
A∗ = det(A)A–1
является транспонированной мат­ри­цей
кофакторов. Если
B = A∗, то A = B∗.
См. подробности
в приложении B.

c ∗ = CQ∗CT ∈ ℝ3×3,

где (·)∗ представляет сопряженную операцию,
а c является
мат­ри­цей, представляющей коническое сечение на плоскости
изображения, а контур – это множество точек p таких, что
p̃Tc p̃ = 0,
и для p̃ = (u, v, 1) его можно развернуть как
Au2 + Buv + Cv2 + Du + Ev + F = 0,
где

Определитель верхней левой подмат­ри­цы определяет тип
коники: отрицательный для гиперболы, 0 для параболы и положительный для эллипса.
В качестве примера определим камеру, смотрящую в начало
координат:
>>> T_camera = pose=SE3.Trans(0.2, 0.1, -5) * SE3.Rx(0.2);
>>> camera = CentralCamera.Default(f=0.015, pose=T_camera);

798 Глава 13 · Формирование изображения

и единичную сферу в начале координат
>>> Q = np.diag([1, 1, 1, -1]);

затем вычислим ее проекцию на плоскость изображения
>>> adj = lambda A: np.linalg.det(A) * np.linalg.inv(A);
>>> C = camera.C();
>>> c = adj(C @ adj(Q) @ C.T)
array([[-5.402e+07, -4.029e+05, 2.389e+10],
[-4.029e+05, -5.231e+07, 4.122e+10],
[2.389e+10, 4.122e+10, -3.753e+13]])

которая представляет собой мат­ри­цу 3×3, описывающую двухмерную конику. Определитель верхней левой подмат­ри­цы
>>> np.linalg.det(c[:2, :2])
2.826e+15

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

from sympy import symbols, Matrix, Eq, plot_implicit
x, y = symbols("x y")
X = Matrix([[x, y, 1]]);
ellipse = X * Matrix(c) * X.T;
plot_implicit(Eq(ellipse[0], 1), (x, 0, 1_000), (y, 0, 1_000));

для x, y ∈ [0, 1000].

13.7.2

Неперспективные камеры

Матрица камеры, представленная в разделе 13.1.4, имеет размеры 3×4. Любая мат­ри­ца 3×4 соответствует некоторому типу
камер, но большинство из них приведут к сильно искаженным
изображениям. Матрица камеры (13.10) представляет особое
подмножество всех возможных матриц камер 3×4 и соответствует перспективной камере. Матрицу проекции камеры C из
уравнения 13.10 можно записать в общем виде

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

Дополнительные темы 799

Ортогональная (параллельная) проекция – это простая проекция трехмерных точек без перспективы на плоскость, как
«вид в плане». Для небольших объектов, близких к камере, эта
проекция может быть достигнута с помощью телецентрического объектива. Видимый размер объекта не зависит от расстояния до него.
В случае летающего робота, летящего высоко над относительно ровной местностью, изменение глубины ΔZ мало по сравне–
–
нию со средней глубиной сцены Z , т. е. ΔZ ≪ Z . В данном случае
можно использовать масштабированную ортогональную проекцию, которая является ортогональной проекцией с последу–
ющим равномерным масштабированием m = f/Z .
Эти две неперспективные камеры являются частными случаями более общей аффинной модели камеры, описываемой
мат­ри­цей вида

которую можно разложить как

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

Подмат­ри­ца 2×3
мат­ри­цы вращения имеет шесть
элементов, но
три ограничения
(две строки имеют
единичные нормы
и ортогональны)
и, следовательно,
имеет 3 степени
свободы.

и оставляет не более 8 степеней свободы. Независимость от
глубины полностью очевидна, так как tz отсутствует. Случай,
когда перекос s = 0 и mx = my = 1, является ортогональной проекцией и имеет только 5 степеней свободы, в то время как масштабированный ортогональный вариант, когда s = 0 и mx = my,

800 Глава 13 · Формирование изображения

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

13.8

Подведение итогов

Мы обсудили первые шаги процесса компьютерного зрения –
формирование изображения мира и его преобразование в массив пикселей, составляющих цифровое изображение. Знакомые
нам изображения – это перспективные проекции мира, в которых три измерения сжаты в два. Это приводит к неоднозначности в отношении размера объекта – большой объект на расстоя­
нии выглядит так же, как маленький объект вблизи. Прямые
линии и коники не изменяются в этой проекции, но происходит искажение формы – параллельные прямые могут казаться
сходящимися, а окружности могут выглядеть как эллипсы. Мы
смоделировали процесс формирования перспективы и описали
его с помощью 11 параметров – внутренних и внешних. Геометрическая дисторсия объектива добавляет в систему дополнительные параметры. Калибровка камеры представляет собой
процесс оценки этих параметров. Мы рассмотрели два подхода
к калибровке, а также обсудили вычисление положения, когда
положение объекта с известной геометрией можно найти по
перспективной проекции, полученной с помощью калиброванной камеры.
Перспективные изображения ограничены полем зрения,
и мы обсудили несколько широкоугольных зрительных систем,
основанных на объективе «рыбий глаз», катадиоптрических камерах и массивах камер. Мы также обсудили идеальную широкоугольную камеру – сферическую, которая в настоящее время
все еще остается теоретической конструкцией. Однако ее можно использовать в качестве промежуточного представления
в унифицированной модели изображения, которая обеспечивает одну модель почти для всех геометрий камеры. Мы использовали унифицированную модель обработки изображений для
преобразования изображения камеры «рыбий глаз» в сферическое изображение, а затем в перспективное изображение вдоль
заданной оси обзора. Узнали о некоторых последних разработках в области камер, таких как массивы панорамных камер
и камеры светового поля. Мы рассмотрели два примера применения для знакомства с системой координатных меток и линейного отображения между плоскостью в мировой системе
координат и плоскостью изображения, которое применимо ко

Подведение итогов 801

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

13.8.1

Дополнительное чтение и ресурсы

Учебники по компьютерному зрению, такие как Davies (2017),
Klette (2014), Gonzalez and Woods (2018), Forsyth and Ponce (2012)
и Szeliski (2022), содержат более глубокое освещение тем, представленных в этой главе. В Hartley and Zisserman (2003) очень
подробно рассматривается формирование изображений с использованием геометрических и математических подходов,
в то время как в Ma et al. (2003) предлагается чисто математический подход. Многие темы геометрического компьютерного
зрения также изучались фотограмметрическим сообществом,
но у них в ходу другие термины. Например, калибровка камеры
называется резекцией камеры, а оценка положения – резекцией пространства. Обновленный классический учебник «Manual
of Photogrammetry» (McGlone 2013) предоставляет всестороннее
освещение этой области, включая историю, теорию и применение авиационных и спутниковых изображений. Переработанный классический учебник DeWitt and Wolf (2014) представляет
собой тщательное и легко читаемое введение в фотограммет­
рию. В Wade (2007) рассматривается развитие понимания человеком визуального процесса на протяжении многих веков.

Калибровка камеры
Калибровка гомогенного преобразования (Sutherland, 1974)
из раздела 13.2.1 в фотограмметрической литературе также
известна как прямое линейное преобразование (direct linear
transform, DLT). В Wolf (1974) описываются расширения линейной калибровки камеры с моделями, которые включают до
18 параметров и подходящие методы оценки нелинейной оптимизации. Более краткое описание нелинейной калибровки
предоставлено в Forsyth and Ponce (2011). В Hartley and Zisserman (2003) описывается, как можно получить модель линейной
калибровки, используя такие признаки, как линии в сцене.
В Tsai (1986) предложен метод калибровки с использованием плоской мишени, который лег в основу большинства современных инструментов калибровки камер. Углы шахматной

802 Глава 13 · Формирование изображения

доски можно определить с очень высокой точностью, тогда как
для сеток точек центр эллипсоидальной проекции не совпадает с центром круглого маркера и будет приводить к ошибкам
(Heikkila and Silven, 1997).
В интернете есть несколько хороших наборов инструментов
для калибровки камеры. MATLAB Toolbox написан Жан-Ивом
Буге и доступен по адресу http://www.vision.caltech.edu/bouguetj/calib_doc. Многие современные инструменты были созданы по образу и подобию этого пакета и добавили дополнительные функции, такие как автоматический поиск шахматной
доски, которую иначе было бы утомительно искать на каждом
изображении. Этот набор инструментов использует OpenCV для
выполнения базовой калибровки. OpenCV также может выполнять калибровку для объективов типа «рыбий глаз», хотя эта
функция не реализована в Toolbox.
Оценка положения – классическая и сложная задача компьютерного зрения, которой посвящено очень много публикаций.
Подходы к решению этой задачи можно разделить на аналитические и итерационные. Предполагая, что дисторсия объектива
была скорректирована, в Fischler and Bolles (1981), DeMenthon
and Davis (1992) и Horaud et al. (1989) предлагаются аналитические решения для трех и четырех неколлинеарных точек. Обычно существует несколько решений, но для четырех копланарных
точек существует единственное решение. Шесть и более точек
всегда дают уникальные решения, а также внутренние парамет­
ры калибровки камеры. Итеративные решения были описаны
в Rosenfeld (1959) и Lowe (1991). Оценка положения в Toolbox
Отступление 13.11. Фотограмметрия
Фотограмметрия – это наука о понимании геометрии мира по изображениям. Эти методы
были разработаны французским инженером
Эме Лосседа (1819–1907), работавшим в Инженерном корпусе армии в 1850-х гг. Он изготовил первую измерительную камеру и разработал математический анализ фотографий как
перспективных проекций. Он был пионером
в использовании аэрофотосъемки в качестве
инструмента для съемки Парижа с крыш высоких зданий, а также беспилотных воздушных
шаров и воздушных змеев.
Фотограмметрия обычно связана с созданием карт из изображений, полученных с большого расстояния, но раздел ближней или наземной фотограмметрии работает
с расстоянием от камеры до объекта менее 100 м, что имеет непосредственное
отношение к робототехнике. (Изображение из La Métrophotographie, Aimé Laussedat, 1899 г.)

Подведение итогов 803

представляет собой оболочку OpenCV-функции solvePnP, которая реализует итеративный и неитеративный алгоритмы, последний из которых основан на методе, предложенном в Lepetit
et al. (2009). Для оценки положения требуется геомет­рическая
модель объекта, и такие подходы компьютерного зрения известны как зрение на основе моделей. Интересный исторический обзор моделирования зрения представлен в видео 1987 г.
Джо Манди (Joe Mundy), которое доступно по адресу http://www.
archive.org/details/JosephMu1987.

Широкоугольные камеры
Интерес к этому типу камер растет в последнее время, и в настоящее время доступны высококачественные легкие объективы типа «рыбий глаз» и системы катадиоптрических камер.
В Nayar (1997) даются прекрасные пояснения и введение в широкоугольные системы получения изображений. Очень полезным онлайн-ресурсом является страница проектирования катадиоптрических сенсоров по адресу http://www.math.drexel.
edu/~ahicks/design и страница со ссылками на исследовательские группы, компании и семинары по адресу http://www.cis.
upenn.edu/~kostas/omni.html. Равноугольные зеркальные системы были описаны в Chahl and Srinivasan (1997) и в Ollis et al.
(1999). Природный орган, глаз морского гребешка, основанный
на эффекте отражения, описан в статье Colicchia et al. (2009).
Книга Daniilidis and Klette (2006) представляет собой сборник
статей по неперспективным изображениям, а книга Benosman
and Kang (2001) – еще один, опубликованный немного раньше
сборник статей. Некоторая информация доступна через CVonline по адресу http://homepages.inf.ed.ac.uk/rbf/CVonline в разделе «Image Physics» («Физика изображения»).
Набор инструментов MATLAB Toolbox для калибровки широкоугольных камер, разработанный Дэвидом Скарамуцца (Davide Scaramuzza), доступен в интернете и содержит код на MATLAB и C++. OpenCV поддерживает калибровку, проецирование
и устранение искажений изображения, полученного камерой
типа «рыбий глаз». В обоих случаях проекция типа «рыбий глаз»
моделируется полиномом радиального искажения r(θ).
Унифицированная модель визуализации в контексте катадиоптрических камер была представлена в Geyer and Daniilidis
(2000). Позже в Ying and Hu (2004) было показано, что многие
камеры типа «рыбий глаз» тоже могут быть описаны этой моделью. Калибровка камеры «рыбий глаз» в разделе 13.4.1 была
описана в Hansen et al. (2010), где предлагается оценивать ℓ и m,
а не полиномиальную функцию r(θ), как это делается в пакете
Toolbox, написанном Скарамуццей.
Существует огромное и постоянно растущее количество пуб­
ликаций по изображениям камер светового поля, но до сих

804 Глава 13 · Формирование изображения

пор нет учебника. Отличным введением в световые поля и их
применение в робототехнике является диссертация Dansereau
(2014). Тем же автором был написан пакет MATLAB Toolbox,
доступный по адресу https://mathworks.com/matlabcentral/
fileexchange/75250-light-field-toolbox. Интересное описание
первого массива камер дано в статье Wilburn et al. (2005), а сопровождающее видео демонстрирует обширные возможности камер такого типа. Съемка в световом поле – это подмно­
жество более крупной и растущей области вычислительной
фотографии.

Координатные метки
AprilTags были описаны в Olson (2011), а их реализацию можно
найти на сайте https://github.com/AprilRobotics/apriltag. Метки
ArUco описаны в Romero-Ramirez et al. (2018), а также по адресу
https://www.uco.es/investiga/grupos/ava/node/26. Оба типа меток обнаруживаются OpenCV-функцией detectMarkers.

13.8.2

Упражнения

1. Создайте центральную камеру и мишень в виде куба и визуализируйте их для различных положений камеры и куба.
Создайте и визуализируйте различные формы трехмерной
сетки, например сгенерированные функциями cylinder,
cuboid и sphere.
2. Напишите скрипт, описывающий полет камеры по орбите
вокруг куба и всегда направленной в сторону центра куба.
3. Напишите скрипт, описывающий полет камеры через куб.
4. Создайте центральную камеру с дисторсией объектива, которая видит плоскую сетку точек 10×10. Измените парамет­
ры искажения и посмотрите, как это повлияет на форму
проецируемой сетки. Создайте подушкообразную и бочко­
образную дисторсию.
5. Повторите упражнение по калибровке гомогенной камеры, описанное в разделе 13.2.1, и декомпозицию в разделе 13.2.3. Исследуйте влияние количества точек калибровки,
шума и искажений камеры на невязку калибровки и расчетное положение мишени.
6. Определите телесный угол для прямоугольного пирамидального поля зрения, образующего углы θh и θv.
7. Выполните пример калибровки камеры самостоятельно.
Используйте предложенные или создайте свои снимки шахматной доски.
8. Откалибруйте камеру на своем компьютере.
9. Выведите уравнение 13.15.

Подведение итогов 805

10. Для примера декомпозиции мат­ри­цы калибровки камеры
(раздел 13.2.3) определите ошибку ориентации по кренутангажу-рысканию между истинным и предполагаемым
положениями камеры.
11. Оценка положения (раздел 13.2.4).
а) Повторите упражнение по оценке положения для разных положений объекта (ближе, дальше).
б) Повторите упражнение для разных уровней шума ка­
меры.
в) Что происходит, когда количество точек уменьшается?
г) Противодействует ли увеличение количества точек эффекту увеличения шума?
д) Измените внутренние параметры камеры перед вызовом метода estpose. Каков эффект изменения фокусного
расстояния и главной точки, скажем, на 5 %?
е) Введите ошибку соответствия p[:, [1,2]] = p[:, [2,1]].
Что произойдет?
12. Повторите упражнения 2 и 3 для камеры «рыбий глаз»
и сферической камеры.
13. Выведите функцию ψ(θ) для параболического зеркала, используя рис. 13.19.
14. Выведите уравнение равноугольного зеркала z(x) в плоскости xz, используя рис. 13.19.
15. Квадрики.
а) Напишите программу для построения квадрики по мат­
ри­це 4×4. Подсказка: для отображения сложной трехмерной графики используйте pyvista, mayavi или plotly.
б) Напишите код для вычисления квадратичной мат­ри­цы
для сферы в произвольном месте и произвольного радиуса.
в) Напишите код для вычисления квадратичной мат­ри­цы
для произвольного круглого цилиндра.
г) Напишите код для построения плоского конического сечения, описанного мат­ри­цей 3×3.
16. Спроецируйте эллипсоидальную или сферическую квадрику на плоскость изображения. Результатом будет неявное
уравнение для конуса – напишите код для построения неявного уравнения.

Глава

14

	 Использование
нескольких изображений

В главе 12 говорилось о детекторах углов, которые находят характерные точки сцены. Эти точки могут быть надежно обнаружены
в разных видах одной и той же сцены независимо от угла обзора
или условий освещения. Такие точки характеризуются высокими градиентами изображения в ортогональных направлениях
и обычно располагаются на углах объектов. Однако трехмерная
координата соответствующей мировой точки была потеряна
в процессе перспективной проекции, которую мы обсуждали
в главе 13, – мы сопоставили трехмерную мировую точку с двухмерной координатой изображения. Нам только известно, что
мировая точка лежит на некотором луче в пространстве, соответствующем координате пикселя, как показано на рис. 13.6. Чтобы
восстановить недостающее третье измерение, нужна дополнительная информация. В разделе 13.2.4 дополнительной информацией были параметры калибровки камеры плюс геомет­рическая
модель объекта, что позволило нам оценить трехмерное положение объекта по данным двухмерного изображения.
В этой главе мы рассмотрим альтернативный подход, при котором дополнительная информация поступает из нескольких
видов одной и той же сцены. Как уже упоминалось, координаты
пикселей, полученные из одного ракурса съемки, говорят нам
лишь о том, что мировая точка лежит на некотором луче. Если
мы сможем найти ту же мировую точку на другом изображении,
полученном для иного, но известного положения, то сможем
определить второй луч, на котором должна лежать эта мировая точка. Тогда истинным положением мировой точки будет
пересечение этих двух лучей – процесс, называемый триангуляцией, или трехмерной реконструкцией. Более того, если мы
наблюдаем достаточное количество точек, то можем оценить
трехмерное движение камеры между ракурсами, а также трехмерную структуру мира.
Основная сложность состоит в том, чтобы найти одну и ту
же точку мира на нескольких изображениях. Это важная, но
нетривиальная задача сопоставления, которую мы обсудим
в разделе 14.1. В разделе 14.2 мы вернемся к фундаментальным геометрическим принципам формирования изображения,

chap14.ipynb

https://go.sn.pub/
LPDi9j

Почти! Мы можем
определить перемещение камеры
только до неизвестного масштабного
коэффициента, т. е.
это перемещение
λt̂ ∈ ℝ3, где направление t̂ известно,
а λ – нет.

Использование нескольких изображений 807

v (пиксели)

v (пиксели)

представленным в главе 13 для случая одной камеры. Если вы
еще не читали ту главу или читали давно, то было бы полезно
(повторно) ознакомиться с этим материалом. Мы расширим эту
геометрическую схему до охвата нескольких плоскостей изображения и покажем взаимосвязь между изображениями в стереопаре. Стереозрение – важный метод робототехники, позволяющий объединить информацию из двух изображений сцены,
снятых с разных точек, для определения трехмерной структуры
мира. Мы подробно рассмотрим разреженный и плотный подходы к стереозрению и реконструкции в разделе 14.3 и 14.4
соответственно. Пакетная подстройка (Bundle Adjustment) –
обобщенный подход к объединению информации со многих
камер – будет представлен в разделе 14.3.2.
Для некоторых приложений мы можем использовать камеры RGBD, которые возвращают информацию о глубине и цвете.
Основной принцип работы камеры структурированного света
представлен в разделе 14.6. Трехмерная информация обычно
представлена в виде облака трехмерных точек, и методы анализа
и использования таких данных будут представлены в разделе 14.7.
Эту главу и эту часть книги завершают три примера практического применения концепций, рассмотренных в данной главе. Раздел 14.8.1 объясняет, как можно преобразовать изображение с очевидным искажением перспективы в изображение
без искажения, синтезируя другой ракурс виртуальной камеры.
В разделе 14.8.2 описывается создание мозаики, т. е. процесс
получения последовательных изображений с движущейся камеры и их объединения в одно большое виртуальное изображение. В разделе 14.8.3 рассказывается, как можно обработать
последовательность изображений с движущейся камеры, чтобы
найти совпадающие точки мира и оценить движение камеры
и трехмерную структуру мира.

а

u (пиксели)

б

u (пиксели)

Рис. 14.1. Два вида на Эйфелеву башню. Изображения были сняты примерно одновременно с помощью двух
разных портативных цифровых камер: а) 7-мегапиксельная камера с f = 7.4 мм; б) 10-мегапиксельная камера
с f = 5.2 мм (фото Люси Корк). Изображения имеют совершенно разный масштаб, высота башни 700 и 600 пикселей
на (а) и (б) соответственно. Снимок (б) снял человек в правом нижнем углу на снимке (а)

808 Глава 14 · Использование нескольких изображений

14.1

Cовмещение признаков

Совмещение признаков (feature correspondence) – это задача
нахождения координат пикселей на двух разных изображениях, соответствующих одной и той же точке мира. Рассмотрим
пару реальных изображений (см. рис. 14.1).
>>> view1 =
Image: 1280
>>> view2 =
Image: 1280

Image.Read("eiffel-1.png")
x 851 (uint8), R:G:B [.../images/eiffel-1.png]
Image.Read("eiffel-2.png")
x 960 (uint8), R:G:B [.../images/eiffel-2.png]

На них изображена одна и та же сцена, снятая с двух разных
точек двумя разными камерами, отличающимися размерами
пикселя, фокусным расстоянием и количеством пикселей. Сцены сложные, и мы сразу видим, что поиск взаимного соответствия характерных точек оказывается весьма нетривиальной
задачей. Более половины пикселей в каждой сцене соответствуют голубому небу, и невозможно сопоставить синий пиксель на
одном изображении с соответствующим синим пикселем на
другом – эти пиксели недостаточно различимы. Это вполне
обычная ситуация, которая может возникнуть с однородными
областями изображения, такими как темные тени, ровная поверхность воды, снега или гладких искусственных объектов, таких как стены или кузова автомобилей.
Решение состоит в том, чтобы выбрать только те точки, которые являются характерными. Мы можем использовать детекторы характерных точек, которые рассмотрели в предыдущей
главе, чтобы найти угловые признаки Харриса (рис. 14.2а):
>>> hf = view1.Harris(nfeat=150)
Harris features, 150 points
>>> view1.disp(darken=True); hf.plot();

Альтернативное решение – определить признаки SIFT
(рис. 14.2б):
>>> sf = view1.SIFT().sort().filter(minscale=10)[:150]
SIFT features, 150 points
>>> view1.disp(darken=True);
>>> sf.plot(filled=True, color="y", alpha=0.3)

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

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

а

v (пиксели)

v (пиксели)

Cовмещение признаков 809

u (пиксели)

б

u (пиксели)

Рис. 14.2. 150 характерных точек, найденных для рис. 14.1a: а) угловые признаки Харриса; б) угловые признаки
SIFT, показывающие масштаб
Координаты точек,
как предполагается, выражаются
действительными
числами, определяемыми с точностью
до субпикселя.
Детектор Харриса – единственный
метод, не обеспечивающий точности до
субпикселя.

Рассмотрим общий случай двух наборов точек признаков:
{1pi ∈ ℝ2, i = 0, ..., N1 - 1} на первом изображении и {2pj ∈ ℝ2, j = 0,
..., N2 - 1} на втором. Поскольку это характерные точки изображения, мы ожидаем, что значительное количество точек на
первом изображении будет соответствовать точкам, найденным на втором. Задача состоит в том, чтобы определить, какие
точки (2uj, 2vj ) соответствуют точкам (1ui, 1vi ).
Мы не можем использовать координаты признаков для
определения соответствия – признаки будут иметь разные координаты на каждом изображении. Например, на рис. 14.1 мы
видим, что на правом снимке большинство признаков расположены ниже. Мы также не можем использовать интенсивность или цвет пикселей. Различия в балансе белого, освещении
и настройках экспозиции делают маловероятным совпадение
значений соответствующих пикселей. Даже если исключить
изменение интенсивности, на другом изображении, вероятно,
будут десятки тысяч пикселей с точно таким же значением интенсивности – оно недостаточно уникально. Нам нужен более
надежный способ описания каждого признака.
На практике принято описывать область пикселей вокруг
угловой точки, которую называют опорной областью, предоставляя четкое и уникальное описание угловой точки и ее непосредственного окружения – дескриптор признака. В реализации детектора Харриса в Toolbox дескриптор признака
>>> hf[0].descriptor.shape
(121,)

представляет собой единичный вектор с содержимым окна
11×11 вокруг угловой точки, из которого вычтено среднее значение. Дескрипторы признаков обычно являются векторами
и поэтому часто называются векторами признаков.
Определение сходства двух дескрипторов с использованием
нормализованной взаимной корреляции – это просто скаляр-

810 Глава 14 · Использование нескольких изображений

ное произведение двух дескрипторов, и результирующая мера
сходства s ∈ [−1, 1] имеет определенное значение – идеальное
совпадение при s = 1, а s ≥ 0.8 обычно считается просто хорошим
совпадением. Если вернуться к предыдущему примеру, то
>>> hf[0].distance(hf[1], metric="ncc")
array([[ 0.5908]])

показатель корреляции указывает на плохое совпадение.
Этот дескриптор обладает хорошей различающей способностью и инвариантен к изменениям интенсивности изоб­
ражения, но не инвариантен к масштабу или повороту. К другим дескрипторам окружающей области, которые мы могли бы
использовать, относятся последовательные и ранговые значения, а также гистограммы интенсивности или цвета. Преимущество гистограмм в том, что они инвариантны к вращению,
но они ничего не говорят о пространственных отношениях
между пикселями, т. е. одни и те же значения пикселей с совершенно другим размещением в пространстве дадут ту же
гистограмму.
Алгоритм SIFT вычисляет другой тип дескриптора
>>> sf[0].descriptor.shape
(128,)

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

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

Отступление 14.1. Детекторы или дескрипторы?
При сопоставлении характерных точек мира (ориентиров) между разными ракурсами мы должны сначала найти точки, которые заслуживают внимания. Это работа детектора, результатом которой является координата точки (u, v) и, возможно,
масштабный коэффициент или ориентация. Существует широкий выбор детекторов: Харриса и его варианты, алгоритм Ши-Томаси, SIFT, SURF, FAST, AGAST, MSER
и т. д.
Вторая задача состоит в получении описания области вокруг точки, которое
можно было использовать для сопоставления с областью вокруг соответствующей
точки в другом изображении. Это – дескриптор, который обычно представляет собой длинный вектор, сформированный из значений пикселей, гистограмм, градиентов, гистограмм градиента и т. д. Существует широкий выбор дескрипторов:
SIFT, SURF, ORB, BRISK, FREAK, CenSurE (он же STAR), HOG, ACF и т. д.
Некоторые алгоритмы, такие как SIFT и SURF, определяют и детектор, и дескриптор. Дескриптор SIFT является разновидностью дескриптора HOG (histogram of oriented gradients – гистограмма ориентированных градиентов).

Cовмещение признаков 811

к изменениям интенсивности изображения. Сходство между
дескрипторами основано на евклидовом расстоянии
>>> sf[0].distance(sf[1], metric="L2")
array([[ 484.1]])

большие значения которого, как в данном случае, указывают
на низкое сходство. Дескриптор SIFT совершенно инвариантен
к интенсивности, масштабу и повороту изображения. SIFT – это
и детектор углов, и дескриптор, тогда как оператор Харриса –
это просто детектор углов, который необходимо использовать
вместе с некоторым дескриптором и соответствующей мерой
расстояния – в Toolbox это нормализованное локальное окно,
а норма расстояния ZNCC – это просто скалярное произведение
двух дескрипторов.
В оставшейся части этой главы мы будем использовать признаки SIFT. Они более дорогостоящи с вычислительной точки
зрения, но дают более высокое качество сопоставления совершенно разных видов одной и той же сцены. Вычислив признаки
SIFT для каждого изображения:
>>> sf1 = view1.SIFT()
SIFT features, 3181 points
>>> sf2 = view2.SIFT()
SIFT features, 3028 points

получим два вектора объектов SIFTFeature. На каждом изображении было обнаружено несколько тысяч угловых признаков.
Теперь сопоставим два набора признаков SIFT на основе расстояния между дескрипторами SIFT:
>>> matches = sf1.match(sf2);
>>> len(matches)
860
Мы называем их и получим объект FeatureMatch, представляющий 860 предпола­
предполагаемыми гаемых, или потенциальных, соответствий. Первые пять предили потенциальными, потому что, полагаемых соответствий:
несмотря на высокую вероятность, эти >>> matches[:5].list()
соответствия еще
0:
24.00 (1118.6, 178.7) <--> (952.5, 417.9)
не подтверждены.
1:
24.12 (900.2, 636.6) <--> (775.6, 802.3)

2:
3:
4:

25.81 (760.3, 125.0) <--> (656.2, 369.1)
27.33 (820.5, 519.0) <--> (708.0, 701.6)
28.12 (801.1, 632.4) <--> (694.1, 800.3)

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

812 Глава 14 · Использование нескольких изображений

Мы можем наложить подмножество этих совпадений на исходную пару изображений:
>>> matches.subset(100).plot(color="yellow")

v (пиксели)

Результат показан на рис. 14.3. Желтые линии соединяют
совпадающие признаки на каждом изображении, и отчетливо
просматривается устойчивая закономерность. Большинство из
этих связей выглядят вполне обоснованными, но некоторые
явно неверны, и мы вскоре рассмотрим их. Метод subset класса
FeatureMatch возвращает другой объект FeatureMatch с указанным
количеством признаков, равномерно выбранных из исходного вектора. Если бы были показаны все соответствия, то мы бы
увидели сплошную желтую массу.

u (пиксели)

Рис. 14.3. Соответствие признаков показано желтыми линиями, соединяющими
предположительно соответствующие точки на двух изображениях, расположенных
горизонтально. Подмножество из 100 совпадений, основанное на сходстве дескрипторов
SIFT. Мы видим, что по крайней мере одно предлагаемое соответствие неверно

Соответствия
>>> c = matches.correspondence();
>>> c[:, :5]
array([[2890, 2467, 1661, 2218, 2081],
[2258, 1945, 1412, 1773, 1705]])

– это мат­ри­ца с одним столбцом на каждое возможное соответствие. В первом столбце указано, что признак 2890 на первом
изображении соответствует признаку 22258 на втором и т. д.
С точки зрения объектов признаков, вычисленных выше, это
говорит о том, что признак SIFT sf1[2890] соответствует признаку SIFT sf2[2258].
Евклидово расстояние между совпадающими дескрипторами признаков задается свойством distance, а их распределение
без применения порогового значения равно
>>> plt.hist(matches.distance, cumulative=True, density=True);

как показано на рис. 14.4. Из гистограммы следует, что 25 % всех
совпадений имеют расстояние между дескрипторами ниже 75,

Объект
FeatureMatch
содержит ссылки
на исходные
изображения
и отображает
их вместе по
горизонтали.

Интегральное распределение

Cовмещение признаков 813

Расстояние дескриптора SURF
Рис. 14.4. Интегральное распределение расстояния между признаками

тогда как максимальное расстояние может быть намного больше, – такие совпадения с меньшей вероятностью будут действительными.
Классический способ выбора подходящих совпадений – применение определенного порогового расстояния
>>> m = sf1.match(sf2, thresh=20);

но выбор подходящего порогового значения – непростая задача. В качестве альтернативы можно отсортировать соответствия по убыванию тесноты и выбрать 10 лучших из них:
>>> m = sf1.match(sf2, sort=True)[:10];

При работе с признаками SIFT обычно используются два
эвристических метода. Первый – проверка отношения: для
каждого признака на первом изображении отыскиваются два
лучших совпадения на втором изображении. Если отношение
расстояний между ними превышает пороговое значение, то мы
считаем совпадение неоднозначным и отбрасываем его. Вот
как включается проверка отношения:
>>> m = sf1.match(sf2, ratio=0.8)
951 matches

По умолчанию выбирается порог 0.75. Второй способ – проверка согласованности, или перекрестная проверка, которая
сопоставляет элемент на первом изображении с элементом на

814 Глава 14 · Использование нескольких изображений

втором, а затем снова сопоставляет его с элементом на первом
изображении. Если исходный элемент не найден, то совпадение
отбрасывается. Эту проверку можно включить так:
>>> m = sf1.match(sf2, crosscheck=True)
1372 matches

По умолчанию эта функция отключена, так как она удваивает
время сопоставления.
Сопоставление признаков требует больших вычислительных ресурсов – это задача O(N 2), поскольку каждый дескриптор
на одном изображении нужно сравнить с каждым дескриптором на другом изображении. Более сложные системы хранят
дескрипторы в структуре данных, такой как kd-дерево, чтобы
можно было легко найти похожие дескрипторы – ближайшие
соседи в пространстве признаков.
Хотя качество сопоставления, показанное на рис. 14.3, выглядит достаточно хорошим, в этом небольшом подмножестве есть
несколько явно неверных совпадений. Мы можем различить закономерность в линиях, соединяющих совпадающие точки, они
слегка сходятся и наклонены вниз вправо. Эта закономерность
является следствием относительной разности положений камеры, и понимание данной закономерности является ключом
к определению того, какие из возможных совпадений являются
правильными. Данной теме посвящен следующий раздел.

14.2

Геометрия нескольких ракурсов

Мы начнем с изучения геометрических отношений между изображениями одной точки P, наблюдаемыми с двух разных точек
зрения, как показано на рис. 14.5. Эта схема может представлять случай, когда две камеры одновременно просматривают
одну и ту же сцену или одна движущаяся камера делает снимок
с двух разных точек. Центр каждой камеры – начало координат {1} и {2}, – а также мировая точка P определяют в пространстве эпиполярную плоскость (epipolar plane). Мировая точка P
проецируется на плоскости изображения двух камер в точках
1
p и 2p соответственно, и эти точки называются сопряженными.
Пересечение эпиполярной плоскости и плоскости изображения
камеры образует эпиполярную линию.
Рассмотрим первое изображение. Точка 1p на плоскости изображения является функцией точки P в мировой системе координат, а точка 1e – функцией относительного положения второй
камеры. Эти две точки, а также центр камеры определяют эпиполярную плоскость и, следовательно, эпиполярную линию 2ℓ

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

Геометрия нескольких ракурсов 815

на втором изображении. По определению сопряженная точка 2p
должна лежать на этой прямой. И наоборот, 1p должна лежать на
эпиполярной линии на первом изображении 1ℓ, которая определяется точкой 2p на втором изображении.
Это фундаментальное и важное геометрическое соотношение: если взять точку на одном изображении, нам известно, что
ее сопряженная точка вынуждена лежать на определенной линии на другом изображении. Проиллюстрируем это на простом
примере, имитирующем геометрию рис. 14.5:
>>> camera1 = CentralCamera(name="camera 1", f=0.002, imagesize=1000,
...
rho=10e-6, pose=SE3.Tx(-0.1)*SE3.Ry(0.4))
Name: camera 1 [CentralCamera]
pixel size: 1e-05 x 1e-05
image size: 1000 x 1000
pose: t = -0.1, 0, 0; rpy/yxz = 0°, 0°, 22.9°
principal pt: [
500
500]
focal length: [ 0.002
0.002]

Этот код возвращает экземпляр класса CentralCamera, как обсуждалось в разделе 13.1.2. Аналогично для второй камеры:
>>> camera2 = CentralCamera(name="camera 2", f=0.002, imagesize=1000,
...
rho=10e-6, pose=SE3.Tx(0.1)*SE3.Ry(-0.4))
Name: camera 2 [CentralCamera]
pixel size: 1e-05 x 1e-05
image size: 1000 x 1000
pose: t = 0.1, 0, 0; rpy/yxz = 0°, 0°, -22.9°
principal pt: [
500
500]
focal length: [ 0.002
0.002]

эпиполярная
плоскость
плос

кост

ь из

обра

жен

ия

эпиполярная п
линия

лос

ь
кост

я

ени

раж

изоб

эпиполярная
точка

Рис. 14.5. Эпиполярная схема, показывающая две камеры со связанными координатными
системами {1} и {2} и плоскостями изображения. Мировая точка P и центры двух камер
образуют эпиполярную плоскость, а пересечение этой плоскости с плоскостями изображений
образует эпиполярные линии

816 Глава 14 · Использование нескольких изображений

Визуализируем положение двух камер с помощью следующего кода:
>>> ax = plotvol3([-0.4, 0.6, -0.5, 0.5, -0.2, 1]);
>>> camera1.plot(ax=ax, scale=0.15, shape="camera", frame=True,
...
color="blue");
>>> camera2.plot(ax=ax, scale=0.15, shape="camera", frame=True,
...
color="red");

как показано на рис. 14.6. Определим произвольную мировую
точку
>>> P=[0.5, 0.1, 0.8];

Отобразим ее как маленькую сферу
>>> plot_sphere(0.03, P, color="blue");

которая также показана на рис. 14.6. Спроецируем эту точку на
обе камеры:
>>> p1 = camera1.plot_point(P)
array([[ 549.7],
[ 520.6]])
>>> p2 = camera2.plot_point(P)
array([[
734],
[ 534.4]])

Рис. 14.6. Моделирование двух камер и целевой точки. Исходные точки двух камер смещены
по оси абсцисс, и камеры сближены, т. е. их оптические оси пересекаются

Геометрия нескольких ракурсов 817

как показано на рис. 14.7. Эпиполи вычисляются путем прое­
цирования центра каждой камеры на плоскость изображения
другой камеры:
>>> e1 = camera1.plot_point(camera2.centre, "kd")
array([[
973],
[
500]])
>>> e2 = camera2.plot_point(camera1.centre, "kd")
array([[ 26.96],
[
500]])

Они показаны на рис. 14.7 черным маркером ♦.
камера 1

камера 2

точка P
эпиполь
эпиполярная
линия

v (пиксели)

v (пиксели)

точка P
эпиполь
эпиполярная
линия

u (пиксели)

u (пиксели)

Рис. 14.7. Виртуальные плоскости изображений двух объектов Toolbox CentralCamera с перспективной проекцией
точки P, проекцией центра другой камеры и эпиполярной линией

14.2.1

Фундаментальная мат­ри­ца

Эпиполярное отношение, показанное графически на рис. 14.5,
можно кратко и элегантно выразить в следующем виде:
p̃T F 1p̃ = 0,

2

(14.1)

где 1p̃ и 2p̃ – точки изображения 1p и 2p, выраженные в однородной форме, а F ⊂ ℝ3×3 называется фундаментальной мат­ри­цей.
Мы можем переписать это выражение как
p̃T 2ℓ˜ = 0,

2

(14.2)

где
ℓ˜ ≃ F 1p̃

2

(14.3)

818 Глава 14 · Использование нескольких изображений

представляет собой уравнение двухмерной эпиполярной линии, вдоль которой должна лежать сопряженная точка на втором изображении. Эта линия является функцией координаты
точки 1p на первом изображении и уравнения (14.2) – мощный
тест сопряженности точки на втором изображении.
Выполнив транспонирование обеих частей уравнения (14.1),
получаем
p̃T FT 2p̃ = 0,

1

(14.4)

откуда можно вывести эпиполярную линию для первой камеры
ℓ˜ ≃ FT 2p̃

1

(14.5)

применительно к точке обзора второй камеры.
Фундаментальная мат­ри­ца является функцией параметров
камеры и относительного ее положения между видами
F ≃ K2-T[2t1]× 2R1K1-1,

(14.6)

где K1 и K2 ∈ ℝ3×3 – внутренние мат­ри­цы камеры, определяемые
уравнением (13.8). ► 2R1 ∈ SO(3) и 2t1 ∈ ℝ3 – относительная ориентация и смещение первой камеры по отношению ко второй,
или 2ξ1.
 братите внимание, что это противоречит ожиданиям: вторая каО
мера относительно первой, но с математической точки зрения так
получается проще. Функции в Toolbox всегда описывают положение
камеры относительно мировой системы координат.

Фундаментальная мат­ри­ца, связывающая два вида, возвращается методом F класса CentralCamera, например
Отступление 14.2. Кратко о двухмерной проективной геометрии
Проективная плоскость ℙ2 – это множество всех точек (x1, x2, x3)T,
xi ∈ ℝ и xi не все равны нулю. Обычно трехэлементный кортеж считается вектором-столбцом. Точка P с евклидовыми координатами
(u, v) представлена в ℙ2 однородными координатами p̃ = (u, v, 1)T.
Масштаб не имеет значения для однородных величин, и мы записываем это как p̃ ≃ λp̃, где оператор ≃ означает равенство с ненулевым масштабным коэффициентом (возможно, неизвестным).
Точка в ℙ2 может быть представлена в неоднородной, или евклидовой, форме p = (x1 /x3, x2 /x3)T в ℝ2. Однородный вектор (u, v, f )T, где
f – фокусное расстояние в пикселях, представляет собой вектор из
начала координат камеры, указывающий на мировую точку P. Более подробная информация приведена в разделе С.2.
Функции e2h и h2e в пакете Toolbox преобразуют евклидовы
и однородные координаты для точек (вектор-столбец) или наборов точек (мат­ри­ца с одним столбцом на точку).

Если оба изображения сняты одной
и той же камерой, то
K1 = K2.

Геометрия нескольких ракурсов 819
>>> F = camera1.F(camera2)
array([[
0, -1.947e-06, 0.0009735],
[-1.947e-06,
0, 0.001895],
[0.0009735, 5.248e-05, -0.9735]])

и для двух точек изображения, вычисленных ранее, можно вычислить (14.1)
>>> e2h(p2).T @ F @ e2h(p1)
array([[
0]])
Результат является
скаляром,
но в NumPy это
массив 1×1.

Нулевой результат говорит о том, что точки смежные.
Фундаментальная мат­ри­ца обладает некоторыми интересными свойствами. Она вырожденная с рангом 2
>>> np.linalg.matrix_rank(F)
2

Матрица F ⊂ ℝ3×3 и имеет семь степеней свободы. Эпиполи кодируются
имеет семь базовых пространстве мат­ри­цы. Эпиполь первой камеры – это
параметров, поэтому ее девять нуль-пространство F
элементов не являются независимыми. >>> e1h = linalg.null_space(F);
Общий масштаб не >>> e1h.T
определен, и на нее array([[ 0.8894,
0.457, 0.0009141]])
наложено ограничение: det(F) = 0.

в нульправое

в однородных координатах или
>>> e1 = h2e(e1h)
array([[
973],
[
500]])

Правое нульпространство
транспонированной
мат­ри­цы,
см. приложение B.

в евклидовых координатах – как показано на рис. 14.7. Эпиполь
фундаменвторой камеры – это левое нуль-пространство
тальной мат­ри­цы
>>> e2h = linalg.null_space(F.T);
>>> e2 = h2e(e2h)
array([[ 26.96],
[
500]])

Пакет Toolbox может отображать эпиполярные линии, используя методы plot_epiline класса CentralCamera
>>> camera2.plot_epiline(F, p1, color="red")

которые показаны на рис. 14.7 красной линией в плоскости двух
изображений камеры. Мы видим, как и ожидалось, что проекция P лежит на этой эпиполярной линии. Эпиполярная линия
для первой камеры:
>>> camera1.plot_epiline(F.T, p2, color="red");

820 Глава 14 · Использование нескольких изображений

14.2.2

Существенная мат­ри­ца

Эпиполярное геометрическое ограничение также можно выразить через нормализованные координаты изображения
x̃ E 1x̃ = 0,

(14.7)

2 T

где E ⊂ ℝ3×3 – существенная мат­ри­ца (essential matrix), а 2x̃ и 1x̃ –
сопряженные точки в однородных нормированных координатах изображения.
Эта мат­ри­ца – простая функция относительного положения камеры
E ≃ [2t1]×2R1,

Для камеры
с фокусным расстоя­
нием 1 и началом
координат в главной
точке, как показано
на рис. 13.3.

(14.8)

где (2R1, 2t1) представляет 2ξ1 – положение первой камеры относительно второй. Существенная мат­ри­ца вырождена, имеет
ранг 2, два равных ненулевых сингулярных значения и одно
нулевое. Существенная мат­ри­ца имеет только пять степеней
свободы и полностью определяется тремя вращательными
и двумя поступательными параметрами. Для чистого вращения, когда t = 0, существенная мат­ри­ца не определена.
Из уравнения (13.8) следует, что p̃ ≃ Kx̃, и, подставляя это соотношение в уравнение (14.7), получаем

См. раздел B.2.2.
Трехмерный
перенос (x, y, z)
с неизвестным
масштабом можно
рассматривать как
(x¢, y¢, 1).

(14.9)

Приравнивание условий с уравнением (14.1) дает связь между двумя мат­ри­ца­ми:
E ≃ K2T F K1

(14.10)

с точки зрения внутренних параметров двух задействованных
камер. Это уравнение реализуется методом E класса CentralCamera
>>> E = camera1.E(F)
array([[
0, -0.07788,
[-0.07788,
0,
[
0, -0.1842,

Если оба
изображения сняты
одной и той же
камерой, то K1 = K2.

0],
0.1842],
0]])

где используются внутренние параметры первой камеры (которая совпадает со второй камерой).
Как и мат­ри­цу камеры в разделе 13.2.3, существенную мат­
ри­цу можно разложить, чтобы получить относительное положение 1ξ2 в форме мат­ри­цы SE(3). ► Обращение не уникально,
и функция OpenCV возвращает четыре решения:

Хотя уравнение 14.8
записывается
в виде 2ξ1 ~ (2R1, 2t1),
функция Toolbox
возвращает 1ξ2.

Геометрия нескольких ракурсов 821
>>>
>>>
t =
t =
t =
t =

T_1_2 = camera1.decomposeE(E);
T_1_2.printline(orient="camera")
-0.921, 0, -0.389; rpy/yxz = 0°, 0°, -45.8°
0.921, 0, 0.389; rpy/yxz = 0°, 0°, -45.8°
-0.921, 0, -0.389; rpy/yxz = 180°, 0°, 180°
0.921, 0, 0.389; rpy/yxz = 180°, 0°, 180°

Как отмечено
в Hartley and Zisserman (2003, стр. 259),
при разложении
существенной мат­
ри­цы определить
знак разложения
невозможно.

где переносы заданы единичными векторами t̂, а истинный перенос – st̂, где s – неизвестный масштаб. Если учесть, что s ∈ ℝ,
т. е. это действительное число со знаком, то существует всего два
решения. Первая и вторая пары – это, по сути, одно решение,
отличающееся знаком s. Истинное положение первой камеры
относительно второй

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

>>> T_1_2_true = camera1.pose.inv() * camera2.pose;
>>> T_1_2_true.printline(orient="camera")
t = 0.184, 0, 0.0779; rpy/yxz = 0°, 0°, -45.8°

и перенос, как единичный вектор, равен
>>> T_1_2_true.t / np.linalg.norm(T_1_2_true.t)
array([ 0.9211,
0, 0.3894])

Это указывает на то, что в данном случае верно решение номер два.
В общем случае мы не знаем положения двух камер, так как
же определить правильное решение на практике? Один из подходов – определить, видна ли мировая точка. Обычно мы выбираем точку на оптической оси перед первой камерой
>>> Q = [0, 0, 10];

и ее проекция на первую камеру
>>> camera1.project_point(Q).T
array([[ 417.8,
500]])

как и ожидалось, находится в середине плоскости изображения.
Эта точка тоже должна быть видна второй камере. Мы можем
проверить видимость предполагаемых относительных положений в pose_1_2
>>> for T in T_1_2:
... print(camera1.project_point(Q, pose=T).T)
[[
746.1
500]]
[[
670
500]]
[[
nan
nan]]
[[
nan
nan]]

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

822 Глава 14 · Использование нескольких изображений
>>> T = camera1.decomposeE(E, Q);
>>> T.printline(orient="camera")
t = -0.921, 0, -0.389; rpy/yxz = 0°, 0°, -45.8°

В этом случае возвращается только первое верное решение
из приведенных выше.
Таким образом, эти мат­ри­цы 3×3 – фундаментальная и существенная – кодируют параметры и относительное положение двух камер. Фундаментальная мат­ри­ца и точка на одном
изображении определяют эпиполярную линию на другом изображении, вдоль которой должны лежать ее сопряженные точки. Существенная мат­ри­ца кодирует относительное положение
центров двух камер, и положение может быть извлечено с двумя возможными значениями и с перемещением, масштабированным неизвестным коэффициентом. В данном примере фундаментальная мат­ри­ца вычислялась по известному движению
камеры и внутренним параметрам. В реальном мире все не
так – движение камеры трудно измерить, и камера может быть
не откалибрована. Вместо этого мы можем вычислить фундаментальную мат­ри­цу непосредственно из соответствующих
точек изображения.

14.2.3

 ычисление фундаментальной мат­ри­цы
В
по данным реального изображения

Предположим, что у нас есть N пар соответствующих точек
в двух проекциях одной и той же сцены (1pi, 2pi), i = 0, ..., N - 1.
В качестве демонстрационного примера создадим набор из
20 случайных угловых признаков в пределах куба 2×2×2 м,
центр которого находится в 3 м перед камерами,
>>> P = np.random.uniform(low=-1, high=1, size=(3, 10))
...
+ np.c_[0, 0, 3].T;

и спроецируем эти точки на плоскости изображения обеих камер:
>>> p1 = camera1.project_point(P);
>>> p2 = camera2.project_point(P);

Если N ≥ 8, то фундаментальную мат­ри­цу можно найти по
этим двум наборам соответствующих точек
>>> F, resid = CentralCamera.points2F(p1, p2)
>>> resid
1.31e-07

где невязка является максимальным значением левой части
уравнения (14.1) и в идеале равна нулю. В данном случае зна-

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

Геометрия нескольких ракурсов 823

чение не нулевое, но очень маленькое, и это связано с накоплением ошибок машинной арифметики, имеющей конечную точность. Оцениваемая мат­ри­ца обладает требуемым свойством
ранга:
>>> np.linalg.matrix_rank(F)
2

Для второй камеры можно построить спроецированные точки
>>> camera2.plot_point(P);

и наложить эпиполярные линии, созданные каждой точкой на
первом изображении:
>>> camera2.plot_epiline(F, p1, color="red")

как показано на рис. 14.8. Мы видим семейство, или пучок, эпиполярных линий, где каждая точка на втором изображении лежит на эпиполярной линии. Обратите внимание, как все эпиполярные линии сходятся на эпиполе, что возможно в данном
случае, потому что две камеры совмещены, как показано на
рис. 14.6.
камера 2
точка P
эпиполь
эпиполярная
линия

v (пиксели)

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

u (пиксели)

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

Чтобы продемонстрировать важность правильного сопряжения точек, повторим приведенный выше пример, но введем
две неправильные ассоциации данных, поменяв местами два
элемента в p2:
>>> p2[:,[5, 6]] = p2[:,[6, 5]];

824 Глава 14 · Использование нескольких изображений

Фундаментальная мат­ри­ца
>>> _, resid = CentralCamera.points2F(p1, p2);
>>> resid
0.007143

теперь имеет невязку, которая более чем на 4 порядка превышает предыдущую. Это означает, что соответствие точек не может быть объяснено соотношением (14.1).
Зная фундаментальную мат­ри­цу, мы могли бы проверить,
действительно ли пара соответствующих точек-кандидатов сопряжена, измерив, насколько далеко одна из них находится от
эпиполярной линии, определяемой другой:
>>> CentralCamera.epidist(F, p1[:, 0], p2[:,0])
array([[4.448e-05]])
>>> CentralCamera.epidist(F, p1[:, 5], p2[:,5])
array([[ 34.82]])

Результат показывает, что точка 0 подходит хорошо, а точка 5
(которую мы поменяли местами с точкой 6) – плохо. Однако мы
должны сначала найти фундаментальную мат­ри­цу, а для этого
необходимо, чтобы было известно соответствие точек. Мы выходим из этого тупика с помощью гениального алгоритма под
названием «Случайная выборка и консенсус» (Random Sampling
and Consensus, RANSAC).
Он основан на восхитительно простом принципе. Для оценки фундаментальной мат­ри­цы требуется восемь точек, поэтому мы случайным образом выбираем восемь возможных подходящих точек (выборка) и находим F для создания модели.
Эта модель проверяется на соответствие всем другим парам
кандидатов, и те, кто подходит, ► голосуют за эту модель. Процесс повторяется несколько раз, и возвращается модель, которая получила наибольшее количество голосов (консенсус).
Из-за малого объема выборки высока вероятность, что все
содержащиеся в ней пары кандидатов – допустимые. Пары
точек, которые поддерживают модель, называются сторонниками (inlier), а те, которые не поддерживают, – противниками
(outlier).
RANSAC удивительно эффективен при поиске набора сторонников даже при наличии большого количества противников
(более 50 %) и применим к широкому кругу задач. В Toolbox мы
выбираем RANSAC как параметр при вычислении фундаментальной мат­ри­цы
>>> F, resid, inliers = CentralCamera.points2F(p1, p2,
...
method="ransac", confidence=0.99, seed=0);
>>> resid
1.54e-07

В пределах
определенного
порога ransac­
ReprojThreshold
перепроецирования в единицах
пикселей.

Геометрия нескольких ракурсов 825

и получаем отличную окончательную невязку. Функция также
возвращает набор сторонников:
>>> inliers
array([True, True, True, True, True, False, False, True, True, True])

и надо отметить, что две неправильные связи, точки 5 и 6, отсутствуют в этом списке. В четвертом параметре передается
желаемый уровень уверенности, что оценка мат­ри­цы верна.
Если выбрать этот параметр слишком близким к единице, то
RANSAC будет выполнять больше попыток, что увеличит время
вычислений. Это, в свою очередь, может привести к превышеОтступление 14.3. Надежная оценка с помощью RANSAC
Создадим набор из десяти дискретных точек на линии y = 3x - 10
>>> x = np.arange(11);
>>> y = 3 * x - 10;
и прибавим случайное число к четырем случайно выбранным точкам
>>> nbad = 4;
>>> np.random.seed(1) # инициализировать генератор случайных чисел
>>> bad = np.random.choice(len(x), nbad,
...
replace=False)
array([2, 3, 4, 9])
>>> y[bad] = y[bad] + np.random.rand(nbad)
... * 10
Надо подобрать прямую, использовав стандартный метод наименьших квад­
ратов
>>> from scipy import stats
>>> m, c, *_ = stats.linregress(x, y)
>>> plt.plot(x, m * x + c, 'r--');
В результате получается красная пунктирная линия, которая явно смещена от
истинной линии из-за выбросов в точках данных.
В отличие от этого подхода, алгоритм RANSAC (см. examples/ransac_line)
>>> params, inliers = ransac_line(x, y)
>>> params
(3.0, -10.0)
>>> inliers
[1, 7, 0, 5, 6, 8, 10]
нашел истинные параметры линии
и вернул список «хороших» точек,
несмотря на то что 40 % точек не
соответствуют модели. RANSAC
определил параметры консенсусной линии, с которой «согласилось»
наибольшее количество точек.

хорошие точки данных
плохие точки данных
оценка методом наименьших квадратов
оценка алгоритмом RANSAC

826 Глава 14 · Использование нескольких изображений

нию максимального числа итераций, что потребует увеличения
уже этого параметра. Также следует учитывать, что результаты
RANSAC будут различаться от запуска к запуску из-за случайного характера субдискретизации. Использование RANSAC может
потребовать некоторого количества проб и ошибок для выбора
подходящих параметров для конкретной задачи.
Вернемся теперь к паре изображений Эйфелевой башни на
рис. 14.3. В конце раздела 14.1 мы остановились на том, что на­
шли соответствия-кандидаты на основе сходства дескрипторов,
но было несколько явно неверных совпадений. Если применить
алгоритм RANSAC для вычисления фундаментальной мат­ри­цы,
то он обнаружит некорректные соответствия и в набор «сторонников» будут включены только пары точек, удовлетворяющие
эпиполярному ограничению фундаментальной мат­ри­цы:
>>> F, resid, inliers = CentralCamera.points2F(matches.p1,
...
matches.p2, method="ransac",
...
confidence=0.99);
>>> resid
0.0324
>>> sum(inliers) / len(inliers)
0.8244

В данном случае в число «сторонников» вошло чуть больше
80 % точек. Дисторсия объектива в этом примере не учитывалась, а значит, эпиполярное соотношение (14.1) не может быть
применено.
В данном случае RANSAC выявил
>>> sum(~inliers)
151

«противника», или неверные ассоциации данных, на этапе сопоставления признаков SIFT – результат оказался хуже, чем
предполагалось.
Альтернативный способ выразить это – заставить объект FeatureMatch выполнить оценку
>>> F, resid = matches.estimate(CentralCamera.points2F,
...
method="ransac", confidence=0.99,
...
seed=0);

и вызвать функцию, заданную первым аргументом. Объект FeatureMatch сохраняет возвращаемую информацию о вложенных
элементах и способен отображать статус каждого совпадения.
>>> matches
860 matches, with 709 (82.4%) inliers
>>> matches[:10].list()
0: + 24.00 (1118.6, 178.7) <--> (952.5, 417.9)
1: + 24.12 (900.2, 636.6) <--> (775.6, 802.3)

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

Геометрия нескольких ракурсов 827
2:
3:
4:
5:
6:
7:
8:
9:

+
+
+
+
+
+
+
+

25.81
27.33
28.12
29.33
31.06
33.60
33.70
33.79

(760.3, 125.0) <--> (656.2, 369.1)
(820.5, 519.0) <--> (708.0, 701.6)
(801.1, 632.4) <--> (694.1, 800.3)
(1094.0, 184.7) <--> (932.9, 423.0)
(781.0, 214.4) <--> (672.8, 443.8)
(1094.0, 184.7) <--> (932.9, 423.0)
(526.5, 484.2) <--> (462.4, 673.6)
(759.7, 332.0) <--> (655.9, 543.0)

со знаком плюс или минус, указывающим, является совпадение
«сторонником» или «противником» соответственно. Мы можем
построить график некоторых «сторонников»
>>> matches.inliers.subset(100).plot(color="g");

или «противников»
>>> matches.outliers.subset(100).plot(color="red")

v (пиксели)

И те, и другие показаны на рис. 14.9.

u (пиксели)

v (пиксели)

а

б

u (пиксели)

Рис. 14.9. Результаты сопоставления признаков SIFT после RANSAC: а) подмножество
совпадений-сторонников; б) подмножество совпадений-противников, некоторые явно
неверны, в то время как другие на первый взгляд выглядят правильными

Самый удобный способ наложить эпиполярные линии на
изображение – создать объект CentralCamera и отобразить изоб­
ражение в его виртуальной плоскости изображения:
>>> camera = CentralCamera();
>>> camera.disp(view1);

828 Глава 14 · Использование нескольких изображений

Теперь мы можем наложить эпиполярные линии, вычисленные из соответствующих точек, найденных на втором изображении:
>>> camera.plot_epiline(F.T, matches.inliers.subset(20).p2,
...
color="black");

Результат показан на рис. 14.10. Эпиполярные линии пересекаются в эпиполярной точке, которую мы ясно видим, это проекция второй камеры на первом изображении. Эпиполь
>>> epipole = h2e(linalg.null_space(F))
array([[ 964.3],
[ 654.4]])
>>> camera.plot_point(epipole, "wd");

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

v (пиксели)

перспектива

u (пиксели)

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

14.2.4

Планарная гомография

В разделе 13.6.2 мы рассматривали одну камеру, наблюдающую
мировые точки, лежащие на плоскости. В этом разделе мы рассмотрим две разные камеры, наблюдающие за набором миро-

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

Геометрия нескольких ракурсов 829

вых точек Pi на плоскости. Плоскости изображений проекций
в камерах обозначены как 1pi и 2pi соответственно и связаны соотношением
p̃i ≃ H 1p̃i,

(14.11)

2

Матрица гомографии имеет произвольный масштаб
и, следовательно, 8
степеней свободы.
Что касается уравнения (14.13), вращение, смещение
и нормаль имеют 3,
3 и 2 степени свободы соответственно,
всего 8. Гомографии
образуют группу:
произведение двух
гомографий является другой гомографией, тождественная гомография
является единичной
мат­ри­цей, и обратная операция
определена как обращение мат­ри­цы.

где H ⊂ ℝ3×3 – невырожденная мат­ри­ца, которая представляет
проективное преобразование, называемое гомографией (homography), планарной гомографией или гомографией, индуци­
рованной плоскостью.
Например, снова рассмотрим пару камер на рис. 14.6, которые теперь наблюдают за сеткой точек 3 × 3
>>> T_grid = SE3.Tz(1) * SE3.Rx(0.1) * SE3.Ry(0.2);
>>> P = mkgrid(3, 1.0, pose=T_grid);

где Tgrid – положение системы координат сетки {G}, а точки сетки центрированы в плоскости xy. Точки проецируются на обе
камеры
>>> p1 = camera1.plot_point(P, "o");
>>> p2 = camera2.plot_point(P, "o");

а проекции показаны на рис. 14.11a, б соответственно.
камера 1

камера 2
спроецированная точка
точка, преобразованная из изображения 1

а

v (пиксели)

v (пиксели)

спроецированная точка

u (пиксели)

б

u (пиксели)

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

Как и в случае с фундаментальной мат­ри­цей, мы можем оценить мат­ри­цу H по двум наборам соответствующих точек:
>>> H, resid = CentralCamera.points2H(p1, p2)
>>> H

830 Глава 14 · Использование нескольких изображений
array([[ -0.4187, -0.0003935,
[ -0.6981, 0.3738,
[-0.001396, -1.459e-05,

397.8],
309.5],
1]])

что требует наличия N ≥ 4 пар соответствующих точек.
Согласно уравнению (14.11), мы можем предсказать положение точек сетки на втором изображении по соответствующим
координатам первого изображения
>>> p2b = homtrans(H, p1);

которые можно наложить на второе изображение как символы
«+»:
>>> camera2.plot_point(p2b, "+");

Результат показан на рис. 14.11б, где можно видеть, что предсказанные точки идеально совпадают с реальной проекцией
мировых точек. Обращенная мат­ри­ца гомографии
p̃i ≃ H-1 2p̃i

(14.12)

1

выполняет обратное преобразование, из координат второго
изображения в координаты первого:
>>> p1b = homtrans(np.linalg.inv(H), p1);

Фундаментальная мат­ри­ца заставляет сопряженную точку
лежать на линии, а гомография говорит нам, где именно будет
сопряженная точка на другом изображении, при условии что
точки лежат на плоскости.
Мы можем воспользоваться этой оговоркой в качестве проверки того, лежат ли точки на плоскости. Добавим несколько
дополнительных мировых точек в наш пример
>>> Q = np.array([
... [-0.2302, -0.0545,
... [ 0.3287,
0.4523,
... [ 0.4000,
0.5000,

0.2537],
0.6024],
0.6000] ]);

которые можно отобразить в трехмерное пространство:
>>>
>>>
>>>
>>>
>>>

plotvol3([-1, 1, -1, 1, 0, 2]);
plot_sphere(0.05, P, color="blue");
plot_sphere(0.05, Q, color="red");
camera1.plot(color="blue", frame=True);
camera2.plot(color="red", frame=True);

как показано на рис. 14.12. Новые точки, показанные красным
цветом, явно лежат не в той же плоскости, что и исходные синие
точки. Если взять вид с первой камеры
>>> p1 = camera1.plot_point(np.hstack((P, Q)), "o");

Эти точки лежат
вдоль луча от цент­
ра камеры до дополнительного ряда
точек в плоскости
сетки. Однако их
координаты z были
выбраны равными
0.4, 0.5 и 0.6 м соответственно.

Геометрия нескольких ракурсов 831

Рис. 14.12. Расположение точек мишени и два положения камер. Синие точки лежат
в плоской сетке, а красные кажутся лежащими в сетке с точки зрения первой камеры
камера 1

камера 2

спроецированная точка на плоскости

спроецированная точка на плоскости

спроецированная точка вне плоскости

спроецированная точка вне плоскости
точка на плоскости, преобразованная из изображения 1

v (пиксели)

v (пиксели)

точка вне плоскости, преобразованная из изображения 1

u (пиксели)
u (пиксели)
а
б
Рис. 14.13. Точки, наблюдаемые с двух разных ракурсов. Точки сетки показаны как пустые круги. Знаки «плюс» в (б)
обозначают точки, преобразованные из плоскости изображения первой камеры с помощью гомографии. Синим
обозначены точки на плоскости сетки, а красным – некомпланарные точки

как показано на рис. 14.13а, эти новые точки появляются в виде
дополнительного ряда в сетке точек, которую мы использовали
выше. Однако во втором представлении
>>> p2 = camera2.plot_point(np.hstack((P, Q)), "o");

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

832 Глава 14 · Использование нескольких изображений
>>> p2h = homtrans(H, p1);

мы найдем, где они должны быть на изображении второй камеры, если бы они принадлежали плоскости, неявной в гомографии:
>>> camera2.plot_point(p2h, "+");

Исходные девять точек перекрываются, а три новые точки –
нет. Мы могли бы сделать автоматический тест на основе ошибки прогноза
>>> np.linalg.norm(homtrans(H, p1) - p2, axis=0)
array([1.262e-05, 9.673e-06, 6.544e-07, 8.302e-06, 4.34e-06,
1.189e-05, 1.679e-05, 5.884e-06, 3.927e-05,
50.6,
46.44,
45.38])

В данном случае видно, что значения велики для этих последних трех точек – они не принадлежат плоскости, которая
индуцировала гомографию.
В этом примере мы оценили гомографию на основе двух наборов сопоставленных точек, которые были проекциями известных точек на плоскости. На практике мы не знаем заранее,
какие точки принадлежат плоскости, поэтому снова можем использовать алгоритм RANSAC
>>> H, resid, inliers = CentralCamera.points2H(p1, p2,
...
method="ransac");
>>> resid
4.838e-05
>>> inliers
array([ True, True, True, True, True, True, True, True, True,
False, False, False])

и найти гомографию, лучше всего объясняющую взаимосвязь
между наборами точек изображения. Он также идентифицировал точки, которые поддерживают гомографию, и три точки вне
плоскости, не вошедшие в список сторонников.
Геометрическая схема, связанная с гомографией, показана
на рис. 14.14. Мы можем выразить гомографию в нормированных координатах изображения ►:
x̃ ≃ HE 1x̃,

2

где НЕ – евклидова гомография, которая описывается уравнением
(14.13)

См. раздел 13.1.2.

Геометрия нескольких ракурсов 833

в терминах относительного положения камеры 2ξ1 и плоскости
nTP + d = 0 в системе координат {1}. Евклидовы и проективные
гомографии связаны соотношением
HE ≃ K-1HK,

где K – мат­ри­ца внутренних параметров камеры.
плоскость объекта

эпиполярная
плоскость

плос

кост

ь из

обра

жен

ия

пло

с

ь
кост

я

ени

раж

изоб

Рис. 14.14. Геометрическая схема гомографии, показывающая две камеры
с соответствующими системами координат {1} и {2} и плоскостями изображения. Мировая
точка P принадлежит плоскости с нормалью к поверхности n. H – это гомография, мат­ри­ца
3×3, которая отображает 1p в 2p
Хотя уравнение
14.13 записывается
с использованием
(2R1, 2t1), функция из
пакета Toolbox возвращает 1ξ2.

Что касается существенной мат­ри­цы, мы можем разложить
проективную гомографию и получить относительное положение 1ξ2 в форме однородного преобразования, а также нормаль к плоскости
>>>
>>>
t =
t =
t =
t =

T, normals = camera1.decomposeH(H);
T.printline(orient="camera")
-0.185, 0, -0.0783; rpy/yxz = -8.39e-06°, 4.61e-06°, -45.8°
0.185, 0, 0.0783; rpy/yxz = -8.39e-06°, 4.61e-06°, -45.8°
0.0197, 0.0192, -0.199; rpy/yxz = 1.09°, 0.338°, -34.4°
-0.0197, -0.0192, 0.199; rpy/yxz = 1.09°, 0.338°, -34.4°

И снова мы получаем несколько решений, поэтому мы должны исследовать дополнительную информацию, чтобы определить, какое из них правильное. Как обычно, поступательный
компонент мат­ри­цы преобразования имеет неизвестный масштабный коэффициент. По рис. 14.12 мы знаем, что камера
движется преимущественно в направлении x и что нормаль
к плоскости примерно параллельна оптической оси камеры,

834 Глава 14 · Использование нескольких изображений

или оси z, и это знание подсказывает, что верно второе решение. Истинное преобразование из первой камеры во вторую
>>> (camera1.pose.inv() * camera2.pose).printline(orient="camera")
t = 0.184, 0, 0.0779; rpy/yxz = 0°, 0°, -45.8°

соответствует нашему выбору. Положение сетки относительно
первой камеры:
>>> camera1.pose.inv() * T_grid
0.9797 -0.03888 -0.1968
0.01983 0.995
-0.09784
0.1996
0.09195 0.9756
0
0
0

-0.2973
0
0.96
1

а третий столбец – это нормаль сетки, ► которая соответствует
предполагаемой нормали, связанной со вторым решением.
>>> normals[1].T
array([[ -0.1968, -0.09784, 0.9756]])

Поскольку точки
находятся в плос­
кости xy системы
координат сетки {G},
нормаль является
осью z.

Мы можем применить эту методику к паре реальных фотографий двора и стены:
>>> walls_l = Image.Read("walls-l.png", reduce=2);
>>> walls_r = Image.Read("walls-r.png", reduce=2);

которые уменьшены в два раза в каждом измерении и показаны на рис. 14.15. Начнем с поиска признаков SIFT
>>> sf_l = walls_l.SIFT();
>>> sf_r = walls_r.SIFT();

и получим 1000 кандидатов на связанные точки:

v (пиксели)

v (пиксели)

>>> matches = sf_l.match(sf_r);

первая плоскость
вторая плоскость
третья плоскость

а

u (пиксели)

б

u (пиксели)

Рис. 14.15. Две фотографии двора, сделанные с разных ракурсов. Изображение (б) было сделано примерно
на 30 см правее изображения (a). На изображение (a) наложены угловые признаки, лежащие на плоскости.
Камера была ручной

Разреженное стереозрение 835

Затем применим RANSAC, чтобы найти набор связанных точек, который лучше всего ложится на плоскость в мире:
>>> H, resid = matches.estimate(CentralCamera.points2H,
...
confidence=0.9, seed=0)
>>> matches
2796 matches, with 1053 (37.7%) inliers

В этом примере порог достоверности снижен, чтобы учесть
дисторсию объектива и неидеальную гладкость плоскостей.
Большинство пар точек не укладываются в модель, т. е. не принадлежат плоскости, индуцирующей гомографию H. Однако
1053 точки принадлежат плоскости, и мы можем наложить их
на изображение
>>> walls_l.disp();
>>> plot_point(matches.inliers.p1, "r.");

как показано на рис. 14.15а. RANSAC нашел консенсус, который
представляет собой плоскость, содержащую правую стену. Если
мы удалим совпадения, соответствующие этой плоскости, оставив только точки-противники
>>> not_plane = matches.outliers;

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

14.3

Разреженное стереозрение

Стереозрение – это метод оценки трехмерной структуры мира
по двум изображениям, снятым с разных ракурсов, как, например, показано на рис. 14.15. Человеческие глаза разнесены на
50–80 мм, и разница между этими двумя точками зрения является важной, но не единственной частью того, как мы ощущаем расстояние. В этом разделе рассматривается разреженное
стереозрение – естественное продолжение сопоставления признаков, которое восстанавливает мировую координату (X, Y, Z)
для каждой соответствующей пары точек. Плотное стереозре-

836 Глава 14 · Использование нескольких изображений

ние пытается восстановить мировую координату (X, Y, Z) для
каждого пикселя изображения.

14.3.1

Трехмерная триангуляция

Чтобы проиллюстрировать разреженное стереозрение, вернемся к паре изображений, показанных на рис. 14.15. Мы уже
на­шли признаки SIFT и установили возможные соответствия
между ними. Теперь найдем фундаментальную мат­ри­цу
>>> matches = sf_l.match(sf_r)
2796 matches
>>> F, resid = matches.estimate(CentralCamera.points2F,
...
confidence=0.99, seed=0);

которая отражает относительную геометрию двух ракурсов,
и сохраним только множество сторонников:
>>> matches = matches.inliers # keep only the inliers
2459 matches, with 2459 (100.0%) inliers

Мы можем отобразить эпиполярные линии для подмножест­
ва точек правого изображения, наложенных на левое:
>>> camera = CentralCamera();
>>> camera.disp(walls_l);
>>> camera.plot_epiline(F.T, matches.subset(40).p2, "yellow");

как показано на рис. 14.16. В этом случае эпиполярные линии
приблизительно горизонтальны и параллельны, что ожидаемо для движения камеры, которое представляет собой чистое
перемещение в направлении x. На рис. 14.17 показана эпиполярная геометрическая схема стереозрения. Ясно, что, по мере
того как точки удаляются от камеры, сопряженные точки P и P¢
на правом изображении перемещаются вправо вдоль эпиполярной линии.
Каждая из точек, 1p и 2p, определяет луч в пространстве. Эти
лучи пересекаются в мировой точке P. Однако для определения
этих лучей нам нужно знать два положения камеры и ее внут­
ренние параметры. Мы можем принять, что система координат первой камеры {1} является опорной, но положение второй
камеры остается неизвестным. Однако мы могли бы найти ее
положение, разложив основную мат­ри­цу, вычисленную для
перехода между двумя ракурсами. У нас есть фундаментальная
мат­ри­ца, но для определения существенной мат­ри­цы по уравнению (14.10) нам нужны внутренние параметры камеры. Немного поискав, мы можем их найти!

Разреженное стереозрение 837

v (пиксели)

перспектива

u (пиксели)

Рис. 14.16. Изображение рис. 14.15а с наложенными эпиполярными линиями для
подмножества точек правого изображения

эпиполярная
плоскость
плос

кост

ь из

обра

жен

ия

ть

эпиполярная п
линия

ос
лоск

я

ени

раж

изоб

эпиполярная
точка

Рис. 14.17. Эпиполярная геометрическая схема стереозрения. Мы можем ясно видеть, что
по мере увеличения глубины мировой точки от P до P¢ проекция перемещается вдоль
эпиполярной линии во второй плоскости изображения

Фокусное расстояние, использованное при съемке, сохраняется в метаданных изображения, как обсуждалось в разделе 11.1.1, и имеет следующее значение:
>>> f = walls_l.metadata("FocalLength")
4.15

выраженное в мм. Размеры пикселей ρw ×ρh не включены в заголовок изображения, но некоторые данные об этой модели камеры, найденные в интернете

838 Глава 14 · Использование нескольких изображений
>>> name = walls_l.metadata("Model")
'iPhone 5s'

свидетельствуют, что эта камера имеет фотомат­ри­цу с пикселями 1.5 мкм. Создадим объект CentralCamera на основе известного
фокусного расстояния, размера пикселя и размера изображения :
>>> camera = CentralCamera(name=name, imagesize=walls_l.shape,
...
f=f/1000, rho=2*1.5e-6)
Name: iPhone 5s [CentralCamera]
pixel size: 3e-06 x 3e-06
image size: 1224 x 1632
pose: t = 0, 0, 0; rpy/yxz = 0°, 0°, 0°
principal pt: [
612
816]
focal length: [ 0.00415 0.00415]

В отсутствие либо другой информации предположим, что
главная точка находится в центре изображения.
Существенная мат­ри­ца получается путем применения внут­
ренних параметров камеры к фундаментальной мат­ри­це:
>>> E = camera.E(F)
array([[-0.09511, -8.169,
[ 7.763,
1.671,
[ 2.583,
42.8,

-2.394],
-41.98],
1.11]])

а затем мы выполняем ее декомпозицию, чтобы определить
движение камеры:
>>> T_1_2 = camera.decomposeE(E, [0, 0, 10]);
>>> T_1_2.printline(orient="camera")
t = -0.982, 0.0639, -0.179; rpy/yxz = -0.39°, 1.92°, 0.474°

Мы выбрали тестовую точку P с координатами (0, 0, 10) –
удаленную от камеры и лежащую на оптической оси, – чтобы
определить правильное решение для относительного движения
камеры. Ориентация камеры поддерживалась максимально постоянной между двумя точками съемки, поэтому углы крена, тангажа и рыскания должны быть близки к нулю – предполагаемый
угол поворота составляет менее двух градусов вокруг любой оси.
Предполагаемое смещение, перенос t из {1} в {2} имеет неизвестный масштабный коэффициент. Поэтому снова внесем дополнительную информацию – когда мы делали снимки, камера
сместилась примерно на 0.3 м в положительном направлении
оси x. Вычисленное смещение имеет правильное направление,
преобладающее движение по оси x, но величина совершенно
неверна. Поэтому мы масштабируем перенос
>>> t = T_1_2.t;
>>> s = 0.3 / t[0] # оценка коэффициента масштабирования смещения

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

Разреженное стереозрение 839
-0.3056
>>> T_1_2.t = s * t # масштабированное смещение
>>> T_1_2.printline(orient="camera")
t = 0.3, -0.0195, 0.0547; rpy/yxz = -0.39°, 1.92°, 0.474°

и у нас есть оценка 1ξ2 – относительное положение первой камеры по отношению ко второй, представленное как мат­ри­ца
SE(3).
Каждая точка p на плоскости изображения соответствует
лучу в пространстве (см. отступление 13.7), который иногда называют ракселем и представляют в виде трехмерной прямой.
В первой паре соответствующих точек луч из первой камеры
определяется как
>>> ray1 = camera.ray(matches[0].p1)
{ 0 0 0; -0.027349 -0.4743 1}

экземпляр объекта Line3, представленный в координатах Плюккера. Соответствующий луч от второй камеры
>>> ray2 = camera.ray(matches[0].p2, pose=T_1_2)
{ -0.0074528 0.30579 0.15004; -0.18397 -0.48815 0.98576}
Метод closest_to_
line возвращает
точку на ray1,
ближайшую к ray2.

задает относительное положение второй камеры, которое мы
только что оценили. Два луча пересекаются в ◄
>>> P, e = ray1.closest_to_line(ray2);
>>> P
array([-0.05306, -0.9203, 1.94])

Это точка с координатой z (глубиной) почти 3 м. Из-за ошибок в оценке положения второй камеры два луча на самом деле
не пересекаются, но возвращается их ближайшая точка. В ближайшей точке
>>> e
0.02978
Даже небольшие лучи находятся на расстоянии почти в 30 мм друг от друга.
ошибки в оценке Учитывая отсутствие строгости в этом упражнении: всего два
ориентации камеры
приведут к большим снимка с ручной камеры и только приблизительное знание
ошибкам в оценке величины смещения камеры, восстановленную информацию
координат точки о глубине можно считать весьма точной.
пересечения лучей
Аналогичные вычисления можно произвести для всех сопозамыкания, вплоть
до нескольких ставленных точек, создав набор трехмерных линий в мировом
метров. Ошибка, пространстве для каждой камеры:
наблюдаемая здесь,
была бы вызвана >>> ray1 = camera.ray(matches.p1);
ошибкой в оценке
вращения менее >>> ray2 = camera.ray(matches.p2, pose=T_1_2);
чем на 1°.

где возвращаемые значения – объекты Line3, каждый из которых содержит несколько линий:

840 Глава 14 · Использование нескольких изображений
>>> len(ray1)
2459

Затем найдем ближайшую точку пересечения для каждой линии в r1 с соответствующей линией в r2:
>>> P, e = ray1.closest_to_line(ray2);
>>> P.shape
(3, 2459)

где P – массив ближайших точек, по одной в каждом столбце,
а последняя строка
>>> z = P[2, :];
>>> z.mean()
2.116

определяет координату глубины. Элементы массива e содержат
расстояние между линиями в их ближайших точках, и некоторые простые статистические данные
>>> np.median(e)
0.02008
>>> e.max()
0.1383

показывают, что медианная ошибка составляет порядка нескольких сантиметров, что очень мало по сравнению с масштабом сцены. Однако существуют некоторые существенные выбросы, о чем свидетельствует максимальное значение ошибки.
Мы можем построить эти точки с помощью Matplotlib
>>> plotvol3();
>>> plt.plot(P[0,:], P[1,:], P[2,:], '.', markersize=2);

но результат получится довольно грубый. Гораздо лучшая альтернатива – создать объект PointCloud ►
>>> walls_pcd = PointCloud(P)
PointCloud with 2459 points.
>>> walls_pcd.transform(SE3.Rx(pi)); # направляем ось y вверх

и отобразить его
>>> walls_pcd.disp()

Результат показан на рис. 14.18a. Диаграмма с облаком точек – интерактивная, что позволяет управлять направлением
обзора и масштабированием. Нажмите клавишу h, чтобы вывести список поддерживаемых комбинаций клавиш.
На диаграмме можно видеть несколько очевидных точек-выбросов, характеризующихся малым количеством соседей. Мы

Для этого необходимо установить
пакет Open3D, см.
приложение A.

Разреженное стереозрение 841

можем отбросить все точки, которые содержат менее 10 точек
внутри сферы с радиусом 20 см
>>> walls_pcd = walls_pcd.remove_outlier(nb_points=10, radius=0.2)
PointCloud with 2341 points.

и получить копию исходного облака точек, из которой удалено
более 100 точек.

https://go.sn.
pub/3XlqAO
а
б
Рис. 14.18. Облака точек, полученные на основе пары изображений на рис. 14.15: а) облако
точек, цвет которых указывает на глубину (точки более теплых тонов находятся ближе),
обратите внимание на некоторые неправильно удаленные (темно-синие) точки; б) цветное
облако точек

Для большего реализма можно задать цвет точек из исходного левого RGB-изображения
>>>
>>>
...
>>>
>>>
Умножение объекта
PointCloud слева на
объект SE3 преобразует точки в соответствии с ξ · Pi.

colors = []
for m in matches:
colors.append(walls_l.image[int(m.p1[1]), int(m.p1[0]), :])
pcd = SE3.Rx(pi) * PointCloud(P, colors=np.array(colors).T)
pcd.disp()

и создать так называемое цветное облако точек, показанное на
рис. 14.18б. Более подробно мы обсудим облака точек в разделе 14.7.
Это пример системы стереопсиса, которая использует информацию из двух перекрывающихся изображений, чтобы сделать
вывод о трехмерном положении точек в мире. По понятным
причинам используемый здесь подход называется разреженным стереозрением, поскольку мы вычисляем расстояние только для крошечного подмножества пикселей изображения. Чаще
всего относительная разница положений между камерами, как
и внутренние параметры камеры, известна заранее.

14.3.2

Пакетная подстройка

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

842 Глава 14 · Использование нескольких изображений

ложении двух камер относительно друг друга. Чтобы оценить
качество нашего решения, можно выполнить обратную проек­
цию (перепроецирование) предполагаемых объемных точек на
плоскость изображения на основе предполагаемых положений
камеры и известной модели камеры. Ошибка обратной проекции – это расстояние в плоскости изображения между прое­
цируемой обратно точкой и ее наблюдаемым положением на
плоскости изображения.
Для предыдущего примера обратная проекция будет сле­
дующей:
>>> p1_reproj = camera.project_point(P[:, 0]);
>>> p2_reproj = camera.project_point(P[:, 0], pose=T_1_2);

а результирующие ошибки плоскости изображения:
>>> (p1_reproj - matches[0].p1).T
array([[
0,
0]])
>>> (p2_reproj - matches[0].p2).T
array([[ -1.154,
23.49]])

Точка в первой камере имеет нулевую ошибку, поскольку
лежит на луче, определяемом этой точкой плоскости изображения, но для второй камеры ошибка составляет более 20 пикселей. Это означает, что имеется ошибка в оценке точки мира,
в оценке положения второй камеры 2 или в обеих этих оценках.
Чтобы уменьшить эту ошибку, можно использовать метод, называемый пакетной подстройкой.
Пакетная подстройка (bundle adjustment) – это процесс оптимизации, при котором одновременно корректируются положения камер и координаты точек, чтобы свести к минимуму
общую ошибку обратной проекции. Он использует двухмерные
измерения из набора изображений одной и той же сцены для
восстановления информации, связанной с трехмерной геомет­
рией отображаемой сцены, а также с расположением и оптическими характеристиками камер. Этот алгоритм также называется «Структура из движения» (Structure from Motion, SfM)
или «Вычисление структуры и движения» (Structure and Motion
Estimation, SaM) – структура представляет собой трехмерные
точечные ориентиры в мире, а движение представляет собой
последовательность положений камеры. Этот метод также называется «визуальным SLAM» (visual SLAM, VSLAM), поскольку
он очень похож на задачу SLAM на основе графа положений, обсуждаемую в разделе 6.5, – задачу, решаемую в трех измерениях
SE(2), тогда как пакетная подстройка включает в себя положения камеры в SE(3) и точки в ℝ3.
Для формализации задачи рассмотрим камеру с известными внутренними параметрами в N различных положениях ξi ∈
SE(3), i = 0, ..., N - 1, и набор из M точек-ориентиров Pj, представ-

Разреженное стереозрение 843

ленных координатными векторами Pj ∈ ℝ3, j = 0, ..., M - 1. Камера
в положении ξi наблюдает Pj, а измеренная проекция плоскости
изображения pj представлена координатным вектором ipj# ∈ ℝ2.
Обозначения для двух камер показаны на рис. 14.19.

плос

кост

ь из

обра

жен

ия

пло

с

ь
кост

я

ени

раж

изоб

Рис. 14.19. Обозначения пакетной подстройки (bundle adjustment) на примере
простой задачи, состоящей только из двух камер и трех мировых точек. Отображаются
предполагаемые положения камеры и положения точек, а также предполагаемые
и измеренные координаты плоскости изображения. Ошибки перепроецирования показаны
пунктирными серыми линиями. Задача считается решенной, когда переменные настроены
так, что суммарная ошибка перепроецирования оказывается минимальной
Информация
о видимости также
может быть представлена матрицей
видимости, массивом N×M, где элемент (i, j ) равен 1,
если ориентир j
виден с камеры i,
и 0 – если не виден.

В общем, с любой камеры можно увидеть только часть ориентиров, и эту информацию о видимости можно элегантно представить с помощью графа, как показано на рис. 14.20, где каждое
положение камеры ξi и каждая точка-ориентир является вершиной. Ребра между камерами и точками-ориентирами представляют наблюдения, а значения ребер – наблюдаемые координаты в плоскости изображения. Оценочное значение проекции
ориентира j на плоскость изображения камеры i равно
p̂ j = 𝒫(ξˆi, P̂j; K),

i

а ошибка перепроецирования определяется как разность между оцененной и наблюдаемой проекцией – ip̂ j - ipj#.
Используя Toolbox, начнем с создания объекта BundleAdjust
В данном случае мы
просто оцениваем >>> bundle = BundleAdjust(camera)
относительное
Передадим ему внутреннюю модель камеры,
положение 1ξ2, но
можем рассматри- предполагается, известна. Затем добавим в граф
вать положение
первой камеры как положений камеры :
опорную систему
координат ξ1 = 0 >>> view0 = bundle.add_view(SE3(), fixed=True);
и ξ2 = 1ξ2. >>> view1 = bundle.add_view(SE3.Tx(0.3));

которая, как
оценки двух

844 Глава 14 · Использование нескольких изображений
точки-ориентиры в 3D

вид

положения камер в SE(3)
Рис. 14.20. Простой граф видимости, содержащий узлы камеры (красные) и узлы ориентиров
(синие). Линии, соединяющие узлы, представляют вид этого узла на данной камере, а значение
ребра – это наблюдаемая координата плоскости изображения. Ориентир P0 здесь наблюдается
только одной камерой, ориентиры P1 и P3 – двумя, а ориентир P3 – тремя камерами

Укажем, что положение первой камеры известно и что его не
следует оптимизировать. Предполагаемое положение второй
камеры получено в разделе 14.3.1 из существенной мат­ри­цы,
но в этом примере мы используем более грубое приближение,
чтобы проиллюстрировать возможности пакетной подстройки.
Метод add_view возвращает экземпляр ViewPoint, описывающий
конкретную вершину положения камеры в графе пакетной подстройки, который мы будем использовать далее.
Далее для подмножества ориентиров, вычисленных ранее
с помощью триангуляции и соответствия признаков:
>>> for (Pj, mj) in zip(P[:, ::4].T, matches[::4]):
... landmark = bundle.add_landmark(Pj)
# добавить вершину
... bundle.add_projection(view0, landmark, mj.p1) # добавить ребро
... bundle.add_projection(view1, landmark, mj.p2) # добавить ребро

создадим вершину-ориентир (экземпляр LandMarkin), а затем
добавим измерения, указывая вершину камеры, вершину ориентира и ее проекцию на плоскость изображения. Теперь задача полностью определена, и можно отобразить ее сводку:
>>> bundle
Bundle adjustment problem: 2 views
1 locked views: [0]
615 landmarks
1230 projections
1857 total states
1851 variable states
2460 equations
landmarks per view: min=615, max=615, avg=615.0
views per landmark: min=2, max=2, avg=2.0

Граф, содержащийся в объекте BundleAdjust, можно построить
с помощью
>>> bundle.plot()

как показано на рис. 14.21.

Z (м)

Разреженное стереозрение 845

м)

Y(

Рис. 14.21. Задача пакетной
подстройки для подмножества
точек показана в виде встроенного
графа. Синие точки представляют
местоположения ориентиров, значки
камеры обозначают положение
камер, а линии обозначают
направления наблюдения. Камера 1
синяя, а камера 2 красная

Х (м)

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

Свойство двойного
покрытия единичных кватернионов
означает, что любой
единичный
кватернион можно
записать с неотрицательной скалярной составляющей.
По определению
единичный кватернион имеет единичную норму, поэтому
скалярную состав­
ляющую можно
легко восстановить
через компоненты
вектора:
s = 1 – vx2 – vy2 – vz2 ,
как показано
в разделе 2.3.7.1.

x = {ξ0, ξ1, … ξN-1 | P0, P1, ..., PM-1} ∈ ℝ6N+3M,

где положение камеры SE(3) представлено в векторном формате ξi ∼ (t, r) ∈ ℝ6, включающем перемещение t ∈ ℝ3 и вращение
r ∈ ℝ3; и Pj ∈ ℝ3. К возможным вариантам представления вращения относятся углы Эйлера, углы крена-тангажа-рыскания,
угловые оси или экспоненциальные представления координат.
Для пакетной подстройки обычно используется векторная компонента единичного кватерниона, которая имеет только три
параметра, и мы используем это представление.
Количество неизвестных в этой системе равно 6N + 3M: шесть
неизвестных для каждого положения камеры и три неизвестных для положения каждой точки-ориентира. При этом у нас
есть до 2NM уравнений из-за измеренных проекций точек на
плоскости изображения. Обычно предполагается, что положение одной камеры является опорной системой координат, и это
уменьшает количество неизвестных до 6 (N - 1) + 3M.
В данной задаче N = 2, но положение одной камеры фиксировано, а M = 615, поэтому имеем 6 × (2 − 1) + 3 × 615 = 1857 неизвестных и 2 × 2 × 615 = 2460 уравнений – переопределенная система уравнений, для которой должно существовать решение.
Для нашей задачи можно извлечь вектор состояния

846 Глава 14 · Использование нескольких изображений
>>> x = bundle.getstate();
>>> x.shape
(1857,)

который включает положение фиксированной камеры, хотя
оно останется постоянным. Положение второй камеры хранится во втором блоке из шести элементов:
>>> x[6:12]
array([
0.3,

0,

0,

0,

0,

0])

как перемещение с последующим вращением, а первый ориентир сохраняется в:
>>> x[12:15]
array([-0.05306, -0.9203,

1.94])

Пакетная подстройка – это задача минимизации, определяю­
щая положения камер и ориентиров, которые минимизируют
ошибку перепроецирования по всем ребрам:

где Fk(·) > 0 – неотрицательная скалярная стоимость, связанная
с ребром k графа, связывающим камеру i с ориентиром j. Ошибка перепроецирования ориентира в точке Pj на камеру в положении ξi равна
fk(x) = 𝒫(ξˆi, P̂j; K) - ipj# ∈ ℝ2,

а скалярная стоимость – это квадрат евклидовой ошибки перепроецирования:
Fk(x) = fkT(x)fk(x).
Несмотря на то что она записана как функция всего вектора
состояния, Fk(x) зависит только от двух элементов этого вектора: ξi и Pj, где i и j – вершины, соединенные ребром k. Общую
ошибку, сумму квадратов ошибок обратной проекции для всех
ребер, можно вычислить для любого значения вектора состояния и для начальных условий как
>>> bundle.errors(x)
1.407e+06

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

Разреженное стереозрение 847

Первым шагом в решении этого класса задач является линеа­
ризация. Ошибка перепроецирования fk(x) может быть линеаризована относительно текущего состояния x0 системы:
fk¢(Δ) ≈ f0,k + JkΔ,
где f0,k = fk(x0) и

Линеаризация
и мат­ри­цы Якоби
обсуждаются
в приложении E,
а решение разреженных нелинейных
уравнений – в разделе F.2.4.

являются мат­ри­цей Якоби, ◄ которая зависит только от положения камеры ξi и положения ориентира Pj, поэтому в основном
состоит из нулей:
Jk = (02×6  Ai  Bj  02×3), где

Класс CentralCamera имеет метод для вычисления двух якобианов и проекции изображения на плоскость за один вызов
>>> p, A, B = camera.derivatives(t, r, P);
Структура мат­ри­цы
Якоби Ai специфична для выбранного
представления положения камеры r.
Якобианы, особенно
Ai, довольно сложно
выводятся аналитически, но могут
быть автоматически
сгенерированы
с помощью SymPy
и скрипта symbolic/
bundle-adjust.py
в Machine Vision
Toolbox.

где t и r – это положение камеры в виде векторов перемещения
и вращения, а P – это вектор координат ориентира. Перемещение камеры на d или перемещение ориентира на –d оказывают эквивалентное воздействие на изображение, поэтому Bj
является отрицательным значением первых трех столбцов Aj.
Теперь, когда все компоненты на своих местах, можно решить задачу пакетной подстройки:
>>> x_new, resid = bundle.optimize(x);
Bundle adjustment cost 1.41e+06 -- initial
Bundle adjustment cost 868 (solved in 2.46
Bundle adjustment cost 238 (solved in 2.44
Bundle adjustment cost 238 (solved in 2.46
Bundle adjustment cost 238 (solved in 2.46
Bundle adjustment cost 238 (solved in 2.46
* 5 iterations in 13.0 seconds
* Final RMS error is 0.44 pixels

sec)
sec)
sec)
sec)
sec)

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

848 Глава 14 · Использование нескольких изображений

были сняты с помощью камеры телефона, и мы полностью проигнорировали искажения объектива.
Результатом является новый вектор состояния с обновленными положениями камер и ориентиров, которые можно использовать для обновления состояния всех вершин точек обзора и ориентиров в графе
>>> bundle.setstate(x_new);

получить уточненное положение камеры
>>> bundle.views[1].pose.printline(orient="camera")
t = 0.31, -0.0268, 0.074; rpy/yxz = -0.585°, 1.25°, 0.336°

и сравнить со значением из разложения существенной мат­ри­
цы, которое было
>>> T_1_2.printline(orient="camera")
t = 0.3, -0.0195, 0.0547; rpy/yxz = -0.39°, 1.92°, 0.474°

Уточненная оценка первого ориентира:
>>> bundle.landmarks[0].P array([-0.05306, -0.9203, 1.94])

Хотя общая среднеквадратическая ошибка невелика, мы можем более подробно рассмотреть окончательную ошибку перепроецирования:
>>> e = np.sqrt(bundle.getresidual());
>>> e.shape
(2, 615)

где элемент (i, j) – ошибка перепроецирования в пикселях для
камеры i и ориентира j. Медианная ошибка для первой и второй
камер
>>> np.median(e, axis=1)
array([ 0.2741, 0.2625])

около четверти пикселя, а максимальные ошибки
>>> np.max(e, axis=1)
array([ 1.614,
1.61])

меньше двух пикселей.
Пакетная подстройка находит оптимальные относитель­
ные положение и позицию, а не абсолютные. Например, если
все камеры и ориентиры переместятся на 1 м в направлении
x, то общая ошибка перепроецирования не изменится. Чтобы
исправить эту неоднозначность, можно зафиксировать одну
или несколько камер или ориентиров – в этом примере мы зафиксировали первую камеру. Значения фиксированных пози-

Разреженное стереозрение 849

ций и положений сохраняются в векторе состояния, но они не
обновляются во время итераций – их якобианы не нужно вычислять, а мат­ри­ца Гессе, используемая для решения обновления на каждой итерации, становится меньше, поскольку строки
и столбцы, соответствующие этим фиксированным парамет­
рам, можно удалить.
Фундаментальная проблема неоднозначности масштаба монокулярных камер, обсуждавшаяся в разделе 14.2.2, актуальна
и здесь. Если изменить размер объектов мира и соответственно
масштабировать изображение камеры, то наблюдаемая модель
мира будет неотличима от реальности. Говоря более формально, если бы вся задача была масштабирована так, что Pj¢ = λPj,
[ξi¢] = λ[ξi]t и λ ≠ 0, общая ошибка повторного проецирования
была бы такой же. Решение, которое мы получили выше, имеет
произвольный масштаб или значение λ – изменение начального условия для положения камеры или координат ориентира
приведет к решению с другим масштабом. Мы можем исправить это, привязав положение как минимум двух камер, одной
камеры и одного ориентира или двух ориентиров.
Методика пакетной подстройки (но не эта реализация) допус­
кает ограничения между камерами. Например, многокамерная
система, перемещающаяся в пространстве, будет использовать
ограничения для обеспечения фиксированного относительного
положения камер на каждом временном шаге. Данные колесной
одометрии или инерциальных измерений можно использовать
для ограничения расстояния между системами координат камер для обеспечения правильного масштаба, или, как вариант,
данные об ориентации от IMU можно использовать для ограничения положения камеры. В базовом графовом представлении
задачи, как показано на рис. 14.20, это потребовало бы добавления дополнительных ребер между узлами камеры. Можно также добавить ограничения взаимного положения ориентиров,
которые имеют известное относительное положение, например
углы окна, – это потребует добавления дополнительных ребер
между соответствующими узлами ориентира.
Конкретная задача, которую мы изучали, необычна тем, что
каждая камера видит каждый ориентир. Намного чаще встречается ситуация, когда камера может перемещаться в очень
обширной среде, поэтому любая камера будет видеть только
небольшое подмножество ориентиров. В системе реального
времени может выполняться ограниченная пакетная подстройка по отношению к случайным кадрам, называемым ключевыми кадрами, и полная пакетная подстройка по всем кадрам или
всем ключевым кадрам, выполняемая с низкой скоростью в фоновом режиме.
В этом примере мы предположили, что внутренние параметры камеры известны и постоянны. Теоретически пакетная

850 Глава 14 · Использование нескольких изображений

подстройка может работать как с внутренними, так и с внешними параметрами. Мы просто добавляем дополнительные параметры для каждой камеры в вектор состояния и соответствующим образом корректируем якобиан A. Однако из-за наличия
связи между внутренними и внешними параметрами это может
привести к ухудшению точности. Если бы мы решили находить
элементы мат­ри­цы камеры (c0,0, ..., c2,2) напрямую, то вектор состояния содержал бы 11, а не 6 элементов для каждой камеры.
Если Ci – истинная камера, то пакетная подстройка сможет найти решение для произвольного линейного преобразования CiQ,
где Q ∈ ℝ4×4 – некоторая невырожденная мат­ри­ца, и соответствующие мировые точки Q−1P̃ j, где P̃ j – их истинные значения.
К счастью, проекционные мат­ри­цы реальных камер имеют четко определенную структуру (13.10) и свойства и обеспечивают
ограничения, которые позволяют оценить Q. Нахождение произвольной Ci называется проективной реконструкцией (projective reconstruction). Она может быть улучшена до аффинной
реконструкции (с использованием аффинной модели камеры)
или метрической реконструкции (с использованием модели
перспективной камеры) путем соответствующего выбора Q.

14.4

Плотное стереозрение

Стереопара чаще всего снимается одновременно двумя камерами, как правило, с параллельными оптическими осями и разнесенными на известное расстояние, называемое съемочным
базисом камеры. На рис. 14.22 показана типичная система
стереокамер, которая делает сразу два снимка и передает их
на главный компьютер для обработки. Стереокамеры широко
используются в самоходной робототехнике (см. рис. 1.5a, 1.9
и 4.16).
Для иллюстрации загрузим левое и правое изображения, составляющие стереопару:
>>> rocks_l = Image.Read("rocks2-l.png", reduce=2)
Image: 638 x 555 (uint8), R:G:B [.../images/rocks2-l.png]
>>> rocks_r = Image.Read("rocks2-r.png", reduce=2)
Image: 638 x 555 (uint8), R:G:B [.../images/rocks2-r.png]

Мы можем отобразить эти два изображения рядом друг
с другом:
>>> rocks_l.stdisp(rocks_r)

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

Матрица камеры
имеет произвольный масштабный
коэффициент. Изменения фокусного
расстояния и перемещения по оси z
дают такие же эффекты на плоскости
изображения, как
и изменения главной точки и перемещения камеры по
осям x и y.

Плотное стереозрение 851

v (пиксели)

v (пиксели)

Рис. 14.22. Стереокамеры, вычисляющие несоответствие на борту с помощью аппаратного
обеспечения ПЛИС. Показаны базовые линии камер 100 и 250 мм (изображение
предоставлено Nerian Vision GmbH)

u (пиксели)

u (пиксели)

Рис. 14.23. Окно просмотра изображений stdisp. Черное перекрестие на левом изображении расположено
в правом верхнем углу цифры «5» на камне на переднем плане. Другое черное перекрестие автоматически
размещается в той же координате на правом изображении. Щелчок по соответствующей точке на правом
изображении устанавливает зеленое перекрестие, а панель вверху показывает сдвиг по горизонтали на 73.18
пикселя влево. Эта пара стереоизображений взята из стереоскопической базы данных Миддлбери (Scharstein and
Pal, 2007). Фокусное расстояние f/ρ составляет 3740 пикселей, а съемочный базис – 160 мм. Изображения были
обрезаны таким образом, что фактическое несовпадение должно быть компенсировано на 274 пикселя

и ту же координату на правом изображении. Щелчок на правом
изображении устанавливает другое вертикальное перекрестие
и отображает разницу между горизонтальными координатами
двух перекрестий. Перекрестие, как показано, установлено на
цифру «5», написанную на одном из камней переднего плана,
и мы наблюдаем несколько вещей. Во-первых, оптическая точка (spot) имеет одинаковую вертикальную координату на обоих
изображениях, а это означает, что эпиполярные линии горизонтальны. Во-вторых, на правом изображении точка сместилась влево на 73.18 пикселя. Если бы мы исследовали больше
точек, мы бы увидели, что несоответствие уменьшается для точек, которые находятся дальше от камеры.

852 Глава 14 · Использование нескольких изображений

Как показано на рис. 14.17, сопряженная точка на правом
изображении перемещается вправо вдоль эпиполярной линии
по мере увеличения глубины. Для геометрической схемы камер
с параллельными осями эпиполярные линии параллельны и горизонтальны, поэтому сопряженные точки имеют одинаковую
v-координату. Если координаты двух сопряженных точек равны
(Lu, Lv) и (Ru, Rv), тогда Rv = Lv. Смещение вдоль горизонтальной
эпиполярной линии d = Lu − Ru, где d ≥ 0, называется диспаратностью (disparity).
Процесс плотного стереосовмещения показан на рис. 14.24.
Если взять пиксель в точке (Lu, Lv) на левом изображении, мы
знаем, что соответствующий ему пиксель находится в некоторой координате (Lu − d, Lv) на правом изображении, где d ∈ [dmin,
..., dmax]. Чтобы надежно найти соответствующую точку для пикселя на левом изображении, мы создаем шаблонную область T
размером N×N пикселей вокруг этого пикселя. Мы перемещаем
окно шаблона горизонтально по правому изображению. Позиция, в которой шаблон наиболее похож, считается соответствующей точкой, от которой рассчитывается диспаратность.
По сравнению с задачей сопоставления, которую мы обсуждали в разделе 11.5.2, здесь ситуация намного проще, потому что
между двумя изображениями нет изменения относительного
масштаба или ориентации.

Рис. 14.24. Стереосовмещение. Окно поиска на правом изображении перемещается влево
вдоль эпиполярной линии v = Lv, начиная с u = Lu, пока не совпадет с окном шаблона T на
левом изображении

Эпиполярное ограничение означает, что достаточно выполнить одномерный поиск соответствующей точки. Шаблон
перемещается на D шагов по одному пикселю в диапазоне dmin
⋯ dmax. В каждой позиции шаблона мы выполняем операцию
сопоставления с шаблоном, как обсуждалось в разделе 11.5.2,
и для шаблона N×N вычислительные затраты составят O(N 2).
Для изображения размером W×H общая вычислительная стоимость плотного стереосопоставления составляет O(DWHN 2), что

Плотное стереозрение 853

довольно много, но осуществимо в режиме реального времени при использовании оптимизированных алгоритмов, набора
инструкций SIMD и, возможно, GPU.
Выполнить стереосовмещение пары изображений на рис. 14.23
с помощью Toolbox довольно просто :

Это простая
реализация
на чистом Python
приводится только
>>> disparity, *_ = rocks_l.stereo_simple(rocks_r, hw=3,
для демонстрации.

...

drange=[40, 90]);

Результатом является мат­ри­ца D того же размера, что
и rocks_l, каждый элемент du,v которой представляет собой пиксель (u, v) на левом изображении. Соответствующий пиксель на
правом изображении будет в точке (u − du,v, v). Мы можем отоб­
разить диспаратность как изображение
>>> disparity.disp(colorbar=True);

Диспаратность (пиксели)

v (пиксели)

Мы могли бы
выбрать такой диапазон, как [0, 90], но
это увеличивает
время поиска: нужно будет оценивать
91 несоответствие
вместо 51. Это
также увеличивает
вероятность ошибок
сопоставления.

Пример такого изображения показан на рис. 14.25. Изображения диспаратности имеют характерный призрачный вид, поскольку все цвета и текстуры поверхности отсутствуют. Вторым
аргументом функции является stereo_simple – это полуширина
шаблона, в данном случае мы используем окно 7 × 7. Третий аргумент – диапазон искомых диспаратностей, в данном случае
от 40 до 90 пикселей, поэтому значения пикселей в изображении диспаратности лежат в диапазоне [40, 90]. Диапазон диспаратности получен путем изучения некоторых дальних и близких точек с помощью stdisp. Для сопоставления с шаблоном
используется ZNCC, представленная в разделе 11.5.2.

u (пиксели)

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

854 Глава 14 · Использование нескольких изображений

На изображении диспаратности ясно видно, что камни
в нижней части кучи имеют большее несоответствие и находятся ближе к камере, чем камни в верхней части. Есть также
некоторые ошибки, такие как аномальные значения яркости
по краям некоторых камней. Эти пиксели отображаются как
находящиеся ближе, чем они есть на самом деле. Показатель
сходства устанавливается равным nan по краю изображения, где
шаблон сопоставления сходства выходит за край изображения,
и везде, где знаменатель показателя сходства ZNCC (табл. 11.1)
равен нулю. ► Значения nan отображаются красным цветом.
Метод stereo_simple возвращает также два дополнительных
значения

Такое возможно,
если все пиксели
в одном из
шаблонов имеют
одинаковое
значение.

>>> disparity, similarity, DSI = \
... rocks_l.stereo_simple(rocks_r, hw=3, drange=[40, 90])

Изображение в пространстве диспаратности (disparity-space
image, DSI) – это трехмерный массив
>>> DSI.shape
(561, 644, 50)

Он обозначается как 𝒟 и показан на рис. 14.26. Его элементы
𝒟(u, v, d) определяют сходство между шаблонами с центрами
(u, v) на левом изображении и (u - d, v) на правом. ► Изображение диспаратности, которое мы видели выше, – это просто положение максимального значения в направлении d, оцененное
для каждого пикселя.

Это большая
матрица
(144 Мбайта),
вот почему
изображения
уменьшились
в размере при
загрузке.

>>> np.argmax(DSI, axis=2);

Рис. 14.26. Изображение пространства
диспаратности (DSI) – это трехмерное
изображение, где элемент 𝒟(u, v, d)
отражает сходство между опорными
областями с центром в (Lu, Lv) на левом
изображении и (Lu − d, Lv) на правом

Изображение подобия similarity имеет тот же размер, что
и rocks_l, а значения пикселей в нем являются пиковыми оценками сходства – максимумами в d-направлении, которые оцениваются для каждого пикселя
>>> similarity_values = np.max(DSI, axis=2);

Плотное стереозрение 855

Каждый столбец в d-направлении (рис. 14.26) содержит меру
сходства/диспаратности для соответствующего пикселя на левом изображении. Для пикселя в точке (395, 281) мы можем построить график
>>> plt.plot(DSI[281, 395, :], "o-");

который показан на рис. 14.27а. Почти идеальное совпадение
обнаруживается в d - dmin = 6, а поскольку dmin = 40, то расхождение на этом пике составляет 46 пикселей.

14.4.1
Это одномерное
уточнение
положения пика
обсуждается
в разделе J.1.

Уточнение положения пиков

Диспаратность в каждом пикселе является целым значением
d = dmin, ..., dmax, при котором обнаружено наибольшее сходство.
На рис. 14.27a показан один однозначный сильный пик, и мы
можем использовать пик и соседние точки для уточнения мес­
тоположения пика. ◄ Смоделируем сходство окружения пика
как функцию диспаратности второго порядка
(14.14)

Подобие
Подобие
NCCNCC

Подобие
Подобие
NCCNCC

s = Ad 2 + Bd + C.

пиксель в точке (351, 543)
пиксель в точке (351, 543)

пиксель в точке (395, 281)
пиксель в точке (395, 281)

б

пиксель в точке (543, 173)
пиксель в точке (543, 173)

пиксель в точке (356, 17)
пиксель в точке (356, 17)

в

Диспаратность d – dmin (пиксели)
Диспаратность d – dmin (пиксели)

Подобие
Подобие
NCCNCC

Диспаратность d – dmin (пиксели)
Диспаратность d – dmin (пиксели)

Подобие
Подобие
NCCNCC

а

Диспаратность d – dmin (пиксели)
Диспаратность d – d (пиксели)

г

Диспаратность d – dmin (пиксели)
Диспаратность d – d (пиксели)

min
Рис. 14.27. Оценка сходства ZNCC вminзависимости от несоответствия в четырех выбранных точках
изображения:
а) одиночный сильный пик; б) несколько пиков; в) слабый пик; г) широкий пик

856 Глава 14 · Использование нескольких изображений

Коэффициенты (A, B, C) можно оценить по трем точкам, поэтому выберем пиковое и два соседних значения. Для меры
сходства ZNCC наилучшим совпадением является максимум,
что соответствует перевернутой параболе и A < 0. Коэффициент A будет иметь большую величину при остром пике.
Максимальное значение параболы соответствует точке, в которой ее производная равна нулю, откуда можно получить более точную оценку положения пика сходства, который возникает при несоответствии диспаратности

Мы можем вычислить уточненные значения диспаратности
и коэффициент А для каждого пикселя с помощью
>>> disparity_refined, A = Image.DSI_refine(DSI)

а

v (пиксели)

v (пиксели)

Результаты этих вычислений показаны на рис. 14.28.

u (пиксели)

б

u (пиксели)

Рис. 14.28. Уточнение положения пиков: а) значение члена A, резкость пика увеличивается с более низкими (более
темными) значениями; б) субпиксельное интерполированное значение диспаратности

14.4.2

Типичные проблемы стереосопоставления

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

14.4.2.1 Множественные пики
На рис. 14.27б показано несколько пиков практически одинаковой амплитуды, а это означает, что искомый шаблон обнаружен
несколько раз. Это происходит, когда на изображении есть ре-

Плотное стереозрение 857

гулярные вертикальные элементы, как это часто бывает в искусственных сценах: кирпичные стены, ряды окон, архитектурные элементы или забор. Проблема, показанная на рис. 14.29,
широко известна как эффект частокола (picket fence effect), или,
точнее, как пространственная неоднозначность.

Рис. 14.29. Эффект частокола. Шаблон будет хорошо сочетаться с рядом различных
диспаратностей. Эта проблема возникает в любой сцене с повторяющимися объектами
Многокамерная
стереосъемка
с использованием
более двух
камер является
мощным методом
устранения этой
неоднозначности.

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

14.4.2.2 Слабое совпадение
Слабое совпадение показано на рис. 14.27в. Обычно оно имеет
место, когда соответствующая точка сцены не видна на правом
изображении из-за окклюзии, также известной как проблема
недостающих частей. Окклюзия дана на рис. 14.30, где показано, что точка 3 видна только левой камере. Алгоритм стереосопоставления всегда будет возвращать наилучшее совпадение, поэтому если точка закрыта, он вернет диспаратность
относительно наиболее похожего, но неправильного шаблона.
Несмот­ря на то что на рисунке показана несколько преувеличенная ситуация, реальные изображения страдают от этой проблемы, когда глубина быстро меняется. В нашем примере это
происходит на краях скал, именно там мы наблюдаем неверные
несоответствия на рис. 14.25. Проблема становится более рас-

858 Глава 14 · Использование нескольких изображений

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

Рис. 14.30. Окклюзия в стереозрении. Поле зрения двух камер показано цветными секторами.
Точки 1 и 7 выходят за пределы перекрывающейся области обзора и видны только одной
камере. Точка 5 закрыта от левой камеры, а точка 3 – от правой. Перечень точек, видимых
каждой камерой, указан под ней

Проблему окклюзии нельзя устранить, но ее можно хотя бы
обнаружить. Самый простой способ – рассмотреть возвращаемую оценку сходства
>>> similarity.disp();

Как показано на рис. 14.31а, ошибочные значения диспаратности соответствуют низким показателям подобия. Результаты
несоответствия, когда сходство низкое, можно отбросить:
>>> similarity.choose("blue", similarity < 0.6).disp();

Это показано на рис. 14.31б, где пиксели с подобием s < 0.7
отображаются синим цветом. Распределение максимальных
оценок сходства
>>> plt.hist(similarity.view1d(), 100, (0, 1), cumulative=True,
...
density=True);

показано на рис. 14.32. Мы видим, что только 5 % пикселей имеют показатель сходства менее 0.6 и более половины пикселей
имеют показатель сходства выше 0.9. Однако некоторые из этих
пикселей могут страдать проблемой множественных пиков.
Простой, но эффективный способ проверить наличие окклюзии – выполнить сопоставление в двух направлениях – провер-

v (пиксели)

v (пиксели)

Подобие ZNCC

Плотное стереозрение 859

Интегральное распределение

u (пиксели)
u (пиксели)
а
б
Рис. 14.31. Сходство стереошаблона: а) изображение подобия, где более яркие пиксели означают большее
сходство, а красным цветом отмечены значения nan там, где не удалось вычислить диспаратность; б) изображение
диспаратности с пикселями, имеющими низкий показатель сходства, отмеченными синим цветом

Подобие ZNCC

Рис. 14.32. Интегральная вероятность оценок ZNCC. Вероятность сходства выше 0.9
составляет ~45 %

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

860 Глава 14 · Использование нескольких изображений

например, точка 1 находится вне поля зрения правой камеры.
Это является причиной большого количества неправильных
совпадений в левой части изображения диспаратности на
рис. 14.25. Обычно с этой проблемой борются отбрасыванием
крайних левых столбцов dmax (в данном случае 90) из изображения диспаратности.

14.4.2.3 Широкие пики
Последняя проблема, которая может возникнуть, – это функция
подобия с очень широким пиком, как показано на рис. 14.24г.
Широкий пик затрудняет точную оценку максимумов. Обычно
это происходит, когда область шаблона очень слабо текстурирована, например соответствует небу, темным теням, слоям
воды, снегу, льду или гладким искусственным объектам. Проще
говоря, в области, полностью окрашенной в серый цвет, серый
шаблон одинаково хорошо сочетается с любым количеством
областей-кандидатов серого цвета.
Один из подходов к обнаружению этой проблемы – применение порогового значения к четкости пика сходства |A|. Другой
подход – количественная оценка изменчивости значений пикселей в шаблоне с использованием таких показателей, как разница между максимальным и минимальным значениями, или
дисперсия значений пикселей. Если шаблон имеет слишком
маленькую дисперсию, вряд ли это приведет к сильному пику.
При анализе снимков, сделанных в помещениях, где часто
встречаются поверхности без текстуры, можно спроецировать
на них искусственную текстуру с помощью инфракрасного
проектора. Этот подход используется в камерах Intel RealSense
D400.

14.4.2.4 Количественная оценка видов проблем
Как уже было сказано, в описанных выше проблемных ситуациях нельзя определить диспаратность, но можно выявить наличие проблемы. Это важно, так как позволяет пометить соответствующие пиксели как не имеющие известного диапазона,
и это позволяет роботу осторожно относиться к областям, трехмерную структуру которых нельзя надежно определить, и не
предполагать, что это пространство свободно.
Мы можем использовать ряд простых метрик для маркировки элементов изображения диспаратности как недействительных или ненадежных. Сначала создадим массив status того же
размера, что и disparity, и инициализируем его единицами:
>>> status = np.ones(disparity.shape);

Плотное стереозрение 861

Элементам задаются различные значения, если они соответствуют определенным признакам наличия проблем:
>>> U, V = disparity.meshgrid()
>>> status[np.isnan(disparity.image)] = 5 #
#
>>> status[U <= 90] = 2
#
>>> status[similarity.image < 0.6] = 3
#
>>> status[A.image >= -0.1] = 4
#

сходство не может быть
вычислено
нет перекрытия
слабое совпадение
широкий пик

Эту мат­ри­цу можно отобразить как изображение
>>> plt.imshow(status);

как показано на рис. 14.33 с цветовой легендой справа. Проб­
лемных пикселей получилось довольно много, но светло-зеленых пикселей оказалось намного больше. Фактически
>>> (status == 1).sum() / status.size * 100
77.01

почти 80 % значений диспаратности проходят нашу серию тес­
тов качества. Синие пиксели, указывающие на слабое сходство,
появляются по краям камней и возникают из-за окклюзии.
Оранжевые пиксели, указывающие на широкий пик, встречаются в достаточно гладких областях, либо в глубоких тенях между камнями, либо на фоне, который не является камнем.

NaN

v (пиксели)

широкий пик

слабое совпадение

нет перекрытия

OK

u (пиксели)

Рис. 14.33. Статус стереосопоставления для каждого пикселя

Теперь можно получить допустимую диспаратность
>>> disparity_valid = disparity.choose(0, status!=1)
Image: 644 x 561 (float32)

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

862 Глава 14 · Использование нескольких изображений

ной для робота форме: ненадежные значения диспаратности
четко обозначены.

14.4.2.5 Получение срезов пространства диспаратности
Альтернативный способ взглянуть на проблемные случаи –
посмотреть, как меняется диспаратность по горизонтали, что
можно сделать, создав последовательность срезов DSI в направлении v. Например, вот как получить срез для v = 100:
>>> Image(DSI[100, :, :].T).disp();

диспаратность (пиксели)

Подобие ZNCC

диспаратность (пиксели)

На рис. 14.34 показана последовательность срезов для выбранных значений v, и внутри каждой из этих ud-плоскостей
можно видеть яркий след (высокие значения сходства), который представляет диспаратность D(u). Обратите внимание на
значительные разрывы следа на плоскости при v = 100, которые
соответствуют внезапным изменениям глубины. Также видно, что след местами затухает. В этих областях максимальное
сходство низкое, сильного совпадения на правом изображении
нет, и наиболее вероятной причиной является окклюзия. Класс
алгоритмов плотного стереосопоставления – полуглобальное
сопоставление блоков – пытается найти путь по изображению,
который максимизирует сходство и непрерывность.

u (пиксели)

u (пиксели)

Рис. 14.34. Срезы трехмерного изображения пространства диспаратности – это двухмерные
изображения сходства, представленные как функции диспаратности. Здесь показаны
четыре среза

Плотное стереозрение 863

14.4.2.6 Подведение итогов
При создании системы стереозрения приходится разрешать
три компромисса. Первый – базовое расстояние между камерами. С увеличением этого расстояния увеличивается и диспаратность, что позволяет точнее оценивать глубину, но усугубляется проблема окклюзии. Второй – необходимость тщательно
выбирать диапазон поиска диспаратности. Если максимальное
значение слишком велико, то увеличивается вероятность пространственного наложения, а если слишком мало, точки, близкие к камере, будут генерировать неверные и слабые соответствия. Большой диапазон диспаратности также увеличивает
время вычислений. Третий – выбор между размером шаблона,
временем вычислений и качеством изображения диспаратности. Шаблон небольшого размера позволяет улавливать тонкую
глубинную структуру, но, как правило, дает гораздо более зашумленные результаты, потому что шаблоны небольшого размера более восприимчивы к неоднозначным совпадениям.
Шаблон большего размера дает более гладкое изображение диспаратности, но требует большего объема вычислений. Кроме
того, с увеличением шаблона растет вероятность, что он будет
содержать пиксели, принадлежащие объектам с разной глубиной (эта проблема называется проблемой смешанных пикселей
и может привести к плохому качеству сопоставления на краях
объектов), из-за чего полученное изображение диспаратности
будет выглядеть размытым. Одно из решений – использование
непараметрического локального преобразования, такого как
ранговое или переписное преобразование перед оценкой корреляции. Поскольку эти преобразования основаны на упорядочении значений интенсивности, а не на самих значениях, они
обеспечивают более высокое качество на границах объектов.

14.4.2.7 Расширенное стереосопоставление
В предыдущих разделах мы использовали простой алгоритм
стереосопоставления, реализованный на Python и входящий
в состав Toolbox. Он ценен как учебный пример, но работает
медленно и далек от современного уровня. Библиотека OpenCV
включает ряд более продвинутых алгоритмов стереосопоставления.
Блочный алгоритм стереосопоставления действует по тому
же принципу работы, что и stereo_simple, но работает быстрее
и обеспечивает оценку диспаратности с точностью до субпикселя. Повторим пример с грудой камней:
>>> disparity_BM = rocks_l.stereo_BM(rocks_r, hw=3, drange=[40, 90],
...
speckle=(200, 2))

864 Глава 14 · Использование нескольких изображений
Image: 638 x 555 (float32)
>>> disparity_BM.disp();

Результаты показаны на рис. 14.35б. Аномальные черные
и белые пятна, называемые дифракционными пятнами, или
спеклами, указывают на пиксели, где диспаратность вычислена неверно. Существует два подхода к уменьшению спеклэффекта: увеличение размера окна сопоставления блоков или
использование спекл-фильтра. Эффект увеличения размера
окна показан на рис. 14.35в–г, где изображение диспаратности
становится более гладким, но разрывы исчезают. Спеклы – это
небольшие области с постоянной диспаратностью, отличаю­
щие­ся от окружающих областей. Для их обнаружения используется один из вариантов анализа связных компонент. Спеклфильтр имеет два параметра: максимальную площадь области
спекл-эффекта в пикселях и максимальное изменение диспаратности внутри области. Эффект спекл-фильтра можно увидеть на рис. 14.35б (получен с включенным фильтром) и рис.
14.35a (получен с выключенным фильтром).
В рассмотренных подходах диспаратность, вычисляемая для
каждого пикселя, не зависит от других пикселей, но в реальных
сценах соседние пиксели обычно принадлежат одной поверхности, и диспаратность будет весьма схожей – это называется
ограничением гладкости. А на краях диспаратность будет прерывистой. В контексте рис. 14.34 это можно представить как поиск
наилучшего возможного пути с сильно схожими значениями по
всему изображению, который также обеспечивает ограничение
гладкости в горизонтальном направлении. Это реализуется алгоритмом полуглобального сопоставления блоков
>>> rocks_l.stereo_SGBM(rocks_r, hw=3, drange=[40, 90],
...
speckle=(200, 2)).disp();

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

14.4.2.8 Трехмерная реконструкция
Заключительный этап стереосопоставления – преобразование
значений диспаратности в пикселях в мировые координаты
в метрах. Этот процесс известен как трехмерная реконструкция. Выше, обсуждая разреженное стереозрение, мы определяли мировую точку по пересечению двух линий в трехмерном
пространстве. Для стереокамеры с параллельными оптическими осями, как показано на рис. 14.22, геометрия значительно
проще (см. рис. 14.36). ► Для красного и синего треугольников
мы можем записать

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

u (пиксели)

u (пиксели)

г

u (пиксели)

v (пиксели)

в

б

v (пиксели)

u (пиксели)

v (пиксели)

а

v (пиксели)

v (пиксели)

Плотное стереозрение 865

д

u (пиксели)

Рис. 14.35. Изображения диспаратности, полученные алгоритмами стереосопоставления из OpenCV: а) stereo_BM
с размером блока 7×7 без спекл-фильтра; б) stereo_BM с размером блока 7×7; в) stereo_BM с размером блока
11×11; г) stereo_BM с размером блока 23×23; д) stereo_SGBM с размером блока 7×7. Изображения б–г получены со
спекл-фильтром (200, 2)

866 Глава 14 · Использование нескольких изображений

X = Z tan θ1

и

X - b = Z tan θ2,

где b – базовая линия, а углы лучей соответствуют горизонтальной координате изображения iu, i = {L, R}

где ρu – ширина пикселя, u0 – горизонтальная координата главной точки, а f – фокусное расстояние. Подстановка и сокращение X дают

Рис. 14.36. Геометрия стереозрения для случая,
когда камеры направлены параллельно.
X и Z измеряются относительно камеры 1,
b – базовая линия

Это уравнение показывает, что глубина обратно пропорцио­
нальна диспаратности. Также есть возможность восстановить
координаты X и Y так, что трехмерная координата точки будет
равна
(14.15)

и эту координату можно вычислить для каждого пикселя.
Хорошая система стереозрения может оценить диспаратность с точностью до 0.2 пикселя. Удаленные точки имеют небольшое расхождение, и ошибка в оценке трехмерных координат будет значительной. Как правило, максимальный радиус
действия систем стереозрения обычно составляет 50b.
 зображения из набора данных Middlebury, представленные на
И
рис. 14.23, были получены камерой с очень широкой базовой линией.
Левый край левого изображения и правый край правого изображения не перекрываются и были обрезаны. Обрезка N пикселей с левой
стороны левого изображения уменьшает диспаратность на N.

Плотное стереозрение 867

Истинная диспаратность определяется как
>>> di = disparity_BM.image * 2 + 274;

где коэффициент 2 учитывает тот факт, что диспаратность вычислена для изображения с половинным разрешением, а 274
компенсирует кадрирование изображения. Вычислим координаты X, Y и Z каждого пикселя как отдельные массивы
>>>
>>>
>>>
>>>
>>>

U, V = disparity_BM.meshgrid();
u0, v0 = disparity.centre;
f = 3740; # в пикселях, как описано на сайте Middlebury
b = 0.160; # в м, как описано на сайте Middlebury
X = b * (U - u0) / di; Y = b * (V - v0) / di; Z = f * b / di;

которые можно отобразить как поверхность
>>> fig, ax = plt.subplots(subplot_kw={"projection": "3d"})
>>> ax.plot_surface(X, Y, Z)
>>> ax.view_init(-100, -100)

Правда, в этом случае получится изображение довольно плохого качества.
Давайте используем популярный пакет Open3D для отображения плотного цветного облака точек. Передадим изображение глубины Z, исходное цветное изображение и проекционную
модель как объект CentralCamera
>>> cam = CentralCamera(f=f, imagesize=rocks_l.shape);
>>> pcd = PointCloud(Z, image=rocks_l, camera=cam, depth_trunc=1.9)
PointCloud with 354090 points.
>>> pcd *= SE3.Rx(pi); # направить ось y вверх
Умножение на месте где параметр depth_trunc указывает, что точки, находящиеся
объекта PointCloud дальше этого расстояния, будут исключены, в том числе и стена
на объект SE3
преобразует точки за кучей камней. Результат
в соответствии
с ξ · Pi. >>> pcd.disp()

показан на рис. 14.37. Интерактивные элементы управления
позволяют изменять точку обзора и масштаб.

14.4.3

Исправление изображения

Стереопара с грудой камней на рис. 14.23 имеет сопряженные
точки в одном ряду на левом и правом изображениях – это пара
изображений, выровненных по эпиполярному полю. Стереокамеры, такие как на рис. 14.22, изготавливаются с достаточно
высокой точностью, обеспечивающей параллельность оптических осей камер и параллельность осей u и v двух сенсоров. Од-

868 Глава 14 · Использование нескольких изображений

https://go.sn.pub/
pjm5oA

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

нако точность механической юстировки ограничена, да и искажения объектива вносят некоторую погрешность. Обычно одно
или оба изображения деформируются для исправления этих
погрешностей. Данный процесс известен как исправление, или
ректификация.
Проиллюстрируем исправление на примере стереопары двора, представленной на рис. 14.15:
>>> walls_l
Image: 1632
>>> walls_r
Image: 1632

=
x
=
x

Image.Read('walls-l.png', reduce=2)
1224 (uint8), R:G:B [.../images/walls-l.png]
Image.Read('walls-r.png', reduce=2)
1224 (uint8), R:G:B [.../images/walls-r.png]

левый и правый снимки которой были далеки от эпиполярного выравнивания, как показано на рис. 14.16. Сначала найдем
признаки SIFT
>>> sf_l = walls_l.SIFT()
SIFT features, 20426 points
>>> sf_r = walls_r.SIFT()
SIFT features, 18941 points

и определим предполагаемые совпадения
>>> matches = sf_l.match(sf_r);

Затем определим эпиполярное отношение:
>>> F, resid = matches.estimate(CentralCamera.points2F,
... method="ransac", confidence=0.95);

Плотное стереозрение 869

Этап исправления требует набора соответствующих точек,
содержащегося в matches, и фундаментальной мат­ри­цы
>>> H_l, H_r = walls_l.rectify_homographies(matches, F)

Он возвращает пару матриц гомографии, которые используются для деформации входных изображений
>>> walls_l_rect =
Image: 1632 x 1224
>>> walls_r_rect =
Image: 1632 x 1224

walls_l.warp_perspective(H_l)
(uint8), R:G:B
walls_r.warp_perspective(H_r)
(uint8), R:G:B

Исправленные изображения
>>> walls_l_rect.stdisp(walls_r_rect)

v (пиксели)

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

u (пиксели)

Рис. 14.38. Исправленные изображения сцены двора с рис. 14.15. Черные пиксели слева
и снизу каждого изображения соответствуют пикселям за пределами исходного изображения

Теперь можно представить, что эти изображения получены
с виртуальной стереокамеры с параллельными осями и выровненными рядами пикселей, и теперь их можно использовать
для плотного стереосопоставления.
>>> walls_l_rect.stereo_SGBM(walls_r_rect, hw=7, drange=[180, 530],
...
speckle=(50, 2)).disp();

Результат показан на рис. 14.39. Параметры диапазона диспаратности были определены интерактивно с помощью stdisp
для проверки диспаратности ближних и дальних точек в паре
исправленных изображений. Половинный размер окна, рав-

870 Глава 14 · Использование нескольких изображений

v (пиксели)

Диспаратность (пиксели)

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

u (пиксели)

Рис. 14.39. Изображение плотного стереосопоставления для сцены двора с рис. 14.15.
Стены и земля демонстрируют четкий градиент глубины

14.5

Анаглифы

Человеческое стереовосприятие глубины работает, потому что каждый глаз смотрит на
сцену с разных точек зрения. Если мы посмот­
рим на фотографию объемной сцены, мы
все равно получим ощущение глубины, хотя
и уменьшенное, потому что помимо стереоэффекта наш мозг использует множество визуальных сигналов для определения глубины.
С момента изобретения фотографии в XIX в. люди были очарованы 3D-фотографиями и фильмами, и нынешняя популярность 3D-фильмов и наличие 3D-телевидения являются еще
одним свидетельством этого.
Ключом ко всем технологиям трехмерного отображения является получение изображений с двух камер с базисной линией,
аналогичной человеческому зрению (примерно 8 см), и повтор-

Анаглифы 871

v (пиксели)

ная демонстрация этих изображений соответствующим глазам
зрителя. Старомодные стереограммы требовали бинокулярного устройства просмотра или заставляли зрителя прищурившись смотреть на стереограмму, скосив глаза. Более современными и удобными средствами просмотра стереопар являются
затворные ЖК-очки (обычно игровые), поляризованные очки,
позволяющие просматривать полноцветные стереофильмы,
или наголовные дисплеи (очки виртуальной реальности).
Старый и недорогой метод просмотра стереоизображений – это анаглифные изображения, в которых левое и правое
изоб­ра­же­ния накладываются друг на друга разными цветами.
Обычно красный цвет используется для левого глаза, а голубой
(зеленовато-синий) – для правого, но используются и многие
другие цветовые комбинации. Такие изображения просматриваются через специальные очки с разноцветными стеклами.
Красное стекло пропускает только красную часть анаглифного
изображения к левому глазу, а голубое стекло – только голубую
часть изображения к правому глазу. Недостатком является то,
что можно изобразить лишь интенсивность сцены, а не ее цвет.
Большим преимуществом анаглифов является то, что их можно распечатать на бумаге или снять на обычную кинопленку
и просмотреть с помощью простых и дешевых очков, таких как
те, что показаны на рис. 14.40а.

u (пиксели)
а
б
Рис. 14.40. Анаглифы для стереопросмотра: а) анаглифные очки с красным и синим стеклами;
б) анаглифная визуализация сцены с камнями на рис. 14.23, где левая часть окрашена
красным, а правая – голубым

Стереопару кучи камней можно отобразить в виде анаглифа
>>> walls_l.anaglyph(walls_r, "rc").disp();

как показано на рис. 14.50б. Аргумент "rc" указывает, что левое
и правое изображения закодированы красным и голубым (red
и cyan) соответственно. К другим вариантам цветовых пар от-

872 Глава 14 · Использование нескольких изображений

носятся: синий–зеленый (blue-green) и пурпурный–оранжевый
(magenta-orange).
Отступление 14.4. Анаглифы
Самые первые разработки появились во Франции. В 1858 г. Джозеф Д’Алмейда проецировал слайд-шоу волшебных фонарей
в формате 3D в виде красно-синих анаглифов, а зрители надевали
красно-синие очки. Примерно в то же время Луи Дю Орон создал
первые печатные анаглифы. Позже, примерно в 1890 г., Уильям
Фризе-Грин создал первые трехмерные анаглифные фильмы, используя камеру с двумя объективами. Анаглифные фильмы, называемые пластиконами или пластиграммами, были повальным
увлечением в 1920-х гг.
Сегодня панорамные анаглифы высокого разрешения можно найти на http://gigapan.com, а анаглифы Марса можно найти
на http://mars.nasa.gov/mars3d. Также существуют синтетиче­
ские анаглифы: https://threejs.org/examples/webgl_effects_anaglyph.html.

14.6

Другие технологии определения глубины

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

14.6.1

Структурированный свет

Старый, но простой и эффективный метод оценки трехмерной
структуры сцены – структурированный свет (structured light).
Концептуально он похож на стереозрение, но заменяет левую
камеру проектором, накладывающим на сцену световой узор.
Простейший пример структурированного света, показанный на рис. 14.41а, основывается на излучении вертикальной
плоскости света. ► В системе стереозрения это эквивалентно
левому изображению, представляющему собой вертикальную
линию. Изображение линии, спроецированное на поверхность,
видимую из правой камеры, будет искаженной версией линии,
как показано на рис. 14.41б. Диспаратность между виртуальным левым изображением и реальным правым изображением
зависит от глубины точек вдоль линии.
Поиск световой полосы на сцене – относительно простая задача компьютерного зрения. В каждой строке изображения мы
ищем пиксель, соответствующий проецируемой полосе, исходя

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

Другие технологии определения глубины 873

плоскость
виртуального
изображения

а

плоскость
изображения

диспаратность

б

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

Этот подход использовался в сенсоре
Kinect для Xbox
360, выпущенном
в 2010 году, и все
еще доступен в сенсоре Asus Xtion. Он
был разработан
израильской компанией PrimeSense,
купленной Apple
в 2013 году. В новой
версии Azure Kinect
используется измерение времени
пролета луча для
каждого пикселя.

из интенсивности или цвета. Если системы координат камеры
параллельны, то глубина вычисляется по уравнению (14.15).
Чтобы оценить глубину сцены, нужно переместить плоскость
света горизонтально по всей сцене. Для этого есть много способов: механическое вращение проектора лазерной полосы, использование движущегося зеркала для отклонения полосы или
использование проектора данных и программного обеспечения для создания изображения с полосами света. Однако перемещение световой плоскости по сцене происходит медленно
и принципиально ограничено скоростью, с которой мы можем
получать последовательные изображения сцены. Один из способов ускорить процесс – спроецировать на сцену несколько
линий, но тогда нам нужно решить проблему неоднозначности
сопоставления, что не так просто, если части некоторых линий
закрыты. Было предложено много решений, но, как правило,
они включают в себя кодирование строк каким-либо образом –
с использованием разных цветов или с использованием последовательности двоичных либо кодированных серым шаблоном
линий, которые могут обеспечить 2N строк всего за N кадров.
Близкий подход состоит в том, чтобы спроецировать на
сцену известный, но случайный набор точек, как показано на
рис. 14.42а. Каждую точку можно идентифицировать по уникальному узору точек в окружающем ее окне. Один объектив
проецирует с помощью лазера с дифракционным оптическим
элементом инфракрасный узор из точек (рис. 14.42а), который
снимает чувствительная инфракрасная камера, расположенная
за объективом справа. С ее помощью вычисляется изображе-

874 Глава 14 · Использование нескольких изображений

б

в

u (пиксели)

Глубина (м)

u (пиксели)

v (пиксели)

а

v (пиксели)

v (пиксели)

ние глубины (рис. 14.42в). Форма точек тоже меняется в зависимости от расстояния из-за несовершенной фокусировки,
и это дает дополнительные подсказки о расстоянии до точки.
Средний объектив предназначен для обычной цветной камеры, обеспечивающей вид, показанный на рис. 14.42б. Это пример конструкции камеры RGBD, возвращающей значения цвета
RGB, а также глубину (D) для каждого пикселя.
Системы структурированного света хорошо работают на расстоянии в несколько метров в помещении, на поверхностях без
текстуры, а также в темноте. Однако на открытом воздухе прое­
цируемый рисунок перекрывается окружающим освещением
от солнца.
В некоторых системах объемного зрения, таких как Intel
RealSense D400, также используется точечный проектор, иног­
да называемый спекл-проектором. Он создает искусственную
текстуру, которая помогает системе стереозрения, когда она
смотрит на бестекстурные поверхности, где совпадение часто
слабое и неоднозначное, как обсуждалось в разделе 14.4.2. Пре-

u (пиксели)

Рис. 14.42. Трехмерное изображение, полученное с помощью сенсора Kinect 360: а) случайный точечный рисунок,
наблюдаемый инфракрасной камерой Kinect; б) исходная сцена, снятая цветной камерой Kinect; c) вычисленное
изображение глубины. Красные пиксели обозначают значения nan, при которых глубина не может быть вычислена
из-за окклюзии или превышения максимального диапазона, например через окно в левой части сцены
(изображения предоставлены Уильямом Чемберленом)

Облака точек 875

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

14.6.2

Определение глубины по времени пролета луча

Альтернативой обычному стереозрению являются камеры,
основанные на измерении времени пролета. Камера излучает
импульс инфракрасного света, который освещает всю сцену,
и каждый пиксель регистрирует интенсивность и время возвращения отраженной энергии. Это очень сложная задача, потому
что для достижения точности разрешения в 1 см требует измерять интервалы времени с точностью порядка 70 пикосекунд,
причем для каждого пикселя.
Камеры этого типа хорошо работают в помещениях и даже
в полной темноте, но на открытом воздухе, при ярком солнце,
максимальная дальность ограничена, как и при использовании метода структурированного света. Более того, он дает еще
более плохой результат, чем метод структурированного света.
Энергия освещения ограничена соображениями безопасности
для глаз, и структурированный свет концентрирует ее на разреженном точечном поле, тогда как камеры с датчиком времени
пролета рассеивают ее по площади.

14.7

Облака точек

Облако точек – это набор координат {(Xi, Yi, Zi), i ∈ 0, ..., N - 1}
точек в пространстве. Цветное облако точек – это набор кортежей {(Xi, Yi, Zi, Ri, Gi, Bi), i ∈ 0, ..., N - 1}, включающий координаты точек мира и их тристимулусы (компоненты цвета). Облака
точек можно получить с помощью различных трехмерных датчиков, включая стереокамеры, RGBD-камеры и лидары. Облако
точек – это удобный способ объединения трехмерных данных
с нескольких датчиков (разных типов) или датчиков, движущихся с течением времени.
Для применения в робототехнике необходимо уметь извлекать краткую информацию из тысяч или миллионов точек в облаке. Распространенные стратегии обработки данных
включают поиск доминирующих плоскостей и привязку точек
к известным трехмерным моделям объектов.

876 Глава 14 · Использование нескольких изображений

Порядок точек, как правило, не важен, и для сокращения
вычислений, необходимых для поиска соседей точки, обычно
строится kd-дерево. Исключением является организованное
облако точек, в котором точки соответствуют пикселям на изображении и указаны в порядке следования строк изображения.
Для иллюстрации мы загрузим версию знаменитого Стэнфордского кролика. Как и в случае с двухмерными изображениями, существует множество распространенных форматов
файлов, и в этом примере мы используем файл PLY :
>>> bunny_pcd = PointCloud.Read('data/bunny.ply')
PointCloud with 35947 points.
>>> bunny_pcd.disp(block=False)

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

Эта модель хорошо
известна в сооб­
ществе компьютерной графики.
Она была создана
Грегом Терком
и Марком Левоем
в 1994 г. в Стэнфордском университете с использованием сканера
Cyberware 3030 MS
и керамической
фигурки кролика.
Формат Polygon
довольно прост.
Файлы в этом
формате содержат
кортежи (X, Y, Z) для
точек, а также информацию о гранях
и цвете. Open3D
может читать файлы
в форматах PLY, STL,
OBJ, OFF и GLTF,
а также списки вершин в виде простого
текста ASCII.

а
б
в
Рис. 14.43. а) Облако точек Стэнфордского кролика, состоящее из 35 947 точек, более близкие
точки имеют более теплый цвет; б) вокселизированное облако точек, представленное в виде https://go.sn.pub/
проволочной сетки; в) диаграмма разреженных игольчатых нормалей к поверхности (модель bosWgD
Стэнфордского кролика предоставлена лабораторией Stanford Graphics Laboratory)

Облаками точек можно манипулировать, например их можно преобразовать в сетку вокселей
>>> pcd = bunny_pcd.voxel_grid(voxel_size=0.01).disp()

которая показана на рис. 14.43б. Каждый куб имеет длину стороны 1 см и включается в трехмерную сетку, если содержит хотя
бы одну точку из облака точек. Это полезное представление для
обнаружения столкновений, а также помогает уменьшить разрешение.
>>> pcd = bunny_pcd.downsample_voxel(voxel_size=0.01)
PointCloud with 795 points.

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

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

Облака точек 877
>>> pcd.normals(radius=0.1, max_nn=30)
>>> pcd.disp(block=False)

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

14.7.1

Поиск плоскости

В нашем вещественном мире часто встречаются разнообразные плоскости, но для робототехники наиболее полезными
плоскостями являются земля (для управления колесным мобильным роботом или посадки БПЛА) и стены. Если мы располагаем набором трехмерных координат в виде облака точек,
простой и эффективный способ поиска плоскости, лучше всего
совпадающей с облаком точек, состоит в том, чтобы подогнать
данные к эллипсоиду. Эллипсоид будет иметь очень маленькую
ось в направлении нормали к искомой плоскости – фактически
это будет сплюснутая эллипсоидная пластина. Матрица инерции точек может быть рассчитана непосредственно по точкам
(14.16)
–
где x = Pi − P – координаты точек относительно их центроида
–
P=
Pi. Радиусы эллипсоида являются собственными значениями J, а собственный вектор, соответствующий наименьшему собственному значению, является направлением минимального радиуса, который является нормалью к плоскости.
Проблемой этого способа поиска плоскости являются выбросы данных, поскольку они значительно искажают решение.
Существует целый ряд способов решения данной проблемы, но
самый простой из них заключается в добавлении весового коэффициента в уравнение 14.16:
Обычно
используется функция Коши–Лоренца
w = β2/(d 2 + β2),
где d – расстояние
точки от плоскости,
а β – полуширина.
Функция является гладкой для
d = [0, ∞) и имеет
значение ½,
когда d = β.

который обратно пропорционален расстоянию xi от плоскости
и вычисляется итеративно. Изначально все веса wi = 1, а на последующих итерациях устанавливаются в соответствии с расстоянием Pi от плоскости, оцененным на предыдущем шаге.
Подробнее об эллипсах рассказывается в разделе C.1.4.
В качестве альтернативы можно применить алгоритм
RANSAC, взяв выборки из трех точек для оценки параметров

878 Глава 14 · Использование нескольких изображений

плоскости aX + bY + cZ + d = 0. Применение этого метода можно
проиллюстрировать на примере сцены во дворе, которую мы
рассматривали выше:
>>> pcd = walls_pcd
PointCloud with 2341 points.
>>> plane, plane_pcd, pcd =
... pcd.segment_plane(distance_threshold=0.05, seed=0)
>>> plane
array([ 0.7343, -0.1192, 0.6682, 0.8823])

Этот код вернул уравнение первой найденной плоскости
и два облака точек. Первое облако точек содержит все точки
внутри плоскости:
>>> plane_pcd
PointCloud with 1245 points.

В нем содержатся 1245 точек, лежащих на расстоянии 5 см от
предполагаемой плоскости. Второе облако содержит все остальные точки сцены, и мы можем повторить процесс:
>>> plane, plane_pcd, pcd =
... pcd.segment_plane(distance_threshold=0.05, seed=0)
>>> plane
array([ 0.6998, 0.1586, -0.6965, -1.413])

чтобы найти следующую плоскость, и т. д.

14.7.2

Сопоставление двух наборов точек

Рассмотрим задачу, показанную на рис. 14.44a, где имеется модель кролика в известной позе, представленная синим облаком
точек. Красное облако точек является результатом либо движения кролика, либо движения трехмерной камеры. Совместив
два облака точек, мы можем определить движение твердого
тела: кролика или камеры.
Формально это задача регистрации. Нам даны два набора
n-мерных точек: модель {Mi ∈ ℝn, i = 0, ..., NM - 1} и некоторые
зашумленные наблюдаемые данные {Dj ∈ ℝn, j = 0, ..., ND - 1},
определяющие движение твердого тела из системы координат
данных в систему координат модели

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

Мы рассматриваем
общий случай, когда
два облака имеют
разное количество
точек, то есть
ND ≠ NM.

Облака точек 879

откуда можно вычислить смещение
– –
t = D - M.
Это довольно
тяжелый
в вычислительном
плане метод, но,
организовав точки
в kd-дерево, можно
снизить затраты.

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

результат которого кодирует поворот между двумя множествами точек. Разложение по сингулярным числам имеет вид J =
UΣVT, а мат­ри­ца поворота задается формулой (F.2).
Расчетное относительное положение между двумя облаками
точек равно ξ Δ ~ (R, t), и точки модели преобразуются таким образом, чтобы они были ближе к точкам данных:
Mi ← ξ Δ · Mi,

i = 0, ..., NM - 1,

ξ ← ξ ⊕ ξ Δ,

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

В таком
случае, когда
сопоставляются
случайные выборки,
вероятность, что
одна и та же точка >>> model = bunny_pcd.downsample_random(0.1, seed=0)
окажется в обоих
облаках, довольно PointCloud with 3594 points.
мала. Несмотря
Данные, которые мы пытаемся подогнать под модель, предна это, ICP очень
хорошо справляется ставляют собой еще одну случайную выборку, на этот раз сос сопоставлением
и оценкой держащую только 5 % точек, к которой было применено преобпреобразования. разование твердого тела :

880 Глава 14 · Использование нескольких изображений
>>> data = SE3.Trans(0.3, 0.4, 0.5) * SE3.Rz(50, unit="deg")
...
* bunny_pcd.downsample_random(0.05, seed=-1);

Раскрасим облака точек в разные цвета и отобразим их
вместе:
>>> model.paint([0, 0, 1]) # синий
PointCloud with 3594 points.
>>> data.paint([1, 0, 0]) # красный
PointCloud with 1797 points.
>>> (model + data).disp(block=False)

как показано на рис. 14.44a. Оператор + объединяет два объекта
PointCloud в новый объект PointCloud.
>>> T, status = model.ICP(data, max_correspondence_distance=1,
...
max_iteration=2_000, relative_fitness=0,
...
relative_rmse=0)
>>> T.printline()
t = 0.3, 0.4, 0.5; rpy/zyx = 0.0976°, -0.0385°, 49.9°

что в точности соответствует «неизвестному» относительному
положению облака точек данных, которое мы выбрали выше.
Теперь можно преобразовать облако точек данных в систему
координат модели и наложить его на модель:
>>> (model + T.inv() * data).disp(block=False)

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

https://go.sn.pub/
URJmIv

а
б
Рис. 14.44. Сопоставление двух облаков точек методом итеративного поиска ближайших
точек (ICP): модель (синий) и данные (красный): а) до регистрации; б) после регистрации;
наблюдаемые точки данных были преобразованы в систему координат модели (модель
Стэнфордского кролика предоставлена лабораторией Stanford Graphics Laboratory)

https://go.sn.pub/
UOaoQ0

Применения 881

14.8

Применения

14.8.1

Коррекция перспективы

Возьмем изображение
>>> notredame = Image.Read("notre-dame.png");
>>> notredame.disp();

показанное на рис. 14.45. Форма здания значительно искажена,
потому что оптическая ось камеры не была перпендикулярна
плоскости здания, и мы видим отчетливые признаки перспективного ракурса (трапецеидальное искажение). Укажем вручную четыре точки, которые являются углами большого прямоугольника на плоскости фасада здания, начиная из левого
нижнего угла по часовой стрелке
>>> picked_points = plt.ginput(4);

и получаем список кортежей, являющихся с координатами u
и v каждой выбранной точки, который можно преобразовать
в массив 2×4
>>> p1 = np.array(picked_points).T;

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

v (пиксели)

искаженная форма

u (пиксели)

Рис. 14.45. На снимке, сделанном с земли, виден эффект перспективного ракурса, который
придает зданию трапециевидный вид (трапециевидное искажение). Четыре точки на
плоскости фасада здания были отмечены вручную белыми круглыми маркерами (на снимке –
собор Парижской Богоматери)

882 Глава 14 · Использование нескольких изображений

вершить этот пример, воспользуемся некоторыми ранее выбранными точками
>>> p1 = np.array([
...
[ 44.1364, 94.0065, 537.8506, 611.8247],
...
[377.0654, 152.7850, 163.4019, 366.4486]]);

и наложим их на изображение собора вместе с полупрозрачной
желтой трапецией:
>>> plot_polygon(p1, filled=True, color="y", alpha=0.4, linewidth=2);
>>> plot_point(p1, "yo");

Воспользуемся экстремумами этих точек для определения
вершин прямоугольника на изображении
>>> mn = p1.min(axis=1);
>>> mx = p1.max(axis=1);
>>> p2 = np.array([[mn[0], mn[0], mx[0], mx[0]],
...
[mx[1], mn[1], mn[1], mx[1]]]);

который наложим на изображение как пунктирный черный
прямоугольник:
>>> plot_polygon(p2, "k--", close=True, linewidth=2);

Наборы точек p1 и p2 являются проекциями мировых точек,
которые приблизительно лежат в плоскости, поэтому мы можем вычислить гомографию:
>>> H, _ = CentralCamera.points2H(p1, p2, method="leastsquares")
>>> H
array([[
1.4, 0.3827, -136.6],
[ -0.07853,
1.805, -83.11],
[-0.0002687, 0.001557,
1]])

Она превратит вершины желтой трапеции в вершины пунк­
тирного прямоугольника :
p̃2 ≃ Hp̃1.

Таким образом, гомография отображает координаты изображения из искаженной трапецеидальной формы в неискаженную прямоугольную.
Эту гомографию можно применить к координате каждого
пикселя в выходном изображении, чтобы деформировать входное изображение. Воспользуемся обобщенной функцией деформации изображения из Toolbox:
>>> notredame.warp_perspective(H).disp();

Результат, показанный на рис. 14.46, представляет собой синтетическую фронто-параллельную проекцию. Она эквивалент-

Гомографию также
можно вычислить по
четырем линиям на
плоскости, но Toolbox не поддерживает этот способ.

Применения 883

v (пиксели)

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

u (пиксели)

Рис. 14.46. Фронто-параллельный вид, синтезированный из рис. 14.45. Изображение было
преобразовано таким образом, что отмеченные точки трапеции стали углами прямоугольника
на новом изображении

В дополнение к созданию этого синтетического вида можно
разложить гомографию, чтобы восстановить движение камеры
от фактической точки зрения к виртуальной, а также нормаль
к поверхности собора. Система координат для этого примера
схематически представлена на рис. 14.47 и показывает фактическое и виртуальное положения камеры. Как было показано
в разделе 14.2.4, чтобы преобразовать проективную гомографию в евклидову, необходимо определить калибровочную мат­
ри­цу камеры. Получим фокусное расстояние из метаданных
в файле изображения:
>>> f = notredame.metadata("FocalLength")
7.4

Фокусное расстояние измеряется в миллиметрах, а сенсор,
как известно, имеет размер 7.18×5.32 мм. Создадим калиброванную камеру:
>>> cam = CentralCamera(imagesize=notredame.shape, f=f/1000,
...
sensorsize=[7.18e-3, 5.32e-3])
Name: perspective [CentralCamera]
pixel size: 1.121875e-05 x 1.2488262910798123e-05
image size: 426 x 640
pose: t = 0, 0, 0; rpy/yxz = 0°, 0°, 0°
principal pt: [ 213 320]
focal length: [ 0.0074 0.0074]

884 Глава 14 · Использование нескольких изображений

виртуальная
камера

ручная камера

Рис. 14.47. Системы координат двух камер для примера со снимком собора Нотр-Дам. Синяя
система {1} – это камера, снявшая исходное изображение, а красная система {2} – точка
зрения камеры для синтетического изображения, дающего фронто-параллельный вид

Теперь используем полученную информацию для вычисления и декомпозиции евклидовой гомографии:
>>>
>>>
t =
t =
t =
t =

pose, normals = cam.decomposeH(H)
pose.printline(orient="camera")
0.139, -0.614, 0.238; rpy/yxz = 3.69°, -36.9°, -8.32°
-0.139, 0.614, -0.238; rpy/yxz = 3.69°, -36.9°, -8.32°
-0.0308, 0.119, -0.661; rpy/yxz = 1.06°, 0.94°, 1.36°
0.0308, -0.119, 0.661; rpy/yxz = 1.06°, 0.94°, 1.36°

и получаем массив экземпляров SE3, хранящих четыре возможных положения камеры, а также список нормалей плоскости
в исходном кадре камеры. В этом случае правильным является
первое решение, так как оно представляет собой значительный
поворот вокруг оси x и перемещение в направлении –y. Камеру
необходимо наклонить вниз, чтобы ее оптическая ось стала горизонтальной, и поднять вверх, чтобы получить фронтально-параллельный ракурс. Расчетное перемещение камеры не в масштабе.
Фронтальная плоскость собора n определена относительно
исходного положения камеры {1}
>>> normals[0].T
array([[-0.02788,

0.1523,

0.9879]])

и лежит в плоскости yz, как следует из рис. 14.47.

14.8.2

Мозаика

Мозаика, или сшивка изображений (image stitching), – это процесс создания крупномасштабного составного изображения

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

Применения 885

Эмпирическое правило заключается
в том, что изображения должны перекрываться на 60 %
площади в прямом
направлении и на
30 % в боковом.

из нескольких перекрывающихся изображений. Она обычно
применяется к изображениям, полученным с дронов, и спутниковым снимкам, чтобы создать визуально непрерывное изображение поверхности Земли. Эту технологию также можно
применять к изображениям морского дна, полученным с направленных вниз камер подводного робота. Программное обес­
печение для создания панорам, поставляемое с цифровыми камерами или встроенное в них, является еще одним примером
обработки мозаики.
Входными данными для процесса создания мозаики является последовательность перекрывающихся изображений. Нет
необходимости знать параметры калибровки камеры или положение камеры, в которой были сделаны изображения, – камера
может произвольно вращаться между изображениями, и масштаб может меняться.
Для большей наглядности в этом примере мы воспользуемся
набором реальных перекрывающихся изображений
>>> images = ImageCollection("mosaic/aerial2-*.png", mono=True);

каждое из которых имеет размеры 1280×1024. Для начала создадим пустое составное изображение с размерами 2000×2000 пикселей
>>> composite = Image.Zeros(2_000, 2_000)
Image: 2000 x 2000 (uint8)

которое будет вмещать мозаику. Основы процесса создания мозаики показаны на рис. 14.48.
Первое изображение обрабатывается просто – мы всего лишь
вставляем его в верхний левый угол
>>> composite.paste(images[0], (0, 0));
первое
изображение

второе
изображение
оригинал

фрагмент
изображения

составное изображение

после искажения

ограничивающая
рамка

Рис. 14.48. Первое изображение в последовательности показано красным, второе – синим.
Второе изображение преобразуется во фрагмент изображения, а затем смешивается
с составным изображением

886 Глава 14 · Использование нескольких изображений

составного изображения, показанного красным на рис. 14.48.
Следующее изображение, показанное синим, требует более
сложной обработки – его нужно повернуть, масштабировать
и переместить, чтобы оно правильно накладывалось на красное изображение.
В данном примере предполагается, что сцена плоская. Это
вполне разумно для снимков, сделанных с большой высоты,
когда вертикальный рельеф невелик. Это означает, что можно использовать гомографию для связывания различных ракурсов камеры. Первый шаг – определение общих характерных
точек, называемых точками привязки (tie point). Воспользуемся уже знакомыми инструментами

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

>>> next_image = images[1]
Image: 1113 x 825 (uint8), id=1 [.../mosaic/aerial2-002.png]
>>> sf_c = composite.SIFT()
SIFT features, 30281 points
>>> sf_next= next_image.SIFT()
SIFT features, 20521 points
>>> match = sf_c.match(sf_next);

и оценим гомографию, отображающую 1p в 2p, использовав алгоритм RANSAC:
>>> H, _ = match.estimate(CentralCamera.points2H, "ransac",
...
confidence=0.99);
>>> H
array([[ 0.9725, 0.137, -245.6],
[ -0.137, 0.9725, -95.92],
[2.668e-09, 1.639e-09,
1]])

Далее отобразим точку 2p на новом изображении в соответствующие координаты на составном изображении
p ≃ H-1 2p;

1

эту операцию нужно применить к каждому пикселю в новом
изображении:
>>> tile, topleft, corners =
... next_image.warp_perspective(H, inverse=True, tile=True)

Как показано на рис. 14.48, деформированное синее изоб­
ражение выходит за границы исходного синего изображения,
а параметр tile=True указывает, что возвращаемое изображение
представляет собой минимальный охватывающий прямоугольник искаженного изображения. Это изображение называется
плиткой (tile) и показано черной пунктирной линией. Возвращаемое значение topleft – это координата системы координат
плитки относительно исходного изображения – вектор t на
рис. 14.48. Как правило, не каждый пиксель плитки имеет со-

Охватывающий
прямоугольник
вычисляется
путем применения
гомографии к углам
изображения
A = (0, 0),
B = (W – 1, 0),
C = (W – 1, H – 1)
и D = (0, H – 1),
где W и H – ширина
и высота соответственно, и нахождения границ в направлениях u и v.

Применения 887

ответствующую точку на входном изображении, и эти пиксели
устанавливаются равными нулю, как указано в пятом аргументе.
Теперь плитку нужно внедрить в составное мозаичное изоб­
ражение:
>>> composite.paste(tile, topleft, method="blend");

v (пиксели)

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

u (пиксели)

Рис. 14.49. Пример мозаики изображений. В нижней части кадра четко видно три
перекрывающихся вида взлетно-посадочной полосы аэропорта, что показывает хорошее
совпадение между кадрами

Параметр "blend" указывает, что пиксель в составном изображении должен получить значение из внедряемого изображения, если в составном изображении он равен нулю, в противном случае для него устанавливается среднее значение
составного и нового пикселей. Это простая и вычислительно
недорогая стратегия, позволяющая сгладить края фрагментов,
если изоб­ражения были сняты с одинаковой экспозицией. Если
экспозиции различаются, то необходимо проанализировать два
набора перекрывающихся пикселей, чтобы определить среднее
смещение интенсивности и масштабный коэффициент, которые можно использовать для коррекции фрагмента перед смешиванием. Этот процесс известен как тональное согласование.
Наконец, необходимо учесть влияние точек на изображении,
которые не находятся в плоскости земли, например на высо-

888 Глава 14 · Использование нескольких изображений

ком здании. На снимке, сделанном в зените, будет видна только крыша здания, а на изображении, снятом с другого ракурса,
будет вид под углом, показывающий здание сбоку. В мозаике
мы стремимся создать иллюзию того, что камера находится
точно над каждой точкой изображения, поэтому мы не должны видеть стороны какого-либо здания. Этот тип изображения
называется ортофотоснимком, или ортофотопланом, и в отличие от вида в перспективе, где лучи сходятся в фокусе камеры,
для него все лучи параллельны, что подразумевает вид из бесконечно удаленной точки. Собирая составное изображение,
можно выбирать пиксели из любой подходящей плитки. Для
наилучшего приближения к идеальному ортофотоснимку мы
должны выбрать пиксель, который до деформации был ближе
всего к главной точке.
В фотограмметрии этот тип мозаики называется неконтролируемой цифровой мозаикой, поскольку в ней не используются явные контрольные точки – вручную определяемые соответствующие элементы на изображениях. Продемонстрированные
здесь принципы также применимы к задаче стабилизации изображения. Гомография используется для сопоставления объектов на новом изображении с местоположением, которое они
имели на предыдущем изображении.

14.8.3

Визуальная одометрия

Распространенной задачей в робототехнике является оценка
расстояния, пройденного роботом, – одометрия. Это ключевой
источник данных для алгоритмов, обсуждавшихся в главе 6. Колесные роботы могут использовать информацию от колесных
энкодеров, хотя они подвержены случайным ошибкам (проскальзывание), а также систематическим ошибкам (погрешность измерения радиуса колеса). Однако для летающего или
подводного робота задача одометрии намного сложнее. Визуальная одометрия (visual odometry, VO) – это процесс использования информации из последовательных изображений для
оценки относительного движения робота в пространстве в промежутке времени между кадрами.
Загрузим последовательность изображений, снятых с автомобиля, едущего по дороге:
>>> left = ZipArchive("bridge-l.zip", filter="*.pgm", mono=True,
...
dtype="uint8", maxintval=4095,
...
roi=[20, 750, 20, 480]);
>>> len(left)
251

Google Earth иног­
да предоставляет
несовершенные
ортофотопланы. При
взгляде на города
мы можем видеть
наклонные виды
зданий.

Применения 889
Это левое изображение в последовательности из набора данных EISATS­
Bridge (Klette 2011)
http://www.
mi.auckland.
ac.nz/EISATS.

Здесь параметры задают область внимания на каждом изоб­
ражении, чтобы устранить неровную черную рамку, и преобразуют пиксели uint16, содержащие 12-битные значения, в 8-бит­ные целые значения.
Последовательность изображений
можно отобразить в виде анимации:
>>> for image in images:
... image.disp(reuse=True, block=0.05)

со скоростью 20 кадров/с.
Для каждого кадра можно вычислить угловые признаки и наложить их на анимацию:
>>> fig, ax = plt.subplots()
>>> for image in left:
... ax.clear()
... image.disp(ax=ax)
... features = image.ORB(nfeatures=20)
... features.plot();
... plt.pause(0.05)

#
#
#
#
#

очистить оси
отобразить изображение
вычислить признаки ORB
отобразить признаки ORB
короткая задержка

Один из кадров в этой последовательности показан на
рис. 14.50. Для разнообразия и эффективности здесь используются признаки ORB, которые широко применяются при решении подобных задач. Наблюдая за анимацией, мы видим, что
угловые признаки надежно «прилипают» к мировым точкам на
протяжении многих кадров. Движение признаков изображения
называется оптическим потоком и является функцией движения камеры в мире и трехмерной структуры мира.

v (пиксели)

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

u (пиксели)

Рис. 14.50. Кадр № 16 из последовательности изображений bridge-l с наложенными
признаками (изображение из проекта .enpeda., Klette et al. 2011)

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

890 Глава 14 · Использование нескольких изображений

лась в разделе 14.3. В этом примере автомобиль был оснащен
стереокамерой, и мы загружаем соответствующие изображения с правой камеры ►:
>>> right = ZipArchive("bridge-r.zip", mono=True, dtype="uint8",
...
maxintval=4095, roi=[20, 750, 20, 480]);

Изображения
в обоих наборах
были очищены
и кадрированы
для исключения
артефактов
деформации.

Для каждой пары левого и правого изображений мы извлекаем
признаки и определяем соответствие путем надежного сопоставления признаков, используя сходство дескрипторов и эпиполярное ограничение, подразумеваемое фундаментальной мат­ри­цей.
Затем вычисляем горизонтальное несоответствие между соответствующими функциями и, предполагая, что камеры полностью
откалиброваны, триангулируем координаты плоскости изоб­ра­
жения, чтобы определить мировые координаты ориентиров относительно левой камеры на транспортном средстве.
Мы можем сопоставить 3D-облака точек на текущем и предыдущем временном шаге, используя такой метод, как итеративный поиск ближайших точек (ICP), чтобы определить изменение положения камеры. Это так называемый метод 3D-3D
визуальной одометрии, и, хотя его принцип верен, на практике
он работает плохо. Во-первых, некоторые из трехмерных точек
могут находиться на других движущихся объектах, и это нарушает предположение ICP о том, что перемещается или сенсор,
или объект, но не оба вместе. Во-вторых, оценка расстояния до
удаленных точек весьма неточна, поскольку ошибки вычисления диспаратности становятся значительными, когда диспаратность мала.
Альтернативный подход, сопоставление 3D-2D, заключается в проецировании 3D-точки на текущем временном шаге на
предыдущее изображение и нахождении положения камеры,
которое минимизирует ошибку относительно координат наблюдаемых объектов, – это пакетная подстройка. Обычно это
делается только для одного изображения, и мы выберем левое
изображение. Чтобы установить соответствие признаков с течением времени, мы находим соответствия между признаками
левого изображения, которые совпадали с правым изображением, и совпадениями с признаками из предыдущего левого изображения, что снова устанавливает эпиполярное ограничение.
Теперь мы знаем соответствие между точками в трех видах сцены, как показано на рис. 14.51.
На каждом временном шаге мы решаем задачу пакетной
подстройки, в которой есть две камеры и ряд ориентиров,
определяемых стереотриангуляцией. Первая камера связана
с предыдущим временным шагом и зафиксирована в начале координат. Вторая камера связана с текущим временным шагом
и должна иметь сдвиг в положительном направлении по оси z.
Мы могли бы получить начальную оценку положения второй

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

Применения 891

камеры, найдя и разложив существенную мат­ри­цу, но вместо
этого мы установим ее в начало координат.
Я не буду приводить здесь полный код скрипта из соображений экономии места. Однако дополнительные подробности
и комментарии вы найдете в самом скрипте
>>> %run -m visodom

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

Рис. 14.51. Сопоставление признаков для визуальной одометрии. Верхний ряд – это
стереопара на текущем временном шаге, а нижний ряд – стереопара на предыдущем
временном шаге. Эпиполярно согласованные соответствия между тремя изображениями
показаны желтым цветом

Окончательные результаты перемещения камеры показаны
на рис. 14.52а, и мы наблюдаем значение около 0.5 м на каждом
временном шаге, но также есть некоторые отсутствующие точки данных и два некорректно выглядящих результата. Процесс
пакетной подстройки возвращает окончательную квадратичную ошибку, которая представлена на рис. 14.52б. Медианная
ошибка составляет около 0.07 квадратного пикселя, а 80 % кад­
ров имеют ошибку подстройки пакета менее 0.1 квадратного
пикселя. Однако максимум составляет около 0.45 квадратного
пикселя, и мы могли бы исключить такие плохие результаты
и, возможно, интерполировать смещение по предыдущему значению. Вероятным источником ошибки являются неправильные сопоставления точек. Пакетная подстройка предполагает,
что все точки мира фиксированы, но в этой последовательности
присутствует множество движущихся объектов. Мы использовали эпиполярное ограничение между текущим и предыдущим
кадрами, чтобы гарантировать, что в наборе сторонников модели будут только точки признаков, соответствующие движущейся камере и фиксированному миру. Но, запустив скрипт,
мы увидим довольно много точек на автомобиле впереди, ко-

892 Глава 14 · Использование нескольких изображений

торые ошибочно включены в набор сторонников модели, – этот
автомобиль движется, но поскольку он находится на большом
и постоянном расстоянии, такие точки не создают достаточной
ошибки, чтобы считаться аутсайдерами. Более сложный алгоритм пакетной подстройки обнаружит и отклонит такие точки.
Наконец, в верхней части сцены преобладают точки, которые,
как правило, достаточно удалены от камер. При более сложном
подходе к обнаружению признаков они будут выбираться более
равномерно по всему изображению.
Количество совпавших сторонников представлено на
рис. 14.52в и существенно изменяется на протяжении последовательности. Однако мы видим, что это изменение слабо коррелирует с ошибкой. Более серьезная проблема заключается в том,
что большинство совпавших угловых точек находятся в верхней
части кадра и соответствуют точкам, расположенным на значительном расстоянии от камер, что приводит к неточной оценке
смещения. Более сложный подход предполагает выбор признаков, более равномерно распределенных по изображению.
Вторая причина ошибок распространена при использовании
видеоданных для роботов. Подсказка заключается в том, что
эти значения подозрительно ровно в два раза отличаются от
других значений. Каждому изображению в последовательности
была назначена метка времени, когда оно было получено компьютером, и эти метки времени можно загрузить:

|t| (м)

>>> ts = np.loadtxt(left.open("timestamps.dat"));

потерянные видеокадры

Кол-во
сторонников

б

e (кв. пикс.)

а

ΔT

в

г
Шаг по времени

Рис. 14.52. Результаты визуальной одометрии: а) расчетное смещение камеры по оси z
(вперед); б) окончательная ошибка пакетной подстройки на кадр; в) количество совпавших
сторонников; г) отметки времени на изображениях

Подведение итогов 893

Если построить на графике разницу между временными метками (рис. 14.52г)
>>> plt.plot(np.diff(ts));

мы увидим, что среднее время между изображениями составляет 44.6 мс, но есть два всплеска, где интервал вдвое больше.
Компьютер, регистрирующий изображения, пропустил кадр,
возможно, он не смог записать данные изображения в память
или на диск так же быстро, как они поступили. Таким образом,
интервал между кадрами был в два раза больше, транспортное средство проехало в два раза больше, и всплески нашего
расчетного смещения фактически верны. Это нередкая ситуация – в роботизированной системе все данные должны иметь
временные метки, которые следует проверять для обнаружения
подобных проблем. Медианная скорость по действительным
оценкам равна 11.53 м/с, или около 40 км/ч.
Предполагаемые перемещения транспортного средства или
робота с течением времени не являются независимыми и связаны кинодинамической моделью транспортного средства,
обсуждавшейся в главе 4. Эту модель можно использовать для
сглаживания результатов и дисконтирования ошибочных оценок скорости. Если алгоритм пакетной подстройки предусмат­
ривает ограничения на положение камеры, можно назначить
весовые коэффициенты, чтобы наказывать невозможное движение в боковом и вертикальном направлениях, а также движение по крену и тангажу.

14.9

Подведение итогов

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

894 Глава 14 · Использование нескольких изображений

мат­ри­цу по данным изображения, отмечен эффект неправильной ассоциации данных и рассказано, как его преодолеть с помощью алгоритма RANSAC. Используя внутренние параметры
камеры, можно вычислить существенную мат­ри­цу, а затем
разложить ее, чтобы получить движение камеры между двумя
видами, но в преобразовании есть неизвестный масштабный
коэффициент. Имея некоторую дополнительную информацию,
например величину смещения, можно полностью рассчитать
движение камеры. Зная движение камеры, можно найти трехмерные координаты мировых точек.
В особом случае, когда мировые точки лежат на плоскости, они
индуцируют гомографию, которая представляет собой линейное
отображение точек между изображениями. Гомографию можно
использовать для обнаружения точек, не лежащих в плос­кости,
и ее можно разложить, чтобы получить движение камеры между двумя ракурсами (преобразование снова имеет неизвестный
масштабный коэффициент) и нормалью к плоскости.
Если фундаментальная мат­ри­ца известна, то пара перекрывающихся изображений может быть доработана для создания
эпиполярно выровненной стереопары, а плотное стереосопоставление можно использовать для восстановления мировых
координат каждой точки. В главе обсуждались ошибки из-за
таких эффектов, как окклюзия и отсутствие текстуры, а также
методы обнаружения этих ситуаций.
Мы использовали пакетную подстройку для решения задачи
оценки структуры сцены и движения, используя 2D-измерения
из набора изображений сцены для восстановления информации, связанной с 3D-геометрией сцены, а также с расположением камер. Стереозрение – это простой случай, когда известно
движение – оно фиксируется базисной линией стереосистемы, –
и нас интересует только структура. В задаче визуальной одомет­
рии нам важно только движение камеры, а не структура сцены.
Мы использовали облака точек, в том числе цветные, для
представления и визуализации трехмерного мира, а также продемонстрировали некоторые операции с данными облаков
точек, такие как удаление выбросов, понижение разрешения,
оценка нормалей, аппроксимация плоскостью и регистрация.
Затем эти многоракурсные методы использовались в ряде
практических примеров, таких как коррекция перспективы,
создание мозаики и визуальная одометрия.

14.9.1

Дополнительное чтение

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

Подведение итогов 895

сообществами компьютерного зрения и искусственного интеллекта с 1960-х гг. Эта книга придерживается языка и обозначений, принятых в литературе по компьютерному зрению,
но чтение фотограмметрической литературы вызовет лишь небольшие дополнительные трудности. Сходство стереокамеры
с двумя глазами очень велико, но хотя мы постоянно пользуемся стерео­зрением, это не единственный метод, который мы
используем для определения расстояния (Cutting, 1997).
Масштабные исследования по многоракурсной геометрии
проводились в таких лабораториях, как Стэнфорд, SRI International, лаборатория искусственного интеллекта Массачусетского технологического института, CMU, JPL, INRIA, Оксфорд
и ETL Japan в 1980-х и 1990-х гг., что привело к публикации ряда
учебников в начале 2000-х. Общепризнанными исчерпывающими работами по геометрии с несколькими представлениями
принято считать книги Hartley and Zisserman (2003) и Ma et al.
(2003). В этих книгах представлены совершенно разные подходы к одному и тому же материалу. Первая основана на более
геометрическом подходе, а вторая – на более математическом.
К сожалению, они используют совершенно разные обозначения, и каждое из них отличается от обозначений, используемых
в этой книге, – сводка наиболее важных обозначений приведена
в табл. 14.1. Все эти книги охватывают следующие темы: извлечение признаков (с использованием угловых признаков Харриса,
поскольку они были опубликованы до того, как были разработаны детекторы инвариантных к масштабу признаков, такие как
SIFT и SURF); геометрия одного, двух и N видов; фундаментальные и существенные мат­ри­цы; гомографии; восстановление
трехмерной структуры сцены и движения камеры с помощью
методов пакетной подгонки в автономном режиме. Обе они
предоставляют ключевые алгоритмы в псевдокоде и сопровождаются вспомогательным кодом MATLAB на соответствующих
веб-сайтах. Более ранняя книга Faugeras et al. (2001) охватывает
большую часть того же материала, используя математический
Таблица 14.1. Краткое перечисление различий в обозначениях между
двумя другими популярными учебниками и этой книгой
Объект
Мировая точка
Точка изображения
i-я точка изображения
Движение камеры
Нормализованные координаты
Матрица камеры
Гомогенные количества
Гомогенная эквивалентность

Hartley & Zisserman (2003)
X
x, x¢
xi, xi¢
R, t
x, x¢
P
x, X
x = PX

Ma et al. (2003)
P
x1, x2
x1i, x2i
R, T
x1, x2
П
x, P
λx = ПP
x ~ ПP

Эта книга
P
1
p, 2p
1
pi, 2pi
R, t
(u–, –v )
C
p̃, P̃
p̃ ≃ CP̃

896 Глава 14 · Использование нескольких изображений

подход и снова с другими обозначениями. В предыдущей книге
Faugeras (1993) основное внимание уделяется разреженным стереоскопическим изображениям из линейных объектов. В книге
Szeliski (2022) представлено очень доступное и более глубокое
обсуждение тем этой главы (версия в формате PDF доступна для
личного использования по адресу https://szeliski.org/Book).
SIFT и другие детекторы признаков обсуждались в разделах 14.1 и 12.3.2.1. Производительность детекторов признаков
и их эффективность согласования описаны в Mikolajczyk and
Schmid (2005), где рассматривается ряд различных дескрипторов признаков. В Arandjelović and Zisserman (2012) обсуждаются
некоторые важные моменты, связанные с сопоставлением векторов признаков.
Алгоритм RANSAC, описанный в Fischler and Bolles (1981),
является самой надежной рабочей лошадкой среди всех методов, основанных на признаках, обсуждаемых в этой главе, но
не работает с очень малыми коэффициентами выбросов. Более
надежными современными разработками являются консенсус
векторного поля (vector field consensus, VFC), описанный в Ma
et al. (2014), и консенсус прогрессивной выборки (progressive
sample consensus, PROSAC), представленный в Chum and Matas
(2005).
Термин «фундаментальная мат­ри­ца» был предложен в диссертации Luong (1992). Книга Xu and Zhang (1996) представляет
собой доступное введение в эпиполярную геометрию. Эпиполярная геометрия также может быть сформулирована для неперспективных камер, и в этом случае эпиполярная линия становится эпиполярной кривой (Mičušík and Pajdla 2003; Svoboda
and Pajdla, 2002). Для трех изображений геометрия описывается трифокальным тензором 𝒯, который представляет собой
тензор 3×3×3 с 18 степенями свободы, связывающий точку на
одном изображении с эпиполярными линиями на двух других
изображениях (Hartley and Zisserman 2003; Ma et al. 2003). Важная ранняя статья по эпиполярной геометрии для последовательности изображений написана Боллесом и соавт. (Bolles et
al., 1987).
Существенная мат­ри­ца была впервые описана десятилетием
ранее в письме в журнал Nature (Longuet-Higgins, 1981) выдающимся химиком-теоретиком и ученым-когнитивистом Крис­
тофером Лонге-Хиггинсом (1923–2004). В статье описан метод
оценки существенной мат­ри­цы по восьми соответствующим
парам точек. Разложение основной мат­ри­цы было впервые
описано в Faugeras (1993), но также фигурирует в Hartley and
Zisserman (2003) и Ma et al. (2003). В этой главе мы нашли движение камеры, сначала вычислив существенную мат­ри­цу, а затем
разложив ее. На первом этапе требуется как минимум восемь
пар сопряженных точек, но алгоритмы таких авторов, как Нис­

Подведение итогов 897

тер (Nistér, 2003), Ли и Хартли (Li and Hartley, 2006), вычисляют
движение непосредственно по пяти парам точек. Разложение
гомографии описано Фогерасом и Лустманом (Faugeras и Lustman, 1988), Хартли и Зиссерманом (Hartley и Zisserman, 2003),
Ма и соавт. (Ma et al., 2003 г.) и в обширной аналитической работе Малиса и Варгаса (Malis and Vargas, 2007 г.). Отношения между этими мат­ри­ца­ми, движением камеры и соответствующими
функциями Toolbox представлены на рис. 14.53.
векторы координат
для сопряженных точек
на плоскости изображения
cam.points2H()

Camera.points2F()
фундаментальная
матрица

гомография

cam.E(F)
cam.H(T)
cam.decomposeH(H)

относительное
положение

cam.F(T)

существенная
матрица

cam.E(T)
cam.decomposeE(E)

Рис. 14.53. Функции в пакете Toolbox и методы объекта камеры, а также их взаимосвязь;
«cam.» обозначает метод экземпляра, а «Camera» – метод класса

Стереокамеры и программное обеспечение для стереосопоставления доступны сегодня от многих поставщиков и могут генерировать карты глубины с высоким разрешением при частоте
более 10 Гц на стандартных компьютерах. Десять лет назад это
было сложно, и для работы в режиме реального времени требовалось специализированное оборудование на основе FPGA
(Corke et al., 1999; Woodfill and Von Herzen, 1997). Применение
стереозрения для навигации планетохода обсуждается в Matthies (1992). Можно использовать более двух камер: многокамерная стереосистема была предложена в Okutomi and Kanade
(1993). Она обеспечивает устойчивость к таким проблемам, как
эффект частокола.
В Brown et al. (2003) предлагается обзор методов стереозрения с акцентом на проблемы реального времени. Книга Shirai
(1987) хоть и старая, но хорошо объясняет принципы стереозрения. В Scharstein and Szeliski (2002) рассматривается стереопроцесс как четыре этапа: сопоставление, агрегирование, вычисление диспаратности и уточнение, и сравниваются стоимость
и производительность различных алгоритмов для каждого
шага. Алгоритм плотного стереосопоставления, представлен-

898 Глава 14 · Использование нескольких изображений

ный в разделе 14.4, – это очень традиционный стереоалгоритм,
основанный на корреляции. Диспаратность, вычисляемая для
каждого пикселя, не зависит от других пикселей, но в большинстве реальных сцен соседние пиксели принадлежат одной и той
же поверхности, и диспаратность будет очень похожей – это
называется ограничением сглаживания (smoothness constraint).
Обеспечить гладкость можно с помощью марковских случайных полей (Markov random field, MRF), полной вариации с регуляризаторами (Pock, 2008) или более эффективных алгоритмов
полуглобального сопоставления (semi-global matching, SGM)
(Hirschmüller 2008). Очень популярная библиотека для эффективного крупномасштабного стереосопоставления (LIBELAS)
от Гейгера и соавт. (Geiger et al., 2010) использует альтернативу глобальной оптимизации, которая обеспечивает быстрые
и точные результаты для различных внутренних и наружных
сцен. Стереозрение обычно нуждается в значительном объеме
вычислений, но есть хорошие возможности распараллеливания
с использованием нескольких ядер, наборов инструкций MIMD,
графических процессоров, специализированных микросхем
и FPGA. Использование непараметрических локальных преобразований описано в Zabih and Woodfill (1994) и Banks and Corke
(2001).
Глубокое обучение использовалось для сопоставления сте­
реоизображений (Luo et al. 2016) и для оценки глубины по
последовательности монокулярных изображений (Garg et al.
2016), возможно, с неявным использованием монокулярных
сигналов, используемых людьми (Cutting 1997). Глубокие сети
также применялись для решения таких задач, как визуальная
одометрия (Zhan et al. 2018).
Алгоритм ICP (Besl and McKay, 1992) используется для широкого спектра приложений от робототехники до медицинской
визуализации. ICP работает быстро, но определение соответствий через ближайших соседей – вычислительно дорогостоящая операция O(N2). Однако его сложность можно уменьшить,
применив kd-деревья. Было разработано множество вариантов
алгоритма, которые делают подход устойчивым к выбросам
данных и повышают скорость вычислений для больших наборов данных. В Salvi et al. (2007) опубликован обзор и сравнение
некоторых алгоритмов. Определение относительной ориентации между двумя наборами точек является классической задачей, и используемый здесь подход SVD описан в Arun et al.
(1987). Решения, основанные на кватернионах и ортонормированных мат­ри­цах вращения, были описаны Хорном (Horn et al.,
1988; Horn, 1987).
Задача «структура через движение» (structure from motion,
SfM) – одновременное восстановление структуры мира и движения камеры – является классической задачей компьютер-

Подведение итогов 899

ного зрения. Две полезные обзорные статьи, в которых приводится систематика подходов, принадлежат Хуангу и Нетравали
(Huang and Netravali, 1994) и Джебаре с соавт. (Jebara et al., 1999).
Бройда и соавт. (Broida et al., 1990) описали один из первых вариантов рекурсивного метода SfM для последовательности монокулярных камер с использованием EKF, где каждая мировая
точка представлена ее координатой (X, Y, Z). Маклокклэн предоставляет подробное описание оценщика состояния переменной длины для SfM (McLauchlan 1999). В Azarbayejani and Pentland (1995) описывается рекурсивный подход, в котором каждая
мировая точка параметризуется скаляром – ее глубиной по отношению к первому изображению. Более новый вариант алгоритма с уменьшенной ошибкой вычисления описан в Chiuso et
al. (2002). Там же обсуждается проблема изменения масштаба.
Система MonoSlam, предложенная в Davison et al. (2007), представляет собой впечатляющую монокулярную систему SfM, которая поддерживает локальную карту, включающую объекты,
даже если они в данный момент не находятся в поле зрения.
Более современное дополнение в Newcombe et al. (2011) выполняет отслеживание камеры и плотную трехмерную реконструкцию с одной движущейся RGB-камеры. Применение SfM для
крупномасштабного картографирования городов становится
все более популярным, и в Pollefeys et al. (2008) можно найти
описание системы для автономной обработки больших наборов
изображений. Семейство визуальных алгоритмов SLAM ORBSLAM (Campos et al. 2021) – это мощный и популярный способ
вычисления трехмерного пути камеры по последовательности
RGB-, RGBD- или стереоизображений.
Пакетная подстройка или структура через движение (SfM) –
это обширная область с многочисленными публикациями, охва­
тывающими различные варианты задачи, например устойчивость к выбросам, а также конкретные приложения и типы
камер. Классические введения опубликованы в Triggs et al.
(2000) и Hartley and Zisserman (2003). Недавние диссертации
Warren (2015), Sünderhauf (2012) и Strasdat (2012) являются
исчерпывающими и вполне удобочитаемыми. К сожалению,
в каждой работе используются разные обозначения. Оценка
мат­ри­цы камеры для каждого вида, вычисление проективной
реконструкции и последующее обновление ее до евклидовой
реконструкции описаны в Hartley and Zisserman (2003) и Ma et
al. (2003).
Задача SFM может быть упрощена путем использования последовательностей стереоскопических, а не монокулярных изображений (Molton and Brady 2000; Zhang et al. 1992) или путем
добавления инерциальных данных (Strelow and Singh 2004). Популярное введение в визуальную одометрию (VO), состоящее из
двух частей, опубликовано в Scaramuzza and Fraundorfer (2011)

900 Глава 14 · Использование нескольких изображений

и Fraundorfer and Scaramuzza (2012). Визуальная одометрия обсуждается также в Nister et al. (2006), где демонстрируется использование угловых признаков и монокулярного или стереоскопического зрения. В Maimone et al. (2007) обсуждается опыт
использования стереокамеры VO на марсоходе, а в Corke et al.
(2004) описывается монокулярный катадиоптрический VO для
прототипа планетохода.
Мозаика – процесс такой же старый, как фотография. В прошлом это было трудоемким делом, требующим высокой квалификации и применения фотоснимков, скальпелей и наждачной
бумаги. Поверхность Луны и близлежащих планет была выложена мозаикой вручную в 1960-х гг. с использованием изображений, отправленных автоматическими космическими аппаратами. Для создания панорам применяются высококачест­венные
автономные инструменты создания мозаики, например Hugin,
PhotoStitcher и AutoStitch.
Анализ последовательности изображений лежит в основе
многих роботизированных систем машинного зрения, работающих в реальном времени. Отслеживание признаков в реальном времени от кадра к кадру описано в Hager and Toyama
(1998) и Lucas and Kanade (1981) и обычно основано на более дешевых в вычислительном отношении детекторах Харриса или
пирамидальном трекере Канаде–Лукаса–Томаси (KLT). Детекторы SIFT и SURF по-прежнему работают слишком медленно,
хотя некоторые реализации на основе GPU способны работать
в режиме реального времени.

14.9.2

Дополнительные ресурсы

Область компьютерного зрения развивалась благодаря доступности стандартных наборов данных. Это позволило исследователям количественно сравнить результаты работы различных
алгоритмов на одних и тех же данных. Одним из первых наборов пар стереоизображений был набор данных JISCT (Bolles et
al., 1993). Более поздний набор данных Middlebury (Scharstein
and Szeliski, 2002), доступный на http://vision.middlebury.edu/
stereo, содержит обширную коллекцию стереоизображений
с высоким разрешением, снятых при различных настройках
экспозиции и включающих эталонные данные. Стереоизображения с различных марсоходов NASA доступны в интернете
в виде пар «левый+правый» или в виде анаглифов. Наборы данных о движении включают в себя людей, движущихся внутри
здания, http://homepages.inf.ed.ac.uk/rbf/CAVIARDATA1; сцены
дорожного движения, http://i21www.ira.uka.de/image_sequences, и кадры, снятые из движущегося транспортного средства:
http://www.mi.auckland.ac.nz/EISATS.

Подведение итогов 901

Популярная библиотека LIBELAS (http://www.cvlibs.net/software/libelas) для крупномасштабного стереосопоставления поддерживает параллельную обработку с использованием OpenMP
и имеет интерфейсы для MATLAB и ROS. Различные алгоритмы
стереозрения сравниваются по скорости и точности на тестовых сайтах KITTI (www.cvlibs.net/datasets/kitti/eval_scene_flow.
php) и Middlebury (vision.middlebury.edu/stereo/eval3).
Набор инструментов эпиполярной геометрии Epipolar Geometry Toolbox (Marriottini and Prattichizzo 2005) для MATLAB
доступен по адресу http://egt.dii.unisi.it. Он поддерживает
перспективную и катадиоптрическую камеры. Монокулярная
визуальная система SLAM Эндрю Дэвисона (MonoSLAM) для
C и MATLAB доступна по адресу https://www.doc.ic.ac.uk/~ajd/
software.html.
Программное обеспечение для разреженной пакетной подгонки, разработанное Луракисом (www.users.ics.forth.gr/~lourakis/
sba), представляет собой эффективную реализацию на языке C, которая широко используется и имеет оболочку MATLAB и OpenCV. Одним из приложений является Bundler (www.
cs.cornell.edu/~snavely/bundler), которое может выполнять сопоставление точек с тысяч камер в масштабе города и реконструировать такие города, как Рим (Agarwal et al. 2014), Венеция
и Дубровник. Некоторые из этих крупномасштабных наборов
данных доступны на сайтах www.grail.cs.washington.edu/projects/bal и www.cs.cornell.edu/projects/p2f. В число решателей
с открытым исходным кодом, которые можно использовать
для настройки разреженной пакетной корректировки, входят
g2o, SSBA и CERES, все они реализованы на C++. Решатель g2o от
Кюммерле и соавт. (Kümmerle et al., 2011) (https://github.com/
RainerKuemmerle/g2o) также можно использовать для решения
задач SLAM. Решатель SSBA Кристофера Зака доступен по адресу https://github.com/chzach/SSBA. Решатель CERES от Google
(www.ceres-solver.org) представляет собой библиотеку для моделирования и решения больших сложных задач оптимизации
на стационарных и мобильных платформах.
Open3D (http://www.open3d.org) – это крупномасштабный,
открытый и автономный пакет для обработки 2D/3D-изо­бра­
жений и облаков точек с поддержкой детекторов и дескрипторов признаков, 3D-регистрации, kd-деревьев, сегментации
формы, построения сетки поверхности, визуализации, интерфейсов камеры, а также включает g2o. Библиотека абстракции
данных точек (PDAL) (www.pdal.io) – это библиотека и набор
инструментов командной строки Unix для управления данными облака точек.
Облака точек можно хранить в нескольких распространенных открытых форматах. Файлы данных облака точек (PCD)
определяются библиотекой Pointcloud (PCL) (www.pointclouds.

902 Глава 14 · Использование нескольких изображений

org). Файлы в полигональном формате (PLY) предназначены
для описания сеток, но могут использоваться для представления облака точек без сетки. Для просмотра этих файлов разработано несколько замечательных визуализаторов, таких как
MeshLab и potree. Open3D и PDAL могут читать, записывать
и преобразовывать многие форматы файлов облаков точек. Для
представления больших и цветных облаков точек используется
формат LAS, а формат LAZ – его сжатая версия. Cloud Compare
(https://www.danielgm.net/cc/) – кросс-платформенный инструмент с открытым исходным кодом и графическим интерфейсом для отображения, сравнения и редактирования больших
облаков точек, хранящихся в различных форматах.
Исчерпывающую информацию о фундаментальных мат­ри­
цах можно найти на http://danielwedge.com/fmatrix/.

14.9.3

Упражнения

1. Угловые признаки и сопоставление (раздел 14.1). Изучите
интегральное распределение силы углов для признаков Харриса и SIFT. Как правильно выбрать сильные углы для сопоставления признаков?
2. Сопоставление признаков. Мы можем определить качество
сопоставления признаков на основе дескрипторов с точки
зрения процента сторонников после применения RANSAC.
а) Возьмите любое изображение. Сопоставьте это изображение с различными преобразованиями самого себя, чтобы
изучить надежность признаков SURF и Харриса. Преобразования такие: (а) масштабировать интенсивность на
70 %; (б) добавить гауссовский шум со стандартным отклонением 0.05, 0.5 и 2 по значениям серого; (в) масштабировать изображение на 0.9, 0.8, 0.7, 0.6 и 0.5; (г) повернуть на 5°, 10°, 15°, 20°, 30°, 40°.
б) Для детектора Харриса сравните производительность
признака на основе структурного тензора и размеров
патча дескриптора 3×3, 7×7, 11×11 и 15×15.
в) Попробуйте увеличить радиус подавления для углов SURF
и Харриса. Улучшает ли более низкая плотность совпадений качество сопоставления?
г) Существует ли какая-либо корреляция между случайными
совпадениями и силой вовлеченных угловых признаков?
3. Напишите уравнение для эпиполярной линии на втором
изображении при заданной точке на первом изображении.
4. Покажите, что эпиполи – это нуль-пространство фундаментальной мат­ри­цы.
5. Можете ли вы определить мат­ри­цу C для второй камеры,
зная фундаментальную мат­ри­цу и мат­ри­цу первой камеры?

Подведение итогов 903

6. Нахождение фундаментальной мат­ри­цы (раздел 14.2.3).
а) В примере с синтетическими данными измените коли­
чество точек и аддитивный гауссовский шум и исследуйте влияние на невязку.
б) Повторите пример с Эйфелевой башней и проследите эффект изменения параметров RANSAC. Повторите пример
с верхними 50 % признаков SIFT.
в) Какова вероятность того, что в случайной выборке (без
замещения) из N сторонников и M противников выпадут
8 точек – сторонников модели?
7. Эпиполярная геометрия.
а) Создайте две центральные камеры, одну в начале координат, а другую в направлении x. Для разреженной фронто-параллельной сетки точек мира отобразите семейство
эпиполярных линий на втором изображении, которые соответствуют спроецированным точкам на первом изоб­
ражении. Опишите эти эпиполярные линии. Повторите
задание для случая, когда вторая камера перемещается по
оси y и z и вращается вокруг осей x, y и z. Повторите задание
для комбинаций движения, таких как перемещение по оси
x и z или перемещение по оси x и вращение вокруг оси y.
б) В примере на рис. 14.16 эпиполярные линии имеют небольшой наклон вверх. Что это говорит о двух ракурсах
камеры?
8. Существенная мат­ри­ца (раздел 14.2.2).
а) Создайте набор соответствующих точек для камеры, совершающей чисто вращательное движение, и вычислите
фундаментальную и существенную мат­ри­цу. Сможете ли
вы восстановить вращательное движение?
б) Для случая поступательного и вращательного движения
визуализируйте оба положения, полученных в результате разложения основной мат­ри­цы. Нарисуйте их вручную
или используйте CentralCamera.plot.
9. Гомография (раздел 7.2.4).
а) Вычислите евклидовы гомографии для переноса в направлениях x, y и z и для вращения вокруг осей x, y и z.
Преобразуйте их в проективные гомографии и примените к фронто-параллельной сетке точек. Является ли результирующее движение изображения тем, что вы ожидали? Примените эти гомографии в качестве деформации
к реальному изображению, такому как фотография Лены
из примера в этой книге.
б) Разложите гомографию рис. 14.15 (изображение двора),
чтобы определить плоскость стены по отношению к камере. Вам понадобятся внутренние параметры камеры.
в) Измените порядок точек, заданных в points2H, на обратный и покажите, что результатом является обратная гомография.

904 Глава 14 · Использование нескольких изображений

10. Загрузите эталонное изображение обложки этой книги
из файла rvc3_cover.png. Затем сделайте снимок передней
обложки книги, вычислите признаки SIFT, сопоставьте
их и используйте RANSAC для оценки гомографии между
двумя изображениями обложки книги. Разложите гомографию, чтобы оценить вращение и перемещение. Поместите
эти действия в цикл в реальном времени и постоянно отображайте положение книги относительно камеры.
11. Разреженное стереосопоставление (раздел 14.3).
а) Метод пересечения лучей может возвращать наименьшее расстояние между лучами (которое в идеале равно
нулю). Постройте гистограмму ошибки замыкания и вычислите среднюю и максимальную ошибки.
б) Предполагаемая величина перемещения камеры составляла 30 см. Повторите расчеты для 25 и 35 см. Изменились ли статистические показатели ошибок замыкания? Сможете ли вы определить, какая величина
перемещения минимизирует эту ошибку?
12. Пакетная подстройка (раздел 14.3.2).
а) Измените начальное условие для второй камеры, например установите для нее единичную мат­ри­цу.
б) Установите начальное перемещение камеры на 3 м в направлении x и масштабируйте координаты ориентира
в 10 раз. Каково конечное значение ошибки обратной
проекции и положения второй камеры?
в) Поэкспериментируйте с привязкой ориентиров и камер.
г) Выведите два якобиана – A и B.
13. Выведите соотношение для глубины с точки зрения диспаратности для случая граничных камер, т. е. камер с пересекающимися оптическими осями, аналогичных камерам,
показанным на рис. 14.6.
14. Стереозрение (на примере кучи камней, рис. 14.23).
а) Увеличьте изображение диспаратности и изучите значения пикселей на границах изображения и по краям
камней.
б) Поэкспериментируйте с различными мерами подобия
и размерами окна. Какое влияние они оказывают на
изображение диспаратности и время вычислений?
в) Поэкспериментируйте с изменением диапазона диспаратности. Попробуйте [50,90], [30,90], [40,80] и [40,100].
Что происходит с изображением диспаратности и по­
чему?
г) Отобразите эпиполярные линии на втором изображении выбранных точек на первом.
д) Получите изображение пространства диспаратности D.
i.	Для выбранных пикселей (u, v) постройте график зависимости D(u, v, d) от d. Найдите пиксели с острым,

Подведение итогов 905

широким и слабым пиками. Для выбранной строки v
отобразите D(u, v, d) в виде изображения. Что оно
представляет?
ii.	Для конкретного пиксельного графика зависимости
s от d постройте параболу вокруг максимумов и наложите ее на график.
iii.	Используйте необработанные данные из DSI и вычислите местоположение пика с субпиксельным разрешением для каждого пикселя.
iv.	Используйте необработанные данные из DSI, найдите второй пик в каждом пикселе и вычислите коэффициент неоднозначности.
15. Анаглифы (раздел 14.5).
а) Скачайте анаглифное изображение и преобразуйте его
в пару изображений в оттенках серого, а затем вычислите плотное стереосопоставление.
б) Создайте свое анаглифное изображение из стереопары.
в) Напишите код для построения фигур, например квадратов, появляющихся на разной глубине. Или одной фигуры, которая перемещается вглубь экрана и обратно.
г) Напишите код для построения трехмерной фигуры, например куба.
16. Стереозрение. Для пары идентичных камер с фокусным
расстоянием 8 мм, сенсором 1000×1000 пикселей, которые
составляют квадрат со стороной 10 мкм на базисной линии
80 мм и с параллельными оптическими осями:
а) нарисуйте поля обзора камеры на виде сверху. Если
камеры смотрят на плоскую поверхность, перпендикулярную главным осям, какова ширина горизонтального
перекрывающегося поля зрения в единицах пикселей?
б) предполагая, что ошибка диспаратности имеет нормальное распределение с σ = 0.1 пикселя, вычислите
и постройте график распределения ошибки по координате z реконструированных 3D-точек, которые имеют
среднюю диспаратность 0.5, 1, 2, 5, 10 и 20 пикселей.
Отобразите 1000 случайных значений диспаратности,
преобразуйте их в Z и постройте гистограмму (распределение) их значений.
17. Мона Лиза на вашей стене. Получите каким-либо способом
изображение комнаты в вашем доме и отобразите его с помощью Toolbox. Выберите четыре точки, используя ginput,
чтобы определить углы виртуальной рамки на стене. Можно использовать углы существующей прямоугольной детали в вашей комнате, например окна, плаката или картины.
Найдите соответствующую гомографию, деформируйте
изображение Моны Лизы и вставьте его в исходное изображение вашей комнаты.

906 Глава 14 · Использование нескольких изображений

18. Подбор плоскостей (раздел 14.7.1).
а) Проверьте устойчивость алгоритма подбора плоскости
к аддитивному шуму и точкам-выбросам.
б) Реализуйте итеративный подход со взвешиванием, чтобы минимизировать влияние выбросов.
в) Создайте алгоритм подбора плоскости на основе
RANSAC, который берет случайные выборки из трех точек для решения уравнения плоскости (раздел C.1.3).
19. ICP (раздел 14.7.2).
а) Измените начальное относительное положение облаков
точек. Попробуйте выполнить очень большие повороты.
б) Исследуйте надежность ICP, смоделировав некоторые
реалистичные ошибки датчиков. Например, добавьте гауссовский или импульсный шум к точкам данных
больше ложных точек данных.
в) Как меняется эффективность сопоставления при увеличении и уменьшении количества точек данных?
г) Исследуйте все варианты ICP, предоставляемые функциями Open3D.
20. Коррекция перспективы (раздел 14.8.1).
а) Создайте виртуальный ракурс, смотрящий вниз под
углом 45° к фасаду собора.
б) Создайте виртуальный ракурс с исходной высоты камеры, но с поворотом камеры на 20° влево.
в) Найдите другое реальное изображение с искажением
перспективы и попытайтесь ее исправить.
21. Мозаика (раздел 14.8.2).
а) Запустите файл примера mosaic и посмотрите, как собирается вся мозаика.
б) Измените способ вставки фрагмента в составное изоб­
ражение и используйте простую вставку пикселей вмес­
то усреднения.
в) Измените способ вставки фрагмента в составное изоб­
ражение, чтобы использовались пиксели, расположенные ближе всего к главной точке.
г) Запустите программу на наборе собственных перекрывающихся изображений и создайте панораму.
22. Стабилизацию изображения можно использовать для виртуальной стабилизации неустойчивой камеры, например
ручной, на дроне или мобильном роботе, пересекающем
пересеченную местность. Захватите короткую последовательность изображений I0, I1, ..., IN-1 с неустойчивой камеры. Для кадра i, где i ≥ 1, найдите гомографию относительно
кадра 0, соответствующим образом деформируйте изображение и сохраните его в массиве. Отобразите стабилизированную последовательность изображений.

Подведение итогов 907

23. Визуальная одометрия (раздел 14.8.2). Сделайте следующее:
а) Используйте признаки Харриса или SIFT вместо ORB.
Как это повлияло на точность и время выполнения?
б) Отследите объекты от кадра к кадру и отобразите векторы смещения (оптического потока) в каждом кадре.
в) Используя масштабно-инвариантный точечный объект,
отследите точки на нескольких кадрах. Создайте структуру данных для управления жизненным циклом каждой точки: она должна сначала появляться, потом появляться еще раз и исчезать.
г) Распределите признаки более равномерно по сцене, исследуйте метод gridify объектов Feature.
д) На каждом временном шаге постройте невязки фундаментальных матриц (их две). Есть ли здесь закономерность? Подберите параметры RANSAC, чтобы уменьшить количество сбоев пакетной подстройки.
е) Используйте надежный пакетный подстройщик. Для
этого либо найдите готовый, либо реализуйте его самостоятельно (это сложнее).
ж) Примените фильтр Калмана к транспортному средству
с простой динамикой, чтобы сгладить оценки скорости.
з) Исследуйте статистику ошибок пакетной подстройки по
всем кадрам. Помогает ли увеличение количества итераций?
и) Пакетная подстройка инициализирует вторую камеру,
устанавливая ее в начало координат. Повысится ли качество, если каждый раз смещать камеру в значение,
определенное по предыдущему кадру?
к) Повторите пакетную подстройку с тремя камерами: левой и правой камерами на текущем временном шаге
и левой камерой на предыдущем шаге.
24. Самостоятельно изучите kd-деревья. Какие задачи в этой
главе можно решить с помощью kd-деревьев?

Часть V	Управление
на основе машинного
зрения
Глава 15
Глава 16

Управление на основе зрения
Продвинутое визуальное сервоуправление

О роботе принято говорить, что он движется к объекту, но на самом деле робот
просто перемещается в положение, в котором, по его ожиданиям, должен находиться объект. Это тонкое, но важное различие. Как следствие, робот не сможет схватить объект, если тот не находится в ожидаемом положении. Он также
потерпит неудачу, если из-за несовершенства механизмов или контроллера
рабочий орган не достигнет заданного положения. Для успешного применения этого традиционного подхода необходимо решить две довольно сложные
задачи: определить положение объекта и обеспечить достижение этого положения роботом.
Первая задача – определение положения объекта – обычно решается способом, гарантирующим точное расположение объекта. Для этого требуются
дорогостоящие механические приспособления и крепления, изготавливаемые
и настраиваемые для каждой отдельной детали, с которой должен взаимодействовать робот, что несколько снижает гибкость роботизированной автоматизации.

« Корень проблемы в том, что робот не может «видеть», что он делает.

Представьте, что робот видит объект и свой рабочий орган и может использовать эту информацию для точного управления рабочим органом. Это то,
что люди называют зрительно-моторной координацией, а мы будем называть
управлением на основе зрения, или визуальным сервоуправлением, т. е. использованием информации с одной или нескольких камер для управления роботом с целью выполнения задачи.
Положение цели не обязательно должно быть известно заранее. Робот движется к наблюдаемой цели, где бы она ни находилась в пределах рабочей зоны.
Такой подход имеет множество преимуществ: расширяются допуски на положение детали, почти автоматически появляется возможность работы с движущимися деталями, а любые погрешности в точности действий робота можно
компенсировать.
Система управления на основе зрения предполагает непрерывную оценку
положения цели и самого робота, используя зрение для формирования обратной связи и перемещения манипулятора робота, пока визуально наблюдаемая
погрешность между роботом и целью не станет равной нулю. Управление на
основе зрения существенно отличается от получения изображения, определения местоположения цели и последующего ее достижения. Преимущество непрерывного измерения и обратной связи заключается в высокой устойчивости
к любым ошибкам в системе.
В этой части книги мы объединим многое из того, что узнали ранее: кинематику и динамику роботизированных манипуляторов и мобильных роботов,
гео­метрические аспекты формирования изображений и выделение признаков.
Часть состоит из двух глав. В главе 15 рассматриваются два классических подхода к визуальному управлению, известных как визуальное сервоуправление
на основе местоположения и визуальное сервоуправление на основе изображения. Координаты объектов на изображении используются для перемещения
робота в желаемое положение относительно наблюдаемого объекта. Первый
подход требует явной оценки положения объекта по его характеристикам, но
поскольку он работает в замкнутом цикле, любые ошибки в оценке положения
компенсируются автоматически. Второй подход не требует оценки положения

и напрямую использует информацию о плоскости изображения. Оба подхода
рассматриваются в контексте перспективной камеры, которая может свободно перемещаться в трех измерениях, и описываются их преимущества и недостатки. В главе также обсуждается проблема определения глубины объекта
и использование линейных и эллиптических признаков изображения.
В главе 16 обсуждается гибридный алгоритм визуального сервоуправления,
который преодолевает ограничения визуального управления на основе мес­
тоположения и управления на основе изображения, используя лучшие возможности обоих. Затем обсуждение распространяется на неперспективные
камеры, такие как объективы типа «рыбий глаз» и катадиоптрическую оптику,
а также на роботов-манипуляторов, голономных и неголономных наземных
роботов и воздушных роботов.
Эта часть книги написана на более высоком уровне, чем предыдущие. Она
предполагает хорошее знакомство с остальной частью книги, и все основные
примеры представлены в виде кратких набросков, а не в виде подробного
описания. В тексте приводятся основные математические и алгоритмические
принципы каждого метода, но полное описание можно найти в исходном коде
классов Python, реализующих регуляторы и контроллеры, или в блок-схемах.
Кроме того, обсуждаемые здесь результаты трудно описать и изобразить
в книге, поэтому я настоятельно рекомендую запускать соответствующий код
Python bdsim и исследовать генерируемые графики или внимательно просмат­
ривать анимацию.

Глава

15

	 Управление
на основе зрения

Задача визуального сервоуправления заключается в управлении положением рабочего органа робота относительно целевого объекта, используя визуальные признаки, извлекаемые из
изображения целевого объекта. Этими признаками могут быть
центроиды точек, уравнения линий или эллипсов, а также яркостные характеристики пикселей. Изображение цели является
функцией ее положения относительно камеры CξG, как и признаки, извлекаемые из изображения.
Существуют две возможные конфигурации камеры и робота. Конфигурация на рис. 15.1а предполагает, что камера
установлена на рабочем органе робота и наблюдает за целью.
Такая конфигурация называется конфигурация с замкнутым
контуром, или «глаз в руке». Конфигурация на рис. 15.1б предполагает, что камера находится в фиксированной точке мира
и наблюдает как за целью, так и за рабочим органом робота.
Такая конфигурация называется конфигурация с разомкнутым
контуром. В оставшейся части книги мы рассмотрим только
конфигурацию «глаз в руке». Система управления на основе
зрения может работать и с другими типами роботов, не только
с манипуляторами: камера может быть прикреплена к мобильному наземному или воздушному роботу, и мы рассмотрим их
в следующей главе.

chap15.ipynb

https://go.sn.pub/
WETNOx

а
б
Рис. 15.1. Конфигурации визуальных сервоприводов в виде графа положений. Системы координат: мировая {0},
рабочего органа {E}, камеры {C} и цели {G}: а) конфигурация с замкнутым контуром (глаз в руке); б) конфигурация
с разомкнутым контуром

Управление на основе зрения 913

Для каждой конфигурации существует два принципиально
разных подхода к управлению: визуальное сервоуправление
на основе местоположения (Position-Based Visual Servo, PBVS)
и визуальное сервоуправление на основе изображения (ImageBased Visual Servo, IBVS). В визуальном сервоуправлении на
основе местоположения (рис. 15.2a) используются наблюдаемые визуальные признаки, калиброванная камера и известная
геометрическая модель целевого объекта для определения его
положения относительно камеры. Затем робот перемещается
в заданное положение, и все управление осуществляется в пространстве задач. Существуют хорошие алгоритмы для оценки
положения (см. раздел 13.2.4), но они критически зависят от
точности калибровки камеры и геометрической модели объекта. PBVS обсуждается в разделе 15.1.
В визуальном сервоуправлении на основе изображения
(рис. 15.2б) отсутствует этап оценки положения и непосредственно используются признаки изображения. Управление осуществляется в пространстве координат изображения ℝ2. Требуемое положение камеры относительно цели определяется
неявно – значениями признаков изображения. IBVS – сложная
задача управления, потому что характеристики изображения
являются сильно нелинейной функцией от местоположения
камеры. IBVS обсуждается в разделе 15.2.

+
–

Управление
на основе
положения

Контроллер
сочленений
Обратная связь от энкодера

Оценка
положения

а

+
–

Управление
на основе
изображения

Извлечение
признаков

Контроллер
сочленений
Обратная связь от энкодера

б

Извлечение
признаков

Рис. 15.2. Два разных класса систем визуального сервоуправления: а) визуальное
сервоуправление на основе местоположения; б) визуальное сервоуправление на основе
изображения

914 Глава 15 · Управление на основе зрения

15.1

В изуальное сервоуправление
на основе местоположения

Для системы PBVS отношения между соответствующими положениями показаны в виде графа на рис. 15.3. Наша задача – переместить камеру из исходного положения {C} в желаемое {C∗},
которое определяется относительно целевой системы коор∗
динат {G} относительным положением C ξG. Критический цикл
графа положений, обозначенный пунктирной черной стрелкой:
∗

ξ Δ ⊕ C ξG = CξG,

(15.1)

где CξG – текущее относительное положение цели относительно
камеры. Однако мы не можем определить ее напрямую, потому что положение цели в мировой системе координат, ξG, показанное серой стрелкой, неизвестно. Вместо этого мы будем
использовать оценку CξˆG положения цели относительно камеры,
полученную на основе зрения. Оценка положения обобщенного
объекта с известной геометрией обсуждалась в разделе 13.2.4,
реперного маркера – в разделе 13.6.1. В обоих случаях требуется
калиброванная камера.

ξC ∗
ξC

{C ∗}

ξ∆
{C}

ξG

C

ξˆG

ξG

C∗

C

{G}
{O}

ξG

Рис. 15.3. Граф положений для примера
PBVS. {C} определяет текущее положение
камеры, {C*} – желаемое положение
камеры, а {G} – целевой объект. Имена
переменных Python показаны в серых
рамках: оценки (^) обозначены
суффиксом e, а желаемые значения (*) –
суффиксом d

Перепишем (15.1) как
∗

ξ Δ = CξˆG ⊝ C ξG.

Это соотношение описывает перемещение камеры в системе
координат самой камеры, которое необходимо выполнить, чтобы достичь желаемого положения относительно цели. Изменение положения может быть довольно значительным, поэтому
мы не будем пытаться совершить это перемещение за один шаг,
а будем перемещаться в точку, более близкую к {C*}
ξC〈k + 1〉 ← ξC〈k〉 ⊕ Λ(ξ Δ〈k〉, λ),

что составляет долю λ ∈ (0, 1) от требуемого перемещения и поворота – Λ(·) является интерполяцией, как обсуждалось в разделе 3.3.5.

Визуальное сервоуправление на основе местоположения 915

Используя пакет Toolbox, определим камеру с внутренними
параметрами по умолчанию
>>> camera = CentralCamera.Default(pose=SE3.Trans(1, 1, -2));

и заданным начальным положением. Цель состоит из четырех точек, образующих квадрат со стороной 0.5 м, лежащий
в плос­кости xy и центрированный относительно начала координат {G}:
>>> P = mkgrid(2, side=0.5)
array([[ -0.25,
-0.25,
[ -0.25,
0.25,
[
0,
0,

0.25,
0.25,
0,

0.25],
-0.25],
0]])

Предположим, что положение цели {G} неизвестно системе
управления. Получим проекцию точек на плоскость изобра­
жения
>>> p = camera.project_point(P, objpose=SE3.Tz(1))
array([[ 166.7,
166.7,
300,
300],
[ 166.7,
300,
300,
166.7]])
В коде мы пред- откуда можно оценить положение цели относительно
ставляем XξY как Cξˆ
.
G
T_X_Y – экземпляр
SE3, который
>>> Te_C_G = camera.estpose(P, p, frame="camera");
следует читать как
«преобразование >>> Te_C_G.printline()
из X в Y». t = -1, -1, 3; rpy/zyx = 0°, 0°, 0°

камеры

В данном случае цель {G} находится на расстоянии 3 м перед
камерой. Требуемое относительное положение – на расстоянии
1 м перед камерой с фронтально-параллельной ориентацией
>>> T_Cd_G = SE3.Tz(1);

поэтому необходимым изменением положения камеры яв­
ляется
>>> T_delta = Te_C_G * T_Cd_G.inv();
>>> T_delta.printline()
t = -1, -1, 2; rpy/zyx = 0°, 0°, 0°
Чтобы гарантировать, что обновНовое положение камеры, учитывая λ = 0.05:
ленное положение
является надлежа- >>> camera.pose = camera.pose * T_delta.interp1(0.05);
щей мат­ри­цей SE(3),
ее необходимо
Мы повторяем процесс перемещения к требуемому относинормализовать
в каждой итерации тельному положению, пока не достигнем цели. Таким образом,
или выполнять даже если цель сама перемещается или робот совершает ошибкомпозицию с использованием ки и движется неточно, то перемещение, вычисленное на слеоператора @. дующем шаге, учтет эту ошибку.

916 Глава 15 · Управление на основе зрения

В Toolbox имеется класс, моделирующий систему PBVS. Он
отображает результаты графически и сохраняет историю перемещений камеры и связанные с ними величины. Начнем, как
и прежде, с определения камеры в некотором начальном положении
>>> camera = CentralCamera.Default(pose = SE3.Trans(1, 1, -2));

и желаемого положения цели относительно камеры – на расстоя­
нии 1 м от камеры с фронтально-параллельной ориентацией
>>> T_Cd_G = SE3.Tz(1);

для чего создадим экземпляр класса PBVS
>>> pbvs = PBVS(camera, P=P, pose_g=SE3.Trans(-1, -1, 2),
...
pose_d=T_Cd_G, plotvol=[-1, 2, -1, 2, -3, 2.5])
Visual servo object: camera=default perspective camera
100 iterations, 0 history
cdTg: t = 0, 0, 1; rpy/yxz = 0°, 0°, 0°

который является подклассом класса VisualServo и реализует
упомянутый выше контроллер. В качестве аргументов конструктор принимает объект CentralCamera, имеющий начальное
положение, положения точек в целевой системе координат {G},
положение цели {G}, необходимое для проецирования точек
и оценки положения, и требуемое положение камеры относительно {G}. Метод run реализует алгоритм PBVS, который управляет камерой
>>> pbvs.run(200);

и в данном примере выполняет симуляцию на протяжении
200 временных шагов, многократно вызывая метод step объекта
для имитации движения в течение одного временного шага. ►
Симуляция создает анимационный эффект движения объектов
в плоскости изображения камеры и трехмерную визуализацию
камеры и точек мира, как показано на рис. 15.4. Она завершается после определенного количества итераций или когда ||ξ Δ||
оказывается ниже некоторого порогового значения. В данном
случае моделирование останавливается через 120 временных
шагов, когда требуемая скорость опускается ниже порогового
значения по умолчанию.
Результаты симуляции сохраняются в объекте для последующего анализа. Мы можем построить график траектории движения
целевых объектов на изображении, скорости камеры в зависимости от времени или положения камеры в зависимости от времени.
>>> pbvs.plot_p();
# построить траекторию на плоскости изображения
>>> pbvs.plot_vel(); # построить график скорости камеры
>>> pbvs.plot_pose(); # построить траекторию камеры

Положение объекта
camera обновляется
на каждом временном шаге. Начальное положение
камеры сохраняется при создании
объекта и восстанавливается при
каждом вызове run
с помощью метода
reset камеры.

Визуальное сервоуправление на основе изображения 917

v (пиксели)

перспективная камера по умолчанию

u (пиксели)

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

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

15.2

Визуальное сервоуправление
на основе изображения

IBVS принципиально отличается от PBVS тем, что не оценивает
положение относительно цели. Относительное положение фактически подразумевается значениями признаков изображения.
На рис. 15.6 показаны два вида квадратного объекта, определяемого четырьмя характерными точками. Вид из исходного
положения камеры показан красным, и на нем видно, что камера смотрит на объект под углом. Желаемый, или целевой,
вид показан синим, где камера находится дальше от объекта,
а ее оптическая ось перпендикулярна плоскости объекта – это
фронтально-параллельный вид.
Задачу управления можно выразить в координатах изображения. Она состоит в том, чтобы переместить характерные точки, обозначенные круглыми маркерами, в точки, обозначенные
звездчатыми маркерами. Точки могут, но не обязаны, следовать прямолинейным траекториям, обозначенным стрелками.
Перемещение характерных точек на изображении неявно изменяет положение камеры, т. е. мы переходим от оценки поло-

918 Глава 15 · Управление на основе зрения

v (пиксели)

начальное положение
целевое положение

u (пиксели)

Декартова скорость

а

Шаг по времени

Ориентация
камеры (рад)

Местоположение
камеры (м)

б

в

Шаг по времени

Рис. 15.5. Результаты
моделирования PBVS:
а) движение точек на
плоскости изображения;
б) декартова скорость;
в) положение камеры, при
котором углы крена, тангажа
и рыскания находятся
в порядке YXZ камеры

Визуальное сервоуправление на основе изображения 919

начальное положение

v (пиксели)

целевое положение

Рис. 15.6. Два вида квадратного
объекта: исходный и целевой

u (пиксели)

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

15.2.1

Движение камеры и изображения

Рассмотрим камеру по умолчанию
>>> camera = CentralCamera.Default();

и точку мира в
>>> P = [1, 1, 5];

которая на изображении имеет координаты
>>> p0 = camera.project_point(P)
array([[
660],
[
660]])ё

Если немного сместить камеру в направлении x, то координаты в пикселях станут
>>> p_dx = camera.project_point(P, pose=SE3.Tx(0.1))
array([[
644],
[
660]])

Используя систему координат камеры, показанную на рис. 13.5,
можно сказать, что камера переместилась вправо, поэтому точка на изображении переместилась влево. Чувствительность
движения изображения к движению камеры Δр/Δх:

920 Глава 15 · Управление на основе зрения
>>> (p_dx - p0) / 0.1
array([[
-160],
[
0]])

что является приближением к производной ∂p/∂x. То же действие можно повторить для перемещения по оси z
>>> (camera.project_point(P, pose=SE3.Tz(0.1) ) - p0) / 0.1
array([[ 32.65],
[ 32.65]])

В результате получаем равномерное движение плоскости
изображения в направлениях u и v. Для поворота вокруг оси x:
>>> (camera.project_point(P, pose=SE3.Rx(0.1)) - p0) / 0.1
array([[ 40.96],
[ 851.9]])

Движение изображения происходит преимущественно в направлении v. Очевидно, что движение камеры вдоль и вокруг
различных степеней свободы в трехмерном пространстве приводит к совершенно разным движениям точек изображения.
Ранее в (13.11) мы выразили перспективную проекцию в функциональной форме:
p = 𝒫(P, K, ξC).

(15.2)

Ее производная по времени равна
(15.3)

где Jp – объект, подобный якобиану, но поскольку мы взяли
производную по положению ξ ∈ SE(3), а не по вектору, то технически она называется мат­ри­цей взаимодействия. Однако
в мире визуального сервоуправления ее чаще называют яко­
бианом изображения, или мат­ри­цей чувствительности к при­
знакам. Это локальная линеаризация для сильно нелинейной
функции (15.2). Камера движется в трехмерном пространстве,
и ее скорость определяется пространственной скоростью v =
(υx, υy, υz, ωx, ωy, ωz) ∈ ℝ6, понятие которой введено в разделе 3.1
и включает поступательную и вращательную составляющие
скорости.
Рассмотрим камеру, движущуюся со скоростью тела v = (υ, ω)
в мировой системе координат и наблюдающую точку мира P,
описываемую вектором относительных координат камеры CP =
(X, Y, Z). Скорость точки в системе координат камеры равна
Ṗ = -(ω × CP + υ),

C

(15.4)

См. приложение E.

Визуальное сервоуправление на основе изображения 921

которую можно записать в скалярной форме как
Ẋ = Yωz - Zωy - υx
Ẏ = Zωx - Xωz - υy
Ż = Xωy - Yωx - υz.

(15.5)

Перспективная проекция (13.2) для нормализованных координат плоскости изображения

а временная производная, с использованием правила деления,
равна

Подставив X = xZ и Y = yZ, мы можем записать все это в мат­
ричной форме

(15.6)

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

которое можно реорганизовать как
(15.7)
где u– = u - u0 и –v = v - v0 – координаты пикселя относительно
главной точки. Временная производная равна
(15.8)

922 Глава 15 · Управление на основе зрения

И подстановка (15.7) и (15.8) в (15.6) приводит к

Обратите внимание, что
и
.
Для типичного случая, когда ρu = ρv = ρ, фокусное расстояние
можно выразить в пикселях f ¢ = f/ρ и записать

(15.9)

в пиксельных координатах относительно главной точки. В крат­
кой матричной форме это можно записать как
ṗ = Jp(p, Z)v,

(15.10)

где Jp – это мат­ри­ца Якоби изображения 2×6 для точечного
объекта с координатами p = (u–, v–) и Z, которая представляет
z-координату точки в системе координат камеры и часто называется глубиной точки.
Класс CentralCamera имеет метод visjac_p для вычисления якобиана изображения, и для приведенного выше примера это
>>> J = camera.visjac_p(p0, depth=5)
array([[
-160,
0,
32,
[
0,
-160,
32,

32,
832,

-832,
-32,

160],
-160]])

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

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

Визуальное сервоуправление на основе изображения 923

точки изображения, вызванную одной единицей соответствующего компонента вектора скорости. Метод flowfield класса
CentralCamera показывает скорость сетки точек мира в плоскости изображения, спроецированных на плоскость изображения,
для конкретной скорости камеры. Для поступательной скорости камеры в направлении оси x поле потока
>>> camera.flowfield([1, 0, 0, 0, 0, 0]);

показано на рис. 15.7а. Как и ожидалось, перемещение камеры вправо приводит к перемещению всех проецируемых точек
влево. Движение точек на плоскости изображения называется
оптическим потоком и может быть вычислено на основе последовательностей изображений путем отслеживания характерных признаков, как обсуждалось в разделе 14.8.3. Уравнение (15.9) часто называют уравнением оптического потока.
При перемещении в направлении z
>>> camera.flowfield([0, 0, 1, 0, 0, 0]);

точки расходятся лучами от главной точки (рис. 15.7б), воспроизводя эффект искривления (как в «Звездном пути»). Поворот
вокруг оси Z
>>> camera.flowfield([0, 0, 0, 0, 0, 1]);

заставляет точки вращаться вокруг главной точки, как показано
на рис. 15.7в.
Вращательное движение вокруг оси y
>>> camera.flowfield([0, 0, 0, 0, 1, 0]);

показано на рис. 15.7г и очень похоже на случай переноса вдоль
оси x с небольшой кривизной для точек, удаленных от главной точки. Это сходство обусловлено тем, что первый и пятый
столбцы якобиана изображения в этом случае приблизительно
равны.
Любая точка на оптической оси будет проецироваться в главную точку, и на глубине 1 м якобиан изображения будет равен
>>> camera.visjac_p(camera.pp, depth=1)
array([[
-800,
0,
0,
[
0,
-800,
0,

0,
800,

-800,
0,

0],
0]])

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

924 Глава 15 · Управление на основе зрения

приближается к значению, кратному первому столбцу. Увеличение фокусного расстояния до f = 20 мм (фокусное расстояние
по умолчанию 8 мм) приводит к полю потока
>>> camera.f = 20e-3;
>>> camera.flowfield([0, 0, 0, 0, 1, 0]);

показанному на рис. 15.7д, который практически идентичен
полю на рис. 15.7а. Напротив, при малых фокусных расстояниях
(широкоугольные камеры) поле потока
>>> camera.f = 4e-3;
>>> camera.flowfield([0, 0, 0, 0, 1, 0]);

показанное на рис. 15.7е, имеет гораздо более выраженную
кривизну. То же верно в отношении второго и четвертого столбцов, за исключением разницы знаков: существует эквивалентность между переносом в направлении оси y и вращением вокруг оси -x.
Кривизна в широкоугольном поле потока, показанная на
рис. 15.7е, означает, что неоднозначность между поступательным и вращательным движениями, обсуждавшаяся ранее, разрешена. Края поля зрения содержат сигналы движения, различающие их. С увеличением поля зрения этот эффект становится
более выраженным. Для узкого поля зрения, например для наших собственных глаз, нам нужна дополнительная информация
от датчиков, чтобы разрешить эту неоднозначность, и датчики
угловой скорости в нашей вестибулярной системе помогают
в этом.
Матрица Якоби (15.9) обладает некоторыми интересными
свойствами. Она совершенно не зависит от мировых координат
X или Y, а только от координат плоскости изображения (u–, v–).
Однако первые три столбца зависят от глубины точки Z, поэтому для перемещающейся камеры скорость плоскости изображения обратно пропорциональна глубине. И снова это легко
продемонстрировать на практике: сместите голову в сторону,
не поворачивая ее, и обратите внимание, что близкие объекты
в поле зрения смещаются сильнее, чем дальние. Однако при повороте головы все объекты, как близкие, так и дальние, смещаются в поле зрения одинаково.
Матрица имеет ранг 2
и, следовательно, четырехмерное
нулевое пространство. Нуль-пространство содержит набор векторов пространственной скорости, которые по отдельности или

Ранг не может быть
меньше 2, даже
если Z → ∞.

Визуальное сервоуправление на основе изображения 925

перенос вдоль оси z

v (пиксели)

v (пиксели)

перенос вдоль оси x

u (пиксели)

u (пиксели)

поворот вокруг оси y, f = 8 мм

v (пиксели)

v (пиксели)

перенос вдоль оси z

u (пиксели)

u (пиксели)

поворот вокруг оси y, f = 4 мм

v (пиксели)

v (пиксели)

поворот вокруг оси y, f = 20 мм

u (пиксели)

u (пиксели)

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

926 Глава 15 · Управление на основе зрения

в любой линейной комбинации приводят к тому, что точка мира
не движется на изображении. Рассмотрим простую точку мира,
лежащую на оптической оси, которая проецируется в главную
точку
>>> J = camera.visjac_p(camera.pp, depth=1);

Нуль-пространство якобиана:
>>> linalg.null_space(J)
array([[
0,
0, -0.7071,
[
0, 0.7071,
0,
[
1,
0,
0,
[
0, 0.7071,
0,
[
0,
0, 0.7071,
[
0,
0,
0,

0],
0],
0],
0],
0],
1]])

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

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

(15.11)

Визуальное сервоуправление на основе изображения 927

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

15.2.2

Управление движением объекта

До сих пор мы показывали, как перемещаются точки в плоскости изображения вследствие движения камеры. Однако, как
это часто случается, не менее полезной оказывается обратная
задача – выяснение, какое движение должна совершить камера для перемещения элементов изображения с желаемой скоростью.
Для случая трех точек {(ui, vi), i = 0, 1, 2} и соответствующих
скоростей {(u̇i, v̇i)} можно инвертировать (15.11):

(15.12)
Обратите внимание,
что в работах, основанных на подходе
с целевой функцией
(Espiau et al. 1992),
это записывается
как фактическое положение минус желаемое и в (15.14)
используется –λ,
чтобы обеспечить
отрицательную
обратную связь.
Мы же используем
здесь общепринятое соглашение из
теории управления,
где сигнал ошибки
«желаемое минус
фактическое».

Нам нужна глубина
точки Z, но мы
рассмотрим этот
вопрос чуть позже.

и найти требуемую пространственную скорость камеры. Остается лишь получить ответ на вопрос: как определить скорость
точки? Обычно мы используем простой линейный регулятор
ṗ∗ = λ(p∗ - p),

(15.13)

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

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

928 Глава 15 · Управление на основе зрения
 оответствие между наблюдаемыми и желаемыми
С
положениями важно.
При вычислении (15.13) крайне важно, чтобы точки pi∗ и pi соответствовали друг другу. В вымышленной сцене эти точки могут быть
центроидами фигур с уникальными цветами, размерами или формами. В более общем случае соответствие может быть установлено,
если одна точка уникальна и известно расположение точек относительно их центроида.

В общем случае, когда число точек N > 3, можно объединить
якобианы всех объектов и вычислить скорость камеры, используя псевдообращение

(15.14)

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

(15.15)

взяв средние значения псевдообратных якобианов изображения в текущем и желаемом состояниях.
При N ≥ 3 мат­ри­ца может быть плохо обусловлена, например если точки почти совпадают или коллинеарны. На практике это означает, что некоторые движения камеры приведут
к очень небольшим смещениям изображения, т. е. движение
будет малозаметным. Это очень похоже на концепцию манипулируемости, обсуждавшуюся в разделе 8.3.2, и аналогичный
подход можно применить к формализации восприимчивости.
Рассмотрим камеру с единичной пространственной скоростью
vTv = 1,

Визуальное сервоуправление на основе изображения 929

и из (15.10) мы можем вывести скорость камеры через псевдообращение
v = J+ṗ,
где J ∈ ℝ2N×6 – объединение якобианов, а ṗ ∈ ℝ2N – скорости точек.
Подстановка дает
ṗT J+T J+ṗ = 1,

ṗT(JJT)-1ṗ = 1,
уравнение эллипса в пространстве скоростей плоскости изображения. Собственные векторы JJT определяют главные оси
эллипса, а собственные значения J являются радиусами. Отношение максимального радиуса к минимальному определяется
числом обусловленности JJT и определяет анизотропию движения объекта. Высокое значение указывает, что некоторые точки
имеют низкую скорость в ответ на некоторые движения камеры. Альтернативой объединению всех якобианов характерных
точек в (15.14) является выборка всего трех из них, которые при
объединении дадут наилучшую обусловленную квадратную
мат­ри­цу, которую затем можно обратить.
При использовании пакета Toolbox сначала необходимо
определить камеру с внутренними параметрами по умолчанию
>>> camera = CentralCamera.Default(pose=SE3.Trans(1, 1, -2));

и в заданном начальном положении. Цель состоит из четырех
точек, образующих квадрат со стороной 0.5 м, лежащий в плос­
кости xy с центром в точке (0, 0, 3):
>>> P = mkgrid(2, side=0.5, pose=SE3.Tz(3));

В целевом положении характерные точки образуют на плос­
кости изображения квадрат 400×400 с центром в главной точке
>>> pd = 200 * np.array([[-1, -1, 1, 1], [-1, 1, 1, -1]])
...
+ np.c_[camera.pp]
array([[
300,
300,
700,
700],
[
300,
700,
700,
300]])

и фронтально-параллельной ориентацией по отношению к камере. Начальная проекция камеры на мировые точки:
>>> p = camera.project_point(P)
array([[
300,
300,
380,
[
300,
380,
380,

380],
300]])

Таким образом, p и pd имеют по одному столбцу на точку.

930 Глава 15 · Управление на основе зрения

Вычислим ошибку плоскости изображения
>>> e = pd - p
array([[
0,
[
0,

0,
320,

320,
320,

320],
0]])

и якобиан объединенного изображения
>>> J = camera.visjac_p(p, depth=1);

В данном случае это мат­ри­ца размером 8×6, поскольку p содержит четыре точки. Для якобиана необходима глубина точки,
которую мы не знаем, поэтому пока просто выберем постоянное значение Z = 1. ► Это важная тема, и мы рассмотрим ее
в разделе 15.2.3.
Закон управления определяет требуемую поступательную
и угловую скорость камеры ►:
>>> lmbda = 0.1;
>>> v = lmbda * np.linalg.pinv(J) @ e.flatten(order="F")
array([
-0.1,
-0.1,
0.4,
0,
0,

0])

где lmbda – коэффициент усиления, положительное число, и выполняется псевдообращение неквадратного якобиана для реализации (15.14). Результирующая скорость выражается в системе координат камеры, и ее интегрирование за единичный шаг
времени приводит к пространственному смещению на ту же
величину. Положение камеры обновляется как
ξC〈k + 1〉 ← ξC〈k〉 ⊕ Δ-1(v〈k〉),

где Δ-1(·) описывается в разделе 3.1.5. С помощью Toolbox это
реализуется так:
>>> camera.pose = camera.pose @ SE3.Delta(v);

где оператор @ гарантирует, что преобразование останется допустимой мат­ри­цей SE(3).
Так же как в примере с PBVS, создадим экземпляр класса IBVS
>>> camera = CentralCamera.Default(pose=SE3.Trans(1, 1, -3)
...
* SE3.Rz(0.6));
>>> ibvs = IBVS(camera, P=P, p_d=pd);

который является подклассом класса VisualServo и реализует
описанный выше регулятор. Конструктор объекта принимает
объект CentralCamera с заданным начальным положением. Затем
регулятор управляет камерой для достижения желаемой конфигурации точек на плоскости изображения, заданной пара­
мет­ром p_d. Выполним симуляцию за 25 временных шагов
>>> ibvs.run(25);

Здесь мы приводим одно значение,
которое принимается за глубину
всех точек. Однако
мы также могли
бы предоставить
одномерный массив,
указав в нем глубину каждой точки по
отдельности.
Чтобы создать
вектор-столбец
ошибок, как описано в (15.12), нужно
преобразовать
двухмерный массив
e в одномерный по
столбцам.

Визуальное сервоуправление на основе изображения 931

в ходе которой многократно будет вызван метод step объекта
для имитации движения в течение одного временного шага.
Симуляция создает анимационный эффект движения объектов
в плоскости изображения камеры и трехмерную визуализацию
камеры и точек мира.
Результаты симуляции сохраняются в объекте для последующего анализа. Мы можем построить график траектории движения целевых объектов на изображении, скорости камеры в зависимости от времени или положения камеры в зависимости
от времени.
>>> ibvs.plot_p();
# построить траекторию на плоскости изображения
>>> ibvs.plot_vel(); # построить график скорости камеры
>>> ibvs.plot_pose(); # построить траекторию камеры

как показано на рис. 15.8. Как видите, характерные точки следуют по приблизительно прямолинейным траекториям, а положение камеры плавно приближается к некоторому конечному
значению. Число обусловленности якобиана изображения
>>> ibvs.plot_jcond();

v (пиксели)

Декартова скорость

начальное положение
целевое положение

u (пиксели)

Ориентация
камеры (рад)

в

б

Шаг по времени

Число обусловленности якобиана

Местоположение
камеры (м)

а

Шаг по времени

г

Шаг по времени

Рис. 15.8. Результаты моделирования IBVS: а) движение точек на плоскости изображения; б) компоненты
пространственной скорости; в) положение камеры, при котором углы крена-тангажа-рыскания находятся
в порядке YXZ камеры; г) число обусловленности якобиана изображения (чем меньше, тем лучше)

932 Глава 15 · Управление на основе зрения

уменьшается по мере движения (т. е. якобиан становится более
обусловленным), что является следствием отдаления точек друг
от друга.
Как определяется p∗? Точки изображения можно найти наглядно, переместив камеру в нужное положение и записав
наблюдаемые координаты изображения. Если известны параметры калибровки камеры и геометрия цели, то желаемые координаты изображения можно вычислить для любого заданного целевого положения. Этот расчет проекции точки на точку
мира не требует больших вычислительных затрат и выполняется только один раз перед началом визуального управления.
Систему IBVS можно также представить в виде блок-схемы,
как показано на рис. 15.9. Эта модель запускается командой ►
>>> %run -m IBVS-main -H

а блоки визуализации воспроизводят анимационный эффект
при изменении характеристик плоскости изображения, скорости камеры и ошибки характеристик во времени. Начальное положение камеры задается параметром блока Camera pose, а мировые точки – параметрами блока констант. Объект CentralCamera
определен в IBVS-main.py и является параметром блоков Central
Camera, Visual Jacobian и ImagePlane. Другие параметры, такие как
модель камеры, мировые точки и коэффициенты усиления,
определяются в свойствах различных блоков и в IBVS-main.py.
После запуска модель добавляет в глобальное пространство
имен объект с результатами моделирования:
>>>out
results:
t
x
xnames

| ndarray(1001,)
| ndarray(0,)
| list=[]

Блок Jacobian
condition

Блок Main

Камера

скорость
камеры

Блок Central
camera

P
Координаты
мировых
точек

Блок Visual
Jacobian

Блок Inverse

Блок Prod

λ

Блок Camera
velocity

Блок Camera
pose
p∗ – p

Блок Image
Plane
ошибка
характеристик
p∗

Блок-схемы были
представлены
в разделе 4.1.1.1.
Параметр -H
предотвращает
блокировку до
закрытия всех окон
моделирования.

Блок Flatten

общая ошибка
характеристик

Блок Error
norm

управление на основе изображения

Желаемые
координаты
плоскости
изображения

Рис. 15.9. Блок-схема модели models/IBVS. Блок Main ссылается на файл IBVS-main.py, который создает экземпляр

Визуальное сервоуправление на основе изображения 933
clock0

y0
y1
y2
ynames

:
>
>
|
|
|
|

Struct
t
| ndarray(1001,)
x
| ndarray(1001,6)
ndarray(1001,)
ndarray(1001,)
ndarray(1001,6)
list=['totalfeatureerror[0]','InverseBlock[1]',
'lambda[0]']

Элемент t содержит время моделирования, а элементы y0, y1
и y2 соответствуют выходным портам блоков, перечисленных
в ynames. Мы можем построить график изменения скорости камеры (выходного сигнала блока λ):
>>> plt.plot(out.t, out.y2)

Положение камеры – это дискретное состояние времени, связанное с clock0, и может быть отображено на графике
>>> plt.plot(out.clock0.t, out.clock0.x)

15.2.3

Оценка глубины объекта

Вычисление якобиана изображения требует знания внутренних
характеристик камеры, главной точки и фокусного расстояния,
но на практике в этих параметрах могут появляться ошибки.
Для вычисления якобиана также требуется знать Zi – z-ко­ор­ди­
на­ту, или глубину, каждой точки. В только что рассмот­ренных
моделях мы предполагали, что глубина известна, но в реальности это не всегда так.
Для решения проблемы неизвестной глубины было предложено несколько подходов. Простейший из них – просто принять постоянное значение, что вполне разумно, если требуемая
скорость камеры приблизительно параллельна плоскости точек
объекта. Для проверки эффективности различных оценок глубины можно сравнить эффект выбора Z = 1 и Z = 10 для примера,
приведенного выше, где истинная глубина изначально Z = 3:
>>>
>>>
>>>
>>>

ibvs = IBVS(camera, P=P, p_d=pd, depth=1);
ibvs.run(50)
ibvs = IBVS(camera, P=P, p_d=pd, depth=10);
ibvs.run(50)

Результаты показаны на рис. 15.10. Как можно заметить, траектории на плоскости изображения больше не являются прямыми, потому что теперь якобиан плохо аппроксимирует соотношение между движением камеры и движением точек на
изображении. Также видно, что для Z = 1 алгоритм сходится
гораздо медленнее, чем для Z = 10. Якобиан для Z = 1 переоце-

934 Глава 15 · Управление на основе зрения

v (пиксели)

Декартова скорость

начальное положение
целевое положение

а

u (пиксели)

Шаг по времени

v (пиксели)

Декартова скорость

начальное положение
целевое положение

б

u (пиксели)

Шаг по времени

Рис. 15.10. Траектории точек на плоскости изображения и положение камеры для IBVS с различными постоянными
оценками глубины: а) для Z = 1; б) для Z = 10

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

Визуальное сервоуправление на основе изображения 935

кадра к кадру, когда известны внутренние характеристики камеры и ее пространственная скорость (v, ω), которая вычисляется регулятором. Получить оценку глубины можно, переписав
(15.9):

и переформулировав в оценочную форму
(15.16)
Правая часть уравнения представляет наблюдаемый оптический поток, из которого вычитается ожидаемый оптический
поток, обусловленный вращением камеры. Этот процесс называется деротацией оптического потока. Оставшийся после вычитания оптический поток обусловлен только линейным смещением. Записав (15.16) в компактной форме
Aθ = b,

(15.17)

получаем простое линейное уравнение с одним неизвестным
параметром θ = 1/Z, которое можно решить методом наименьших квадратов.
В нашем примере это можно сделать следующим образом:
>>> ibvs = IBVS(camera, P=P, p_d=pd, depthest=True);
>>> ibvs.run()

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

936 Глава 15 · Управление на основе зрения

v (пиксели)

Декартова скорость

начальное положение
целевое положение

u (пиксели)
Шаг по времени
а
б
Рис. 15.11. IBVS с оценкой глубины: а) траектории объектов; б) сравнение расчетной и истинной глубины для всех
четырех точек

15.2.4

Проблемы с производительностью

Закон управления PBVS определяется в терминах трехмерного положения, поэтому не существует механизма, который напрямую регулировал бы движение точек изображения. Пример
PBVS на рис. 15.5 показывает, что характерные точки следовали по криволинейным траекториям в плоскости изображения
и потому могли выйти из поля зрения камеры. Для другого начального положения камеры
>>> pbvs.pose_0 = SE3.Trans(-2.1, 0, -3) * SE3.Rz(5*pi/4);
>>> pbvs.run()

как раз был получен такой результат (см. рис. 15.12а), когда две
точки вышли за пределы изображения, что может привести
к сбою управления PBVS.
Напротив, управление IBVS для того же начального положения камеры
>>> ibvs.pose_0 = pbvs.pose_0;
>>> ibvs.run()
>>> ibvs.plot_p();

дает траектории объектов, показанные на рис. 15.12б, но в этом
случае отсутствует возможность прямого управления скоростью камеры в пространстве задач. Это иногда может приводить к неожиданным движениям, особенно при вращении цели
вокруг оптической оси:
>>> ibvs.pose_0 = SE3.Tz(-1) * SE3.Rz(2);
>>> ibvs.run(50)

В этой симуляции координаты
точек на плоскости изображения
по-прежнему вычисляются и используются, даже если они
выходят за границы
изображения. Проверку видимости
можно включить
с помощью пара­
метра visible метода project_point
камеры.

Визуальное сервоуправление на основе изображения 937

v (пиксели)

v (пиксели)

начальное положение
целевое положение

начальное положение
целевое положение

u (пиксели)
а
б
Рис. 15.12. Траектории точек на плоскости изображения: а) для PBVS; б) IBVS

u (пиксели)

Результат показан на рис. 15.13a. Как можно заметить, камера совершила ненужное смещение вдоль оси z – сначала от
цели, а затем обратно. Это явление называется отступлением
камеры (camera retreat). Результирующее движение не является оптимальным по времени и может потребовать большего и, возможно, недопустимого движения камеры. Последний
пример – чистое вращение вокруг оптической оси на угол π рад
>>> ibvs.pose_0 = SE3.Tz(-1) * SE3.Rz(pi);
>>> ibvs.run(10)

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

– показан на рис. 15.13б. Характеристические точки, как обычно, движутся по прямой к своим заданным значениям, но
в данной задаче все траектории проходят через главную точку.
Целевые точки могут оказаться в главной точке, только если камера находится в отрицательной бесконечности, а именно туда
она и направляется! ◄
И последнее соображение: якобиан изображения представляет линеаризацию нелинейной системы, поэтому если движение на каждом шаге будет слишком велико, то линеаризация
станет недействительной и точки будут следовать по криволинейным траекториям на плоскости изображения, как показано
на рис. 15.10. Это может произойти, если желаемые положения
объектов находятся далеко от начальных и/или коэффициент
усиления слишком велик. Одно из решений – ограничение максимальной нормы заданной скорости

Ориентация
камеры (рад)

v (пиксели)

начальное положение
целевое положение

Местоположение
камеры (м)

938 Глава 15 · Управление на основе зрения

а

Шаг по времени

Ориентация
камеры (рад)

v (пиксели)

начальное положение
целевое положение

Местоположение
камеры (м)

u (пиксели)

б

u (пиксели)

Шаг по времени

Рис. 15.13. Траектории точек на плоскости изображения и положение камеры для IBVS с чистым вращением цели
вокруг оптической оси: а) поворот на 1 рад; б) поворот на π рад

Это ограничение включается параметром vmax конструктора. Из всех графиков скорости камеры видно, что самая высокая скорость задается на первом шаге времени, когда ошибка
характеристик самая большая. Для ослабления этого эффекта
можно использовать прием, называемый плавным стартом,
подменяющий определение ошибки на плоскости изображения
e = p∗ - p выражением
e¢ = e - e0 e-μt,

перед тем как она будет использована для вычисления управляющего воздействия, где e0 – значение e в момент времени t = 0.
Второй член гарантирует, что e¢ изначально равно нулю и постепенно растет со временем. Задействовать этот прием можно
с помощью параметра smoothstart= в вызове конструктора.
Траектории точек не обязательно должны быть прямыми,
и точки не обязательно должны двигаться с асимптотической
скоростью – мы использовали эти условия исключительно ради

Использование других признаков изображений 939

простоты. Используя методы планирования траектории, описанные в разделе 3.3, можно заставить точки следовать по любой произвольной траектории на изображении.
Подводя итог, можно сказать, что IBVS – это исключительно
надежный подход к управлению на основе зрения. Он устойчив
к ошибкам в определении глубины точек. Мы также показали,
что он может генерировать неоптимальные декартовы траектории в случае больших поворотов вокруг оптической оси, и способы решения этих проблем мы обсудим в следующей главе. Конечно, есть и некоторые практические сложности. Если камера
расположена на рабочем органе робота, она может помешать
выполнению задачи, или, когда робот находится близко к цели,
камера может не сфокусироваться, или цель может быть перекрыта захватом.

15.3

Использование других признаков
изображений

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

15.3.1

Линейные объекты

Мы представляем линию, используя параметризацию (θ, ρ), которая была введена для преобразования Хафа в разделе 12.2
u cos θ + v sin θ = ρ.
Скорость изменения параметров линии связана со скоростью
камеры соотношением
,
и якобиан – это

940 Глава 15 · Управление на основе зрения

По аналогии с якобианом точечного объекта, для вычисления
которого требовались некоторые частичные знания о трехмерной ориентации (глубина точки Z), якобиан линейного объекта
требует знания положения трехмерной плоскости, содержащей
прямую aX + bY + cZ + d = 0 и λθ = (a sin θ - b cos θ)/d и λρ = (aρ cos θ +
bρ sin θ + c)/d. Существует бесконечное число плоскостей, содержащих прямую, и мы выбираем ту, для которой d ≠ 0. Все необходимые вычисления производит метод visjac_l объекта Cent­
ralCamera.
Линейный объект, как и точечный, предоставляет якобиан
с двумя строками, поэтому нам требуется минимум три строки,
чтобы получить якобиан полного ранга. ►
Проиллюстрируем это на примере с тремя прямыми, лежащими в одной плоскости Z = 3. Мы можем построить три точки
на этой плоскости, используя функцию окружности всего с тремя граничными точками:

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

>>> P = circle([0, 0, 3], 0.5, resolution=3);

и использовать знакомые методы класса CentralCamera для прое­
цирования их на изображение. Для каждой пары точек {(ui, vi),
(uj, vj), i ≠ j} вычислим уравнения прямой

Симуляция выполняется как обычно:
>>> ibvs = IBVS_l.Example(camera); # настройка задачи
>>> ibvs.run()

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

15.3.2

Эллиптические объекты

Круги часто встречаются в искусственных средах, и в общем
случае круг в мире будет проецироваться на плоскость изображения в виде эллипса. Поскольку круг – это особый случай
эллипса, мы можем описать и тот, и другой с помощью универсального уравнения эллипса
u2 + E0v2 - 2E1uv + 2E2u + 2E3v + E4 = 0.

(15.18)

Эллипсы и подгонка
эллипсов к данным
более подробно
описываются
в разделе C.1.4.

v (пиксели)

Использование других признаков изображений 941

а

б

u (пиксели)

Рис. 15.14. IBVS для линейных объектов: а) плоскость изображения с тремя линейными объектами (красный,
зеленый и синий), перемещающимися с течением времени (сплошные) и целевыми (пунктирные); б) камера и три
точки, образующие три мировые линии

Скорость изменения параметров эллипса Ei связана со скоростью камеры соотношением

,

а якобиан имеет вид

И снова для вычисления якобиана эллипса необходимы некоторые частичные знания трехмерного пространства, в частности необходимо знать уравнение трехмерной плоскости, содержащей эллипс aX + bY + cZ + d = 0 и α = -a/d, β = -b/d и γ = -c/d.
Этот якобиан имеет максимальный ранг 5, но он снижается до
трех, если проекция представляет собой окружность с центром
в плоскости изображения, и до двух, если окружность имеет нулевой радиус. Якобиан изображения для эллипса вычисляется
методом visjac_e класса CentralCamera.

942 Глава 15 · Управление на основе зрения

Преимущество эллиптических объектов заключается в том,
что эллипс можно вычислить по множеству граничных точек
без необходимости решать задачу соответствия. Эллипс также
можно вычислить по моментам всех точек, находящихся внут­
ри эллипса.
Проиллюстрируем это на примере окружности из десяти точек в мире
>>> P = circle([0, 0, 3], 0.5, resolution=10);

и их проекции на плоскость изображения
>>> p = camera.project_point(P, pose=camera.pose, retinal=True);

Преобразуем данные в координаты изображения на сетчатке. Параметры эллипса рассчитываются с использованием методов, что описаны в разделе C.1.4.2
>>>
>>>
>>>
>>>

x, y = p
A = np.column_stack([y**2, -2*x*y, 2*x, 2*y, np.ones(x.shape)]);
b = -(x**2);
E, *_ = np.linalg.lstsq(A, b, rcond=None)

В результате получается пятимерный вектор параметров эллипса. Якобиан
>>>
>>>
>>>
(5,

plane = [0, 0, 1, -3]; # plane Z=3
J = camera.visjac_e(E, plane);
J.shape
6)

представляет собой мат­ри­цу 5×6, максимальный ранг которой
равен всего 5, поэтому мы не можем однозначно определить
скорость камеры. У нас есть как минимум два варианта. Вопервых, если конечный вид – это круг, то можно не беспокоиться о вращении вокруг нормали к плоскости круга, и в этом
случае мы можем удалить шестой столбец якобиана, чтобы сделать его квадратным, и установить ωz равным нулю. Во-вторых,
подход, принятый в этом примере, заключается в объединении
характеристик эллипса и одной точки ►:

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

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

Использование других признаков изображений 943

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

Симуляция запускается как обычно:
>>> ibvs = IBVS_e.Example(); # настройка задачи
>>> ibvs.run()

На рис. 15.15 показан моментальный снимок с результатами.

15.3.3

Фотометрические признаки

v (пиксели)

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

а

u (пиксели)

б

Рис. 15.15. IBVS с использованием эллиптических объектов: а) плоскость изображения с текущими точками (кружки)
и целевыми (звездочки); б) представление мира с точками и камерой

944 Глава 15 · Управление на основе зрения

фотометрическом визуальном сервоуправлении мы работаем
непосредственно со значениями пикселей, и устанавливать соответствие не требуется.
Характеристика изображения представляет собой вектор, содержащий интенсивность каждого пикселя изображения (текущего или желаемого), сложенную в очень высокий вектор-столбец высотой N = W × H. Скорость изменения значений пикселей İ
связана со скоростью камеры соотношением

.

Якобиан в этом случае имеет вид

где pi – координата пикселя на плоскости изображения, соответствующего i-му элементу вектора признаков, ∇I(p) = (∇u(p),
∇v(p)) ∈ ℝ1×2 – градиенты изображения в этом пикселе в направлениях u и v, а Jp(·) ∈ ℝ2×6 – якобиан точечного объекта на плоскости изображения (15.9), вычисленный в этом пикселе.
Нам снова нужна глубина точки Zi, и мы должны использовать
наилучшую оценку, как обсуждалось выше. Если сервоуправление осуществляется относительно плоского изображения, то
глубина может быть приблизительно известна, и, как мы уже
отмечали ранее, IBVS довольно устойчив к ошибкам в глубине
точки. Если же сервоуправление осуществляется относительно
сложной трехмерной сцены, то глубину в каждом пикселе будет
очень сложно определить, и мы снова полагаемся на устойчивость IBVS.
Для достижения сходимости фактическое и конечное изображения должны иметь значительное перекрытие. При этом
предполагается, что сцена имеет ламбертовское отражение, отсутствуют зеркальные блики, а интенсивность и направление
освещения не меняются со временем. На практике фотомет­
рическое визуальное сервоуправление работает хорошо, даже
если эти предположения не выполняются или изображения
час­тично перекрываются во время движения камеры.

Подведение итогов 945

15.4

Подведение итогов

В этой главе мы познакомились с основами управления роботами на основе машинного зрения и основными методами, разработанными за два десятилетия, вплоть до середины 1990-х годов. Существуют две различные конфигурации. Камеру можно
прикрепить к роботу, наблюдающему за целью («глаз в руке»),
или закрепить ее неподвижно где-то в мире и наблюдать через
нее как за роботом, так и за целью. Еще одно различие – структура управления: визуальное сервоуправление на основе мес­
тоположения (Position-Based Visual Servo, PBVS) и визуальное
сервоуправление на основе изображения (Image-Based Visual
Servo, IBVS). Первый вариант предполагает оценку положения
на основе калиброванной камеры и геометрической модели
целевого объекта, тогда как второй осуществляет управление
непосредственно в плоскости изображения. Оба подхода имеют свои достоинства и недостатки. PBVS обеспечивает эффективное прямолинейное декартово движение камеры в мире, но
может приводить к выходу объектов за пределы плоскости изображения. IBVS всегда удерживает объекты в плоскости изоб­
ражения, но может приводить к траекториям, выходящим за
пределы досягаемости робота, особенно если требуется большой угол поворота вокруг оптической оси камеры. IBVS также
требует иметь некоторую трехмерную информацию (глубину
точек), но он довольно устойчив к ошибкам в глубине и позволяет оценивать глубину по мере движения камеры. IBVS можно
адаптировать для работы не только с точечными, но также с линейными и эллипсовидными объектами и даже со значениями
пикселей. Произвольное количество объектов (которые могут
быть любой комбинацией точек, линий или эллипсов), наблюдаемых с произвольного количества камер, можно объединить
простым объединением соответствующих матриц Якоби.
До сих пор в ходе моделирования мы определяли необходимую скорость камеры и перемещали ее, не принимая во внимание особенности механизма или робота, который ее перемещает. В следующей главе мы рассмотрим камеры, закрепленные
на роботах-манипуляторах, мобильных наземных роботах и воз­
душных роботах.

15.4.1

Дополнительное чтение

Учебная статья Hutchinson et al. (1996) стала первым всеобъемлющим изложением и таксономией этой области, а в Chaumette
and Hutchinson (2006 и 2007) представлено более современное

946 Глава 15 · Управление на основе зрения

введение. Главы, посвященные визуальному сервоуправлению,
включены в Siciliano and Khatib (2016, § 34) и Spong et al. (2006,
§ 12).
Хорошо известно, что IBVS очень устойчив к ошибкам в определении глубины, и его влияние на эффективность управления подробно рассмотрено в Marey and Chaumette (2008).
В Feddema andMitchell (1989) выполнена частичная трехмерная реконструкция для определения глубины точки на основе наблюдаемых характеристик и известной геометрии цели.
В Papanikolopoulos and Khosla (1993) описаны методы адаптивного управления с оценкой глубины, используемые в этой главе. В Hosoda and Asada (1994), Jägersand et al. (1996) и Piepmeier
et al. (1999) показано, как оценить мат­ри­цу Якоби изображения
динамически, используя измерения движения робота и изображения. Метод визуального сервоуправления второго порядка был предложен в Malis (2004).
Наиболее распространенный якобиан изображения основан на движении точек на изображении, но его также можно получить для параметров линий в плоскости изображения
(Chaumette, 1990; Espiau et al. 1992) и параметров эллипса
в плоскости изображения (Espiau et al. 1992). В Mahoney et al.
(2002) описывается, как выбор признаков влияет на динамику
замкнутого контура. В Chaumette (2004) и Tahri and Chaumette
(2005) предлагается использовать моменты бинарных областей
для визуального управления плоскими сценами. В Collewet
et al. (2008) и в последующих статьях, а также совсем недавно
в Bakthavatchalam et al. (2015) и Crombez et al. (2015) была описана возможность сервоуправления напрямую на основе значений пикселей изображения без сегментации или извлечения
признаков. В Bateux et al. (2018) была продемонстрирована возможность использования глубокого обучения для визуального
сервоуправления. Литературы по PBVS гораздо меньше, тем не
менее нельзя не отметить замечательное введение в эту тему
в Westmore and Wilson (1991) – авторы используют EKF для неявной оценки положения: целевое положение является состоянием фильтра, а изменение между прогнозируемыми и наблюдаемыми координатами объекта обновляет состояние целевого
положения. В Hashimoto et al. (1991) представлено сравнение
подходов, основанных на местоположении и на изображении.

История и предыстория
Визуальное сервоуправление имеет очень долгую историю – самое раннее упоминание о нем встречается в Shirai and Inoue
(1973), где описывается, как можно использовать визуальную
обратную связь для коррекции положения робота с целью повышения точности выполнения задач. Авторы продемонстрировали систему с циклом сервоуправления длительностью

Подведение итогов 947

10 с, что наглядно демонстрирует ранние проблемы извлечения признаков в реальном времени. До конца 1990-х для этого
требовалось громоздкое и дорогостоящее специализированное
оборудование, подобное показанному на рис. 15.16. Значительные ранние работы по промышленному применению были
проведены в SRI International в конце 1970-х (Hill and Park, 1979;
Makhlin, 1985).

Рис. 15.16. В начале 1990-х (Corke 1994) для визуального сервоуправления в реальном
времени требовалась 19-дюймовая стойка VMEbus с аппаратными картами обработки
изображений, обеспечивающая пропускную способность 10 Мпикс/с или частоту кадров 50 Гц
для изображений 512×512

В 1980-х в Weiss et al. (1987) была представлена классификация систем визуального сервоуправления на основе местоположения и на основе изображения. Авторы также ввели различие
между визуальным сервоуправлением и динамическим зрением и движением: в первом случае используется только визуальная обратная связь, а во втором – обратная связь по сочленениям и визуальная обратная связь. Это последнее различие уже
не актуально, и большинство современных систем визуального
сервоуправления используют обратную связь по положению
сочленений и визуальную обратную связь, обычно основанную
на энкодерах скорости, как обсуждалось в разделе 9.1.6, с внешним контуром обратной связи на основе визуального восприятия. В Weiss (1984) были применены методы адаптивного управления для IBVS роботизированной руки без обратной
связи на уровне суставов, но эксперименты были ограничены
манипуляторами с малым числом степеней свободы из-за низкой частоты дискретизации видеоданных, доступной в то вре-

948 Глава 15 · Управление на основе зрения

мя. Другие исследователи рассматривали возможность включения динамики манипулятора (9.11) в контроллеры, напрямую
управляющие крутящим моментом двигателя (Kelly 1996; Kelly
et al. 2002a, 2002b), но все они по-прежнему требуют определения углов сочленений для оценки якобиана манипулятора
и углов сочленений для демпфирования. Феддема (Feddema and
Mitchell 1989; Feddema 1989) использовал замкнутый контур
управления движением для преодоления проблем, связанных
с низкой частотой дискретизации изображения, и продемонстрировал возможность применения IBVS для манипулятора
с четырьмя степенями свободы. Шометт, Ривс и Эспио (Chaumette et al. 1991; Rives et al. 1989) описали аналогичный подход
с использованием метода целевой функции (Samson et al. 1990)
и показали экспериментальные результаты позиционирования
робота с применением целевого объекта с четырьмя характеристиками. В Feddema et al. (1991) был описан алгоритм выбора
подмножества доступных характеристик, дающего наилучший
обусловленный квадратный якобиан. В Hashimoto et al. (1991)
было показано, что использование большого числа признаков
и псевдообратной мат­ри­цы для определения скорости дает
определенные преимущества. Вопросы управления и устойчивости в замкнутых системах визуального управления рассмат­
ривались несколькими исследователями (Corke and Good 1992;
Espiau et al. 1992; Papanikolopoulos et al. 1993), и ими были предложены контроллеры с прямой связью вместо обратной (Corke
(1994) и Corke and Good (1996)).
Книга Hashimoto (1993) стала первым сборником статей, посвященных подходам и приложениям в области визуального
сервоуправления. Книга Corke (1996b) уже не переиздается, но
доступна бесплатно в интернете. Она охватывает основы робототехники и применения машинного зрения для управления
динамикой визуальной системы сервоуправления на основе
изображений. Она содержит обширную, хотя и устаревшую,
подборку ссылок на приложения визуального сервоуправления,
включая управление камерами для слежения, высокоскоростными планарными микроманипуляторами, дорожными транспортными средствами, дозаправку самолетов и сбор фруктов.
Другой важный сборник статей (Kriegman et al. 1998) вытекает
из семинара 1998 года по синергии управления и зрения: как
зрение можно использовать для управления и как управление
можно использовать для зрения. Другой сборник семинаров
Chesi and Hashimoto (2010) посвящен новейшим разработкам
алгоритмов и их применению.
Визуальное сервоуправление применялось к широкому спект­
ру задач, которые обычно требуют навыков рук и глаз человека,
таких как пинг-понг (Andersson 1989), жонглирование (Rizzi and
Koditschek 1991) и балансировка перевернутого маятника (Dick-

Подведение итогов 949

manns and Graefe 1988a; Andersen et al. 1993), ловля (Sakaguchi
et al. 1993; Buttazzo et al. 1993; Bukowski et al. 1991; Skofteland
and Hirzinger 1991; Skaar et al. 1987; Lin et al. 1989) и управление
игрой в лабиринте (Andersen et al. 1993).

15.4.2

Упражнения

1. Визуальное сервоуправление на основе положения (раздел 15.1).
а) Запустите пример PBVS. Поэкспериментируйте с различными параметрами, такими как начальное положение
камеры, шаг смещения вдоль траектории λ и добавление
пиксельного шума в выходной сигнал камеры.
б) Создайте систему PBVS, работающую в соответствии
с координатной меткой из раздела 13.6.1.
в) Создайте блок-схему модели для PBVS.
г) Используйте другую модель камеры для оценки положения (немного другое фокусное расстояние или главную
точку) и посмотрите, как это повлияет на окончательное
положение рабочего органа.
д) Реализуйте систему PBVS на основе EKF, как описано
в Westmore и Wilson (1991).
2. Поле оптического потока (раздел 15.2.1).
а) Постройте поле оптического потока для камер с различными фокусными расстояниями.
б) Постройте поле потока для некоторых составных движений камеры, таких как перемещение по осям x и y, перемещение по осям x и z, а также перемещение по осям x
и вращение вокруг оси z.
3. В случае с двумя точками якобиан изображения имеет размеры 4×6, а нуль-пространство содержит два столбца. Каким
движениям камеры они соответствуют?
4. Визуальное сервоуправление на основе изображений (раздел 15.2.2).
а) Запустите пример IBVS: либо скрипт Python, либо версию блок-схемы. Попробуйте разные коэффициенты
усиления. Напомню, что это может быть скаляр или диагональная мат­ри­ца, которая позволяет задавать разные
настройки для каждой степени свободы.
б) Реализуйте ограничение максимальной нормы задаваемой скорости.
в) Поэкспериментируйте с добавлением пиксельного шума
в выходной сигнал камеры.
г) Поэкспериментируйте с различными начальными и целевыми положениями камеры.

950 Глава 15 · Управление на основе зрения

д) Поэкспериментируйте с разным количеством целевых
точек, от трех до десяти. Для случаев N > 3 сравните производительность псевдообратного алгоритма с выбором
подмножества из трех точек (первые три или три случайные). Разработайте алгоритм, который выбирает подмножество точек, которое приводит к составному якобиану
с наилучшим числом обусловленности.
е) Создайте набор целевых точек на плоскости изображения,
образующих прямоугольник, а не квадрат. Не сущест­вует
точки зрения, с которой квадрат выглядит как прямо­
угольник (почему?). Что делает система IBVS?
ж) Создайте набор целевых точек на плоскости изображения, которые недостижимы, например поменяв местами
две соседние мировые точки или точки на изображении.
Что делает система IBVS?
з) Используйте другую модель камеры для вычисления якобиана изображения (с другим фокусным расстоянием или
главной точкой) и посмотрите, как это повлияет на окончательное положение рабочего органа.
и) Реализуйте IBVS второго порядка, используя (15.15).
к) В IBVS мы обычно заставляем точки двигаться по прямым
линиям, но так делается исключительно для удобства.
Используйте генератор траекторий, чтобы переместить
точки из исходного положения в желаемое с некоторым
боковым движением, например на половину или полный
период синусоиды. Как это повлияет на декартово движение камеры?
л) Реализуйте стерео IBVS. Подсказка: сложите якобианы
точечных объектов для обеих камер и определите желаемые положения точек на плоскости изображения каждой
камеры.
м) Смоделируйте выходные данные камеры в произвольном положении, наблюдающей за плоской целью. Для
достижения этой цели используйте знания о гомографии
и искажении изображений.
н) Реализуйте контроллер IBVS с использованием функций
SIFT. Используйте дескрипторы SIFT для обеспечения надежного соответствия.
5. Выведите якобиан изображения для случая, когда камера
ограничена только панорамированием и наклоном. Что произойдет, если камера будет панорамировать и наклоняться
относительно точки, не являющейся ее центром?
6. При обсуждении восприимчивости движения мы использовали тождество Jp+T Jp+ = (Jp JpT)-1. Докажите его. Подсказка: используйте сингулярное разложение J = UΣVT и помните, что
U и V – ортогональные мат­ри­цы.

Подведение итогов 951

7. Системы визуального сервоуправления с разомкнутым
контуром не обсуждались в этой книге. Рассмотрите группу целевых точек на рабочем органе робота, а также группы
целевых точек на целевом объекте, которые обе наблюдаются одной камерой.
а) Создайте конечную систему PBVS с открытым контуром.
б) Используйте другую модель камеры для оценки положения (немного другое фокусное расстояние или главную
точку) и посмотрите, как это повлияет на окончательное
положение рабочего органа.
в) Создайте конечную систему IBVS с открытым контуром.
г) Используйте другую модель камеры для вычисления
якобиана изображения (с другим фокусным расстоянием или главной точкой) и посмотрите, как это повлияет
на относительное положение рабочего органа.
8. Выполните пример визуального сервоуправления на основе линейного объекта (раздел 15.3.1).
9. Визуальное сервоуправление на основе эллипса (раздел 15.3.2).
а) Выполните пример визуального сервоуправления на основе эллипса.
б) Измените пример так, чтобы сервопривод поворачивал
камеру на пять градусов, используя только параметры
эллипса (без точечных объектов).
в) Для произвольной формы можно вычислить эквивалентный эллипс, выраженный через тензор инерции
и цент­роид. Определите параметры эллипса (15.18) по
тензору инерции и центроиду. Создайте контроллер визуального сервоуправления для эллипса, перемещающий объект произвольной формы в желаемый вид.
г) Попробуйте применить сервоуправление к эквивалентному эллипсу капли, например изображению акулы на
рис. 12.8в.
10. Реализуйте модель фотометрического визуального сервоуправления. Вы можете использовать гомографии и искажение перспективы для моделирования движения камеры
относительно плоского изображения, а также производную
гауссова ядра для вычисления градиентов изображения.
Исследуйте производительность при различных начальных
перемещениях и поворотах камеры. Попробуйте изменять
предполагаемую глубину и параметры производного ядра.

Глава

16

	 Продвинутое визуальное
сервоуправление

Эта глава основана на предыдущей и знакомит с некоторыми
продвинутыми методами и применениями визуального серво­
управления. В разделе 16.1 вашему вниманию будет представлен гибридный метод визуального сервоуправления, позволяющий избежать некоторых ограничений, присущих стандартным
подходам IBVS и PBVS, описанным ранее.
Широкоугольные камеры, например с катадиоптрическими объективами и объективами типа «рыбий глаз», обладают
значительными преимуществами для визуального сервоуправления. В разделе 16.2 будет показано, как переформулировать
IBVS в терминах полярных, а не декартовых координат плоскости изображения. Это напрямую относится к объективам типа
«рыбий глаз», но также обеспечивает улучшенный контроль за
поворотами при использовании перспективной камеры. Унифицированная модель формирования изображений, описанная
в разделе 13.4, позволяет представить большинство камер (перспективных, «рыбий глаз» и панорамных) в виде сферической
проекционной модели, а в разделе 16.3 я покажу, как переформулировать IBVS для использования в комплексе со сферическими камерами.
В разделе 16.4 будет представлено несколько кратких примеров применения. С использованием блок-схем они покажут, как
можно использовать визуальное сервоуправление вместе с различными типами камер (перспективными и сферическими)
и типами роботов (манипуляторами, мобильными наземными
роботами и воздушными роботами). Примерами нам послужат
6-осевой робот-манипулятор с камерой, расположенной в руке;
мобильный робот, принимающий определенное положение,
например для пересечения дверного проема или стыковки,
и квадрокоптер, принимающий фиксированное положение
и зависающий в ней относительно цели на земле.

chap16.ipynb

https://go.sn.pub/
jdeUql

IBVS с разделением XY/Z 953

16.1

IBVS с разделением XY/Z

В разделе 15.2.4 мы столкнулись с проблемой отступления камеры (camera retreat) в системе IBVS. Это явление можно объяснить тем, что закон управления IBVS заставляет точки объекта двигаться по прямым линиям в плоскости изображения, но
при вращении камеры вокруг своей оптической оси эти точки
естест­венным образом будут двигаться по дугам окружностей.
Линейный регулятор IBVS динамически изменяет общий масштаб изображения, поэтому движение по дуге воспринимается
как движение по прямой. Изменение масштаба достигается перемещением по оси z, которое мы наблюдаем как отступление
камеры.
Методы с разделением исключают отступление камеры,
используя IBVS для управления одними степенями свободы,
и другие регуляторы – для управления остальными. Гибридные
схемы XY/Z рассматривают оси x и y как одну группу, а ось z – как
другую. Этот подход основан на нескольких идеях. Во-первых,
что совершенно очевидно, проблема отступления камеры связана с явлением оси z: поворот вокруг оси z приводит к нежелательному перемещению вдоль нее. Во-вторых, как показано на
рис. 15.7, движение плоскости изображения вследствие поступательного и вращательного движения по осям x и y довольно
схоже, тогда как оптический поток вследствие вращения и перемещения по оси z радикально отличается.
Мы разбиваем оптический поток (15.10) так, что
ṗ = Jxyvxy + Jzvz,

(16.1)

где vxy = (υx, υy, ωx, ωy), vz = (υz, ωz), а Jxy и Jz – столбцы 0, 1, 3, 4 и 2,
5 мат­ри­цы Jp соответственно. Мы вычисляем vz с помощью отдельного контроллера, поэтому можем записать (16.1) как
vxy = λJ +xy(ṗ∗ - Jzvz),

θ в этом случае
имеет иное
значение, чем
в разделах 12.2
и 15.3.1.
Здесь прямая
определяется как
u sin θ + v cos θ + d = 0.

(16.2)

где ṗ∗ – желаемая скорость точки объекта, которую мы использовали ранее для IBVS как в (15.13).
Скорости vz = (υz, ωz) вычисляются непосредственно из двух
скалярных характеристик изображения: θ и A, показанных на
рис. 16.1. Первая характеристика θ ∈ [-π, π] представляет угол
между осью u и направленным отрезком, соединяющим точки i
и j. Для численного обусловливания выгодно выбрать самый
длинный отрезок, который можно построить для точек, и допустить, что он может измениться во время движения при изменении конфигурации точек. Требуемая скорость вращения
достигается с помощью простого пропорционального закона
управления

954 Глава 16 · Продвинутое визуальное сервоуправление

ω∗z = λωz(θ∗ ⊝ θ),

где оператор ⊝ обозначает вычитание по модулю 2π, которое
в пакете Toolbox реализуется функциями angdiff, wrap_mpi_pi
и wrap_0_2pi. Как обычно, при движении по окружности есть два
направления к цели. Если вращение ограничено, например механическим упором на сочленении робота, то знак ωz следует
выбрать таким, чтобы избежать движения через этот упор.

Рис. 16.1. Признаки изображения
для IBVS с разделением XY/Z. Помимо
координат четырех точек, используемых
непосредственно для IBVS, используется
угол θ наклона самого длинного отрезка
и площадь многоугольника A

Вторая характеристика – это функция площади A ∈ ℝ>0 правильного многоугольника, вершинами которого являются точки изображения. Вот основные преимущества этой меры:
сильно зависит от смещения по оси z;
является скалярной;
„„ инвариантна к повороту вокруг оси z, что позволяет отделить вращение камеры от перемещения по оси z;
„„ легко вычисляется.
Площадь многоугольника – это просто момент нулевого порядка m00, который можно вычислить по вершинам с помощью
функции Polygon2(p).area() из пакета Toolbox. Для управления
мы будем использовать квадратный корень площади.
„„
„„

которая измеряется в пикселях. Требуемая скорость перемещения камеры по оси z достигается с помощью простого пропорционального закона управления
v∗z = λvz(σ∗ - σ).

(16.3)

Рассмотренные выше особенности управления перемещением и вращением по оси z просты и не требуют больших вычислительных затрат, но они особенно эффективны, когда нормаль
цели находится в пределах ±40° от оптической оси камеры. Если
плоскость цели не ортогональна оптической оси, то ее площадь

IBVS с разделением XY/Z 955

будет казаться уменьшенной из-за перспективы, что приведет
к первоначальному приближению камеры к цели. Перспектива также изменяет воспринимаемый угол наклона отрезка, что
может привести к небольшому вращательному движению вокруг оси z.
Блок-схема модели представлена на рис. 16.2. Симуляция запускается командой
>>> %run -m IBVS-partitioned-main -H
Параметр -H
предотвращает
блокировку до
закрытия всех окон
моделирования.

и воспроизводит анимационный эффект при изменении скорости камеры и ошибки характеристик с течением времени.
Такие параметры, как модель камеры, начальное положение
камеры и мировые точки, определяются в свойствах различных
блоков и в исходном коде IBVS-partitioned-main.

Камера

Блок Central
camera

P
Координаты
мировых
точек

Блок Visual
Jacobian

Jxy

Блок Inverse

Блок Camera
pose

Блок Image Plane

новые
скалярные
характеристики

управление
на основе
изображения

Блок Jacobian
condition

Блок Prod

Jz
v_z

(σ∗ – σ, θ∗ – θ)

λz
p∗ – p

ошибка
характеристик

p∗

Блок
Derotate flow

Блок Flatten

Желаемые
координаты
плоскости
изображения

общая ошибка
характеристик
Блок Camera
velocity
скорость камеры

Блок Main

Блок Prod 1

Блок Error
norm

v_xy
перестановка:
014235

Блок Mux

λ

Рис. 16.2. Блок-схема модели IBVS-partitioned визуального сервоуправления с разделением по осям XY/Z,
расширяющей модель IBVS, показанную на рис. 15.9. Начальное положение камеры задается в блоке Camera pose,
а целевые точки на плоскости изображения p∗ – в нижнем левом красном блоке

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

956 Глава 16 · Продвинутое визуальное сервоуправление

где d(p) – кратчайшее расстояние от точки p до края плоскости
изображения, а d0 – ширина зоны изображения, в которой действует отталкивающая сила. Для изображения W×H
d(p) = min(u, v, W - u, H - v).

(16.4)

Такая сила отталкивания может быть включена в регулятор
перемещения по оси z

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

16.2

IBVS с использованием полярных координат

В разделе 15.3 мы показали якобианы для точечных объектов,
выраженных в декартовых координатах (u, v), но ничто не мешает использовать и другие системы координат. В полярных
координатах точка изображения представлена как p = (ϕ, r), где
ϕ – угол
(16.5)
ϕ = tan-1(v–/u–),
а u– и v– – координаты изображения относительно главной точки, а не начала координат. Расстояние точки от главной точки
определяется как
(16.6)
Декартовы и полярные представления связаны соотноше­
нием
u– = r cos ϕ, v– = r sin ϕ.
(16.7)
Взяв производные по времени

инвертировав

IBVS с использованием полярных координат 957

и подставив в (15.9) вместе с (16.7), получаем

(16.8)

где якобиан имеет вид

(16.9)

Этот якобиан необычен тем, что содержит три постоянных
элемента. Ноль в первой строке означает, что полярный угол
инвариантен к перемещению вдоль оптической оси (точки
движутся вдоль радиальных линий), а отрицательное значение указывает на то, что полярный угол объекта (относительно
оси u) уменьшается при положительном повороте камеры. Ноль
во второй строке означает, что радиус r инвариантен к вращению вокруг оси z. Что касается декартовых координат точечных объектов, то поступательная часть якобиана (первые три
столбца) пропорциональна 1/Z. Обратите также внимание, что
якобиан не определен для r = 0, то есть для точки на оптической
оси. Якобиан изображения вычисляется методом visjac_p_polar
класса CentralCamera.
Желаемая скорость объекта является функцией ошибки характеристики

где ⊝ – вычитание по модулю 2π для угловой составляющей.
Выбор единиц измерения (пиксели и радианы) означает, что
|r| ≫ |ϕ| и радиус должны быть нормализованы:
так что r и ϕ имеют значения примерно одного порядка.
Пример IBVS, использующий полярные координаты, реализован классом IBVS_polar. Чтобы опробовать его, сначала нужно
создать модель камеры с настройками по умолчанию:

958 Глава 16 · Продвинутое визуальное сервоуправление
>>> camera = CentralCamera.Default(pose=SE3.Tz(-2)*SE3.Rz(pi))
>>> P = mkgrid(2, 0.5, pose=SE3.Tz(2))
>>> ibvs = IBVS_polar(camera, lmbda=0.1, P=P, pose_d=SE3.Tz(1),
...
depth=2, graphics=False)

что соответствует случаю на рис. 15.13, при повороте на угол π
вокруг оптической оси, в котором стандартный алгоритм IBVS
потерпел неудачу. Симуляция запускается командой
>>> ibvs.run()

В процессе симуляции можно видеть, как камера движется
прямо к цели и поворачивается по мере необходимости, а объекты перемещаются по прямой траектории на полярной плос­
кости изображения. Временную динамику движения объекта
и камеры можно отобразить вызовами
>>> ibvs.plot_p()
>>> ibvs.plot_pose()

Ориентация
камеры (рад)

Нормализованный радиус r

начальное положение
целевое положение

Местоположение
камеры (м)

На рис. 16.3а видно, что объекты переместились вверх и влево на ϕr-плоскости, которую можно рассматривать как цилиндр, левая и правая стороны соединены, и мы видим, что
полярный угол двух объектов охватывает диапазон от -π до π.
На рис. 16.3б показано, что камера совершила монотонное поступательное движение в направлении z и эффект отступления
камеры никак не проявляется.
Подход IBVS с использованием полярных координат дополняет подход IBVS на основе декартовых координат – он генерирует более оптимальное движение для большого угла поворота
камеры, но менее оптимальное для большого поступательного
перемещения. Чтобы использовать преимущества обоих представлений, можно для каждой точки объекта объединить якобиан 2×6 для декартовых координат точки (15.9) и якобиан 2×6
для полярных координат (16.9). В результате получится состав-

Азимут φ (рад)
Шаг по времени
а
б
Рис. 16.3. IBVS с использованием полярных координат: а) движение объекта в полярном ϕr-пространстве;
б) движение камеры в декартовом пространстве

IBVS для сферической камеры 959

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

16.3

IBVS для сферической камеры

В разделе 13.3 мы рассмотрели несколько неперспективных камер, таких как камера с объективом «рыбий глаз» и катадиопт­
рическая камера. Учитывая конкретные уравнения проекции
для любой камеры, можно вывести якобиан объектов изображения из первых принципов. Однако множество различных
форм линз и зеркал приводит ко множеству различных моделей проекции и якобианов изображения. В разделе 13.4 мы
видели, что характеристические точки с камеры любого типа
можно спроецировать на сферу, поэтому нам нужно всего лишь
вывести якобиан изображения для сферической камеры.
Действуем аналогично перспективной камере в разделе 15.2.1. Как показано на рис. 13.21, точка P представлена вектором P = (X, Y, Z) в системе координат камеры и проецируется
на поверхность сферы в точке p, представленной вектором p =
(x, y, z), где проекционный луч пересекает сферу. Мы можем записать
(16.10)
где
– расстояние от начала координат камеры до точки мира.
Для единичной сферы изображения ограничение поверхности x2 + y2 + z2 = 1 означает, что одна из декартовых координат
избыточна. Мы будем использовать минимальную сферическую систему координат, включающую азимутальный угол (или
долготу)
(16.11)
Коширота
равна нулю на
северном полюсе
и увеличивается
до π на южном.

и угол кошироты
θ = sin-1r ∈ [0, π],

(16.12)

где
, и это дает вектор точечного объекта p = (ϕ, θ).
Взяв производные (16.12) и (16.11) по времени и подставив
(15.5), а также
X = R sin θ cos ϕ,

Y = R sin θ sin ϕ,

Z = R cos θ,

(16.13)

960 Глава 16 · Продвинутое визуальное сервоуправление

получаем, в матричной форме, уравнение сферического оптического потока

(16.14)

где якобиан характеристики изображения равен

(16.15)

Как и в случае полярных координат, этот якобиан тоже имеет
три постоянных значения. Ноль в первой строке означает, что
азимутальный угол инвариантен к перемещению вдоль оптической оси, а отрицательное значение указывает на то, что азимут изменяется в направлении, противоположном вращению
камеры вокруг оптической оси. Ноль во второй строке означает,
что коширота инвариантна к вращению вокруг оптической оси.
Как и для всех якобианов изображений, подмат­ри­ца поступательного движения (первые три столбца) является функцией
глубины точки R. Этот якобиан вычисляется методом visjac_p
класса SphericalCamera.
Якобиан не определен на северном и южном полюсах, где
sin θ = 0, и азимут тоже не имеет смысла в этих точках. Это точки
сингулярности, и, как отмечалось в разделе 2.3.1.3, в контексте
представления ориентации в виде углов Эйлера это является
следствием использования минимального представления. Однако в целом преимущества такого применения перевешивают
издержки.
Для целей контроля мы следуем обычной процедуре вычисления одного якобиана 2×6 (16.15) для каждого из N объектов
и их объединения для формирования мат­ри­цы 2N×6:

(16.16)

IBVS для сферической камеры 961

Закон управления имеет вид
v = J+ṗ∗,

(16.17)

ṗ∗ = λ(p∗ ⊝ p),

(16.18)

где ṗ∗ – целевая скорость объектов в ϕθ-пространстве. Обычно
мы выбираем ее пропорциональной ошибке характеристики

где λ – положительный коэффициент усиления, p – текущая
точка в системе ϕθ-координат, а p∗ – искомое значение. Это
приводит к локальному линейному движению объектов в пространстве признаков. ⊝ обозначает вычитание по модулю
и возвращает наименьшее угловое расстояние при условии, что
ϕ = [-π, π] и θ ∈ [0, π].
Пример IBVS, использующий сферические координаты
(рис. 16.4), реализуется классом IBVS_sph. Сначала создается
сферическая камера и набор мировых точек
>>> camera = SphericalCamera(pose=SE3.Trans(0.3, 0.3, -2)
...
* SE3.Rz(0.4))
>>> P = mkgrid(2, side=1.5, pose=SE3.Tz(0.5))

а затем объект IBVS для сферической камеры
>>> ibvs = IBVS_sph(camera, P=P, pose_d=SE3.Tz(-1.5), verbose=False,
...
graphics=False)

где pose_dis – положение камеры, используемое для определения целевых точек на сфере изображения – оно не используется
для управления. Запустив симуляцию
>>> ibvs.run()

Так же, как при
повороте вокруг оси
x и перемещении
вдоль оси y.
При условии что
точки мира равномерно распределены по сфере.

можно увидеть движение объекта на ϕθ‑плоскости, а также камеру и точки мира.
Сферическое представление имеет множество преимуществ
для визуального сервоуправления. Во-первых, сферическая камера устраняет необходимость удержания объектов в поле зрения, что, как мы видели, является проблемой для визуального
сервоуправления на основе как положения, так и некоторых
других гибридных схем. Во-вторых, ранее мы наблюдали неоднозначность между полями оптического потока при повороте
вокруг оси y и перемещении вдоль оси x для небольшого поля
зрения. В системах IBVS с длиннофокусной камерой это может
привести к медленной сходимости и/или высокой чувствительности к неточностям в определении координат объектов. Для
сферической камеры с максимально возможным полем зрения
эта неоднозначность уменьшается. ◄

Нормализованный радиус r

962 Глава 16 · Продвинутое визуальное сервоуправление

начальное положение
целевое положение

а

Азимут φ (рад)

б

Рис. 16.4. IBVS для сферической камеры и координат: а) движение объекта в ϕθ‑пространстве; б) четыре целевые
точки, спроецированные на сферу в их исходном положении

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

16.4

Применение

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

16.4.1

Робот-манипулятор

В этом примере камера установлена на 6-осевом манипуляторе,
способном управлять всеми шестью степенями свободы положения камеры. Будем считать, что сочленения робота являются идеальными источниками скорости, то есть движутся точно
с заданной скоростью. Современный робот очень близок к этому
идеалу и обычно оснащен высокопроизводительными контроллерами сочленений, использующими обратную связь по скорости и положению от энкодеров, установленных в сочленениях.
Вложенная структура управления сочленениями робота обсуждалась в разделе 9.1.7. Внутренний контур использует обрат-

Камера на
рис. 13.27б
охватывает 90 %
сферического поля
зрения.

Применение 963

В данном случае
почтенный Puma
560 из части III этой
книги, но вы можете
выбрать любую другую модель робота,
поддерживаемую
пакетом Toolbox.

ную связь по скорости, чтобы обеспечить движение сочле­нения
с заданной скоростью. Внешний контур использует обратную
связь по положению сочленения для определения скорости,
с которой должно перемещаться сочленение, чтобы следовать
по заданной траектории. Эти контуры часто работают с частотой дискретизации 1 кГц. В системе визуального сервоуправления контур управления положением основывается на системе технического зрения, которая имеет намного более низкую
частоту дискретизации, обычно 25 или 30 Гц, и нередко имеет
задержку в один или два периода выборки.
Блок-схема модели «глаз в руке» показана на рис. 16.5. Она
Требуемая
имитирует камеру, управление IBVS и робота.
скорость камеры подается на вход контроллера движения, осуществляющего управление на основе якобиана манипулятора.
Угловые скорости сочленений подаются на вход дискретного
интегратора, который образует контур управления скоростью
робота. На основе результирующих углов сочленений блок прямой кинематики определяет положение рабочего органа. Перспективная камера с параметрами по умолчанию установлена
на рабочем органе, и ее оси совмещены с осями координат рабочего органа. Блок камеры имеет единственный параметр – объект CentralCamera, а его входные данные – координаты целевых
точек – углов квадрата в плоскости yz – и положение камеры
в мировой системе координат. Объекты на выходном изобра-

Камера

P

управление на основе изображения

Блок Central
camera

Блок Visual
Jacobian

Координаты
мировых
точек

Блок Inverse

Блок Image Plane

Блок Camera
velocity

λ

Блок Jacobian
condition

p∗ – p

ошибка
характеристик

p∗

Желаемые
координаты
плоскости
изображения

Блок Prod

Блок Flatten

общая ошибка
характеристик
Блок Error
norm

робот-манипулятор

скорость
камеры
Блок Armplot

углы сочленений
Блок Inverse 1

Блок Main
q
прямая
кинематика

qdot
контур
управления
скоростью

Блок Manipulator
Jacobian

Блок Prod 1

Рис. 16.5. Блок-схема модели IBVS-armus, которая использует подход IBVS для управления манипулятором,
держащим камеру

964 Глава 16 · Продвинутое визуальное сервоуправление

жении используются для вычисления якобиана изображения
с предполагаемым значением Z для каждой точки, а также для
определения погрешности вычисления положения объекта
в пространстве изображения. Якобиан изображения инвертируется, и применяется коэффициент усиления для определения
пространственной скорости камеры. Контур управления получается замкнутым, и система будет управлять роботом и камерой для достижения желаемой конфигурации точек в плоскости
изображения.
Симуляция запускается как обычно:
>>> %run -m IBVS-arm-main -H

В процессе выполнения она отображает анимацию перемещения робота и объектов на плоскости изображения виртуальной камеры.
Симуляция создает в глобальном пространстве объект out
с результатами. Углы поворота сочленений робота на каждом
шаге времени хранятся как состояние дискретного интегратора, подключенного к clock0, и могут быть отображены на графике:
>>> plt.plot(out.clock0.t, out.clock0.x)

Такие параметры, как модель робота и камеры, начальная
конфигурация робота и точки мира, определяются в свойствах
различных блоков и в исходном коде в IBVS-arm-main.py.
Обратите внимание, что эта модель не учитывает динамику
ни манипулятора, ни системы зрения – сочленения моделируются как идеальные устройства, а система зрения – как не имеющая задержки. Однако эти два динамических эффекта очень
важны для высокоточного визуального управления и должны
учитываться. Тем не менее эта модель вполне может стать основой для более точной модели, учитывающей реальные эффекты.

16.4.2

Мобильный робот

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

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

Применение 965

16.4.2.1 Голономный мобильный робот
Для решения этой задачи предположим, что на роботе закреп­
лена перспективная камера, направленная вверх, и имеется несколько ориентиров, смонтированных на потолке, которые постоянно видны камере во время движения. Система координат
робота такова, что ось x направлена вперед, а ось z – вверх.
Определим перспективную камеру
>>> camera = CentralCamera.Default(f=0.002);

с широким полем зрения, позволяющим удерживать ориентиры в поле зрения во время движения. По отношению к корпусу
робота {B} камера установлена в постоянном относительном
положении BξC
>>> T_B_C = SE3.Trans(0.2, 0.1, 0.3);

спереди слева на высоте 30 см над уровнем земли. Ее оптическая ось направлена вверх, а ось x – вперед. Два ориентира находятся на высоте 3 м над землей и расположены в точках x = 0
и y = ±1 м в мировой системе координат.
>>> P = np.array([[0, 1, 3], [0, -1, 3]]).T;

Желаемое положение робота – центр его задней оси – находится в точке (-2, 0).
Робот перемещается в плоскости xy и может вращаться только вокруг оси z, поэтому удалим из (15.9) столбцы якобиана
изоб­ражения, соответствующие недопустимому движению,
и запишем
(16.19)
Как и в стандартном случае IBVS, объединим эти якобианы,
по одному на каждый ориентир, а затем инвертируем уравнение для вычисления скорости робота. Поскольку неизвестных
компонентов скорости всего три, а каждый ориентир участвует
в двух уравнениях, для вычисления скорости нам потребуется
не менее двух опорных точек.
Модель блок-схемы показана на рис. 16.6 и представляет собой гибрид более ранних моделей, изображенных на рис. 4.7
и 15.9. Симуляция запускается командой
>>> %run -m IBVS-holonomic-main -H

Она отображает изменение траектории транспортного средства в плоскости xy и вид с камеры. Результаты сохраняются

966 Глава 16 · Продвинутое визуальное сервоуправление

Камера

P
Координаты
мировых
точек

управление на основе изображения

Блок Central
camera

Блок Visual
Jacobian

Jxyz

Блок Inverse

Блок Prod

Блок Camera
velocity

λ

Блок Jacobian
condition

Блок Image Plane
p∗ – p
ошибка
характеристик

p∗

Блок Flatten

общая ошибка
характеристик
Блок Error
norm

Желаемые
координаты
плоскости
изображения

скорость
камеры

мобильный робот

q

Блок Main

bξc

преобразование
q в SE3

qdot

анимация движения
транспортного средства

контур управления
скоростью

Рис. 16.6. Блок-схема модели IBVS-holonomic, которая приводит голономный мобильный робот в целевое
положение с использованием подхода IBVS

в объекте out и доступны для отображения, как и в предыдущих
примерах. Параметры, такие как модель робота и модель камеры, начальная конфигурация робота и точки мира, определяются в свойствах различных блоков и в исходном коде IBVSholonomic-main.py.

16.4.2.2 Неголономный мобильный робот
Задача приведения неголономного мобильного робота в определенное положение обсуждалась в главе 4, и в разделе 4.1.1.4 был
представлен соответствующий нелинейный регулятор. Блоксхема модели, о которой идет речь, показана на рис. 16.7, и в ней
снова используется контроллер, основанный на полярных координатах (ρ, α, β). В этом примере будут использоваться методы
PBVS для оценки переменных, необходимых для управления. По
условиям задачи робот оснащен перспективной камерой, которая закреплена на его корпусе {B} с относительным положением
B
ξC, имеется ряд ориентиров с известными местоположениями
относительно целевой системы координат {G}, которые постоянно находятся в поле зрения, и есть возможность в любой момент
определить направление движения транспортного средства, например с помощью компаса или другого датчика.
Блок-схема модели показана на рис. 16.8, а вид ориентиров
имитируется блоком Camera, выходные данные которого – проекции точек – передаются на вход блока оценки положения,
параметрами которого являются известные местоположения
ориентиров.

Применение 967

цель

камера

Рис. 16.7. PBVS для неголономного транспортного средства (модель велосипеда).
Транспортное средство движется к целевому положению: ρ – расстояние до цели, β – угол
вектора цели относительно мировой системы координат, α – угол вектора цели относительно
системы координат транспортного средства. P1 и P2 – ориентиры, которые находятся под
углами Cψ1 и Cψ2 относительно камеры
контроллер положения
виртуальная
одноколесная модель
Блок Stop
в полярные

Kp

∗

скорость
со знаком

Блок Bicycle
ω

ω

Kα

положение
относительно
цели

∗

∗

∗

atan

ω со знаком

∗

Блок Vehicleplot

θ

β

q_est

Kβ

θ

ML
q

∗

(x , y , θ )

q

v

∗

txy

∗

get θ

ошибка
положения

∗

(x , y , 0)

Блок Demux

0
Конфигурация
модели

камера
p
Блок Camera

Блок Constant

ξb
SE(2) в SE3

Блок Main

cξg
Блок Estpose_p

gξc
Блок Pose_inverse

gξb
cξb

ξc

Оценка положения

bξc

Блок Image Plane

Рис. 16.8. Блок-схема модели IBVS-holonomic, которая приводит неголономный мобильный робот в целевое
положение (построена на основе блок-схемы на рис. 4.14)

968 Глава 16 · Продвинутое визуальное сервоуправление

Как обсуждалось в разделе 13.2.4, для решения задачи необходимо не менее трех ориентиров, а в данном примере их используется пять. Блок оценки положения выдает предполагаемое положение ориентиров относительно камеры CξˆG, которое
преобразуется в положение робота относительно цели. Поступательные компоненты x и y этого положения объединяются
с предполагаемым углом направления ► для получения оценки
конфигурации модели (x̂, ŷ, θ̂), которая поступает на вход регулятора положения. Остальная часть системы фактически аналогична примеру на рис. 4.14.
Симуляция запускается как обычно:
>>> %run -m IBVS-nonholonomic-main -H

и отображает изменение траектории транспортного средства
в плоскости xy и изображения с камеры. Результаты сохраняются в объекте out и доступны для отображения, как и в предыдущих примерах. Параметры, такие как модель робота и камеры, начальная конфигурация робота и точки мира, определены
в свойствах различных блоков и в исходном коде IBVS-nonholonomic-main.py.

16.4.3

Воздушный робот

Сферическая камера особенно хорошо подходит для воздушных и подводных роботов, перемещающихся в трехмерном
пространстве. В этом примере мы рассмотрим квадрокоптер,
оснащенный сферической камерой, и используем IBVS для перемещения квадрокоптера в определенное положение относительно четырех целевых точек на земле.
Как обсуждалось в разделе 4.3, квадрокоптер – малопривод­
ный робот и не позволяет независимо управлять всеми шестью
степенями свободы в пространстве задач. Мы можем контролировать только положение по осям (X, Y, Z) и угол рыскания. Углы
крена и тангажа являются управляющими входами, используемыми для перемещения в горизонтальной плоскости, и должны быть равны нулю, когда аппарат находится в равновесии.
Блок-схема модели показана на рис. 16.9. Этот контроллер пытается удерживать квадрокоптер в постоянном относительном
положении относительно целевых точек. Если цель движется,
то и квадрокоптер будет двигаться – можно представить, как
подобная схема используется для посадки квадрокоптера на
движущееся транспортное средство – автомобиль, мобильный
робот или лодку.
Модель представляет собой гибрид контроллера квадрокоптера (рис. 4.24) и малоприводной системы IBVS (рис. 16.6). Од-

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

Применение 969
управление полетом

БПЛА

Анимация
многовинтовой модели

силы
и моменты
планера

Блок Main

требование
крена
и тангажа
∗

∗

управление
скоростью

(x , y )

ограничения
крена
и тангажа

Многовинтовая
модель

управление
угловой
скоростью

выбор x

конфигурация
БПЛА

выбор vb

Блок Scope

Управление
винтами

вектор состояния х

Блок Demux

управление
скоростью рыскания

управление скоростью
изменения высоты

∗

∗

∗

∗

желаемая скорость камеры (xd , yd , zd , yd )

вектор состояния

камера

Блок Camera

Блок Constant 1

управление на основе изображения

Блок Visjac_p

Блок Slice2

Блок Inverse

Блок Prod

Блок Gain

Требуемая скорость

положение
камеры SE3
Блок Jacobian
condition

Блок Image Plane

∗

p

∗

p –p

Блок Flatten

Блок Norm

Ошибка
характеристик

Рис. 16.9. Блок-схема модели IBVS-quadrotor. Блок p* – p имеет параметр, позволяющий корректно обрабатывать
углы на сфере при вычитании

Это обычная
аппроксимация
движения объекта.

нако существует ряд ключевых отличий. Во-первых, в системе
управления квадрокоптером (рис. 4.24) использовалась мат­ри­
ца вращения для отображения ошибки xy в мировую систему координат с требуемыми углами тангажа и крена аппарата. В случае визуального сервоуправления этого не требуется, потому
что ошибка по осям xy регистрируется непосредственно камерой на корпусе. Во-вторых, как и в случае мобильного робота,
транспортное средство малоприводное, и его якобиан содержит только столбцы, соответствующие (vx, vy, vz, ωz). В-третьих,
мы используем сферическую камеру, поэтому блокам Camera
и Visual Jacobian передается объект SphericalCamera.
В-четвертых, между креном-тангажом квадрокоптера и координатами объекта в плоскости изображения существует
связь. Напомним, что квадрокоптер не может выполнять перемещения без предварительного наклона в желаемом направлении, что влечет смещение объектов на изображении и увеличение погрешности изображения. При небольших углах крена
и тангажа эту погрешность можно игнорировать, но при резких
маневрах необходимо ее учитывать. Хотя в данной модели это
не реализовано, но вообще идея заключается в использовании
якобиана изображения для аппроксимации ► смещения объ-

970 Глава 16 · Продвинутое визуальное сервоуправление

ектов (Δϕ, Δθ) как функции смещения угла наклона и тангажа
камеры, которые представляют собой повороты вокруг осей x
и y соответственно:

Эти данные можно вычесть из положений объектов, наблюдаемых камерой, чтобы получить положения объектов, которые
наблюдались бы камерой из начала координат {B}, но с осями,
параллельными осям системы координат {0}. Эту схему иног­
да называют деротацией характеристик, потому что она программно имитирует эффект неподвижной или стабилизированной карданным подвесом камеры.
Сравнивая рис. 16.9 и рис. 4.24, можно заметить, что контроллер визуального управления выполняет функцию самых внеш­
них контуров управления координатами x и y, высотой и рысканием. Он напрямую генерирует необходимые входные данные
для контуров управлением скоростью по этим степеням свободы. Отметьте также, что информация о текущей скорости попрежнему должна передаваться на вход контура управления
скоростью, и в реальном воздушном роботе она будет получена
от инерциального измерительного устройства.
Симуляция запускается командой
>>> %run -m IBVS-quadrotor-main -H

отображает анимацию изменения траектории движения аппарата в трех измерениях и вид с камеры, а различные блоки
слежения (Scope) строят изменения скорости камеры и погрешности характеристик. Результаты сохраняются в объекте out
и доступны для отображения, как и в предыдущих примерах.
Такие параметры, как модель квадрокоптера и камеры, начальная конфигурация квадрокоптера и точки мира, определяются
в свойствах различных блоков и в исходном коде IBVS-quadrotormain.py.

16.5

Подведение итогов

16.5.1

Дополнительное чтение

Хорошим введением в продвинутые технологии визуального
сервоуправления является обучающая статья Chaumette and
Hutchinson (2007), а также глава о визуальном сервоуправлении

Подведение итогов 971

в Siciliano and Khatib (2016, § 34). Значительный интерес к так
называемым гибридным методам в свое время был вызван работой Шометта (Chaumette 1998), в которой представлен конкретный пример перемещения камеры точечной системы IBVS
в бесконечность для случая вращения цели вокруг оптической
оси. Одним из первых методов решения этой проблемы было
2.5-мерное визуальное сервоуправление, предложенное в Malis et al. (1999), которое дополняет точечные признаки изображения минимальным декартовым признаком. Другие известные ранние гибридные методы были предложены в Morel et al.
(2000) и Deguchi (1998), авторы которых разделили якобиан изображения на поступательную и вращательную части. Гомография вычисляется между исходным и конечным видами (поэтому целевые точки должны быть плоскими), а затем разлагается
для определения поворота и перемещения. Морель с коллегами
объединили эту информацию о повороте с управлением перемещением, основанным на IBVS точечных объектов. Дегучи
с коллегами, напротив, объединили эту информацию о перемещении с управлением перемещением, основанным на IBVS.
Поскольку перемещение определяется только с точностью до
неизвестного масштабного коэффициента, требуются дополнительные средства определения масштаба.
В Corke and Hutchinson (2001) представлено простое и понятное геометрическое объяснение проблемы движения камеры от
цели во время сервопривода и предложена схема разделения по
осям: перемещение и вращение по осям x и y в одной группе,
а перемещение вдоль z – в другой. Другой подход к гибридному
визуальному сервоуправлению заключается в быстром переключении между подходами IBVS и PBVS (Gans et al. 2003), там
же можно найти сравнение нескольких схем разделения.
Полярная форма якобиана изображения для точечных объектов (Iwatsuki and Okiyama 2002a, 2002b; Chaumette and Hutchinson 2007) хорошо справляется со случаем отказа IBVS, но приводит к несколько неоптимальному поступательному движению
камеры (Corke et al. 2009) – обратному тому, что происходит
в декартовой форме.
Якобиан для сферической камеры аналогичен полярной форме. Двухугловая параметризация была впервые описана в Corke
(2010) и использовалась для управления и оценки структуры по
движению. Исследования сферического визуального сервоуправления относительно немногочисленны. В Fomena and Chaumette
(2007) рассматривается случай одного сферического объекта, из
которого извлекаются характеристики, полученные из проекции
на сферическую плоскость изображения, такие как центр окружности и ее видимый радиус. В Tahri et al. (2009) рассматриваются
такие особенности сферического изображения, как линии и моменты. В Hamel and Mahony (2002) описывается кинодинамиче-

972 Глава 16 · Продвинутое визуальное сервоуправление

ское управление малоприводным (underactua­ted) воздушным
роботом с использованием точечных характеристик.
Динамика робота-манипулятора (9.11) и перспективная проекция (13.2) являются крайне нелинейными и зависят от состояния манипулятора и цели. Почти все системы визуального сервоуправления исходят из того, что робот управляется по
скорости, а лежащая в его основе динамика подавляется и линеаризуется жесткими контурами управления. Как мы узнали
в разделе 9.1, это в полной мере относится к роботам с манипулятором, и в примере с квадрокоптером мы использовали
похожую вложенную структуру управления. Такой подход обусловлен малыми постоянными времени базовой механической
динамики, а также низкой частотой дискретизации и задержкой любого визуального контура управления. Современные
компьютеры и высокоскоростные камеры теоретически позволяют отказаться от контуров управления скоростью на уровне
осей, но они намного проще в использовании. Высокочастотные камеры становятся все более доступными, но имеют более
низкое разрешение и могут требовать более высокого уровня
освещенности сцены, поэтому задержка системы зрения приобретает все большую важность.
Визуальное сервоуправление неголономными роботами –
нетривиальная задача, потому что согласно теореме Брокетта
(Brockett 1983) ни один линейный стационарный контроллер не
может им управлять. Подход, используемый в этой главе, основан
на позиционировании и представляет собой небольшое расширение регулятора положения, представленного в разделе 4.1.1.4.
Подходы IBVS были предложены в Tsakiris et al. (1998) и Masutani
et al. (1994), но они требуют, чтобы камера была прикреплена
к подвижному основанию робота с небольшим числом степеней
свободы. В Mariottini et al. (2007) описывается двухэтапный подход к сервоуправлению, когда камера жестко закреплена на основании, а эпиполюса геометрии, определяемые текущим и желаемым видами камеры, явно сервоуправляемы. Ашер (Usher et
al. 2003; Usher 2005) описывает закон переключения управления,
который подводит робота на линию, проходящую через заданное
положение, а затем перемещает вдоль этой линии к требуемому
положению, и приводит результаты экспериментов, полученные
на транспортном средстве на открытом воздухе. Сходство между
навигацией мобильного робота и задачей визуального серво­
управления обсуждается в работе Corke (2001).

16.5.2

Ресурсы

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

Подведение итогов 973

ных систем, и работали гораздо медленнее, чем в реальном
времени. Для применения визуального управления на практике необходимы быстрые алгоритмы обработки изображений
и извлечения признаков, а также средства связи с аппаратным
обеспечением робота. К счастью, существует множество инструментов и технологий, помогающих в этом: Robot Operating
System (также известная как ROS www.ros.org) предлагает комплексную платформу программного обеспечения для создания
роботов, OpenCV – для обработки изображений (www.opencv.
org) и ViSP – для создания визуальных трекеров и контроллеров
https://visp.inria.fr.

16.5.3

Упражнения

1. IBVS с разделением XY/Z (раздел 16.1).
а) Исследуйте движение, генерируемое для различных комбинаций камеры, перемещения и вращения, и сравните
с классической схемой IBVS из предыдущей главы.
б) Реализуйте сценарий, в котором объекты покидают изображение.
в) Добавьте отталкивающее поле, чтобы гарантировать, что
объекты останутся в пределах изображения.
г) Исследуйте вариации уравнения (16.3). Вместо того чтобы стремиться к нулевой разности площадей, попробуйте
стремиться к единичному отношению текущей и искомой площадей или к нулевому логарифму этого отношения. См. Mahony et al. (2002).
2. Исследуйте IBVS для полярных и сферических координат
с различными комбинациями перемещения и вращения камеры и сравните с классической схемой IBVS в предыдущей
главе.
3. Пример IBVS робота-манипулятора (раздел 16.4.1).
а) Добавьте смещение (вращение и/или перемещение) между рабочим органом и камерой. Для его учета вам потребуется включить дополнительный якобиан в ваш регулятор (см. раздел 3.1.3).
б) Добавьте задержку после блока Camera, чтобы сымитировать время обработки изображения. Исследуйте реакцию
при увеличении задержки и компромисс между усилением и задержкой. Постройте диаграмму корневых годографов для этой динамической системы в дискретном времени.
в) Смоделируйте движущуюся цель. Покажите погрешность
слежения, то есть расстояние между камерой и целью.
г) Исследуйте методы прямой связи для улучшения управления (Corke 1996b). Попробуйте основываться не на том,

974 Глава 16 · Продвинутое визуальное сервоуправление

4.

5.

6.

7.

где цель была видна камере, а на том, где она будет находиться в ближайшем будущем. Насколько далеко в будущем? Какая модель хорошо подходит для такой оценки?
Исследуйте фильтры α – β как один из вариантов простого предиктора.
д) Использование камеры слежения за объектом при стыковке может привести к проблемам, потому что она
слишком близко подходит к цели. Как настроить точки
цели и камеру, чтобы избежать этого?
Визуальное сервоуправление мобильным роботом (раздел 16.4.2).
а) Для голономного и неголономного случаев замените перспективную камеру катадиоптрической.
б) В случае голономного робота с катадиоптрической камерой переместите его через ряд промежуточных точек,
каждая из которых определена в терминах набора желаемых координат объекта.
в) Для неголономного робота реализуйте контроллеры чис­
того преследования и следования по линии, которые бы­
ли описаны в главе 7, но с использованием приемов визуального управления. Для случая чистого преследования
предположите, что преследуемый объект имеет один или
два точечных признака. Для случая следования по линии
рассмотрите возможность использования одного или
двух линейных объектов.
Отобразите поля потока признаков, как на рис. 15.7, для полярной r - ϕ и сферической θ - ϕ проекций (разделы 16.2
и 16.3). В случае сферической проекции проверьте, можно ли
построить векторы потока на поверхности сферы.
Визуальное
сервоуправление
квадрокоптером
(раздел 16.4.3).
а) Замените сферическую камеру перспективной.
б) Создайте контроллер, который будет следовать за серией
точечных объектов, а не наводить курсор на одну точку.
в) Добавьте деротацию элементов изображения, чтобы минимизировать влияние крена и тангажа транспортного
средства на визуальное управление.
Реализуйте 2.5-мерную схему визуального сервоуправления, предложенную в Malis et al. (1999).

Приложения
Приложение А
Приложение В
Приложение С
Приложение D
Приложение Е
Приложение F
Приложение G
Приложение H
Приложение I
Приложение J

Установка наборов инструментов
Линейная алгебра
Геометрия
Группы и алгебра Ли
Линеаризация, якобианы и гессианы
Решение систем уравнений
Гауссовы случайные величины
Фильтр Калмана
Графы
Поиск пиков

Приложение

A

	Установка наборов
инструментов

Самые актуальные инструкции по установке необходимого программного обеспечения всегда можно найти на сайте
https://github.com/petercorke/RVC3-python.

A.1

Установка пакетов

В этой книге используются следующие пакеты Python, распространяемые с открытым исходным кодом:
„„
„„

Robotics Toolbox for Python (roboticstoolbox);
Machine Vision Toolbox for Python (machinevisiontoolbox).

Эти пакеты, в свою очередь, имеют зависимости от других
пакетов, созданных автором и третьими лицами.
Пакет Python rvc3python позволяет установить все эти наборы
инструментов и их зависимости за один шаг:
$ pip install rvc3python

или
$ conda install rvc3python

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

rvctool – сценарий командной строки, представляющий
собой оболочку для IPython. Он импортирует вышеупомянутые пакеты с помощью import *, а затем предоставляет
интерактивную вычислительную среду. По умолчанию
rvctool выводит приглашение к вводу, подобно обычному интерпретатору Python, и автоматически отображает
результаты выражений, подобно MATLAB® – чтобы подавить такое поведение, добавляйте точку с запятой в конце
строк. rvctool позволяет вырезать и вставлять строки из
книги и игнорирует символы приглашения к вводу;

Приложение A · Установка наборов инструментов 977

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

Репозиторий RVC3-python на GitHub также содержит готовые
для онлайн-демонстрации версии выбранных 3D-фигур и все
линейные чертежи в формате EPS.

A.2

Модели блок-схем

Модели блок-схем запускаются с помощью пакета bdsim, который может запускать модели:
написанные на Python с использованием классов bdsim
и методов связывания;
„„ созданные с помощью графического интерфейса bdedit
и сохраненные в файлы .bd (формат JSON).
„„

A.3

Получение помощи

В репозитории RVC3-python на GitHub размещено несколько
других ресурсов для пользователей:
страницы вики с ответами на часто задаваемые вопросы;
обсуждения пользователями различных аспектов книги
и основных инструментов;
„„ раздел Issues (Проблемы), где можно сообщить о проблемах
и ошибках, а также обсудить их.
„„
„„

Приложение

B

	Линейная алгебра

B.1

Векторы

Термин вектор имеет несколько значений, поэтому бесконт­
рольное его использование может привести к путанице:
в информатике вектор – это массив или кортеж чисел;
в физике и технике вектор – это физическая величина,
такая как сила или скорость, которая имеет величину, направление и единицу измерения;
„„ в математике вектор – это объект, принадлежащий векторному пространству;
„„ в линейной алгебре вектор – это одномерная мат­ри­ца, организованная либо в виде строки, либо в виде столбца.
„„
„„

В этой книге используются все эти интерпретации, поэтому
для устранения неоднозначности мы полагаемся на контекст,
но в этом разделе мы остановимся только на двух последних
интерпретациях. Мы будем рассматривать лишь вещественные
векторы, которые представляют собой упорядоченный набор
из n вещественных чисел и обычно записываются как
υ = (υ0, υ1, ..., υn-1) ∈ ℝn,

где υ1, υ2 и т. д. называются скалярными компонентами или
элементами υ, а υi называется i-й компонентой υ. Символ ℝn =
ℝ × ℝ × ... × ℝ – это декартово произведение, обозначающее
множество упорядоченных n-мерных кортежей действительных чисел. Для 3-вектора мы часто записываем элементы как
υ = (υx, υy, υz).
Координата точки в n-мерном пространстве тоже представлена кортежем из n действительных чисел. Координатный вектор – это тот же кортеж, но он интерпретируется как линейная
комбинация υ = υ0e0 + υ1e1 + ... + υn-1en-1 ортогональных базисных
векторов {e0, e1, ..., en-1} пространства – это вектор из начала координат в точку. В этой книге базисные векторы обозначаются
как {x̂, ŷ} или {x̂, ŷ, ẑ} для двух или трех измерений соответственно.

Приложение B · Линейная алгебра 979

В математике n-мерное векторное пространство (также называемое линейным пространством) – это объект, подобный
группе, который содержит набор объектов, называемых векто­
рами υ ∈ ℝn. Оно поддерживает операции сложения векторов
a + b = (a0 + b0, a1 + b1, ..., an-1 + bn-1) и умножения («масштабирования») на число s, называемое скаляром sa = (sa0, sa1, ..., san-1).
Отрицание вектора, масштабирование на -1, получается путем
отрицания каждого элемента вектора -a = (-a0, -a1, ..., -an-1).
Символ ℝn используется для обозначения пространства точек
или векторов, и для разрешения этой неоднозначности необходим контекст. Следует тщательно различать точки и векторы,
потому что операции сложения и скалярного умножения, допустимые для векторов, бессмысленны для точек. Мы можем
сложить вектор с вектором координат одной точки, чтобы получить вектор координат другой точки, и вычесть один вектор
координат из другого, получив в результате смещение между
точками.
Для многих операций в линейной алгебре важно различать
векторы-столбцы и векторы-строки

которые эквивалентны мат­ри­цам размером n×1 и 1×n (см. следующий раздел) соответственно. Большинство векторов в этой
книге являются векторами-столбцами, которые иногда записываются в компактном виде как (υ0, υ1, ..., υn-1)T, где ·T обозначает
операцию транспонирования мат­ри­цы. Оба вида векторов могут быть обозначены как ℝn или различаться как ℝn×1 или ℝ1×n
для векторов-столбцов и векторов-строк соответственно.
Величина, или длина, вектора – это неотрицательная скалярная величина, заданная его p-нормой:
np.linalg.norm(v)

Единичный вектор,
соответствующий
вектору υ, –
1

это υ̂ = ||υ|| υ.
2

Евклидова длина вектора определяется как ||υ||2, часто называется нормой L2, и обычно в таком случае букву p опускают, например ||υ||. Единичный вектор – это такой вектор, для
которого ||υ||2 = 1, и он обозначается υ̂. Норма L1 – это сумма
абсолютных значений элементов и также известна как манхэттенское расстояние, расстояние, пройденное при ограничении
движения вдоль линий сетки. Норма L∞ является максимальным элементом вектора.

980 Приложение B · Линейная алгебра

Скалярное произведение двух векторов-столбцов является
скаляром
np.dot(a, b)

где θ – угол между векторами; a · b = 0, когда векторы ортогональны. Если a и b – векторы-столбцы, где a, b ∈ ℝn×1, то скалярное произведение можно записать как
a · b = aTb = bTa.

Внешнее произведение abT ∈ ℝn×n имеет максимальный ранг,
равный единице, а если b = a, то в результате получается симметричная мат­ри­ца.
Для трехмерных векторов векторное произведение записывается как

np.cross(a, b)

где x̂ – единичный вектор, параллельный оси x и т. д., а n̂ – единичный вектор, нормальный к плоскости, содержащей a и b.
Если векторы параллельны, a × b = 0.

B.2

Матрицы

В этой книге нас интересуют только вещественные мат­ри­цы
размера m×n

с m строками и n столбцами. Если n = m, мат­ри­ца квадратная.
Матрицы одинакового размера A ∈ ℝm×n и B ∈ ℝm×n можно
складывать:
C = А + B,

ci,j = ai,j + bi,j,

i = 0, ..., m - 1, j = 0, ..., n - 1;

Вещественные мат­
ри­цы являются подмножеством всех
матриц. В общем
случае комплексных матриц термин
«эрмитова мат­ри­ца»
является аналогом
симметричной
мат­ри­цы, а «унитарная» – аналогом
ортогональной. AH
обозначает эрмитово транспонирование – комплексно-­
сопряженную
транспонированную
комплексную мат­
ри­цу A. Матрицы
являются тензорами
второго ранга.
A+B

умножать поэлементно:
C = А ∘ B,

ci,j = ai,j bi,j,

i = 0, ..., m - 1, j = 0, ..., n - 1,

A*B

Приложение B · Линейная алгебра 981

это также называется произведением Адамара. Две мат­ри­цы
с совместимыми размерностями A ∈ ℝm×n и B ∈ ℝn×p можно перемножить.
A@B

что представляет собой умножение матриц и C ∈ ℝm×p.
Транспонирование записывается следующим образом:
A.T

B = AT,

bi,j = aj,i, ∀i, j,

и можно показать, что
(AB)T = BTAT,

(ABC)T = CTBTAT

и т. д.

Элементы мат­ри­цы, в свою очередь, могут быть мат­ри­ца­ми,
образующими общую блочную мат­ри­цу

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

Таксономия матриц показана на рис. B.1.

B.2.1
np.linalg.inv(A)

Квадратные мат­ри­цы

Квадратная мат­ри­ца может иметь обратную мат­ри­цу A−1,
и в этом случае
AA-1 = A-1A = 1n×n,

982 Приложение B · Линейная алгебра
все матрицы

комплексные

вещественные

неквадратные

квадратные

общие
det≠0
обратимые

SE(n)

нормальные

det=0

вырожденные

симметричные

ортогональные O(n)
det=+1

+ve знакоопределенные

кососимметричные

det=–1

SO(n)

диагональные

тождественные

Рис. B.1. Таксономия матриц. Матрицы, показанные синим цветом, никогда не бывают сингулярными

где

np.eye(n)

– единичная диагональная мат­ри­ца, иногда обозначается как I.
Обратная мат­ри­ца A-1 существует при условии, что исходная
мат­ри­ца A невырождена, т. е. ее определитель det(A) ≠ 0. Обратную мат­ри­цу можно вычислить из мат­ри­цы кофакторов
(алгебраических дополнений). Если мат­ри­цы A и B квадратные
и невырождены, то
(AB)-1 = B-1A-1,

(ABC)-1 = C-1B-1A-1

и т. д.,

а также
(AT)-1 = (A-1)T.
Обратную мат­ри­цу можно записать как

np.linalg.det(A)

Приложение B · Линейная алгебра 983
Как ни странно,
ее иногда
тоже называют
сопряженной матри­
цей, но в этой книге
данный термин
используется
для обозначения
матрицы,
представленной
в разделе 3.1.3.

где adj(A) – транспонированная мат­ри­ца кофакторов, называемая присоединенной, или дополняющей, мат­ри­цей и иногда
обозначаемая как A∗. Если A – невырожденная, то сопряжение можно вычислить по формуле

skew(v)

(B.1)

adj(A) = det(A)A-1.
Для квадратной мат­ри­цы, если:
А = AT – мат­ри­ца симметрична. Обратная мат­ри­ца также
симметрична. Многие мат­ри­цы, с которыми мы сталкиваемся в робототехнике, симметричны, например ковариационные мат­ри­цы и мат­ри­цы инерции манипулятора;
„„ A-1 = AT – мат­ри­ца ортогональна. Матрица также называется ортонормированной, поскольку ее векторы-столбцы (и векторы-строки) имеют единичную длину и ортогональны друг другу. Произведение двух ортогональных
матриц одинакового размера также является ортогональной мат­ри­цей. Набор ортогональных матриц размера n×n
образует ортогональную группу O(n). Определитель ортогональной мат­ри­цы равен либо +1, либо -1. Подгруппа,
состоящая из ортогональных матриц с определителем +1,
называется специальной ортогональной группой и обозначается как SO(n);
„„ A = −AT – мат­ри­ца кососимметрична или антисиммет­
рична. Такая мат­ри­ца имеет нулевую диагональ, всегда
вырождена, если n – нечетное число. Любую мат­ри­цу можно выразить как сумму симметричной и кососимметричной матриц.
Между вектором и кососимметричной мат­ри­цей су­
ществует связь, которую для случая v ∈ ℝ3 можно выразить
как
„„

Обратная операция:
υ = ∨×(A).

Если υ ∈ ℝn и A ∈ ℝn×n, то [Aυ]× = det(A)A-T[υ]×А-1.
Для трехмерного случая векторное произведение можно записать как произведение мат­ри­цы и вектора υ1 × υ2 =
[υ1]×υ2, и υT[υ]×υ = 0, ∀υ. Если R ∈ SO(3), то [Rυ] = R[υ]×RT;
„„ ATA = AAT – мат­ри­ца нормальная и может быть диагонализирована ортогональной мат­ри­цей U так, что UTAU – диа­
гональная мат­ри­ца. Все симметричные, кососимметрич-

984 Приложение B · Линейная алгебра

ные и ортогональные мат­ри­цы являются нормальными,
как и мат­ри­цы вида A = BTB = BBT, где B – произвольная
мат­ри­ца.
Квадратную мат­ри­цу A ∈ ℝn×n можно применить как линейное преобразование к вектору x ∈ ℝn:
x¢ = Ax,

что дает другой вектор, как правило, с изменением его длины
и направления. Например, в двухмерном пространстве если x –
множество всех точек, лежащих на окружности, то x¢ определяет
точки, лежащие на некотором эллипсе. Однако есть несколько
важных частных случаев. Если A ∈ SO(n), то преобразование
изометрично и длина вектора неизменна, ||x¢|| = ||x||.
Собственными векторами квадратной мат­ри­цы являются такие векторы x, что
Ax = λi x,

i = 0, ..., n - 1,

np.linalg.eig(A)

(B.2)

т. е. их направление не меняется при преобразовании мат­ри­
цей. Они просто масштабируются по λi, соответствующей собственному значению. Матрица A имеет n собственных значений (спектр мат­ри­цы), которые могут быть вещественными или
комплексными. У ортогональной мат­ри­цы собственные значения лежат на единичной окружности комплексной плоскости,
|λi| = 1, и все собственные векторы ортогональны друг другу.
Демонстрационная программа в пакете Toolbox
>>> %run -m eigdemo 1 2 3 4

показывает анимацию вращающегося двухмерного координатного вектора x и преобразованного вектора Ax. Аргументы –
элементы мат­ри­цы A, расположенные по строкам. Дважды за
оборот x и Ax оказываются параллельными – это собственные
векторы A.
Если A невырождена, то собственные значения A-1 являются
обратными собственным значениям A, а собственные векторы
A-1 параллельны собственным векторам A. Транспонированная
мат­ри­ца AT имеет такие же собственные значения, что и A, но
другие собственные векторы.
След мат­ри­цы – это сумма диагональных элементов
np.trace(A)

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

Приложение B · Линейная алгебра 985

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

относительно оси вращения, но из-за ограниченного диапазона cos-1 значения θ выше π невозможно правильно определить.
Определитель мат­ри­цы равен произведению собственных
значений

соответственно, мат­ри­ца с одним или несколькими нулевыми
собственными значениями будет вырожденной.
Все собственные значения вещественной симметричной
мат­ри­цы A вещественные, и мат­ри­цы классифицируют в соответствии со знаком ее собственных значений:
λ > 0, ∀i – положительно определенные, никогда не могут
быть вырожденными и записываются как A ≻ 0;
„„ λi ≥ 0, ∀i – положительно полуопределенные, могут быть
сингулярными и записываются как A ≽ 0;
„„ λi < 0, ∀i – отрицательно определенные, никогда не могут
быть вырожденными и записываются как A ≺ 0;
„„ в остальных случаях не определена.
„„ i

Диагональные элементы положительно определенной мат­
ри­цы положительны, ai,i > 0, а ее определитель положителен,
det(A) > 0. Матрица, обратная к положительно определенной
мат­ри­це, тоже положительно определена.
Мы часто будем встречать мат­ри­цы квадратной формы, для
которой произведение
s = xTAx

(B.3)

является скаляром. Если A положительно определена, то s > 0,
∀x ≠ 0. В случае когда A – диагональная, это можно записать как
– взвешенную сумму квадратов. Если A = 1, то s = (||υ||2)2. Если
A – симметричная, то

и результат также включает произведения или корреляции
между элементами x.

986 Приложение B · Линейная алгебра

Расстояние Махаланобиса – это взвешенное расстояние, или
норма

где P ∈ ℝn×n – ковариационная мат­ри­ца, которая понижает вес
элементов x там, где неопределенность высока.
Матрицы ATA и AAT всегда симметричны и положительно
полуопределены. Это означает, что любую симметричную мат­
ри­цу A можно записать как
A = LLT,
где L – разложение Холецкого мат­ри­цы A.
Другие матричные факторизации A включают квадратный
корень из мат­ри­цы

np.linalg.cholesky(A)
sp.linalg.sqrtm(A)

A = SS,
где S – квадратный корень A или A1/2. Матрица S положительно
определена (и симметрична), если A положительно определена,
и QR-разложение
A = QR,
где Q – ортогональная мат­ри­ца, а R – верхняя треугольная мат­
ри­ца.
Если T – любая невырожденная мат­ри­ца, то
A = TBT -1.
Это так называемое преобразование подобия, а A и B называются подобными, и можно показать, что собственные значения
не меняются в результате преобразования.
Матричная форма в уравнении B.2 записывается в виде
AX = XΛ,
где X ∈ ℝn×n – мат­ри­ца собственных векторов A, расположенных
по столбцам, а Λ – диагональная мат­ри­ца соответствующих собственных значений. Если X не вырождена, мы можем выполнить перестановку
A = XΛX-1,
которая является собственным значением или спектральным
разложением мат­ри­цы. Отсюда следует, что мат­ри­цу можно
диагонализировать преобразованием подобия:
Λ = X-1AX.

np.linalg.qr(A)

Приложение B · Линейная алгебра 987

Если A симметрична, то X ортогональна, и вместо этого мы
можем написать
A = XΛXT.

np.linalg.
matrix_rank(A)

(B.4)

Определитель квадратной мат­ри­цы A ∈ ℝn×n – это множитель,
на который преобразование изменяет объемы в n-мерном пространстве. Для случая двухмерного пространства представьте
себе форму, определяемую точками xi с замкнутой областью Λ.
Форма, образованная точками Axi, будет иметь замкнутую область Λ adet(A). Если A вырождена, точки Axi лежат в одной точке или вдоль прямой и имеют нулевую замкнутую площадь.
Аналогичным образом для случая трехмерного пространства
определитель представляет собой масштабный коэффициент,
применяемый к объему набора точек, отображенных посредством преобразования А.
Столбцы мат­ри­цы A = (c0, c1, ..., cn-1) можно рассматривать
как набор векторов, определяющих пространство столбцов.
Точно так же строки A можно рассматривать как набор векторов, определяющих пространство строк. Столбцовый ранг – это
число линейно независимых столбцов мат­ри­цы A. Аналогично
строковый ранг – это число линейно независимых строк мат­ри­
цы A. Столбцовый и строковый ранги всегда равны и называются просто рангом мат­ри­цы A. Ранг имеет верхнюю границу n.
Ранг – это размерность наибольшей невырожденной квадратной подмат­ри­цы, которую можно составить из A. Квадратная
мат­ри­ца, для которой rank(A) < n, называется мат­ри­цей не­
полного ранга. Неполнота ранга min(m, n) − rank(A) называется недействительностью A. Кроме того, rank(AB) ≤ min(rank(A),
rank(B)) и rank(A + B) ≤ rank(A) + rank(В). Если x – вектор-столбец, то мат­ри­ца xxT, внешнее произведение x, имеет ранг 1 для
всех x ≠ 0.

B.2.2

Неквадратные и вырожденные мат­ри­цы

Вырожденные квадратные мат­ри­цы можно рассматривать как
частный случай неквадратной мат­ри­цы. Для неквадратной мат­
ри­цы A ∈ ℝm×n ранг – это размерность наибольшей невырожденной квадратной подмат­ри­цы, которую можно образовать из A,
и имеет верхнюю границу min(m, n).
Неквадратную или вырожденную мат­ри­цу нельзя обратить,
но можно определить левую обобщенную обратную мат­ри­цу
(ее также называют просто псевдообратной, или псевдообратной Мура–Пенроуза):
A+A = In×n,

988 Приложение B · Линейная алгебра

где А+ = (АТА)-1АТ. Правая обобщенная обратная мат­ри­ца:
AA+ = Im×m,
где А+ = АТ(ААТ)−1. Уточнение «левая» или «правая» указывает,
с какой стороны от A находится псевдообратный элемент.
Если мат­ри­ца A не имеет полного ранга, то она имеет конечное нуль-пространство (null space), или ядро. Вектор x лежит
в нуль-пространстве мат­ри­цы, если
Ax = 0.

sp.linalg.null_space(A)

Точнее, это правое нуль-пространство. Вектор лежит в левом
нуль-пространстве, если
xTA = 0.
Левое нуль-пространство равно правому нуль-простран­
ству AT.
Нуль-пространство определяется набором ортогональных
базисных векторов, размерность которых равна нулю A. Любая
линейная комбинация этих базисных векторов нулевого пространства лежит в нуль-пространстве.
Для неквадратной мат­ри­цы A ∈ ℝm×n аналог уравнения B.2
имеет вид
Aυi = σi ui,

где ui ∈ ℝm и υi ∈ ℝn – право- и левовырожденные векторы мат­
ри­цы A, а σi – ее особые значения. Особые значения (singular
value) – это неотрицательные вещественные числа, которые
представляют собой квадратный корень из собственных значений AAT, а ui – соответствующие собственные векторы. υi – собственные векторы ATA.
Разложение по особым значениям (сингулярное разложение,
singular value decomposition, SVD) мат­ри­цы A равно
A = UΣVT,
где U ∈ ℝm×m и V ∈ ℝn×n – ортогональные мат­ри­цы, содержащие
в качестве столбцов соответствующие сингулярные векторы ui
и vi. Σ ∈ ℝm×n – диагональная мат­ри­ца особых значений σi в порядке убывания величины

np.linalg.svd(A)

Приложение B · Линейная алгебра 989

np.linalg.cond(A)

где r = rank(A) – ранг A и σi+1 ≤ σi. В случае когда r < n, диагональ будет иметь n – r нулевых элементов, как показано выше.
Столбцы V, соответствующие нулевым столбцам Σ, определяют
нуль-пространство мат­ри­цы A. Число обусловленности (коэффициент переноса) мат­ри­цы A равно max(σ)/min(σ), а высокое
значение означает, что мат­ри­ца близка к вырожденной, или
«плохо обусловленной».

Приложение

C

	Геометрия

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

C.1

Евклидова геометрия

C.1.1

Точки

Точка в n-мерном пространстве представлена n-кортежем –
упорядоченным набором n чисел (x0, x1, ..., xn-1), которые определяют координаты точки. Кортеж также можно интерпретировать как вектор от начала координат до точки (координатный
вектор). Точка в двухмерном пространстве записывается как
p = (x, y)T, а в трехмерном – как p = (x, y, z)T.

C.1.2

Прямые

C.1.2.1

Прямые в двухмерном пространстве

Прямая ℓ = (a, b, c)T определяется следующим уравнением:
ax + by + c = 0,

(С.1)

которое является обобщением уравнения прямой, изучаемого
в школе, y = mx + c, с помощью которого можно легко представить вертикальную прямую, задав b = 0. Здесь υ = (a, b) – вектор,
параллельный прямой, и υ = (−b, a) – вектор нормали к прямой.
Прямая, соединяющая две точки, p1 и p2, задается решением
уравнения

Line2.Join

Приложение C · Геометрия 991

l1.intersects(l2)

которое находится из правого нуль-пространства самого левого члена. Точка пересечения двух прямых, ℓ1 и ℓ2, определяется
уравнением

у которого нет решения, если прямые параллельны, – крайний
левый член вырожден.
Прямую можно также представить в полярной форме
x cos θ + y sin θ + ρ = 0,
где θ – угол между осью x и прямой, а ρ – нормальное расстояние
между прямой и началом координат, как показано на рис. 12.20.

C.1.2.2

Прямые в трехмерном пространстве
и координаты Плюккера

Мы можем определить прямую двумя точками, p и q, как показано на рис. C.1, что потребует в общей сложности шести параметров ℓ = (px, py, pz, qx, qy, qz). Однако, поскольку эти точки могут
быть выбраны произвольно, существует бесконечный набор
параметров, представляющих одну и ту же прямую, что затрудняет определение эквивалентности двух прямых.

Рис. С.1. Описание прямой в трех измерениях

Есть преимущества в представлении прямой следующим образом:
ℓ = (ω × q, p - q) = (υ, ω) ∈ ℝ6,

где ω – направление прямой, а υ – момент прямой – вектор
из начала координат в точку на прямой и нормаль к прямой.
Это координатный вектор Плюккера – шестимерная величина
с двумя ограничениями: координаты однородны и, следовательно, инвариантны к общему масштабному коэффициенту;

992 Приложение C · Геометрия

и υ · ω = 0. Таким образом, прямые имеют четыре степени свободы , а координаты Плюккера лежат на четырехмерном многообразии в шестимерном пространстве. Прямые с ω = 0 лежат
на бесконечности и называются идеальными прямыми .
Давайте сначала определим две точки
>>> P = [2, 3, 4]; Q = [3, 5, 7];

а затем создадим объект прямой Плюккера:
>>> L = Line3.Join(P, Q)
{ 1 -2 1; -1 -2 -3}

который отображает компоненты v и ω. К ним можно получить
доступ как к свойствам:
>>> L.v.T
array([
>>> L.w.T
array([

1,

-2,

1])

-1,

-2,

-3])

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

Прямую Плюккера также можно представить как кососиммет­
ричную мат­ри­цу:
>>> L.skew()
array([[
0,
[
-1,
[
-2,
[
1,

1,
0,
-1,
2,

2,
1,
0,
3,

-1],
-2],
-3],
0]])

Чтобы нарисовать эту прямую, сначала определим область
трехмерного пространства, а затем отобразим ее синим цветом:
>>> plotvol3([-5, 5]);
>>> L.plot("b");

Прямая – это множество всех точек

которое можно сгенерировать через скалярный параметр λ
>>> L.point([0, 1,
array([[ 0.5714,
[ 0.1429,
[ -0.2857,

2])
0.3042,
-0.3917,
-1.087,

0.03691],
-0.9262],
-1.889]])

где столбцы – это точки на прямой, соответствующие λ = 0, 1, 2.
Точка x находится ближе всего к прямой, когда

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

Приложение C · Геометрия 993

Для точки (1, 2, 3)T ближайшая точка на прямой и расстояние
до нее определяются как
>>> [x, d] = L.closest_to_point([1, 2, 3])
>>> x
array([ 1.571,
2.143,
2.714])
>>> d
0.6547

Прямая пересекает плоскость nTx + d = 0 в точке с коорди­
натой

Прямая пересекает плоскость xy в точке с координатами
>>> p, _ = L.intersect_plane([0, 0, 1, 0])
>>> p
array([ 0.6667, 0.3333,
0])

Две прямые могут быть идентичными, компланарными или
скрещенными. Идентичные прямые имеют линейно зависимые координаты Плюккера, т. е. ℓ1 = λℓ2, или ℓˆ1 = ℓˆ2. Компланарные прямые могут быть параллельны или пересекаться, а если
скрещенные, то могут пересекаться или не пересекаться. Если
для прямых ω1 × ω2 = 0, они параллельны, в противном случае
они скрещенные.
Минимальное расстояние между двумя прямыми равно
d = ω1 · υ2 + ω2 · υ1
и равно нулю, если они пересекаются.
Отступление C.1. Юлиус Плюккер
Плюккер (1801–1868) – немецкий математик и физик,
внесший вклад в изучение катодных лучей и аналитической геометрии. Родился в Эльберфельде, учился в Дюссельдорфе, Бонне, Гейдельберге и Берлине,
а в 1823 г. отправился в Париж, где проникся идеями
французской геометрии. В 1825 году вернулся в Боннский университет, стал профессором математики
в 1828 году и профессором физики в 1836-м. В 1858 го­
ду предположил, что линии спектра, открытые его
коллегой Генрихом Гейсслером (известным нам по
трубке Гейсслера), характерны для испускающего их
химического вещества. В 1865-м он вернулся к геометрии и изобрел так называемую линейную геомет­
рию. Был награжден медалью Копли от Королевского
общества в 1866 году. Похоронен на Альтер Фридхоф (Старое кладбище) в Бонне.

994 Приложение C · Геометрия

Оператор side представляет собой перестановочное скалярное произведение
side(ℓ 1, ℓ 2) = ℓ01ℓ42 + ℓ11ℓ52 + ℓ21ℓ32 + ℓ31ℓ22 + ℓ41ℓ02 + ℓ51ℓ12,
которое равно нулю, если прямые пересекаются или параллельны, и вычисляется методом side. Для объектов Line3, L1 и L2,
операторы L1 ^ L2 и L1 | L2 возвращают True, если прямые пересекаются или параллельны соответственно.
Мы можем преобразовать прямую Плюккера с помощью сопряженного движения твердого тела
ℓ¢ = Ad(BξA)Aℓ,

B

которое описывается формулой (D.2). Его можно вычислить,
умножив экземпляр Line3 на экземпляр SE3.

C.1.3

Плоскости

Плоскость определяется четырехмерным вектором π = (a, b, c,
d)T и представляет множество всех точек x = (x, y, z)T такое, что
ax + by + cz + d = 0.
Это уравнение можно записать в точечно-нормальной форме как
nT(x - p) = 0,
где n = (a, b, c) – нормаль к плоскости, а p ∈ ℝ3 – точка на плос­
кости.
Плоскость с нормалью n и содержащая точку с координатным
вектором p определяется как π = (nx, ny, nz, n · p)T. Также плос­кость
можно задать тремя точками, p1, p2 и p3, где pi = (xi, yi, zi)

Plane.PointNormal(p, n)

Plane.ThreePoints(p)

Это уравнение можно решить с использованием правого
нуль-пространства самого левого члена, или через две непараллельные прямые, ℓ1 и ℓ2:
π = (ω1 × ω2, υ1 · ω2),

Plane.TwoLines(l1, l2)

или через прямую Плюккера (υ, w) и точку с координатным вектором p:
π = (ω × p - υ, υ · p).

Plane.LinePoint(l, p)

Приложение C · Геометрия 995

Точку также можно определить как точку пересечения трех
плоскостей π1, π2 и π3:

Plane.intersection()

Если левая мат­ри­ца вырождена, то две или более плоскости
параллельны и имеют либо ноль, либо бесконечно много точек
пересечения.
Прямая Плюккера, образованная пересечением двух плоскостей, π1 и π2:
ℓ = (n1 × n2, d2n1 - d1n2).

C.1.4

Эллипсы и эллипсоиды

Эллипс принадлежит к семейству плоских кривых, называемых
кониками (conic). Простейшая форма эллипса с центром в (0, 0)
определяется неявно точками (x, y) такими, что
,
Окружность
является частным
случаем эллипса,
где a = b = r
и r – радиус.

и показана на рис. C.2a. ► Этот канонический эллипс имеет
центр в начале координат, а его большая и малая оси параллельны осям x и y. Радиус в направлении x равен a, а в направлении y равен b. Более длинный из двух радиусов называется
длиной большой полуоси, а короткий – длиной малой полуоси.
Мы можем записать эллипс в матричной квадратичной форме (уравнение B.3) как

малая ось
большая ось
длина большой полуоси
длина
малой полуоси
длина большой полуоси

ориентация
центр

длина
малой полуоси

а
б
Рис. С.2. Эллипсы: а) канонический эллипс с центром в начале координат, ориентированный по осям x и y;
б) обобщенная форма эллипса

996 Приложение C · Геометрия

или более компактно
xTEx = 1,

(C.2)

где x = (x, y)T и E – симметричная мат­ри­ца
(C.3)
определитель которой det(E) = αβ − γ2 определяет тип коники:

Следовательно, эллипс представлен положительно определенной симметричной мат­ри­цей E. И наоборот, любая положительно определенная симметричная мат­ри­ца 2×2, такая как
инвертированный тензор инерции или мат­ри­ца ковариации,
может быть представлена эллипсом.
Ненулевые значения γ изменяют ориентацию эллипса. Центр
эллипса можно разместить в произвольной точке xc = (xc, yc) в соответствии с уравнением
(x - xc)TE(x - xc) = 1,
что приводит нас к обобщенному эллипсу, показанному на
рис. C.2b.
Поскольку мат­ри­ца E симметрична, ее можно диагонализировать по уравнению (B.4)
E = XΛXT,
где X – ортогональная мат­ри­ца, состоящая из собственных векторов E, а диагональные значения Λ являются собственными
значениями E. Квадратичная форма (C.2) принимает вид
xTXΛXTx = 1,
(XTx)TΛ(XTx) = 1,
x¢TΛx¢ = 1.
Это похоже на уравнение (C.2), но с эллипсом, определяемым
диагональной мат­ри­цей Λ относительно повернутой системы
координат x¢ = XTx. Большая и малая оси эллипса ориентированы вдоль собственных векторов E.
Радиусы эллипса связаны с обратным квадратным корнем
собственных значений соотношением

Приложение C · Геометрия 997

(С.4)
то есть большой и малый радиусы эллипса, a и b, определяются
наименьшим и наибольшим собственными значениями соответственно. Площадь эллипса равна
(С.5)
так как det(E) = ∏ λi. Эксцентриситет равен

Альтернативно эллипс можно представить в полиномиальной форме, записав как
,
и расширяется до
e0x2 + e1y2 + e2xy + e3x + e4y + e5 = 0,
где e0 = α, e1 = β, e2 = 2γ, e3 = -2(αxc + γyc), e4 = -2(βyc + γxc) и e5 =
αxc2 + βyc2 + 2γxc yc - 1. Эллипс имеет только пять степеней свободы,
свою центральную координату и три уникальных элемента в E.
Для невырожденного эллипса, где e1 ≠ 0, мы можем переписать
многочлен в нормализованной форме
x2 + ϵ0y2 + ϵ1xy + ϵ2x + ϵ3y + ϵ4 = 0

(C.6)

с пятью уникальными параметрами ϵi = ei+1 /e0, i = 0, ..., 4.
Рассмотрим эллипс

который представлен в Python как
>>> E = np.array([[1, 1], [1, 2]])
array([[1, 1],
[1, 2]])

Мы можем построить его вызовом
>>> plot_ellipse(E)

и получить результат, как показано на рис. C.3. Собственные
векторы и собственные значения E равны соответственно

998 Приложение C · Геометрия
>>> e, v
>>> e
array([
>>> v
array([[
[

= np.linalg.eig(E)
0.382,

2.618])

-0.8507, -0.5257],
0.5257, -0.8507]])
единичная окружность
эллипс
малая ось
большая ось

Рис. С.3. Эллипс, соответствующий симметричной мат­ри­це 2×2. Стрелки указывают большую
и малую оси эллипса

Радиусы эллипса равны
>>> r = 1 / np.sqrt(e)
array([ 1.618,
0.618])

Если любой из радиусов равен нулю, эллипс вырождается
и становится прямой. Если оба радиуса равны нулю, эллипс вырождается в точку.
Собственные векторы представляют собой единичные векторы в направлениях малой и большой осей, ► и мы масштабируем их по радиусам, чтобы получить радиус-векторы, которые
можно построить:
>>> plot_arrow((0, 0), v[:,0]*r[0], color="r", width=0.02);
>>> plot_arrow((0, 0), v[:,1]*r[1], color="b", width=0.02);

Ориентация эллипса представляет собой угол между большой осью и горизонтальной осью системы

где υ = (υx, υy) – собственный вектор, соответствующий наименьшему собственному значению (наибольшему радиусу). Для нашего примера это

Обратите внимание,
что np.linalg.eig
не сортирует
собственные
значения.

Приложение C · Геометрия 999
>>> np.rad2deg(np.arctan2(v[1, 0], v[0, 0]))
148.3

градусов против часовой стрелки от оси x.
В Toolbox имеется функция plot_ellipsoid, которая нарисует
эллипс для мат­ри­цы E 3×3.

C.1.4.1

Рисование эллипса

Чтобы нарисовать эллипс, сначала определим координату точки y = (x, y)T на единичной окружности
y Ty = 1

(C.7)

и перепишем уравнение (С.2) как
xT E1/2 E1/2x = 1,

(C.8)

где E1/2 – квадратный корень мат­ри­цы. Приравняем (C.7) и (C.8)
xT E1/2 E1/2x = yTy,
и получаем
y = E1/2x.
Теперь произведем перестановку
x = E–1/2y,
которая переводит точку на единичной окружности в точку на
эллипсе, а E–1/2 – это обратная мат­ри­ца квадратного корня. Если
центр эллипса находится в точке xc, а не в начале координат, мы
можем выполнить замену координат
(x – xc)T E1/2 E1/2(x – xc) = 1,
откуда получаем преобразование в виде
x = E–1/2 y + xc.
Рисование эллипсоида решается аналогичным образом.
Продолжим пример, начатый выше:
>>> E = np.array([[1, 1], [1, 2]]);

Определим множество точек на единичной окружности
>>> th = np.linspace(0, 2*pi, 50);
>>> y = np.vstack([np.cos(th), np.sin(th)]);

1000 Приложение C · Геометрия

которые мы преобразуем в точки на периметре эллипса:
>>> x = linalg.sqrtm(np.linalg.inv(E)) @ y;
>>> plt.plot(x[0, :], x[1, :], '.');

инкапсулированного в функции Toolbox:
>>> plot_ellipse(E, centre=[0, 0])

Во многих случаях бывает необходимо нарисовать эллипс,
заданный A-1. Вычисление мат­ри­цы, обратной мат­ри­це A, было
бы неэффективно, потому что plot_ellipse снова ее обращает,
поэтому мы пишем plot_ellipse(A, inverted=True).

C.1.4.2

Подгонка эллипса к данным

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

По набору внутренних точек
Обычный подход состоит в том, чтобы найти эллипс, обладающий теми же свойствами массы, что и набор точек. По набору
N точек xi = (xi, yi)T можно вычислить моменты

Центр эллипса совпадает с центроидом множества точек

что позволяет вычислить центральные вторые моменты:

Тензор инерции обобщенного эллипса представляет собой
симметричную мат­ри­цу

Приложение C · Геометрия 1001

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

Тензор инерции и эквивалентный ему эллипс связаны соотношением

Чтобы продемонстрировать это, создадим набор точек, которые лежат внутри эллипса, используемого в примере выше:
>>>
>>>
>>>
>>>
...
...
...
>>>
>>>
>>>
>>>
>>>
>>>
>>>
>>>
>>>
>>>
>>>
>>>
>>>
>>>
>>>
>>>
>>>
>>>

rng = np.random.default_rng(0);
# сгенерируем 200 случайных точек внутри эллипса
x = [];
while len(x) < 200:
p = rng.uniform(low=-2, high=2, size=(2,1))
if np.linalg.norm(p.T @ E @ p) <= 1:
x.append(p)
x = np.hstack(x); # создадим массив 2x50
plt.plot(x[0, :], x[1, :], "k."); # отобразим точки
# вычислим моменты
m00 = mpq_point(x, 0, 0);
m10 = mpq_point(x, 1, 0);
m01 = mpq_point(x, 0, 1);
xc = np.c_[m10, m01] / m00;
# вычислим вторые моменты относительно центроида
x0 = x - xc.T;
u20 = mpq_point(x0, 2, 0);
u02 = mpq_point(x0, 0, 2);
u11 = mpq_point(x0, 1, 1);
# вычислим тензор инерции и мат­ри­цу эллипса
J = np.array([[u20, u11], [u11, u02]]);
E_est = m00 / 4 * np.linalg.inv(J);

Выполнение этого кода дает следующий результат:
>>> E_est
array([[ 1.053,
[ 1.038,

1.038],
1.98]])

близкий к исходному значению E. Точки данных показаны на
рис. C.4. Мы можем наложить полученный эллипс на эти данные:
>>> plot_ellipse(E_est, "r")

1002 Приложение C · Геометрия

Результат показан красным на рис. C.4.
точки данных
описывающий эллипс

Рис. С.4. 200 случайных точек и описывающий эллипс

По набору граничных точек
В этой задаче нужно найти уравнение эллипса по набору точек
(xi, yi), определяющих границу эллипса. Применяя полиномиальную форму уравнения эллипса (C.6) к каждой точке, запишем задачу в матричной форме:

откуда для N ≥ 5 можно найти вектор параметров эллипса.

C.2

Однородные координаты

Точку в однородных координатах, или в проективном пространстве ℙn, представляет координатный вектор x̃ = (x̃ 0, x̃ 1, ...,
x̃ n). Евклидовы координаты связаны с проективными координатами соотношением

Приложение C · Геометрия 1003

И наоборот, вектор однородных координат можно сконструи­
ровать из вектора евклидовых координат:
x̃ = (x0, x1, ..., xn-1, 1).

то

р

пр

ое

кт

ив

ны

йл

уч

Дополнительная степень свободы, предлагаемая проективными координатами, имеет несколько преимуществ. Она позволяет представлять точки и прямые на бесконечности – так
называемые идеальные точки и прямые – с использованием
только конечных чисел. Это также означает, что масштаб не
имеет значения, т. е. x̃ и x̃¢ = λx̃ оба представляют одну и ту же
евклидову точку для всех λ ≠ 0. Это свойство записывают как
x̃ ≃ x̃¢. Также можно применить преобразование жесткого тела
к точкам x̃ ∈ ℙn, умножив однородную координату на мат­ри­цу
однородного преобразования n×n.

пр

евклидова плоскость

ое

кт

ив

ны

йв

ек

1

проективное пространство

Рис. С.5. Точка P на евклидовой плоскости описывается координатным вектором p ∈ ℝ2,
который эквивалентен трехмерному вектору в проективном пространстве p̃ ∈ ℙ2

Однородные векторы важны в компьютерном зрении, когда
мы рассматриваем точки и прямые, существующие на плос­
кости изображения камеры. Мы также можем считать, что
однородная форма представляет собой луч в евклидовом пространстве, как показано на рис. C.5. Отношение между точками
и лучами лежит в основе проективного преобразования. Вы можете поэкспериментировать с этим преобразованием, запустив
блокнот examples/homogeneous-coords.ipynb.

C.2.1

Два измерения

C.2.1.1

Точки и прямые

В двух измерениях существует двойственность между точками
и прямыми. В ℙ2 прямая задается тройкой ℓ˜ = (ℓ0, ℓ1, ℓ2)T, где не
все нули, и уравнение прямой есть множество всех точек x̃ ∈ ℙ2

1004 Приложение C · Геометрия

ℓ˜T x̃ = 0,
которое расширяется до ℓ0 x + ℓ1y + ℓ2 = 0 и может быть преоб.
разовано в более знакомое представление прямой
Обратите внимание, что это уравнение может описывать вертикальную прямую, параллельную оси y, чего не может более
привычное уравнение y = mx + c. Неоднородный вектор (ℓ0, ℓ1)T
перпендикулярен прямой, ► а (-ℓ1, ℓ0) – параллелен ей.
Точка определяется пересечением двух прямых. Если записать точечные уравнения для двух прямых ℓ˜1Tx̃ = 0 и ℓ˜2Tx̃ = 0, то
их пересечением будет точка с однородными координатами
p̃ = ℓ˜1 × ℓ˜2.
Это так называемое линейное уравнение точки. Точно так же
прямая, соединяющая две точки, p̃1 и p̃2 ∈ ℙ2, задается векторным
произведением:
ℓ˜12 = p̃1 × p̃2.

Рассмотрим случай двух параллельных прямых под углом 45°
к горизонтальной оси
>>> l1 = [1, -1, 0];
>>> l2 = [1, -1, -1];

которые мы можем построить:
>>> plot_homline(l1, "b");
>>> plot_homline(l2, "r");

Точка пересечения этих параллельных прямых имеет следующие координаты:
>>> np.cross(l1, l2)
array([1, 1, 0])

Это идеальная точка, поскольку третья координата равна
нулю, – эквивалентная евклидова точка была бы на бесконечности. Проективные координаты позволяют просто представлять точки и прямые на бесконечности и манипулировать ими
без специальной логики.
Расстояние от точки с координатами p̃ до прямой ℓ˜ равно
(C.9)

Поэтому такое
представление
прямой называется
нормальной
формой.

Приложение C · Геометрия 1005

C.2.1.2

Коники

Конические сечения – это важное семейство плоских кривых,
включающее окружности, эллипсы, параболы и гиперболы, которые можно описать как множество точек x̃ ∈ ℙ2 таких, что
x̃ TΩx̃ = 0,

где Ω – симметричная мат­ри­ца
(C.10)
Определитель верхней левой подмат­ри­цы указывает тип коники: отрицательный для гиперболы, нулевой для параболы
и положительный для эллипса.

C.2.2

Три измерения

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

C.2.2.1

Прямые

Используя однородное представление двух точек p̃ и q̃ ∈ ℙ3, мы
можем сформировать кососимметричную мат­ри­цу 4×4

(C.11)

чьи шесть уникальных элементов составляют вектор координат
Плюккера. Эта мат­ри­ца имеет ранг 2, а определитель является
квадратичным в координатах Плюккера – четырехмерной квад­
ратичной гиперповерхностью, известной как квадрика Клейна.
Все точки, лежащие на этом многообразии, являются допус­
тимыми прямыми. Многие отношения в разделе C.1.2.2 (между
прямыми, точками и плоскостями) можно выразить с помощью
этой мат­ри­цы, возвращаемой методом skew класса Line3.

1006 Приложение C · Геометрия

Для перспективной камеры с мат­ри­цей камеры C ∈ ℝ3×4 трехмерная прямая Плюккера, представленная кососимметричной
мат­ри­цей L 4×4, проецируется на плоскость изображения как
ℓ˜ = CLCT ∈ ℙ2,

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

C.2.2.2

Плоскости

Плоскость π̃ ∈ ℙ3 описывается множеством точек x̃ ∈ ℙ3 таким,
что π̃ Tx̃ = 0, и может быть задана прямой L ∈ ℝ4×4 и однородной
точкой p̃
π̃ = Lp̃

Plane.LinePoint()

или тремя точками

Plane.ThreePoints()

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

Plane.intersection()

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

C.2.2.3

Квадрики

Квадрики (quadric), сокращение от «квадратичная поверхность», – богатое семейство трехмерных поверхностей. Су­
ществует 17 стандартных типов, включая сферы, эллипсоиды,
гиперболоиды, параболоиды, цилиндры и конусы, описываемые множеством точек x̃ ∈ ℙ3, таким что
x̃ TQx̃ = 0,

где Q ∈ ℝ4×4 симметрична.
Для перспективной камеры с мат­ри­цей камеры C ∈ ℝ3×4
контур квадрики проецируется на плоскость изображения по
формуле

Приложение C · Геометрия 1007

Ω∗ = CQ∗CT,

где Ω задается уравнением (C.10), а (·)∗ представляет операцию
сопряжения, см. раздел B.1.

C.3

Геометрические преобразования

Линейное преобразование – это
y = Ax,

(C.12)

а аффинное преобразование описывается уравнением
y = Ax + b,

Масштабирование
относительно произвольной точки.

(С.13)

которое включает линейное преобразование и изменение начала координат. Всякое линейное преобразование аффинно,
но не всякое аффинное преобразование линейно. К примерам
аффинных преобразований относятся перенос, масштабирование, гомотетия, преобразование подобия, отражение, вращение, сдвиговое отображение и их комбинации в любой комбинации и последовательности.
В однородных координатах (C.13) можно записать как
ỹ = Hx̃, где
и преобразование действует в точке с однородными координатами x̃.
Проективное пространство является обобщением аффинного, а аффинное – обобщением евклидова. Аффинное пространство не имеет выделенной точки, служащей началом координат. Следовательно, ни один вектор не имеет фиксированного
начала координат, и ни один вектор не может быть однозначно
связан с точкой аффинного пространства, вместо этого сущест­
вуют векторы смещения между двумя точками пространства.
Вычитание двух точек дает вектор смещения, а прибавление
вектора смещения к точке дает новую точку. Если вектор смещения определить как разность между двумя однородными
точками p̃ и q̃, то разность p̃ - q̃ представляет собой четырехмерный вектор, последний элемент которого равен нулю, что
отличает точку от вектора смещения.
В двух измерениях наиболее общим преобразованием является проективное преобразование, также известное как коллинеация (collineation)

1008 Приложение C · Геометрия

которое уникально до масштаба, и один элемент был нормализован к единице. Коллинеация имеет восемь степеней свободы.
Аффинное преобразование – это подмножество, в котором
элементы последней строки фиксированы:

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

где R ∈ SO(2), что дает всего четыре степени свободы, а s < 0
вызывает отражение. Преобразования подобия без отражения
иногда называют прокрустовым преобразованием.
Наконец, евклидово преобразование, или преобразование
твердого тела
,
является наиболее ограничительным и имеет только три степени свободы. Некоторые графические примеры воздействия
различных преобразований на квадрат показаны на рис. C.6.
Возможные геометрические преобразования для каждого
типа преобразования приведены в табл. С.1 вместе с геометрическими свойствами, которые остаются неизменными или инвариантными при таком преобразовании. Мы видим, что хотя
евклидово преобразование является наиболее строгим, оно
способно сохранять такие важные свойства, как длина и угол.

Приложение C · Геометрия 1009

евклидово
подобия
аффинное
проективное

Рис. С.6. Двухмерный квадрат (темно-серый) подвергается различным преобразованиям
от самых ограниченных (евклидовых) до самых общих (проективных)

Таблица С.1. Для различных семейств планарных преобразований
перечислены возможные геометрические преобразования и сохраняемые
геометрические свойства
Евклидово Подобия
Геометрическое преобразование
Вращение
ü
ü
Перенос
ü
ü
Отражение
ü
Равномерное масштабирование
ü
Неравномерное масштабирование
Наклон
Перспективная проекция
Евклидово Подобия
Сохранение геометрических свойств (инварианты)
Длина
ü
Угол
ü
ü
Отношение длин
ü
ü
Параллельность
ü
ü
Пересечение
ü
ü
Негармоническое отношение
ü
ü

Аффинное Проективное
ü
ü
ü
ü
ü
ü

ü
ü
ü
ü
ü
ü
ü
Аффинное Проективное

ü
ü
ü

ü
ü

Приложение

D

	Группы и алгебра Ли

Мы не можем достаточно далеко продвинуться в изучении вращений или движения твердого тела, не встретив таких терминов, как «группы Ли», «алгебра Ли» или «скобки Ли», – все они
названы в честь норвежского математика Софуса Ли. Вращения
и движения твердого тела в двух и трех измерениях могут быть
представлены мат­ри­ца­ми, которые образуют группы Ли и подчиняются алгебре Ли.
Мы начнем просто с рассмотрения множества всех вещест­
венных матриц 2×2 A ∈ ℝ2×2

Эту мат­ри­цу можно записать как линейную комбинацию базисных матриц

где каждая базисная мат­ри­ца представляет направление в четырехмерном пространстве матриц 2×2. То есть четыре оси этого пространства параллельны каждой из этих базисных матриц.
Любую мат­ри­цу 2×2 можно представить точкой в этом пространстве – эта конкретная мат­ри­ца является точкой с координатами (a0,0, a0,1, a1,0, a1,1).
Все собственно ортогональные мат­ри­цы вращения, принадлежащие SO(2), являются подмножеством точек в пространстве
всех матриц 2×2. В этом примере точки лежат в одномерном
подмножестве, замкнутой кривой, в четырехмерном пространстве. Это пример многообразия – гладкой поверхности более
низкого измерения, встроенной в пространство.
Понятие кривой в четырехмерном пространстве имеет
смысл, если учесть, что мат­ри­ца вращения SO(2)

Приложение D · Группы и алгебра Ли 1011

имеет только один свободный параметр, и изменение этого параметра перемещает точку вдоль многообразия.
С формальной математической точки зрения можно сказать,
что вращения SO(2) и SO(3) и движения твердого тела SE(2)
и SE(3) являются матричными группами Ли, и это имеет два
следствия. Во-первых, это алгебраическая группа – математическая структура, состоящая из элементов и одного оператора.
Проще говоря, группа G обладает следующими свойствами.
В обозначениях,
принятых в этой
книге, оператор ⊕
является оператором группы.
В обозначениях,
принятых в этой
книге, тождество
обозначается
пустым множеством
(подразумевая
нулевое движение),
поэтому мы можем
сказать, что
ξ ⊕ Ø = Ø ⊕ ξ = ξ.

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

1. Если g1 и g2 – элементы группы, т. е. g1, g2 ∈ G, то результат
группового оператора ◇ также является элементом группы:
g1 ◇ g2 ∈ G. В общем случае группы некоммутативны, поэтому g1 ◇ g2 ≠ g2 ◇ g1. Для вращений и движений твердого тела
групповой оператор ◇ представляет композицию.
2. Групповой оператор ассоциативен, т. е. (g1 ◇ g2) ◇ g3 = g1 ◇
(g2 ◇ g3).
3. Для g ∈ G существует тождественный элемент I ∈ G, такой
что g ◇ I = I ◇ g = g.
4. Для каждого g ∈ G существует единственный обратный
h ∈ G такой, что g ◇ h = h ◇ g = I.

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

Отступление D.1. Софус Ли
Ли (1842–1899) – норвежский математик, получивший докторскую степень в Университете
Христиании в Осло в 1871 г. Он провел некоторое
время в Берлине, работая с Феликсом Кляйном,
а позже участвовал в программе Кляйна в Эрлангане по описанию геометрий, основанных на
тео­рии групп и проективной геометрии. Во время визита в Милан в годы Франко-прусской войны он был арестован как немецкий шпион и провел один месяц в тюрьме. Он наиболее известен
своим открытием того, что группы непрерывных
преобразований (теперь называемые группами
Ли) могут быть поняты путем их линеаризации
и изучения их порождающих векторных пространств. Он похоронен в гробнице Вар Фрелзерс
в Осло. (Фото Людвика Шачински.)

1012 Приложение D · Группы и алгебра Ли

Касательное пространство описывается алгеброй групп Ли,
а базисные направления касательного пространства называются образующими группы. Точки в этом касательном пространстве сопоставляются с элементами группы через степенную
функцию. Если g – алгебра Ли группы G, то
eX ∈ G,

∀X ∈ g,

где элементы g и G – мат­ри­цы одинакового размера, каждая из
которых имеет определенную структуру.
Поверхность сферы представляет собой многообразие в трех­
мерном пространстве, и в любой точке этой поверхности
можно создать касательный вектор. На самом деле их можно
создать бесконечное количество, и они лежат в плоскости, которая представляет собой двухмерное векторное пространст­
во – касательное пространство. Мы можем выбрать множество
основных направлений и установить двухмерную систему координат, а также сопоставить точки на плоскости с точками на
поверхности сферы.
Теперь рассмотрим произвольную вещественную мат­ри­цу
3×3 A ∈ ℝ3×3

которую можно записать как линейную комбинацию базисных
матриц:

где каждая базисная мат­ри­ца представляет направление в девятимерном пространстве матриц 3×3. Каждая возможная мат­ри­
ца 3×3 представлена точкой в этом пространстве.
Не все мат­ри­цы в этом пространстве являются собственно ортогональными мат­ри­ца­ми вращения, принадлежащими
SO(3), а только те, которые лежат на многообразии, поскольку
SO(3) является группой Ли. Нулевое вращение, представленное
единичной мат­ри­цей, является одной точкой в этом пространстве. В этой точке можно построить касательное пространство,
которое имеет только три измерения. Каждая точка касательного пространства – производные многообразия – может быть
выражена как линейная комбинация базисных матриц

Приложение D · Группы и алгебра Ли 1013

(D.1)

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

которая является алгеброй Ли группы SO(3). Базисы этого пространства G1, G2 и G3 называются образующими SO(3) и принадлежат so(3).
Уравнение D.1 можно записать в виде кососимметричной
мат­ри­цы, параметризованной вектором ω = (ω1, ω2, ω3) ∈ ℝ3

и известной как вектор Эйлера, или экспоненциальные координаты. Она отражает три степени свободы группы SO(3), вложенной в пространство всех матриц 3×3. Три степени свободы
согласуются с нашим интуитивным представлением о вращении в трехмерном пространстве, а также с теоремой Эйлера
о вращении.
Часто требуется сопоставление между векторами и кососимметричными мат­ри­ца­ми, поэтому далее будем использовать
следующее сокращенное обозначение:
[·]×: ℝ ↦ so(2), ℝ3 ↦ so(3),

⋁×(·): so(2) ↦ ℝ, so(3) ↦ ℝ3.

Первое сопоставление выполняется с помощью функции skew
из пакета Toolbox, а второе – с помощью vex (названа так в честь
оператора ⋁×).
Экспонента любой мат­ри­цы в so(3) является действительным
членом SO(3)
R(θ, ω̂) = e[θω̂]× ∈ SO(3),

а решение в аналитической форме вытекает из формулы вращения Родригеса
R(θ, ω̂) = 1 + sin θ[ω̂]× + (1 - cos θ)[ω̂]×2.
Наконец, рассмотрим произвольную вещественную мат­ри­цу
4×4 A ∈ ℝ4×4

1014 Приложение D · Группы и алгебра Ли

которую можно записать как линейную комбинацию базисных
матриц

где каждая базисная мат­ри­ца представляет направление
в 16-мер­ном пространстве всех возможных матриц 4×4. Каждая
мат­ри­ца 4×4 представлена точкой в этом пространстве.
Не все мат­ри­цы в этом пространстве являются собственно
ортогональными мат­ри­ца­ми однородного преобразования,
принадлежащими SE(3), а только те, которые лежат на гладком
многообразии. Нулевое движение (нулевое вращение и пере­
мещение), представленное тождественной мат­ри­цей, представляет собой одну точку в этом пространстве. В этой точке
можно построить касательное пространство, которое в данном
случае имеет шесть измерений, а точки в касательном пространстве могут быть выражены как линейная комбинация базисных матриц

и эти порождающие мат­ри­цы принадлежат алгебре Ли группы
SE(3) и обозначаются se(3). Их можно записать в общем виде
расширенной кососимметричной мат­ри­цы

параметризованной S = (υ, ω) ∈ ℝ6, которая называется кручением (twist) и имеет физическую интерпретацию через понятия направления и положения оси винта. Структура разреженной мат­ри­цы и эта краткая параметризация отражают шесть

Приложение D · Группы и алгебра Ли 1015

степеней свободы группы SE(3), встроенной в пространство
всех матриц 4×4. Расширим нашу предыдущую сокращенную
запись:
[·]: ℝ3 ↦ se(2), ℝ6 ↦ se(3),

⋁(·): se(2) ↦ ℝ3, se(3) ↦ ℝ6.

Мы можем использовать эти операторы для преобразования
между представлением кручения, которое является шестимерным вектором, и представлением алгебры Ли, являющимся дополненной кососимметричной мат­ри­цей 4×4. Алгебру Ли можно конвертировать в представление группы Ли по формуле
T(θSˆ) = e[θŜ] ∈ SE(3)

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

Преобразование кручения – сопряженное представление
В разделе 2.4.7 было показано, что движения твердого тела
могут быть описаны кручением, которое представляет движение через направление и положение оси винта. Например, на
рис. D.1 кручение AS может использоваться для преобразования
точек на теле. Если винт жестко прикреплен к телу, которое совершает какое-либо движение в AξB, то новое кручение равно
S = Ad(BξA)AS,

B

где
(D.2)

является сопряженным представлением движения твердого
тела. В качестве альтернативы можно написать
Ad(e[S]) = ead(S),
где ad(S) – логарифм сопряжения, определяемый через парамет­
ры кручения как

Отношения между упомянутыми математическими объектами показаны на рис. D.2.

1016 Приложение D · Группы и алгебра Ли

Рис. D.1. Точки тела (серое облако) можно трансформировать с помощью кручения AS. Если
тело и ось винта претерпевают преобразование твердого тела AξB, то новое кручение равно BS

SE3.Ad
сопряженные

logm
Twist()
expm

Twist.T

Twist.ad

SE3.log

SE (3)
SE3.exp

логарифмически
сопряженная

vexa,Twist()
skewa,Twist.se
векторы 6×1:
– кручение
– скорость кручения
– момент движения
– координаты Плюкера

матрицы 6×6:
– сопряженные
– логарифмически сопряженная

se (3)
матрицы 4×4:
– SE(3), однородные
– se(3)

однородные
векторы 4×1:
– точки

Рис. D.2. Обзор связанных с SE(3) величин. Значения мат­ри­цы обозначены следующим образом: 0 (черный),
1 (белый), другие (серый). Преобразования между типами обозначены синими стрелками с названием
соответствующего класса и метода. Операции обозначены красными стрелками: объект в хвосте стрелки
воздействует на объект в острие стрелки и приводит к появлению другого объекта того же типа

Приложение

E

	Линеаризация, якобианы
и гессианы
В робототехнике и компьютерном зрении многие уравнения
являются нелинейными. Чтобы применить знакомые мощные
аналитические методы, приходится работать с линейными или
квадратичными приближениями к этим уравнениям. Принцип
проиллюстрирован на рис. Е.1 для одномерного случая, а аналитические приближения, показанные красным, сделаны при
х = х0. Аппроксимация равна нелинейной функции в точке x0,
но становится все менее точной по мере удаления от нее. Этот
подход называется локальной аппроксимацией, поскольку он
действителен в области, локальной для x0, а размер допустимой
области зависит от величины нелинейности. Его можно распространить на произвольное количество измерений.

E.1

Скалярная функция скаляра

Функция f: ℝ ↦ ℝ может быть представлена в виде ряда Тей­лора:

а

б

Рис. E.1. Нелинейная функция f(x) аппроксимируется в точке x = x0: а) прямой –
аппроксимация первого порядка или линейная; б) параболой – аппроксимация второго
порядка. В точке линеаризации обе кривые равны и касаются функции, а для (б) совпадают
вторые производные

1018 Приложение E · Линеаризация, якобианы и гессианы
Отступление E.1. Людвиг Отто Гессе
Гессе (1811–1874) – немецкий математик, родился
в Кенигсберге, Пруссия, учился у Якоби и Бесселя
в Кенигсбергском университете. Преподавал в Кенигсберге, Галле, Гейдельберге и, наконец, во вновь
созданной Политехнической школе в Мюнхене.
В 1869 г. поступил в Баварскую академию наук.
Скончался в Мюнхене, но похоронен в Гейдельберге – в городе, который он считал своим вторым
домом.

который мы усекаем, чтобы сформировать первое или линейное приближение
f ¢(Δ) ≈ f(x0) + J(x0)Δ,
или приближение второго порядка
f ¢(Δ) ≈ f(x0) + J(x0)Δ + ½H(x0)Δ2,
где Δ ∈ ℝ – бесконечно малое изменение x относительно точки
линеаризации x0, а первая и вторая производные равны

соответственно.

E.2

Скалярная функция вектора

Скалярное поле f(x): ℝn ↦ ℝ можно представить в виде ряда Тейлора:
f(x0 + Δ) = f(x0) + J(x0)Δ + ½ΔTH(x0)Δ + ...,

который можно усечь, чтобы сформировать первое или линейное приближение
f ¢(Δ) ≈ f(x0) + J(x0)Δ

Приложение E · Линеаризация, якобианы и гессианы 1019

или приближение второго порядка
f ¢(Δ) ≈ f(x0) + J(x0)Δ+ ½ΔTH(x0)Δ,
где Δ ∈ ℝn – бесконечно малое изменение x ∈ ℝn относительно точки линеаризации x0, J ∈ ℝ1×n – векторный вариант первой
производной, а H ∈ ℝn×n – гессиан или матричный вариант второй производной.
Производная функции f(·) по вектору x равна

и сама является вектором-столбцом, указывающим в направлении, в котором функция f(x) имеет максимальное возрастание. Производную часто записывают как ∇x f, чтобы было ясно,
что дифференцирование производится по x.
Гессиан – это симметричная мат­ри­ца n×n вторых производ­
ных

Функция находится в критической точке, когда ||J(x)|| = 0.
Если гессиан H(x) положительно определен, то функция находится в локальном минимуме, если отрицательно определен, то
в локальном максимуме, а если не определен, то функция находится в седловой точке.
Для функций, квадратичных по x, как в случае задач наименьших квадратов, можно показать, что гессиан равен

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

1020 Приложение E · Линеаризация, якобианы и гессианы

E.3

Векторная функция вектора

Векторное поле f(x): ℝn ↦ ℝm можно также записать как

где f i: ℝm → ℝ, i = 0, ..., m - 1. Производную f по вектору x можно
выразить в матричной форме как мат­ри­цу Якоби (якобиан)

которую также можно записать как

Эта производная также известна как касательное отображение f, обозначаемое Tf, или дифференциал f, обозначаемый Df.
Чтобы было ясно, что дифференцирование производится по x,
мат­ри­цу можно обозначить как Jx, Tx f, Dx f или даже ∂f/∂x.
Гессиан в этом случае – это H = (H0(x), ..., Hm-1(x)). В Python это
можно выразить в виде массива n×n×m.

E.4

Нахождение якобианов

Якобианы функций требуются для многих алгоритмов оптимизации, а также для расширенного фильтра Калмана и могут
быть найдены численно или символически.
Рассмотрим уравнение (6.9) оценки дальности и угла азимута
ориентира с учетом положения транспортного средства и положения ориентира. Мы можем выразить это как очень простую
лямбда-функцию:
>>> zrange = lambda xi, xv, w: np.array([
...
np.linalg.norm(xi - xv[:2]) + w[0],

Приложение E · Линеаризация, якобианы и гессианы 1021
...
...

np.arctan((xi[1] - xv[1]) / (xi[0] - xv[0]))
-xv[2] + w[1]]);

Чтобы оценить якобиан Hxv = ∂h/∂xv для xv = (1, 2, π/3) и xi =
(10, 8), мы можем вычислить числовую разность первого по­
рядка:
>>> xv = np.r_[1, 2, pi/3]; xi
>>> h0 = zrange(xi, xv, w)
array([ 10.82, -0.4592])
>>> d = 0.001;
>>> J = np.column_stack([
...
zrange(xi, xv + [d,
...
zrange(xi, xv + [0,
...
zrange(xi, xv + [0,
... ]) / d
array([[ -0.832, -0.5547,
[ 0.05129, -0.07693,

= np.r_[10, 8]; w = np.r_[0, 0];

0, 0], w) - h0,
d, 0], w) - h0,
0, d], w) - h0
0],
-1]])

где последний столбец совпадает с якобианом, представленным
в (6.14). Обратите внимание, что при вычислении этого якобиана мы установили шум измерения w равным нулю. Основная
трудность при таком подходе заключается в выборе d, разности,
используемой для вычисления конечно-разностной аппроксимации производной. Если взять слишком большое значение, то
для нелинейной функции результаты будут весьма неточными; если слишком маленькое – численные проблемы приведут
к снижению точности.
Для этого в Toolbox имеется удобная функция
>>> numjac(lambda x: zrange(xi, x, w), xv)
array([[ -0.8321, -0.5547,
0],
[ 0.05128, -0.07692,
-1]])

где аргументы – скалярная функция вектора и значение вектора, относительно которого выполняется линеаризация.
В качестве альтернативы дифференцирование можно выполнить символически. Эта конкретная функция относительно
проста, и ее производные можно легко определить с помощью
дифференциального исчисления. Численную производную
можно использовать для быстрой проверки правильности. Чтобы избежать ошибки (или в случае более сложных функций),
можно выполнить дифференцирование, используя практически любой пакет компьютерной алгебры – их очень много. Используя SymPy, можно объявить некоторые символьные переменные xi и xv, которые представляют собой двумерные массивы
>>> from sympy import Matrix, MatrixSymbol, sqrt, atan, simplify, pycode
>>> xi = MatrixSymbol("xi", 2, 1)
xi
>>> xv = MatrixSymbol("xv", 3, 1)

1022 Приложение E · Линеаризация, якобианы и гессианы
xv
>>> w = Matrix([0, 0])
Matrix([
[0],
[0]])

Теперь нам нужно переписать функцию в удобном для SymPy
виде, удалив функции NumPy и заменив их эквивалентными
функциями SymPy , которая представляет все то же уравнение
(6.9), только в символьной форме SymPy, используя мат­ри­цу
SymPy вместо массива NumPy. Результат получился менее компактным, потому что SymPy поддерживает только двумерные
массивы, поэтому нам нужно указать два индекса для ссылки
на элемент вектора. Якобиан вычисляется, как показано ниже:
>>> J = z.jacobian(xv)
Matrix([
[(-xi[0, 0] + xv[0, 0])/sqrt((xi[0, 0] - xv[0, 0])**2
+ (xi[1, 0] - xv[1, 0])**2),
(-xi[1, 0] + xv[1, 0])/sqrt((xi[0, 0] - xv[0, 0])**2
+ (xi[1, 0] - xv[1, 0])**2),
0],
[(xi[1, 0] - xv[1, 0])/((xi[0, 0] - xv[0, 0])**2
+ (xi[1, 0] - xv[1, 0])**2),
(-xi[0, 0] + xv[0, 0])/((xi[0, 0] - xv[0, 0])**2
+ (xi[1, 0] - xv[1, 0])**2),
-1]
])

который имеет необходимую форму
>>> J.shape
(2, 3)

и характеризует последний столбец. Этот код можно вырезать
и вставить в свою программу или автоматически создать вызываемую функцию на Python, используя pycode.
Другой интересный подход – автоматическое дифференцирование кода. Пакет ADOL-C – инструмент с открытым исходным кодом, автоматически различающий языки C и C++, т. е.,
получая функцию, написанную на C, он будет возвращать функцию якобиана на C. Пакет доступен по адресу https://github.
com/coin-or/ADOL-C. Для Python есть инструмент с открытым
исходным кодом под названием Tangent, доступный по адресу
https://github.com/google/tangent.

Функции NumPy
не принимают
символические
переменные.

Приложение

F

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

F.1

Линейные задачи

F.1.1

Неоднородные системы

Это уравнения вида
Ax = b,
где нужно найти неизвестный вектор x ∈ ℝn, а A ∈ ℝm×n и b ∈ ℝm –
константы.
Если n = m, то A – квадратная, а если A невырожденная, то
решение получается с помощью обратной мат­ри­цы:
x = A-1b.
На практике часто встречаются системы, где m > n, т. е. уравнений больше, чем неизвестных. В общем случае точного решения не будет, но мы можем попытаться найти лучшее решение
методом наименьших квадратов
x∗ = argmin ||Ax - b||.
x

Это решение получается следующим способом:
x∗ = (ATA)-1ATb = A+b,

который называется псевдообращением, или, более строго, левообобщенным обращением. Используя SVD, где A = UΣV T, получаем
x = VΣ−1U Tb,
где Σ−1 – просто поэлементное обращение диагональных элементов Σ.

1024 Приложение F · Решение систем уравнений

Если мат­ри­ца вырождена или на систему наложено ограничение n < m, то существует бесконечно много решений. Мы снова можем использовать подход SVD
x = VΣ-1U Tb,
где на этот раз Σ−1 является поэлементным обращением нену­
левых диагональных элементов Σ, все остальные нули остаются
неизменными.
Все эти задачи можно решить с помощью пакета SciPy:
>>> x = linalg.spsolve(A, b)

F.1.2

Однородные системы

Это уравнения вида
Ax = 0,

(F.1)

которые всегда имеют тривиальное решение x = 0. Если A –
квад­ратная и невырожденная, то это единственное решение.
Иначе, если A не имеет полного ранга, т. е. мат­ри­ца неквадратная или квадратная, но вырожденная, то существует бесконечное количество решений, представляющих собой линейные
комбинации векторов в правом нуль-пространстве A, которое
вычисляется функцией sp.linalg.null_space из пакета SciPy.

F.1.3

Вычисление мат­ри­цы вращения

Рассмотрим два множества точек в ℝn, связанных неизвестным
поворотом R ∈ SO(n). Точки расположены по столбцам как P =
{p0, ..., pm-1} ∈ ℝn×m и Q = {q0, ..., qm-1} ∈ ℝn×m так, что
RP = Q.

Чтобы найти R, сначала нужно вычислить мат­ри­цу моментов

и затем взять SVD таким образом, чтобы M = UΣV T. Оценка наименьших квадратов мат­ри­цы вращения определяется как
(F.2)

Приложение F · Решение систем уравнений 1025

и гарантированно является мат­ри­цей SO(3) (Horn 1987; Umeyama 1991).

F.2

Нелинейные задачи

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

F.2.1

Поиск корней уравнений

Рассмотрим систему уравнений, представленную в виде
f(x) = 0,
где f : ℝn ↦ ℝm. Это нелинейная версия описанной выше однородной системы. Сначала линеаризуем уравнение относительно наилучшей оценки решения x0:
f(x0 + Δ) = f(x0) + J(x0)Δ + ½ΔTH(x0)Δ + H.O.T.,
Для оценки ΔTH(x0)
запишем это как
– Δ), где
ΔT(H(x0)×
2
используется
тензорно-векторное
произведение.

(F.4)

где f0 = f(x0) ∈ ℝm×1 – значение функции, J = J(x0) ∈ ℝm×n – якобиан,
оба оцениваются в точке линеаризации, Δ ∈ ℝn×1 – бесконечно
малое изменение x относительно x0, а H.O.T. обозначает higher-­
order terms (члены более высокого порядка). H(x0) – гессиан
n×n×m.
Берем первые два члена из (F.4) и формируем линейное приближение
f ¢(Δ) ≈ f0 + J(x0)Δ,

(F.5)

находим приближенное решение нашей исходной задачи
f ¢(Δ) = 0:
f0 + J(x0)Δ = 0 ⇒ Δ = -J-1(x0)f0.

Если n ≠ m, то J не квадратная, и мы можем использовать
функцию псевдообращения np.linalg.pinv(J) или функцию аппроксимации методом наименьших квадратов scipy.linalg.
spsolve(J, f0). Вычисленный шаг Δ основан на аппроксимации
исходной нелинейной функции, поэтому x0 + Δ, как правило,
не будет решением, но станет ближе к нему. Это приводит нас
к итеративному решению – методу Ньютона–Рафсона:

1026 Приложение F · Решение систем уравнений

repeat
| вычислить f0 = f(x0), J = J(x0)
Δ = -J-1f0
| x0 ← x0 + Δ
until ||f0|| < ϵ

|

F.2.2

Нелинейная минимизация

Широко распространенным классом задач является поиск ми­
нимума скалярной функции f(x): ℝn ↦ ℝ, которая может быть
выражена как
x∗ = argmin f(x).
x

Производная линеаризованной системы (F.5):

Если мы рассматриваем функцию как многомерную поверхность, то J(x0) является вектором, указывающим направление
и величину наклона при x = x0, поэтому обновление
Δ = -βJ(x0)
сдвинет оценку вниз по склону к минимуму. Таким образом,
мы приходим к итеративному решению, называемому градиентным спуском:
repeat
| вычислить J = J(x0)
Δ = -β J
| x0 ← x0 + Δ
until ||Δ|| < ϵ

|

и задача состоит в том, чтобы выбрать подходящий размер
шага β. Если выбрать слишком маленький шаг, то потребуется
выполнить много итераций. Если слишком большой, то решение может оказаться нестабильным и не сойтись.
Если добавить член второго порядка из уравнения (F.4), аппроксимация примет вид
f( Δ) ≈ f0 + J(x0)Δ + ½ΔTH(x0)Δ,
включающий гессиан n×n. Чтобы найти минимумы, возьмем
производную и приравняем ее к нулю

Приложение F · Решение систем уравнений 1027

где изменение
Δ = -H-1(x0)J(x0).
Это приводит нас к другому итеративному решению – методу Ньютона. Задача состоит в том, чтобы определить гессиан
нелинейной системы либо с помощью числовой аппроксимации, либо с помощью символьных манипуляций.

F.2.3

Нелинейная минимизация методом
наименьших квадратов

Очень часто скалярная функция, которую требуется оптимизировать, представляет собой квадратичную функцию стоимости:
F(x) = ||f(x)||2 = f(x)Tf(x),
где f(x): ℝn ↦ ℝm – некоторая векторнозначная нелинейная
функция, которую можно линеаризовать как
f ¢(Δ) ≈ f0 + JΔ,

а скалярная стоимость
F(Δ) ≈ ( f0 + JΔ)T( f0 + JΔ)
≈ f0Tf0 + f0T JΔ + ΔT JTf0 + ΔT JT JΔ
≈ f0Tf0 + 2f0T JΔ + ΔT JT JΔ,
Один из подчеркнутых членов получается транспонированием другого,
но поскольку оба
приводят к скалярному транспонированию, это не имеет
значения.

где JT J ∈ ℝn×n – приближенный гессиан из E.2.
Чтобы минимизировать ошибку этой линеаризованной системы наименьших квадратов, мы берем производную по Δ
и приравниваем ее к нулю:

Это уравнение можно решить для локально оптимального
обновления
Δ = -(JT J)-1JTf0,

(F.6)

в котором можно разглядеть псевдо- или левообобщенное обращение J. Чтобы найти решение, снова прибегнем к итерации.
Это так называемая итерация Гаусса–Ньютона.

Вычислительные проблемы
При решении уравнения (F.6) иногда оказывается, что гессиан
JT J плохо обусловлен или вырожден, но это можно исправить,
добавив демпфирующий член λ

1028 Приложение F · Решение систем уравнений

Δ = -(JT J + λ1n×n)-1JTf0,
что делает систему более положительно определенной. Поскольку JT J + λ1n×n фактически находится в знаменателе, увеличение λ приведет к уменьшению ||Δ|| и медленной сходимости.
Как правильно выбрать λ? Можно поэкспериментировать
с разными значениями, но лучшим способом является алгоритм Левенберга–Марквардта (алгоритм F.1), который корректирует λ, чтобы гарантировать сходимость. Если ошибка увеличивается по сравнению с последним шагом, то шаг повторяется
с увеличением λ, чтобы уменьшить размер шага. Если ошибка
уменьшается, то λ уменьшается, чтобы увеличить скорость сходимости. Обновления постоянно варьируются между методом
Гаусса–Ньютона (низкое значение λ) и градиентным спуском
(высокое значение λ).
Алгоритм F.1. Алгоритм Левенберга–Марквардта. Значение c
обычно выбирается в диапазоне от 2 до 10
1 инициализация λ
2 repeat
3 | вычисление f0 = f(x0), J = J(x0), H = JT J
4
Δ = -(H + λ1n×n)-1JTf0
5
if f(x0 + Δ) < f(x0) then
6
| – ошибка снизилась: уменьшить демпфирование
7
x0 ← x0 + Δ
8
| λ ← λ/c
9
else
10
| – ошибка возросла: отменить шаг и увеличить
демпфирование
11
| λ ← cλ
12 | end
13 until ||Δ|| < ϵ

|
|
|
| |
|
|
|
| |
|

Для задач, где n велико, обращение приблизительного гессиа­
на n×n обходится дорого. Обычно m < n, что означает, что якобиан не является квадратным, и уравнение (F.6) можно переписать как
Δ = -JT(JJT)-1f0,
что является правым псевдообращением и требует обращения
меньшей мат­ри­цы. Мы можем снова ввести демпфирующий
член
Δ = -JT(JJT + λ1m×m)-1f0,
и при большом λ уравнение становится проще:

Приложение F · Решение систем уравнений 1029

Δ ≈ -βJTf0,
но показывает очень медленную сходимость.
Если fk(·) содержит аддитивный шум, который имеет нулевое среднее, нормально распределен и неизменен во времени, то мы получаем оцениватель максимального правдоподобия x. Выбросы данных оказывают значительное влияние
на результат, поскольку ошибки возводятся в квадрат. Робастные оцениватели минимизируют влияние выбросов данных,
а в М-оценивателе
F(x) = ρ( fk(x))
квадрат нормы заменяется функцией потерь ρ(·), которая моделирует вероятность своего аргумента. В отличие от квадрата нормы, эти функции сглаживаются при больших значениях. К распространенным примерам относятся функция потерь
Хьюбера и двухвесовая функция Тьюки.

F.2.4

Р азреженные нелинейные методы
наименьших квадратов

Для большого класса задач общая стоимость представляет собой сумму квадратов стоимостей
(F.7)
Рассмотрим задачу подгонки модели z = ϕ(w, x), где ϕ: ℝp ↦
ℝ с параметрами x ∈ ℝn, к набору точек данных (wk, zk). Вектор
ошибки, связанный с k-й точкой данных, равен
m

fk(x) = zk - ϕ(wk; x) ∈ ℝm,

и минимизация уравнения (F.7) дает оптимальные параметры
модели x.
Другим примером является оптимизация графа положений,
используемая для SLAM и пакетной подстройки. Ребро k в графе
соединяет вершины i и j и имеет соответствующую стоимость
fk: ℝn ↦ ℝm:
fk(x) = ê k(x) - ek#,

(F.8)

где ek# – наблюдаемое значение параметра ребра, а ê k(x) – оценка, основанная на состоянии x графа положений. После линеаризации
fk¢(Δ) ≈ f0,k + JkΔ

1030 Приложение F · Решение систем уравнений

квадратичная ошибка для ребра равна
Fk(x) = fkT(x)Ωk fk(x),
где Ωk ∈ ℝm×m – положительно определенная постоянная мат­ри­
ца, ► которую применяют следующим образом:
Fk(Δ) ≈ (f0,k + JkΔ)TΩk(f0,k + JkΔ)

T
T
Ωk f0,k + f 0,k
Ωk Jk Δ + ΔT JkTΩk f0,k + ΔT JkTΩk Jk Δ
≈ f 0,k

≈ ck + 2bkTΔ + ΔT Hk Δ,
T
T
где ck = f 0,k
Ωk f0,k, bkT = f 0,k
Ωk Jk и Hk = JkTΩk Jk. Общая стоимость представляет собой сумму всех стоимостей ребер

где

После их вычисления действуем, как и раньше: берем производную по Δ, приравниваем ее к нулю, находим обновление Δ
и выполняем итерации с использованием алгоритма (F.2).

Вектор состояния
Вектор состояния представляет собой конкатенацию всех положений и координат в задаче оптимизации. Для графа положений SLAM он принимает форму
x = {ξ0, ξ1, …, ξN-1} ∈ ℝNx.

Положения должны быть представлены в векторной форме
и, предпочтительно, компактно и без вырожденности. Для SE(2)
это довольно просто, если использовать ξ ∼ (x, y, θ) ∈ ℝ3 и Nξ = 3.
Для SE(3) используют ξ ∼ (t, r) ∈ ℝ6, включающее перенос t ∈ ℝ3,
поворот r ∈ ℝ3 и Nξ = 6. Повороты могут быть представлены
тройными углами (Эйлера или крена-тангажа-рыскания), углами относительно оси, экспоненциальными координатами или
векторной частью единичного кватерниона, как обсуждалось
в разделе 2.3.1.7. Вектор состояния x имеет длину Nx = NNξ и состоит из последовательности подвекторов, по одному на положение – i-й подвектор, это xi ∈ ℝNξ.

Ее можно использовать для определения значимости
ребра det(Ωk) по
отношению к другим ребрам, а также
относительной
значимости элементов fk(·).

Приложение F · Решение систем уравнений 1031

В случае SLAM с графом положений с ориентирами или пакетной подстройки вектор состояния содержит векторы положений и координат
x = {ξ0, ξ1, …, ξN-1 | P0, P1, …, PM-1} ∈ ℝNx,

а i-й и j-й подвекторы в x обозначаются как ℝNξ и ℝNP и соответствуют ξi и Pj соответственно, где NP = 2 для SE(2) и NP = 3 для
SE(3). Вектор состояния x имеет длину Nx = NNξ + MNP.

Внутренняя структура
Ключевым является тот факт, что вектор ошибок fk(x) для ребра
k зависит только от ассоциированных вершин i и j, а это означает, что якобиан

в основном состоит из нулей

где Ai ∈ ℝm×Nξ и Bj ∈ ℝm×Nξ или Bj ∈ ℝm×NP в соответствии со структурой вектора состояния.
Эта разреженная блочная структура означает, что вектор bk
и гессиан JkTΩk Jk тоже имеют разреженную блочную структуру,
как показано на рис. F.1. Гессиан содержит только четыре небольших ненулевых блока, поэтому вместо вычисления произведения JkTΩk Jk, которое содержит большое количество умножений на ноль, мы можем просто вычислить четыре ненулевых
блока и добавить их в гессиан для системы наименьших квад­
ратов. Все блоки в строке имеют одинаковую высоту, а в столбце – одинаковую ширину. В случае графа положений SLAM
с ориентирами или пакетной подстройки блоки имеют разные
размеры, как показано на рис. F.1б.
Если значение ребра представляет положение, то уравнение
(F.8) следует заменить на fk(x) = ê k (x) ⊖ ek#. Мы обобщаем его
с помощью оператора ⊟, чтобы указать, что следует использовать операторы «–» или «⊖» в зависимости от ситуации. Точно
так же при обновлении вектора состояния в конце итерации положения должны быть объединены в композицию x0 ← x0 ⊕ Δ,
и мы обобщаем это действие оператором ⊞. Задача оптимизации графа положений решается итеративным способом в алгоритме F.2.

1032 Приложение F · Решение систем уравнений

а

б

Рис. F.1. Внутренняя структура вектора ошибок, мат­ри­цы Якоби и Гессе для задач наименьших квадратов на основе
графов: а) граф положений SLAM с N узлами, представляющими положение робота как ℝNξ; б) пакетная подстройка
с N узлами, представляющими положение камеры как ℝNξ, и M узлами, представляющими положение ориентира
как ℝNP. Индексы i и j обозначают i-й и j-й блоки, а не i-ю и j-ю строку или столбец. Белый цвет обозначает нулевые
значения

Алгоритм F.2. Оптимизация графа положений. Для оптимизации
Левенберга–Марквардта замените строку 14 строками 4–12
из алгоритма F.1
1 repeat
2
H ← 0, b ← 0
3
foreach k do
4
f0,k(x0) = êk(x) ⊟ ek#
5
(i, j) = вершины(k)
6
вычисление Ai(xi), Bj(xj)
T
7
bi ← bi + f 0,k
Ωk Ai
T
8
bj ← bj + f 0,k Ωk Bj
9
Hi,i ← Hi,i + AiTΩk Ai
10
Hi, j ← Hi, j + AiTΩk Bj
11
Hj,i ← Hj,i + BjTΩk Ai
12
Hj, j ← Hj, j + BjTΩk Bj
13
end
14
Δ = -H-1b
15
x0 ← x0 ⊞ Δ
16 until ||Δ|| < ϵ

|

|
|
|
|
|
|
|
|
|
|
|
|
|

|

|
|
|
|
|
|
|
|

Проблемы большого масштаба

Для SLAM с графом, представляющим тысячи положений, или
пакетной подстройки с тысячами камер и миллионами ориентиров мат­ри­ца Гессе будет огромной, что приведет к проблемам

Приложение F · Решение систем уравнений 1033

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

0

50
1000
100
2000
150

3000

200

250
4000
300
5000
350

а

0

1000

2000

3000

nz = 67795

4000

5000

б

0

50

100

150

200

nz = 20646

250

300

350

Рис. F.2. Карта разреженности Гессе, созданная с использованием функции plt.spy, количество ненулевых
элементов показано под графиком: а) гессиан для задачи SLAM на основе графа положений (рис. 6.18),
диагональные элементы представляют ограничения положений между последовательными вершинами изза одометрии, а недиагональные члены – ограничения из-за повторного посещения местоположений (петли);
б) гессиан для задачи пакетной подстройки с 10 камерами и 110 ориентирами (examples/bademo)

Во-первых, гессиан разреженный, т. е. содержит в основном
нули. Вместо хранения всех этих нулей (по 8 байт каждый) можно воспользоваться поддержкой разреженных матриц, которые
просто хранят список ненулевых элементов. В модуле scipy.
sparse есть классы для создания разреженных матриц, поведение которых похоже на поведение обычных, неразреженных
массивов NumPy, а также функция spsolve для решения систем
уравнений Ax = b, представленных разреженными мат­ри­ца­ми
и векторами.
Во-вторых, в случае пакетной подстройки гессиан имеет две
блочно-диагональные подмат­ри­цы, поэтому можно разделить
систему следующим образом:

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

где B и C – диагонали блоков. Нижние индексы ξ и P обозна­
чают блоки Δ и b, связанные с положениями камеры и пози­
циями ориентиров соответственно. Сначала решим систему
для обновлений положений камеры Δξ

1034 Приложение F · Решение систем уравнений

SΔξ = bξ - EC-1bP,
где S = B − EC−1ET – дополнение Шура, представляющее собой
симметричную положительно определенную мат­ри­цу, которая
также является блочно-диагональной. Затем мы решаем систему для обновлений позиций ориентиров
ΔP = C-1(bP + ETΔξ).
Более сложные методы используют мелкомасштабную блочную структуру для дальнейшего сокращения времени вычислений, например GTSAM (https://github.com/borglab/gtsam)
и SLAM++ (https://sourceforge.net/projects/slam-plus-plus).

Закрепление
Оптимизация обеспечивает решение, в котором относитель­
ные положения и позиции дают наименьшую общую стоимость,
и такое решение будет представлять собой произвольное преобразование по отношению к глобальной системе отсчета. Чтобы получить абсолютные положения и позиции, мы должны
закрепить или зафиксировать некоторые узлы – присвоить им
значения относительно глобальной системы координат и предотвратить их корректировку при оптимизации. Подходящий
способ добиться этого – удалить из H и b строки и столбцы, соответствующие закрепленным положениям и позициям. Затем
мы решаем задачу более низкой размерности для Δ¢, которая
будет короче, чем x, и требуется тщательный подход, чтобы
правильно сопоставить подвекторы Δ¢ с векторами x для обновления.

Приложение

G

	Гауссовы случайные
величины
Одномерная гауссовская функция
(G.1)
полностью описывается местоположением пика μ и его шириной. Общая площадь под кривой равна единице, а g(x) > 0, ∀x.
График функции можно построить с помощью функций из пакета Toolbox
>>> x = np.linspace(-6, 6, 500);
>>> plt.plot(x, gauss1d(0, 1, x), "r");
>>> plt.plot(x, gauss1d(0, 2**2, x), "b--");

На рис. G.1 показаны два графика, две гауссианы, с нулевым
средним и σ = 1 и σ = 2. Обратите внимание, что во втором аргументе функции gauss1d передается σ2.
Если рассматривать гауссово распределение как функцию
плотности вероятности (probability density function, PDF), то это
хорошо известное нормальное распределение, где местоположение пика – это среднее значение, а ширина – стандартное
отклонение. Случайная величина, полученная из нормального
распределения, часто записывается как X ~ N(μ, σ2), где σ2 – дисперсия. N(0, 1) называется стандартным нормальным распределением – функция random.normal из пакета NumPy генерирует
случайные числа из этого распределения. Вот как можно получить сто гауссовских случайных чисел со средним значением mu
и стандартным отклонением sigma:
>>> g = np.random.normal(loc=mu, scale=sigma, size=(100,));

Вероятность, что случайное значение попадает в интервал
x ∈ [x1, x2], определяется интегрированием
,

или оценкой функции кумулятивного нормального распределения Φ(x), возвращаемой вызовом scipy.stats.norm.cdf. От-

1036 Приложение G · Гауссовы случайные величины

меченные точки на рис. G.1 в интервале μ ± 1σ ограничивают
доверительный интервал шириной в 1σ. Площадь под кривой
в этом интервале равна 0.68, поэтому вероятность получения
случайного значения из этого интервала составляет 68 %.

Рис. G.1. Две гауссовские функции, обе со средним значением μ = 0 и стандартными
отклонениями σ = 1 и σ = 2. Отмеченные точки соответствуют значениям x = μ ± 1. Площадь
под обеими кривыми равна единице

Гауссово распределение можно расширить до произвольного
числа измерений. n-мерное гауссово распределение, или многомерное нормальное распределение, имеет вид
,

(G.2)

и по сравнению со скалярным случаем (G.1) x ∈ ℝn и μ ∈ ℝn теперь
выражаются векторами, квадратный член в показателе степени
заменен матричной квадратной формой, а σ2 (дисперсия) стала
симметричной положительно определенной мат­ри­цей P ∈ ℝn×n.
Мы можем построить двумерную гауссиану
>>>
...
>>>
>>>
>>>
>>>
>>>

x, y = np.meshgrid(np.linspace(-5, 5, 100),
np.linspace(-5, 5, 100));
P = np.diag([1, 2])**2;
g = gauss2d([0, 0], P, x, y);
ax = plotvol3();
ax.plot_surface(x, y, g);
ax.contour(x, y, g, zdir="z", offset=-0.05);

в виде поверхности, как показано на рис. G.2. Изолинии на горизонтальной плоскости внизу имеют форму эллипсов, и в данном примере радиусы по осям y и x находятся в соотношении

Приложение G · Гауссовы случайные величины 1037

2 : 1, согласно отношению стандартных отклонений. Если ковариационная мат­ри­ца диагональная, как в данном случае, то радиусы эллипсов параллельны осям x и y, как показано в разделе C.1.4, иначе они повернуты.

Рис. G.2. Двухмерная гауссовская
функция с ковариацией P = diag(12, 22).
Внизу показаны изолинии постоянной
плотности вероятности

Это также определение расстояния
Махаланобиса, ковариационно-взвешенного расстояния
между x и μ.
Если выбрать из
многомерного гауссова распределения
вектор длины n, то
каждый его элемент
будет распределен
нормально. Сумма
квадратов независимых нормально
распределенных
значений имеет
­распределение
χ2(хи-квадрат)
с n степенями
свободы.

Если двухмерное гауссово распределение рассматривается как функция распределения вероятностей (PDF), то P – это
ковариационная мат­ри­ца, диагональные элементы pi,i которой
представляют дисперсию xi, а недиагональные элементы pi,j –
корреляции между xi и xj. Обратная ковариационная мат­ри­ца
называется информационной мат­ри­цей. Если переменные независимы, или некоррелированы, то мат­ри­ца P будет диагональной. В этом случае μ = (0, 0) и P = diag(12, 22), что соответствует
некоррелированным переменным со стандартным отклонением 1 и 2 соответственно. Если функция плотности вероятности
отображает положение транспортного средства, то оно, скорее
всего, находится в точке с координатами (0, 0). Вероятность нахождения в любой области плоскости xy определяется объемом
под поверхностью этой области и требует нетривиального интегрирования.
Связь между гауссовскими функциями плотности вероятности и эллипсами можно найти в квадратичном показателе (G.2),
который является уравнением эллипса.
Все точки которых
удовлетворяют уравнению
(x - μ)T P-1(x - μ) = s,
что приводит к постоянному значению плотности вероятности,
т. е. к изолинии двухмерного гауссова распределения. Если p –
вероятность, что точка x лежит внутри эллипса, тогда s определяется обратной кумулятивной функцией распределения ­χ2
с n степенями свободы (в данном случае с двумя). Например,
50%-ный доверительный интервал равен

1038 Приложение G · Гауссовы случайные величины
>>> from scipy.stats.distributions import chi2
>>> chi2.ppf(0.5, 2)
1.386

где первый аргумент – вероятность, а второй – число степеней
свободы.
Чтобы построить двухмерный ковариационный эллипс, используется стандартный подход, описанный в разделе C.1.4,
только правая часть уравнения эллипса равна s, а не 1, а E = P-1.
С помощью Toolbox это выглядит так:
plot_ellipse(P,confidence=0.5, inverted=True)

Приложение

H

	Фильтр Калмана
полезными.
« Нет верных моделей. Но некоторые могут быть– Джордж
Бокс
Рассмотрим систему на рис. H.1. Физический робот представляет собой «черный ящик» и имеет истинное состояние (положение) x, меняющееся со временем в зависимости от входных
данных. Мы не можем напрямую измерить это состояние, но
датчики робота выдают результаты, являющиеся функцией
этого истинного состояния. Наша задача: учитывая входные
данные системы s и выходные сигналы датчиков, оценить неизвестное истинное состояние x и определить, насколько точна
эта оценка.
выходной сигнал датчика

z

k+1

= h(x

k+1

,w

k

)

оценки ковариации
источника шума
шум датчика

N (0, W )
случайный

x

k+1

= f (x

k

,u

k

,v

k

)

оценки интервала перехода
и функций датчика

истинная
динамика

f (·)

V̂

h(·)

Ŵ

xˆ k + 1
Pˆ k + 1

шум процесса

N (0, V )

роботизированная платформа

случайный

расширенный
фильтр Калмана

физический робот
истинное состояние
не поддается
прямому измерению

входные
данные

Рис. H.1. Физический робот слева – это «черный ящик», истинное состояние которого
невозможно измерить напрямую. Однако его можно оценить на основе выходных данных
датчика, которые являются функцией неизвестного истинного состояния, и предполагаемой
модели

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

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

1040 Приложение H · Фильтр Калмана

данные u системы. Наша модель вряд ли будет идеальной , и эту
неопределенность обычно представляют с помощью воображаемого генератора случайных чисел, который искажает состояние системы – имитируя шум процесса. Во-вторых, мы знаем,
как выходной сигнал датчика зависит от состояния – это модель
датчика h(·), и его неопределенность тоже моделируется воображаемым генератором случайных чисел – шумом датчика.
Воображаемые источники случайных чисел v и w находятся
внутри «черного ящика», поэтому сами случайные числа также неизвестны. Однако мы можем описать характеристики
этих случайных чисел – их распределение, которое показывает
вероятность получения случайного числа с определенным значением. Значительная часть шума в физических системах хорошо моделируется с помощью гауссова (нормального) распределения N(μ, σ2), которое характеризуется средним значением
и стандартным отклонением и имеет ряд полезных математических свойств, на которые мы будем опираться. Обычно мы
предполагаем, что шум имеет нулевое среднее значение, а ковариации шума процесса и шума датчика равны V и W соответственно.
В целом задача, стоящая перед нами, заключается в следующем:

Например,
проскальзывание
колес наземного
робота или снос
ветром БПЛА.
Существует бесконечно много возможных гауссовых
распределений
с источниками
шума, которые могут
быть асимметричными или иметь несколько пиков. Мы
никогда не должны
предполагать, что
шум имеет гауссово
распределение,
и должны попытаться определить
распределение, опираясь на понимание
физики процесса
и датчика, или с помощью тщательных
измерений и анализа результатов.

Имея модель системы f(·), h(·), оценки V и W, известные входные
данные u и некоторые зашумленные измерения датчика z, найти
оценку x̂ состояния системы и неопределенность P̂ этой оценки.

В контексте определения местонахождения робота, x – это
неизвестные координаты и, возможно, ориентация робота,
u – команды, посылаемые двигателям, а z – выходные данные
различных датчиков на роботе. Для наземного робота x представляет положение в SE(2), u – команды двигателям, а z может быть показаниями одометра или расстоянием и пеленгом
до ориентиров. Для воздушного робота x будет положением
в SE(3), u – известные силы, прикладываемые к планеру винтами, а z может быть измеренными ускорениями и угловыми
скоростями. ►

H.1

Линейные системы – фильтр Калмана

Рассмотрим модель перехода, описанную как дискретная линейная система, инвариантная во времени:
x⟨k + 1⟩ = Fx〈k〉 + Gu〈k〉 + v〈k〉,

(H.1)

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

Приложение H · Фильтр Калмана 1041

z〈k〉 = Hx〈k〉 + w〈k〉,

(H.2)

где k – временной шаг, x ∈ ℝn – вектор состояния, а u ∈ ℝm – вектор входных сигналов системы в момент времени k, например
команда скорости или приложенные силы и моменты. Матрица
F ∈ ℝn×n описывает динамику системы, т. е. изменение состояния
со временем. Матрица G ∈ ℝn×m описывает взаимосвязь входных сигналов и состояний системы. Вектор z ∈ ℝp представляет
выходные сигналы системы, измеренные датчиками. Матрица
H ∈ ℝp×n описывает отображение состояния системы в выходные сигналы, доступные для наблюдения.
Для учета ошибок в модели движения (F и G) или немоделированных возмущений введем гауссову случайную величину
υ ∈ ℝn, называемую шумом процесса. υ〈k〉 ~ N(0, V), т. е. имеет нулевое среднее значение и ковариацию V ∈ ℝn×n. Ковариация – это симметричная, положительно определенная мат­ри­
ца, представляющая дисперсию многомерного распределения.
Модель измерения датчика H тоже не идеальна, и эта неидеальность моделируется шумом измерения датчика – гауссовой случайной величиной w ∈ ℝp, w〈k〉 ~ N(0, W) и ковариацией W ∈ ℝp×p.
Фильтр Калмана является оптимальным средством оценки
в случае, когда шум процесса и шум измерения имеют гауссово
распределение с нулевым средним. Фильтр включает два этапа:
прогнозирование и обновление. Прогнозирование основано на
предыдущем состоянии и использованных входных данных
x̂ +〈k + 1⟩ = Fx̂〈k〉 + Gu〈k〉,

P̂+⟨k + 1⟩ = FP̂⟨k⟩F T + V̂,

(H.3)
(H.4)

где x̂+ – прогнозируемая оценка состояния, а P̂ ∈ ℝn×n – прогнозируемая оценка ковариации, или неопределенности, в x̂+. Обозначение + явно указывает, что левая часть уравнения представляет собой прогноз на момент времени k + 1, основанный на
информации, полученной в момент времени k. V̂ – константа
и наша наилучшая оценка ковариации шума процесса.
Подчеркнутый член в (H.4) проецирует оценку ковариации
из текущего временного шага на следующий. Рассмотрим одномерный пример, где F – скаляр, а оценка состояния x̂〈k〉 имеет гауссово распределение со средним μ〈k〉 и дисперсией σ2〈k〉.
Уравнение прогнозирования отображает состояние и его гауссово распределение в новое гауссово распределение со средним
значением Fμ〈k〉 и дисперсией F 2σ2〈k〉. Член F〈k〉P〈k〉F〈k〉T – это
матричная форма, а поскольку
cov(Fx) = F cov(x)FT,

ковариация масштабируется соответствующим образом.

(H.5)

1042 Приложение H · Фильтр Калмана

Прогнозирование P̂ в (H.4) предполагает сложение двух положительно определенных матриц, что приводит к увеличению
неопределенности. Это вполне ожидаемо, потому что мы использовали неопределенную модель для прогнозирования будущего значения уже неопределенной оценки. V̂ должно быть
разумной оценкой ковариации фактического шума процесса.
Если переоценить ее, т. е. если взять завышенную оценку шума
процесса, то на этом этапе неопределенность увеличится сильнее, чем необходимо, что приведет к пессимистичной оценке
уверенности.
Чтобы противостоять этому росту неопределенности, необходимо ввести новую информацию, такую как измерения, производимые датчиками, которые зависят от состояния. Разница
между тем, что измеряют и что они должны измерять, заключается в следующем:
v = z#⟨k + 1⟩ - Hx̂+⟨k + 1⟩ ∈ ℝp.

Частично эта разница обусловлена шумом датчика – шумом
измерения, но остальная часть несет ценную информацию
о погрешности между фактическим и прогнозируемым значениями состояния. Вместо того чтобы считать это ошибкой, мы
используем другое название с более позитивной коннотацией:
инновацией – новой информацией.
Второй этап фильтра Калмана, этап обновления, отображает
инновации в поправку для прогнозируемого состояния, оптимально корректируя оценку на основе того, что наблюдали датчики:
x̂⟨k + 1⟩ = x̂+⟨k + 1⟩ + Kv,

(H.6)

P̂⟨k + 1⟩ = P̂+⟨k + 1⟩ – KHP̂+⟨k + 1⟩.

(H.7)

K = P+⟨k + 1⟩HT(HP+⟨k + 1⟩HT + Ŵ)-1 ∈ ℝn×p

(H.8)

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

известна как коэффициент усиления Калмана. Член в скобках – это оценочная ковариация инновации, которая включает
неопределенность состояния и оценочную ковариацию шума
измерения. Если в некоторых измерениях инновация имеет
высокую неопределенность, то коэффициент усиления Калмана будет соответственно малым, т. е. если новая информация
неопределенна, то в вектор состояния вносятся лишь незначи-

Приложение H · Фильтр Калмана 1043

тельные изменения. Подчеркнутый член проецирует ковариацию оценки состояния в пространство значений датчика. Ŵ –
константа и наша наилучшая оценка ковариации шума датчика.
Ковариационная мат­ри­ца должна быть симметричной, но
после многочисленных обновлений накопленные числовые
ошибки могут привести к потере симметричности мат­ри­цы.
Симметричную структуру можно обеспечить, используя форму
Джозефа (H.7):
P̂⟨k + 1⟩ = (1n×n - KH)P̂+⟨k + 1⟩(1n×n - KH)T + KV̂KT,

но это более затратно в вычислительном плане.
Уравнения выше составляют классический фильтр Калмана, широко используемый в робототехнике, аэрокосмической
отрасли и эконометрических приложениях. Фильтр обладает рядом важных характеристик. Во-первых, он оптимален,
но только если шум является истинно гауссовым с нулевым
средним и не зависящими от времени параметрами. Часто это
предположение является хорошим, но не всегда. Во-вторых, он
рекурсивен: выход одной итерации является входом для следующей. В-третьих, он асинхронен. В конкретной итерации,
если информация с датчика недоступна, выполняется только
шаг прогнозирования, а шаг обновления не выполняется. Если
имеются разные датчики, каждый со своей мат­ри­цей H и разными частотами дискретизации, то можно просто применять
обновления с соответствующими z и H, когда данные с датчика
становятся доступными.
Фильтр должен инициализироваться разумными значениями x̂ и P̂, а также надлежащим образом должны быть выбраны
оценочные ковариационные мат­ри­цы V̂ и Ŵ. В процессе работы фильтра оценочная ковариация ||P̂|| уменьшается, но никогда не достигает нуля – можно показать, что минимальное
значение является функцией V̂ и Ŵ. Фильтр Калмана–Бьюси
представляет собой непрерывную во времени версию этого
фильтра.
Ковариационная мат­ри­ца P̂ богата информацией. Диагональные элементы p̂ i,i представляют дисперсию, или неопределенность, состояния xi. Внедиагональные элементы p̂ i,j представляют корреляции между состояниями xi и xj и указывают, что
ошибки в состояниях не являются независимыми. Корреляции
критически важны для прохождения любой новой информации
и корректировки всех состояний, влияющих на результат конкретного процесса.
Если элементы вектора состояния представляют двухмерное
местоположение, то соответствующая подмат­ри­ца 2×2 в P̂ является обратной мат­ри­цей эквивалентного эллипса неопределенности E-1, см. раздел C.1.4.

1044 Приложение H · Фильтр Калмана

H.2

 елинейные системы – расширенный
Н
фильтр Калмана

Когда система не является линейной, то в общем случае ее можно описать двумя функциями: переходом состояния (моделью
движения в робототехнике) и моделью датчика
x⟨k + 1⟩ = f(x〈k〉, u〈k〉, v〈k〉),
z〈k〉 = h(x〈k〉, w〈k〉),

(H.9)
(H.10)

а неопределенность модели, внешние возмущения и шум датчика, как и прежде, можно представить гауссовыми случайными величинами v и w.
Линеаризуем функцию перехода состояния относительно
текущей оценки состояния x̂〈k〉, как показано на рис. H.2, в результате чего получаем
x¢〈k + 1〉 ≈ Fx x¢〈k〉 + Fu u〈k〉 + Fv v〈k〉,

(H.11)

z¢〈k〉 ≈ Hx x¢〈k〉 + Hw w〈k〉,

(H.12)

где Fx = ∂f/∂x ∈ ℝn×n, Fu = ∂f/∂u ∈ ℝn×m, Fv = ∂f/∂v ∈ ℝn×n, Hx = ∂h/∂x ∈
ℝp×n и Hw = ∂h/∂w ∈ ℝp×p – якобианы (см. приложение E) функций
f(·) и h(·). Приравнивая коэффициенты между (H.1) и (H.11), получаем F ~ Fx, G ~ Fu и v〈k〉 ~ Fv v〈k〉, а приравнивая коэффициенты
между (H.2) и (H.12), получаем H ~ Hx и w〈k〉 ~ Hw w〈k〉.

Рис. H.2. Одномерный
пример, иллюстрирующий,
как нелинейная функция
перехода состояния f : xk
↦ xk+1, показанная черным,
линеаризуется относительно
точки (x̂〈k〉, x̂〈k + 1〉), показана
красным

Принимая уравнение прогнозирования (H.9) с v〈k〉 = 0 и уравнение ковариации (H.4) с подставленными линеаризованными
членами, можно записать шаг прогнозирования как

Приложение H · Фильтр Калмана 1045

x̂ +⟨k + 1⟩ = f(x̂〈k〉, u〈k〉),

P̂+⟨k + 1⟩ = Fx P̂⟨k⟩FxT + FvV̂FvT

и шаг обновления как

x̂⟨k + 1⟩ = x̂ +⟨k + 1⟩ + Kv,

P̂⟨k + 1⟩ = P̂+⟨k + 1⟩ - KHx P̂+⟨k + 1⟩,

где коэффициент усиления Калмана теперь имеет вид

Соответственно, эти
мат­ри­цы должны
быть обозначены
как зависящие от
временного шага,
например Fx〈k〉, но
для удобочитаемости эти обозначения
были опущены.

K = P+⟨k + 1⟩HxT(Hx P+⟨k + 1⟩HxT + Hw ŴHwT)-1.

(H.13)

Эти уравнения справедливы только в точке линеаризации
x̂⟨k〉 – якобианы Fx, Fv, Hx, Hw должны вычисляться в каждой итерации. Полная процедура представлена в алгоритме H.1.
Алгоритм H.1. Процедура EKF
Вход: x̂〈k〉 ∈ ℝn, P̂〈k〉 ∈ ℝn×n, u〈k〉 ∈ ℝm, z⟨k + 1⟩ ∈ ℝp; V̂ ∈ ℝn×n, Ŵ ∈ ℝp×p
Выход: x̂〈k + 1〉 ∈ ℝn, P̂〈k + 1〉 ∈ ℝn×n
– линеаризовать относительно x = x̂〈k〉
вычислить якобианы: Fx ∈ ℝn×n, Fv ∈ ℝn×n, Hx ∈ ℝp×n, Hw ∈ ℝp×p
– этап прогнозирования
x̂+〈k + 1〉 = f(x̂〈k〉, u〈k〉) // спрогнозировать состояние
// на следующем шаге
P̂+〈k + 1〉 = FxP̂〈k〉FxT + FvV̂FvT // предсказать ковариацию
// на следующем шаге
– этап обновления
v = z〈k + 1〉 - h(x̂+〈k + 1〉) // инновация: измеренное –
// предсказанное значение датчика
K = P+〈k + 1〉HxT(Hx P++〈k + 1〉HxT + HwŴHwT)-1 // коэффициент
// усиления Калмана
x̂〈k + 1〉 = x̂+〈k + 1〉 + Kv // обновление оценки состояния
P̂〈k + 1〉 = P̂+〈k + 1〉 - KHx P̂+〈k + 1〉 // обновление оценки
// ковариации

Фундаментальная проблема расширенного фильтра Калмана
состоит в том, что плотности распределения случайных величин перестают быть гауссовыми после обработки нелинейными
функциями f(·) и h(·). Это можно проиллюстрировать на примере нелинейной скалярной функции y = (x = 2)2/4. Выберем
миллион случайных значений из нормального распределения
N(5, 4) со средним значением 5 и стандартным отклонением 2:

1046 Приложение H · Фильтр Калмана
>>> x = np.random.normal(loc=5, scale=2, size=(1_000_000,));

Затем применим функцию
>>> у = (х + 2)**2 / 4;

и построим график функции плотности вероятности y:
>>> plt.hist(y, bins=200, density=True, histtype="step");

Он показан на рис. H.3. Как видите, функция распределения
вероятностей y существенно изменилась и больше не является
гауссовой. Она утратила симметрию, и среднее значение стало больше модального. Якобианы, входящие в уравнения EKF,
масштабируют ковариацию, но результирующее негауссово
распределение нарушает предположения, гарантирующие оптимальность оценки фильтра Калмана. Альтернативные подходы к решению проблемы нелинейности системы включают
итерационный алгоритм EKF, описанный в Jazwinski (2007),
сигма-точечный фильтр Калмана (Unscented Kalman Filter, UKF)
(Julier and Uhlmann 2004) или сигма-точечный фильтр, который
использует дискретные точки выборки для аппроксимации
функции распределения вероятностей.

Рис. H.3. Плотность распределения (PDF) состояния x, которая является гауссовой функцией
N(5, 4), и плотность распределения нелинейной функции y = (x + 2)2/4. Распределение y
больше не является гауссовым, а среднее значение не соответствует пику

Приложение

I

	Графы
Граф – это абстрактное представление множества объектов,
связанных ребрами и изображаемое графически, как показано
на рис. I.1. Математически граф обозначается как G(V, E), где
V – вершины или узлы, а E – связи, соединяющие пары вершин
и называемые ребрами, или дугами. Ребра могут быть направленными (стрелки) или ненаправленными, как в данном случае.
Ребра могут иметь собственный вес, т. е. стоимость, связанную
с перемещением из одной вершины в другую. Последовательность ребер из одной вершины в другую называется путем,
а последовательность, начинающаяся и заканчивающаяся в одной вершине, – циклом. Ребро из вершины в себя является петлей. Графы можно использовать для представления маршрутов
транспорта, сетей связи или социальных сетей, а соответствующая область математики называется теорией графов.

Рис. I.1. Пример графа,
сгенерированного
классом UGraph

Иллюстрировать графы мы будем с помощью пакета Python
pgraph, который поддерживает вложенные графы, вершины которых связаны с точками в n-мерном пространстве. Создадим
новый граф:
>>> import pgraph
>>> g = pgraph.UGraph()

1048 Приложение I · Графы

По умолчанию узлы графа существуют в двухмерном пространстве. Добавим в граф пять узлов со случайными координатами:
>>> np.random.seed(0) # чтобы гарантировать повторимость результатов
>>> for i in range(5):
... g.add_vertex(np.random.rand(2));

Метод add_vertex возвращает ссылку на только что добавленный узел, но мы можем обращаться к вершинам с помощью индексов. Например, вершина 1 доступна как
>>> g[1]
UVertex[#1, coord=(0.6028, 0.5449)]

и является экземпляром UVertex, вершиной в неориентированном графе. Она имеет координаты (0.6028, 0.5449), и ей
было присвоено имя по умолчанию "#1", но мы могли бы задать другое имя.
Таким образом, мы можем сослаться на
вершину по ее имени:
>>> g["#1"]
UVertex[#1, coord=(0.6028, 0.5449)]

Мы можем создавать ребра между парами вершин:
>>>
>>>
>>>
>>>
>>>
>>>

g.add_edge(g[0],
g.add_edge(g[0],
g.add_edge(g[0],
g.add_edge(g[1],
g.add_edge(g[1],
g.add_edge(g[3],

g[1]);
g[2]);
g[3]);
g[2]);
g[3]);
g[4]);

Метод add_edge возвращает ссылку на только что созданный
объект ребра. По умолчанию стоимость ребра равна евклидову
расстоянию между вершинами, но это поведение можно переопределить, указав третий аргумент в вызове add_edge.
Сводную информацию о графе можно отобразить вызовом
>>> print(g)
UGraph: 5 vertices, 6 edges, 1 component

Этот граф имеет один компонент, т. е. все вершины объединены в одну сеть. Граф можно нарисовать вызовом (см. рис. I.1)
>>> g.plot()

Вершины отображаются синими кружками, а ребра – линиями, соединяющими вершины. Существует множество парамет­
ров для изменения поведения по умолчанию функции отобра-

Имена вершин
должны быть
уникальными
в пределах графа.

Приложение I · Графы 1049

жения графа. Но имейте в виду, что отобразить можно только
графы в двухмерном и трехмерном пространствах.
Запросим соседей вершины 1:
>>> g[1].adjacent()
[UVertex[#0, coord=(0.5488, 0.7152)],
UVertex[#2, coord=(0.4237, 0.6459)],
UVertex[#3, coord=(0.4376, 0.8918)]]

В ответ мы получили список объектов соседних вершин, соединенных ребрами с вершиной 1. Каждое ребро – это объект,
и мы можем получить список всех ребер, соединяющихся с вершиной 1:
>>> g[1].edges()
[Edge{[#0] -- [#1], cost=0.1786},
Edge{[#1] -- [#2], cost=0.2056},
Edge{[#1] -- [#3], cost=0.3842}]

Причем каждое ребро имеет ссылки на вершины, которые
оно связывает:
>>> g[1].edges()[0].endpoints
[UVertex[#0, coord=(0.5488, 0.7152)],
UVertex[#1, coord=(0.6028, 0.5449)]]

Стоимость, или длина, ребра определяется как
>>> g[1].edges()[0].cost
0.1786

К любому узлу или ребру можно присоединить произвольные данные, использовав для этого атрибуты объектов вершин
и ребер или реализовав свои классы, наследующие классы UVertex и Edge.
Вершина, ближайшая к координате (0.5, 0.5):
>>> g.closest((0.5, 0.5))
(UVertex[#1, coord=(0.6028, 0.5449)], 0.1121)

Путь с минимальной стоимостью между любыми двумя узлами в графе можно вычислить с помощью хорошо известных
алгоритмов, таких как A* (Hart et al. 1968):
>>> path, length, _ = g.path_Astar(g[2], g[4])
>>> path
[UVertex[#2, coord=(0.4237, 0.6459)],
UVertex[#0, coord=(0.5488, 0.7152)],
UVertex[#3, coord=(0.4376, 0.8918)],
UVertex[#4, coord=(0.9637, 0.3834)]]

1050 Приложение I · Графы

В качестве результата возвращается список вершин, представляющих путь с общей длиной:
>>> length
1.083

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

Приложение

J

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

J.1

Одномерный сигнал

Рассмотрим одномерный дискретный сигнал y(k), k ∈ ℕ0 (см.
рис. J.1a):
>>> y = mvtb_load_matfile("data/peakfit.mat")["y"];
>>> plt.plot(y, "-o");

Найти положение пика с точностью до ближайшего целого
числа очень просто, используя функцию argmax из NumPy:
>>> k = np.argmax(y)
7

которая сообщает, что пик приходится на элемент с индексом
k = 7 и имеет значение
>>> y[k]
0.9905
дискретные данные
аппроксимирующая парабола
оценка максимума

а
б
Рис. J.1. Поиск пика: а) сигнал с несколькими локальными максимумами; б) крупный план первых максимумов
с аппроксимирующей параболой

1052 Приложение J · Определение пика

В данном случае имеется более одного пика, и для их поиска
можно использовать функцию findpeaks из пакета Toolbox:
>>> k, ypk = findpeaks(y)
>>> k
array([ 7, 0, 24, 15])
>>> ypk
array([ 0.9905,
0.873,

0.6718, -0.5799])

которая в этом примере вернула четыре максимума, упорядочив их по убыванию величины. Обычным критерием качества
пика является его величина и отношение высоты второго пика
к высоте первого:
>>> ypk[1] / ypk[0]
0.8813

которое называется коэффициентом неоднозначности и в идеа­
ле должно быть небольшим.
Сигнал y(k) на рис. J.1 представляет собой выборочное представление непрерывного базового сигнала y(x), и реальный
пик может фактически находиться между выбранными значениями. Если посмотреть на увеличенный фрагмент сигнала
(рис. J.1б), то можно увидеть, что максимум приходится на k = 7,
значение в точке k = 8 лишь немного ниже, поэтому пик может
находиться где-то между ними. Обычный подход состоит в аппроксимации параболой
y = aδx2 + bδx + c, δx ∈ ℝ,

(J.1)

по точкам, окружающим пик: (-1, yk-1), (0, yk) и (1, yk+1), где δx = 0,
когда k = 7. Подставив эти точки в (J.1), можно записать три
уравнения:
yk-1 = a - b + c;
yk = c;
yk+1 = a + b + c,
или в компактной матричной форме:

а затем найти параболические коэффициенты:
(J.2)

Приложение J · Определение пика 1053

Максимум параболы достигается в точке, где ее производная
равна нулю:
2aδx + b = 0.
Подставляя значения a и b из (J.2), можно найти смещение
вершины аппроксимирующей параболы относительно дискретного максимума

Таким образом, уточненное или интерполированное положение максимума находится в точке
x̂ = k + δx ∈ ℝ,

и расчетное значение максимума получается путем подстановки δx в (J.1).
Коэффициент а, отрицательный для максимумов, указывает
на крутизну пика, что может помочь определить, является ли
пик достаточно острым. Большое значение указывает на четко
выраженный острый пик, тогда как низкое – на очень широкий
пик, для которого оценка уточненного пика может быть не такой точной.
Продолжим предыдущий пример и используем функцию
findpeaks для оценки уточненных позиций пиков:
>>> findpeaks(y, interp=True)
(array([ 7.439,
23.73,
15.24]),
array([ 0.9969, 0.7037, -0.578]))

где аргумент interp указывает, что аппроксимация должна выполняться полиномом второго порядка. Полученная парабола
показана красным на рис. J.1б.
Если сигнал имеет наложенный шум, вероятно, будет несколько пиков, многие из которых весьма незначительны и от
них можно избавиться, указав масштаб пика. Например, пиками, которые превышают все остальные значения в пределах
±5 значений в горизонтальном направлении, являются
>>> findpeaks(y, scale=5)
(array([ 7, 0, 24]), array([ 0.9905,

0.873, 0.6718]))

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

1054 Приложение J · Определение пика

J.2

Двухмерный сигнал

Для иллюстрации приемов работы с двухмерным сигналом загрузите данные, как показано ниже:
>>> img = mvtb_load_matfile("data/peakfit.mat")["image"]
array([[ -0.0696, 0.03483, 0.1394, 0.2436,
0.348],
[
0.08, 0.3754, 0.3202,
0.44,
0.56],
[ 0.04003, 0.1717, 0.3662, 0.4117,
0.52],
[ 0.000214, 0.2062, 0.8766, 0.4462, 0.4802],
[ -0.03997, 0.09166, 0.2862, 0.3317,
0.44],
[
-0.08, 0.04003, 0.1602,
0.28,
0.4]])

с максимальным значением 0.8766 в [3, 2] (если следовать соглашениям об индексировании, принятым в NumPy). Мы можем найти его:
>>> k = np.argmax(img)
17

То есть максимальное значение находится в элементе 17,
если счет вести по строкам. Это можно представить как индексацию по одномерному массиву, в котором строки следуют
друг за другом.
>>> img.ravel()[k]
0.8766

В качестве альтернативы можно преобразовать порядковый
номер элемента в индексы массива:
>>> np.unravel_index(k, img.shape)
(3, 2)

Отыскать все пики можно с помощью функции findpeaks2d из
пакета RVC Toolbox:
>>> xy = findpeaks2d(img)
array([[
2,
3,
[
1,
1,

0.8766],
0.3754]])

которая в данном случае вернула два локальных максимума, по
одному на столбец возвращаемых переменных. Эта функция
вернет все нелокальные максимумы, где размер локальной области задается параметром scale.
Мы можем уточнить оценку двухмерного пика, выполнив
процедуру, аналогичную той, что применялась в одномерном
случае. Для этого аппроксимируем точки парабаллоидом
z = ax2 + by2 + cx + dy + e,

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

Приложение J · Определение пика 1055
Параболоид
обычно имеет
член xy, и в сумме
получается
6 параметров. Для
учета этого члена
потребовалась
бы еще одна
дополнительная
точка, что
нарушает простоту
и симметрию схемы.
Также можно было
бы использовать
всех соседей, что
потребовало бы
использования
метода наименьших
квадратов для
поиска решения.

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

В этом случае коэффициенты a и b представляют остроту
пика в направлениях x и y, а качество пика можно рассматривать как min(|a|, |b|).
В нашем примере мы получаем следующие уточненные пики:
>>> xy = findpeaks2d(img, interp=True)
array([[ 2.109,
2.964, 0.8839, 0.5505],
[ 1.343,
1.126, 0.4003, 0.1753]])

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

Литература
1. Achtelik MW (2014) Advanced closed loop visual navigation for micro aerial vehicles. Ph.D. thesis, ETH Zurich.
2. Adorno B, MarinhoM (2021) DQ Robotics: A Library for Robot Modeling and Control.
IEEE Robotics Automation Magazine, vol. 28, 102–116.
3. Agarwal S, Furukawa Y, Snavely N, Simon I, Curless B, Seitz SM, Szeliski R (2011)
Building Rome in a day. Commun ACM 54(10):105–112.
4. Agarwal P, Burgard W, Stachniss C (2014) Survey of geodetic mapping methods:
Geodetic approaches to mapping and the relationship to graph-based SLAM. IEEE
Robot Autom Mag 21(3):63–80.
5. Agrawal M, Konolige K, Blas M (2008) CenSurE: Center surround extremas for realtime feature detection and matching. In: Forsyth D, Torr P, Zisserman A (eds) Lecture notes in computer science. Computer Vision – ECCV 2008, vol 5305. SpringerVerlag, Berlin Heidelberg, pp 102–115.
6. Albertos P, Mareels I (2010) Feedback and control for everyone. Springer-Verlag,
Berlin Heidelberg1.
7. Altmann SL (1989) Hamilton, Rodrigues, and the quaternion scandal. Math Mag
62(5):291–308.
8. Alton K, Mitchell IM (2006) Optimal path planning under defferent norms in conti­
nuous state spaces. In: Proceedings of the IEEE International Conference on Robo­
tics and Automation (ICRA). pp 866–872.
9. Andersen N, Ravn O, Sørensen A (1993) Real-time vision based control of servomechanical systems. In: Chatila R, Hirzinger G (eds) Lecture notes in control and
information sciences. Experimental Robotics II, vol 190. Springer-Verlag, Berlin Heidelberg, pp 388–402.
10. Andersson RL (1989) Dynamic sensing in a ping-pong playing robot. IEEE T Robotic
Autom 5(6):728–739.
11. Antonelli G (2014) Underwater robots: Motion and force control of vehicle-manipulator systems, 3rd ed. Springer Tracts in Advanced Robotics, vol 2. Springer-Verlag,
Berlin Heidelberg.
12. Arandjelović R, Zisserman A (2012) Three things everyone should know to improve
object retrieval. In: IEEE Conference on Computer Vision and Pattern Recognition
(CVPR). pp 2911–2918.
13. Arkin RC (1999) Behavior-based robotics. MIT Press, Cambridge, Massachusetts.
14. Armstrong WW (1979) Recursive solution to the equations of motion of an N-link
manipulator. In: Proceedings of the 5th World Congress on Theory of Machines and
Mechanisms, Montreal, Jul, pp 1343–1346.
15. Armstrong BS (1988) Dynamics for robot control: Friction modelling and ensuring
excitation during parameter identification. Stanford University.

1

Альбертос П., Мариэлс И. Обратная связь и управление для всех. М.: ДМК-Пресс, 2023.
ISBN 978-5-93700-235-8. – Прим. перев.

Определение пика 1057

16. Armstrong B (1989) On finding exciting trajectories for identification experiments
involving systems with nonlinear dynamics. Int J Robot Res 8(6):28.
17. Armstrong B, Khatib O, Burdick J (1986) The explicit dynamic model and inertial
parameters of the Puma 560 Arm. In: Proceedings of the IEEE International Confe­
rence on Robotics and Automation (ICRA), vol 3. pp 510–518.
18. Armstrong-Hélouvry B, Dupont P, De Wit CC (1994) A survey of models, analysis
tools and compensation methods for the control of machines with friction. Automatica 30(7):1083–1138 .
19. Arun KS, Huang TS, Blostein SD (1987) Least-squares fitting of 2 3-D point sets.
IEEE T Pattern Anal 9(5):699–700.
20. Asada H (1983) A geometrical representation of manipulator dynamics and its application to arm design. J Dyn Syst-T ASME 105:131.
21. Astolfi A (1999) Exponential stabilization of a wheeled mobile robot via discontinuous control. J Dyn Syst-T ASME 121(1):121–126.
22. Azarbayejani A, Pentland AP (1995) Recursive estimation of motion, structure, and
focal length. IEEE T Pattern Anal 17(6):562–575.
23. Bailey T, Durrant-Whyte H (2006) Simultaneous localization and mapping: Part II.
IEEE Robot Autom Mag 13(3):108–117.
24. Bakthavatchalam M, Chaumette F, Tahri O (2015) An improved modelling scheme
for photometric moments with inclusion of spatial weights for visual servoing with
partial appearance/disappearance. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). pp 6037–6043.
25. Baldridge AM, Hook SJ, Grove CI, Rivera G (2009) The ASTER spect­ral library version
2.0. Remote Sens Environ 113(4):711–715.
26. Ball RS (1876) The theory of screws: A study in the dynamics of a rigid body. Hodges,
Foster & Co., Dublin.
27. Ball RS (1908) A treatise on spherical astronomy. Cambridge University Press, New
York.
28. Ballard DH (1981) Generalizing the Hough transform to detect arbitrary shapes.
Pattern Recogn 13(2):111–122.
29. Banks J, Corke PI (2001) Quantitative evaluation of matching methods and validity
measures for stereo vision. Int J Robot Res 20(7):512–532.
30. Barfoot T (2017) State Estimation for Robotics. Cambridge University Press.
31. Bar-Shalom Y, Fortmann T (1988) Tracking and data association. Mathematics in
science and engineering, vol 182. Academic Press, London Oxford.
32. Bar-Shalom Y, Li XR, Kirubarajan T (2001) Estimation with applications to tracking
and navigation. John Wiley & Sons, Inc., Chichester.
33. Bateux Q, Marchand E, Leitner J, Chaumette F, Corke P (2018) Training Deep Neural Networks for Visual Servoing. IEEE International Conference On Robotics And
Auto­mation (ICRA). pp 3307–3314.
34. Bauer J, Sünderhauf N, Protzel P (2007) Comparing several implementations of two
recently published feature detectors. In: IFAC Symposium on Intelligent Autonomous Vehicles (IAV). Toulouse.
35. Bay H, Ess A, Tuytelaars T, Van Gool L (2008) Speeded-up robust features (SURF).
Comput Vis Image Und 110(3):346–359.
36. Benosman R, Kang SB (2001) Panoramic vision: Sensors, theory, and applications.
Springer-Verlag, Berlin Heidelberg.
37. Benson KB (ed) (1986) Television engineering handbook. McGraw-Hill, New York
Berns RS (2019) Billmeyer and Saltz­man’s Principles of Color Technology, 4th ed.
Wiley.

1058 Определение пика

38. Bertolazzi E, Frego M (2014) G1 fitting with clothoids.MathematicalMethods in the
Applied Sciences 38(5):881–897.
39. Bertozzi M, Broggi A, Cardarelli E, Fedriga R, Mazzei L, Porta P (2011) VIAC expedition: Toward autonomous mobility. IEEE Robot Autom Mag 18(3):120–124.
40. Besl PJ, McKay HD (1992) A method for registration of 3-D shapes. IEEE T Pattern
Anal 14(2): 239–256.
41. Bhat DN, Nayar SK (2002) Ordinal measures for image correspondence. IEEE T Pattern Anal 20(4): 415–423.
42. Biber P, Straßer W (2003) The normal distributions transform: A new approach to
laser scan matching. In: Proceedings of the IEEE/RSJ International Conference on
intelligent robots and systems (IROS), vol 3. pp 2743–2748.
43. Bishop CM (2006) Pattern recognition and machine learning. Information science
and statistics. Springer-Verlag, New York.
44. Blewitt M (2011) Celestial navigation for yachtsmen. Adlard Coles Nautical, London
Bolles RC, Baker HH, Marimont DH (1987) Epipolar-plane image analysis: An approach to determining structure from motion. Int J Comput Vision 1(1):7–55, Mar.
45. Bolles RC, Baker HH, HannahMJ (1993) The JISCT stereo eva­lua­tion. In: Image Understanding Workshop: proceedings of a workshop held inWashington, DC apr 18–
21, 1993. Morgan Kaufmann, San Francisco, pp 263.
46. Bolton W (2015) Mechatronics: Electronic control systems in mechanical and electrical engineering, 6th ed. Pearson, Harlow.
47. Borenstein J, Everett HR, Feng L (1996) Navigatingmobile robots: Systems and techniques. AK Peters, Ltd. Natick, MA, USA, Out of print and available at http://wwwpersonal.umich.edu/~johannb/Papers/pos96rep.pdf.
48. Borgefors G (1986) Distance transformations in digital images. Comput Vision
Graph 34(3):344–371.
49. Bostrom N (2016) Superintelligence: Paths, dangers, strategies. Oxford University
Press, Oxford, 432 p.
50. Bouguet J-Y (2010) Camera calibration toolbox for MATLAB®. http://www.vision.
caltech.edu/bouguetj/calib_doc.
51. Brady M, Hollerbach JM, Johnson TL, Lozano-Pérez T, Mason MT (eds) (1982) Robot
motion: Planning and control. MIT Press, Cambridge, Massachusetts.
52. Braitenberg V (1986) Vehicles: Experiments in synthetic psychology. MIT Press,
Cambridge, Massachusetts.
53. Bray H (2014) You are here: From the compass to GPS, the history and future of how
we find ourselves. Basic Books, New York.
54. Brockett RW (1983) Asymptotic stability and feedback stabilization. In: Brockett
RW, Millmann RS, Sussmann HJ (eds) Progress in mathematics. Differential geometric control theory, vol 27. pp 181–191.
55. Broida TJ, Chandrashekhar S, Chellappa R (1990) Recursive 3-D motion estimation
from a monocular image sequence. IEEE T Aero Elec Sys 26(4):639–656.
56. Brooks RA (1986) A robust layered control system for a mobile robot. IEEE T Robotic
Autom 2(1):14–23.
57. Brooks RA (1989) A robot that walks: Emergent behaviors from a carefully evolved
network. MIT AI Lab, Memo 1091.
58. Brown MZ, Burschka D, Hager GD (2003) Advances in computational stereo. IEEE T
Pattern Anal 25(8):993–1008.
59. Brynjolfsson E, McAfee A (2014) The second machine age: Work, progress, and prosperity in a time of brilliant technologies.W.W. Norton & Co., New York.

Определение пика 1059

60. Buehler M, Iagnemma K, Singh S (eds) (2007) The 2005 DARPA grand challenge:
The great robot race. Springer Tracts in Advanced Robotics, vol 36. Springer-Verlag,
Berlin Heidelberg.
61. Buehler M, Iagnemma K, Singh S (eds) (2010) The DARPA urban challenge. Tracts in
Advanced Robotics, vol 56. Springer-Verlag, Berlin Heidelberg.
62. Bukowski R, Haynes LS, Geng Z, Coleman N, Santucci A, Lam K, Paz A, May R, DeVito M (1991) Robot hand-eye coordination rapid prototyping environment. In: Proc
ISIR, pp 16.15–16.28.
63. Buttazzo GC, Allotta B, Fanizza FP (1993) Mousebuster: A robot system for catching
fast moving objects by vision. In: Proceedings of the IEEE International Conference
on Robotics and Auto­mation (ICRA). Atlanta, pp 932–937.
64. Calonder M, Lepetit V, Strecha C, Fua P (2010) BRIEF: Binary robust independent
elementary features. In: Daniilidis K, Maragos P, Paragios N (eds) Lecture notes in
computer science. Computer Vision – ECCV 2010, vol 6311. Springer-Verlag, Berlin
Heidelberg, pp 778–792.
65. Campos C, Elvira R, Rodríguez JJG, Montiel JMM, Tardós JD (2021) ORB-SLAM3:
An Accurate Open-Source Library for Visual, Visual–Inertial, and Multimap SLAM.
IEEE Trans on Robotics, 37(6), pp 1874–1890.
66. Canny JF (1983) Finding edges and lines in images.MIT, Artificial Intelligence Laboratory, AI-TR-720. Cambridge, MA.
67. Canny J (1987) A computational approach to edge detection. In: Fischler MA, Firschein O (eds) Readings in computer vision: Issues, problems, principles, and paradigms. Morgan Kaufmann, San Francisco, pp 184–203.
68. Censi A (2008) An ICP variant using a point-to-line metric. In: Proceedings of the
IEEE International Conference on Robotics and Automation (ICRA). pp 19–25.
69. Chahl JS, Srinivasan MV (1997) Reflective surfaces for panoramic imaging. Appl Optics 31(36):8275–8285.
70. Chaumette F (1990) La relation vision-commande: Théorie et application et des
tâches robotiques. Ph.D. thesis, Université de Rennes 1.
71. Chaumette F (1998) Potential problems of stability and convergence in image-based
and position-based visual servoing. In: Kriegman DJ, Hager GD, Morse AS (eds) Lecture notes in control and information sciences. The confluence of vision and control, vol 237. Springer-Verlag, Berlin Heidelberg, pp 66–78.
72. Chaumette F (2004) Image moments: A general and useful set of features for visual
servoing. IEEE T Robotic Autom 20(4):713–723.
73. Chaumette F, Hutchinson S (2006) Visual servo control 1: Basic approaches. IEEE
Robot Autom Mag 13(4):82–90.
74. Chaumette F, Hutchinson S (2007) Visual servo control 2: Advanced approaches.
IEEE Robot Autom Mag 14(1):109–118.
75. Chaumette F, Rives P, Espiau B (1991) Positioning of a robot with respect to an
object, tracking it and estimating its velocity by visual servoing. In: Proceedings
of the IEEE International Conference on Robotics and Automation (ICRA). Seoul,
pp 2248–2253.
76. Chesi G, Hashimoto K (eds) (2010) Visual servoing via advanced numerical methods.
Lecture notes in computer science, vol 401. Springer-Verlag, Berlin Heidelberg.
77. Chiuso A, Favaro P, Jin H, Soatto S (2002) Structure from motion causally integrated
over time. IEEE T Pattern Anal 24(4):523–535.
78. Choset HM, Lynch KM, Hutchinson S, Kantor G, Burgard W, Kavraki LE, Thrun S
(2005) Principles of robot motion. MIT Press, Cambridge, Massachusetts.

1060 Определение пика

79. Chum O, Matas J (2005) Matching with PROSAC - progressive sample consensus.
IEEE Computer Society Conference On Computer Vision And Pattern Recognition
(CVPR), pp 220–226.
80. Colicchia G, Waltner C, Hopf M,Wiesner H (2009) The scallop’s eye – A concave mirror in the context of biology. Physics Education 44(2):175–179.
81. Collewet C, Marchand E, Chaumette F (2008) Visual servoing set free from image
processing. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). pp 81–86.
82. Commission Internationale de L’Éclairage (1987) Colorimetry, 2nd ed. Commission
Internationale de L’Eclairage, CIE No 15.2.
83. Corke PI (1994) High-performance visual closed-loop robot control. University of
Melbourne, Dept. Mechanical and Manufacturing Engineering. https://hdl.handle.
net/11343/38847.
84. Corke PI (1996a) In situ measurement of robot motor electrical constants. Robotica
14(4):433–436.
85. Corke PI (1996b) Visual control of robots: High-performance visual servoing. Mechatronics, vol 2. Research Studies Press (John Wiley). Out of print and available at
http://www.petercorke.com/bluebook.
86. Corke PI (2001) Mobile robot navigation as a planar visual servoing problem. In:
Jarvis RA, Zelinsky A (eds) Springer tracts in advanced robotics. Robotics Research:
The 10th International Symposium, vol 6. IFRR, Lorne, pp 361–372.
87. Corke PI (2007) A simple and systematic approach to assigning Denavit-Hartenberg
parameters. IEEE T Robotic Autom 23(3):590–594.
88. Corke PI (2010) Spherical image-based visual servo and structure estimation. In:
Proceedings of the IEEE International Conference on Robotics and Automation
(ICRA). Anchorage, pp 5550–5555.
89. Corke PI, Armstrong-Hélouvry BS (1994) A search for consensus among model parameters reported for the PUMA 560 robot. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). San Diego, pp 1608–1613.
90. Corke PI, Armstrong-Hélouvry BS (1995) A meta-study of PUMA 560 dynamics:
A critical appraisal of literature data. Robotica 13(3):253–258.
91. Corke PI, Good MC (1992) Dynamic effects in high-performance visual servoing.
In: Proceedings of the IEEE International Conference on Robotics and Automation
(ICRA). Nice, pp 1838–1843.
92. Corke PI, Good MC (1996) Dynamic effects in visual closed-loop systems. IEEE T
Robotic Autom 12(5):671–683.
93. Corke PI, Haviland J (2021) Not your grandmother’s toolbox – the Robotics Toolbox
reinvented for Python. IEEE International Conference on Robotics and Automation
(ICRA). pp 11357–11363.
94. Corke PI, Hutchinson SA (2001) A new partitioned approach to image-based visual
servo control. IEEE T Robotic Autom 17(4):507–515.
95. Corke PI, Dunn PA, Banks JE (1999) Frame-rate stereopsis using non-parametric
transforms and programmable logic. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Detroit, pp 1928–1933.
96. Corke PI, Lobo J, Dias J (2007) An introduction to inertial and visual sensing. The
International Journal of Robotics Research, 26(6). pp 519–536.
97. Corke PI, Strelow D, Singh S (2004) Omnidirectional visual odometry for a planetary
rover. In: Proceedings of the International Conference on Intelligent Robots and
Systems (IROS). Sendai, pp 4007–4012.

Определение пика 1061

98. Corke PI, Spindler F, Chaumette F (2009) Combining Cartesian and polar coordinates in IBVS. In: Proceedings of the International Conference on Intelligent
Robots and Systems (IROS). St. Louis, pp 5962–5967.
99. Corke PI, Paul R, Churchill W, Newman P (2013) Dealing with shadows: Capturing
intrinsic scene appearance for image-based outdoor localisation. In: Proceedings
of the IEEE/RSJ International.
100. Conference on Intelligent Robots and Systems (IROS). pp 2085–2092.
101. Craig JJ (1986) Introduction to robotics: Mechanics and control, 1st ed. AddisonWesley.
102. Craig JJ (1987) Adaptive control of mechanical manipulators. Addison-Wesley
Longman Publishing Co., Inc. Boston.
103. Craig JJ (2005) Introduction to robotics: Mechanics and control, 3rd ed. Pearson/
Prentice Hall1.
104. Craig JJ, Hsu P, Sastry SS (1987) Adaptive control ofmechanicalmanipulators. Int
J Robot Res 6(2):16–28.
105. Crombez N, Caron G, Mouaddib EM (2015) Photometric Gaussian mixtures based
visual servoing. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). pp 5486–5491.
106. Crone RA (1999) A history of color: The evolution of theories of light and color.
Kluwer Academic, Dordrecht.
107. Cummins M, Newman P (2008) FAB-MAP: Probabilistic localization and mapping
in the space of appearance. Int J Robot Res 27(6):647.
108. Cutting JE (1997) How the eye measures reality and virtual reality. Behav Res Meth
Ins C 29(1):27– 36.
109. DaniilidisK, Klette R (eds) (2006) Imaging beyond the pinhole camera. Computational Imaging, vol 33. Springer-Verlag, Berlin Heidelberg.
110. Dansereau DG (2014) Plenoptic signal processing for robust vision in field robo­
tics. Ph.D. thesis, The University of Sydney.
111. Davies ER (2017) Computer Vision: Principles, Algorithms, Applications, Learning
5th ed. Academic Press.
112. Davison AJ, Reid ID, Molton ND, Stasse O (2007) MonoSLAM: Real-time single
camera SLAM. IEEE T Pattern Anal 29(6):1052–1067.
113. Deguchi K (1998) Optimal motion control for image-based visual servoing by decoupling translation and rotation. In: Proceedings of the International Conference
on Intelligent Robots and Systems (IROS). Victoria, Canada, pp 705–711.
114. Dellaert F, Kaess M (2006) Square root SAM: Simultaneous localization and mapping via square root information smoo­thing. Int J Robot Res 25(12):1181–1203.
115. DeMenthon D, Davis LS (1992) Exact and approximate solutions of the perspective-three-point problem. IEEE T Pattern Anal 14(11):1100–1105.
116. Denavit J, Hartenberg RS (1955) A kinematic notation for lo­wer-pair mechanisms
based on matrices. J Appl Mech-T ASME 22(1):215–221.
117. Deo AS, Walker ID (1995) Overview of damped least-squares methods for inverse
kinematics of robot manipulators. J Intell Robot Syst 14(1):43–68.
118. Deriche R, Giraudon G (1993) A computational approach for corner and vertex detection. Int J Comput Vision 10(2):101–124.
1

Джон Крейг. Введение в робототехнику. Механика и управление. Изд. «Институт компьютерных исследований», 2013. ISBN: 978-5-43440-164-7. – Прим. перев.

1062 Определение пика

119. Diankov R (2010) Automated Construction of Robotic Manipulation Programs.
PhD thesis, Carnegie Mellon University, Robotics Institute, CMU-RI-TR-10-29.
120. Dickmanns ED (2007) Dynamic vision for perception and control of motion.
Springer-Verlag, London.
121. Dickmanns ED, Graefe V (1988a) Applications of dynamic monocular machine vision. Mach Vision Appl 1:241–261.
122. Dickmanns ED, Graefe V (1988b) Dynamic monocular machine vision. Mach Vision
Appl 1(4):223–240.
123. Dickmanns ED, Zapp A (1987) Autonomous high speed road vehicle guidance by
computer vision. In: Tenth Triennial World Congress of the International Federation of Automatic Control, vol 4. Munich, pp 221–226.
124. Dijkstra EW (1959) A note on two problems in connexion with graphs. Numer Math
1(1):269–271.
125. Dougherty ER, Lotufo RA (2003) Hands-on morphological ima­ge processing. Socie­
ty of Photo-Optical Instrumentation Engineers (SPIE).
126. Dubins LE (1957). On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents. American
Journal of Ma­thematics, 79(3), 497–516.
127. Duda RO, Hart PE (1972) Use of the Hough transformation to detect lines and curves
in pictures. Commun ACM 15(1):11–15.
128. Durrant-Whyte H, Bailey T (2006) Simultaneous localization and mapping: Part I.
IEEE Robot Autom Mag 13(2):99–110.
129. Espiau B, Chaumette F, Rives P (1992) A new approach to visual servoing in robo­
tics. IEEE T Robotic Autom 8(3):313–326.
130. Everett HR (1995) Sensors for mobile robots: Theory and application. AK Peters
Ltd.,Wellesley.
131. Faugeras OD (1993) Three-dimensional computer vision: A geometric viewpoint.
MIT Press, Cambridge, Massachusetts.
132. Faugeras OD, Lustman F (1988) Motion and structure from motion in a piecewise
planar environment. Int J Pattern Recogn 2(3):485–508.
133. Faugeras O, Luong QT, Papadopoulou T (2001) The geometry of multiple images:
The laws that govern the formation of images of a scene and some of their applications. MIT Press, Cambridge, Massachusetts.
134. Featherstone R (1987) Robot dynamics algorithms. Springer.
135. Featherstone R (2010a) A Beginner’s Guide to 6-D Vectors (Part 1). IEEE Robotics
Automation Magazine, vol. 17, 83–94.
136. Featherstone R (2010b) A Beginner’s Guide to 6-D Vectors (Part 2). IEEE Robotics
Automation Magazine, vol. 17, 88–99.
137. Feddema JT (1989) Real time visual feedback control for hand-eye coordinated robotic systems. Purdue University.
138. Feddema JT, Mitchell OR (1989) Vision-guided servoing with feature-based trajectory generation. IEEE T Robotic Autom 5(5):691–700.
139. Feddema JT, Lee CSG, Mitchell OR (1991) Weighted selection of image features for
resolved rate visual feedback control. IEEE T Robotic Autom 7(1):31–47.
140. Felzenszwalb PF, Huttenlocher DP (2004) Efficient graph-based image segmentation. Int J Comput Vision 59(2):167–181.
141. Ferguson D, Stentz A (2006) Using interpolation to improve path planning: The
Field Dalgorithm. J Field Robotics 23(2):79–101.

Определение пика 1063

142. Fischler MA, Bolles RC (1981) Random sample consensus: A paradigm for model
fitting with applications to image analysis and automated cartography. Commun
ACM 24(6):381–395.
143. Flusser J (2000) On the independence of rotation moment invariants. Pattern
Recogn 33(9):1405–1410.
144. Fomena R, Chaumette F (2007) Visual servoing from spheres using a spherical projection model. In: Proceedings of the IEEE International Conference on Robotics
and Automation (ICRA). Rome, pp 2080–2085.
145. Ford M (2015) Rise of the robots: Technology and the threat of a jobless future. Basic
Books, New York Förstner W (1994) A framework for low level feature extraction. In:
Ecklundh J-O (ed) Lecture notes in computer science. Computer Vision – ECCV 1994,
vol 800. Springer-Verlag, Berlin Heidelberg, pp 383–394.
146. Förstner W, Gülch E (1987) A fast operator for detection and precise location of
distinct points, corners and centres of circular features. In: ISPRS Intercommission
Workshop. Interlaken, pp 149–155.
147. Forsyth DA, Ponce J (2012) Computer vision: A modern approach, 2nd ed. Pearson,
London.
148. Fraundorfer F, Scaramuzza D (2012) Visual odometry: Part II – Matching, robustness, optimization, and applications. IEEE Robot Autom Mag 19(2):78–90.
149. Freeman H (1974) Computer processing of line-drawing ima­ges. ACM Comput
Surv 6(1):57–97.
150. Friedman DP, Felleisen M, Bibby D (1987) The little LISPer. MIT Press, Cambridge,
Massachusetts.
151. Frisby JP, Stone JV (2010) Seeing: The Computational Approach to Biological Vision. MIT Press.
152. Fu KS, Gonzalez RC, Lee CSG (1987) Robotics: Control, Sensing, Vision, and Intelligence.McGraw-Hill.
153. Funda J, Taylor RH, Paul RP (1990) On homogeneous transforms, quaternions, and
computational efficiency. IEEE T Robotic Autom 6(3):382–388.
154. Gans NR, Hutchinson SA, Corke PI (2003) Performance tests for visual servo control systems, with application to partitioned approaches to visual servo control. Int
J Robot Res 22(10–11):955.
155. Garg R, Kumar B, Carneiro G, Reid I (2016) Unsupervised CNN for Single View
Depth Estimation: Geometry to the Rescue. ECCV.
156. Gautier M, Khalil W (1992) Exciting trajectories for the identification of base inertial parameters of robots. Int J Robot Res 11(4):362.
157. Geiger A, Roser M, Urtasun R (2010) Efficient large-scale stereo matching. In: Kimmel R, Klette R, Sugimoto A (eds) Computer vision – ACCV 2010: 10th Asian Conference on Computer Vision, Queenstown, New Zealand, November 8–12, 2010,
revised selected papers, part I. Springer-Verlag, Berlin Heidelberg, pp 25–38.
158. Geraerts R, Overmars MH (2004) A comparative study of probabilistic roadmap
planners. In: Boissonnat J-D, Burdick J, Goldberg K, Hutchinson S (eds) Springer
tracts in advanced robotics. Algorithmic Foundations of Robotics V, vol 7. Sprin­gerVerlag, Berlin Heidelberg, pp 43–58.
159. Gevers T, Gijsenij A, van deWeijer J, Geusebroek J-M (2012) Color in computer vision: Fundamentals and applications. John Wiley & Sons, Inc., Chichester.
160. Geyer C, Daniilidis K (2000) A unifying theory for central panoramic systems and
practical implications. In: Vernon D (ed) Lecture notes in computer science. Computer vision – ECCV 2000, vol 1843. Springer-Verlag, Berlin Heidelberg, pp 445–461.

1064 Определение пика

161. Glover A, Maddern W, Warren M, Reid S, Milford M, Wyeth G (2012) OpenFABMAP:
An open source toolbox for appearance-based loop closure detection. In: Procee­
dings of the IEEE International Conference on Robotics and Automation (ICRA).
pp 4730–4735.
162. Gonzalez RC, Woods RE (2018) Digital image processing, 4th ed. Pearson Grassia
FS (1998) Practical parameterization of rotations using the exponential map. Journal of Graphics Tools 3(3):29–48.
163. Gregory RL (1997) Eye and brain: The psychology of seeing. Princeton University
Press, Princeton, New Jersey.
164. Grey CGP (2014) Humans need not apply. YouTube video, www.youtube.com/
watch?v=7Pq-S557XQU.
165. Grisetti G, Kümmerle R, Stachniss C, Burgard W (2010) A Tutorial on Graph-Based
SLAM. IEEE Intelligent Transportation Systems Magazine 2(4). pp 31–43.
166. Groves PD (2013) Principles of GNSS, inertial, and multisensor integrated navigation systems, 2nd ed. Artech House, Norwood, USA.
167. Hager GD, Toyama K (1998) X Vision: A portable substrate for real-time vision app­
lications. Comput Vis Image Und 69(1):23–37.
168. Hamel T, Mahony R (2002) Visual servoing of an under-actua­ted dynamic rigidbody system: An image based approach. IEEE T Robotic Autom 18(2):187–198.
169. Hamel T, Mahony R, Lozano R, Ostrowski J (2002) Dynamic modelling and configuration stabilization for an X4-flyer. IFAC World Congress 1(2), p 3.
170. Hansen P, Corke PI, Boles W (2010) Wide-angle visual feature matching for outdoor
localization. Int J Robot Res 29(1–2):267–297.
171. Harris CG, Stephens MJ (1988) A combined corner and edge detector. In: Procee­
dings of the Fourth Alvey Vision Conference. Manchester, pp 147–151.
172. Hart PE (2009) How the Hough transform was invented [DSP history]. IEEE Signal
ProcMag 26(6):18–22.
173. Hart PE, Nilsson NJ, Raphael B (1968) A Formal Basis for the Heuristic Determination of Minimum Cost Paths. IEEE Trans Systems Science and Cybernetics,
4(2):100–107.
174. Hartenberg RS, Denavit J (1964) Kinematic synthesis of lin­kages. McGraw-Hill,
New York.
175. Hartley R, Zisserman A (2003) Multiple view geometry in computer vision. Cambridge University Press, New York.
176. Hashimoto K (ed) (1993) Visual servoing. In: Robotics and auto­mated systems,
vol 7. World Scientific, Singapore.
177. Hashimoto K, Kimoto T, Ebine T, Kimura H (1991) Manipulator control with imagebased visual servo. In: Proceedings of the IEEE International Conference on Robo­
tics and Automation (ICRA). Seoul, pp 2267–2272.
178. Heikkila J, and Silven O (1997) A four-step camera calibration procedure with imp­
licit image correction, Proc IEEE Computer Society Conference on Computer Vision and Pattern Recognition (CVPR), pp 1106–1112.
179. Herschel W (1800) Experiments on the refrangibility of the invisible rays of the
sun. Phil Trans R Soc Lond 90:284–292.
180. Hill J, Park WT (1979) Real time control of a robot with a mobile camera. In: Proceedings of the 9th ISIR, SME. Washington, DC. Mar, pp 233–246.
181. Hirata T (1996) A unified linear-time algorithm for computing distance maps. Inform Process Lett 58(3):129–133.

Определение пика 1065

182. Hirschmüller H (2008) Stereo processing by semiglobalmatching and mutual
information. IEEE Transactions on Pattern Analysis and Machine Intelligence
30(2):328–341.
183. Hirt C, Claessens S, Fecher T, Kuhn M, Pail R, Rexer M (2013) New ultrahigh-resolution picture of Earth’s gravity field. Geophys Res Lett 40:4279–4283.
184. Hoag D (1963) Consideration of Apollo IMU gimbal lock. MIT Instrumentation
Laboratory, E–1344, https://www.hq.nasa.gov/alsj/e-1344.htm.
185. Holland O (2003) Exploration and high adventure: the legacy of GreyWalter. Philosophical Trans of the Royal Society of London. Series A: Mathematical, Physical
and Engineering Sciences, 361(1811).
186. Hollerbach JM (1980) A recursive Lagrangian formulation of manipulator dynamics and a comparative study of dynamics formulation complexity. IEEE T Syst Man
Cyb 10(11):730–736, Nov.
187. Hollerbach JM (1982) Dynamics. In: Brady M, Hollerbach JM, Johnson TL, LozanoPérez T, Mason MT (eds) Robot motion – Planning and control. MIT Press, Cambridge, Massachusetts, pp 51–71.
188. Horaud R, Canio B, Leboullenx O (1989) An analytic solution for the perspective
4-point problem. Comput Vision Graph 47(1):33–44.
189. Horn BKP (1987) Closed-form solution of absolute orientation using unit quaternions. J Opt Soc Am A 4(4):629–642.
190. Horn BKP, Hilden HM, Negahdaripour S (1988) Closed-form solution of absolute
orientation using orthonormal matrices. J Opt Soc Am A 5(7):1127–1135.
191. Hosoda K, Asada M (1994) Versatile visual servoing without knowledge of true Jacobian. In: Proceedings of the International Conference on Intelligent Robots and
Systems (IROS). Munich, pp 186–193.
192. Howard TM, Green CJ, Kelly A, Ferguson D (2008) State space sampling of feasible
motions for high performance mobile robot navigation in complex environments.
J Field Robotics 25(6–7):325–345.
193. Hu MK (1962) Visual pattern recognition by moment invariants. IRE T Inform Theor 8:179–187.
194. Hua M-D, Ducard G, Hamel T, Mahony R, Rudin K (2014) Implementation of a nonlinear attitude estimator for aerial robotic vehicles. IEEE T Contr Syst T 22(1):
201–213.
195. Huang TS, Netravali AN (1994) Motion and structure from feature correspondences: A review. P IEEE 82(2):252–268.
196. Humenberger M, Zinner C, Kubinger W (2009) Performance evaluation of a census-based stereomatching algorithm on embedded and multi-core hardware. In:
Proceedings of the 19th International Symposium on Image and Signal Proces­sing
and Analysis (ISPA), pp 388–393.
197. Hunt RWG (1987) The reproduction of colour, 4th ed. Fountain Press, Tolworth.
198. Hunter RS, Harold RW (1987) The measurement of appearance. John Wiley & Sons,
Inc., Chichester.
199. Hutchinson S, Hager G, Corke PI (1996) A tutorial on visual servo control. IEEE T
Robotic Autom 12(5):651–670.
200. Huynh DQ (2009) Metrics for 3D Rotations: Comparison and Analysis. J Math Imaging Vis 35, pp 155–164.
201. Ings S (2008) The Eye: A Natural History. Bloomsbury Publi­shing.
202. Iwatsuki M, Okiyama N (2002a) A new formulation of visual servoing based on cylindrical coordinate system with shiftable origin. In: Proceedings of the International
Conference on Intelligent Robots and Systems (IROS). Lausanne, pp 354–359.

1066 Определение пика

203. Iwatsuki M, Okiyama N (2002b) Rotation-oriented visual servoing based on cylindrical coordinates. In: Proceedings of the IEEE International Conference on Robo­
tics and Automation (ICRA). Washington, DC, May, pp 4198–4203.
204. Izaguirre A, Paul RP (1985) Computation of the inertial and gravitational coefficients of the dynamics equations for a robot manipulator with a load. In: Procee­
dings of the IEEE International Conference on Robotics and Automation (ICRA).
Mar, pp 1024–1032.
205. Jägersand M, Fuentes O, Nelson R (1996) Experimental eva­luation of uncalibrated visual servoing for precision manipu­lation. In: Proceedings of the IEEE International Conferen­ce on Robotics and Automation (ICRA). Albuquerque, NM,
pp 2874–2880.
206. Jarvis RA, Byrne JC (1988) An automated guided vehicle with map building and
path finding capabilities. In: Robotics Research: The Fourth international symposium. MIT Press, Cambridge, Massachusetts, pp 497–504.
207. Jazwinski AH (2007) Stochastic processes and filtering theory. Dover Publications,
Mineola.
208. Jebara T, Azarbayejani A, Pentland A (1999) 3D structure from 2D motion. IEEE
Signal Proc Mag 16(3):66–84.
209. Julier SJ, Uhlmann JK (2004) Unscented filtering and nonlinear estimation. P IEEE
92(3):401–422.
210. Kaehler A, Bradski G (2017) Learning OpenCV 3: Computer vision in C++ with the
OpenCV library. O’Reilly & Associates, Köln.
211. Kaess M, Ranganathan A, Dellaert F (2007) iSAM: Fast incremental smoothing and
mapping with efficient data association. In: Proceedings of the IEEE International
Conference on Robotics and Automation (ICRA). pp 1670–1677.
212. Kahn ME (1969) The near-minimum time control of open-loop articulated kinematic linkages. Stanford University, AIM-106.
213. Kálmán RE (1960) A new approach to linear filtering and prediction problems.
J Basic Eng-T Asme 82(1):35–45.
214. Kane TR, Levinson DA (1983) The use of Kane’s dynamical equations in robotics.
Int J Robot Res 2(3):3–21.
215. Karaman S, Walter MR, Perez A, Frazzoli E, Teller S (2011) Anytime motion planning using the RRT*. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). pp 1478–1483.
216. Kavraki LE, Svestka P, Latombe JC, Overmars MH (1996) Probabilistic roadmaps for
path planning in high-dimensional configuration spaces. IEEE T Robotic Autom
12(4):566–580.
217. Kelly R (1996) Robust asymptotically stable visual servoing of planar robots. IEEE
T Robotic Autom 12(5):759–766.
218. Kelly A (2013) Mobile robotics: Mathematics,models, and methods. Cambridge
University Press, New York.
219. Kelly R, Carelli R, Nasisi O, Kuchen B, Reyes F (2002a) Stable visual servoing of
camera-in-hand robotic systems. IEEE-ASME T Mech 5(1):39–48.
220. Kelly R, Shirkey P, Spong MW (2002b) Fixed-camera visual servo control for planar robots. In: Proceedings of the IEEE International Conference on Robotics and
Auto­mation (ICRA). Washington, DC, pp 2643–2649.
221. Kenwright K (2012) Dual-Quaternions: From Classical Mechanics to Computer
Graphics and Beyond. https://xbdev.net/misc_demos/demos/dual_quaternions_
beyond/paper.pdf.

Определение пика 1067

222. Khalil W, Creusot D (1997) SYMORO+: A system for the symbolic modelling of
robots. Robotica 15(2):153–161.
223. Khalil W, Dombre E (2002) Modeling, identification and control of robots. Kogan
Page Science, London Khatib O (1987) A unified approach for motion and force
control of robot manipulators: The operational space formulation. IEEE T Robotic
Autom 3(1):43–53.
224. King-Hele D (2002) Erasmus Darwin’s improved design for steering carriages and
cars. Notes and Records of the Royal Society of London 56(1):41–62.
225. Klafter RD, Chmielewski TA, Negin M (1989) Robotic engineering – An integrated
approach. Prentice Hall, Upper Saddle River, New Jersey.
226. Klein CA, Huang CH (1983) Review of pseudoinverse control for use with kinematically redundant manipulators. IEEE T Syst Man Cyb 13:245–250.
227. Klein G, Murray D (2007) Parallel tracking and mapping for small AR workspaces.
In: Sixth IEEE and ACM International Symposium on Mixed and Augmented Reality (ISMAR 2007), pp 225–234.
228. Klette R, Kruger N, Vaudrey T, Pauwels K, van Hulle M, Morales S, Kandil F,
Haeusler R, Pugeault N, Rabe C (2011) Performance of correspondence algorithms
in vision-based driver assistance using an online image sequence database. IEEE
T Veh Technol 60(5):2012–2026.
229. Klette R (2014) Concise Computer Vision: An Introduction into Theory and Algorithms. Springer Koenderink JJ (1984) The structure of images. Biol Cybern
50(5):363–370.
230. Koenderink JJ (2010) Color for the sciences. MIT Press, Cambridge, Massachusetts.
231. Koenig S, Likhachev M (2005) Fast replanning for navigation in unknown terrain.
IEEE T Robotic Autom 21(3):354–363.
232. Krajník T, Cristóforis P, Kusumam K, Neubert P, Duckett T (2017) Image features
for visual teach-and-repeat navigation in changing environments. Robotics and
Autonomous Systems. 88 pp 127–141.
233. Kriegman DJ, Hager GD, Morse AS (eds) (1998) The confluence of vision and cont­
rol. Lecture notes in control and information sciences, vol 237. Springer-Verlag,
Berlin Heidelberg.
234. Kuipers JB (1999) Quaternions and rotation sequences: A primer with applications
to orbits, aerospace and virtual rea­lity. Princeton University Press, Princeton, New
Jersey.
235. Kümmerle R, Grisetti G, Strasdat H, Konolige K, Burgard W (2011) g2o: A general
framework for graph optimization. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pp 3607–3613.
236. Lam O, Dayoub F, Schulz R, Corke P (2015) Automated topometric graph generation
from floor plan analysis. In: Proceedings of the AustralasianConference on Robotics
and Automation. Australasian Robotics and Automation Association (ARAA).
237. Lamport L (1994) LATEX: A document preparation system. User’s guide and reference manual. Addison-Wesley Publishing Company, Reading.
238. Land EH, McCann J (1971) Lightness and retinex theory. J Opt Soc Am A 61(1):1–11.
239. Land MF, Nilsson D-E (2002) Animal eyes. Oxford University Press, Oxford.
240. LaValle SM (1998) Rapidly-exploring random trees: A new tool for path planning.
Computer Science Dept., Iowa State University, TR 98–11.

1068 Определение пика

241. LaValle SM (2006) Planning algorithms. Cambridge University Press, New York.
242. LaValle SM (2011a) Motion planning: The essentials. IEEE Robot Autom Mag
18(1):79–89.
243. LaValle SM (2011b) Motion planning: Wild frontiers. IEEE Robot Autom Mag
18(2):108–118.
244. LaValle SM, Kuffner JJ (2001) Randomized kinodynamic planning. Int J Robot Res
20(5):378–400.
245. Laussedat A (1899) La métrophotographie. Enseignement supérieur de la photographie. Gauthier-Villars, 52 p.
246. Leavers VF (1993) Which Hough transform? Comput Vis Image Und 58(2):250–264.
247. Lee CSG, Lee BH, Nigham R (1983) Development of the generalized D’Alembert
equations of motion for mechanical manipulators. In: Proceedings of the 22nd
CDC, San Antonio, Texas. pp 1205–1210.
248. Lepetit V, Moreno-Noguer F, Fua P (2009) EPnP: An accurate O(n) solution to the
PnP problem. Int J Comput Vision 81(2):155–166.
249. Li H, Hartley R (2006) Five-point motion estimation made easy. In: 18th International Conference on Pattern Recognition ICPR 2006. Hong Kong, pp 630–633.
250. Li Y, Jia W, Shen C, van den Hengel A (2014) Characterness: An indicator of text in
the wild. IEEE T Image Process 23(4):1666–1677.
251. Li T, Bolic M, Djuric P (2015) Resampling methods for particle filtering: Classification, implementation, and strategies. IEEE Signal Proc Mag 32(3):70–86.
252. Lin Z, Zeman V, Patel RV (1989) On-line robot trajectory planning for catching
a moving object. In: Proceedings of the IEEE International Conference on Robotics
and Automation (ICRA), pp 1726–1731.
253. Lindeberg T (1993) Scale-space theory in computer vision. Springer-Verlag, Berlin
Heidelberg Lipkin H (2005) A Note on Denavit-Hartenberg Notation in Robotics.
Proceedings of the 29th ASME Mechanisms and Robotics Conference, pp 921–926.
254. Lloyd J, Hayward V (1991) Real-time trajectory generation using blend functions.
In: Proceedings of the IEEE International Conference on Robotics and Automation
(ICRA). Seoul, pp 784–789.
255. Longuet-Higgins H (1981) A computer algorithm for reconstruction of a scene from
two projections. Nature 293:133–135.
256. Lovell J, Kluger J (1994) Apollo 13. Coronet Books.
257. Lowe DG (1991) Fitting parametrized three-dimensional mo­dels to images. IEEE
T Pattern Anal 13(5): 441–450.
258. Lowe DG (2004) Distinctive image features from scale-invariant keypoints. Int
J Comput Vision 60(2):91–110.
259. Lowry S, Sunderhauf N, Newman P, Leonard J, Cox D, Corke P, Milford M (2015)
Visual place recognition: A survey. Robotics, IEEE Transactions on (99):1–19.
260. Lu F, Milios E (1997) Globally consistent range scan alignment for environment
mapping. Auton Robot 4:333–349.
261. Lucas SM (2005) ICDAR 2005 text locating competition results. In: Proceedings
of the Eighth International Conference on Document Analysis and Recognition,
ICDAR05. pp 80–84.
262. Lucas BD, Kanade T (1981) An iterative image registration technique with an application to stereo vision. In: International joint conference on artificial intel-

Определение пика 1069

263.
264.
265.
266.
267.
268.
269.
270.
271.
272.

273.

274.
275.
276.

277.

278.

279.
280.

ligence (IJCAI), Vancouver, vol 2. https://www.ijcai.org/Past/%20Proceedings/
IJCAI-81-VOL-2/PDF/017.pdf, pp 674–679.
Luh JYS, Walker MW, Paul RPC (1980) On-line computational scheme for mechanical manipulators. J Dyn Syst-T ASME 102(2):69–76.
Lumelsky V, Stepanov A (1986) Dynamic path planning for a mobile automaton with
limited information on the environment. IEEE T Automat Contr 31(11):1058–1063.
Luo W, Schwing A, Urtasun R (2016) Efficient Deep Learning for Stereo Matching.
IEEE Conference On Computer Vision and Pattern Recognition (CVPR).
Luong QT (1992) Matrice fondamentale et autocalibration en vision par ordinateur. Ph.D. thesis, Université de Paris-Sud, Orsay, France.
Lynch KM, Park FC (2017) Modern robotics: Mechanics, planning, and control.
Cambridge University Press, New York.
Ma Y, Kosecka J, Soatto S, Sastry S (2003) An invitation to 3D. Springer-Verlag,
Berlin Heidelberg.
Ma J, Zhao J, Tian J, Yuille A, Tu Z (2014) Robust Point Mat­ching via Vector Field
Consensus, IEEE Trans on Image Processing, 23(4), pp 1706–1721.
Maddern W, Pascoe G, Linegar C, Newman P (2016) 1 Year, 1000km: The Oxford
RobotCar Dataset. The International Journal of Robotics Research 36(1). pp 3–15.
Magnusson M, Lilienthal A, Duckett T (2007) Scan registration for autonomous
mining vehicles using 3D-NDT. J Field Robo­tics 24(10):803–827.
Magnusson M, Nuchter A, Lorken C, Lilienthal AJ, Hertzberg J (2009) Evaluation of
3D registration reliability and speed – A comparison of ICP and NDT. In: Procee­
dings of the IEEE International Conference on Robotics and Automation (ICRA).
pp 3907–3912.
Mahony R, Corke P, Chaumette F (2002) Choice of image features for depth-axis
control in image based visual servo control. IEEE/RSJ International Conference On
Intelligent Robots and Systems, pp 390–395 vol.1.
Mahony R, Kumar V, Corke P (2012) Multirotor aerial vehicles: Modeling, estimation, and control of quadrotor. IEEE Robot Autom Mag (19):20–32.
Maimone M, Cheng Y, Matthies L (2007) Two years of visual odometry on theMars
exploration rovers. J Field Robotics 24(3):169–186.
Makhlin AG (1985) Stability and sensitivity of servo vision systems. In: Proc 5th International Conference on Robot Vision and Sensory Controls – RoViSeC 5. IFS
(Publications), Amsterdam, pp 79–89.
Malis E (2004) Improving vision-based control using efficient second-order minimization techniques. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). pp 1843–1848.
Malis E, Vargas M (2007) Deeper understanding of the homography decomposition
for vision-based control. Research Report, RR-6303, Institut National de Recherche en Informatique et en Automatique (INRIA), 90 p, https://hal.inria.fr/inria00174036v3/document.
Malis E, Chaumette F, Boudet S (1999) 2-1/2D visual servoing. IEEE T Robotic
Autom­15(2):238–250.
Marey M, Chaumette F (2008) Analysis of classical and new visual servoing control
laws. In: Proceedings of the IEEE International Conference on Robotics and Automation (ICRA). Pasadena, pp 3244–3249.

1070 Определение пика

281. Mariottini GL, Prattichizzo D (2005) EGT for multiple view geometry and visual
servoing: Robotics vision with pinhole and panoramic cameras. IEEE T Robotic
Autom 12(4):26–39.
282. Mariottini GL, Oriolo G, Prattichizzo D (2007) Image-based visual servoing for
nonholonomic mobile robots using epipolar geometry. IEEE T Robotic Autom
23(1):87–100.
283. Marr D (2010) Vision: A computational investigation into the human representation and processing of visual information. MIT Press, Cambridge, Massachusetts.
284. Martin D, Fowlkes C, Tal D, Malik J (2001) A database of human segmented natural
images and its application to evaluating segmentation algorithms and measuring
ecological statistics. Proceedings of the 8th International Conference on Computer
Vision, vol 2. pp 416–423.
285. Martins FN, Celeste WC, Carelli R, Sarcinelli-Filho M, Bastos-Filho TF (2008) An
adaptive dynamic controller for autonomous mobile robot trajectory tracking.
Control Eng Pract 16(11):1354–1363.
286. Masutani Y, Mikawa M, Maru N, Miyazaki F (1994) Visual servoing for non-holonomic mobile robots. In: Proceedings of the International Conference on Intelligent Robots and Systems (IROS). Munich, pp 1133–1140.
287. Matarić MJ (2007) The robotics primer. MIT Press, Cambridge, Massachusetts.
288. Matas J, Chum O, Urban M, Pajdla T (2004) Robust wide-baseline stereo from maximally stable extremal regions. Image Vision Comput 22(10):761–767.
289. Matthews ND, An PE, Harris CJ (1995) Vehicle detection and recognition for autonomous intelligent cruise control. Technical Report, University of Southampton.
290. Matthies L (1992) Stereo vision for planetary rovers: Stochastic modeling to near
real-time implementation. Int J Comput Vision 8(1):71–91.
291. Mayeda H, Yoshida K, Osuka K (1990) Base parameters of manipulator dynamic
models. IEEE T Robotic Autom 6(3):312–321.
292. McGee LA, Schmidt SF (1985) Discovery of the Kalman filter as a Practical Tool
for Aerospace and Industry. NASA-TM-86847.
293. McGlone C (ed) (2013) Manual of photogrammetry, 6th ed. American Society of
Photogrammetry McLauchlan PF (1999) The variable state dimension filter applied to surface-based structure from motion. University of Surrey, VSSP-TR-4/99.
294. Merlet JP (2006) Parallel robots. Kluwer Academic, Dordrecht.
295. Mettler B (2003) Identification modeling and characteristics of miniature rotorcraft. Kluwer Academic, Dordrecht.
296. Micušík B, Pajdla T (2003) Estimation of omnidirectional camera model from epipolar
geometry. In: IEEE Conference on Compu­ter Vision and Pattern Recognition, vol 1.
Madison, pp 485–490.
297. Middleton RH, Goodwin GC (1988) Adaptive computed torque control for rigid link
manipulations. Syst Control Lett 10(1):9–16.
298. Mikolajczyk K, Schmid C (2004) Scale and affine invariant interest point detectors.
Int J Comput Vision 60(1):63–86.
299. Mikolajczyk K, Schmid C (2005) A performance evaluation of local descriptors.
IEEE T Pattern Anal 27(10):1615–1630.
300. Mindell DA (2008) Digital Apollo. MIT Press, Cambridge, Massachusetts.
301. Molton N, Brady M (2000) Practical structure and motion from stereo when motion
is unconstrained. Int J Comput Vision 39(1):5–23.
302. Montemerlo M, Thrun S (2007) FastSLAM: A scalable method for the simultaneous localization and mapping problem in robotics, vol 27. Springer-Verlag, Berlin
Heidelberg.

Определение пика 1071

303. Montemerlo M, Thrun S, Koller D, Wegbreit B (2003) FastSLAM2.0: An improved
particle filtering algorithm for simultaneous localization and mapping that pro­
vably converges. In: Proceedings of the 18th International Joint Conference on
Artificial Intelligence. Morgan Kaufmann, San Francisco, pp 1151–1156.
304. Moravec H (1980) Obstacle avoidance and navigation in the real world by a seeing
robot rover. Ph.D. thesis, Stanford University.
305. Morel G, Liebezeit T, Szewczyk J, Boudet S, Pot J (2000) Explicit incorporation of 2D
constraints in vision based control of robot manipulators. In: Corke PI, Trevelyan J
(eds) Lecture notes in control and information sciences. Experimental robotics VI,
vol 250. Springer-Verlag, Berlin Heidelberg, pp 99–108.
306. Muja M, Lowe DG (2009) Fast approximate nearest neighbors with automatic algorithm configuration. International Conference on Computer Vision Theory and
Applications (VISAPP), Lisbon, Portugal (Feb 2009), pp 331–340.
307. Murray RM, Sastry SS, Zexiang L (1994) A mathematical introduction to robotic
manipulation. CRC Press, Inc., Boca Raton.
308. NASA (1970) Apollo 13: Technical air-to-ground voice transcription. Test Division,
Apollo Spacecraft Program Office, https://www.hq.nasa.gov/alsj/a13/AS13_TEC.
PDF.
309. Nayar SK (1997) Catadioptric omnidirectional camera. In: Proceedings of the
IEEE Conference on Computer Vision and Pattern Recognition. Los Alamitos, CA,
pp 482–488.
310. Neira J, Tardós JD (2001) Data association in stochastic mapping using the joint
compatibility test. IEEE T Robotic Autom 17(6):890–897.
311. Neira J, Davison A, Leonard J (2008) Guest editorial special issue on Visual SLAM.
IEEE T Robotic Autom 24(5):929–931.
312. Newcombe RA, Lovegrove SJ, Davison AJ (2011) DTAM: Dense tracking and mapping in real-time. In: Proceedings of the International Conference on Computer
Vision, pp 2320–2327.
313. Ng J, Bräunl T (2007) Performance comparison of bug navigation algorithms. J Intell Robot Syst 50(1):73–84.
314. Niblack W (1985) An introduction to digital image processing. Strandberg Publishing Company Birkeroed, Denmark.
315. Nistér D (2003) An efficient solution to the five-point relative pose problem. In:
IEEE Conference on Computer Vision and Pattern Recognition, vol 2. Madison,
pp 195–202.
316. Nistér D, Naroditsky O, Bergen J (2006) Visual odometry for ground vehicle applications. J Field Robotics 23(1):3–20.
317. Nixon MS, Aguado AS (2019) Feature extraction and image processing for Computer Vision, 4th ed. Academic Press, London Oxford.
318. Noble JA (1988) Finding corners. Image Vision Comput 6(2): 121–128.
319. Okutomi M, Kanade T (1993) A multiple-baseline stereo. IEEE T Pattern Anal
15(4):353–363.
320. Ollis M, Herman H, Singh S (1999) Analysis and design of panoramic stereo vision
using equi-angular pixel cameras. Robo­tics Institute, Carnegie Mellon University,
CMU-RI-TR-99-04, Pittsburgh, PA.
321. Olson E (2011) AprilTag: A robust and flexible visual fiducial system. In: Procee­
dings of the IEEE International Conference on Robotics and Automation (ICRA).
pp 3400–3407.

1072 Определение пика

322. Orin DE, McGhee RB, Vukobratovic M, Hartoch G (1979) Kinematics and kine­
tic analysis of openchain linkages utilizing Newton-Euler methods. Math Biosci
43(1/2):107–130.
323. Ortega R, Spong MW (1989) Adaptive motion control of rigid robots: Atutorial.
Automatica 25(6):877–888.
324. Otsu N (1975) A threshold selection method from gray-level histograms. Automatica 11:285–296.
325. Owen M, Beard RW, McLain TW (2015) Implementing Dubins Airplane Paths on
Fixed-Wing UAVs. Handbook of Uncrewed Aerial Vehicles, pp 1677–1701.
326. Papanikolopoulos NP, Khosla PK (1993) Adaptive robot visual tracking: Theory and
experiments. IEEE T Automat Contr 38(3):429–445.
327. Papanikolopoulos NP, Khosla PK, Kanade T (1993) Visual tracking of a moving target by a camera mounted on a robot: A combination of vision and control. IEEE
T Robotic Autom 9(1):14–35.
328. Patel S, Sobh T (2015) Manipulator performance measures-a comprehensive lite­
rature survey. Journal of Intelligent and Robotic Systems, vol. 77, pp 547–570.
329. Paul R (1972) Modelling, trajectory calculation and servoing of a computer controlled arm. Ph.D. thesis, technical report AIM-177, Stanford University.
330. Paul R (1979) Manipulator Cartesian path control. IEEE T Syst Man Cyb 9:702–711.
331. Paul RP (1981) Robot manipulators: Mathematics, programming, and control. MIT
Press, Cambridge, Massachusetts.
332. Paul RP, Shimano B (1978) Kinematic control equations for simple manipulators.
In: IEEE Conference on Decision and Control, vol 17. pp 1398–1406.
333. Paul RP, Zhang H (1986) Computationally efficient kinematics for manipulators with
spherical wrists based on the homogeneous transformation representation. Int J Robot
Res 5(2):32–44.
334. Piepmeier JA, McMurray G, Lipkin H (1999) A dynamic quasi-Newton method for
uncalibrated visual servoing. In: Proceedings of the IEEE International Conference
on Robotics and Automation (ICRA). Detroit, pp 1595–1600.
335. Pivtoraiko M, Knepper RA, Kelly A (2009) Differentially constrained mobile robot
motion planning in state lattices. J Field Robotics 26(3):308–333.
336. Pock T (2008) Fast total variation for computer vision. Ph.D. thesis, Graz University
of Technology.
337. Pollefeys M, Nistér D, Frahm JM, Akbarzadeh A, Mordohai P, Clipp B, Engels C, Gallup D, Kim SJ, Merrell P, et al. (2008) Detailed real-time urban 3D reconstruction
from video. Int J Comput Vision 78(2):143–167, Jul.
338. Pomerleau D, Jochem T (1995) No hands across America Journal. https://www.
cs.cmu.edu/~tjochem/nhaa/Journal.html.
339. Pomerleau D, Jochem T (1996) Rapidly adapting machine vision for automated
vehicle steering. IEEE Expert 11(1):19–27.
340. Posner I, Corke P, Newman P (2010) Using text-spotting to query the world. In:
IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).
IEEE, pp 3181–3186.
341. Pounds P (2007) Design, construction and control of a large quadrotor micro air
vehicle. Ph.D. thesis, Australian National University.
342. Pounds P, Mahony R, Gresham J, Corke PI, Roberts J (2004) Towards dynamicallyfavourable quadrotor aerial robots. In: Proceedings of the Australasian Conference
on Robotics and Automation. Canberra.

Определение пика 1073

343. Pounds P, Mahony R, Corke PI (2006) A practical quad-rotor robot. In: Proceedings
of the Australasian Conference on Robotics and Automation. Auckland.
344. Poynton CA (2012) Digital video and HD algorithms and interfaces. Morgan
Kaufmann, Burlington Press WH, Teukolsky SA, Vetterling WT, Flannery BP (2007)
Numerical recipes, 3rd ed. Cambridge University Press, New York.
345. Prince SJ (2012) Computer vision: Models, learning, and infe­rence. Cambridge University Press, New York.
346. Prouty RW (2002) Helicopter performance, stability, and control. Krieger, Malabar FL.
347. Pujol J (2012) Hamilton, Rodrigues, Gauss, Quaternions, and Rotations: A Historical Reassessment. Communications in Mathematical Analysis, vol. 13.
348. Pynchon T (2006) Against the day. Jonathan Cape, London.
349. Reeds J, Shepp L (1990) Optimal paths for a car that goes both forwards and backwards. Pacific Journal of Mathematics, vol. 145, pp 367–393.
350. Rekleitis IM (2004) A particle filter tutorial for mobile robot localization. Technical
report (TR-CIM-04-02), Centre for Intelligent Machines, McGill University.
351. Rives P, Chaumette F, Espiau B (1989) Positioning of a robot with respect to an
object, tracking it and estimating its velo­city by visual servoing. In: Hayward V,
Kha­tib O (eds) Lecture notes in control and information sciences. Experimental
robotics I, vol 139. Springer-Verlag, Berlin Heidelberg, pp 412–428.
352. Rizzi AA, Koditschek DE (1991) Preliminary experiments in spatial robot juggling.
In: Chatila R, Hirzinger G (eds) Lecture notes in control and information sciences.
Experimental robotics II, vol 190. Springer-Verlag, Berlin Heidelberg, pp 282–298.
353. Roberts LG (1963) Machine perception of three-dimensional solids. MIT Lincoln
Laboratory, TR 315, https://dspace.mit.edu/handle/1721.1/11589.
354. Romero-Ramirez FJ, Muñoz-Salinas R, Medina-Carnicer R (2018) Speeded up detection of squared fiducial markers. Image and Vision Computing, vol 76, pp 38–47.
355. Rosenfeld GH (1959) The problem of exterior orientation in photogrammetry. Photogramm Eng 25(4):536–553.
356. Rosten E, Porter R, Drummond T (2010) FASTER and better: A machine learning
approach to corner detection. IEEE T Pattern Anal 32:105–119 Russell S, Norvig P
(2020) Artificial intelligence: A modern approach, 4th ed. Pearson.
357. Sakaguchi T, Fujita M, Watanabe H, Miyazaki F (1993) Motion planning and control
for a robot performer. In: Proceedings of the IEEE International Conference on
Robotics and Automation (ICRA). Atlanta, May, pp 925–931.
358. Salvi J, Matabosch C, Fofi D, Forest J (2007) A review of recent range image registration methods with accuracy evaluation. Image Vision Comput 25(5):578–596.
359. Samson C, Espiau B, Le Borgne M (1990) Robot control: The task function approach. Oxford University Press, Oxford.
360. Scaramuzza D, Martinelli A, Siegwart R (2006) A Toolbox for Easy Calibrating Omnidirectional Cameras. Proc IEEE/RSJ Int Conf on Intelligent Robots and Systems,
pp 5695–5701.
361. Scaramuzza D, Fraundorfer F (2011) Visual odometry [tutorial]. IEEE Robot Autom
Mag 18(4):80–92.
362. Scharstein D, Pal C (2007) Learning conditional random fields for stereo. In:
IEEE Computer Society Conference on Computer Vision and Pattern Recognition
(CVPR). Minneapolis, MN.
363. Scharstein D, Szeliski R (2002) A taxonomy and evaluation of dense two-frame
stereo correspondence algorithms. Int J Comput Vision 47(1):7–42.

1074 Определение пика

364. Selig JM (2005) Geometric fundamentals of robotics. Springer-Verlag, Berlin Heidelberg.
365. Sharf I, Nahon M, Harmat A, Khan W, Michini M, Speal N, Trentini M, Tsadok T,
Wang T (2014) Ground effect experiments and model validation with Draganflyer
X8 rotorcraft. Int Conf Uncrewed Aircraft Systems (ICUAS), pp 1158–1166.
366. Sharp A (1896) Bicycles & tricycles: An elementary treatise on their design an
construction; With examples and tables. Longmans, Green and Co., London New
York Bombay.
367. Sheridan TB (2003) Telerobotics, automation, and human supervisory control. MIT
Press, Cambridge, Massachusetts, 415 p.
368. Shi J, Tomasi C (1994) Good features to track. In: Proceedings of the Computer Vision and Pattern Recognition. IEEE Computer Society, Seattle, pp 593–593.
369. Shih FY (2009) Image processing and mathematicalmorpho­logy: Fundamentals
and applications, CRC Press, Boca Raton.
370. Shirai Y (1987) Three-dimensional computer vision. Springer-Verlag, New York.
371. Shirai Y, Inoue H (1973) Guiding a robot by visual feedback in assembling tasks.
Pattern Recogn 5(2):99–106.
372. Shoemake K (1985) Animating rotation with quaternion curves. In: Proceedings of
ACM SIGGRAPH, San Francisco, pp 245–254.
373. Siciliano B, Khatib O (eds) (2016) Springer handbook of robo­tics, 2nd ed. SpringerVerlag, New York.
374. Siciliano B, Sciavicco L, Villani L, Oriolo G (2009) Robotics: Modelling, planning
and control. Springer-Verlag, Berlin Heidelberg.
375. Siegwart R, Nourbakhsh IR, Scaramuzza D (2011) Introduction to autonomous mobile robots, 2nd ed. MIT Press, Cambridge, Massachusetts.
376. SilverWM (1982) On the equivalence of Lagrangian and Newton-Euler dynamics
for manipulators. Int J Robot Res 1(2):60–70.
377. Sivic J, Zisserman A (2003) Video Google: A text retrieval approach to object
matching in videos. In: Proceedings of the Ninth IEEE International Conference
on Computer Vision. pp 1470–1477.
378. Skaar SB, Brockman WH, Hanson R (1987) Camera-space manipulation. Int J Robot
Res 6(4):20–32.
379. Skofteland G, Hirzinger G (1991) Computing position and orientation of a freeflying polyhedron from 3D data. In: Procee­dings of the IEEE International Confe­
rence on Robotics and Auto­mation (ICRA). Seoul, pp 150–155.
380. Smith R (2007) An overview of the Tesseract OCR engine. In: 9th International
Conference on Document Analysis and Recognition (ICDAR), pp 629–633.
381. Sobel D (1996) Longitude: The true story of a lone genius who solved the greatest
scientific problem of his time. Fourth Estate, London1.
382. Soille P (2003) Morphological image analysis: Principles and applications. Springer-Verlag, Berlin Heidelberg.
383. Solà J (2017) Quaternion kinematics for the error-state Kalman filter. arXiv
1711.02508.
384. Solà J, Deray J, Atchuthan D (2018) A micro Lie theory for state estimation in robo­
tics. arXiv 1812.01537.
385. Spong MW (1989) Adaptive control of flexible joint manipulators. Syst Control Lett
13(1):15–21.
1

Дава Собел. Долгота. Астрель, 2012. ISBN: 978-5-271-42800-5. – Прим. перев.

Определение пика 1075

386. Spong MW, Hutchinson S, Vidyasagar M (2006) Robot mode­ling and control,
2nd ed. John Wiley & Sons, Inc., Chichester.
387. Srinivasan VV, Venkatesh S (1997) From living eyes to seeing machines. Oxford
University Press, Oxford.
388. Stachniss C, Burgard W (2014) Particle filters for robot navigation. Foundations
and Trends in Robotics 3(4):211–282.
389. Steinvall A (2002) English colour terms in context. Ph.D. thesis, Umeå Universitet.
390. Stentz A (1994) The Dalgorithm for real-time planning of optimal traverses. The
Robotics Institute, Carnegie-Mellon University, CMU-RI-TR-94-37.
391. Stewart A (2014) Localisation using the appearance of prior structure. Ph.D. thesis,
University of Oxford.
392. Stone JV (2012) Vision and brain: How we perceive the world. MIT Press, Cambridge, Massachusetts.
393. Strasdat H (2012) Local accuracy and global consistency for efficient visual SLAM.
Ph.D. thesis, Imperial College London.
394. Strelow D, Singh S (2004) Motion estimation from image and inertial measurements. Int J Robot Res 23(12):1157–1195.
395. Sünderhauf N (2012) Robust optimization for simultaneous localization and mapping. Ph.D. thesis, Technische Universität Chemnitz.
396. Sussman GJ, Wisdom J, Mayer ME (2001) Structure and interpretation of classical
mechanics. MIT Press, Cambridge, Massachusetts.
397. Sutherland IE (1974) Three-dimensional data input by tablet. P IEEE 62(4):453–
461.
398. Svoboda T, Pajdla T (2002) Epipolar geometry for central catadioptric cameras. Int
J Comput Vision 49(1):23–37.
399. Szeliski R (2022) Computer vision: Algorithms and applications. Springer-Verlag,
Berlin Heidelberg.
400. Tahri O, Chaumette F (2005) Point-based and region-based imagemoments for visual servoing of planar objects. IEEE T Robotic Autom 21(6):1116–1127.
401. Tahri O, Mezouar Y, Chaumette F, Corke PI (2009) Generic decoupled image-based
visual servoing for cameras obeying the unified projection model. In: Proceedings
of the IEEE International Conference on Robotics and Automation (ICRA). Kobe,
pp 1116–1121.
402. Taylor RA (1979) Planning and execution of straight line manipulator trajectories.
IBM J Res Dev 23(4):424–436.
403. ter Haar Romeny BM (1996) Introduction to scale-space theory: Multiscale geometric image analysis. Utrecht University.
404. Thrun S, Burgard W, Fox D (2005) Probabilistic robotics. MIT Press, Cambridge,
Massachusetts.
405. Tissainayagam P, Suter D (2004) Assessing the performance of corner detectors for
point feature tracking applications. Image Vision Comput 22(8):663–679.
406. Titterton DH, Weston JL (2005) Strapdown inertial navigation technology. IEE Radar, Sonar, Navigation and Avionics Series, vol 17, The Institution of Engineering
and Technology (IET), 576 p.
407. Tomasi C, Kanade T (1991) Detection and tracking of point features. Carnegie Mellon University, CMU-CS-91-132.
408. Triggs B, McLauchlan P, Hartley R, Fitzgibbon A (2000) Bundle adjustment –
A modern synthesis. Lecture notes in computer science. Vision algorithms: theory
and practice, vol 1883. Springer-Verlag, Berlin Heidelberg, pp 153–177.

1076 Определение пика

409. Tsai RY (1986) An Efficient and Accurate Camera Calibration Technique for 3D Machine Vision. Proc IEEE Conference on Computer Vision and Pattern Recognition
(CVPR), pp 364–374.
410. Tsakiris D, Rives P, Samson C (1998) Extending visual servoing techniques to nonholonomic mobile robots. In: Kriegman DJ, Hager GD, Morse AS (eds) Lecture notes
in control and information sciences. The confluence of vision and control, vol 237.
Springer-Verlag, Berlin Heidelberg, pp 106–117.
411. Terzakis G, Lourakis MIA, Ait-Boudaoud D (2018) Modified Rod­rigues Parameters:
An Efficient Representation of Orientation in 3D Vision and Graphics. J Math Ima­
ging Vis. 60:422–442.
412. Uicker JJ (1965) On the dynamic analysis of spatial linkages using 4 by 4 matrices.
Dept. Mechanical Engineering and Ast­ronautical Sciences, NorthWestern University.
413. Umeyama, S (1991) Least-Squares Estimation of Transformation Parameters Between Two Point Patterns. IEEE Trans Pattern Analysis and Machine Intellence
13(4). pp 376–380.
414. Usher K (2005) Visual homing for a car-like vehicle. Ph.D. thesis, Queensland University of Technology.
415. Usher K, Ridley P, Corke PI (2003) Visual servoing of a car-like vehicle – An application of omnidirectional vision. In: Proceedings of the IEEE International Confe­
rence on Robotics and Automation (ICRA). Taipei, Sep, pp 4288–4293.
416. Valgren C, Lilienthal AJ (2010) SIFT, SURF & seasons: Appearance-based longterm localization in outdoor environments. Robot Auton Syst 58(2):149–156.
417. Vanderborght B, Sugar T, Lefeber D (2008) Adaptable compliance or variable stiffness for robotic applications. IEEE Robot Autom Mag 15(3):8–9.
418. Vince J (2011) Quaternions for Computer Graphics. Springer.
419. Wade NJ (2007) Image, eye, and retina. J Opt Soc Am A 24(5):1229–1249.
420. Walker MW, Orin DE (1982) Efficient dynamic computer simulation of robotic
mechanisms. J Dyn Syst-T ASME 104(3):205–211.
421. WalterWG (1950) An imitation of life. Sci Am 182(5):42–45.
422. WalterWG (1951) A machine that learns. Sci Am 185(2):60–63.
423. WalterWG (1953) The living brain. Duckworth, London.
424. Warren M (2015) Long-range stereo visual odometry for unmanned aerial vehicles.
Ph.D. thesis, Queensland University of Technology.
425. Weiss LE (1984) Dynamic visual servo control of robots: An adaptive image-based
approach. Ph.D. thesis, technical report CMU-RI-TR-84-16, Carnegie-Mellon University.
426. Weiss L, Sanderson AC, Neuman CP (1987) Dynamic sensor-based control of robots
with visual feedback. IEEE T Robotic Autom 3(1):404–417.
427. Westmore DB, Wilson WJ (1991) Direct dynamic control of a robot using an endpoint mounted camera and Kalman filter position estimation. In: Proceedings of
the IEEE International Conference on Robotics and Automation (ICRA). Seoul, Apr,
pp 2376–2384.
428. Whitney DE (1969) Resolved motion rate control of manipulators and human prostheses. IEEE T Man Machine 10(2):47–53.
429. Wiener N (1965) Cybernetics or control and communication in the animal and the
machine. MIT Press, Cambridge, Massachusetts.

Определение пика 1077

430. Wilburn B, Joshi N, Vaish V, Talvala E-V, Antunez E, Barth A, Adams A, Horowitz M,
Levoy M (2005) High performance ima­ging using large camera arrays. ACM Transactions on Graphics (TOG) – Proceedings of ACM SIGGRAPH 2005 24(3):765–776.
431. Wolf PR, DeWitt BA (2014) Elements of photogrammetry, 4th ed. McGraw-Hill,
New York.
432. Woodfill J, Von Herzen B (1997) Real-time stereo vision on the PARTS reconfi­
gurable computer. In: Proceedings of the IEEE Symposium on FPGAs for Custom
Computing Machines, Grenoble. pp 201–210.
433. Xu G, Zhang Z (1996) Epipolar geometry in stereo, motion, and object recognition:
A unified approach. Springer-Verlag, Berlin Heidelberg.
434. Yi K, Trulls E, Lepetit V, Fua P (2016) LIFT: Learned Invariant Feature Transform.
ECCV 2016. pp 467–483.
435. Ying X, Hu Z (2004) Can we consider central catiodioptric cameras and fisheye
cameras within a unified imaging model. In: Pajdla T, Matas J (eds) Lecture notes
in computer science. Computer vision – ECCV 2004, vol 3021. Springer-Verlag, Berlin Heidelberg, pp 442–455.
436. Yoshikawa T (1984) Analysis and control of robot manipulators with redundancy.
In: Brady M, Paul R (eds) Robotics research: The first international symposium.
MIT Press, Cambridge, Massachusetts, pp 735–747.
437. Zabih R, Woodfill J (1994) Non-parametric local transforms for computing visual
correspondence. In: Ecklundh J-O (ed) Lecture notes in computer science. Computer Vision – ECCV 1994, vol 800. Springer-Verlag, Berlin Heidelberg, pp 151–158.
438. Zarchan P, Musoff H (2005) Fundamentals of Kalman filtering: A practical approach. Progress in Astronautics and Aeronautics, vol 208. American Institute of
Aeronautics and Astronautics.
439. Zhan H, Garg R, Weerasekera C, Li K, Agarwal H, Reid I (2018) Unsupervised Lear­
ning of Monocular Depth Estimation and Visual Odometry With Deep Feature
Reconstruction. IEEE Conference On Computer Vision and Pattern Recognition
(CVPR).
440. Zhang Z, Faugeras O, Kohonen T, Hunag TS, Schroeder MR (1992) Three D-dynamic scene analysis: A stereo based approach. Springer-Verlag, New York.
441. Zhou Q, Park J, Koltun V (2018) Open3D: A Modern Library for 3D Data Processing.
ArXiv:1801.09847.
442. Ziegler J, Bender P, Schreiber M, Lategahn H, Strauss T, Stil­ler C, Thao Dang, Fran­
ke U, Appenrodt N, Keller CG, Kaus E, Herrtwich RG, Rabe C, Pfeiffer D, Lindner F,
Stein F, Erbs F, EnzweilerM, Knöppel C, Hipp J, Haueis M, Trepte M, Brenk C,
Tamke A, Ghanaat M, Braun M, Joos A, Fritz H, Mock H, Hein M, Zeeb E (2014)
Making Bertha drive – An autonomous journey on a historic route. IEEE Intelligent
Transportation Systems Magazine 6(2):8–20.

Предметный указатель

Символы
@, оператор, 175
ξ, относительное положение, 61

A
abs, функция, 606
AHRS, 184
angdiff, 203, 954
angvec2r, 103
antialias=True параметр, 600
Apollo Guidance Computer, 174
append, 137
AprilTags, 792
argmax, функция, 1051
ArUco, 792
atan2, функция, 608

B
BagOfWords, объект, 730
bdsim, среда создания блок-схем, 414
Beidou, система
геопозиционирования, 306
Bicycle, 212, 310
step(), 310
blobs, метод, 691
Blobs, объект, 691
blue, метод, 590
braitenberg, 243
bug2, 266
Bug2, 245
.plot, 245
.run, 245

C
CameraBase, класс, 749
CentralCamera, класс, 749, 815, 923, 957
closest_to_line, метод, 839
COCO, набор данных, 702
COLLADA, формат, 391

color2name, 555
ColorChecker, диаграмма, 571
colorspace, 557, 558
configs, словарь конфигураций, 383
convolve, метод, 620
coriolis, 492, 511
coriolis_x, 511
ctraj, функция, 170, 412

D
D*, алгоритм, 268
delta2tr, 152
DGraph, 254
DHLink, класс, 397
disp, метод, 588
distance, свойство, 812
distanceTransform, функция, 649
DistanceTransformPlanner, 262
.plan, 263
.plot, 263
.query, 265
dot, 254
draw_box(), метод, 599
draw_circle, метод, 599
draw_line, метод, 599
draw_xx, методы, 599
driveconfig, 211
driveline, 206
drivepoint, 203, 204
drivepursuit, 208
DstarPlanner, 269
.costmap, 269
.nexpand, 269
.plan, 269
.query, 269
DubinsPlanner, 278

E
e2h, 81
EarthView, класс, 597

Определение пика 1079
Edge, 251
.endpoints, 252
EKF, 314
ERobot2, 439
ERobot, класс, 382
ET2, 439
ET2, объект, 374
ETS2, класс, 372
ETS, класс, 382
ETS (elementary transformation sequence –
последовательность элементарных
преобразований), 372
eul2r, 93, 94
exp, 113

F
Faster R-CNN, 702
FastSLAM, 349
FCN-ResNet, сверточная сеть, 680
FeatureMatch, класс, 812
FeatureMatch, объект, 811
findContours, функция, 698
fkine, метод, 382
fkine, функция, 372
Flickr, 583
flowfield, метод, 923
friction, 511
friction_x, 511

G
Galileo, система
геопозиционирования, 306
gamma, параметр, 583
gamma_decode, функция, 606
GEE, система геопозиционирования, 306
Google Images, 583
GOOGLE_KEY, переменная окружения, 597
GPS (Global Positioning System), 301
дифференциальная, 306
GraphViz, 254
gravload, 495, 511
gravload_x, 511
green, метод, 590

homtrans, функция, 795
HoughLines, функция, 707

I
IBVS для сферической камеры, 959
IBVS с разделением XY/Z, 953
ikine_LM, 465
Image, класс, 588, 670
image, свойство, 590
IMU, 185
.accel, 185
.gyro, 185
.magno, 185
inertia, 492, 511
inertia_x, 511
inormhist, метод, 606
iread, функция, 583, 588
isstretch, метод, 606
ithresh, метод, 670

J
jacob0, 441
jacob0_analytical, 444
jacobe, 443
Jacobian, 445
JPEG, формат, 587
метаданные, 591
jtraj, функция, 412

L
lambda2rg, 550
LandmarkMap, 319, 323
lanechange, 201
LatticePlanner, 281
len, 137
linalg.logm, 104
Line3, класс, 1005
Line3, объект, 796
Link2, объект, 379
Link, класс, 382
LORAN, система
геопозиционирования, 306

H

M

h2e, 81
Histogram, класс, 600
hitormiss, метод, 648
homtrans, 81

manipulability, 453
Matplotlib, 584
max, функция, 608
min, функция, 608

1080 Определение пика
MIT Robotic Garden, проект, 673
models.DH.Puma560(), 493
models.list(), 493
mstraj, 164
mtraj, 162, 166
mtraj, многоосевой драйвер, 410
mvtb-images, пакет, 583

N
name2color, 555
nofriction(), 505
numpy
.angle, 102
.cross, 154, 188
.eye, 121
.finfo.eps, 122
.linalg.det, 74, 121, 122
linalg.det, 151
.linalg.eig, 101
.linalg.inv, 81
linalg.inv, 154
linalg.logm, 75
.linspace, 160, 161
.zeros, 188
NumPy, 263
.random.choice, 263
.zeros, 263

O
Omron Cobra 600 SCARA, робот, 407
OpenCV, 263, 591

P
ParticleFilter, 347
PASCAL VOC, набор данных, 680
PBVS, класс, 916
perimeter_approx, метод, 701
pgraph, 250
Picasa, 583
Pillow, 263
plot, метод, 382
plot_accumulator vtnjl, 707
plot_ellipsoid, 452
plot_point, 80, 82, 134, 249
plotvol2, 79, 81
plot_xx, методы, 599
PointCloud, объект, 841
polarmatch, метод, 701
PoseGraph, 338

PR2, робот, 394
PRM, алгоритм, 273
PRMPlanner, 273
.plan, 273
project_point, метод, 749
Puma 560, манипулятор, 445
PUMA 560, робот, 377, 493
PUMA, манипулятор, 376
pytessearct, 726
Python, 48
python-control, 483
PyTorch, 680
PyVista, 453

Q
qr, свойство, 383
quadrotor, 225, 226
quintic, 160, 162
quintic, функция, 410
qz, свойство, 383

R
RandomPath, 310
RangeBearingSensor, 320, 322
RangerBot, 229
RANSAC, алгоритм, 824
raw-файлы, 543
Read, метод, 588
reading, метод, 320
red, метод, 590
ReedsSheppPlanner, 280
retrieve, метод, 734
reuse, параметр, 596
RevoluteDH, класс, 397
RevoluteMDH, класс, 397
Robot, класс, 493
accel, 503
rne, 493
Robot, объект, 483
rot2, 74, 78
rotvelxform, 444
rotx, 90, 91, 103, 105, 134, 150
roty, 91
rotz, 91
rpy2r, 95, 101, 107, 121
RRMC2, 447
RRT, алгоритм, 287
rtb_load_jsonfile, 249
rtb_load_matfile, 244

Определение пика 1081

S
SaM, алгоритм, 842
SCARA, робот, 368
scipy
.linalg.expm, 75, 84, 104, 112, 113
.linalg.logm, 84, 103, 111
SciPy, 263
SE2, класс, 380
SE(2), матрица, 78
SE3, 134, 147, 168
.Ad, 148
.interp, 168
.inv, 155
.jacob, 147
.rpy, 163
.Rx, 147
.Rz, 147
.t, 163
.Trans, 168
.Tx, 147
SE3, класс, 412
SE(3), матрица, 111, 168
SE3, объект, 113
SensorBase, 320
sensorfunc, 269
SfM, алгоритм, 842
shadow_invariant, 575
Shakey, роботизированная система, 257
showgraph, метод, 388
SIFT, алгоритм, 725, 810
SIFT, признаки, 808
similarity, метод, 733
SINS, 174
skew, 75, 104, 150
skew, метод, 1005
skewa, 83, 112
SLAM
внутренний механизм, 342
интерфейсная часть, 341
на основе графа положений, 335
на основе теоремы Рао–Блэквелла, 349
одновременные локализация и
картографирование, 330
smooth, метод, 618
SO2, 164
so(2), матрица, 76
SO(2), матрица, 74
SO3, 134, 166
.animate, 166
.rpy, 166
.Rx, 134, 137
.Ry, 166

.Rz, 166
SO(3), матрица, 90, 92
spatialmath, 134
Spatial Math Toolbox, пакет, 599
SphericalCamera, класс, 960
SPIRE, 174
sqrt, функция, 606
STL, формат, 391
sympy, 439
.det, 74
.Matrix, 74
.simpify, 74
.Symbol, 74
symbols, 439
SymPy, 338
SymPy, пакет, 401

T
t2r, 110
teach, метод, 382
teach(), метод, 373
Tesseract, 726
touch, свойство, 692
tr2angvec, 101, 151
tr2delta, 152, 447
tr2eul, 93, 94
tr2rpy, 96
Trajectory, 164
.plot, 160, 161
.qd, 161
Trajectory, класс, 410
tranimate, 90
transl, 110, 111, 129
transl2, 79, 81, 85, 134
trapezoidal, 161, 163
trapezoidal, функция, 410
trexp, 105, 128, 134, 150
tripleangledemo, 96
trot2, 78, 85
trotx, 110, 113
trplot, 90, 110, 134
trplot2, 79, 81
Twist2, 84, 85, 127
.exp, 84, 85
.pole, 85
.UnitPrismatic, 85
.UnitRevolute, 84
.w, 85
Twist3, 127
.UnitRevolute, 128
twistdemo, 130

1082 Определение пика

U

X

UGraph, 250
.add_edge, 250
.add_vertex, 250
.average_degree, 251
.degree, 251
.highlight_path, 252
.n, 251
.nc, 251
.ne, 251
.path_BFS, 252
.path_UCS, 255
.plot, 251
UnitQuaternion, 107, 154, 166, 186
.dot, 146
.dotb, 146
.EulerVec, 155
.increment, 175
.interp, 166
.plot, 155
.R, 155
.Rx, 121
.Rz, 121
unwrap, параметр, 415
URDF, формат, 370, 385, 390
URDF (Unified Robot Description Format –
унифицированный формат описания
роботов), 390
UVertex, 251
.adjacent, 251

Xacro, язык макросов, 390
xplot, 164, 169, 175, 186, 446
XYZ, крена-тангажа-рыскания углы, 94

V
Vehicle, 313, 317
VehicleBase, 315
vellipse, 453
vex, 75, 104
vexa, 83, 111, 112
VideoCamera, класс, 592
VideoFile, класс, 594
visjac_p, метод, 960
visjac_p_polar, метод, 957
Visual Object Classes, 680
VisualServo, класс, 916

W
workspace, 310
wrap_0_2pi, 954
wrap_mpi_pi, 203, 954

Y
YuMi, робот, 369

Z
ZYX, крена-тангажа-рыскания углы, 94

А
Абелева группа, 67
Абстрактное положение ξ, 61
Автоматы, 244
Автономные беспилотные системы
(АБС), 195
Автономные подводные аппараты
(АПА), 195
Адаптивный порог, 672
Аддитивный шум, 593
Азимов, Айзек, 34
Аккермана рулевое управление, 200
Аккерман, Рудольф, 201
Акселерометры, 175
инерциальная масса, 176
как работают, 176
Активное вращение, 118
Алгебраическая группа, 1011
Алгебра Ли, 76, 83, 112, 1010
Алгоритм
быстрого исследования случайного
дерева, 287
Дейкстры, 292
истончения, 272
лесного пожара, 267
оптимизации, Левенберга–
Марквардта, 465
поведения жука, 244
поиска кратчайшего пути, 292
преобразование расстояния, 261
скелетизации, 272
с обратной связью, 447
степного пожара, 267
фронта волны, 267
A*, 257
bug2, 245, 266
D*, 268

Определение пика 1083
PRM, 273
RRT, 287
Анаглифы, 870
Аналитический якобиан, 443, 511
Антропоморфная конструкция, 377
Апертура объектива, 747
Аполлон-13, миссия, 99
«Аполлон-13», фильм, 96
Априорная вероятность, 313
Арифметика, ошибки, 121
Ассоциация данных, 325
Астигматизм, 758
Астронавигация, 304
Аффинные преобразования, 658, 1007,
1008
вращение, 1007
гомотетия, 1007
масштабирование, 1007
отражение, 1007
перенос, 1007
преобразование подобия, 1007
сдвиговое отображение, 1007
Аэродинамическое сопротивление, 222

Б
Базисные векторы, 72, 86, 89
Базисные матрицы, 1010
Байера шаблон, 543
Байес, Томас преподобный, 313
Безэкипажные катера (БЭК), 195
Беспилотные летательные аппараты
(БПЛА), 195, 218
Беспороговый подход, 674
Бикватернионы, 116
Бимодальное распределение, 670
Бинарная классификация, 669
Бинарные изображения, 604
Блобы
иерархия, 694
описательные параметры, 691
Блокировка подвеса, 96
Блок управления винтами, 225
Блэк, Гарольд, 41
Боде, Хенрик, 41
Болл, Роберт, 125, 131
Большой круг, 167
Бортовое управление, 214
Бочкообразная дисторсия, 759
Брайан, Джордж, 94
Брайтенберга тележка, 241, 242, 264
Брайтенберг, Валентино, 242

Брокетта теорема, 231
Быстрого исследования случайного
дерева алгоритм, 287
Бэббидж, Чарльз, 33

В
Вектор, 978
координат, 65
определение, 978
подхода, 99, 409
признаков, 809
Эйлера, 1013
Вероятностной дорожной карты,
метод, 273
Вертолеты, 220
Визуальная одометрия, 888
Визуальное сервоуправление, 474, 912,
952
деротация оптического потока, 935
конфигурация с замкнутым
контуром, 912
конфигурация с разомкнутым
контуром, 912
линейные объекты, 939
матрица взаимодействия, 920
на основе изображения, 913, 917
на основе местоположения, 913, 914
оптический поток, 923
оценка глубины объекта, 933
проблемы производительности, 936
пространственная скорость, 920
соответствие между наблюдаемыми
и желаемыми положениями важно, 928
управление движением объекта, 927
фотометрические признаки, 943
центроиды точек, 912
эллиптические объекты, 940
якобиан изображения, 920
IBVS, 913
PBVS, 913
Визуальное управление
воздушным роботом, 968
мобильным роботом, 964
роботом-манипулятором, 962
Визуальные слова, 730
Винер, Роберт, 41
Винтовое движение, 112
Винтокрылые летальные аппараты, 219
Винты динамические, 156
Воздействие на основание, 501
Воздушный робот, 218

1084 Определение пика
визуальное управление, 968
Возмущающие моменты, 473
Вокансона утка, 33
Воксель, 876
Волновой фронт, 267
Вороного диаграмма, 274
Вороной, Георгий, 274
Восприятие, 42
Вращение
активное, 118
в приращениях, 149
кратчайший путь, 167
пассивное, 118
Второй закон Ньютона, 221, 479
Выполнение запроса к плану, 264
Вычисление структуры и движения,
алгоритм, 842
Вязкое трение, 512

Г
Гамильтон, Уильям Роуэн, 107
Гармонические передачи, 480
Гаусса–Ньютона итерация, 1027
Гаусс, Карл Фридрих, 625
Гауссова функция, 1035
Гауссово распределение, 1035
Геликоидальная интерполяция, 130
Генератор
случайных чисел, 275
смещения твердого тела, 127
Геометрическая дисторсия, 759
Геометрические преобразования, 1007
Геометрический якобиан, 441
Геометрия
аналитическая, 70
декартова, 70
евклидова, 70
нескольких ракурсов, 814
Гершель, Уильям, 534
Гессе, Людвиг Отто, 1018
Гессиан, 1019
Гессиана определитель, 715
Гете, Иоганн Вольфганг, 541
Гетц, Рэй, 39
Гештальт-принцип эмерджентности, 685
Гибридные траектории, 161
Гиперспектральные камеры, 576
Гиперэллипсоид, 451
Гиппокамп, 248
Гироскоп, 171, 200
волоконно-оптический, 172

как работают, 172
кольцевой лазерный, 172
момент импульса, 172
оценка ориентации, 173
постоянная ориентация, 172
Гистограмма
изображений, 600
инверсная кумулятивная, 606
Главная точка, 746
Гладкие одномерные траектории, 158
Гладкий путь, 168
Глазные пятна, 45
ГЛОНАСС, система
геопозиционирования, 306
Голономный мобильный робот, 965
Гомогенные области, 668
Грассмана закон, 552
Граф, 247, 1047
вершины, 247
вложенный, 247
замыкание петли, 335
конфигураций робота, 289
неориентированный, 247
непогруженный, 249
ориентированный, 66, 247
ошибка ребра, 337
погруженный, 247
положений, SLAM, 335
положения, 66, 379, 463
преобразование карт сеток
занятости в, 293
ребра, 247
топологический, 249
Графовая сегментация изображений, 684
Групповая операция, 151
Группы, 67
Ли, 1010
ортогональные, 72
SE(2), 78

Д
Дальномер, 350
Дальтон, Джон, 545
Датчик Холла, 180
Двигатель, 473
бесщеточный, 473
ограничения, 487
щеточный, 473
Движение
в декартовом пространстве, 412
вдоль линии, 205

Определение пика 1085
вдоль произвольного пути, 207
в пространстве сочленений, 410
декартово, 168
модель одноколесная, 211
нулевое (пустое), 62
обратное, 62
с заданной скоростью, управление, 445
твердого тела, 152, 492
через сингулярность, 414
ξ, 61
Двухвекторное представление, 99
Двухколесная кинематическая
модель, 198, 209
Двухмерная проективной геометрии, 818
Девол, Джордж, 35
Дейкстра, Эдсгер Вибе, 292
Дейтеранопы, 545
Декартова геометрия, 70
Декартова плоскость, 70
Декартова система координат, 71
Декартово движение, 168
Декарт, Рене, 70
Демпфирование, 512
Денавита–Хартенберга нотация, 370, 386,
395, 442
модифицированная, 397, 427
Денавит, Жак, 397
Дерево поиска, 254
Деротация оптического потока, 935
Дескриптор признака, 809
Дескрипторы и детекторы, 810
Детектор
Нобля, 714
углов Моравека, 711
углов Плесси, 714
Харриса, 714, 809
Ши–Томаси, 714
Детекторы
и дескрипторы, 810
углов в масштабируемом
пространстве, 718
Деформация изображений, 655
Диагональная ковариационная
матрица, 320
Диаграмма Вороного, 274
Диадические операции, 607
Дилатация, 643
Динамика
пространства задач и управление, 509
прямая, 503
прямая и кулоновское трение, 505
рабочего пространства, 510

Динамическая манипулируемость, 501
Динамические винты, 156
Динамические параметры звена, 494
Динамические параметры робота, 478
Динамический винт
преобразование в пространство
сочленений, 461
рабочего органа, 461
Динамический диапазон сенсора, 595
Диоптрия, 744
Дискретная модель изменения
конфигурации на основе одометрии, 308
Дискретная плоскость изображения, 751
Диспаратность, 852
Дисперсия, 1035
Дисторсия объектива, 758
коррекция, 766
Дифференциальная кинематика, 438
Дифференциальное рулевое
управление, 212, 214
Длина оптического пути, 535
Доверительные границы положения, 316
Доминирующие линии, 709
Дорожная карта, 271
Дрейпер, Чарльз Старк, 174, 175
Дрейф датчика, 185
Дубинса путь, 278
Дэви, Хемфри, 534

Е
Евклид Александрийский, 69, 760
Евклидова геометрия, 70
Евклидова гомография, 832
Евклидова плоскость, 70
Евклидовы координаты, 70
Евклидовы точки, 81
Единичная матрица, 149
Единичное кручение, 84, 113, 429
Единичный вектор, 979
Единичный кватернион, 175
Единичный определитель, 121

Ж
Жесткое тело, 472

З
Задача
поиска Уолли, 634
сопоставления, 806

1086 Определение пика
Замыкание петли, 335
Запястье сферическое, 402
Звено, динамические параметры, 494
Зеркальное отражение, 778
Зеркальные блики, 679
Значимые пики, 601
Зрительная ямка высокого
разрешения, 539

И
Идентификация ориентиров, 325
Избыточно подвижные (overactuated)
системы, 133
Избыточный робот, 511
Излучающее черное тело, 532
Изменение
положения во времени, 144
формы, 651
изменение размера, 652
обрезка, 651
Изображения
анаглифы, 870
бинарная классификация, 669
бинарные, 604
визуальная одометрия, 888
геометрия нескольких ракурсов, 814
гистограммы, 600
главная точка, 746
графовая сегментация, 684
детектор Харриса, 809
деформация, 655
диадические операции, 582, 607
дилатация, 643
дискретная плоскость, 751
диспаратность, 852
дисторсия объектива, 758
евклидова гомография, 832
задача сопоставления, 806
зеркальное отражение, 778
из видеофайлов, 594
из интернета, 596
из кода, 598
из космоса, 597
изменение
размера, 652
формы, 651
из последовательностей файлов, 591
из файлов, 583
информация о форме из периметра, 698
использование нескольких, 806
исправление, 867

камеры светового поля, 789
классификация
монохромных изображений, 669
пикселей, 668
классические детекторы углов, 711
конические сечения, 747
коррекция перспективы, 881
линейная пространственная
фильтрация, 614
математическая морфология, 641
матрица гомографии, 829
многокамерные массивы, 788
мозаика, 884
монадические операции, 582, 602
нелинейные операции, 638
неоднозначность перспективной
проекции, 754
облака точек, 875
обнаружение
границ, 647
движения, 612
объектов с использованием глубокого
обучения, 701
обработка, 582
обрезка, 651
опорная область, 809
определение краев, 621
пакетная подстройка, 841
перспективная проекция, 743
пирамиды, 654
планарная гомография, 793, 828
плотное стереосовмещение, 852
поиск, 727
поиск плоскости, 877
получение, 583
получение признаков из линий, 705
получение признаков из точек, 710
поп-арт-постеризация, 606
пороговая бинаризация, 669
последовательное преобразование, 637
потенциальные соответствия, 811
представление экземпляра
объекта, 668, 681
преобразование сферического
изображения в перспективное, 786
преобразование Хафа, 707
признаки, 665
проецирование произвольных
линий, 796
пространственные операции, 582, 614
ранговое преобразование, 637
сглаживание, 616

Определение пика 1087
сегментация, 666
семантическая классификация, 668
совмещение признаков, 808
со встроенной камеры, 592
соотношение сторон, 596
сопоставление двух наборов точек, 878
сравнение с шаблоном, 631
существенная матрица, 820
сферическая камера, 779
сходство, 632
угловые признаки Харриса, 808
удаление шума, 645
унифицированная модель
формирования, 781
форматы файлов, 585
формирование, 743
фундаментальная матрица, 817
хромакей, 609
широкоугольные камеры, 771
экстраполяция за границы, 620
эпиполярная линия, 814
эпиполярная плоскость, 814
эрозия, 643
SIFT, алгоритм, 810
SIFT, признаки, 808
Илон, Бенгт, 215
Имитация погрешности датчика, 326
Импедансное управление, 513
Инварианты моментов, 697
Инверсная кумулятивная
гистограмма, 606
Инвертирование матрицы, 452
Инерциальная масса, 176
Инерциальная навигационная
система, 170
Инерциальная навигация, 144, 152
применение, 170
Инерциальная сила, 156
Инерциальная система координат, 156
Инерциальный измерительный блок
(IMU), 184
Инерционное ускорение, 177
Инерция нагрузки, 480
Инкрементное перепланирование, 268
Инновация, 321
Интегральное насыщение, 488
Интерполяция
единичных кватернионов, 168
линейная, 167
ориентации, 165
сферическая линейная, 167
Информационная матрица, 1037

Информация о форме
из моментов, 696
из периметра, 698
Инфракрасное излучение, 534
Искусственные ориентиры, 325
Использование нескольких
изображений, 806
Истинный север, 180
Истончения, алгоритм, 272
Итеративный поиск ближайших
точек, 353

К
Калибровка, 184
камеры, 761
Калмана–Бьюси фильтр, 1043
Калмана матрица усиления, 332
Калмана расширенный фильтр, 311, 1044
Калмана фильтр, 311, 1041
Калман, Рудольф, 314
Камера-обскура, 45, 744
Камеры светового поля, 789
Кардано, Джероламо, 95
Карман, Теодор фон, 397
Карта
гравитации, 176
занятости, 261
занятости (создание), 263
ориентиров (создание), 326
Картографирование на основе
лидара, 354
Катадиоптрическая камера, 775
Квадранты, 102, 205
Квадратная матрица, 455
Квадрика Клейна, 1005
Квадрокоптеры, 133, 220
Кватернионное дуальное число, 116
Кватернион тождественный
(нейтральный), 108
Кватернионы
бикватернионы, 116
единичный дуальный кватернион, 116
левосторонние, 109
упорядочение элементов, 109
чистый кватернион, 116
Кибернетика, 34, 41, 240
Кинематика
в виде блок-схемы, 414
дифференциальная, 438
мгновенная прямая, 441
обратная, 369, 399, 438

1088 Определение пика
аналитическое решение, 400
численное решение, 402
прямая, 369
численная обратная, 463
Классификация
пикселей, 668
цветных изображений, 673
Классические детекторы углов, 711
Клиффорд, Уильям, 116
Клотоида, 199
Колесо Илона, 215
Коллинеация, 1007
Колориметрия, 551
Компас, 302
Компенсация динамики твердого
тела, 505
Компенсирующий сигнал, 488
Компьютерное зрение, 530
Конические сечения, 747
Контрольные точки, 163
Контур, 640
Конфигурационное пространство, 131, 227
перечень характеристик, 231
Конфигурация
пространство, 198, 227
с замкнутым контуром («глаз в
руке»), 912
системы, 131
сочленений, 375
с разомкнутым контуром, 912
Координатные векторы, 73
Координатные метки, 792
Координатный вектор Плюккера, 991
Координаты
декартовы, 70, 71
неоднородные, 78
номинальные, 404
однородные, 78
пикселя и его индексы в массиве, 584
Плюккера, 431
сетки занятости и индексы матрицы –
не одно и то же, 263
координаты сочленений, 375
Кориолиса ускорение, 158
Кориолис, Гаспар-Гюстав, 157
Коррекция
дисторсии объектива, 766
перспективы, 881
Кососимметричная матрица, 104, 145, 149
Коэффициент
корреляции, 316
неоднозначности, 1052

Крена-тангажа-рыскания углы, 94
XYZ-порядок, 94
ZYX-порядок, 94
Криволинейные полиномы, 285
Крутящий момент, 474
рыскания, 222
тангажа, 222
Кручение, 84, 429
в 3D, 112
векторы, 84
винтовое движение, 112
единичное кручение, 84, 113, 126
момент, 84
полюс (центр поворота), 84
Кук, Джеймс, капитан, 304
Кулоновское трение и прямая
динамика, 505
Кулон, Шарль-Огюстен де, 476
Кумулятивное распределение, 318
Курс, 198

Л
Лаборатория приборостроения
Массачусетского технологического
института (MIT), 174
Лавлейс, Ада, 33
Лагранжа метод, 492
Лагранж, Жозеф-Луи, 495
Лампа накаливания, 532
Лапласа преобразование, 482
Лаплас, Пьер-Симон, 628
Лаццарини, Марио, 342
Левенберга–Марквардта, алгоритм
оптимизации, 465, 1028
Левообобщенное обращение, 1023
Левосторонние кватернионы, 109
Леклер, Жорж-Луи, 342
Летальные аппараты
винтокрылые, 219
с фиксированным крылом, 219
Летательные микроаппараты (micro air
vehicles, MAV), 219
Ли
алгебра, 83, 112
группы, 75
Лидар, 350
картографирование, 354
локализация, 356
одометрия, 352
применение, 350
Линеаризация с обратной связью, 513

Определение пика 1089
Линейная интерполяция, 167
Линейная пространственная
фильтрация, 614
Линии
Брезенхэма, 355
гребней, 627
TD, 306
Ли, Софус , 1011
Ловкость манипулятора, 415
Логарифм матрицы, 75, 83, 103
Локализация, 301
методом Монте-Карло, 343
на основе лидара, 356
по радио, 306
с помощью ориентиров на карте, 317
Локальная аппроксимация, 1017
Локально горизонтальная касательная
плоскость, 171

М
Магнитный север, 180
Магнитометры, 180
как работают, 180
калибровка, 184
оценка направления, 182
Маккалок, Уоррен, 41
Маккарти, Джон, 41
Максимально устойчивые экстремальные
области (MSER), алгоритм, 684
Малоприводность, 197, 227
Малоприводные (underactuated)
системы, 133
Малоприводный манипулятор, 407
Маневр, 197
Манипулируемость, 448
мера, 453
эллипсоид, 450
якобиан, 465
Манипулируемость, масштабирование
и единицы измерения, 466
Манипулятор, 368
кинематика, 438
ловкость, 415
малоприводный, 407
многоприводный, 409
планарный, 371
трехмерный, 376
якобиан, 438
PUMA, 376
SCARA, 368
Манхэттенский проект, 342

Манхэттенское расстояние, 261
Марра–Хилдрета оператор, 629
Марр Дэвид, 634
Масса звена, 477
Массив накопителей, 706
Математическая морфология, 641
Матрица, 980
антисимметричная, 76, 983
базисная, 1010
взаимодействия, 920
вращения, 72, 73, 104, 105
чтение, 91
гессиан, 1019
гомографии, 829
диагональная ковариационная, 320
единичная, 149
единичная диагональная, 982
единичный определитель, 121
инвертирование, 452
инерции, 497
информационная, 1037
камеры, 749
свойства, 769
квадратная, 455, 981, 984
ковариации, 317
Кориолиса, 499
Кориолиса и центростремительной
связи, 499
кориолисовой связи, 506
кососимметричная, 104, 149, 983
логарифм, 75
направляющих косинусов, 119
нормальная, 983
обратная, 452
однородного преобразования, 78, 109
определение, 980
ортогональная, 72, 89, 983
ортонормированная, 983
положительно определенная, 316
псевдообратная, 456
разложение Холецкого, 986
разреженная, 1033
ранг, 449
расширенная кососимметричная, 83, 112
симметричная, 983
собственные векторы, 984
собственные значения, 984
с полным рангом, 456
таксономия, 981
транспонирование, 981
усиления Калмана, 332
чувствительность к признакам, 920

1090 Определение пика
экспонента, 75, 83
якобиан, 1020
Якоби (якобиан), 440
SE(3), 168
Матричные экспоненты, 103
Махаланобиса расстояние, 325
Маховое движение лопастей винта, 221
Мгновенная прямая кинематика, 441
Мгновенный центр поворота (МЦП), 199
Меканум, 215
Мера манипулируемости, 453
Меркатора поперечная проекция, 249
Местоположение рабочего органа, 439
Метод
Ньютона, 1027
Ньютона–Рафсона, 1025
k средних, 675
Метрополис, Николас, 342
Мешок слов, 727
Микроэлектромеханические системы
(MEMS), 172
Минимальная траектория рывка, 160
Мински, Марвин, 41
Многокамерные массивы, 788
Многокоординатные траектории, 162
Многомерное нормальное
распределение, 1036
Многоприводный манипулятор, 409
Многорукие роботы, 387
Многосегментные траектории, 163
Мобильные роботы, 36, 38
визуальное управление, 964
воздушные, 218
всенаправленные, 215
дифференциальное рулевое
управление, 212
локализация и картографирование, 301
подводные, 220
SLAM, 301
Моделирование
робота, 308
сочленения робота, 481
Модель
двухколесная, 198, 209
квадрокоптера, 224
кинематическая, 209, 212
одноколесная, 209, 212
центральной проекции, 751
Модульная арифметика, 608
Мозаика, 884
Момент
инерции ротора, 172
кручения, 84

Моменты и силы, 461
Монадические операции, 602
Моноцикл, 213
Монте-Карло метод, 313
локализация, 343
Моравека детектор углов, 711
Моравека оператор, 711
Морская миля, 302
Муаровый узор, 653

Н
Навигационные углы, 94
Навигация, 238
инерциальная, 152
ориентир, 302
по картам, 246
по карте, 239
по радио, 306
реактивная, 238
роботов, 238
счисление пути, 301
эфемериды, 304
Надежная оценка с помощью
RANSAC, 825
Найквист, Гарри, 41
Направление
восток-север-вверх, 171
движения, 216
качения, 216
север-восток-вниз, 171
Неголономное ограничение, 201, 215, 230
Неголономность, 197
Неголономные и малоприводные
системы, 227
Неголономный мобильный робот, 966
Недоопределенная система, 455
Неинерциальная система координат, 156
Нейман, Джон фон, 342
Нелинейная минимизация, 1026
методом наименьших квадратов, 1027
Нелинейные задачи, 1025
Нелинейные операции, 638
Ненулевой крутящий момент, 494
Неоднородные координаты, 78
Неопределенность, 316
местоположения, 316
Непараметрические локальные
преобразования, 637
Неперспективные камеры, 798
Неподвижная система координат, 171
Нобля детектор, 714

Определение пика 1091
Номинальные координаты, 404
Нормализация, 121
Нормальное распределение, 1035
Норма расстояния ZNCC, 811
Нулевое (пустое) движение, 62
Нулевой поворот, 174
Нуль-пространство, 458
движение сочленений, 459
Ньютона второй закон, 221, 492
Ньютона–Эйлера рекурсивный
алгоритм, 492
Ньютон, Исаак, 153
Ньюэлл, Аллен, 41

О
Облака точек, 875
Обнаружение
движения, 612
объектов с использованием глубокого
обучения, 701
Обновление, 312
Обработка изображений, 582
Обратная засечка, 303
Обратная кинематика, 369, 399
аналитическое решение, 400
численное решение, 402
Обратная матрица, 452
Обратная проекция, 842
Обратная частота документа, 732
Обратная ЭДС, 474, 488
Обратное движение, 62
Обратное относительное положение, 155
Обусловленность якобиана
и манипулируемость, 448
Объектив «рыбий глаз», 772
Объект робота, 380
Ограничение
качения, 230
неголономности, 201, 215, 230
скорости, 230
Ограничения двигателей, 487
Одновременные локализация и
картографирование (SLAM), 330
Одноколесная модель, 211, 212
Однородные координаты, 78
Однородные преобразования, 109
Однородные преобразования 2D, 76, 78
Одометр, 307
Одометрия, 307
на основе лидара, 352
шум, моделирование, 310

Оператор
@, 175
•, 66
⊕, 62, 65
⊖, 63, 66
Моравека, 711
Операции с изображениями
диадические, 607
дилатация, 643
монадические, 602
нелинейные, 638
пространственные, 614
эрозия, 643
Описательные параметры блобов, 691
Опорная область, 721, 809
Определение
глубины по времени пролета луча, 875
или уточнение оценки положения
пика, 1051
краев на изображении, 621
положения откалиброванной
камеры, 770
положения пика (одномерный
сигнал), 1051
робота, 241
столкновений, 432
Определитель гессиана, 715
Опсины, 540
Оптические датчики, 320
Оптический поток, 923
Оптический сенсор, 752
Ориентация, 87
в трех измерениях, 87
интерполяция, 165
транспортного средства, 198, 209
Ориентир, 302, 325
идентификация, 325
искусственный, 325
создание карты , 326
Оси
вращения, 145
фиксированные, 166
Относительное положение, 61, 66
Отношения сил, 461
Отступление камеры, проблема, 953
Оценка
глубины объекта, 933
местоположения в пространстве, 301
положения, 311
стоимости предстоящего перехода, 259
Ошибка
позиционирования, 226
ребра, 337

1092 Определение пика

П
Пакетная подстройка, 841
Парабаллоид, 1054
Параметры Эйлера, 103
Пассивное вращение, 118
Переопределенная система, 455
Перспективная камера, 744
моделирование, 748
Перспективная проекция, 743
неоднозначность, 754
Пикселизации, 658
Пирамиды изображений, 654
Питтс, Уолтер, 41
Планарная гомография, 793, 828
Планирование
маршрута, 239, 252
в конфигурационном
пространстве, 287
криволинейные полиномы, 285
локальное и глобальное, 293
по картам, 246
по карте занятости, 261
по решетке, 280
проходимого, 277
с использованием дорожных карт, 271
с минимальным временем, 259
Планковский источник, 532. См. Черное
тело
Пленоптические камеры, 789
Плесси детектор углов, 714
Плоскость изображения, 746
Плотное стереозрение, 850
Плотное стереосовмещение, 852
Плотность магнитного потока, 180
Плохая обусловленность якобиана, 450
Плюккера координаты, 431
Поведение поиска цели, 244
Поворот
вокруг произвольного вектора, 101
системы координат, 118
точки, 118
Подавление
возмущений, 488
нелокальных максимумов, 1053
Подводные роботы, 220
Поза, 60
Поиск
изображений, 727
корней уравнений, 1025
плоскости, 877
с равномерной стоимостью, 254
A*, 257

Полевые роботы, 38, 57
Полезная нагрузка, влияние, 499
Поле зрения объектива, 755
Полноприводность, 228
Положение, 60
абстрактное ξ, 61
в двух измерениях, 76
граф, 66
изменение во времени, 144
конкретное
как единичный дуальный
кватернион, 116
как единичный кватернион, 108
как кручение в 2D, 86
как кручение в 3D, 115
как матрица SE(3), 111
как матрица SO(3), 92
как пара вектор–кватернион, 116
как SE(2), 78
оценка, 311
скорость изменения, 146
Положительно определенная
матрица, 316
Получение
изображений, 583
признаков из точек, 710
Полюс, центр поворота, 85
Понимание геометрии зрения, 760
Поп-арт-постеризация, 606
Пороговая бинаризация, 669
Портальный робот, 368
Последовательное преобразование, 637
Последовательность элементарных
преобразований (elementary
transformation sequence, ETS), 372
Поступательное движение, 126
Потенциальные соответствия, 811
Правило
поворота вокруг вектора, 88
правой руки, 86
Представление экземпляра объекта, 681
Преобразование
Лапласа, 482
однородное в 2D, 76
подобия, 1008
«попадание или промах», 647
пространственных скоростей, 147
расстояния, 649
алгоритм, 261
сил и моментов, 155
сферического изображения
в перспективное, 786

Определение пика 1093
твердого тела, 61
Хафа, 707
Преодоление трения, 476
Прецессия, 172
Привод, 473
гидравлический, 473
переменной жесткости (Series-Elastic
Actuator, SEA), 520
пневматический, 473
Призматические сочленения, 376
Признаки изображений, 665
адаптивный порог, 672
беспороговый подход, 674
бинарная классификация, 669
гештальт-принцип
эмерджентности, 685
графовая сегментация, 684
детектор
Нобля, 714
Плесси, 714
Харриса, 714
Ши–Томаси, 714
иерархия блобов, 694
инварианты моментов, 697
информация о форме
из моментов, 696
из периметра, 698
классификация
монохромных изображений, 669
пикселей, 668
цветных изображений, 673
классические детекторы углов, 711
максимально устойчивые
экстремальные области (MSER),
алгоритм, 684
метод k средних, 675
мешок слов, 727
моменты, 687
обнаружение объектов
с использованием глубокого
обучения, 701
ограничивающие рамки, 686
оператор Моравека, 711
описание экземпляра объекта, 668
описательные параметры блобов, 691
опорная область, 721
определитель гессиана, 715
площадь, 686
поиск изображений, 727
получение из точек в масштабном
пространстве, 723
получение признаков из линий, 705
получение признаков из точек, 710

пороговая бинаризация, 669
представление экземпляра
объекта, 668, 681
преобразование Хафа, 707
распознавание символов, 725
сегментация, 666
семантическая классификация, 668, 680
суть сцены, 665
угловые детекторы в масштабируемом
пространстве, 718
Приложения
создание надписи на поверхности, 417
четвероногий шагающий робот, 419
Принцип подобия треугольников, 746
Приращение поворота, 173
Проблемы большого масштаба, 1032
Прогнозирование, 312
Проективное пространство, 78
Проецирование произвольных линий, 796
Произведение
Гамильтона, 116
тензора mode-1, 510
экспонент, 429
Производная векторной функции от
векторного аргумента, 440
Производная гауссиана, 625
Производная якобиана по времени, 510
Прокрустово преобразование, 1008
Промежуточные точки, 163
Промышленные роботы, 35, 36
Пропорционально-дифференциальный
регулятор, 223
Пропорционально-интегральнодифференциальный регулятор, 225, 489
Проприоцепция, 42
Пространственная скорость, 438, 920
Пространственное смещение, 152
Пространственные операции, 614
Пространственный полосовой
фильтр, 626
Пространство
задач, 375
конфигураций, 131
проективное, 78
сочленений
скорость, 440
степени свободы, 441
Противники (outlier), 824
Пружина–масса–амортизатор,
система, 514
Прямая динамика, 503
Прямая кинематика, 369

1094 Определение пика
на основе графов положений, 370
Псевдообратная матрица, 456
Псевдообратный якобиан
Мура–Пенроуза, 454
Псевдообращение, 1023
Пульсации крутящего момента, 480
Путь, планирование движения, 207

Р
Рабочий орган, 367
динамический винт, 461
местоположение, 439
скорость, 440
якобиан в системе координат, 442
Радиальная дисторсия, 759
Радио- или оптические датчики
обнаружения и измерения дальности, 320
Радиометрические и фотометрические
величины, 541
Разделимые фильтры, 619
Разложение
калибровочной матрицы камеры, 767
Холецкого, 986
Разность гауссианов, 630
Разреженные матрицы, 1033
Разреженные нелинейные методы
наименьших квадратов, 1029
Разрыв
скорости, 170
ускорения, 170
Ранговое преобразование, 637
Рао–Блэквелла теорема, 349
Расстояние городских кварталов, 261
Расфокусировка, 616
Расширение размеров препятствий, 267
коэффициент расширения, 267
Расширенная кососимметричная
матрица, 112
Расширенное стереосопоставление, 863
трехмерная реконструкция, 864
Расширенные кососимметричные
матрицы, 83
Расширенный фильтр Калмана, 1044
Реактивная навигация, 238
Регулятор
ориентации, 224
пропорциональнодифференциальный, 223
пропорционально-интегральнодифференциальный, 225, 489
пропорциональный, 203, 206

с вычисляемым крутящим
моментом, 507
Редуктор, 479
Реперные маркеры, 194
Репрезентативная сингулярность, 444
Ретинекса теория, 564
Решение систем уравнений, 1023
Ридса–Шеппа путь, 279
Робот
многорукий, 387
мобильный, 38, 195
определение, 42, 241, 246
полевой, 38
сервисный, 38
типы, 36
хирургический, 40
человекоподобный, 38
PR2, 394
Shakey, 257
Роботизированные лодки, 195
Робот-манипулятор, визуальное
управление, 962
Родригеса формула поворота, 102
Родригес, Олинде, 102
Россум, Гвидо, 49
Рулевое управление Аккермана, 200
Рыскания угол, 198

С
Саймон, Герберт, 41
Саккула (сферический мешочек), 178
Сатуратор, 483
Свет и цвет, 531
баланс белого, 565
баланс цветов, эксперименты, 548
белый цвет, 562
воспринимаемая яркость, 542
воспроизведение цветов, 546
гамма, 569
гамма-коррекция, 570
дальтонизм, 545
двухцветное отражение, 568
излучающее черное тело, 532
изменение из-за поглощения, 566
измерение цвета, 543
инфракрасное излучение, 534
колориметрия, 551
ламбертово отражение, 536, 568
лампа накаливания, 532
названия цветов, 554

Определение пика 1095
насыщенность, 556
опсины, 540
основные цвета, 546
отражение, 536
оттенок, 556
поанковский источник, 532
поглощение, 535
постоянство цвета, 564
преобразование между основными
цветами, 559
радиометрические и фотометрические
величины, 541
сенсор в цифровой камере, 542
спектр, 532
спектральное представление света, 531
сравнение цветовых пространств, 571
теория противоположных цветов, 541
тетрахроматы, 539
трехкомпонентная теория, 540
трихроматы, 539
удаление теней, 574
френелевское отражение, 568
цвет, 538
цветность, 549
цветовая температура, 563
цветовое пространство, 549
цветовые пространства и гамма, 570
человеческий глаз, 538
экспонометры, освещенность
и яркость, 544
яркость, 536
яркость сцены, 564
Свет онохроматический, 532
Свойства
гауссиана, 620
матрицы камеры, 769
свертки, 616
Свон, Джозеф, 534
Сглаживание изображений, 616
Сегментация изображения, 666
Семантическая классификация, 668, 680
Сенсор в цифровой камере, 542
Сенсорные датчики, 171
Сервисные роботы, 38
Сервомеханизм, 474
Сервопривод, 474
Сервоусилители, 474
Сетка занятости, 244
создание, 263
Сила
Даламбера, 156
трения, 498
тяжести, 494

Силы
и моменты, 461
инерциальные, 156
кажущиеся, 156
отношения, 461
эллипсоид, 462
Сингулярная конфигурация, 454
Сингулярность, 414
Система координат, 71, 86
базисные векторы, 64, 72, 86
глобальная, 64
инерциальная, 156
корпуса, 171
мировая, 64
начало, 64
неинерциальная, 156
неподвижная, 171
ориентация, 65
оси, 64
перемещение, 66
правосторонняя, 64, 86
тела, 64
транспортного средства, 200
Система координат звена, 378
Скалярные траектории, 162
Скелетизации алгоритм, 272
Скобки Ли, 1010
Скорость
в пространстве конфигурации
сочленений, 440
вращения колеса, 216
вращения ротора, 172
изменения
ориентации, 145, 226
положения, 146
качения вбок, 216
рабочего органа, 440
угловая, 173
эллипсоид, 450
Слияние инерциальных датчиков, 184
Случайная выборка и консенсус,
алгоритм, 824
Случайные числа, 275
Смещение, 185
оценка, 186
пространственное, 152
Смитсоновский музей американской
истории, 376
Собственное ускорение, 177
Собственные векторы, 101, 102
Собственные значения, 102
Совмещение признаков, 808

1096 Определение пика
Создание
карты ориентиров, 326
кинематической модели робота, 425
надписи на поверхности, 417
Соотношение сторон изображения, 596
Сопоставление двух наборов точек, 878
Сочленения, 369, 473
винтовые, 370
высшие пары, 370
движение в пространстве
сочленений, 410
конфигурация, 375
координаты, 375
моделирование, 481
независимое управление, 473
низшие пары, 370
поворотные, 369
призматические, 369, 376
скользящие, 369
сферические, 370
телескопические, 369
углы, 375
цилиндрические, 370
Спектральное представление света, 531
Спектральный импульс, 532
Спектр света, 532
Срезы пространства диспаратности, 862
Стабилизированный элемент, 96
Стандартное нормальное
распределение, 1035
Степени свободы, 99
в пространстве сочленений, 441
Стереозрение, 835
плотное, 850
разреженное, 835
трехмерная триангуляция, 836
Стереоочки, 90
Стефана–Больцмана закон, 533
Стоимость
перехода, 254
пройденного пути, 254
Сторонники (inlier), 824
Структура из движения, алгоритм, 842
Структурированный свет, 872
Структурирующий элемент, 641
Стэнфордский манипулятор, 376
Субпиксельная интерполяция, 1055
Суммарный крутящий момент, 222
Существенная матрица, 820
Сферическая аберрация, 758
Сферическая камера, 779
Сферическая линейная

интерполяция, 167
Сферическое запястье, 402
Сходство изображений, 632
Счисление пути, 301
с использованием одометрии, 307

Т
Твердое тело
движение, 61, 152
компенсация динамики, 505
преобразование, 61
уравнения движения, 492
Твердотельные усилители мощности, 474
Тейта–Брайана углы, 94
Тейт, Питер, 94
Телероботы, 40
Темновой ток, 593
Тензор
инерции, 393
инерции вращения, 154
Теорема
Байеса, 313
Брокетта, 231
вращения Эйлера, 92
Шаля, 125
Теория
графов, 1047
групп Ли, 75
противоположных цветов, 541
Тесла, Никола, 40
Типичные проблемы
стереосопоставления, 856
количественная оценка, 860
множественные пики, 856
слабое совпадение, 857
широкие пики, 860
Типы роботов, 36
Тождественный (нейтральный)
кватернион, 108
Точки
евклидовы, 81
проекции, 755
Траектории, 410
гибридные, 161
многокоординатные, 162
многосегментные, 163
скалярные, 162
трапециевидные, 161
Трапециевидные траектории, 161
Трение, 475, 498
вязкое, 512

Определение пика 1097
кулоновское, 476
кулоновское и прямая динамика, 505
Штрибека, 475
Трехмерная реконструкция, 864.
См. Триангуляция
Трехмерная триангуляция, 836
Триангуляция, 303
Трихроматы, 539
Тьюки функция, 1029
Тьюринг, Алан, 41

У
Угловая скорость, 145, 173
Угловые детекторы в масштабируемом
пространстве, 718
Угловые признаки Харриса, 808
Углы
Кардано, 88, 94
крена, 94
крена тангажа рыскания, 162, 169
курсовые, 94
навигационные, 94
наклонения, 180
падения, 180
поворота рулевого колеса, 200
поворота управляемых колес, 200
положнения, 94
разность, 203
рыскания, 94, 198
склонения, 180
сочленений, 375
тангажа, 94
Тейта–Брайана, 94
управляемых колес, 201
Эйлера, 88, 93, 99, 162
Угол между векторами, 980
Угол-ось представление, 149
Удаление теней, 574
Удельное ускорение, 177
Улам, Станислав, 342
Ультрафиолетовые камеры, 576
Универсальные роботы Россума, 38
Унифицированная модель формирования
изображений, 781
Унифицированный формат описания
роботов (Unified Robot Description Format,
URDF), 390
Уолд, Джордж, 540
Уолли задача поиска, 634
Уолтер, Уильям Грей, 240
Упорядочение элементов
кватерниона, 109

Управление
бортовое, 214
в операционном пространстве, 516
движением объекта, 927
дифференциальное, 214
импедансное, 513
моментом с упреждением, 488
на основе зрения, 912
направлением бокового качения, 216
податливостью, 516
положением, 489
пропорциональное, 206
скоростью, 202
вращения винтов, 227
с прямой связью, 506
Уравнение
движения
двухколесной модели, 198
моноцикла, 214
твердого тела, 492
кватерниона, 107
Ускорение, 153
инерционное, 177
Кориолиса, 158
свободного падения, 176, 178
нисходящее, 176
собственное, 177
удельное, 177
центробежное, 176
центростремительное, 158
Утка Вокансона, 33
Уточнение положения пиков, 855
Утрикула (эллипсовидный мешочек), 178

Ф
Фаза запроса, 271
Ферромагнитные материалы, 184
Фиксированные оси, 166
Фильтр
Калмана, 1041
Калмана–Бьюси, 1043
частиц, 346
Фокус и глубина резкости, 748
Форматы
видеофайлов, 595
файлов изображений, 585
Формирование изображений, 743
Формовка инерции, 514
Формула
вращения Родригеса, 1013
Родригеса, 102

1098 Определение пика
Фотометрические и радиометрические
величины, 541
Фотоны и значения пикселей, 593
Фототаксис, 241
Фрикционный элемент, 177
Фундаментальная матрица, 817
вычисление по реальному
изображению, 822
Функция плотности вероятности
(PDF), 305, 1035

Х
Харриса детектор, 714
Хартенберг, Ричард, 397
Хафа преобразование, 707
Хирургические роботы, 40
Холла датчики, 180
Холл, Эдвин, 183
Хромакей, 609
Хроматическая аберрация, 758
Хьюбера функция потерь, 1029

Ц
Цвет, 538
баланс белого, 565
баланс цветов, эксперименты, 548
белый цвет, 562
воспроизведение цветов, 546
гамма, 569
гамма-коррекция, 570
дальтонизм, 545
двухцветное отражение, 568
изменение из-за поглощения, 566
измерение, 543
колориметрия, 551
названия цветов, 554
насыщенность, 556
основные цвета, 546
оттенок, 556
постоянство цвета, 564
преобразование между основными
цветами, 559
сравнение цветовых пространств, 571
удаление теней, 574
френелевское отражение, 568
цветность, 549
цветовая температура, 563
цветовое пространство, 549
цветовые пространства и гамма, 570
яркость сцены, 564

Цветность, 549
Центр масс, 385
Центробежное ускорение, 176
Центроиды точек, 912
Центростремительная сила, 230
Центростремительное ускорение, 158

Ч
Чапек, Карел, 33
Человекоподобные роботы, 38
Четвероногий шагающий робот, 419
Численная обратная кинематика, 463
Чистое преследование, 207
Чистый кватернион, 116

Ш
Шагающий робот, 369
Шаг винта, 126
Шаль, Мишель, 127
Шведское колесо, 215
Шейнман, Виктор, 376
Шеннон, Клод, 41
Широкоугольные камеры, 771, 952
Широтно-импульсная модуляция, 474
Ши–Томаси детектор, 714
Шмидт, Стэнли Ф., 314

Э
Эвристическое расстояние, 257
Эдисон, Томас, 534
Эйлера уравнение движения , 492
Эйлер, Леонард, 92, 248
Эйлерова теорема вращения, 92
Эйлеровы векторы, 103
Эйлеровы параметры, 103
Эйлеровы углы, 92, 99
Эйнштейн, Альберт, 156
Эквивалентный эллипс, 689
Экспонента матрицы, 75
Экспоненциальное отображение, 122
Экспоненциальные координаты, 511, 1013
Экспонометры, освещенность
и яркость, 544
Экстероцепция, 42
Экстраполяция за границы
изображений, 620
Электродвигатель, 473
Электронные регуляторы скорости, 474

Определение пика 1099
Эллипсоид
сил, 462
скорости и манипулируемость, 450
Эллипсы ошибок, 318
Энкодер, 483
Эпиполярная линия, 814
Эпиполярная плоскость, 814
Эпиполярное ограничение, 852
Эрозия, 643
Этические аспекты робототехники, 45
Эфемериды, 304
Эффект
воздушной подушки, 221
желе, 593
рычага, 478

Я
Явный дополнительный фильтр, 187
Якобиан, 312, 1020. См. Матрица Якоби
аналитический, 443

в системе координат рабочего
органа, 442
вырожденный, 448
геометрический, 441
изображения, 920
и манипулируемость, 448
малоприводного робота, 456
манипулируемости, 465
манипулятора, 439, 510
манипулятора с использованием
кручений, 465
матрица, 439
многоприводного робота, 458
неквадратный, 455
обусловленность, 448
плохая обусловленность, 450
псевдообратный Мура–Пенроуза, 454
ранг, 449
Якоби, Карл, 440
Якоби матрица, 312
Янг, Томас, 540

Книги издательства «ДМК Пресс»
можно купить оптом и в розницу на складе издательства по адресу:
Москва, ул. Электродная, д. 2, стр. 12, офис 7, тел. +7 (499) 322-19-38,
а также заказать на сайте www.dmkpress.com
с доставкой в любой регион РФ

Питер Корк

Машинное зрение и управление в робототехнике
Фундаментальные алгоритмы на Python
Главный редактор

Яценков В. С.

dmkpress@gmail.com

Перевод
Корректор
Верстка
Дизайн обложки

Киселев А. Н.
Синяева Г. И.
Чаннова А. А.
Трофимова С. В.

Гарнитура PT Serif. Печать цифровая.
Усл. печ. л. 89,38. Тираж 200 экз.
Веб-сайт издательства: www.dmkpress.com