Які методи боротьби з компасним відставанням (залежний від швидкості гістерезис)?

У мене робот, керований протектором, з низькоточними кодувальниками коліс для відстеження відстані та електронним компасом для визначення курсу. Компас має значне (> 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$

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

$q$ $R_c$ $Q_c$

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

Для вбудованого додатка вам може знадобитися самостійно реалізувати фільтр Kalman, хоча, можливо, існує реалізація C.

— ronalchn
джерело

Дякую за відмінну та глибоку відповідь. Я дотримуюся суті вашого першого рішення, і я впевнений, що зможу це зробити. Другий, як ви кажете, є складнішим, і мені доведеться попрацювати, щоб зрозуміти, чи зможу я все це дотримуватися.

— ВіденьМайк

Гіроскоп - це проста відповідь. Я завжди чув, гіроскоп для коротких вимірювань, компас надовго. І реально чашка фільтру каллмена між двома більшістю часу. Ціна гіроскопа / аккаунта 6DOF в ці дні становить менше 20 доларів, що занадто дешево, щоб не використовувати його.

Свого часу я працював через чужий фільтр Kallman . і він працював. Фільтр Kallman насправді є скоріше підходом, а не точною реалізацією, і у випадку гіроскопа кінцевий результат не потребує використання матричної математики. Це робить набагато простішим код.

— Шип3
джерело