3.2. Планирование пути методом полиномиальной аппроксимации

Рассмотрим наиболее простой случай планирования пути манипуляторов. Пусть в начальный момент времени t₀ манипулятор находится в некоторой конфигурации q⁰. В момент времени T он должен прибыть в конфигурацию q^T. Находясь в точке q⁰, манипулятор покоится, т. е. обобщенные скорости и ускорения равны 0. По прибытии в q^T обобщенные скорости и ускорения манипулятора также должны быть равны 0. Примем, что t₀ = 0 и T = 1. Будем также считать, что в рабочей зоне манипулятора препятствия отсутствуют. Необходимо спланировать путь, удовлетворяющий указанным выше условиям. Сформулируем задачу планирования пути математически. Необходимо найти вектор-функцию q(t), удовлетворяющую следующим условиям:

q(t₀) = q⁰; (3.4)

q(T) = q^T; (3.5)

(t₀) = 0; (3.6)

(T) = 0; (3.7)

(t₀) = 0; (3.8)

(T) = 0. (3.9)

Будем считать, что вектор q(t) состоит из n компонент q_i(t), i=1,…,n. Очевидно, что каждая компонента вектора q(t) должна удовлетворять условиям (3.4) – (3.9), поэтому найдем формулу для произвольной i-й компоненты, и эта формула будет пригодна для всех компонент вектора q(t).

Представим i-ю компоненту вектора q(t) в виде полинома:

q_i(t) = a₀t⁰ + a₁t¹ + a₂t² + ... + a_mt^m, i=1,...,n. (3.10)

К оэффициенты a_j, j = 0,...,m неизвестны и подлежат определению. Поскольку q_i(t) должна удовлетворять шести условиям (3.4)–(3.9), примем m = 5. Тогда получим систему (3.11), состоящую из 6 уравнений с 6 неизвестными коэффициентами:

или

Решая эту систему, получим

. (3.13)

Итак, получаем выражение для i-й компоненты вектора q(t):

q_i(t) = q_i⁰ + 10(q_i^T – q_i⁰)t³ – 15(q_i^T – q_i⁰)t⁴ + 6(q_i^T – q_i⁰)t⁵, (3.14)

где i = 1,…,n. Эту формулу можно применять для описания любой компоненты вектора q(t) – достаточно лишь подставить в нее соответствующие значения q_i⁰ и q_i^T, i =1,…,n.

Продифференцировав формулу (3.14) один раз, получим выражение для i-й обобщенной скорости:

_i(t) = 30(q_i^T – q_i⁰)t² – 60(q_i^T – q_i⁰)t³ + 30(q_i^T – q_i⁰)t⁴, (3.15)

где i = 1,…,n.

Теперь продифференцируем один раз формулу (3.15) и получим выражение для i-го обобщенного ускорения:

_i(t) = 60(q_i^T – q_i⁰)t – 180(q_i^T – q_i⁰)t² + 120(q_i^T – q_i⁰)t³, (3.16)

i = 1,...,n.

Читателю предоставляется возможность самому убедиться, что формулы (3.14)–(3.16) удовлетворяют условиям (3.4)–(3.9).

3.3. Планирование пути с учетом ограничений на положение, скорость и ускорение

Рассмотрим метод планирования пути, учитывающий ограничения на обобщенные координаты, скорости и ускорения (3.1)–(3.3).

Поставим задачу: найти вектор-функцию q(t), чтобы она удовлетворяла следующим условиям:

q(t₀) = q⁰, (3.17)

q(T) = q^T, (3.18)

q(t)  a¹, (3.19)

–q(t)  –a², (3.20)

(t)  b¹, (3.21)

– (t)  –b², (3.22)

(t)  w¹, (3.23)

– (t)  –w². (3.24)

Каждую компоненту вектора q(t) будем искать в виде полинома степени m:

q_i(t) = c₀t⁰ + c₁t¹ + c₂t² + ... + c_mt^m, (3.25)

i = 1, … , n. Коэффициенты c_j, j = 0, ... , m подлежат определению.

Заметим также, что

_i(t) = c₀0 + 1c₁t⁰ + 2c₂t¹ + ... + mc_mt^m-1, (3.26)

_i(t) = c₀0 + c₁0 + 21c₂t⁰ + ... + m (m-1) c_mt^m-2. (3.27)

i = 1, … , n.

Требования (3.17)–(3.24), которым должна удовлетворять q_i(t), запишем в виде следующих выражений:

c₀t₀⁰ + c₁t₀¹ + c₂t₀² + ... + c_mt₀^m = q_i⁰, (3.28)

c₀T⁰ + c₁T ¹ + c₂T² + ... + c_mT^m = q_i^T, (3.29)

c₀t⁰ + c₁t¹ + c₂t² + ... + c_mt^m  a_i¹, (3.30)

–c₀t⁰ – c₁t¹ – c₂t² – ... – c_mt^m  –a_i², (3.31)

c₀0 + 1c₁t⁰ + 2c₂t¹ + ... + mc_mt^m-1 b_i¹, (3.32)

–c₀0 – 1c₁t⁰ – 2c₂t¹ – ... – mc_mt^m-1  –b_i², (3.33)

c₀0 + c₁0 + 21c₂t⁰ + ... +m(m–1) c_mt^m-2  w_i¹, (3.34)

–c₀0 – c₁0 – 21c₂t⁰ – ... –m(m–1) c_mt^m-2  –w_i². (3.35)

Неравенства (3.30)–(3.35) должны выполняться для любого t[t₀,T]. Выделим из отрезка [t₀,T] k моментов времени t₀, t₁, ..., t_k-1= T (чем больше k, тем лучше) и получим системы

c₀t₀⁰ + c₁t₀¹ + c₂t₀² + ... + c_mt₀^m = q_i⁰, (3.36)

c₀T⁰ + c₁T ¹ + c₂T² + ... + c_mT^m = q_i^T, (3.37)

(3.38)

(3.39)

(3.40)

(3.41)

(3.42)

(3.43)

В каждой системе (3.38)–(3.43) содержится по k неравенств. Каждое неравенство соответствует определенному моменту времени t_k[t₀,T]. В уравнениях (3.36), (3.37) и системах неравенств (3.38)–(3.43) неизвестными являются величины c_j, а коэффициенты при них известны.

Запишем величины c_j в вектор неизвестных величин, а коэффициенты при них в матрицу:

. (3.44)

Определитель матрицы в этой формуле равен нулю, так как в ней есть одинаковые строки.

Итак, получили систему неравенств вида

. (3.45)

Чтобы преобразовать неравенства в равенства, введем в каждое неравенство по одной новой переменной, умноженной на 1, тогда A и x преобразуются к виду

. (3.46)

Определитель этой матрицы уже не равен нулю, так как все строки в ней различны. Последние (f–2) переменных должны подчиняться условию

x_g+1  0, x_g+2  0, ... , x_g+(f-2)  0. (3.47)

Для того, чтобы можно было решить систему (3.46), число неизвестных в ней должно быть равно числу строк в матрице, т. е. f. У нас число неизвестных равно g + f–2. Придадим (g–2) нижним переменным произвольные значения, соблюдая условие (3.47). Эти (g–2) переменных становятся константами. Получаем

(3.48)

или

. (3.49)

Из этой системы находим x_j, j = 1,...,f. Вместе с вычисленными нами константами x_g+1, x_g+2, ... , x_g+(f-2) найденные величины и будут составлять искомый вектор (c₀, c₁, c₂, ... , c_m).