Библиотеки: фильтрация

Реализация различных динамических фильтров. Дифференцирование сигналов. Интегрирование сигналов. Объединение двух поз для целей плановой стыковки траектории. Реализации стандартных динамических звеньев: передаточная функция, гистерезис и т.п.

Классы добавляются по мере необходимости.

Интерфейс

Интерфейс соответствует s-функциям MATLAB. Фильтр имеет состояние, меняющееся с течением времени. Время дискретно с постоянным периодом. Схема работы стандартная: на каждом шаге фильтр получает новое значение входа u[k], по текущему состоянию x[k] и входу u[k] вычисляет новое состояниеx[k+1] и выход y[k]. Здесь k --- дискретное время.

 Filter(параметры, начальные условия x0) --- конструктор.
 virtual void start(), void start(начальные условия x0) --- установить состояние фильтра на начальное значение: x[k] = x0.
 virtual OutputType output(InputType u) --- вычислить выход фильтра на текущем шаге при данном входе, но не модифицирует состояние фильтра: y[k] = g(x[k], u[k]).
 virtual OutputType update(InputType ) --- вычислить выход фильтра на текущем шаге при данном входе, модифицирует состояние фильтра: x[k+1] = f(x[k], u[k]), y[k] = g(x[k], u[k])

Замечание: для ряда приложений может быть удобнее другой вариант update:

 virtual void update() --- сделать шаг, используя последний заднный при помощи `output`  вход, модифицирует состояние фильтра: x[k+1] = f(x[k], u[k]), y[k] = g(x[k], u[k])

Такая реализация может просто буфферизовать в output новое сотояние. Выбор конкретной реализации зависит от удобства применния.

Интегратор

Интегрирует входной сигнал.

Имена интерфейсов: Integrator, IntegratorArray, IntegratorJointState.

Типы данных: * Вход, выход: double, JntArray, JointState, Quaternion, Pose, Vector3.

Базовая реализация:

x[k+1] = x[k] + T*u[k],
y[k] = x[k].

Здесь и далее T --- период дискретизации.

Замечание: в случае ориентации надо интегрировать уравнение Пуассона.

Замечание: возможна реализация без состояния. В этом случае входа два (x --- позиция, u --- скорость), а шаг возвращает новое значение позиции x. однако тогда несколько усложняется реализация метода трапеций и метода Симпсона.

Дифференцирование

Вычисляет производную входного сигнала.

Имена интерфейсов: Integrator, IntegratorArray, IntegratorJointState.

Типы данных: * Вход, состояние, выход: double, JntArray, JointState.

Базовая реализация:

x[k+1] = u[k],
y[k] = (u[k] - x[k])/T.

Варианты реализации:

* инерционно-дифференцирующее звено,
* дифференцирующий полиномиальный фильтр КИХ,

Объединение траекторий

Предназначены для синтеза "переходной" траектории между двумя заданным, синтеза "переходной" траектории от некоторой начальной позиции к заданной траектории.

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

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

Области применения фильтров:

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

Объединение двух траекторий: первый порядок

На вход фильтра подаются две траектории u1 и u2.

Имена интерфейсов: FusionTypename

Типы данных: Вход, выход: double, JntArray, Vector3, Quaternion, Pose, Twist Состояние: duoble x.

Варианты реализации:

  • линейный переход FusionTypenameLinear

    y[k] = x[k]*u1[k] + (1-x[k])*u2[k],
x[k+1] = max(0, x[k] - T/tau), 
x0 = 1.

    tau --- длительность процесса.

  • экспоненциальный переход FusionTypenameExp

    y[k] = x[k]*u1[k] + (1-x[k])*u2[k],
x[k+1] = x[k]*(1 - T/tau), 
x0 = 1.

    tau --- характерная длительность процесса.

  • экспоненциальный переход c ограничением скорости

Переход от заданного состояния к траектории: первый порядок

На вход фильтра подается желаемая траектория ur и текущее состояние (скорость, поза) u. Переменная u фактически играет роль состояния фильтра, т.к. предполагается, что после после вычисления его выхода y[k] он присваивается очередному значению u[k+1], т.е. 

u[k+1] = y[k].

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

Имена интерфейсов: TransientTypename

Типы данных: * Вход, выход: double, JntArray, Vector3, Quaternion, Pose, Twist

Варианты реализации:

  • линейный переход

    y[k] = u[k] + ort(ur[k] - u[k])*min(v*T,|ur[k] - u[k]|),

    v --- скорость.

  • экспоненциальный переход

    y[k] = u[k]*(1 - tau/T) + ur[k]*tau/T,

    tau --- характерная длительность процесса.

  • экспоненциальный переход c ограничением скорости

Переход от заданного состояния к траектории: второй порядок

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

Используются следующие обозначения: x, v --- текущая позиция и скорость робота, xr, vr --- желаемая позиция и скорость, ar --- желаемое ускорение.

Очевидно, они подчиняются следующим уравнениям:

.       
x = v,                   (1)
.        .
xr = v,  vr = ur,        (2)

Введем ошибку:

xe = xr - x,  ve = vr - v.    (3)

Зададимся желаемой динамикой ошибки:

.        .
xe = ve, ve = - f(xe,ve).     (4)

К примеру, выбрать линейный закон f(xe, ve) = -a0*xe - a1*ve, обеспечивающий экспоненциальную сходимость. Аналогично легко ввести экспоненциальный закон с ограниченным ускорением и/или скоростью

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

.      .
x = v, v = ur + f(xr-x, vr-v).  (5)

Замечание: в случае ориентации ситуация несколько усложняется. Для описания ее эволюции в (1)-(2) надо использовать уравнение Пуассона, ошибка вводится при помощи групповой операции группы Ли SO(3). Некоторую сложность может представить выбор уравнения (4).

Типы данных (без ориентации): Вход: x, v, xr, vr (double, Vector3) Выход: x, v (double, Vector3) * Состояние: нет, фактически роль состояния играют x и v.

Варианты реализации (основаны на дискретизации (5)): экспоненциальный переход, экспоненциальный переход c ограничением ускорения, * экспоненциальный переход c ограничением ускорения и скорости.