Привет, Хабр!
В данной статье разобран алгоритм монотонной кубической интерполяции, предложенный Фритчем и Карлосоном в работе [1].
На рисунке красным обозначен результат обычной кубической интерполяции Эрмита, а синим - монотонной, кругами - опорные точки траектории.
Примеры кода написаны на C++, исходники всей библиотеки лежат здесь. Также написана копия библиотеки на Java, исходники лежат здесь.
Механические ограничения
При управлении механическими системами часто используют узловые траектории. В таком подходе зависимость от времени каждой из управляемых координат описывают с помощью непрерывных траекторий.
Каждая из этих траекторий описывается набором узлов. Узлом называется объединения аргумента функции, её значения, а также зачастую некоторое число значений производных (чаще всего скоростей и ускорений).
Проще говоря, каждая узловая траектория описывается узлами и интерполяционными полиномами, причём для каждой пары соседних узлов определяется свой полином. Получение значения координат траектории по её узловым значениям называется интерполяцией.
При этом к полиномам предъявляются требования совпадения значений в общих узлах:
где f_k - интерполяционный полином промежутка от узла x_k до узла x_{k+1}, а f_{k+1} - от x_{k+1} до x_{k+2}
Для реализации таких траекторий в режиме реального времени за ошибку управления принимают отклонение реального значения координаты от значения, полученного из траектории по текущей временной метке.
При реализации траекторного управления механическими системами очень важно обеспечить непрерывность скорости и ускорения. Если, например, скорость имеет резкий перепад, то ускорение, которое должно его обеспечить, в пределе - бесконечным.
В случае перепадов ускорения такое же ограничение накладывается на рывок (jerk). Его ограничения связаны с инертностью приводов, система с условно бесконечным рывном в макромире невозможна, поэтому сплайны для механических систем должны не иметь разрывов в графиках скоростей и ускорений.
На рисунке изображен результат монотонной интерполяции, p(x) - значение интерполированной координаты, v(x) - скорость её изменения, a(x) - ускорение.
На языке математики отсутствие перепадов называется непрерывностью, а отсутствие разрывов у производных вплоть до второй степени включительно - принадлежностью к C2классу непрерывности.
Вторым ограничением механических систем является то, что значения на каждом из промежутков не должны выходить за граничные. Такое свойство называется монотонностью на промежутке.
Если не обеспечить монотонность сплайна на промежутках, то промежуточное значение может выйти за границы допустимых. Такие ситуации называются вылетами (overshooting).
На рисунке красным обозначен участок траектории, полученный обычной кубической интерполяцией, а синим - монотонной. Хотя перепад между граничными значениями промежутка незначителен, вылет настолько значителен, что может всерьёз нарушить технологический процесс. В заголовке статьи представлен полный график этой же интерполяции.
Рассматриваемый алгоритм обеспечивает оба этих требования.
Интерполяция Эрмита
Алгоритм Фритча-Карлсона использует кубическую интерполяцию базисными функциями Эрмита. Этот алгоритм похож на классическую интерполяцию, но у него можно указать произвольное количество производных в каждой точке. При этом у разных узлов порядки производных или их количество могут отличаться.
Каждая опорная точка x называется узлом k-ой степени, где k равно порядку старшей производной, заданной для этого узла. Если у узла задано только значение, то такой узел называет узлом нулевой степени.
Чтобы построить интерполяционный полином, можно воспользоваться интерполяционной формулой Эрмита:
где f^{(k)}(x_i) - производная функции f(x) порядка k в точке x_i
ℎ_{ki}(x) - базисный полином (базисная функция) Эрмита. Базисными они называются потому, что итоговый полином раскладывается на сумму попарных произведений произведений специальных полиномов на значения функции и её производных на границах промежутка.
Здесь работает полная аналогия с базисными векторами в линейном пространстве: каждый итоговый вектор раскладывается на сумму попарных произведений базисных векторов (ортов) и соответствтующих координат. В случае с базисными полиномами каждый из полиномов - это своего рода орт, а значения и производные в узлах - координаты.
Первый индекс ℎ_{ki}(x)указывает на порядок производной (0 - значение функции, 1 - первая производная, 2 - вторая и т.д.) , второй отвечает за номер узла.
Например, если узла всего два (как в нашем случае), то полином можно найти по формуле:
Если к тому же указаны только первые производные, то выражение упростится ещё больше:
С этой формулой мы и будем дальше работать.
Базисные функции
Так как полученный полином должен обеспечивать требуемые значения и соответствующие скорости, то можно составить систему уравнений, подставив значения в точках 0 и 1. После этого можно сделать замену
и подставить в полученный полином H_{N-1}(x). Такой переход позволяет решить задачу только для интервала [0,1] и перед вызовом выполнять преобразование от x к t.
При этом ℎ_{ki} - это базисный полином. Так как полином третьей степени можно представить в виде
то для каждого промежутка можно составить по уравнению для пары значений и пары производных:
Если x_0 = 0, а x_1 = 1, то можно переписать их следующим образом:
Эти уравнения сформулировать в матричном виде:
Найдём матрицу, связывающую коэффициенты полинома со значениями a, b, c, d
Запишем в матричном виде p(t)
Каждый столбец полученной матрицы является базисным полиномом Эрмита
Кубическая интерполяция
Для непосредственной интерполяции написан отдельный класс CubicHermiteSpline. Он принимает в качестве аргументов массив x_k координат узлов, значения f(x_k) и производные f′(x_k) в конструкторе
/**
* Конструктор
* @param x_ptr - указатель на x - координаты
* @param y_ptr - указатель на y - координаты
* @param dy_ptr - указатель на y - координаты
* @param size - количество опорных точек траектории
*/
CubicHermiteSpline(const double *x_ptr, const double *y_ptr, const double *dy_ptr, const size_t size) :
_x(x_ptr, x_ptr + size),
_y(y_ptr, y_ptr + size),
_dy(dy_ptr, dy_ptr + size),
_size(size) {}Также у него прописан метод, возвращающий интерполированное значение для любого из промежутков с помощью бинарного поиска
/**
* Получить интерполированные значения
* @param x - координата
* @return значение интерполяционного полинома в заданной точке
*/
double CubicHermiteSpline::getInterpolatedValue(const double x) const {
const size_t idx = _binarySearch(x);
if (idx == _size - 1)
return _y[_size - 1];
const double t = (x - _x[idx]) / (_x[idx + 1] - _x[idx]);
return _interpFunc(t, idx);
}Когда промежуток определён, вызывается сама функция интерполяции
/**
* Функция интерполирования
* @param t - время
* @param idx - индекс левой границы диапазона интерполирования
* @return интерполированное значение, соответствующее времени t
*/
double CubicHermiteSpline::_interpFunc(const double t, const size_t idx) const {
return (2 * std::pow(t, 3) - 3 * std::pow(t, 2) + 1) * _y[idx] +
(std::pow(t, 3) - 2 * std::pow(t, 2) + t) * (_x[idx + 1] - _x[idx]) * _dy[idx] +
(-2 * std::pow(t, 3) + 3 * std::pow(t, 2)) * _y[idx + 1] +
(std::pow(t, 3) - std::pow(t, 2)) * (_x[idx + 1] - _x[idx]) * _dy[idx + 1];
}Этот сплайн используется в разных интерполяторах. Так как логика их работы немного отличается, то общие тривиальные функции были помещены в базовый класс CubicInterpolation. В нём же хранится объект сплайна.
Остальная логика реализуется уже в двух дочерних классах: HermiteCubicInterpolation и MonotoneCubicInterpolation. Оба класса принимают в качестве аргументов конструктора только координаты узлов и соответствующие им значения. Первые производные вычисляются по-разному.
В этом блоке будет рассмотрен класс обычной интерполяции Эрмита. Скорость в крайних точках равна нулю, а в промежуточных - среднему гармоническому (трёхточечная разность):
/**
* Вычислить первые производные в опорных точках методом трёхточечной разности
* @param dy - вектор, в который нужно записать первые производные
* @param x_ptr - указатель на x - координаты
* @param y_ptr - указатель на y - координаты
* @param size - количество опорных точек траектории
*/
void HermiteCubicInterpolation::calculateDY(
std::vector<double> &dy, const double *x_ptr, const double *y_ptr, const size_t size
) {
// рассчитываем dy методом трёхточечной разности
std::vector<double> tangents(size, 0);
for (int i = 0; i < size - 1; ++i) tangents[i] = (y_ptr[i + 1] - y_ptr[i]) / (x_ptr[i + 1] - x_ptr[i]);
for (int i = 1; i < size - 1; ++i) dy[i] = (tangents[i - 1] + tangents[i]) / 2;
dy[0] = 0;
dy[size - 1] = 0;
}Монотонные сплайны
Алгоритм Фритча-Карлсона делает монотонными те участки, которые ими не были. Он прописан в классе MonotoneCubicInterpolation. При вычислении скоростей f'(x) сначала определяются промежуточные значения, как в HermiteCubicInterpolation. Но после они модифицируются в несколько шагов.
Алгоритм последовательно обрабатывает каждый промежуток и может менять обе скорости: как у левой границы, так и у правой. Поэтому значения скоростей в некоторых узлах могут меняться дважды.
Для каждого промежутка [x_k, x_{k+1}] выполняются следующие шаги:
-
Если грачиные значения функций совпадают
f(x_k)=f(x_{k+1}), то нужно обнулить обе производные:for (int i = 1; i < (size - 1); ++i) { // значения совпадают if (std::abs(tangents[i]) < _KEPS) dy[i] = dy[i + 1] = 0; } -
Необходимо проверить, не имеют ли углы наклона в узлах разные знаки. Если это так, то узел является локальным минимумом, и скорость в нём должна быть равна нулю.
for (int i = 0; i < (size - 2); ++i) { // тангенсы касательных имеют разные знаки if (tangents[i] * tangents[i + 1] <= 0) dy[i] = dy[i + 1] = 0;
-
Если они имеют один и тот же знак, то необходимо ввести специальные коэффициенты
αиβ, которые равны отношению скоростей к касательным. Если сумма квадратов этих коэффициентов больше девяти, то нужно пропорционально уменьшить скорости (вектор(α,β)должен стать ограничен окружностью радиуса3)double alpha = dy[i] / tangents[i]; double betta = dy[i + 1] / tangents[i]; double distance = alpha * alpha + betta * betta; if (distance > 9) { double tau = 3 / std::sqrt(distance); dy[i] = tau * alpha * tangents[i]; dy[i + 1] = tau * betta * tangents[i]; }
Весь код метода под спойлером
/**
* Вычислить первые производные в опорных точках методом трёхточечной разности
* @param dy - вектор, в который нужно записать первые производные
* @param x_ptr - указатель на x - координаты
* @param y_ptr - указатель на y - координаты
* @param size - количество опорных точек траектории
*/
void MonotoneCubicInterpolation::calculateDY(
std::vector<double> &dy, const double *x_ptr, const double *y_ptr, const size_t size
) {
std::vector<double> tangents(size, 0);
// рассчитываем dy методом трёхточечной разности
for (int i = 0; i < size - 1; ++i) tangents[i] = (y_ptr[i + 1] - y_ptr[i]) / (x_ptr[i + 1] - x_ptr[i]);
for (int i = 1; i < size - 1; ++i) dy[i] = (tangents[i - 1] + tangents[i]) / 2;
dy[0] = 0;
for (int i = 1; i < (size - 1); ++i) {
// значения совпадают
if (std::abs(tangents[i]) < _KEPS)
dy[i] = dy[i + 1] = 0;
}
// применяем алгоритм Фритча-Карлсона
for (int i = 0; i < (size - 2); ++i) {
// тангенсы касательных имеют разные знаки
if (tangents[i] * tangents[i + 1] <= 0)
dy[i] = dy[i + 1] = 0;
else {
double alpha = dy[i] / tangents[i];
double betta = dy[i + 1] / tangents[i];
double distance = alpha * alpha + betta * betta;
if (distance > 9) {
double tau = 3 / std::sqrt(distance);
dy[i] = tau * alpha * tangents[i];
dy[i + 1] = tau * betta * tangents[i];
}
}
}
// делаем дополнительную проверку для предпоследней точки
if (tangents[size - 2] * tangents[size - 1] <= 0)
dy[size - 2] = 0;
dy[size - 1] = 0;
}Список литературы
Fritsch, F. N.; Carlson, R. E. (1980). "Monotone Piecewise Cubic Interpolation". SIAM Journal on Numerical Analysis. SIAM. 17 (2): 238–246. doi:10.1137/0717021