最近做出来的Arduino姿态解算龙格库塔

westloveohyeah · 发表于 2014-12-3 16:25:29

研究导航算法有几个月了，最近做了做实体仿真，感觉龙格库塔积分器的精度还是可以，解算四元数微分方程的效率还可以。

在处理龙格库塔积分的时候，有个东西值得注意，就是角速度在半步长时期的取值是有讨论的。在传感器采样率不高以及处理器计算速率不高的情况下，半步长角速度值可以取线性插值的结果。如果高的话就要选择两次采样合并计算的方法。

不多说，上程序：

void RungeKutta(double Q0_m_Quaternion1,double Q0_m_Quaternion2,double Q0_m_Quaternion3
,double Q0_m_Quaternion4,double W0_GyroX,double W0_GyroY,double W0_GyroZ,
double W01_GyroX,double W01_GyroY,double W01_GyroZ,
double W1_GyroX,double W1_GyroY,double W1_GyroZ)
{
      double T=0.00013;


double Q1_m_Quaternion1;
      double Q1_m_Quaternion2;
      double Q1_m_Quaternion3;
      double Q1_m_Quaternion4;
Q1_m_Quaternion1=1,Q1_m_Quaternion2=0,Q1_m_Quaternion3=0,Q1_m_Quaternion4=0;
      double q1=Q0_m_Quaternion1;
      double q2=Q0_m_Quaternion2;
      double q3=Q0_m_Quaternion3;
      double q4=Q0_m_Quaternion4;
double K10=0.5*(-W0_GyroX*Q0_m_Quaternion2-W0_GyroY*Q0_m_Quaternion3-W0_GyroZ*Q0_m_Quaternion4);
      //Serial.println(K10,10);
double K11=0.5*(W0_GyroX*Q0_m_Quaternion1+W0_GyroZ*Q0_m_Quaternion3-W0_GyroY*Q0_m_Quaternion4);
double K12=0.5*(W0_GyroY*Q0_m_Quaternion1-W0_GyroZ*Q0_m_Quaternion2+W0_GyroX*Q0_m_Quaternion4);
double K13=0.5*(W0_GyroZ*Q0_m_Quaternion1+W0_GyroY*Q0_m_Quaternion2-W0_GyroX*Q0_m_Quaternion3);
double a0=q1+K10/2.0*T;

      double a1=q2+K11/2.0*T;
      double a2=q3+K12/2.0*T;
      double a3=q4+K13/2.0*T;

double K20=0.5*(-W01_GyroX*a1-W01_GyroY*a2-W01_GyroZ*a3);
double K21=0.5*(W01_GyroX*a0+W01_GyroZ*a2-W01_GyroY*a3);
double K22=0.5*(W01_GyroY*a0-W01_GyroZ*a1+W01_GyroX*a3);
double K23=0.5*(W01_GyroZ*a0+W01_GyroY*a1-W01_GyroX*a2);
double b0=Q0_m_Quaternion1+K20/2.0*T;
      double b1=Q0_m_Quaternion2+K21/2.0*T;
      double b2=Q0_m_Quaternion3+K22/2.0*T;
      double b3=Q0_m_Quaternion4+K23/2.0*T;

double K30=0.5*(-W01_GyroX*b1-W01_GyroY*b2-W01_GyroZ*b3);
double K31=0.5*(W01_GyroX*b0+W01_GyroZ*b2-W01_GyroY*b3);
double K32=0.5*(W01_GyroY*b0-W01_GyroZ*b1+W01_GyroX*b3);
double K33=0.5*(W01_GyroZ*b0+W01_GyroY*b1-W01_GyroX*b2);
double c0=Q0_m_Quaternion1+K30*T;
      double c1=Q0_m_Quaternion2+K31*T;
      double c2=Q0_m_Quaternion3+K32*T;
      double c3=Q0_m_Quaternion4+K33*T;

double K40=0.5*(-W1_GyroX*c1-W1_GyroY*c2-W1_GyroZ*c3);
double K41=0.5*(W1_GyroX*c0+W1_GyroZ*c2-W1_GyroY*c3);
double K42=0.5*(W1_GyroY*c0-W1_GyroZ*c1+W1_GyroX*c3);
double K43=0.5*(W1_GyroZ*c0+W1_GyroY*c1-W1_GyroX*c2);

Q1_m_Quaternion1=Q0_m_Quaternion1+T/6.0*(K10+2.0*K20+2.0*K30+K40);
Q1_m_Quaternion2=Q0_m_Quaternion2+T/6.0*(K11+2.0*K21+2.0*K31+K41);
Q1_m_Quaternion3=Q0_m_Quaternion3+T/6.0*(K12+2.0*K22+2.0*K32+K42);
Q1_m_Quaternion4=Q0_m_Quaternion4+T/6.0*(K13+2.0*K23+2.0*K33+K43);
//%Q1
double q=sqrt(pow(Q1_m_Quaternion1,2.0)+pow(Q1_m_Quaternion2,2.0)+pow(Q1_m_Quaternion3,2.0)+pow(Q1_m_Quaternion4,2.0          ));
      //Serial.println(q);
//%q
Q_m_Quaternion1=Q1_m_Quaternion1/q;
Q_m_Quaternion2=Q1_m_Quaternion2/q;
Q_m_Quaternion3=Q1_m_Quaternion3/q;
Q_m_Quaternion4=Q1_m_Quaternion4/q;
      //Serial.println(Q_m_Quaternion1);
//return Q1;

      return;
}

liangquan · 发表于 2014-12-3 16:41:24

zhe me niu

leicheng · 发表于 2014-12-3 22:50:25

最大的问题是写得太费内存了！再加个其他的功能，很容易溢出。代码也应该有适当的注释。
在T/2时，一般采用定时器，要善于利用时隙去计算。根据经验，利用I2C方式（400kHz）读陀螺仪耗时不到0.4ms（包括换算成角速度），RK4在16MHz的运算速度下，耗时不超过4ms。
好好改进！

westloveohyeah · 发表于 2014-12-5 18:56:54

leicheng 发表于 2014-12-3 22:50
最大的问题是写得太费内存了！再加个其他的功能，很容易溢出。代码也应该有适当的注释。
在T/2时，一般采用 ...

你是研究生吧，我大一的

leicheng · 发表于 2014-12-6 10:55:15

westloveohyeah 发表于 2014-12-5 18:56
你是研究生吧，我大一的

还以为你是某高校国家重点实验室的。貌似大一不给分实验室。

westloveohyeah · 发表于 2014-12-6 13:59:42

leicheng 发表于 2014-12-6 10:55
还以为你是某高校国家重点实验室的。貌似大一不给分实验室。

哈哈，虽然大一，但现在在UESTC 机器人研究所帮老板做项目

帐号		自动登录	找回密码
密码			注册