初学PX4之飞控算法.docx
- 文档编号:30744320
- 上传时间:2023-08-20
- 格式:DOCX
- 页数:25
- 大小:27.19KB
初学PX4之飞控算法.docx
《初学PX4之飞控算法.docx》由会员分享,可在线阅读,更多相关《初学PX4之飞控算法.docx(25页珍藏版)》请在冰豆网上搜索。
初学PX4之飞控算法
初学PX4之飞控算法
通知:
如果你对本站无人机文章不熟悉,建议查看无人机学习概览!
!
!
注意:
基于参考原因,本文参杂了APM的算法分析。
本篇文章首先简述了下px4和apm调用姿态相关应用程序出处,然后对APM的DCM姿态解算算法参考的英文文档进行了翻译与概括,并结合源代码予以分析,在此之前,分析了starlino的DCM,并进行了matlab的实现,因为它更加利于理解。
后段时间会对px4的四元数姿态解算进行分析。
姿态控制部分描述了串级PID在APM里的实现流程,同样后期会完善对px4的分析。
最后针对自己平时使用的一些调试技巧进行了总结。
姿态出处分析
下面看下重要的一个脚本/etc/init.d/rc.mc_apps,可以知道姿态估计用的是attitude_estimator_q和position_estimator_inav,用户也可以选择local_position_estimator、ekf2,而姿态控制应用为mc_att_control和mc_pos_control。
#!
nsh
ifparamcompareINAV_ENABLED1
then
attitude_estimator_qstart
position_estimator_inavstart
else
ifparamcompareLPE_ENABLED1
then
attitude_estimator_qstart
local_position_estimatorstart
else
ekf2start
fi
fi
ifmc_att_controlstart
then
else
#trythemultiplatformversion
mc_att_control_mstart
fi
ifmc_pos_controlstart
then
else
#trythemultiplatformversion
mc_pos_control_mstart
fi
...
而在ardupilot中,姿态解算与控制算法在ArduCopter.cpp的fast_loop任务中以400Hz的频率运行。
//Mainloop-400hz
voidCopter:
:
fast_loop()
{
//IMUDCMAlgorithm
//--------------------
read_AHRS();
...
}
voidCopter:
:
read_AHRS(void)
{
...
ahrs.update();
}
了解了上面的源码出处后,下面将分具体应用进行分析。
姿态估算
DCM_tutorial
imu_guide/imu_guide中文翻译/dcm_tutorial/wiki资料查询/该部分算法源码参考
将该算法转换为了matlab实现,想了解的可以查看我的github里的DCM工程,能够更好的理解算法,另外,该matlab实现还有一定的bug,希望各位大神的pullrequest~
这部分翻译自dcm_tutorial,并结合源码进行分析,可作为下部分DCM理论介绍的基础哦,所以建议先将这部分看完再往下看~
DCM矩阵:
设机体坐标系为Oxyz,与机体坐标系同x,y,z方向的单位向量为i,j,k。
地理坐标系为OXYZ,同理地理坐标系的单位向量为I,J,K。
共同原点为O。
如图
IG"role="presentation">GG={1,0,0}T"role="presentation">TT,JG"role="presentation">GG={0,1,0}T"role="presentation">TT,KG"role="presentation">GG={0,0,1}T"role="presentation">TT
iB"role="presentation">BB={1,0,0}T"role="presentation">TT,jB"role="presentation">BB={0,1,0}T"role="presentation">TT,kB"role="presentation">BB={0,0,1}T"role="presentation">TT
下面将i,j,k向量用地理坐标系表示,首先以i作为一个例子。
iG"role="presentation">GG={ix"role="presentation">xxG"role="presentation">GG,iy"role="presentation">yyG"role="presentation">GG,iz"role="presentation">zzG"role="presentation">GG}T"role="presentation">TT
其中ix"role="presentation">xxG"role="presentation">GG表示的是i向量在地理坐标系X轴上的投影,即
ix"role="presentation">xxG"role="presentation">GG=|i|cos(X,i)=cos(I,i)
在这个式子中,|i|是i单位向量的范式(长度),cos(I,i)是向量I和向量i夹角的余弦,因此可以这样写:
ix"role="presentation">xxG"role="presentation">GG=cos(I,i)=|I||i|cos(I,i)=I.i
在这个式子中,I.i是向量I和向量i的点积,由于只是计算点积,我们并不关心向量是在哪个坐标系中测量的,只要都是以同一个坐标系表示即可。
所以:
I.i=IB"role="presentation">BB.iB"role="presentation">BB=IG"role="presentation">GG.iG"role="presentation">GG=cos(IB"role="presentation">BB.iB"role="presentation">BB)=cos(IG"role="presentation">GG.iG"role="presentation">GG)
同样的:
iy"role="presentation">yyG"role="presentation">GG=J.i,iz"role="presentation">zzG"role="presentation">GG=K.i
所以i向量可以用地理坐标系表示为:
iG"role="presentation">GG={I.i,J.i,K.i}T"role="presentation">TT
同样的,j,k向量可以表示为:
jG"role="presentation">GG={I.j,J.j,K.j}T"role="presentation">TT,kG"role="presentation">GG={I.k,J.k,K.k}T"role="presentation">TT
将机体坐标i,j,k的地理坐标表示以矩阵的形式表示为:
这就是方向余弦矩阵,它是由机体坐标系和地理坐标系向量所有两两向量组合的夹角的余弦组成,可以算出共有9×"role="presentation">××9种。
下一节的DCM理论里面有另外一种推理DCM的方法,建议交互思考!
而将地理坐标系在机体坐标系中表示与将机体坐标系在地理坐标系中表示是对称的,所以只需简单交换I,J,K和i,j,k即可。
IB"role="presentation">BB={I.i,I.j,I.k}T"role="presentation">TT,JB"role="presentation">BB={J.i,J.j,J.k}T"role="presentation">TT,KB"role="presentation">BB={K.i,K.j,K.k}T"role="presentation">TT
转化为矩阵形式为:
很容易可以发现:
DCMB"role="presentation">BB=(DCMG"role="presentation">GG)T"role="presentation">TTorDCMG"role="presentation">GG=(DCMB"role="presentation">BB)T"role="presentation">TT,换句话说,这两个矩阵是可以相互转换的。
也可以发现:
DCMB"role="presentation">BB.DCMG"role="presentation">GG=(DCMG"role="presentation">GG)T"role="presentation">TT.DCMG"role="presentation">GG=DCMB"role="presentation">BB.(DCMB"role="presentation">BB)T"role="presentation">TT=I3"role="presentation">33
其中I3"role="presentation">33为3×"role="presentation">××3的单位矩阵,换句话说,DCM矩阵是正交矩阵。
证明如下:
证明这个我们需要知道这些特性:
iGT"role="presentation">GTGT.iG"role="presentation">GG=|iG"role="presentation">GG||iG"role="presentation">GG|cos(0)=1,iGT"role="presentation">GTGT.jG"role="presentation">GG=0,因为i和j是正交的。
方向余弦矩阵也称为旋转矩阵,如果知道机体坐标,则可以算出任意向量的地理坐标(反之同理),下面以机体坐标系向量使用DCM算出地理坐标作为例子进行推理:
rB"role="presentation">BB={rx"role="presentation">xxB"role="presentation">BB,ry"role="presentation">yyB"role="presentation">BB,rz"role="presentation">zzB"role="presentation">BB}T"role="presentation">TT
而rG"role="presentation">GG={rx"role="presentation">xxG"role="presentation">GG,ry"role="presentation">yyG"role="presentation">GG,rz"role="presentation">zzG"role="presentation">GG}T"role="presentation">TT
现在让我们分析第一个坐标rx"role="presentation">xxG"role="presentation">GG:
rx"role="presentation">xxG"role="presentation">GG=|rG"role="presentation">GG|cos(IG"role="presentation">GG,rG"role="presentation">GG),由于坐标系旋转,向量的大小,夹角都不会变,故有:
|rG"role="presentation">GG|=|rB"role="presentation">BB|,|IG"role="presentation">GG|=|IB"role="presentation">BB|=1,cos(IG"role="presentation">GG,rG"role="presentation">GG)=cos(IB"role="presentation">BB,rB"role="presentation">BB),所以:
rx"role="presentation">xxG"role="presentation">GG=|rG"role="presentation">GG|cos(IG"role="presentation">GG,rG"role="presentation">GG)=|IB"role="presentation">BB||rB"role="presentation">BB|cos(IB"role="presentation">BB,rB"role="presentation">BB)=IB"role="presentation">BB.rB"role="presentation">BB=IB"role="presentation">BB.{rx"role="presentation">xxB"role="presentation">BB,ry"role="presentation">yyB"role="presentation">BB,rz"role="presentation">zzB"role="presentation">BB}T"role="presentation">TT
由上可知,rB"role="presentation">BB={rx"role="presentation">xxB"role="presentation">BB,ry"role="presentation">yyB"role="presentation">BB,rz"role="presentation">zzB"role="presentation">BB}T"role="presentation">TT,替换得:
rx"role="presentation">xxG"role="presentation">GG=IB"role="presentation">BB.rB"role="presentation">BB={I.i,I.j,I.k}T"role="presentation">TT.{rx"role="presentation">xxB"role="presentation">BB,ry"role="presentation">yyB"role="presentation">BB,rz"role="presentation">zzB"role="presentation">BB}T"role="presentation">TT=rx"role="presentation">xxB"role="presentation">BBI.i+ry"role="presentation">yyB"role="presentation">BBI.j+rz"role="presentation">zzB"role="presentation">BBI.k
同样的思路:
ry"role="presentation">yyG"role="presentation">GG=rxB"role="presentation">BBJ.i+ryB"role="presentation">BBJ.j+rzB"role="presentation">BBJ.k
rzG"role="presentation">GG=rxB"role="presentation">BBK.i+ryB"role="presentation">BBK.j+rzB"role="presentation">BBK.k
转化为矩阵形式为:
得证。
同样的思路可以证明:
也可以这样证明:
DCMB"role="presentation">BBrG"role="presentation">GG=DCMB"role="presentation">BBDCMG"role="presentation">GGrB"role="presentation">BB=DCMGT"role="presentation">GTGTDCMG"role="presentation">GGrB"role="presentation">BB=I3"role="presentation">33rB"role="presentation">BB=rB"role="presentation">BB
角速度:
如下图所示,r为任意的旋转向量,t时刻的坐标为r(t)。
时间间隔dt后:
r=r(t),r’=r(t+dt)anddr=r’–r。
dt时间后向量r绕着与单位向量u同向的轴旋转了dθ"role="presentation">θθ,停到了向量r'的位置。
其中u垂直与旋转的机身,因此u正交于r与r',图中显示了u与u',它们与r和r‘的叉乘结果方向相同。
故有
u=(rxr’)/|rxr’|=(rxr’)/(|r||r’|sin(dθ))=(rxr’)/(|r|2"role="presentation">22sin(dθ))
由于旋转并不改变向量的长度,因此有|r’|=|r|。
向量r的线速度可以表示如下:
v=dr/dt=(r’–r)/dt
当dθ→0时,向量r和dr的夹角α"role="presentation">αα可通过r,r'和dr组成的等腰三角形计算:
α=(π–dθ)/2当dθ→0时,α→π/2。
这告诉我们,当dt→0时,r垂直于dr,因此r⊥v(v和dr的方向是一致的)。
现在定义角速度向量,其中反应了角度的变化率和旋转轴方向。
w=(dθ/dt)u
下面分析w和v之间的关系:
w=(dθ/dt)u=(dθ/dt)(rxr’)/(|r|2"role="presentation">22sin(dθ))
当dt→0时,dθ→0,因此sin(dθ)≈dθ。
化简得:
w=(rxr’)/(|r|2"role="presentation">22dt)
现在由于r’=r+dr,dr/dt=v,rxr=0,利用叉乘的加法分配率可得:
w=(rx(r+dr))/(|r|2"role="presentation">22dt)=(rxr+rxdr))/(|r|2"role="presentation">22dt)=rx(dr/dt)/|r|2"role="presentation">22
最后得出:
w=rxv/|r|2"role="presentation">22
下面反向推理证明v=wxr
利用向量的三重积公式:
(axb)xc=(a.c)b–(b.c)a,以及v和r是垂直的,所以v.r=0
wxr=(rxv/|r|2"role="presentation">22-)xr=(rxv)xr/|r|2"role="presentation">22-=((r.r)v+(v.r)r)/|r|2"role="presentation">22-=(|r|2"role="presentation">22-v+0)|r|2"role="presentation">22=v
得证。
陀螺仪及角速度向量
如果我们定期获取陀螺仪的值,时间间隔为dt,那么陀螺仪将会告诉我们在这段时间,地球绕陀螺仪各轴旋转的度数。
dθx"role="presentation">xx=wx"role="presentation">xxdt,dθy"role="presentation">yy=wy"role="presentation">yydt,dθz"role="presentation">zz=wz"role="presentation">zzdt
其中wx"role="presentation">xx=wx"role="presentation">xxi={wx"role="presentation">xx,0,0}T"role="presentation">TT,wy"role="presentation">yy=wy"role="presentation">yyj={0,wy"role="presentation">yy,0}T"role="presentation">TT,wz"role="presentation">zz=wz"role="presentation">zzk={0,0,wz"role="presentation">zz}T"role="presentation">TT
每次旋转都会产生线性的位移
dr1"role="presentation">11=dtv1"role="presentation">11=dt(wx"role="presentation">xxxr);dr2"role="presentation">22=dtv2"role="presentation">22=dt(wy"role="presentation">yyxr);dr3"role="presentation">33=dtv3"role="presentation">33=dt(wz"role="presentation">zzxr).
矢量相加:
dr=dr1"role="presentation">11+dr2"role="presentation">22+dr3"role="presentation">33=dt(wx"role="presentation">xxxr+wy"role="presentation">yyxr+wz"role="presentation">zzxr)=dt(wx"role="presentation">xx+wy"role="presentation">yy+wz"role="presentation">zz)xr
因此线速度可以表示为:
v=dr/dt=(wx"role="presentation">xx+wy"role="presentation">yy+wz"role="presentation">zz)xr=
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 初学 PX4 算法