3892
- 收藏
- 点赞
- 分享
- 举报
四轴飞行的姿态估计(转载)
本帖最后由 ming1006 于 2013-8-23 11:45 编辑
距离我发电调那一贴,已经经过零零散散的一年时间,初步完成了姿态估计部分,因本人基础学科较差,大部分时间都是在反复阅读理解各种资料,呵呵.俗话说的好,知识在于积累,四轴在于坚持.与大家共勉.
一个稳定的系统,要有三个部分,(1)听话的执行模块(2)精确的测量模块(3)牛X的控制模块
对于四旋翼来说,电调以及电机是执行模块,姿态估计,高度估计,位置估计等等这些都是测量模块,控制就不说了
这里仅仅概述一下关于姿态估计部分的一些东西
【1】数学平台
这里所说的姿态,是用来描述一个刚体的固连坐标系和参考坐标系之间的角位置关系,有一些数学表示方法,很常见的就是欧拉角,也就是pitch,yaw,roll三个转角,转来转去很容易把人转糊涂,所以我们还是看一下另外一种
想象一下有两根筷子,它们的一端相连,并且两根筷子可以绕这一点旋转,呈一定角度,比如30度,那么现要让他们重合,很简单,把他们转到一起就行了,我相信没有人会先pitch XX度,然后yaw XX度 然后roll XX度,大家都会直接
把它们转到一起,一次就可以搞定的事情,干嘛分三步?
然后,更加理想化,固定其中一只筷子,旋转另外一只,那么很明显我们需要旋转30度,而旋转轴呢? 两根筷子确定一个平面A,沿它们的焦点做平面A的垂线,这就是旋转轴. 于是这就是一次旋转的姿态描述方式,爱尔兰数学家哈密
顿发明了一种叫做[四元数]的东西可以用来描述这种一次旋转,关于[四元数]的相关知识,可以看这个
四元数附件:Shoemake-quattut.pdf
我们要解决四旋翼和地球的相对姿态问题,地球太大了,换一个小一点的概念,将四轴放在水平地面上,四轴上固定着一个坐标系r,地面上固定着一个坐标系R,用四元数Q来描述r和R的角位置关系.于是现在有了基本的数学平台。
【2】传感器
再来说说测量的问题
1.陀螺仪,可以测量角速度,具有高动态特性,但是它是一个间接测量器件,它测量的是角度的导数,角速度,显然我们要将角速度对时间积分才能得到角度
如果这个世界是理想的,美好的,那我们的问题到此就解决了,不过很遗憾,现实是残酷的,误差的引入,使得积分出现了问题
假设陀螺仪固定不动,理想角速度值是0dps(degree per second),但是有一个偏置0.1dps加在上面,于是测量出来是0.1dps,积分一秒之后,得到的角度是0.1度,1分钟之后是6度,还能忍受,一小时之后是360度,转了一圈,也就是
说,陀螺仪在短时间内有很大的参考价值。
2.加速度计,可以测量加速度,包括重力加速度,于是在静止或匀速运动的时候,加速度计仅仅测量的是重力加速度,而重力加速度与刚才所说的R坐标系是固连的,通过这种关系,可以得到加速度计所在平面 与 地面 的角度关系.
但是加速度计若是绕着重力加速度的轴转动,则测量值不会改变,也就是说无法感知这种水平旋转。
3.磁传感器,可以测量磁场,在没有其他磁场的情况下,仅仅测量的是地球的磁场,而地磁也是和R坐标系固连的,通过这种关系,可以得到平面A和地平面的关系
(平面A:和磁场方向垂直的平面),同样的,若是沿着磁场方向的轴旋转,测量值不会改变,无法感知这种旋转。
综合考虑,加速度计和磁传感器都是极易受外部干扰的传感器,都只能得到2维的角度关系,但是测量值随时间的变化相对较小,结合加速度计和磁传感器可以得到3维的角度关系。陀螺仪可以积分得到三维的角度关系,动态
性能好,受外部干扰小,但测量值随时间变化比较大。可以看出,它们优缺点互补,结合起来才能有好的效果。
【3】数据融合
现在有了三个传感器,都能在一定程度上测量角度关系,但是究竟相信谁?
根据刚才的分析,应该是在短时间内更加相信陀螺仪,隔三差五的问问加速度计和磁传感器,我角度飘了多少了?
有一点必须非常明确,陀螺仪才是主角,加速度计和磁传感器仅仅是跑龙套的
关于数据融合,也有好多方法,这里采用一种最为形象,贴切的,简单易懂的,[互补滤波],可以先通过这个简单的单轴融合的例子了解,来自CROSSBOW
CROSSBOW
通过上图可以看出,陀螺仪在融合后的角度基础上积分,然后与加速度计测量的角度融合,然后又提供给下一次积分,看起来更像一个反馈系统.
【3】全姿态融合
上面的例子仅仅是单轴的融合,理解了单轴就可以结合四元数,得到3维的姿态融合方法了
图中的符号解释:Bg是加速度计测得的值,扩充为四元数,Bg=[0,Bgx,Bgy,Bgz],上标B代表是在体坐标系(Body Frame)下的测量值
Bh是磁传感器计测得的值,扩充为四元数,Bh=[0,Bhx,Bhy,Bhz]
Eh是参考坐标系下的固有磁场,地磁场,上标E代表是在参考坐标系(earth Frame)下的值
Eg是参考坐标系下的固有加速度,重力加速度
p,q,r是陀螺仪测得的角速度
[a,b,c,d]是姿态四元数Q
(1)关于用加速度计和磁传感器求姿态
假设现在已经得到正确的姿态四元数Q,那么可以利用四元数旋转将参考坐标系和体坐标系下的向量互相转换,将Eh和Eg转换到体坐标系下(BEh=Q×Eh×Q* , BEg=Q×Eg×Q*,BEh,BEg是参考坐标系的Eh,Eg由Q旋转到体坐标系下)
如果这个世界是理想的,美好的,那么Bg=BEg,Bh=BEh,也就是 { Q×Eh×Q* - Bh = 0} , { Q×Eg×Q* - Bg = 0}这两个方程成立,联立这两个方程就可以解得姿态四元数Q,不过很遗憾,现实是残酷的,由于各种误差的存在,这个
方程组无解,因此我们只能找到最优解,找最优解的最小方差问题有许多方法可以采用,如[梯度下降][高斯牛顿],图中是用的高斯牛顿法,由于高斯牛顿法含有矩阵求逆的计算,计算量超大,处理器不是DSP或FPGA的话就算了,关于
[梯度下降]和[高斯牛顿],可以在google,wiki百科搜索.
(2)陀螺仪积分求姿态
这个在网上搜索,陀螺仪,四元数微分方程 等等就可以找到了,秦永元[惯性导航]书上也有详细的
下图是一个很老的全姿态互补滤波的流程,不过很巧妙,PDF点击此处下载附件:Orientation Tracking for Humans and Robots.pdf加速度计和磁传感器经过高斯牛顿迭代得到姿态误差速率,陀螺仪直接通过四元数微分方程得到姿态四元数速率,两个加起来积分得到姿态四元数
最后,附上我的测试视频,手机拍摄,有点模糊
上位机:MFC+OpenGL
传感器:LSM303,L3G4200D
MCU:TMS320F28335
附件:姿态演示.rar
距离我发电调那一贴,已经经过零零散散的一年时间,初步完成了姿态估计部分,因本人基础学科较差,大部分时间都是在反复阅读理解各种资料,呵呵.俗话说的好,知识在于积累,四轴在于坚持.与大家共勉.
一个稳定的系统,要有三个部分,(1)听话的执行模块(2)精确的测量模块(3)牛X的控制模块
对于四旋翼来说,电调以及电机是执行模块,姿态估计,高度估计,位置估计等等这些都是测量模块,控制就不说了
这里仅仅概述一下关于姿态估计部分的一些东西
【1】数学平台
这里所说的姿态,是用来描述一个刚体的固连坐标系和参考坐标系之间的角位置关系,有一些数学表示方法,很常见的就是欧拉角,也就是pitch,yaw,roll三个转角,转来转去很容易把人转糊涂,所以我们还是看一下另外一种
想象一下有两根筷子,它们的一端相连,并且两根筷子可以绕这一点旋转,呈一定角度,比如30度,那么现要让他们重合,很简单,把他们转到一起就行了,我相信没有人会先pitch XX度,然后yaw XX度 然后roll XX度,大家都会直接
把它们转到一起,一次就可以搞定的事情,干嘛分三步?
然后,更加理想化,固定其中一只筷子,旋转另外一只,那么很明显我们需要旋转30度,而旋转轴呢? 两根筷子确定一个平面A,沿它们的焦点做平面A的垂线,这就是旋转轴. 于是这就是一次旋转的姿态描述方式,爱尔兰数学家哈密
顿发明了一种叫做[四元数]的东西可以用来描述这种一次旋转,关于[四元数]的相关知识,可以看这个
四元数附件:Shoemake-quattut.pdf
我们要解决四旋翼和地球的相对姿态问题,地球太大了,换一个小一点的概念,将四轴放在水平地面上,四轴上固定着一个坐标系r,地面上固定着一个坐标系R,用四元数Q来描述r和R的角位置关系.于是现在有了基本的数学平台。
【2】传感器
再来说说测量的问题
1.陀螺仪,可以测量角速度,具有高动态特性,但是它是一个间接测量器件,它测量的是角度的导数,角速度,显然我们要将角速度对时间积分才能得到角度
如果这个世界是理想的,美好的,那我们的问题到此就解决了,不过很遗憾,现实是残酷的,误差的引入,使得积分出现了问题
假设陀螺仪固定不动,理想角速度值是0dps(degree per second),但是有一个偏置0.1dps加在上面,于是测量出来是0.1dps,积分一秒之后,得到的角度是0.1度,1分钟之后是6度,还能忍受,一小时之后是360度,转了一圈,也就是
说,陀螺仪在短时间内有很大的参考价值。
2.加速度计,可以测量加速度,包括重力加速度,于是在静止或匀速运动的时候,加速度计仅仅测量的是重力加速度,而重力加速度与刚才所说的R坐标系是固连的,通过这种关系,可以得到加速度计所在平面 与 地面 的角度关系.
但是加速度计若是绕着重力加速度的轴转动,则测量值不会改变,也就是说无法感知这种水平旋转。
3.磁传感器,可以测量磁场,在没有其他磁场的情况下,仅仅测量的是地球的磁场,而地磁也是和R坐标系固连的,通过这种关系,可以得到平面A和地平面的关系
(平面A:和磁场方向垂直的平面),同样的,若是沿着磁场方向的轴旋转,测量值不会改变,无法感知这种旋转。
综合考虑,加速度计和磁传感器都是极易受外部干扰的传感器,都只能得到2维的角度关系,但是测量值随时间的变化相对较小,结合加速度计和磁传感器可以得到3维的角度关系。陀螺仪可以积分得到三维的角度关系,动态
性能好,受外部干扰小,但测量值随时间变化比较大。可以看出,它们优缺点互补,结合起来才能有好的效果。
【3】数据融合
现在有了三个传感器,都能在一定程度上测量角度关系,但是究竟相信谁?
根据刚才的分析,应该是在短时间内更加相信陀螺仪,隔三差五的问问加速度计和磁传感器,我角度飘了多少了?
有一点必须非常明确,陀螺仪才是主角,加速度计和磁传感器仅仅是跑龙套的
关于数据融合,也有好多方法,这里采用一种最为形象,贴切的,简单易懂的,[互补滤波],可以先通过这个简单的单轴融合的例子了解,来自CROSSBOW
CROSSBOW
通过上图可以看出,陀螺仪在融合后的角度基础上积分,然后与加速度计测量的角度融合,然后又提供给下一次积分,看起来更像一个反馈系统.
【3】全姿态融合
上面的例子仅仅是单轴的融合,理解了单轴就可以结合四元数,得到3维的姿态融合方法了
图中的符号解释:Bg是加速度计测得的值,扩充为四元数,Bg=[0,Bgx,Bgy,Bgz],上标B代表是在体坐标系(Body Frame)下的测量值
Bh是磁传感器计测得的值,扩充为四元数,Bh=[0,Bhx,Bhy,Bhz]
Eh是参考坐标系下的固有磁场,地磁场,上标E代表是在参考坐标系(earth Frame)下的值
Eg是参考坐标系下的固有加速度,重力加速度
p,q,r是陀螺仪测得的角速度
[a,b,c,d]是姿态四元数Q
(1)关于用加速度计和磁传感器求姿态
假设现在已经得到正确的姿态四元数Q,那么可以利用四元数旋转将参考坐标系和体坐标系下的向量互相转换,将Eh和Eg转换到体坐标系下(BEh=Q×Eh×Q* , BEg=Q×Eg×Q*,BEh,BEg是参考坐标系的Eh,Eg由Q旋转到体坐标系下)
如果这个世界是理想的,美好的,那么Bg=BEg,Bh=BEh,也就是 { Q×Eh×Q* - Bh = 0} , { Q×Eg×Q* - Bg = 0}这两个方程成立,联立这两个方程就可以解得姿态四元数Q,不过很遗憾,现实是残酷的,由于各种误差的存在,这个
方程组无解,因此我们只能找到最优解,找最优解的最小方差问题有许多方法可以采用,如[梯度下降][高斯牛顿],图中是用的高斯牛顿法,由于高斯牛顿法含有矩阵求逆的计算,计算量超大,处理器不是DSP或FPGA的话就算了,关于
[梯度下降]和[高斯牛顿],可以在google,wiki百科搜索.
(2)陀螺仪积分求姿态
这个在网上搜索,陀螺仪,四元数微分方程 等等就可以找到了,秦永元[惯性导航]书上也有详细的
下图是一个很老的全姿态互补滤波的流程,不过很巧妙,PDF点击此处下载附件:Orientation Tracking for Humans and Robots.pdf加速度计和磁传感器经过高斯牛顿迭代得到姿态误差速率,陀螺仪直接通过四元数微分方程得到姿态四元数速率,两个加起来积分得到姿态四元数
最后,附上我的测试视频,手机拍摄,有点模糊
上位机:MFC+OpenGL
传感器:LSM303,L3G4200D
MCU:TMS320F28335
附件:姿态演示.rar
文件: 姿态演示.rar
下载
文件: Shoemake-quattut.pdf
下载
文件: Combine Angular Rate Sensors with Accelerometers.pdf
下载
文件: Orientation Tracking for Humans and Robots.pdf
下载
我来回答
回答2个
时间排序
认可量排序
认可0
认可0
或将文件直接拖到这里
悬赏:
E币
网盘
* 网盘链接:
* 提取码:
悬赏:
E币
Markdown 语法
- 加粗**内容**
- 斜体*内容*
- 删除线~~内容~~
- 引用> 引用内容
- 代码`代码`
- 代码块```编程语言↵代码```
- 链接[链接标题](url)
- 无序列表- 内容
- 有序列表1. 内容
- 缩进内容
- 图片![alt](url)
相关问答
-
2013-08-23 13:07:36
-
2013-08-23 13:23:27
-
2013-08-23 11:31:18
-
2013-08-23 13:58:36
-
2013-12-14 14:18:08
-
2013-08-23 13:15:44
-
2013-08-23 11:17:11
-
2013-08-23 10:46:59
-
212013-12-01 12:28:16
-
2015-02-09 17:58:50
-
2017-05-04 21:03:53
-
2013-12-13 16:50:00
-
2013-08-23 13:18:43
-
2013-08-23 13:37:59
-
2013-08-23 17:30:26
-
12014-11-12 11:26:16
-
32014-04-04 21:58:27
-
02015-05-07 00:17:17
-
2013-12-03 17:23:26
无更多相似问答 去提问
点击登录
-- 积分
-- E币
提问
—
收益
—
被采纳
—
我要提问
切换马甲
上一页
下一页
悬赏问答
-
50如何获取vpss chn的图像修改后发送至vo
-
5FPGA通过Bt1120传YUV422数据过来,vi接收不到数据——3516dv500
-
50SS928 运行PQtools 拼接 推到设备里有一半画面会异常
-
53536AV100的sample_vdec输出到CVBS显示
-
10海思板子mpp怎么在vi阶段改变视频数据尺寸
-
10HI3559AV100 多摄像头同步模式
-
9海思ss928单路摄像头vio中加入opencv处理并显示
-
10EB-RV1126-BC-191板子运行自己编码的程序
-
10求HI3519DV500_SDK_V2.0.1.1
-
5有偿求HI3516DV500 + OV5647驱动
举报反馈
举报类型
- 内容涉黄/赌/毒
- 内容侵权/抄袭
- 政治相关
- 涉嫌广告
- 侮辱谩骂
- 其他
详细说明
提醒
你的问题还没有最佳答案,是否结题,结题后将扣除20%的悬赏金
取消
确认
提醒
你的问题还没有最佳答案,是否结题,结题后将根据回答情况扣除相应悬赏金(1回答=1E币)
取消
确认