나침반 지연 (속도 의존 이력 현상)을 처리하는 방법은 무엇입니까?

거리를 추적하기위한 고정밀 휠 인코더와 방향을 결정하기위한 전자 나침반이있는 트레드 구동 로봇이 있습니다. 나침반은 로봇이 빠르게 회전 할 때 (예 : 웨이 포인트에 도달 한 후) 새 방향을 가리 키도록 회전하는 데 상당한 지연 (> 1 초)이 걸립니다.

지연을 처리하는 방법은 무엇입니까? 나는 많은 측정을 수행하고 나침반 반응을 모델링 할 수 있다고 생각합니다. 그러나 이것은 속도에 따라 다르며 순간적인 속도를 모릅니다.

간단하지만 느리게 접근하기 위해 올바른 방향을 향할 때까지 로봇을 돌린 다음 올바른 방향을 가리킬 때까지 짧은 측정 일시 중지로 아주 작은 증분 회전을합니다. 이것을 다루는 다른 방법이 있습니까?

sensors compass

— 비엔나 마이크
소스

나침반의 지연은 저역 통과 필터로 인해 고주파 노이즈를 억제합니다.

소음이 적고 지연이 적은 고가의 자력계가 있습니다.
자이로 스코프를 사용하여 정확도를 향상시킬 수도 있습니다. 실제로 이것은 관성 측정 장치 (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 (선형 2 차 레귤레이터)을 사용하여 칼만 필터와 전체 상태 피드백 컨트롤러를 설계 할 수 있습니다. 칼만 필터와 LQR 컨트롤러의 조합은 LQG 컨트롤러 (선형 2 차 가우스)라고도하며 루프 전송 복구를 사용하여 우수한 컨트롤러를 얻습니다.

이렇게하려면 (이산 시간) 상태 공간 방정식을 생각해보십시오.

$\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)$
여기서 은 로봇을 돌리는 모터의 힘을 나타내고 , , , 은 위치와 속도에 따라 머리글에 영향을 미치는 정도입니다 (0이 아닌 값을 선택할 수 있음) 행렬 의 다른 요소 와 행렬 의 첫 번째 행에 대해서도 마찬가지입니다. $\vec{u}(t-1)$ $A_0$ $A_1$ $B_0$ $B_1$ $B$ $A$

그런 다음 프로세스 노이즈 및 측정 노이즈에 대한 노이즈 추정치 및 를 선택하여 옵저버 (칼만 필터)를 구축 할 수 있습니다 . 그런 다음 Kalman Filter는 잡음에 대한 가정을 고려하여 최적의 제목 추정치를 찾을 수 있습니다. 노이즈 추정을 선택한 후 구현은 칼만 필터 코드 구현에 달려 있습니다 (위키 백과에서 방정식을 찾을 수 있으므로 여기서 다루지 않겠습니다). $Q_o$ $R_o$

그런 다음, LQR 컨트롤러를 설계 할 수 있습니다. 이번에 는 제목을 조정하는 데 주어진 가중치를 나타내는 및 선택 하고 액추에이터 사용을 제한하려고합니다. 이 경우 및 . 이는 LQR이 비용 함수를 최소화하기위한 최적의 컨트롤러를 찾으므로 수행됩니다. $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)$

마지막으로이 작업을 수행하면 효과가 떨어지며 노이즈로 인해 불안정 할 수 있습니다. 사실, 이는 저역 통과 필터를 통해 를 먼저 넣지 않으면 옵션 1이 작동하지 않을 수 있음을 의미 합니다 (긴 유효 지연 시간이 반드시 긴 것은 아님). 칼만 필터를 사용하자마자 LQR이 안정적으로 보장되지만 보증이 손실되기 때문입니다. $\bar\theta$

이 문제를 해결하려면, 우리는 새로운 선택 대신 칼만 필터를 조정하고 루프 전달 복구 기술을 사용 , 원래이다 칼만 필터가 최적이되도록 조정 매트릭스를, . 는 양의 한정된 대칭 행렬이며, ID 행렬 ( ) 로 선택할 수 있습니다 . 그런 다음 스칼라 선택하십시오 . 그 결과 제어기는 (이상)이되어야 안정 있지만 행렬 덜 최적이되는 수단, 디 튜닝된다. $Q_o = Q_0 + q^2BVB^T$ $Q_0$ $Q$ $V$ $V=I$ $q$ $q \rightarrow \infty$ $Q_o$

따라서 안정 될 때까지 늘리 십시오. 안정적으로 만들 수있는 또 다른 방법은 컨트롤러를 느리게 만들기 위해 를 늘리 거나 줄이는 입니다. $q$ $R_c$ $Q_c$

이 게시물의 개념은 상당히 발전했지만 Riccati 방정식과 같은 문제를 해결해야하는 경우 MATLAB 또는 기타 소프트웨어를 사용하여이를 수행 할 수 있습니다. 이미 Kalman 필터를 구현하는 라이브러리가있을 수도 있습니다 (MATLAB 도이 작업을 수행한다고 생각합니다).

임베디드 애플리케이션의 경우 C 구현이있을 수 있지만 Kalman 필터를 직접 구현해야 할 수도 있습니다.

— ronalchn
소스

훌륭하고 심층적 인 답변에 감사드립니다. 나는 당신의 첫 번째 해결책의 요점을 따르며 그것을 해결할 수 있다고 확신합니다. 두 번째로, 당신이 말한 것처럼, 더 도전적이고 내가 그것을 모두 따를 수 있는지 확인하기 위해 노력해야 할 것입니다.

— ViennaMike

자이로는 간단한 대답입니다. 나는 항상 짧은 측정을위한 자이로 컴퍼스, 오랫동안 나침반을 들었습니다. 그리고 실제로 두 시간 사이에 칼만 필터를 사용합니다. 6DOF 자이로 보드 / acc 보드의 가격은 요즘 20 달러 미만이며, 보드를 사용하기에는 너무 저렴합니다.

한 번에 다른 사람의 Kallman 필터를 통해 작업했습니다 . 그리고 작동하게했습니다. Kallman 필터는 실제로 정확한 구현이 아니라 접근 방식에 가깝습니다. 자이로의 경우 최종 결과에는 행렬 수학을 사용할 필요가 없습니다. 훨씬 간단한 코드를 만듭니다.

— 스파이크 3
소스