四阶龙格库塔法RK4在求解陀螺仪位姿中的应用

        龙格库塔法一般用于求解一阶微分方程，是通过迭代的方式求解的，对于比较复杂的一阶微分方程，不方便用数学的方法求解，我们可以通过龙格库塔法进行求解。

拉格朗日中值定理

根据拉格朗日中值定理，设函数$f(x)$，满足：
　　（1）在闭区间$[x_0,x_1]$上连续；
　　（2）在开区间$(x_0,x_1)$内可导；
　　那么在开区间$(x_0,x_1)$内至少有一点$\xi$使：
　　

$f(x_1)-f(x_0)=f\prime (\xi )(x_1-x_0)$
　　即：
　　 $f(x_1)=f(x_0)+f\prime (\xi )(x_1-x_0)$
　　其中 $f\prime (\xi )$为函数 $f(x)$在点 $\xi$处的导数。
　　
　　从上可知 $x_1$时刻的函数值 $f(x_1)$可由 $x_0$时刻的函数值 $f(x_0)$和导数值 $f\prime (\xi )$预测得到。

龙格库塔法

       龙格库塔法的核心是求解$f\prime (\xi )$，我们可在$[x_0,x_1]$之间取N个导数值，然后将其进行加权平均，作为$f\prime (\xi )$的值，如果N=4，即是4阶龙格库塔法RK4。
　　RK4是通过不断迭代的方式求原函数$f(x)$的值，已知条件如下：
　　（1）关于原函数的一阶微分方程：$f\prime (x)=g(f(x),x)$
　　（2）在$x_0$时刻函数的初始值$f(x_0)$
　　求解函数在$x_1$时刻的函数值$f(x_1)$，当求解得$f(x_1)$后，我们可将其作为初始条件，根据一阶微分方程求解函数在$x_2$时刻的函数值$f(x_2)$，如此不断迭代下去，求得原函数在各个时刻的函数值。
　　RK4具体过程如下：
　　由$x_0$时刻的函数值$f(x_0)$预测$x_1$时刻的函数值$f(x_1)$，其中$h=x_1-x_0$。
　　Step1:根据初值$f(x_0)$，$x_0$，带入一阶微分方程求的$k_1$:
　　

$k_1=h\ast g(f(x_0),x_0)$
　　Step2:根据初值 $f(x_0)$， $x_0$， $k_1$按如下方式求 $k_2$:
　　 $k_2=h\ast g(f(x_0)+0.5\ast k_1,x_0+0.5\ast h)$
　　Step3:根据初值 $f(x_0)$， $x_0$， $k_2$按如下方式求 $k_3$:
　　 $k_3=h\ast g(f(x_0)+0.5\ast k_2,x_0+0.5\ast h)$
　　Step4:根据初值 $f(x_0)$， $x_0$， $k_3$按如下方式求 $k_4$:
　　 $k_4=h\ast g(f(x_0)+k_3,x_0+h)$
　　Step5:对上面的 $k_1,k_2,k_3,k_4$进行加权平均，得到函数值的增量，再求 $f(x_1)$：
　　 $f(x_1)=f(x_0)+\frac{1}{6}(k_1+2\ast k_2+2\ast k_3+k_4)$

应用1 求解一阶微分方程

       使用RK4求解一阶微分方程，设方程为:

$f\prime (x)=f(x)(x^2+1)，且x=0,f(0)=1。$

采用数学方法求解

为了 验证RK4的正确性，我们可先用数学的方法求解该微分方程的解。
　　$$\begin{array}{c}\frac{d_f(x)}{d_x}=f(x)(x^2+1)\\ \frac{d_f(x)}{f(x)}=(x^2+1)dx\end{array}$$
两边求积分得：
$$\begin{array}{c}ln(f(x))+C_1=\frac{1}{3}\ast x^3+x+C_2\\ ln(f(x))=\frac{1}{3}\ast x^3+x+C_3\\ f(x)=C\ast exp(\frac{1}{3}\ast x^3+x)\end{array}$$　
因为$x=0，f(0)=1$，故$C=1$，求得：$f(x)=exp(\frac{1}{3}\ast x^3+x)$

RK4方法求解

具体参考上面Step1–>Step5的实现，下面用Matlab实现RK4求解过程，并将两种方法求得的结果画成曲线，我们发现两条曲线是几乎重合的。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
%设一阶微分方程为：y(x)'=y(x)(x^2+1)
%初值：x=0,y(x)=1,y(x)'=1;
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear,close all;
dt=0.01;
x=0:dt:2;

%采用数学方法求解
y=exp(1/3*x.^3+x);
plot(x,y,'-o')

%采用龙格库塔法求解y(x)
h=dt;
N=size(x,2);

init_x = 0.0,init_y = 1;
rkx=[],rkx=[rkx,init_x];
rky=[],rky=[rky,init_y];

for i = 1:N-1
    k1=init_y*(init_x*init_x+1);
	
	temp_x = init_x + 0.5*h;
	temp_y = init_y + 0.5*h*k1;
	k2=temp_y*(temp_x*temp_x+1);
	
	temp_y = init_y + 0.5*h*k2;
	k3=temp_y*(temp_x*temp_x+1);
	
	temp_x = init_x + h;
	temp_y = init_y + h*k3;
	k4=temp_y*(temp_x*temp_x+1);
	
	init_y = init_y + h/6*(k1+2*k2+2*k3+k4);
	init_x = init_x + h;
	
	rkx=[rkx,init_x];
	rky=[rky,init_y];
end

hold on;
plot(rkx,rky,'-r')
legend('Math method','RK4 method')
set(gca, 'XLim',[0 3]);

       曲线如下：

图中蓝色线条为数学方法求得结果，红色曲线为RK4方法求得结果，两条曲线是重合的，证明了RK4方法是有效的。

应用2 求解陀螺仪位姿

       设有两个四元数:$a=[a_1,a_2,a_3,a_4],b=[b_1,b_2,b_3,b_4]$，写成复数形式：
$$a=a_1+a_{2}i+a_{3}j+a_{4}k,b=b_1+b_{2}i+b_{3}j+b_{4}k$$
定义运算：$i\ast j=k,j\ast k=i,k\ast i=j,j\ast i=-k,k\ast j=-i,i\ast k=-j$
　　　　　$i\ast i=j\ast j=k\ast k=-1$
则：
$a\ast b=(a_1+a_{2}i+a_{3}j+a_{4}k)\ast (b_1+b_{2}i+b_{3}j+b_{4}k)$
　　$=(a_1\ast b_1-a_2\ast b_2-a_3\ast b_3-a_4\ast b_4)+$
　　　$(a_1\ast b_2+a_2\ast b_1+a_3\ast b_4-a_4\ast b_3)i+$
　　　$(a_1\ast b_3-a_2\ast b_4+a_3\ast b_1+a_4\ast b_2)j+$
　　　$(a_1\ast b_4+a_2\ast b_3-a_3\ast b_2+a_4\ast b_1)k$
再写成矩阵形式:
$a\ast b=\left[\begin{array}{cccc}b1& -b_2& -b_3& -b_4\\ b_2& b_1& b_4& -b_3\\ b_3& -b_4& b_1& b_2\\ b_4& b_3& -b_2& b_1\end{array}\right]\left[\begin{array}{c}{a}_1\\ a_2\\ a_3\\ a_4\end{array}\right]$
       这里陀螺仪的位姿采用四元数表示，对于局部坐标系中一点$(x,y,z)$，假设开始时局部坐标系与世界坐标系重合，我们让局部坐标系整体在世界坐标系中绕旋转向量$(n_x,n_y,n_z)$旋转$\theta$度，求旋转后该点在世界坐标系中的坐标，该过程可用四元数表示。
　　定义四元数$q=[cos(\frac{\theta }{2}),sin(\frac{\theta }{2})(n_x,n_y,n_z)],p=[0,x,y,z]$,$p_o$为旋转后点的坐标，$q^{\prime }$为四元数$q$的导数，则：
　　$$p_o=qp{q}^{\prime }$$
　　对于运动的陀螺仪来说，位姿$q$是随时间变化的，这里定义成$q(t)$，$q(t)$表示$t$时刻陀螺仪坐标系与世界坐标系间的旋转关系，$q(t)=[q_1,q_2,q_3,q_4]$。
　　设$t$时刻陀螺仪的三轴角速度为$w_x,w_y,w_z$,写成四元数形式为$w_t=[0,w_x,w_y,w_z]$，则$t$时刻四元数的导数为：
　　$$q^{\prime }(t)=\frac{1}{2}q(t)\ast w_t=\frac{1}{2}\left[\begin{array}{cccc}0& -w_x& -w_y& -w_z\\ w_x& 0& w_z& -w_y\\ w_y& -w_z& 0& w_x\\ w_z& w_y& -w_x& 0\end{array}\right]\left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]=\frac{1}{2}\Omega \left[\begin{array}{c}{q}_1\\ q_2\\ q_3\\ q_4\end{array}\right]$$
　　关于四元数的导数推导，参考这里。
　　到这里我们有了位姿的一阶微分方程，而位姿的初始值$q(0)$我们可以根据实际情况给定，陀螺仪在每个时刻的三轴角速度都是已知的，故我们可以利用RK4进行求解：
　　设初始位姿$q(0)$已知，初始三轴角速度为$[w_{x}0,w_{y}0,w_{z}0]$，下一时刻三轴角速度为：$[w_{x}1,w_{y}1,w_{z}1]$，两时刻时间间隔为：$\Delta t$，且$\Delta x=w_{x}1-w_{x}0,\Delta y=w_{y}1-w_{y}0,\Delta z=w_{z}1-w_{z}0$。
　　Step1:计算$k_1$，设${\Omega }_0=\left[\begin{array}{cccc}0& -w_x& -w_y& -w_z\\ w_x& 0& w_z& -w_y\\ w_y& -w_z& 0& w_x\\ w_z& w_y& -w_x& 0\end{array}\right]$:因为$q(0)$已知，可求出：
　　$$k_1=\frac{1}{2}{\Omega }_0\ast q(0)\ast \Delta t$$
　　Step2:计算$k_2$，令${\Omega }_m=\left[\begin{array}{cccc}0& -(w_x+0.5\Delta x)& -(w_y+0.5\Delta y)& -(w_z+0.5\Delta z)\\ (w_x+0.5\Delta x)& 0& (w_z+0.5\Delta z)& -(w_y+0.5\Delta y)\\ (w_y+0.5\Delta y)& -(w_z+0.5\Delta z)& 0& (w_x+0.5\Delta x)\\ (w_z+0.5\Delta z)& (w_y+0.5\Delta y)& -(w_x+0.5\Delta x)& 0\end{array}\right]$，
$q_m=q(0)+0.5\ast k_1$，可求出：
　　$$k_2=\frac{1}{2}{\Omega }_m\ast q_m\ast \Delta t$$
　　Step3:计算$k_3$，令${\Omega }_m=\left[\begin{array}{cccc}0& -(w_x+0.5\Delta x)& -(w_y+0.5\Delta y)& -(w_z+0.5\Delta z)\\ (w_x+0.5\Delta x)& 0& (w_z+0.5\Delta z)& -(w_y+0.5\Delta y)\\ (w_y+0.5\Delta y)& -(w_z+0.5\Delta z)& 0& (w_x+0.5\Delta x)\\ (w_z+0.5\Delta z)& (w_y+0.5\Delta y)& -(w_x+0.5\Delta x)& 0\end{array}\right]$，
$q_m=q(0)+0.5\ast k_2$，可求出：
　　$$k_3=\frac{1}{2}{\Omega }_m\ast q_m\ast \Delta t$$
　　Step4:计算$k_4$，令${\Omega }_m=\left[\begin{array}{cccc}0& -(w_x+\Delta x)& -(w_y+\Delta y)& -(w_z+\Delta z)\\ (w_x+\Delta x)& 0& (w_z+\Delta z)& -(w_y+\Delta y)\\ (w_y+\Delta y)& -(w_z+\Delta z)& 0& (w_x+\Delta x)\\ (w_z+\Delta z)& (w_y+\Delta y)& -(w_x+\Delta x)& 0\end{array}\right]$，
$q_m=q(0)+k_3$，可求出：
　　$$k_4=\frac{1}{2}{\Omega }_m\ast q_m\ast \Delta t$$
　　Step5:对上面的$k_1,k_2,k_3,k_4$进行加权平均，得到位姿的增量，再求$q(1)$：
　　$$q(1)=q(0)+\frac{1}{6}(k_1+2\ast k_2+2\ast k_3+k_4)$$

不断重复$Step1-Step5$，可求出陀螺仪在每一时刻的位姿。