角度の不連続性の取り扱いについて

In [90]:
%matplotlib inline

# import matplotlib.pyplot as plt
from matplotlib.pyplot import *
import numpy as np

vec = lambda x: x[:,np.newaxis]
pi = np.pi
cos = np.cos
sin = np.sin
atan2 = np.arctan2

自律移動車両の制御において，359degの次が0degだったり，180degの次が-179degだったり，実際には1degしか変わらないのに数値上は大きく変化していて困ることがある．

例えば目標角度-179deg，現在の角度179degとかだと実際には2degしか変わらないのに数値上は358degマイナスしなきゃいけないように見える．もし角度偏差に比例するような制御コマンドを送ってたりするとだいぶ大回りしてしまう．

あるいは，ある時間内に得られた方位角を平均することで方位センサのノイズを低減したいときにも数値上の大きな変化に悩まされることがある．

例えば-179degと179degを平均したら180degになって欲しいが，単純に平均すると0degになってしまう．

このような角度の不連続性によって生じる問題を解決するために角度をもっと上手く取り扱いたい．

1. 角度の定義

角度はラジアンを扱う． また，角度を$\theta$とするとその範囲は$-\pi \lt \theta \leq \pi $であるとする．

ちなみに適当な角度を上記の範囲に変換するには以下の関数がシンプルで便利

In [91]:
def pi2pi(p):
    return atan2(sin(p),cos(p))
In [92]:
theta = pi*5
print(pi2pi(theta))

3.1415926535897927

2.角度ベクトルの定義

-179degと180degの不連続性を解決するにはsin,cosあたりを使うと良さそうである．

なので，とある(我々におなじみの)角度$\theta$に対応するベクトル$\boldsymbol{q}$を以下のように定義する．(角度ベクトルと呼ぶ)

$$\boldsymbol{q}(\theta)=\left( \begin{array}{c} \cos \theta \\ \sin \theta \end{array} \right)$$

この定義によって-175degと170degは距離的に近いことが表現できる．

In [93]:
def th2q(th):
    return vec(np.array([cos(th),sin(th)]))
In [94]:
t=np.linspace(-np.pi,np.pi,100)
th1,th2=-175,170
q1,q2=th2q(d2r(th1)),th2q(d2r(th2))
figure()
plot(cos(t),sin(t))
plot([0,q1[0]],[0,q1[1]],'om-',label='q1')
plot([0,q2[0]],[0,q2[1]],'oc-',label='q2')
xlim(-1,1)
ylim(-1,1)   
legend()
grid()
axis('equal')
Out[94]:
(-1.0999748271191592,
 1.0994713695023444,
 -1.0998615404412626,
 1.0998615404412626)

3.角度ベクトルの差

角度ベクトル同士の差を考えたい．しかし，普通にベクトル同士の差を考えると角度の差と上手く対応しない．

そこで回り道してまず，角度ベクトル同士の和を定義する．ただし，通常のベクトル和と区別するために角度ベクトル和を$\oplus$と表記する． 角度ベクトル和について，直感的に以下の関係が成り立って欲しい． $$\boldsymbol{q}(\theta_1) \oplus \boldsymbol{q}(\theta_2) = \boldsymbol{q}(\theta_1 + \theta_2)$$

これをベクトル$\boldsymbol{q}(\theta_1)$を$\theta_2$だけ回転させると解釈すると$\oplus$という演算は以下のように表すことができる．

$$\boldsymbol{q}(\theta_1) \oplus \boldsymbol{q}(\theta_2) = \left ( \begin{array}{c} \cos \theta_2&-\sin \theta_2 \\ \sin \theta_2 & \cos \theta_2 \end{array} \right) \boldsymbol{q}(\theta_1)$$

※この演算は可換なのでどっちを回転させると解釈してもよい

In [95]:
def q2R(q):
    q2=vec(np.r_[-q[1],q[0]])
    qR=np.c_[q,q2]
    return qR
    
def qpls(q1,q2):
    q3=q2R(q2)@q1
    return q3

角度ベクトルの差は「負の角度ベクトル」との和として計算できる． 「負の角度ベクトル」について，直感的に以下の関係がなりたって欲しい．

$$\boldsymbol{q}(\theta_1)\oplus (-\boldsymbol{q}(\theta_2)) = \boldsymbol{q}(\theta_1 - \theta_2)$$

これを，ベクトル$\boldsymbol{q}(\theta_1)$を$-\theta_2$だけ回転させると解釈すると

$$\boldsymbol{q}(\theta_1) \oplus (-\boldsymbol{q}(\theta_2)) = \left ( \begin{array}{c} \cos \theta_2& \sin \theta_2 \\ -\sin \theta_2 & \cos \theta_2 \end{array} \right) \boldsymbol{q}(\theta_1)$$

したがって，負の角度ベクトルは以下のように定義できる． $$-\boldsymbol{q}(\theta)=\left( \begin{array}{c} \cos \theta \\ -\sin \theta \end{array} \right)$$

In [96]:
def qconj(q):
    q2=np.r_[q[0],-q[1]]
    return q2

def qdev(qtgt,qcur):
    return qpls(qtgt,qconj(qcur))

以上の角度ベクトルの和，負，差について図示する．

In [97]:
q2_conj=qconj(q2)
q1pq2=qpls(q1,q2)
q1mq2=qpls(q1,qconj(q2))
figure()
plot(cos(t),sin(t))
plot([0,q1[0]],[0,q1[1]],'xm-',label='q1')
plot([0,q2[0]],[0,q2[1]],'xc-',label='q2')    
plot([0,q2_conj[0]],[0,q2_conj[1]],'oc-',label='q2 conj')
plot([0,q1pq2[0]],[0,q1pq2[1]],'ob-',label='q1+q2')
plot([0,q1mq2[0]],[0,q1mq2[1]],'oy-',label='q1-q2')
xlim(-1,1)
ylim(-1,1)   
legend(loc=0)
grid()
axis('equal')
Out[97]:
(-1.0999748271191592,
 1.0994713695023444,
 -1.0998615404412626,
 1.0998615404412626)

また，車両制御でよく使う角度ベクトル差分について検証する． 目標角度-170degに対して，現在の角度が-180 ~ 180degまで変化したときの角度差分の変化を見る

In [98]:
th_target=-170
q_target=th2q(d2r(th_target))
for th_cur in np.linspace(-180,90,4):
    figure()
    q_cur=th2q(d2r(th_cur))
    dq=qdev(q_target,q_cur)#delta q

    plot([0,q_target[0]],[0,q_target[1]],'o-b',label='target')
    plot(cos(t),sin(t))
    plot([0,q_cur[0]],[0,q_cur[1]],'o-m',label='current')
    plot([0,dq[0]],[0,dq[1]],'o-y',label='difference')
    xlim(-1,1)
    ylim(-1,1)
    grid()
    axis('equal')
    legend()

差分角度ベクトルを見ると，きちんと角度の正負，大きさの情報が含まれていそうである．したがって，以下の式によって不連続性を考慮した角度差分を計算することが可能である．　

$$\Delta \theta = \mathrm{atan2}( \Delta\boldsymbol{q}[2], \Delta\boldsymbol{q}[1])$$

ただし， $$\Delta\boldsymbol{q} =\boldsymbol{q}(\theta_{\mathrm{target}}) \oplus (-\boldsymbol{q}(\theta_{\mathrm{current}})) $$

また， $$\Delta\boldsymbol{q} = \left( \begin{array}{c} \Delta\boldsymbol{q}[1] \\ \Delta\boldsymbol{q}[2] \end{array} \right)$$

In [99]:
def q2th(q):
    return atan2(q[1],q[0])

def dev(thtgt,thcur,radian=True):
    f=1 if radian is True else d2r(1)    
    dq=qdev(th2q(f*thtgt),th2q(f*thcur))
    return q2th(dq)/f

def pls(th1,th2,radian=True):
    f=1 if radian is True else d2r(1)
    qp=qpls(th2q(f*th1),th2q(f*th2))
    return q2th(qp)/f
In [100]:
print('theta1,theta2 =',th1,',',th2)
print('delta theta = ',dev(th1,th2,radian=False))

theta1,theta2 = -175 , 170
delta theta =  [15.]

ここまでまとめ

角度に対応する角度ベクトルを以下で定義する． $$\boldsymbol{q}(\theta)=\left( \begin{array}{c} \cos \theta \\ \sin \theta \end{array} \right)$$

また，負の角度ベクトルを以下で定義する． $$-\boldsymbol{q}(\theta)=\left( \begin{array}{c} \cos \theta \\ -\sin \theta \end{array} \right)$$

角度ベクトル同士の和を以下で定義する． $$\boldsymbol{q}(\theta_1) \oplus \boldsymbol{q}(\theta_2) = \left ( \begin{array}{c} \cos \theta_2&-\sin \theta_2 \\ \sin \theta_2 & \cos \theta_2 \end{array} \right) \boldsymbol{q}(\theta_1)$$

このとき，目標角度$\theta_\mathrm{target}$，現在の角度$\theta_\mathrm{current}$に対して，角度差は以下の式で計算すると角度の不連続性に悩まされなくて済む．

$$\Delta \theta = \mathrm{atan2}( \Delta\boldsymbol{q}[2], \Delta\boldsymbol{q}[1])$$

ただし， $$\Delta\boldsymbol{q} =\boldsymbol{q}(\theta_{\mathrm{target}}) \oplus (-\boldsymbol{q}(\theta_{\mathrm{current}})) $$

また， $$\Delta\boldsymbol{q} = \left( \begin{array}{c} \Delta\boldsymbol{q}[1] \\ \Delta\boldsymbol{q}[2] \end{array} \right)$$

注記

この方法で計算した角度差も実は不連続点がある． それは目標角度と現在角度の差が180degの点である． なので，現在角度が観測誤差で大きく振れるような場合，角度差が-179degから180degに飛ぶこと考えられる． そのときには舵を左右に交互に大きく切る挙動が予測される．

4.角度ベクトルの平均

$\theta_1,\theta_2$ = -170,170の2つを平均することを考える． 平均した結果は180degまたは0degという2通りが考えられる． しかし，実際の運用上は180degが欲しいというパターンが多い．

そこで，-170deg,170degから180degが出力できるような演算として球面線形補間を考える．

球面線形補間

説明省略 以下の式である．

$$ \boldsymbol{q}_{\mathrm{t}} = \frac{\sin((1-t)\theta)\boldsymbol{q}_{\mathrm{s}} +\sin(t \theta) \boldsymbol{q}_{\mathrm{e}} }{\sin(\theta)}$$

ただし，$0 \leq t \leq 1$，また，$\theta$は$\boldsymbol{q}_{\mathrm{e}} ,\boldsymbol{q}_{\mathrm{s}}$のなす角である．

ここで$t=1/2 , \boldsymbol{q}_{\mathrm{s}} =\boldsymbol{q}_{\theta_1} ,\boldsymbol{q}_{\mathrm{e}} = \boldsymbol{q}_{\theta_2}$とすると，2つの角度の平均が取れる．

In [101]:
def qslerp(qs,qe,t): # spherical linear interpolation, return qe@t=1 qs@t=0
    th=q2th(qdev(qe,qs))
    return (sin((1-t)*th)*qs + sin(t*th)*qe )/(sin(th))

ちなみに球面線形補間を以下のように見てみると，もう少し平均をとってる感が出て来る． $$\mathrm{qslerp}(\boldsymbol{q}_{\mathrm{s}},\boldsymbol{q}_{\mathrm{e}},t) =\left[(1-t)\circ \boldsymbol{q}_{\mathrm{s}} \right] \dagger \left[t \circ \boldsymbol{q}_{\mathrm{e}} \right]\\ =\left[\frac{1}{2}\circ \boldsymbol{q}_{\mathrm{s}} \right] \dagger \left[\frac{1}{2} \circ \boldsymbol{q}_{\mathrm{e}} \right]$$

In [102]:
qs=th2q(d2r(170))
qe=th2q(d2r(-170))
qave=qslerp(qs,qe,0.5)
thave=r2d(q2th(qave))
print(thave)

[180.]

次に$\theta_1,\theta_2,,\theta_3$ = -170,170,177の平均を考える． 考えるにあたって，せっかく確立した2つの角度の平均演算を活用したい．そこで，n-1個の角度の平均値を使ってn個の角度の平均値を求める式を導出する．

角度に関係なく一般的に，n個の何らかの値を$x_\mathrm{n}$，値の総和を$S_\mathrm{n}$，平均値を$a_\mathrm{n}$とおくとき，以下の関係式が成り立つ． $$\begin{eqnarray} a_\mathrm{n} &=& \frac{S_\mathrm{n}}{n}\\ &=&\frac{S_\mathrm{n-1} + x_\mathrm{n}}{n}\\ &=&\frac{S_\mathrm{n-1}}{n} + \frac{x_\mathrm{n}}{n}\\ &=&\frac{n-1}{n} \frac{S_\mathrm{n-1}}{n-1} + \frac{x_\mathrm{n}}{n}\\ &=&\frac{n-1}{n} a_\mathrm{n-1} + \frac{x_\mathrm{n}}{n}\\ \end{eqnarray}$$

具体的にnに数字を入れてみる．ただし$a_1=x_1$は自明であるとする． $$\begin{eqnarray} a_1&=&x_1\\ a_2&=&\frac{1}{2} a_1 + \frac{1}{2} x_2\\ a_3&=&\frac{2}{3} a_2 + \frac{1}{3} x_3 \end{eqnarray}$$

ここで，球面線形補間の式に戻ってみる．n個目までの角度ベクトルの平均を$\boldsymbol{a}_\mathrm{n}$とおく．$\boldsymbol{a}_\mathrm{1}=\boldsymbol{q}_\mathrm{1}$が自明であるとすると，以下が成り立つ． $$\begin{eqnarray} \boldsymbol{a}_\mathrm{1}&=&\boldsymbol{q}_\mathrm{1} \\ \boldsymbol{a}_2 &=& \left[\frac{1}{2}\circ \boldsymbol{a}_{\mathrm{1}} \right] \dagger \left[\frac{1}{2} \circ \boldsymbol{q}_{\mathrm{2}} \right] \end{eqnarray}$$

一般的に$a_3=\frac{2}{3} a_2 + \frac{1}{3} x_3$が成り立つことを踏まえると，アナロジーとして，3つの角度ベクトルの平均について以下の式が成り立ちそうである． $$\begin{eqnarray} \boldsymbol{a}_3 &=& \left[\frac{2}{3}\circ \boldsymbol{a}_{\mathrm{2}} \right] \dagger \left[\frac{1}{3} \circ \boldsymbol{q}_{\mathrm{3}} \right]\\ &=& \mathrm{qslerp}( \boldsymbol{a}_{\mathrm{2}},\boldsymbol{q}_{\mathrm{3}},1/3) \end{eqnarray} $$

また，更に一般化して以下の式が成り立ちそうである． $$ \boldsymbol{a}_\mathrm{n} = \mathrm{qslerp}( \boldsymbol{a}_{\mathrm{n-1}},\boldsymbol{q}_{\mathrm{n}},1/n) $$

上記の平均値算出式を試すと上手く出来ていそうである． もう少しきちんと証明したい．

In [103]:
def qmean(qlist): # ave_(n) = {(n-1)/n)}*ave_(n-1) + (1/n)*x_(n)
    an=np.ones((2,1))
    for i,q in enumerate(qlist):
        j=i+1
        an=qslerp(an,q,1/j)
    return an

def mean(thlist,radian=True):
    f=1 if radian is True else d2r(1)
    f=d2r(1)
    qlist=[th2q(f*th) for th in thlist]
    qave=qmean(qlist)
    return q2th(qave)/f
In [104]:
##6
th1,th2=-170,170
th3=177
q1=th2q(d2r(th1))
q2=th2q(d2r(th2))       
q3=th2q(d2r(th3))      
qlist=[q1,q2,q3]    
qave=qmean(qlist)

figure()    
plot(cos(t),sin(t))
plot([0,q1[0]],[0,q1[1]],'or-',label='q1')
plot([0,q2[0]],[0,q2[1]],'ob-',label='q2')    
plot([0,q3[0]],[0,q3[1]],'og-',label='q3')
plot([0,qave[0]],[0,qave[1]],'ok-',label='qave')
xlim(-1,1)
ylim(-1,1)   
legend()
grid()
axis('equal')

print("th ave = ",r2d(q2th(qave)))

th ave =  [179.]