\documentclass[a4paper,10pt, twocolumn, fleqn]{jsarticle}
\usepackage{sty/config}

\title{\gtfamily\bfseries ���O������ɂ���}
\author{\gtfamily\bfseries �O���T��}

\begin{document}
\maketitle

\section{�ړI�ƊT�v}


\section{�L���E�p��̒�`}

\begin{description}
\item[$\varDelta t$] :
�V�~�����[�V������1�X�e�b�v������̎��ԍ��݁D���������ăX�e�b�v$n$�ɂ����鎞����$n \varDelta t$�ł���D

\item[$\bm{traj}(t)$] :
���O���D����$t$�̎��ʉ߈ʒu��\���D

\item[$\bm{r}_n$] :
�X�e�b�v$n$�ɂ�����ڕW���ʒu�D

\item[$\bm{\theta}_n$] :
�X�e�b�v$n$�ɂ�����ڕW�֐ߊp�x�D

\item[$\bm{\omega}_n$] :
�X�e�b�v$n$�ɂ�����ڕW�֐ߊp���x�D

\item[$\bm{p}_n$] :
�X�e�b�v$n$�ɂ�������ʒu�D

\item[$\bm{q}_n$] :
�X�e�b�v$n$�ɂ�����֐ߊp�x�D

\item[$J(\theta)$] :
�֐ߊp�x$\bm{\theta}$�ɂ�����֐ߊp�����ψ�$\varDelta\bm{\theta}$�����ʒu�����ψ�$\varDelta\bm{r}$�ɕϊ����郄�R�r�A���D$\varDelta\bm{\theta} = J(\bm{\theta}) \varDelta\bm{r}$�D
\end{description}


\section{�t�^���w}

�t�^���w��p���āC���O������ڕW�֐ߊp�����߂�D�V�~�����[�V����1�X�e�b�v������̎��ړ��ʂ͔����ł��邽�߁C���R�r�A���̋[���t�s��ɂ������t�^���w��1�X�e�b�v������1��s�����ƂŎ�������D

\subsection{�ڕW�֐ߊp�x�̌v�Z}

�X�e�b�v$n$����$n+1$�ɂ����Ă̖ڕW���ʒu�̕ω���
\begin{gather*}
\bm{r}_{n+1} - \bm{r}_n \\
( = \bm{traj}(n\varDelta t+\varDelta t) - \bm{traj}(n\varDelta t) )
\end{gather*}
�ł���D������$\varDelta t$���\���������C��r�I���炩�Ȏ��O���Ȃ��$\bm{traj}(t+\varDelta t) - \bm{traj}(t)$�͔����ł���Ƒz�肳���D���������ăX�e�b�v�Ԃ̖ڕW���ʒu�ω�$\bm{r}_{n+1} - \bm{r}_n$�ƖڕW�֐ߊp�x�ω�$\bm{\theta}_{n+1} - \bm{\theta}_n$�̊֌W�̓��R�r�A��$J(\bm{\theta}_n)$�ŋߎ��ł���i���R�r�A���̋�̓I�ȓ��e��\��{jacobian}�ŏq�ׂ�j�D
\begin{equation*}
\bm{r}_{n+1} - \bm{r}_n = J(\bm{\theta}_n) (\bm{\theta}_{n+1} - \bm{\theta}_n)
\end{equation*}
$J(\bm{\theta}_n)$�͈�ʂɐ����s��ł͂Ȃ��̂ŁC�[���t�s��$J^\#$��p����$\bm{\theta}_{n+1} - \bm{\theta}_n$�ɂ��ĉ�����
\begin{equation*}
\bm{\theta}_{n+1} - \bm{\theta}_n = J^\#(\bm{\theta}_n) (\bm{r}_{n+1} - \bm{r}_n)
\end{equation*}

���������āC�X�e�b�v$n$�ɂ�����ڕW���ʒu$\bm{r}_n$�C�ڕW�֐ߊp�x$\bm{\theta}_n$�����܂��Ă���΁C�X�e�b�v$n+1$�ɂ�����ڕW�֐ߊp$\bm{\theta}_{n+1}$��
\begin{gather*}
\bm{\theta}_{n+1} = J^\#(\bm{\theta}_n) (\bm{r}_{n+1} - \bm{r}_n) + \bm{\theta}_n \\
\text{(������$\bm{r}_{n+1} = \bm{traj}((n+1)\varDelta t)$)}
\end{gather*}
�œ�����D


\subsection{�ڕW�֐ߊp���x�̌v�Z}

1�X�e�b�v�Ŋ֐ߊp�x��$\bm{\theta}_{n+1} - \bm{\theta}_n$�����ω������邽�߂̖ڕW�֐ߊp���x$\bm{\omega}_{n+1}$�́C
\begin{equation*}
\bm{\omega}_{n+1} = (\bm{\theta}_{n+1} - \bm{\theta}_n) / \varDelta t
\end{equation*}
�ł���\footnote{�����F�ϕ���@���֌W����Ǝv����̂Ō��؂̂��ƁD}�D


\section{���̈��艻}

�[���t�s����͓��َp���t�߂ɂȂ�Ƌɒ[�ɑ傫�Ȓl���o�Ă��܂����肵�Ȃ��D�����ŁCTikhonov���������s���������艻����D

$J$�̓��ْl������$U \Sigma V^T$�Ƃ���D�ȉ��Ɏ���$J^\#�f$��$J^\#$�̑���ɗp���ĉ��𓾂�D
\begin{gather*}
J^\#�f = V \sigma U^T \\
������ \sigma_{ii} = \frac{\Sigma_{ii}}{\Sigma_{ii}^2 + \epsilon^2}
\end{gather*}
$\epsilon$�͐������p�����[�^�ŁC�傫���قǉ������肳����i���̈���Ő����������o�Ȃ��Ȃ�j�D



\section{��p�����A}

$J^\#(\bm{\theta}_n)$�ɂ����́C�X�e�b�v�Ԃ̖ڕW�֐ߊp�x�ω��̓��a$(\bm{\theta}_{n+1} - \bm{\theta}_n)^T(\bm{\theta}_{n+1} - \bm{\theta}_n)$���ŏ�������D���̂��Ƃ͋O���ɂ���Ă͕s���R�Ȏp���◍�܂�̌����ɂȂ�D

�s���R�Ȏp�����������@�Ƃ��āC���R�Ȋ�p����ݒ肵�C�����N�A�[���̏璷���R�x��p���Ċ�p���ɋ߂����𓾂���@���l������i�璷���R�x�𐧌䂵�Ďw�肵���֐ߑ��x�ɋɗ͋߂����𓾂���@�͗Ⴆ�Ε���\cite{gemsjp2012}�ŏЉ��Ă���j�D

��p����$\bm{\theta}_{normal}$�Ƃ��C�ڕW�֐ߊp$\bm{\theta}_n$��$\bm{\theta}_{normal}$�ɋ߂Â���֐ߊp���x��$\bm{\omega}_{pullback}(\bm{\theta}_n, \bm{\theta}_{normal})$�Ƃ���i$\bm{\omega}_{pullback}$�̋�̓I�Ȍv�Z��\��{pullback}�ŏq�ׂ�j�D

���`������
\begin{gather*}
\varDelta\bm{r} = J \varDelta\bm{\theta}\\
������ \varDelta\bm{\theta} = \bm{\theta}_{n+1} - \bm{\theta}_n�C\varDelta\bm{r} = \bm{r}_{n+1} - \bm{r}_n�CJ=J(\bm{\theta}_n)
\end{gather*}
�́C�璷���R�x���܂މ���
\begin{equation*}
\varDelta\bm{\theta} = J^\# \varDelta\bm{r} + (I - J^\# J)\bm{k}
\end{equation*}
�ł���i���ӂɍ�����$J$���|����ƉE�ӑ�񍀂͋[���t�s��̐��� $J=J J^\# J$ �ɂ�������̂�$J\varDelta\bm{\theta}$�ɉe�����Ȃ����Ƃ��킩��j�D

�����ŁC��p���Ɍ������֐ߊp���x$\bm{\omega}_{pullback}(\bm{\theta}_n, \bm{\theta}_{normal})$���\�Ȍ�����������Ƃ���ƁC$\varDelta\bm{r}=\bm{0}$�̂Ƃ�$\varDelta\bm{\theta}=\bm{\omega}_{pullback}(\bm{\theta}_n, \bm{\theta}_{normal})$�ł���D���������āC
\begin{equation*}
\bm{\omega}_{pullback}(\bm{\theta}_n, \bm{\theta}_{normal}) = (I - J^\# J)\bm{k}
\end{equation*}
���̐��`��������$k$�ɂ��ĉ����D�E�ӂ��璷���R�x��\�������l�����$(I - J^\# J)$�̓t�������N�ł͂Ȃ����߁C�[���t�s���p����
\begin{equation*}
\bm{k} = (I - J^\# J)^\# \bm{\omega}_{pullback}(\bm{\theta}_n, \bm{\theta}_{normal})
\end{equation*}
�ł���D

�Ƃ���ŁC���ۂɂ͈ȉ��Ɏ����悤��$(I - J^\# J)^\# = I$�ł���C����������$\bm{k} = \bm{\omega}_{pullback}(\bm{\theta}_n, \bm{\theta}_{normal})$�ł���D
\begin{gather*}
(I - J^\# J)^\# \\
= (I - J^\# J)^T ( (I - J^\# J) (I - J^\# J)^T )^{-1} \\
��I - J^\# J �́i���Ԃ�j�Ώ̍s��Ȃ̂Ł� \\
= (I - J^\# J) ( (I - J^\# J) (I - J^\# J) )^{-1} \\
= (I - J^\# J) ( (I - 2 J^\# J + J^\# J J^\# J) )^{-1} \\
��J J^\# J = J ��聄\\
= (I - J^\# J) ( (I - J^\# J) )^{-1} \text{�c\footnote{�ł�$(I - J^\# J)$�͋t�s��������Ȃ��񂶂�Ȃ����������H}} \\
= I
\end{gather*}

�������C$\varDelta\bm{\theta}$�����߂鎮�͈ȉ��̒ʂ�ƂȂ�D
\begin{equation*}
\varDelta\bm{\theta} = J^\# \varDelta\bm{r} + (I - J^\# J)\bm{\omega}_{pullback}(\bm{\theta}_n, \bm{\theta}_{normal})
\end{equation*}



\section{�֐߂̏d�ݕt��}

���d�[���t�s���p����ƁC�֐߂Ɏg�p�D��x��^���邱�Ƃ��ł���D�傫�ȏd�݂�^����ꂽ�֐߂��ɗ͓������Ȃ��悤�ɓ��삷��D

�d�݂�\���Ίp�s���$W_{ii}$�Ƃ���ƁC���d�[���t�s�����
\begin{equation*}
\varDelta\bm{\theta} = W^{-1}J^T(J W^{-1} J^T)^{-1} \varDelta\bm{r}
\end{equation*}
�ƂȂ�\footnote{���̂����Ȃ邩�͍�����$J$���|���Ă݂�΂킩��D}�D

����C���������̓s������C���ۂɂ�$J$�̓��ْl������p����$J^\#$�����߂���@���Ƃ肽���D
�����ŁC$W^{-1} = D^2$�Ȃ�Ίp�s��$D$���`���C�ȉ��̂悤�Ɏ��ό`����D
\begin{gather*}
\varDelta\bm{\theta} = W^{-1}J^T(J W^{-1} J^T)^{-1} \varDelta\bm{r} \\
\iff \varDelta\bm{\theta} = D^2 J^T(J D^2 J^T)^{-1} \varDelta\bm{r} \\
\iff \varDelta\bm{\theta} = D D^TJ^T(J D D^TJ^T)^{-1} \varDelta\bm{r} \\
\iff \varDelta\bm{\theta} = D (JD)^T(JD (JD)^T)^{-1} \varDelta\bm{r} \\
\iff \varDelta\bm{\theta} = D (JD)^\# \varDelta\bm{r}
\end{gather*}
������C$W$���d�ݍs��Ƃ�����d�[���t�s����́C
\begin{gather*}
\varDelta\bm{\theta} = D (JD)^\# \varDelta\bm{r} \\
�i������ D_{ii} = \frac{1}{\sqrt{W_{ii}}}�j
\end{gather*}
�œ����邱�Ƃ��킩��D

������́C���R�r�A��$J$�̑���ɏd�ݕt�����R�r�A��$JD$��p���C$JD$�ɂ��[���t�s���������ꂽ��$D$���|������ŖڕW�֐ߊp�x�ɉ�����D�����̏����͊֐߂��Ƃɍs����̂Ŏ������₷���D


�Ȃ��C����Ɉȉ��̂悤�ɕό`����Ή��d�[���t�s����� $J \varDelta\bm{\theta} = \varDelta\bm{r}$ �𖞂������ł��邱�Ƃ��m���߂���D
\begin{gather*}
\varDelta\bm{\theta} = D (JD)^\# \varDelta\bm{r} \\
\iff D^{-1} \varDelta\bm{\theta} = (JD)^\# \varDelta\bm{r} \\
\Leftarrow JD D^{-1} \varDelta\bm{\theta} = \varDelta\bm{r} \\
\iff J \varDelta\bm{\theta} = \varDelta\bm{r}
\end{gather*}


% ���d�[���t�s����Ɨ��Ԃ̊֌W�ɂ��Ă͂�����x�l�����ق��������Ǝv���D




\section{�O���Ǐ]����}

\subsection{�_�炩���O���Ǐ]����}

�������̓I�ɋL�q����\��D
�����������Ă͕���\cite{pliantmotion}���Q�Ƃ��ꂽ���D


\subsection{LCP�v�Z�̔����񐔂ƒǏ]�덷}

�傫�Ȋ֐߃g���N��v����^���ł́C���m�ȃI�t�Z�b�g�g���N�����߂�̂ɂ�葽����LCP�����v�Z��v����D�Ⴆ�Ί֐߃_���p���ő���ɐݒ肵��2�����N�A�[���i�A�[�������莿�ʂ�1[kg]�j���d�͂ɋt����ĐÎ~������̂ɔ�����$niter = 50$���x�ȏ��v�����iSpringhead�̃f�t�H���g��$niter=15$�j�D������̔�����v����̂̓I�t�Z�b�g�g���N���v�Z����e�X�g�V�~�����[�V�����̊ԂɌ�����̂ŁC�e�X�g�V�~�����[�V�������͔����񐔂�傫���Ƃ�C�{�ԃV�~�����[�V�����ł͒ʏ�̔����񐔂ɖ߂��Čv�Z����΂悢�D

�֐߂̃o�l�E�_���p�͎��ʒu���O����ɗ��߂�悤��p����̂ŁC�o�l�E�_���p�W������r�I�傫����΃I�t�Z�b�g�g���N�v�Z�Ɍ덷�������Ă��z�������D���̏ꍇ�C�����񐔂ƃA�[���̏_����g���[�h�I�t�ƂȂ�D


\section{���̊֐߃��f���ł̎���}

\subsection{���R�r�A��} \label{sec_jacobian}


\subsection{�p�����A���x} \label{sec_pullback}


\subsection{�֐߉^���̎���}


\subsection{���^���w}



\newpage
\bibliographystyle{jplain}
\bibliography{trajik}

\end{document}
