Рекуррентный метод сглаживания кривизны траекторий в задачах планирования пути для колесных роботов Текст научной статьи по специальности «Математика»

У

правление подвижными объектами

УДК 519.6

РЕКУРРЕНТНЫЙ МЕТОД СГЛАЖИВАНИЯ КРИВИЗНЫ ТРАЕКТВРИЙ В ЗАДАЧАХ ПЛАНИРОВАНИЯ ПУТИ ДЛЯ КОЛЕСНЫХ РОБОТОВ1

Р.Ф. Гилимьянов

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

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

ВВЕДЕНИЕ

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

Построенная траектория должна удовлетворять ограничениям, наложенным на неголономную систему (колесный робот). Поскольку колесный ро-

1 Работа выполнена при финансовой поддержке Программы № 15 ОЭММПУ РАН и государственной программы поддержки ведущих научных школ РФ (НШ-1676.2008.1).

бот не может двигаться в боковом направлении без проскальзывания, угол поворота передних колес и скорость их вращения ограничены, то траектория должна быть гладкой в смысле плавности графика кривизны, дважды непрерывно-дифференцируемой, т. е. принадлежать классу С , а также ее кривизна не должна превышать максимального значения ктах = 1/^т1п, соответствующего минимальному радиусу ^тт разворота колесного робота. Также важно уметь эффективно вычислять расстояние от робота до траектории. Под гладкостью траектории и кривой в смысле плавности графика кривизны в данной статье будем понимать следующее. Кривая гладкая, если график ее кривизны непрерывен, имеет соответствующий знак и представляет собой кусочно-монотонную функцию с как можно меньшим числом участков монотонности [1, 2].

Существуют множество способов представления пути, например, с помощью последовательности прямых линий и дуг окружностей, полиномиальных сплайнов, клотоид, или обобщенных спиралей Корню. Один из таких способов основывается на аппроксимации траектории с помощью однородных кубических Б-сплайнов [3] (в данной статье также вводится функция квазирасстояния

для оценки отклонения робота от целевой кривой). В результате такой аппроксимации получается C -гладкая кривая. Однако малые ошибки GNSS-измерений координат точек приводят к значительному искажению кривизны траектории. Кривизна хаотически меняется и может превышать максимальное значение kmax. На практике это приводит к ухудшению качества управления.

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

Существуют различные методы получения гладких сплайновых кривых, например, с помощью сглаживающих сплайнов (smoothing spline) [4—7] и фэринга (fairing) [1, 2, 8—12]. Широкое распространение получили следующие два подхода. В первом из них процесс построения кривой совмещен с ее сглаживанием, в другом подходе сначала строится аппроксимирующая или интерполирующая кривая, а затем производится ее сглаживание.

Ко второму подходу относится метод, представленный в работе [12]. В его основе лежит задача квадратичного программирования, сложность и время решения которой нелинейно возрастает с ростом числа точек. Для повышения эффективности в настоящей работе предлагается перейти от задачи условной минимизации к задаче безусловной минимизации, для решения которой строится рекуррентная схема. На основе полученной схемы разработаны методы со скользящим окном, позволяющие сглаживать кривизну длинных траекторий при ограничениях на оперативную память и в режиме реального времени с небольшой задержкой. Помимо всего перечисленного, последнее может быть также полезным в задачах группового управления (formation control). В настоящей статье приводятся результаты применения этих методов к данным реальных GNSS-измерений, а также результаты сравнения с предыдущим методом, описанным в работе [12].

1. ЗАДАЧА УСЛОВНОЙ МИНИМИЗАЦИИ

Введем обозначения и кратко повторим основные моменты работы [12]. Пусть есть набор равноточных GNSS-измерений r1, r2,..., r , г. е R , т. е. предполагается, что шумы измерений имеют одинаковую дисперсию. Пусть расстояние между любыми двумя соседними контрольными точками

приблизительно одинаково: ||г/ + 1 — г/|| « ||г. + 1 — г.||, где ||^|| — евклидова норма вектора. Искомая траектория аппроксимируется однородными кубическими Б-сплайнами, каждый из которых строится по четверке контрольных точек следующим образом

Ло = я^т, л = и - р /•,, г,+р г,+2],

M = 1[

1 -3 3 -1 1

4 0 -6 3 , T(t) = t t2

1 3 3 -3

0 0 0 1. _t3_

t е [0,1],

где ? — параметр сплайна.

Небольшой шум измерений существенно влияет на кривизну, определяемой формулой к(?) =

= || г (?) х г (?)||/|| г (?)||3. Как будет видно из результатов численного эксперимента, кривизна траектории, построенной таким образом по исходным данным имеет неудовлетворительный вид. Колесному роботу с ограниченными углом и скоростью поворота передних колес сложно точно повторить траекторию с такой кривизной в автоматическом режиме.

В случае равномерно расположенных точек норма вектора первой производной по параметру сплайна || г (?)|| примерно постоянна, и кривизна кривой пропорциональна норме второй производной сплайна: к ~ ||г(?)||. Последняя является непрерывной функцией на каждом элементарном сплайне и в точке соединения с соседним сплайном

принимает значения г(-) (0) = г(- - ^ (1) = г{ -1 —

— 2г;. + г. + 1. Производная кривизны к ~ || г (?)|| постоянна на протяжении одного элементарного сплайна г(-) = — г. _ 1 + 3г;. — 3г; + 1 + г. + 2 и имеет разрыв в точке соединения с соседним сплайном

А?. ^ г«) _ (- 1) = ^ _ 2 - 4г, _ 1 + 6г-. - 4г, + 1 + г, + 2. Эти скачки третьей производной в точках соединения двух смежных сплайнов влияют на характер графика кривизны.

В статье [12] предлагается путем малых (в пределах ошибки измерений 8) вариаций е. контрольных точек в перпендикулярном к кривой направлении (малый сдвиг вдоль кривой не сильно меняет ее форму) г{ = г. + N. е. минимизировать проекции скачков третьей производной (А?1, N ), где N. — нормаль к Б-сплайновой кривой в точке

соединения /-го и (/ + 1)-го элементарных сплайнов. Запишем в вариациях проекции скачков

Fi(г) = F(0) + Щ - 2, - 2 - 4(^ - р Ц)г{ - х +

+ Ц - 4(^ + !, М)г, + ! + (Л, + 2, Щ*, + 2, (1)

где F(0) = (А г,, ^-) = (г, - 2, ^) - 4(г, - р Ж,) + 6(г,, ^) - 4(г, + !, Щ + (г, + 2, Щ.

Для вычисления значений F1, F2 и Fn - р Fn нужны дополнительные точки: две точки г_1, г0 в начале и две точки гп + 1, гп + 2 в конце траектории. Для них положим Б-1 = е0 = *п + 1 = *п + 2 = 0. О том, как выбирать дополнительные граничные точки и на что этот выбор влияет, см. работу [12].

т

Введем векторные обозначения е = [*1, ..., *п] , т

F(е) = [/,.(*), ..., Fn(E)] и перепишем выражение (1) в матричной форме

Де) = F(0) + Се, (2)

где С — пятидиагональная симметричная матрица

6 -4 п12 п13 0

п 1 6 -4 п23 п24

п31 -4п32 6 -4 п34

0 п42 -4п43 6

п.. = (Ж, Ж) — скалярное произведение двух нормалей. Таким образом, минимизация скачков третьих производных свелась к минимизации квадратичного функционала ||Р(е)||2. Возводя выражение

(2) в квадрат и отбрасывая свободный член, получаем квадратичный функционал

Фс(е) = 2 вТНсв + /е, Нс = СТС, / = СТД0),

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

тіпФс(е), е є К”, —8 < е < 8. (3)

Є с .

С ростом числа точек нелинейно возрастает сложность данной задачи и время ее решения. В общем случае задача квадратичного программирования является ЖР-сложной. В нашем случае, при положительно определенной матрице Ис, задача

(3) решается за полиномиальное время и требует

выполнений около 0(прЬ) арифметических операций (см., например, книгу [13] и приведенные там ссылки), где Ь — длина входа задачи (величина, определяющая число двоичных символов, необхо-

димых для записи входной информации задачи), р > 1, например, при решении задачи методом внутренней точки р = 3,5.

2. ЗАДАЧА БЕЗУСЛОВНОЙ МИНИМИЗАЦИИ

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

Ф(е) = Фс(е) + 2 У*Те = 2 еТНе + /те,

Н = Нс + у/,

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

тш Ф(е), е е Рп. (4)

£

Ее решение сводится к решению системы линейных уравнений

Не = -/т (5)

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

угольных ленточных матриц Н = ЬЬ и методом прямой и обратной прогонки решить ленточные треугольные системы

Ьу = -/т, (6)

ЬТе = у. (7)

При нижней ширине р ленты матрицы Ь, равной 4, решение системы (5) требует выполнение около 46п операций сложений и умножений, а также п вычислений квадратных корней [14]. Это гораздо меньшее число вычислений по сравнению с решением задачи квадратичного программирования (3).

3. РЕКУРРЕНТНАЯ СХЕМА

Построим рекуррентную схему решения задачи

(4). Пусть есть измерения координат точек г1, г2, ..., гк + 1. Добавим две граничные точки г-1, г0 в начале траектории. В качестве двух конечных граничных точек в конце траектории возьмем точки гк, гк + 1 (напомним, что координаты граничных точек не варьируются). По данным измерениям построим матрицы Ск - 1, Нк - 1 и Ьк - 1 размера

(к - 1) х (к - 1) и векторы Fk 1(0), /к 1, ук 1, к - 1

е размера к - 1.

Рассмотрим, как они изменятся, когда появится новое измерение гк + 2 и граничными конечными точками станут точки гк + 1, гк + 2.

• К вектору F(0) добавится новый элемент F]^ (0).

• К матрице С добавятся строка и столбец: Ск =

Ck-1 cA

c/fe ckk

,ck = [0

, 0, ck - 2, k, Ck - 1, k] , Ckk 1

• У вектора / изменятся два последних элемента и

добавится новый /к :/к = [/-1 )Т, /к ]Т, /к-1 —

это вектор /к 1, к последним двум компонентам которого прибавлены компоненты вектора

*£(0)[Ск-2, к, Ск-1, к]Т, /кк = [*12(0), ^кк-1 (0),

(0)][ск - 2, к, Ск - 1, к, Скк] .

• У матриц Н и Ь изменится правый нижний блок размером 2 х 2 и добавятся строка и стол-

бец: Hk =

H

k -1

hk

h/fe hkk + У

~ k -1

, где матрица H —

rk - 1

это матрица H которой прибавлена матрица D =

к правому нижнему углу

d1 d3 d2 d4

2

ck - 2, k

ck - 2, k ck - 1, k 2

, d2 d3, [ hfe hkk]

_ck - 2, kck - 1, k ck - 1, k

= [cT ckk]Ck(1 : k, k - 4 : k).

k

• L =

Lk -1 0

iT і _ lk

rk-1

'k -1

, где L представляет собой

матрицу L за исключением трех элементов

= fLF-T

VLk - 2,

Lk 1

Lk - 2, k - 2

+ d1

rk -

"k - 2, k - 2 + d1 ^ d2)

Lk 1

Lk - 1, k - 2

__ / t k - 1 j k - 1 л \ / tk 1 Tk 1 _

= (Lk-2, k-2 Lk- 1, k-2 + d2)/ Lk-2, k-2 , Lk- 1, k- 1 =

— І~і~ї~Ік~~1 Г2 / гк-1 ч2 / гк-1 ч2

= V(Ьк- 1, к-2І -(Ьк- 1, к-2) + (Ьк- 1, к- 1) + й4 .

Имея Ьк 1 — разложение Холецкого матрицы Нк 1 и строку [ , Нкк + у], можно досчитать

разложение Ьк и найти строку [ ^ /кк].

У вектора решения у системы (6) изменятся два последних элемента и добавится новый ук, все

их можно найти за три шага прямой прогонки

к г/ к—1/17 о\\Т к к к-|Т

у = [(у (1 : к - 3)) , Ук-2, Ук-1, Ук ] .

• Вектор решения е системы (7) изменится полностью. Вектор *к получается полной обратной прогонкой.

Для возможности вести прямую прогонку и получать решение у системы (6) при появлении нового измерения достаточно иметь не более пяти последних элементов векторов F(0), / и правый нижний блок размера 5 х 5 матриц С, Н и Ь. Для получения е нужно иметь всю матрицу Ь и вектор у.

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

4. МЕТОДЫ СО СКОЛЬЗЯЩИМ ОКНОМ

Замечено, что для достаточно большого к < п

к - 1 к

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

тем, что у задач с матрицами Нк 1 и Нк одинаковые граничные условия в начале и разные в конце. Поэтому вместо решения системы (5) с матрицей

Нк можно взять первые к - I компонент решения

*к 1 системы (5) с матрицей Нк 1, а оставшиеся

компоненты *к найти за I шагов обратной прогон-

ки системы (У) с матрицей (Lk)T:

єk = [^k-1(1 : k

or

k

єk-l+1■

kT ^] .

Можно построить рекуррентный метод сглаживания кривизны траектории, в котором используется скользящее окно (sliding window) фиксированного размера l х l, l1 p + 1, p = 4. Матрица L и вектор у хранятся в массивах размерности l. При заполнении массива делается сдвиг, в результате которого стирается первый элемент и освобождается последний. Схематически данный процесс показан на левом рис. 1. С появлением нового измерения согласно рекуррентной схеме вычисляются матрица L, вектор у и заполняются соответствующие массивы. Если массивы не заполнены и траектория еще не закончилась, то обратная прогонка не делается. Иначе, делается l шагов обратной прогонки и выдается в качестве результата первый элемент полученного решения е, а если закончилась траектория — все l элементов е.

Рис. 1. Схема методов со скользящим окном

Предлагается следующая модификация, отличающаяся только способом проведения обратной прогонки и размером окна ^, ц> > /. При появлении нового измерения также ведется прямая прогонка, но не делается обратная, пока не заполнятся массивы Ь и у. Как только это произойдет, делается полная обратная прогонка (м> шагов) и выдаются первые ц> - / компонент решения е. Затем делается сдвиг, в результате которого стираются ц> - / первых компонент массивов Ь, у и е. Схематически данный процесс показан на рис. 1 справа.

Если число измерений п, то при использовании предыдущего метода нужно выполнить (п - / + 1)/ шагов обратной прогонки, а модифицированного метода — около п(1 + //(^ - /)). Чем больше размер окна w по сравнению с /, тем требуется меньшее число обратных прогонок, но тем с большой задержкой будем получать решение.

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

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

Были получены еще два набора точек путем сдвига исходных точек вдоль нормалей на величины есоп и ецпс, найденные при решении задачи условной минимизации с параметром 8 = 0,025 м и задачи безусловной минимизации с параметром у = 0,001. На рис. 2 изображены сплошной и штриховой линиями соответственно траектории, построенные по этим наборам точек. На рис. 3 толстой

40 60 *’м 48,14 48,142 м

Рис. 2. Траектории, построенные по разным наборам точек (справа показан в увеличенном масштабе фрагмент траекторий)

5. ЧИСЛЕННЫЕ ЭКСПЕРИМЕНТЫ

Предложенный метод сглаживания кривизны апробировался на многих траекториях, построенных по данным реальных навигационных измерений. В качестве колесного робота была взята автомашина, оборудованная спутниковыми антеннами, приемником, приводом поворота рулевых колес и датчиком угла поворота колес. Минимальный радиус разворота машины ^тт « 5 м (ктах « 0,2 м 1). В описываемом далее эксперименте машина, управляемая вручную, проводилась по желаемой траектории, координаты точек которой измерялись приемником в фазово-дифференциальном режиме со средним квадратичным отклонением горизонтальной ошибки измерения около 1,5 см.

На рис. 2 пунктирной линией изображена траектория, построенная по полученному набору точек Гр..., гп, п = 454, а на рис. 3 тонкой линией изоб-

0 50 100 150 200 і, м

Рис. 3. Зависимость кривизны траекторий от пройденного пути

8тГЬ.,' М х |0 J

lllll

нЬіаАІ *"TnnP Uw |Mui ip

I

О 50 100 150 200 250 300 350 400 450

* |0 ‘

J J - Л

\ -"V

О 50 100 150 200 250 300 350 400 450

Рис. 4. Разность решений (по оси абсцисс отложен номер компоненты вектора)

линией изображена кривизна только второй из них, поскольку графики кривизны обеих траекторий не сильно отличаются. Ни одна из компонент вектора полученного решения есоп по абсолютному значению не превысила 0,025 м, и только 6 из 454 компонент ецпс превысили данное значение, причем максимальная из них равна 0,036 м.

Применение метода со скользящим окном / = 50 и модифицированного метода с ц> = 150 и / = 50 дало решения е 1 и *2 практически не отличающие от решения ецпс, (рис. 4). Соответствующие графики кривизны не приводятся, так как они практически совпадают с графиком, изображенным на рис. 3 жирной линией.

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

ЗАКЛЮЧЕНИЕ

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

ЛИТЕРАТУРА

1. Farin G., Sapidis N. Curvature and the Fairness of Curves and Surfaces // IEEE Computer Graphics and Applications. — 1989. — Vol. 9, № 2. — P. 52—57.

2. Sapidis N., Farin G. Automatic Fairing Algorithm for B-Spline Curves // Computer-Aided Design. — 1990. — Vol. 22. — P. 121—129.

3. Пестерев А.В., Гилимьянов Р.Ф. Планирование пути для колесного робота // Проблемы вычислений в распределенной среде: распределенные приложения, коммуникационные системы, математические модели и оптимизация / Труды ИСА РАН. — Москва, 2006. — Т. 25. — С. 204—211.

4. Schoenberg I. Spline functions and the problem of graduation // Proc. Nat. Acad. Sci. — 1964. — Vol. 52. — P. 947—950.

5. Reinsch C. Smoothing by spline functions // Numer. Math. — 1967. — Vol. 10. — P. 177—183.

6. De Boor C. A Practical Guide to Splines. — New York: Spring-er-Verlag, 1978. — 392 p.

7. Завьялов Ю.С., Квасов Б.И., Мирошниченко В.Л. Методы сплайн-функций. — М.: Наука, 1980. — 352 с.

8. Kjellander J. Smoothing of Cubic Parametric Splines // Computer-Aided Design. — 1983. — Vol. 15(3). — P. 175—179.

9. Farin G. et al. Fairing Cubic B-Spline Curves // Computer Aided Geometric Design. — 1987. — Vol. 4. — P. 91—103.

10. Eck M., Hadenfeld J. Local Energy Fairing of B-spline Curves // Computing Supplement. — 1995. — Vol. 10. — P. 129—147.

11. Xu S., Li W., Zhao G. Target curvature based automatic fairing of planar b-spline curves // Computer Aided Geometric Design. — 2004. — Vol. 21(5). — P. 427—530.

12. Гилимьянов Р. Ф., Пестерев А.В., Рапопорт Л.Б. Сглаживание кривизны траекторий, построенных по зашумленным измерениям, в задачах планирования пути для колесных роботов // Изв. РАН. Теория и системы управления. — 2008. — Т. 47, № 5. — С. 152—159.

13. Floudas C.A., Pardalos P.M. Encyclopedia of Optimization. — New York: Springer-Verlag, 2009. — 4626 p.

14. Golub G.H., Van Loan C.F. Matrix Computations. — London: The John Hopkins University Press, 1996. — 728 p.

15. Гилимьянов Р.Ф., Пестерев А.В., Рапопорт Л.Б. Управление движением колесного робота в задаче следования вдоль криволинейного пути // Изв. РАН. Теория и системы управления. — 2008. — Т. 47, № 6. — С. 209—216.

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

В.Ю. Рутковским.

Гилимьянов Руслан Фаильевич — мл. науч. сотрудник,

Институт проблем управления им. В.А. Трапезникова РАН,

г. Москва, в (495) 926-52-00 доб. 11-49, 334-93-69,

И R.Gilimyanov@javad.com.