|
|
发表于 2012-9-5 18:41:37
|
显示全部楼层
本帖最后由 wanxuncpx 于 2012-9-5 18:43 编辑
按楼主的思路和公式行不通啊
假如陀螺仪1s钟内,yaw偏航角偏航了10度,pitch俯仰角偏了10度,roll偏了0度, 将这个角度转为弧度,然后按400Hz陀螺仪速度切割为400份后构成的小三角近似的四元数qn,
然后用qn去叉乘更新四元数quaternion 400次,照说得到的欧拉角应为10° 10° 0°啊可我计算的结果roll不为0啊
求帮助,求分析!
代码如下:
/* 输入欧拉角，能看到四元数，以及再转换回去成欧拉角
Yaw范围（-180~180）
Pitch范围（-90~90）
Roll范围（-180~180）
*/
#include "stdio.h"
#include "math.h"
#include "conio.h"
/*---------------------*
* A- 数据类型定义 *
*----------------------*/
typedef struct
{
float gx;
float gy;
float gz;
}tg_L3G4200D_TYPE;
typedef struct{
float w;
float x;
float y;
float z;
}tg_Quaternion;
typedef struct
{
float yaw;
float pitch;
float roll;
}tg_AHRS_TYPE;
tg_Quaternion quaternion={1,0,0,0};
tg_AHRS_TYPE ahrs={0,0,0};
tg_L3G4200D_TYPE l3g4200={0,0,0};
#define PI 3.141592654
#define halfT 0.5f
/******************************************************************************
/ 函数功能:快速陀螺仪四元数更新
/ 修改日期:none
/ 输入参数:none
/ 输出参数:none
/ 使用说明:none
******************************************************************************/
void FastFormat(tg_Quaternion * pSrc,tg_Quaternion * pDst)
{
tg_Quaternion dQ,Qn,Qdst;
float sq;
//用小三角近似法快速得到微旋转的四元数,省去了大量的三角函数变换
dQ.w = pSrc->w;
dQ.x = pSrc->x;
dQ.y = pSrc->y;
dQ.z = pSrc->z;
//提取目标四元数
Qdst.w = pDst->w;
Qdst.x = pDst->x;
Qdst.y = pDst->y;
Qdst.z = pDst->z;
//四元数乘法
Qn.w = (Qdst.w * dQ.w) - (Qdst.x * dQ.x) - (Qdst.y * dQ.y) - (Qdst.z* dQ.z);
Qn.x = (Qdst.w * dQ.x) + (Qdst.x * dQ.w) + (Qdst.y * dQ.z) - (Qdst.z* dQ.y);
Qn.y = (Qdst.w * dQ.y) - (Qdst.x * dQ.z) + (Qdst.y * dQ.w) + (Qdst.z* dQ.x);
Qn.z = (Qdst.w * dQ.z) + (Qdst.x * dQ.y) - (Qdst.y * dQ.x) + (Qdst.z* dQ.w);
//单位化,并返回四元数
sq = Qn.w * Qn.w + Qn.x*Qn.x + Qn.y*Qn.y + Qn.z*Qn.z;
sq = sqrt(sq);
pDst->w = Qn.w / sq;
pDst->x = Qn.x / sq;
pDst->y = Qn.y / sq;
pDst->z = Qn.z / sq;
}
/******************************************************************************
/ 函数功能:欧拉角转四元数
/ 修改日期:none
/ 输入参数:none
/ 输出参数:none
/ 使用说明:none
******************************************************************************/
tg_Quaternion FromEulerAngle(tg_L3G4200D_TYPE *pIn)
{
tg_Quaternion Qn;
float sq;
//用小三角近似法快速得到微旋转的四元数,省去了海量的三角函数变换
Qn.w = 1;
Qn.x = ((float)pIn->gx)*(0.07*0.5*0.0025);
Qn.y = ((float)pIn->gy)*(0.07*0.5*0.0025);
Qn.z = ((float)pIn->gz)*(0.07*0.5*0.0025);
return Qn;
//*/
}
/******************************************************************************
/ 函数功能:将四元数转换为欧拉角
/ 修改日期:none
/ 输入参数:
/ 输出参数:
/ 使用说明:none
******************************************************************************/
void ToEulerAngle(tg_Quaternion * pIn,tg_AHRS_TYPE * pOut)
{
tg_Quaternion Qn;
float a,b,c,d,e,sq;
float T[3][3],Q[4];
/* 获取四元数数据 */
Qn.w = pIn->w;
Qn.x = pIn->x;
Qn.y = pIn->y;
Qn.z = pIn->z;
/* 计算中间量 */
///*
a = 2*(Qn.w * Qn.z + Qn.x * Qn.y);
b = 1 - 2 * ( Qn.y * Qn.y + Qn.z * Qn.z);
c = 2 * (Qn.w * Qn.y - Qn.z *Qn.x );
d = 2 * (Qn.w * Qn.x + Qn.y * Qn.z );
e = 1 - 2 * (Qn.x * Qn.x + Qn.y * Qn.y );
// 计算欧拉角
pOut->yaw = (float)(atan2(a,b)*(180.0/PI)); // yaw: z轴,偏航角
pOut->pitch = (float)(asin(c)* (180.0/PI)); // pitch:y轴,俯仰角
pOut->roll = (float)(atan2(d,e)*(180.0/PI)); // roll: x轴,滚动角
}
void main(void)
{
int i;
tg_Quaternion qn;
do{
/* 模拟陀螺仪的输出欧拉角(积分一秒) */
printf("\nYaw = ");
scanf("%f",&l3g4200.gz);
printf("\nPitch = ");
scanf("%f",&l3g4200.gy);
printf("\nRoll = ");
scanf("%f",&l3g4200.gx);
l3g4200.gz *= (PI/180f)/0.07f;
l3g4200.gy *= (PI/180f)/0.07f;
l3g4200.gx *= (PI/180f)/0.07f;
qn = FromEulerAngle(&l3g4200);
printf("\nQ=[ %f %f %f %f]",qn.w,qn.x,qn.y,qn.z);
printf("\n\nPlease wait!\n") ;
/* 将角度变化模拟为L3g4200的采集数据值 */
for(i=0; i<400;i++)
{
FastFormat(&qn,&quaternion); //qn是陀螺仪积分的四元数,quaternion是待更新四元数
}
ToEulerAngle(&quaternion, &ahrs); //将四元数转为角度
printf("\nQ=[ %f %f %f %f]",quaternion.w,quaternion.x,quaternion.y,quaternion.z);
printf("\nYaw = %f\nPitch = %f\nRoll = %f\n",ahrs.yaw,ahrs.pitch,ahrs.roll);
getch();
}while(1);
}
|
本帖子中包含更多资源
您需要 登录 才可以下载或查看，没有帐号？注册
x
|
[复制链接]
楼主
nice
