2011-8 第4問 (2)

カルマンフィルタとは、誤差のある離散的な観測値から時間変化するシステムの現在の状態を推定する手法。システムの現在の観測値と1ステップ前の推定値から、システムの現在の状態の推定値と1ステップ先の予測値を与える反復型の予測を行う。
カルマンフィルタは各ステップごとに予測と更新の二つを行う。予測では前ステップの推定値から現在の推定状態を計算し、更新では現在の観測値を利用して推定値を補正してより正確な値に近づける。

2011-8 第4問 (1)

フィードバック制御は、連続動作するシステムにおいて、前回の入力に対する出力を入力要素の一つとすることで、より精密な出力を得る手法。制御を乱す外的要因(外乱)による影響を自動的に修正できるが、外乱による影響を検出してからでないと修正ができないという欠点が存在する。
フィードフォワード制御は、外乱による影響が現れる前に外乱を検知して修正を行う制御方式。対象となる外乱の影響を最小限にできるが、予期しない外乱に弱く、また一定の状態を維持するといったことができない。
そのため、通常はフィードバック制御とフィードフォワード制御を併用する。

2011-8 第3問 (5)

w_x = h_x - l₃cosh_θ
w_y = h_y - l₃sinh_θ

なので、(w_x, w_y) ≠ (0, 0) のとき、
(3)で示した手続きによりθ₁およびθ₂を求めることができる。

このとき、θ₃ = h_θ - θ₁ - θ₂ より、
残りのθ₃も求めることができる。

また、(w_x, w_y) = (0, 0)のとき、
θ₁およびθ₂は一意に決まらない。

2011-8 第3問 (4)

h_θ = θ₁ + θ₂ + θ₃
h_x = w_x + l₃cosh_θ = l₁cosθ₁ + l₂cos(θ₁ + θ₂) + l₃cos(θ₁ + θ₂ + θ₃)
h_y = w_y + l₃sinh_θ = l₁sinθ₁ + l₂sin(θ₁ + θ₂) + l₃sin(θ₁ + θ₂ + θ₃)

2011-8 第3問 (3)

直線OWとX軸のなす角をθ_wとし、
l_w = sqrt(w_x² + w_y²)

余弦定理より、
l₂² = l₁² + l_w² - 2l₁l_wcos(θ_w - θ₁)

加法定理より、
l₂² = l₁² + l_w² - 2l₁cosθ₁ * l_wcosθ_w - 2l₁sinθ₁ * l_wsinθ_w

関節Eの座標を(e_x, e_y)とすると、
l₂² = l₁² + l_w² - 2e_xw_x - 2e_yw_y
2e_yw_y = l₁² - l₂² + l_w² - 2e_xw_x

両辺を二乗して、
4e_y²w_y² = (l₁² - l₂² + l_w²)² - 4e_xw_x(l₁² - l₂² + l_w²) + 4e_x²w_x²

e_x² + e_y² = l₁²なので、
4w_y²l₁² - 4e_x²w_y² = (l₁² - l₂² + l_w²)² - 4e_xw_x(l₁² - l₂² + l_w²) + 4e_x²w_x²

e_xについて整理して、
4(w_x² + w_y²)e_x² - 4w_x(l₁² - l₂² + l_w²)e_x + (l₁² - l₂² + l_w²)² - 4w_y²l₁² = 0

w_x² + w_y² = l_w²なので、
4l_w²e_x² - 4w_x(l₁² - l₂² + l_w²)e_x + (l₁² - l₂² + l_w²)² - 4w_y²l₁² = 0

この2次方程式の判別式をD_xとすると、

D_x/16 = w_x²(l₁² - l₂² + l_w²)² - l_w²((l₁² - l₂² + l_w²)² - 4w_y²l₁²)

展開して整理すると、

D_x/16 = (w_x² - l_w²)(l₁² - l₂² + l_w²)² + 4w_y²l₁²l_w² 

w_x² - l_w² = - w_y²なので、
D_x/16 = w_y²(4l₁²l_w² - (l₁² - l₂² + l_w²)²) 

因数分解して、
D_x/16 = w_y²(l₂ - l₁ + l_w)(l₂ + l₁ - l_w)(l₁ - l₂ + l_w)(l₁ + l₂ + l_w)

l₁ + l₂ + l_w > 0なので、D_x > 0となるのは
w_y ≠ 0 かつ l₁ + l₂ > l_w かつ l₁ + l_w > l₂ かつ l₂ + l_w > l₁のとき、
つまり、
WがX軸上に存在せず、三点OEWが一直線上に並ばないときである。

このとき、DをD_x/16 = w_y²Dとすると、
e_x = (w_x(l₁² - l₂² + l_w²) ± w_{y *} sqrt(D)) / 2l_w²

e_x² + e_y² = l₁²より、
e_x = (w_x(l₁² - l₂² + l_w²) + w_y * sqrt(D)) / 2l_w² のとき e_y = (w_y(l₁² - l₂² + l_w²) - w_x * sqrt(D)) / 2l_w²
e_x = (w_x(l₁² - l₂² + l_w²) - w_y * sqrt(D)) / 2l_w² のとき e_y = (w_y(l₁² - l₂² + l_w²) + w_x * sqrt(D)) / 2l_w²

また、(w_x, w_y) ≠ (0, 0) で、関節WがX軸上に存在する場合、
e_x = w_x(l₁² - l₂² + l_w²) / 2l_w² = (l₁² - l₂² + w_x²) / 2w_x
e_y = ±w_x * sqrt(D) / 2l_w² = ±sqrt(D) / 2w_x

同様に、(w_x, w_y) ≠ (0, 0) で、関節WがY軸上に存在する場合、
e_x = ±sqrt(D) / 2w_y
e_y = (l₁² - l₂² + w_y²) / 2w_y

また、(w_x, w_y) ≠ (0, 0)で、三点OEWが一直線上に並ぶ場合、
e_x = w_x(l₁² - l₂² + l_w²) / 2l_w²
e_y = w_y(l₁² - l₂² + l_w²) / 2l_w²

よって、(w_x, w_y) ≠ (0, 0) のとき、
θ₁, θ₂はこれらのe_x, e_yを用いて、

θ₁ = atan(e_y, e_x)
θ₂ = atan(w_y - e_y, w_x - e_x) - θ₁
と表せる。

また、(w_x, w_y) = (0, 0) のとき、
θ₁, θ₂は一意に決まらない。