导:根据我前面的视频又说过这个流程:姿态角来自四元素,四元素取决于陀螺仪,陀螺仪存在温度漂移、零点漂移和时间积分偏差,因此使用加计(理想与实际的err值)来pi校准陀螺;html
如今,磁力计对航向角yaw的校准,就是一样的道理:使用磁力计(理想与实际的err)来校准陀螺;
app
故:如今的err构成为: 如今重点分析 的计算过程;spa
1.既然磁力计的校准与加速度计的校准具备类似性,同时,此过程就是姿态结算的又一次呈现,朋友们能够先看个人视频总结;再看此文豁然开朗,印象更加深入;下发链接以下:.net
https://blog.csdn.net/xiaoxilang/article/details/80326013 博客文章结尾有链接!
code
2.分析err的以下图:orm
3.代码分析:请您细看1-9点标注之处,是代码关键点,对照我以前的视频,可相互印证!视频
void AHRSupdate(unsigned char ahrs_flag) { float norm,Spin_rate,P_gain,W_yaw_P[3]; float change_limit,W_P[3]; float q0,q1,q2,q3; float q0q1,q0q2,q0q3,q1q1,q1q2,q1q3,q2q2,q2q3,q3q3; float mag_nx,mag_ny,mag_nz,mag_bx,mag_by,true_my,true_mz; float f_bx,f_by,f_bz; float err1_x,err1_y,err1_z,err2_x,err2_y,err2_z; float gx,gy,gz,ax,ay,az,mx,my,mz; //1.9轴原始数据得到----------------------------- gx = alldata.ina_gyro1; //right gy = alldata.ina_gyro2; //front gz = alldata.ina_gyro3; //up mx = alldata.ina_mag1; //right my = alldata.ina_mag2; //front mz = alldata.ina_mag3; //up ax = alldata.ina_acc1;//x_acc_filter;//outPut_buf[0][6]; // right ay = alldata.ina_acc2;//outPut_buf[1][6]; // front az = alldata.ina_acc3;//outPut_buf[2][6]; // up //2.计算p参数:gyro_drift_limit = 0.01f*rad,Spin_rate_limit1 = 10.0f*rad,Spin_rate_limit2 = 50.0f*rad //计算P_gain Spin_rate = __sqrtf(gx*gx + gy*gy + gz*gz); if (Spin_rate < Spin_rate_limit1) P_gain = 1.0f; if (Spin_rate > Spin_rate_limit2) P_gain = 5.0f; if ((Spin_rate >= Spin_rate_limit1)&&(Spin_rate <= Spin_rate_limit2)) P_gain = Spin_rate/(Spin_rate_limit1); //3.由于陀螺须要加计个磁力计校准:因此先归一化加计和磁力计三轴数据 // normalise the measurements norm = __sqrtf(ax*ax + ay*ay + az*az); ax = ax / norm; ay = ay / norm; az = az / norm; norm = __sqrtf(mx*mx + my*my + mz*mz); mx = mx / norm; my = my / norm; mz = mz / norm; q0 = ahrs_q[0]; q1 = ahrs_q[1]; q2 = ahrs_q[2]; q3 = ahrs_q[3]; q0q1 = q0*q1; q0q2 = q0*q2; q0q3 = q0*q3; q1q1 = q1*q1; q1q2 = q1*q2; q1q3 = q1*q3; q2q2 = q2*q2; q2q3 = q2*q3; q3q3 = q3*q3; //4.准备数据以求磁力计的实际和理想 mag_nx = 2.0f*mx*(0.5f - q2q2 - q3q3) + 2.0f*my*(q1q2 - q0q3) + 2.0f*mz*(q1q3 + q0q2); mag_ny = 2.f*mx*(q1q2 + q0q3) + 2.0f*my*(0.5f - q1q1 - q3q3) + 2.0f*mz*(q2q3 - q0q1); mag_nz = 2.0f*mx*(q1q3 - q0q2) + 2.0f*my*(q2q3 + q0q1) + 2.0f*mz*(0.5f - q1q1 - q2q2); true_my = __sqrtf(mag_nx*mag_nx+mag_ny*mag_ny); true_mz = mag_nz; mag_bx = 2.0f*true_my*(q1q2+q0q3)+2.0f*true_mz*(q1q3-q0q2); mag_by = 2.0f*true_my*(0.5f-q1q1-q3q3)+2.0f*true_mz*(q2q3+q0q1); f_bx = -2.0f*(q1q3-q0q2); f_by = -2.0f*(q2q3+q0q1); f_bz = -2.0f*(0.5f-q1q1-q2q2); //5.加计偏差:利用叉乘计算重力加速度参考向量通过Tbn矩阵转换到载体系后和实测的载体系加速度之间的偏差 err1_x = ay*f_bz - az*f_by; err1_y = az*f_bx - ax*f_bz; err1_z = ax*f_by - ay*f_bx;
err2_x = 0;//6.磁力计偏差:利用差乘...注意:只校航向!!! err2_y = 0; err2_z = mx*mag_by - my*mag_bx; W_yaw_P[0] = err2_x*P_gain*Kp_yaw;//7.磁力计的pi校准 W_yaw_P[1] = err2_y*P_gain*Kp_yaw; W_yaw_P[2] = err2_z*P_gain*Kp_yaw; if (Spin_rate<Spin_rate_limit1) { W_sum_I[0] = W_sum_I[0]+err2_x*Ki_yaw+err1_x*Ki_acc; W_sum_I[1] = W_sum_I[1]+err2_y*Ki_yaw+err1_y*Ki_acc; W_sum_I[2] = W_sum_I[2]+err2_z*Ki_yaw+err1_z*Ki_acc; W_sum_I_time++; } if (W_sum_I_time>=50) { change_limit = gyro_drift_limit*W_sum_I_time*T; W_sum_I[0] = Math_fConstrain(W_sum_I[0],-change_limit,change_limit); W_sum_I[1] = Math_fConstrain(W_sum_I[1],-change_limit,change_limit); W_sum_I[2] = Math_fConstrain(W_sum_I[2],-change_limit,change_limit); W_I[0] = W_I[0]+W_sum_I[0]; W_I[1] = W_I[1]+W_sum_I[1]; W_I[2] = W_I[2]+W_sum_I[2]; W_sum_I[0] = 0; W_sum_I[1] = 0; W_sum_I[2] = 0; W_sum_I_time = 0; } W_P[0] = err1_x*P_gain*Kp_acc;//8.加计的pi校准 W_P[1] = err1_y*P_gain*Kp_acc; W_P[2] = err1_z*P_gain*Kp_acc; gx = gx+W_I[0]+W_P[0]+W_yaw_P[0];//9.陀螺的最终校准结果 gy = gy+W_I[1]+W_P[1]+W_yaw_P[1]; gz = gz+W_I[2]+W_P[2]+W_yaw_P[2]; ahrs_qLonge_Kuta(gx, gy, gz);//10.由陀螺计算四元素 norm_ahrsq(); //11.四元素归一化,为下一次过程准备数据!!! //12.由四元素到姿态角 ,请见视频说明!!!! }
4.参考技术文档htm
陀螺仪、加速度计 、磁力计的介绍
http://www.360doc.com/content/13/1217/19/235269_337950743.shtml
磁力计工做原理介绍
blog
http://blog.sina.com.cn/s/blog_402c071e0102v8ig.htmlv8
四旋翼姿态解算——互补滤波法补充(融合磁力计)
https://blog.csdn.net/hongbin_xu/article/details/59110226