Вычисляем скорость наименьшими квадратами

Запись от Eddy_Em размещена 01.08.2025 в 08:45

Метки c, math, вычисления, математика, си

Вожусь с системой управления телескопами на основе контроллеров sidservo (адская дрянь, но что есть, то есть). "Особенностью" контроллера является то, что невозможно получить текущее значение скорости, с которой телескоп двигается по обеим осям. Да и сам ответ на запрос состояния приходит через 50-60мс. Никакой стабильности нет. Поэтому единственный вариант - пользоваться осевыми энкодерами, которые показывают реальное положение телескопа, и которые я могу опрашивать с частотой до 4кГц (для этого сделал специальный контроллер на основе STM32: при подключении к компьютеру создаются устройства /dev/encoder_cmd0, /dev/encoder_X0 и /dev/encoder_Y0: настройки и данные с энкодеров в строковом формате).
Сначала вычислял скорость 1 раз в промежуток времени как простое отношение разности координат к разности времен. Конечно, получалось очень шумно и ступенчато. Сейчас запилил "плавающее" вычисление наименьшими квадратами. Значительно красивее вышло. И по "болтанке" скорости на интервале, где она должна быть постоянна, сразу понял, что я неправильно настроил ПИД-регулятор sidservo (то ли I, то ли P задрал выше, чем нужно).

Код прост до примитивизма: заводим массивы для накопления N данных в соответствии с каждой используемой суммой. При поступлении новых данных в сумму добавляем свежее и вычитаем последнее.

C
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
typedef struct{
    double *x, *t, *t2, *xt; // arrays of coord/time and multiply
    double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply
    size_t idx; // index of current data in array
    size_t arraysz; // size of arrays
} less_square_t;
 
less_square_t *LS_init(size_t Ndata){
    if(Ndata < 5) return NULL;
    less_square_t *l = calloc(1, sizeof(less_square_t));
    l->x = calloc(Ndata, sizeof(double));
    l->t2 = calloc(Ndata, sizeof(double));
    l->t = calloc(Ndata, sizeof(double));
    l->xt = calloc(Ndata, sizeof(double));
    l->arraysz = Ndata;
    return l;
}
void LS_delete(less_square_t **l){
    if(!l || !*l) return;
    free((*l)->x); free((*l)->t2); free((*l)->t); free((*l)->xt);
    free(*l);
    *l = NULL;
}
// add next data portion and calculate current slope
double LS_calc_slope(less_square_t *l, double x, double t){
    if(!l) return 0.;
    size_t idx = l->idx;
    double oldx = l->x[idx], oldt = l->t[idx], oldt2 = l->t2[idx], oldxt = l->xt[idx];
    double t2 = t * t, xt = x * t;
    l->x[idx] = x; l->t2[idx] = t2;
    l->t[idx] = t; l->xt[idx] = xt;
    ++idx;
    l->idx = (idx >= l->arraysz) ? 0 : idx;
    l->xsum += x - oldx;
    l->t2sum += t2 - oldt2;
    l->tsum += t - oldt;
    l->xtsum += xt - oldxt;
    double n = (double)l->arraysz;
    double denominator = n * l->t2sum - l->tsum * l->tsum;
    if(fabs(denominator) < 1e-7) return 0.;
    double numerator = n * l->xtsum - l->xsum * l->tsum;
    return (numerator / denominator);
}

Метки c, math, вычисления, математика, си

Размещено в Всячина, Железо

« Контроллер холловского датчика Главная страница И опять про трапециевидный рамп »

Надоела реклама? Зарегистрируйтесь и она исчезнет полностью.

Всего комментариев 2

Комментарии

Крутое решение Переход на "скользящую" МНК для оценки скорости по энкодерам — именно то, что нужно при рваных ответах sidservo. Ниже — точечные улучшения, которые обычно дают ещё более гладкую и устойчивую скорость, плюс пара альтернатив для сравнения.

Что улучшить прямо в вашей МНК-реализации
Центрируйте время (и координату) в формулах.
Ваши формулы эквивалентны стандартной регрессии, но в таком виде часто страдают от потери точности, когда t большие, а разброс по окну небольшой (катастрофическая отмена). Вычисляйте наклон через ковариацию и дисперсию «по центру окна»:

Пусть Sx = sum(x), St = sum(t), Sxx = sum(x*x) (не обязательно, но может пригодиться), Stt = sum(t*t), Sxt = sum(x*t), n = N.

Тогда

num = Sxt - (St * Sx) / n (ковариация t и x * n)

den = Stt - (St * St) / n (дисперсия t * n)

slope = num / den
Это численно устойчивее, чем n*Sxt - Sx*St и n*Stt - St*St.

Ведите счётчик фактического заполнения окна.
Пока окно ещё не заполнено, используйте n = count, а не arraysz. Это уменьшит "дёрганье" на старте.

Монотонные метки времени и локальная шкала.
Используйте monotonic clock. Для численной устойчивости храните t "локально": например, вычтите t0 (время первого/самого старого элемента в окне). Так уменьшится величина самих t, а формулы будут точнее.

Компенсированное суммирование (Kahan).
На 4 кГц даже с double стоит добавить компенсацию суммы для Sx, St, Stt, Sxt, чтобы замедлить накопление плавающей ошибки.

Границы и NaN-ы.
Если den слишком мал (разброс времени в окне почти нулевой из-за одинаковых меток), возвращайте последнюю валидную скорость или применяйте плавный hold/экстраполяцию.

Неровная дискретизация.
Ваш метод уже устойчив к неравномерным t. Это преимущество перед обычными центральными разностями. Главное — чтобы t были реальными метками, а не счётчиком сэмплов.

Небольшой рефактор под центрирование и "тёплый старт" :

C
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
typedef struct{
    double *x, *t, *t2, *xt;
    double Sx, St, Stt, Sxt;
    size_t idx, arraysz, count;
} ls_t;
 
ls_t *ls_init(size_t N){
    if (N < 5) return NULL;
    ls_t *l = calloc(1, sizeof(*l));
    l->x  = calloc(N, sizeof(double));
    l->t  = calloc(N, sizeof(double));
    l->t2 = calloc(N, sizeof(double));
    l->xt = calloc(N, sizeof(double));
    l->arraysz = N;
    return l;
}
void ls_free(ls_t **lp){
    if (!lp || !*lp) return;
    ls_t *l = *lp;
    free(l->x); free(l->t); free(l->t2); free(l->xt);
    free(l);
    *lp = NULL;
}
 
double ls_push_and_slope(ls_t *l, double x, double t){
    if (!l) return 0.0;
    size_t i = l->idx;
    double oldx = l->x[i], oldt = l->t[i], oldt2 = l->t2[i], oldxt = l->xt[i];
 
    double t2 = t * t, xt = x * t;
 
    l->x[i]  = x;   l->t[i]  = t;
    l->t2[i] = t2;  l->xt[i] = xt;
 
    if (l->count < l->arraysz) l->count++;
    l->idx = (i + 1 == l->arraysz) ? 0 : i + 1;
 
    // обновить суммы
    l->Sx  += x  - oldx;
    l->St  += t  - oldt;
    l->Stt += t2 - oldt2;
    l->Sxt += xt - oldxt;
 
    double n = (double)l->count;
    // центрированные ковариация и дисперсия времени
    double num = l->Sxt - (l->St * l->Sx) / n;
    double den = l->Stt - (l->St * l->St) / n;
 
    if (fabs(den) < 1e-12) return 0.0;
    return num / den; // скорость
}

Если хотите ещё устойчивее — храните t как "смещённые": t' = t - t_ref (например, t_ref = время самого старого элемента) и обновляйте все четыре суммы в этих координатах.

Альтернативы (иногда лучше МНК)
Savitzky–Golay (SG) дифференциатор: полиномиально-аппроксимирующий FIR-фильтр, сразу даёт сглаженную оценку производной. Для равномерной дискретизации заранее посчитали коэффициенты — и просто свёртка по окну. Прелесть SG в том, что он лучше сохраняет формы (постоянную скорость и постоянное ускорение) и даёт малую задержку для короткого окна.

α–β (g-h) фильтр / "poor man’s Kalman": если модель движений оси — "положение + скорость + белый шум", α–β фильтр очень хорош: вычислительно лёгкий, выдаёт гладкую скорость с минимальной фазовой задержкой. Подстройка α,β по вашим шумам даёт отличное поведение для наведения.

Калман по положению-скорости (2-состояния): если готовы к матрицам — получите оптимальную оценку скорости с учётом шума квантования энкодера, джиттера времени и, при желании, командного профиля скорости как входа (feed-forward).

Практика для телескопов и связи с ПИД
Двухконтурное управление: внутренний контур по скорости (от вашей оценки), внешний — по положению. Это резко уменьшает "болтанку".

Внутренний — PI по скорости, без интегрального насыщения (anti-windup обязателен).

Внешний — P по положению + небольшая I (или вообще без I), плюс feed-forward по скорости из траекторного планировщика (если он есть).

Запись от Python_Val размещена 13.08.2025 в 05:47 Python_Val вне форума

Настройка: сначала добейтесь устойчивого и быстрого скоростного контура (по вашей slope) при закрытом положении (маленький P по положению), затем медленно поднимайте P по положению. Если видите "пилу" скорости при постоянной команде — обычно завышен I скоростного или P позиционного.

Окно и задержка: при 4 кГц окно 21–41 отсчёт даёт сглаживание с задержкой порядка 2–5 мс — обычно ок для наведения. Если нужно быстрее — уменьшайте окно, но компенсируйте шумом SG/α–β.

Квантование энкодеров: при малом шаге и малых скоростях полезно усреднять "дельты" по окну (не сырые положения), либо держать окно в "импульсах", а не в миллисекундах, чтобы каждый шаг кванта попадал в регрессию несколько раз.

Джиттер времени: ваша МНК уже учитывает реальные t, но важно, чтобы t были из одного монотонного источника (CLOCK_MONOTONIC).

Итог
Ваш метод — отличный базис. Добавьте:

центрирование в формулах + счётчик заполнения окна,

(по желанию) Kahan и локальную шкалу времени,

и подумайте про SG или α–β как альтернативы, если захочется ещё меньше задержки при том же шуме.

Если хотите, скидывайте кусок кода чтения /dev/encoder_* и как вы формируете t — могу адаптировать под ваши таймеры (монотонные, тики, double сек) и подобрать размеры окна / параметры α–β под характерный шум ваших энкодеров.

Запись от Python_Val размещена 13.08.2025 в 05:48 Python_Val вне форума

Новые блоги и статьи Все статьи Все блоги /
Нейросеть на алгоритме "эстафета хвоста" как перспектива. Hrethgir 06.05.2026 На десерт, когда запущу сервер. Статья тут https:/ / habr. com/ ru/ articles/ 1030914/ . Автор я сам, нейросеть только помогает в вопросах которые мне не известны - не знаю людей которые знали-бы. . .	Асинхронный приём данных из COM-порта Argus19 01.05.2026 Асинхронный приём данных из COM-порта Купил на aliexpress термопринтер QR701. Он оказался странным. Поключил к Arduino Nano. Был очень удивлён. Наотрез отказывается печатать русские буквы. Чтобы. . .	попытка написать игровой сервер на C++ pyirrlicht 29.04.2026 попытка написать игровой сервер на плюсах с открытым бесконечным миром. возможно получится прикрутить интерпретатор питон для кастомизации игровой логики. что есть на текущий момент:. . .	Контроль уникальности выбранного документа-основания при изменении реквизита Maks 28.04.2026 Алгоритм из решения ниже разработан на примере нетипового документа "ЗаявкаНаРемонтСпецтехники", разработанного в КА2. Задача: уведомлять пользователя, если указанная заявка (документ-основание). . .
Благородство как наказание Maks 24.04.2026 У хорошего человека отношения с женщинами всегда складываются трудно. А я человек хороший. Заявляю без тени смущения, потому что гордиться тут нечем. От хорошего человека ждут соответствующего. . .	Валидация и контроль данных табличной части документа перед записью Maks 22.04.2026 Алгоритм из решения ниже реализован на примере нетипового документа, разработанного в КА2. Задача: контроль и валидация данных табличной части документа перед записью с учетом регламента компании. . .	Отчёт о затраченных материалах за определенный период с макетом печатной формы Maks 21.04.2026 Отчёт из решения ниже размещён в конфигурации КА2. Задача: разработка отчёта по затраченным материалам за определённый период, с возможностью вывода печатной формы отчёта с шапкой и подвалом. В. . .	Отчёт о спецтехнике находящейся в ремонте Maks 20.04.2026 Отчёт из решения ниже размещен в конфигурации КА2. Задача: отобразить спецтехнику, которая на данный момент находится в ремонте. Есть нетиповой документ "Заявка на ремонт спецтехники" который. . .

Сообщение форума

Отменить изменения