问个低级的问题 直接对角速度积分相比四元数有什么弱点吗 这个问题不知道应该怎么查
问个低级的问题 直接对角速度积分相比四元数有什么弱点吗 这个问题不知道应该怎么查 回复【楼主位】liguole李国乐问个低级的问题 直接对角速度积分相比四元数有什么弱点吗 这个问题不知道应该怎么查
-----------------------------------------------------------------------
同问！！ 角速度直接積分在pitch或roll大於正負30度以上的值基本上是垃圾。 回复【2楼】g921002
-----------------------------------------------------------------------
不会吧？陀螺仪应该不会吧？
如果是加速度计转换成角度，才会受arctan 的影响以至于角度大了精度下降。 看一遍惯性导航原理就明白了 开别人源！
以下均为原作者的话，如有版权问题请与我联系！120538967
原作者：5iMX dongfang
资料来源：http://bbs.5imx.com/bbs/viewthread.php?tid=504301&highlight=%CB%C4%D4%AA%CA%FD
ourdev_692510DX36ZT.doc(文件大小:258K) (原文件名:捷联系统的四元数法姿态算法.doc)
算法名：捷联系统的四元数法姿态算法
算法输入：物体的初始姿态，3轴陀螺仪不同时刻的Yaw，Pitch，Roll的角速度；
算法输出：物体的当前姿态。
具体算法：（有颜色的是李国乐加的）
1. 初始姿态的四元数(w,x,y,z)=(1,0,0,0) 命名为A（四元数初值）
2. 读取3轴陀螺仪当前时刻的Yaw,Pitch,Roll角速度，乘以上次计算以来的间隔时间，得到上一时刻以来(Yaw,Pitch,Roll)的变化量，命名为欧拉角b（角速度对时间积分的角度变化量得到欧拉角）
3. b是Tait-Bryan angle定义的欧拉角，将其转为四元数B（欧拉角->四元数）
4. A=A×B，做四元数乘法，即可得到当前姿态对应的新的四元数A（更新四元数）
5.重复2～4部，即可连续更新姿态（循环计算）
6.将四元数A重新转换为Tait-Bryan angle形式的欧拉角a，就可以以直观的形式查看当前姿态（四元数->欧拉角）
这里严重感谢http://www.cppblog.com/heath/archive/2009/12/13/103127.html 的作者，他的代码解决了我最重要的问题: Tait-Bryan angle定义的欧拉角和四元数的双向转换。但他是个游戏开发者，没做姿态更新，我补充了姿态更新的代码。
另外补充一句，我使用的是arduino做的开发，也就是12M的8位AVR单片机，目前，每秒在获取数据400个的速度下，如此大工作量的算法依然能跑。3D旋转N圈，大约10秒钟，各轴漂移在5度左右。
\核心算法1，欧拉角转四元数
void Quaternion::FromEulerAngle(const EulerAngle &ea)
{
float fCosHRoll = cos(ea.fRoll * .5f);
float fSinHRoll = sin(ea.fRoll * .5f);
float fCosHPitch = cos(ea.fPitch * .5f);
float fSinHPitch = sin(ea.fPitch * .5f);
float fCosHYaw = cos(ea.fYaw * .5f);
float fSinHYaw = sin(ea.fYaw * .5f);
w = fCosHRoll * fCosHPitch * fCosHYaw + fSinHRoll * fSinHPitch * fSinHYaw;
x = fCosHRoll * fSinHPitch * fCosHYaw + fSinHRoll * fCosHPitch * fSinHYaw;
y = fCosHRoll * fCosHPitch * fSinHYaw - fSinHRoll * fSinHPitch * fCosHYaw;
z = fSinHRoll * fCosHPitch * fCosHYaw - fCosHRoll * fSinHPitch * fSinHYaw;
}
核心算法2，四元数转欧拉角
EulerAngle Quaternion::ToEulerAngle() const
{
EulerAngle ea;
ea.fRoll= atan2(2 * (w * z + x * y) , 1 - 2 * (z * z + x * x));
ea.fPitch = asin(CLAMP(2 * (w * x - y * z) , -1.0f , 1.0f));
ea.fYaw = atan2(2 * (w * y + z * x) , 1 - 2 * (x * x + y * y));
return ea;
}
核心算法3，四元数乘法
Quaternion Quaternion::Multiply(const Quaternion &b)
{
Quaternion c;
c.w=w*b.w -x*b.x -y*b.y -z*b.z;
c.x=w*b.x +x*b.w +y*b.z -z*b.y;
c.y=w*b.y -x*b.z +y*b.w +z*b.x;
c.z=w*b.z +x*b.y -y*b.x +z*b.w;
c.Normalize();
return c;
}
次要的规范化算法：
voidQuaternion::Normalize(){
float s=getS();
w/=s;
x/=s;
y/=s;
z/=s;
}
float Quaternion::getS(){
return sqrt(w*w+x*x+y*y+z*z);
}
我的loop函数，算法的集成部分：
Quaternion nowQ;
void loop() {
int intx, inty,intz;
floatpitch,roll,yaw;
gyro.ReadGyroOutCalibrated_Radian(&pitch, &roll, &yaw);
EulerAngle dt;
dt.fRoll=roll;
dt.fPitch=pitch;
dt.fYaw=-yaw;
Quaternion dQ;
dQ.FromEulerAngle(dt);
nowQ=nowQ.Multiply(dQ);
count++;
if (count>1000){
EulerAngle nowG=nowQ.ToEulerAngle();
Serial.print(nowG.fRoll/3.1415926535*180,11);//横滚
Serial.print(",");
Serial.print(nowG.fPitch/3.1415926535*180,11);//俯仰
Serial.print(",");
Serial.print(nowG.fYaw/3.1415926535*180,11);//偏航
Serial.print(",");
Serial.print(nowQ.getS(),11);//偏航
Serial.println();
count=0;
}
} 回复【4楼】zht9961020
-----------------------------------------------------------------------
在这里发问，主要是因为看不懂惯导理论 尝试着找个捷径！还请不吝指教！先谢了 ./emotion/em063.gif 回复【6楼】liguole 李国乐
-----------------------------------------------------------------------
这个是真的没有捷径。实际上你在尝试寻找捷径的过程中，就会发现没有所谓的捷径，呵呵。 Mark! 感觉便宜的传感器数据直接用很短时间就漂移的不能用了 回复【5楼】liguole李国乐
开别人源！
以下均为原作者的话，如有版权问题请与我联系！120538967
原作者：5imx dongfang
资料来源：http://bbs.5imx.com/bbs/viewthread.php?tid=504301&highlight=%cb%c4%d4%aa%ca%fd
(原文件名:捷联系统的四元数法姿态算法.doc)
算法名：捷联系统的四元数法姿态算法
算法输入：物体的初始姿态，3轴陀螺仪不同时刻的yaw，pitch，roll的角速度；
算法输出：物体的当前姿态。
具体算法：（有颜色的是李国乐加的）
1. 初始姿态的四元数(w,x,y,z)=(1,0,0,0) 命名为a（四元数初值）
2. 读取3轴陀螺仪当前时刻的yaw,pitch,roll角速度，乘以上次计算以来的间隔时间，得到上一时刻以来(yaw,pitch,......
-----------------------------------------------------------------------
这个我已经从新写成C语言版的了。需要的话，在我的飞行器那个帖子里有源码。
里面的thread_posture.c 线程里面有。 捷径就是直接把更新那几个公式背下来就行，中间推导过程不用管，看惯性导航的书都是公式，我也看不懂，到最后用还是那几个公式，所以直接用就行了，基本原理明白就可以了 mark mark 假如在横滚90度的情况下，飞机俯仰陀螺如果感知90度的输出，如果只是按单个陀螺仪积分，那么会得出飞机俯仰了90度，而实际上飞机只是偏航90度 帖子中的链接刚看过，不错 majianjia 发表于 2011-11-6 22:39 static/image/common/back.gif
回复【5楼】liguole李国乐
开别人源！
以下均为原作者的话，如有版权问题请与我联系！120538967
现在帖子很多 不好找能否给个链接谢谢 四元数是用来做姿态解算的，直接对角度积分得到的是机体系的角度，控制的时候需要地理系中的角度，所以可以使用四元数进行解算 本帖最后由 liguole 于 2013-2-7 22:00 编辑
第七实验室 姿态板的配套资料，只发了姿态解算及更新部分，在这里上传也不知道合适不合适！抛别人的砖引更好的玉吧！ 老外的开源奉献精神，着实让我学习，没有MK的开源，四轴项目就不会得到这么好的发展！ 如有不妥请联系我，随时删除。QQ120538967 不一定 在线！{:smile:} liguole 发表于 2013-2-7 21:58 static/image/common/back.gif
第七实验室 姿态板的配套资料，只发了姿态解算及更新部分，在这里上传也不知道合适不合适！抛别人的砖引更 ...
里面用的是《捷联惯导算法心得》里的代码，貌似阿莫论坛所有的姿态解算都用的是那个帖子里的代码。 liguole 发表于 2011-11-6 18:31 static/image/common/back.gif
开别人源！
以下均为原作者的话，如有版权问题请与我联系！120538967
非常感谢你的分享！ jmp2002911 发表于 2011-11-28 09:46
假如在横滚90度的情况下，飞机俯仰陀螺如果感知90度的输出，如果只是按单个陀螺仪积分，那么会得出飞机俯仰 ...
这个例子非常好，正解~~ gunsir 发表于 2014-3-5 13:45
这个例子非常好，正解~~
不大理解,能帮忙详细点吗 zhuyi 发表于 2014-7-29 18:11
不大理解,能帮忙详细点吗
各个姿态角（欧拉角）之间有耦合关系，在很小的幅度内各轴单独积分可以近似为姿态角，但是范围一大，这种误差会非常大，直至完全不准确，因为原理上就是不对的，各个轴单独积分就相当于忽略了相互间的影响，要知道一个坐标架的三个轴是绑在一起的~~建议看看清楚欧拉角的定义就明白了~~ gunsir 发表于 2014-8-12 08:10
各个姿态角（欧拉角）之间有耦合关系，在很小的幅度内各轴单独积分可以近似为姿态角，但是范围一大，这种 ...
哦,大概就是说动作幅度大了,XYZ轴混乱了是吗? 那如果四轴动作幅度不大,不做特技,只求飞温,小幅度的情况下,单轴积分应该可以用吧? zhuyi 发表于 2014-8-12 09:56
哦,大概就是说动作幅度大了,XYZ轴混乱了是吗? 那如果四轴动作幅度不大,不做特技,只求飞温,小幅度的情况下 ...
那你也得要融合加速度计的数据~~而且我感觉传感器准确是控制好的前提，否则很危险~~ gunsir 发表于 2014-8-15 08:32
那你也得要融合加速度计的数据~~而且我感觉传感器准确是控制好的前提，否则很危险~~ ...
大概了解了,谢谢! 陀螺仪会飘。
当然也有KK板只用3个陀螺仪也控制的很好的。
页:
[1]