aokomoriuta's blog

青子守歌のブログ

回転前後の座標が分かっている場合の回転中心座標を求める

剛体の移動を追跡する実験をしたときに、その剛体の2点に測定点を貼り付けてカメラ等で動画撮影する事がよくあります。この時、その剛体の特性がよく分かってない場合、その回転中心（もし拘束条件がなければ重心）はどのように求められるか？という解法について、とある事情で必要になったのでメモがてら書いておきます。

具体的に言うと、下図のような2点p0, p1が、ある点c周りに回転してP0, P1になった時、cの座標と回転角θを求めたいということです。

p0 p0 p1 P0 c P1

結果は以下の通りになりました。

\displaystyle \begin{align*} \theta&=\text{sign} \left\{ \left( \mathbf{p}_1-\mathbf{p}_0 \right) \times \left( \mathbf{P}_1-\mathbf{P}_0 \right) \right\} \cos^{-1} \frac{\left( \mathbf{p}_1-\mathbf{p}_0 \right) \cdot \left( \mathbf{P}_1-\mathbf{P}_0 \right)}{\left| \mathbf{p}_1-\mathbf{p}_0 \right| \left| \mathbf{P}_1-\mathbf{P}_0 \right|} \\ c_x&=\frac{X_0 + x_0}{2} \pm \sqrt{\left(\frac{X_0 + x_0}{2} \right)^2 - X_0 x_0 - \frac{L^2}{4} + \left(\frac{Y_0 - y_0 }{2 \sin \frac{\theta}{2}}\right)^2 } \\ c_y&=\frac{{X_0}^2-{x_0}^2+{Y_0}^2-{y_0}^2- 2 X_0 + 2x_0 }{Y_0 - y_0}c_x \end{align*}

以下証明など

まず回転角について考える。これは線分\overline{p_1 p_0}が\overline{P_1 P_0}に移動したことから、回転角θは明らかにそれらのなす角になるので

\displaystyle \begin{align*} \theta&=\angle \left( \mathbf{p}_1-\mathbf{p}_0, \mathbf{P}_1-\mathbf{P}_0 \right) \\ \cos \theta&=\frac{\left( \mathbf{p}_1-\mathbf{p}_0 \right) \cdot \left( \mathbf{P}_1-\mathbf{P}_0 \right)}{\left| \mathbf{p}_1-\mathbf{p}_0 \right| \left| \mathbf{P}_1-\mathbf{P}_0 \right|} \\ \theta&=\text{sign} \left\{ \left( \mathbf{p}_1-\mathbf{p}_0 \right) \times \left( \mathbf{P}_1-\mathbf{P}_0 \right) \right\} \cos^{-1} \frac{\left( \mathbf{p}_1-\mathbf{p}_0 \right) \cdot \left( \mathbf{P}_1-\mathbf{P}_0 \right)}{\left| \mathbf{p}_1-\mathbf{p}_0 \right| \left| \mathbf{P}_1-\mathbf{P}_0 \right|} \end{align*}

と求められる。

次に、この回転角θでp0からP0に移動させる場合の回転半径R0を考える。\triangle{p_0cP_0}は、\overline{cp_0}=\overline{cp_1}かつ\angle p_0cP_0 = \thetaな、二等辺三角形であることから

\displaystyle \angle cp_0P_0 = \angle cP_0p_0 = \frac{\pi - \theta}{2}

である。よって余弦定理と三角関数の性質より

\displaystyle \begin{align*} \frac{\overline{cp_0}}{\sin \angle cP_0p_0} &= \frac{\overline{p_0P_0}}{\sin \angle P_0cp_0} \\ \frac{R_0}{\sin \frac{\pi - \theta}{2}} &= \frac{L}{\sin \theta} \\ \frac{R_0}{\cos \frac{\theta}{2}} &= \frac{L}{2 \sin \frac{\theta}{2} \cos \frac{\theta}{2}} \\ R_0 &= \frac{L}{2 \sin \frac{\theta}{2}} \end{align*}

と求められる。なおここで、p0とP0の距離すなわち\overline{p_0P_0}をLと置いた。

ということで、あとは半径R0で2点p0,P0を通る円の中心点を求める問題に帰着する（高校生で良くやる問題レベルのはず）。色々やり方はあるが、今回は、円の方程式(1)と二等分線(2)の交点となると立てて

\displaystyle \begin{align*} \left( x_0-c_x \right)^2 + \left( y_0 - c_y \right)^2 &= {R_0}^2 \tag{1} \\ \left( X_0-x_0\right) c_x + c_y \left( Y_0-y_0 \right) &= \frac{{X_0}^2-{x_0}^2+{Y_0}^2-{y_0}^2}{2} \tag{2} \end{align*}

(2)を(1)に代入してcxのみの方程式に整理してやると

\displaystyle L^2 {c_x}^2 − L^2 \left(X_0 + x_0 \right) c_x + \frac{4 L^2 X_0 x_0+L^4}{4} - {R_0}^2 \left(Y_0 - y_0 \right)^2 = 0

という二次方程式を得る。R0を代入して変形すると

\displaystyle \begin{align*} & L^2 {c_x}^2 − L^2 \left(X_0 + x_0 \right) c_x + \frac{4 L^2 X_0 x_0+L^4}{4} - \left( \frac{L}{2 \sin \frac{\theta}{2}} \right)^2 \left(Y_0 - y_0 \right)^2 = 0 \\ & {c_x}^2 − \left(X_0 + x_0 \right) c_x + X_0 x_0+\frac{L^2}{4} - \left(\frac{Y_0 - y_0 }{2 \sin \frac{\theta}{2}}\right)^2 = 0 \end{align*}

よって、中心x座標cxはこの方程式の解となり

\displaystyle \begin{align*} c_x &= \frac{X_0 + x_0 \pm \sqrt{\left(X_0 + x_0 \right)^2 - 4 \left\{ X_0 x_0+\frac{L^2}{4} - \left(\frac{Y_0 - y_0 }{2 \sin \frac{\theta}{2}}\right)^2 \right\} }}{2} \\ &= \frac{X_0 + x_0}{2} \pm \sqrt{\left(\frac{X_0 + x_0}{2} \right)^2 - X_0 x_0 - \frac{L^2}{4} + \left(\frac{Y_0 - y_0 }{2 \sin \frac{\theta}{2}}\right)^2 } \end{align*}

となる。

中心y座標は、(2)にこの結果を代入することで得られる。

なお、自明ではあるが導出から分かる通り、「回転運動のみ」で得られている仮定があることから、P1は自由変数ではなく、p0,P0から得られたこの中心座標cと回転角θからP1が確定する。もしP1が算出された値と異なる場合は、それは並進運動も含まれていることを意味する。

並進運動を加味した方程式はまたどこかで書くかもしれない。


Y axisX axis00-2-22244-2-22244p0p1P0P1Expression 5Expression 12Expression 13Expression 23Expression 24Expression 25Expression 27Expression 28Expression 29p0p0p1p1P0P0P1P1left parenthesis, 1.0 7 , 1.3 2 6 , right parenthesis1.07, 1.326left parenthesis, 1.0 7 , 1.3 2 6 , right parenthesis1.07, 1.326