Каковы методы борьбы с компасом (гистерезис, зависящий от скорости)?

У меня есть робот с приводом от протектора, с колесными энкодерами низкой точности для отслеживания расстояния и электронным компасом для определения курса. Компас имеет значительную (> 1 секунды) задержку, когда робот быстро поворачивает, например, после достижения путевой точки - поворота на месте, чтобы указать его новый курс.

Какие способы борьбы с отставанием? Я думаю, что можно было бы сделать много измерений и смоделировать реакцию компаса. Тем не менее, это кажется проблематичным, так как это зависит от скорости, и я не знаю мгновенную скорость.

В качестве простого, но медленного подхода, я делаю поворот робота, пока он не будет очень грубо направлен в правильном направлении, затем делаю очень маленькие пошаговые повороты с короткими паузами измерения, пока он не будет направлен в правильном направлении. Есть ли другие способы борьбы с этим?

sensors compass

— ViennaMike
источник

Задержка в компасе из-за фильтра нижних частот, чтобы подавить высокочастотный шум.

Существуют более дорогие магнитометры, которые имеют меньший шум и, следовательно, меньшую задержку.
Также можно использовать гироскоп для повышения точности. Фактически, это то, что делают единицы измерения инерции (IMU). Это может быть достигнуто с помощью фильтра Калмана. Повышение точности помогает уменьшить задержку, потому что повышенная точность снижает зависимость от фильтра нижних частот для подавления шума. Фильтр Калмана объединяет данные магнитометра, а также гироскопа (который измеряет скорость изменения курса).

Если вы придерживаетесь своего текущего компаса, есть два возможных решения (Внимание, это становится все более продвинутым, но вариант 1 должен быть доступен для большинства людей без особой работы).

Вы можете попробовать отменить фильтр. Это может устранить задержку, но также увеличивает высокочастотный шум. После этого вы можете попытаться управлять роботом на основе новой оценки курса. Чтобы сделать это, вы должны поэкспериментировать с параметрами фильтра нижних частот. Например, в дискретное время вы можете найти:

где
$\hat{θ} (t) = a_{0} θ (t) + a_{1} θ (t - 1) + \dots + a_{k} θ (t - k)$ $\hat\theta(t)=a_0\theta(t)+a_1\theta(t-1)+\cdots+a_k\theta(t-k)$ представляет собой расчетный заголовок (выход компаса) в момент времени , - фактический курс (основная правда) в момент времени . $\hat\theta(t)$ $t$ $\theta$ $t$
Вы можете найти параметры , выполнив эксперимент, в котором вы измеряете основную правду, используя другие внешние средства. Принимая во внимание выборки, то есть это $a_i$ $n+k+1$
$[\begin{matrix} \hat{θ} (k) \\ ⋮ \\ \hat{θ} (k + n) \end{matrix}] = [\begin{matrix} θ (k) & θ (k - 1) & \dots & θ (0) \\ ⋮ & ⋮ & ⋮ \\ θ (k + n) & θ (k + n - 1) & \dots & θ (n) \end{matrix}] [\begin{matrix} a_{0} \\ a_{1} \\ ⋮ \\ a_{k} \end{matrix}]$ $\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]$
$[\begin{matrix} a_{0} \\ a_{1} \\ ⋮ \\ a_{k} \end{matrix}] = {[\begin{matrix} θ (k) & θ (k - 1) & \dots & θ (0) \\ ⋮ & ⋮ & ⋮ \\ θ (k + n) & θ (k + n - 1) & \dots & θ (n) \end{matrix}]}^{+} [\begin{matrix} \hat{θ} (k) \\ ⋮ \\ \hat{θ} (k + n) \end{matrix}]$ $\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]^{+}\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]$ $M^+$ $M$ $k$ так что вы, наверное, просто догадаетесь. Для бонусных баллов это предполагает, что шум является белым и независимым, но вы можете сначала отбелить его, чтобы устранить смещение, и, следовательно, улучшить свою оценку параметров.
Вы можете преобразовать это в передаточную функцию (также известную как Z-преобразование в дискретной временной области):

$\frac{\hat{Θ} (z)}{Θ (z)} = a_{0} + a_{1} z^{- 1} + . . . + a_{k} z^{- k}$ $\frac{\hat\Theta(z)}{\Theta(z)}=a_0+a_1 z^{-1}+...+a_k z^{-k}$
$\bar\theta(t)$

$\frac{\bar{Θ} (z)}{\hat{Θ} (z)} = \frac{1}{a_{0} + a_{1} z^{- 1} + \dots + a_{k} z^{- k}}$ $\frac{\bar\Theta(z)}{\hat\Theta(z)}=\frac{1}{a_0+a_1 z^{-1}+\cdots+a_k z^{-k}}$
Преобразование обратно во временную область:

$a_{0} \bar{θ} (t) + a_{1} \bar{θ} (t - 1) + \dots + a_{k} \bar{θ} (t - k) = \hat{θ} (t)$ $a_0\bar\theta(t)+a_1\bar\theta(t-1)+\cdots+a_k \bar\theta(t-k)=\hat\theta(t)$
$\bar{θ} (t) = \frac{\hat{θ} (t) - a_{1} \bar{θ} (t - 1) - \dots - a_{k} \bar{θ} (t - k)}{a_{0}}$ $\bar\theta(t)=\frac{\hat\theta(t)-a_1\bar\theta(t-1)-\cdots-a_k \bar\theta(t-k)}{a_0}$
тогда мы можем использовать $\bar\theta$

Это будет очень шумно, так что вы можете по-прежнему ставить $\bar\theta$
Вышеупомянутое решение все еще не лучший способ. Оценка шума может быть не очень полезной. Если мы поместим это в уравнение пространства состояний, мы можем разработать фильтр Калмана и контроллер обратной связи с полным состоянием, используя LQR (линейный квадратичный регулятор). Комбинация фильтра Калмана и контроллера LQR также известна как контроллер LQG (линейный квадратичный гауссов) и использует восстановление с циклическим переносом для получения хорошего контроллера.

Чтобы сделать это, придумайте (в дискретном времени) уравнения пространства состояний:

$\vec{x}(t)=A\vec{x}(t-1)+B\vec{u}(t-1)$ $\vec{y}(t)=C\vec{x}(t)$

$\vec{x} (t) = [\begin{matrix} θ (t) \\ θ (t - 1) \\ \dots \\ θ (t - k) \end{matrix}] = [\begin{matrix} A_{1} & A_{2} & \dots & 0 & 0 & 0 \\ 1 & 0 & \dots & 0 & 0 & 0 \\ 0 & 1 & \dots & 0 & 0 & 0 \\ ⋮ & ⋮ & ⋮ & ⋮ & ⋮ \\ 0 & 0 & \dots & 1 & 0 & 0 \\ 0 & 0 & \dots & 0 & 1 & 0 \end{matrix}] \vec{x} (t - 1) + [\begin{matrix} B_{0} \\ B_{1} \\ 0 \\ ⋮ \\ 0 \\ 0 \end{matrix}] \vec{u} (t - 1)$ $\vec{x}(t)=\left[\matrix{\theta(t)\\\theta(t-1)\\\cdots\\\theta(t-k)}\right]= \left[\matrix{ A_1&A_2&\cdots&0&0&0\\ 1&0&\cdots&0&0&0\\ 0&1&\cdots&0&0&0\\ \vdots&\vdots&&\vdots&\vdots&\vdots\\ 0&0&\cdots&1&0&0\\ 0&0&\cdots&0&1&0 }\right] \vec{x}(t-1) + \left[\matrix{B_0\\B_1\\0\\\vdots\\0\\0}\right]\vec{u}(t-1)$
$\vec{y} (t) = [\begin{matrix} \hat{θ} (t) \end{matrix}] = [\begin{matrix} a_{0} \\ a_{1} \\ ⋮ \\ a_{k} \end{matrix}] \vec{x} (t)$ $\vec{y}(t)=\left[\matrix{\hat\theta(t)}\right]=\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]\vec{x}(t)$
$\vec{u}(t-1)$ $A_0$ $A_1$ $B_0$ $B_1$ $B$ $A$

$Q_o$ $R_o$

$Q_c$ $R_c$ $Q_c = \left[\matrix{1&0&0&\cdots&0\\0&0&0&\cdots&0\\\vdots&\vdots&\vdots&&\vdots\\0&0&0&\cdots&0}\right]$ $R_c = \left[1\right]$ $J = \sum{(\vec{x}^T Q\vec{x} + \vec{u}^T R \vec{u})}$

Затем вы просто вводите его через алгебраическое уравнение Риккати с дискретным временем:

$P = Q + A^{T} (P - P B {(R + B^{T} P B)}^{- 1} B^{T} P) A$ $P = Q + A^T \left( P - P B \left( R + B^T P B \right)^{-1} B^T P \right) A$
и решить для положительно определенной матрицы $P$

Таким образом, ваш закон о контроле может быть задан:

$\vec{u} (t) = - K (\vec{x} (t) - {\vec{x}}_{r e f} (t))$ $\vec{u}(t)=-K(\vec{x}(t)-\vec{x}_{ref}(t))$
$K = (R + B^T P B)^{-1}(B^T P A)$

$\bar\theta$ пропустите через фильтр нижних частот (хотя и не обязательно с таким большим эффективным временем задержки). Это потому, что, хотя LQR гарантированно стабильно, как только вы используете фильтр Калмана, гарантия теряется.

$Q_o = Q_0 + q^2BVB^T$ $Q_0$ $Q$ $V$ $V=I$ $q$ $q \rightarrow \infty$ $Q_o$ матрица становится не настроенной, что означает, что она становится менее оптимальной.

$q$ $R_c$ $Q_c$ ), чтобы замедлить контроллер LQR.

Концепции в этом посте достаточно продвинуты, но если вам нужно решить такие вещи, как уравнение Риккати, вы можете использовать MATLAB или другое программное обеспечение для этого. Также могут быть библиотеки, уже реализующие фильтр Калмана (опять же, я полагаю, что MATLAB также делает это).

Для встроенного приложения может потребоваться реализовать фильтр Калмана самостоятельно, хотя, вероятно, существует реализация на языке Си.

— ronalchn
источник

Спасибо за отличный и подробный ответ. Я следую сути вашего первого решения и уверен, что смогу справиться с ним. Второй, как вы говорите, более сложный, и мне придется поработать, чтобы понять, смогу ли я все это выполнить.

— ViennaMike

Гироскоп - это простой ответ. Я всегда слышал, гироскоп для коротких измерений, компас для длинных. И реально чашка фильтра Каллмана между ними большую часть времени. Цена платы 6DOF gyro / acc в наши дни составляет менее 20 долларов, слишком дешево, чтобы ее не использовать.

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

— Spiked3
источник