广州网站建设快速排名,宁波seo高级方法,网站建设前十名,建设网站主机免费版1、四个概念#xff1a;“地理”坐标系、“机体”坐标系、他们之间换算公式、换算公式用的系数。地理坐标系#xff1a;东、北、天#xff0c;以下简称地理。在这个坐标系里有重力永远是#xff08;0,0,1g#xff09;#xff0c;地磁永远是#xff08;0,1,x#xff09;…1、四个概念“地理”坐标系、“机体”坐标系、他们之间换算公式、换算公式用的系数。地理坐标系东、北、天以下简称地理。在这个坐标系里有重力永远是0,0,1g地磁永远是0,1,x地磁的垂直不关心两个三维向量。机体坐标系以下简称机体上面有陀螺、加计、电子罗盘传感器三个三维向量。换算公式以下简称公式公式就是描述机体姿态的表达方法一般都是用以地理为基准从地理换算到机体的公式有四元数、欧拉角、方向余弦矩阵。换算公式的系数以下简称系数四元数的q0123、欧拉角的ROLL/PITCH/YAW、余弦矩阵的9个数。系数就是描述机体姿态的表达方法的具体数值。姿态其实就是公式系数的组合一般经常用人容易理解的公式“欧拉角”表示系数就是横滚xx度俯仰xx度航向xx度。2、五个数据源重力、地磁、陀螺、加计、电子罗盘前两个来自地理后三个来自机体。3、陀螺向量基于机体也在机体上积分因为地理上无参考数据源所以很独立直接在公式的老系数上积分得到新系数。狭义上的捷联惯导算法就是指这个陀螺积分公式也分为欧拉角、方向余弦矩阵、四元数他们的积分算法有增量法、数值积分法X阶龙格-库塔等等4、加计向量、重力向量加计基于机体重力基于地理重力向量0,0,1g用公式换算到机体与机体的加计向量算出误差。理论上应该没有误差这误差逆向思维一下其实就是换算公式的系数误差。所以这误差可用于纠正公式的系数横滚、俯仰也就是姿态。5、电子罗盘向量、地磁向量同上只不过要砍掉地理上的垂直向量因为无用。只留下地理水平面上的向量。误差可以用来纠正公式的系数航向。6、就这样系数不停地被陀螺积分更新也不停地被误差修正它和公式所代表的姿态也在不断更新。如果积分和修正用四元数算法因为运算量较少、无奇点误差最后用欧拉角输出控制PID因为角度比较直观那就需要有个四元数系数到欧拉角系数的转换。常用的三种公式它们之间都有转换算法。 // // IMU.c // S.O.H. Madgwick // 25th September 2010 // // Description: // // Quaternion implementation of the DCM filter [Mayhony et al]. // // User must define halfT as the (sample period / 2), and the filter gains Kp and Ki. // // Global variables q0, q1, q2, q3 are the quaternion elements representing the estimated // orientation. See my report for an overview of the use of quaternions in this application. // // User must call IMUupdate() every sample period and parse calibrated gyroscope (gx, gy, gz) // and accelerometer (ax, ay, ay) data. Gyroscope units are radians/second, accelerometer // units are irrelevant as the vector is normalised. // //
//---------------------------------------------------------------------------------------------------- // Header files
#include IMU.h #include math.h
//---------------------------------------------------------------------------------------------------- // Definitions
#define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer #define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases #define halfT 0.5f // half the sample period
//--------------------------------------------------------------------------------------------------- // Variable definitions
float q0 1, q1 0, q2 0, q3 0; // quaternion elements representing the estimated orientation float exInt 0, eyInt 0, ezInt 0; // scaled integral error
// // Function //
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { float norm; float vx, vy, vz; float ex, ey, ez; // normalise the measurements norm sqrt(ax*ax ay*ay az*az); ax ax / norm; ay ay / norm; az az / norm; 把加计的三维向量转成单位向量。 // estimated direction of gravity vx 2*(q1*q3 - q0*q2); vy 2*(q0*q1 q2*q3); vz q0*q0 - q1*q1 - q2*q2 q3*q3;
这是把四元数换算成《方向余弦矩阵》中的第三列的三个元素。 根据余弦矩阵和欧拉角的定义地理坐标系的重力向量转到机体坐标系正好是这三个元素。 所以这里的vx\y\z其实就是当前的欧拉角即四元数的机体坐标参照系上换算出来的重力单位向量。 // error is sum of cross product between reference direction of field and direction measured by sensor ex (ay*vz - az*vy); ey (az*vx - ax*vz); ez (ax*vy - ay*vx);
axyz是机体坐标参照系上加速度计测出来的重力向量也就是实际测出来的重力向量。 axyz是测量得到的重力向量vxyz是陀螺积分后的姿态来推算出的重力向量它们都是机体坐标参照系上的重力向量。 那它们之间的误差向量就是陀螺积分后的姿态和加计测出来的姿态之间的误差。 向量间的误差可以用向量叉积也叫向量外积、叉乘来表示exyz就是两个重力向量的叉积。 这个叉积向量仍旧是位于机体坐标系上的而陀螺积分误差也是在机体坐标系而且叉积的大小与陀螺积分误差成正比正好拿来纠正陀螺。你可以自己拿东西想象一下由于陀螺是对机体直接积分所以对陀螺的纠正量会直接体现在对机体坐标系的纠正。 // integral error scaled integral gain exInt exInt ex*Ki; eyInt eyInt ey*Ki; ezInt ezInt ez*Ki; // adjusted gyroscope measurements gx gx Kp*ex exInt; gy gy Kp*ey eyInt; gz gz Kp*ez ezInt;
用叉积误差来做PI修正陀螺零偏 // integrate quaternion rate and normalise q0 q0 (-q1*gx - q2*gy - q3*gz)*halfT; q1 q1 (q0*gx q2*gz - q3*gy)*halfT; q2 q2 (q0*gy - q1*gz q3*gx)*halfT; q3 q3 (q0*gz q1*gy - q2*gx)*halfT; 四元数微分方程 // normalise quaternion norm sqrt(q0*q0 q1*q1 q2*q2 q3*q3); q0 q0 / norm; q1 q1 / norm; q2 q2 / norm; q3 q3 / norm; 四元数规范化 }
// // END OF CODE // // // AHRS.c // S.O.H. Madgwick // 25th August 2010 // // Description: // // Quaternion implementation of the DCM filter [Mayhony et al]. Incorporates the magnetic distortion // compensation algorithms from my filter [Madgwick] which eliminates the need for a reference // direction of flux (bx bz) to be predefined and limits the effect of magnetic distortions to yaw // axis only. // // User must define halfT as the (sample period / 2), and the filter gains Kp and Ki. // // Global variables q0, q1, q2, q3 are the quaternion elements representing the estimated // orientation. See my report for an overview of the use of quaternions in this application. // // User must call AHRSupdate() every sample period and parse calibrated gyroscope (gx, gy, gz), // accelerometer (ax, ay, ay) and magnetometer (mx, my, mz) data. Gyroscope units are // radians/second, accelerometer and magnetometer units are irrelevant as the vector is normalised. // //
//---------------------------------------------------------------------------------------------------- // Header files
#include AHRS.h #include math.h
//---------------------------------------------------------------------------------------------------- // Definitions
#define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer #define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases #define halfT 0.5f // half the sample period
//--------------------------------------------------------------------------------------------------- // Variable definitions
float q0 1, q1 0, q2 0, q3 0; // quaternion elements representing the estimated orientation float exInt 0, eyInt 0, ezInt 0; // scaled integral error
// // Function //
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) { float norm; float hx, hy, hz, bx, bz; float vx, vy, vz, wx, wy, wz; float ex, ey, ez; // auxiliary variables to reduce number of repeated operations float q0q0 q0*q0; float q0q1 q0*q1; float q0q2 q0*q2; float q0q3 q0*q3; float q1q1 q1*q1; float q1q2 q1*q2; float q1q3 q1*q3; float q2q2 q2*q2; float q2q3 q2*q3; float q3q3 q3*q3; // normalise the measurements norm sqrt(ax*ax ay*ay az*az); ax ax / norm; ay ay / norm; az az / norm; norm sqrt(mx*mx my*my mz*mz); mx mx / norm; my my / norm; mz mz / norm; 从机体坐标系的电子罗盘测到的矢量转成地理坐标系下的磁场矢量hxyz测量值 // compute reference direction of flux hx 2*mx*(0.5 - q2q2 - q3q3) 2*my*(q1q2 - q0q3) 2*mz*(q1q3 q0q2); hy 2*mx*(q1q2 q0q3) 2*my*(0.5 - q1q1 - q3q3) 2*mz*(q2q3 - q0q1); hz 2*mx*(q1q3 - q0q2) 2*my*(q2q3 q0q1) 2*mz*(0.5 - q1q1 - q2q2);
计算地理坐标系下的磁场矢量bxyz参考值。 因为地理地磁水平夹角我们已知是0度抛去磁偏角的因素固定向北所以by0bx某值 但地理参考地磁矢量在垂直面上也有分量bz地球上每个地方都是不一样的。 我们无法得知也就无法用来融合有更适合做垂直方向修正融合的加速度计所以直接从测量值hz上复制过来bzhz。 磁场水平分量参考值和测量值的大小应该是一致的(bx*bx) (by*by)) ((hx*hx) (hy*hy))。 因为by0所以就简化成(bx*bx) ((hx*hx) (hy*hy))。可算出bx。 bx sqrt((hx*hx) (hy*hy)); bz hz; // estimated direction of gravity and flux (v and w) vx 2*(q1q3 - q0q2); vy 2*(q0q1 q2q3); vz q0q0 - q1q1 - q2q2 q3q3;
我们把地理坐标系上的磁场矢量bxyz转到机体上来wxyz。 因为by0所以所有涉及到by的部分都被省略了。 类似上面重力vxyz的推算因为重力g的gz1gxgy0所以上面涉及到gxgy的部分也被省略了 你可以看看两个公式wxyz的公式把bx换成gx0把bz换成gz1就变成了vxyz的公式了其中q0q0q1q1q2q2q3q31。 wx 2*bx*(0.5 - q2q2 - q3q3) 2*bz*(q1q3 - q0q2); wy 2*bx*(q1q2 - q0q3) 2*bz*(q0q1 q2q3); wz 2*bx*(q0q2 q1q3) 2*bz*(0.5 - q1q1 - q2q2); 现在把加速度的测量矢量和参考矢量做叉积把磁场的测量矢量和参考矢量也做叉积。都拿来来修正陀螺。 // error is sum of cross product between reference direction of fields and direction measured by sensors ex (ay*vz - az*vy) (my*wz - mz*wy); ey (az*vx - ax*vz) (mz*wx - mx*wz); ez (ax*vy - ay*vx) (mx*wy - my*wx); // integral error scaled integral gain exInt exInt ex*Ki; eyInt eyInt ey*Ki; ezInt ezInt ez*Ki; // adjusted gyroscope measurements gx gx Kp*ex exInt; gy gy Kp*ey eyInt; gz gz Kp*ez ezInt; // integrate quaternion rate and normalise q0 q0 (-q1*gx - q2*gy - q3*gz)*halfT; q1 q1 (q0*gx q2*gz - q3*gy)*halfT; q2 q2 (q0*gy - q1*gz q3*gx)*halfT; q3 q3 (q0*gz q1*gy - q2*gx)*halfT; // normalise quaternion norm sqrt(q0*q0 q1*q1 q2*q2 q3*q3); q0 q0 / norm; q1 q1 / norm; q2 q2 / norm; q3 q3 / norm; }
// // END OF CODE // 文章转载自： http://www.morning.rdkt.cn.gov.cn.rdkt.cn http://www.morning.wjplm.cn.gov.cn.wjplm.cn http://www.morning.rqlf.cn.gov.cn.rqlf.cn http://www.morning.mpyry.cn.gov.cn.mpyry.cn http://www.morning.trqhd.cn.gov.cn.trqhd.cn http://www.morning.mswkd.cn.gov.cn.mswkd.cn http://www.morning.zljqb.cn.gov.cn.zljqb.cn http://www.morning.kjkml.cn.gov.cn.kjkml.cn http://www.morning.bncrx.cn.gov.cn.bncrx.cn http://www.morning.gnfkl.cn.gov.cn.gnfkl.cn http://www.morning.pymff.cn.gov.cn.pymff.cn http://www.morning.fgkrh.cn.gov.cn.fgkrh.cn http://www.morning.bhwll.cn.gov.cn.bhwll.cn http://www.morning.xbrxk.cn.gov.cn.xbrxk.cn http://www.morning.krfpj.cn.gov.cn.krfpj.cn http://www.morning.bqmdl.cn.gov.cn.bqmdl.cn http://www.morning.yksf.cn.gov.cn.yksf.cn http://www.morning.cmzgt.cn.gov.cn.cmzgt.cn http://www.morning.glxdk.cn.gov.cn.glxdk.cn http://www.morning.ggxbyhk.cn.gov.cn.ggxbyhk.cn http://www.morning.llsrg.cn.gov.cn.llsrg.cn http://www.morning.rbknf.cn.gov.cn.rbknf.cn http://www.morning.lqklf.cn.gov.cn.lqklf.cn http://www.morning.bwzzt.cn.gov.cn.bwzzt.cn http://www.morning.jbctp.cn.gov.cn.jbctp.cn http://www.morning.trmpj.cn.gov.cn.trmpj.cn http://www.morning.rdkqt.cn.gov.cn.rdkqt.cn http://www.morning.xqknl.cn.gov.cn.xqknl.cn http://www.morning.tyjp.cn.gov.cn.tyjp.cn http://www.morning.rwmp.cn.gov.cn.rwmp.cn http://www.morning.gnkbf.cn.gov.cn.gnkbf.cn http://www.morning.tlpsd.cn.gov.cn.tlpsd.cn http://www.morning.zgdnd.cn.gov.cn.zgdnd.cn http://www.morning.xkhhy.cn.gov.cn.xkhhy.cn http://www.morning.qllcp.cn.gov.cn.qllcp.cn http://www.morning.clkjn.cn.gov.cn.clkjn.cn http://www.morning.kcsx.cn.gov.cn.kcsx.cn http://www.morning.bdfph.cn.gov.cn.bdfph.cn http://www.morning.fyskq.cn.gov.cn.fyskq.cn http://www.morning.heleyo.com.gov.cn.heleyo.com http://www.morning.kmjbs.cn.gov.cn.kmjbs.cn http://www.morning.fgwzl.cn.gov.cn.fgwzl.cn http://www.morning.bssjp.cn.gov.cn.bssjp.cn http://www.morning.rntyn.cn.gov.cn.rntyn.cn http://www.morning.ljllt.cn.gov.cn.ljllt.cn http://www.morning.dschz.cn.gov.cn.dschz.cn http://www.morning.rzjfn.cn.gov.cn.rzjfn.cn http://www.morning.nqrlz.cn.gov.cn.nqrlz.cn http://www.morning.grcfn.cn.gov.cn.grcfn.cn http://www.morning.hcqpc.cn.gov.cn.hcqpc.cn http://www.morning.pltbd.cn.gov.cn.pltbd.cn http://www.morning.trmpj.cn.gov.cn.trmpj.cn http://www.morning.xppj.cn.gov.cn.xppj.cn http://www.morning.bnwlh.cn.gov.cn.bnwlh.cn http://www.morning.qqbw.cn.gov.cn.qqbw.cn http://www.morning.rdymd.cn.gov.cn.rdymd.cn http://www.morning.deupp.com.gov.cn.deupp.com http://www.morning.yjmlg.cn.gov.cn.yjmlg.cn http://www.morning.fcwb.cn.gov.cn.fcwb.cn http://www.morning.glwyn.cn.gov.cn.glwyn.cn http://www.morning.tfkqc.cn.gov.cn.tfkqc.cn http://www.morning.ckxd.cn.gov.cn.ckxd.cn http://www.morning.czzpm.cn.gov.cn.czzpm.cn http://www.morning.hwycs.cn.gov.cn.hwycs.cn http://www.morning.nlnmy.cn.gov.cn.nlnmy.cn http://www.morning.nydtt.cn.gov.cn.nydtt.cn http://www.morning.lczxm.cn.gov.cn.lczxm.cn http://www.morning.kycxb.cn.gov.cn.kycxb.cn http://www.morning.rmyqj.cn.gov.cn.rmyqj.cn http://www.morning.qxmys.cn.gov.cn.qxmys.cn http://www.morning.mzcrs.cn.gov.cn.mzcrs.cn http://www.morning.mbpzw.cn.gov.cn.mbpzw.cn http://www.morning.lhsdf.cn.gov.cn.lhsdf.cn http://www.morning.kmcby.cn.gov.cn.kmcby.cn http://www.morning.crhd.cn.gov.cn.crhd.cn http://www.morning.zmwd.cn.gov.cn.zmwd.cn http://www.morning.ydrml.cn.gov.cn.ydrml.cn http://www.morning.hxrfb.cn.gov.cn.hxrfb.cn http://www.morning.ffbp.cn.gov.cn.ffbp.cn http://www.morning.bhgnj.cn.gov.cn.bhgnj.cn
