CN103697890B - 基于证据融合的四旋翼飞行器姿态估计方法 - Google Patents
基于证据融合的四旋翼飞行器姿态估计方法 Download PDFInfo
- Publication number
- CN103697890B CN103697890B CN201310641284.8A CN201310641284A CN103697890B CN 103697890 B CN103697890 B CN 103697890B CN 201310641284 A CN201310641284 A CN 201310641284A CN 103697890 B CN103697890 B CN 103697890B
- Authority
- CN
- China
- Prior art keywords
- alpha
- rsqb
- lsqb
- overbar
- lambda
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 52
- 230000004927 fusion Effects 0.000 title claims abstract description 46
- 238000005315 distribution function Methods 0.000 claims abstract description 9
- 239000013598 vector Substances 0.000 claims description 108
- 239000011159 matrix material Substances 0.000 claims description 14
- 230000007704 transition Effects 0.000 claims description 14
- 239000000047 product Substances 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 9
- 238000010276 construction Methods 0.000 claims description 7
- 239000012467 final product Substances 0.000 claims description 6
- VKYKSIONXSXAKP-UHFFFAOYSA-N hexamethylenetetramine Chemical compound C1N(C2)CN3CN1CN2C3 VKYKSIONXSXAKP-UHFFFAOYSA-N 0.000 claims description 6
- 238000012546 transfer Methods 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 230000014509 gene expression Effects 0.000 claims 1
- 238000009826 distribution Methods 0.000 abstract description 5
- 230000015572 biosynthetic process Effects 0.000 abstract 1
- 238000003786 synthesis reaction Methods 0.000 abstract 1
- 238000005516 engineering process Methods 0.000 description 4
- 238000001914 filtration Methods 0.000 description 4
- 238000013507 mapping Methods 0.000 description 3
- 102100035971 Molybdopterin molybdenumtransferase Human genes 0.000 description 2
- 101710119577 Molybdopterin molybdenumtransferase Proteins 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000007123 defense Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 239000000203 mixture Substances 0.000 description 1
- 230000002123 temporal effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明涉及一种基于证据融合的四旋翼飞行器姿态估计方法。本发明建立四旋翼飞行器姿态的状态方程与观测方程,其中的状态与观测噪声都被建模为三角形可能性分布函数。将该函数转换为噪声证据,然后将它们带入状态方程和观测方程与实际观测值合成后,即可生成三种描述飞行器状态的证据,利用证据组合规则实现这些证据的融合,通过区间映射工具实现三个证据在连续时刻的递归传播。对融合后证据所包含的信度赋值进行加权,即可得到飞行器姿态的估计值。本发明所提方法无需满足状态和观测噪声概率分布已知的苛刻要求,而只需限定噪声有界且可用简单的三角形函数描述。这增加了在实际姿态估计中的实用性,与已有经典方法相比,也具有较高的估计精度。
Description
技术领域
本发明涉及一种基于证据融合的四旋翼飞行器姿态估计方法,属于飞行器姿态估计与控制技术领域。
背景技术
现代传感器技术、计算机技术和各种智能信息处理技术的快速发展,推动了面向各种功能需求的航空航天飞行器在国防和民用领域中的广泛应用。由于功能需求多样且应用环境复杂,使得对飞行器姿态估计的稳定性和精准性方面提出了较高的要求,因此对飞行器姿态估计方法的研究具有十分重要的理论和实际意义。
根据飞行器的不同应用场景和性能要求,可采用不同的姿态测量***及估计方法,但其本质都是以非线性滤波为基础。目前广泛使用的非线性滤波方法是扩展卡尔曼滤波(ExtendedKalmanFilter,EKF)和无迹卡尔曼滤波(UnscentedKalmanFilter,UKF)等,它们都要求***的状态噪声与观测噪声的概率分布等统计特性精确已知,但是在实际中,通常没有足够的先验数据用于统计出精确的概率分布,更容易得到的是噪声变化的大致界限。所以,研究噪声有界下的飞行器姿态估计问题,更加贴合实际应用环境,且具有较强的理论研究价值。
发明内容
本发明的目的在于,所提出的一种基于证据融合的四旋翼飞行器姿态估计方法,可以在状态与观测噪声有界情况下,建立关于飞行器姿态状态方程、观测方程及实际测量的证据,通过证据融合得到姿态估计值,利用区间映射工具实现证据的传播,从而解决状态和观测噪声统计分布不能精确获得的情况下的姿态估计问题。
本发明提出的一种基于证据融合的四旋翼飞行器姿态估计方法,包括以下各步骤:
(1)建立四旋翼飞行器姿态的状态方程与观测方程,具体步骤如下
(1-1)建立四旋翼飞行器姿态的状态方程如式(1)所示:
xk+1=Axk+Q(v)(1)
式(1)中,k=1,2,3,…,表示时刻, 表示k时刻四旋翼飞行器姿态的四元数状态向量,A为状态转移矩阵
其为一个三角形可能性分布函数,其中,(1-2)建立四旋翼飞行器姿态的观测方程如式(3)所示:
zk+1=h(xk+1)+R(w)(3)
其中,表示k+1时刻四旋翼飞行器姿态的观测向量,θk+1和ψk+1分别为四旋翼飞行器k+1时刻的横滚角、俯仰角、偏航角的取值,状态向量xk+1到观测向量zk+1的转换函数为:
记r1、r2和r3分别表示四旋翼飞行器姿态的横滚角、俯仰角、偏航角,则 为观测噪声函数向量, 是四旋翼飞行器姿态的观测噪声变化向量,并有
其为一个三角形可能性分布函数,其中
四旋翼飞行器姿态的四元数状态向量 中的各元素与观测向量 各元素之间的转换关系函数如式(6)所示
其中
(2)构造状态噪声函数向量Q(v)关于四元数状态向量xk中元素qi,k的证据 具体步骤如下:
(2-1)构造状态噪声函数向量Q(v)关于四元数状态向量xk元素qi,k的初始证据其中 表示关于k时刻四旋翼飞行器姿态的四元数状态向量 中元素qi,k(i=0,1,2,3)的状态噪声区间集合,为中的第j个取值为区间的元素,j=0,1,…,p-1,p∈[3,5],为它的左端点,为它的右端点,且有
其中αj=j/p,则有
是中各区间元素的信度组成的集合,并有
其元素的取值如式(12)所示
(2-2)对步骤(2-1)获取的初始证据分别进行折扣计算,获得状态噪声函数向量Q(v)关于四元数状态向量xk元素qi,k的证据其中关于qi,k的状态噪声区间集合为
其中
这里折扣率εQ∈[0.03,0.08];
(3)通过状态转移矩阵A获取k时刻关于向量xk的元素qi,k的预测证据其中下标k+1|k表示利用k时刻的状态对k+1时刻的状态进行预测,具体步骤如下:
(3-1)构建xk元素qi,k的状态估计证据具体步骤如下:
(a)当k=0时,取zk的元素r1,kr2,kr3,k分别带入式(7)-(10)得到
再利用式(6)得到
由此得到状态估计向量 则状态估计证据中的
亦即
(b)k≥1时,由递归计算得到状态估计向量则元素qi,k的状态估计证据为
亦即
(3-2)通过状态转移矩阵A获取向量xk的元素qi,k的预测证据 其中
这里,Ai+1,i+1表示状态转移矩阵A中,第i+1行i+1列的元素,其中i=0,1,2,3,并有Ai+1,i+1=1,故并有即:
(4)通过观测方程获取k+1时刻关于四旋翼飞行器姿态的观测向量zk+1的元素rl,k+1的观测预测证据具体步骤如下:
(4-1)通过步骤(3)得到状态预测证据之后,分别抽取中的一个元素进行排列组合,共计产生(p+1)4个四元区间组,将这些四元区间组作为式(4)的输入量,利用MATLAB2010a软件中的工具箱intlab,分别得到关于rl,k+1的(p+1)4个区间,它们组成的区间集合为
并得到式(22)中每个区间的信度赋值组成的信度集合为
其中的为所对应式(4)输入的四元区间组中每个区间信度赋值的乘积,由式(22)和(23)可以构成关于rl,k+1的初始观测预测证据
(4-2)将步骤(4-1)所得的进行简化后得到元素rl,k+1的观测预测证据 其中
式(24)中的为中信度赋值最大的那个区间,则式(25)中该区间的信度赋值若有多个区间信度赋值相等且最大,则将它们取并后构成将它们的信度相加后构成式(24)中的为剩余各区间取并后得到的区间,且这些区间的信度相加后构成
(5)利用Dempster组合规则求出k+1时刻观测域的融合证据 具体步骤如下:
(5-1)构建zk+1元素rl,k+1的实时观测证据具体步骤如下
(a)构造观测噪声函数向量R(w)关于观测向量zk+1元素rl,k+1的初始证据其中 表示关于k时刻四旋翼飞行器姿态观测向量zk中元素rl,k的观测噪声区间集合,为中的第j个取值为区间的元素,j=0,1,…,p-1,p∈[3,5],为它的左端点,为它的右端点,且有
其中αj=j/p,则有
(b)对步骤(a)获取的初始证据分别进行折扣计算,获得观测噪声向量函数R(w)关于观测向量zk+1元素rl,k+1的证据其中关于rl,k+1的观测噪声区间集合为
其中
这里折扣率εR∈[0.03,0.08];
(c)当从陀螺仪观测到向量 则构建元素rl,k+1的观测证据 为
亦即:
(5-2)将得到的观测证据与观测预测证据利用Dempster组合规则进行证据融合,得到观测域的融合证据
令 其信度赋值为 令 其信度赋值为
利用Dempster组合规则将和组合后得到
其中ζ=1,2,…,n,n≤2·(p+1)表示通过Ag∩Ch共计生成了n个不同的区间 则观测域的融合证据 为
(6)通过观测方程的逆运算获得k+1时刻关于向量xk+1的元素qi,k+1的状态域的新证据具体步骤如下:
(6-1)由步骤(5)得到的观测域的融合证据之后,分别抽取中的一个元素进行排列组合,共计产生n3个三元区间组,将这些三元区间组分别作为式(7-10)的输入量,利用MATLAB2010a软件中的工具箱intlab,得到关于qi,k+1的n3个区间,它们组成的区间集合为
并可得到式(33)中每个区间的信度赋值组成的信度集合为
其中的为所对应式(7-10)输入的三元区间组中每个区间信度赋值的乘积,由式(33)和(34)可以构成关于qi,k+1的状态域的初始新证据
(6-2)将步骤(6-1)所得的进行简化后得到关于元素qi,k+1的状态域的初始新证据其中
式(35)中的为中信度赋值最大的那个区间,则式(36)中该区间的信度赋值若有多个区间信度赋值相等且最大,则将它们取并后构成将它们的信度相加后构成式(35)中的为剩余各区间取并后得到的区间,且这些区间的信度相加后构成
(7)利用Dempster组合规则求出k+1时刻状态估计的融合证据
将步骤(6)中得到的关于向量xk+1的元素qi,k+1的状态域的新证据和步骤(3)中得到的关于向量xk的元素qi,k的预测证据利用Dempster组合规则进行证据融合,得到k+1时刻状态估计的融合证据
令 其信度赋值为 令 其信度赋值为
利用Dempster组合规则将和组合后得到
其中ξ=1,2,…,m,m≤2·(p+1)表示通过Gg∩Hh共计生成m个不同的区间,则状态估计的融合证据为
(8)获得k+1时刻状态估计向量的元素的状态估计值:
其中,
然后将获得的k+1时刻状态估计向量 带入步骤(3)进行下一时刻算法迭代,便可递推得出每一时刻的状态估计。
上述方法的关键技术在于:利用三角形可能性分布函数建模状态和观测方程中的有界噪声,利用取值为区间的元素及其信度赋值构成状态和观测噪声的证据。将它们带入状态方程和观测方程与实际观测值合成后,即可生成三种描述飞行器姿态的证据,利用证据组合规则实现这些证据的融合,通过区间映射工具实现三个证据在连续时刻的递归传播。对融合后证据所包含的信度赋值进行加权求和,即可得到飞行器姿态的估计值。
本发明所提方法不需满足状态和观测噪声概率分布已知的苛刻要求,而只限定噪声有界且可用简单的三角形函数描述。这增加了方法在实际姿态估计中的实用性,且与已有经典方法相比,也具有较高的估计精度。根据本发明方法编制的程序(编译环境LabVIEW,C++等)可以在四旋翼飞行器控制单元的处理器上运行,得到的姿态估计结果可以用于飞行器控制,提高控制的稳定性和机动性。
附图说明
图1是本发明方法的流程框图。
图2是本发明方法实施例中四旋翼飞行器姿态的各角度估计值曲线图。
图3是本发明方法实施例中四旋翼飞行器姿态的各角度估计值绝对误差曲线图。
具体实施方式
本发明提出的基于证据融合的四旋翼飞行器姿态估计方法,其流程图如图1所示,包括以下各步骤:
(1)建立四旋翼飞行器姿态的状态方程与观测方程,具体步骤如下
(1-1)建立四旋翼飞行器姿态的状态方程如式(1)所示:
xk+1=Axk+Q(v)(1)
式(1)中,k=0,1,2,3,…,表示时刻, 表示k时刻四旋翼飞行器姿态的四元数状态向量,A为状态转移矩阵
其为一个三角形可能性分布函数,其中,
(1-2)建立四旋翼飞行器姿态的观测方程如式(3)所示:
zk+1=h(xk+1)+R(w)(3)
其中,表示k+1时刻四旋翼飞行器姿态的观测向量,θk+1和ψk+1分别为四旋翼飞行器k+1时刻姿态的横滚角、俯仰角、偏航角的取值,状态向量xk+1到观测向量zk+1的转换函数为:
记r1、r2和r3分别表示横滚角、俯仰角、偏航角,则 为观测噪声函数向量, 是四旋翼飞行器姿态的观测噪声变化向量,并有
其为一个三角形可能性分布函数,其中
四旋翼飞行器四元数状态向量 中的各元素与观测向量 各元素之间的转换关系函数如式(6)所示
其中
(2)构造状态噪声函数向量Q(v)关于四元数状态向量xk中元素qi,k的证据 具体步骤如下:
(2-1)构造状态噪声函数向量Q(v)关于四元数状态向量xk元素qi,k的初始证据其中 表示关于k时刻四旋翼飞行器四元数状态向量 中元素qi,k(i=0,1,2,3)的状态噪声区间集合,为中的第j个取值为区间的元素,j=0,1,…,p-1,p∈[3,5],为它的左端点,为它的右端点,且有
其中αj=j/p,则有
(2-2)对步骤(2-1)获取的初始证据分别进行折扣计算,获得状态噪声函数向量Q(v)关于四元数状态向量xk元素qi,k的证据其中关于qi,k的状态噪声区间集合为
其中
这里折扣率εQ∈[0.03,0.08];
为便于理解,这里举例说明,这里p=3,i=0,分别取值 即 令εQ=0.05,λ=10,则由式(13-14)得如表1所示
表1的区间元素及信度赋值
(3)通过状态转移矩阵A获取k时刻关于向量xk的元素qi,k的预测证据其中下标k+1|k表示利用k时刻的状态对k+1时刻的状态进行预测,具体步骤如下:
(3-1)构建xk元素qi,k的状态估计证据具体步骤如下:
(a)当k=0时,取zk的元素r1,kr2,kr3,k分别带入式(7)-(10)得到
再利用式(6)得到
由此得到状态估计向量 则状态估计证据中的
亦即
(b)k≥1时,由递归计算得到状态估计向量则元素qi,k的状态估计证据为
亦即,
(3-2)通过状态转移矩阵A获取向量xk的元素qi,k的预测证据 其中
这里,Ai+1,i+1表示状态转移矩阵A中,第i+1行i+1列的元素,其中i=0,1,2,3,并有Ai+1,i+1=1,故并有即:
(4)通过观测方程获取k+1时刻关于四旋翼飞行器姿态的观测向量zk+1的元素rl,k+1的观测预测证据具体步骤如下:
(4-1)通过步骤(3)得到状态预测证据之后,分别抽取中的一个元素进行排列组合,共计产生(p+1)4个四元区间组,将这些四元区间组作为式(4)的输入量,利用MATLAB2010a软件中的工具箱intlab,分别得到关于rl,k+1的(p+1)4个区间,它们组成的区间集合为
并得到式(22)中每个区间的信度赋值组成的信度集合为
其中的为所对应式(4)输入的四元区间组中每个区间信度赋值的乘积,由式(22)和(23)可以构成关于rl,k+1的初始观测预测证据
为便于理解,这里举例说明,取
分别抽取中的一个元素进行排列组合,共计产生16个四元区间组,这里,选择r1横滚角的观测预测证据为例,将这些四元区间组作为式(4)第一行函数的输入量,利用MATLAB2010a软件中的工具箱intlab可得:
表2观测预测初始证据的区间元素及信度赋值
(4-2)将步骤(4-1)所得的进行简化后得到元素rl,k的观测预测证据 其中
式(24)中的为中信度赋值最大的那个区间,则式(25)中该区间的信度赋值若有多个区间信度赋值相等且最大,则将它们取并后构成将它们的信度相加后构成式(24)中的为剩余各区间取并后得到的区间,且这些区间的信度相加后构成
为便于理解,这里举例说明,由表2得到的初始证据化简得
(5)利用Dempster组合规则求出k+1时刻观测域的融合证据 具体步骤如下:
(5-1)构建zk+1元素rl,k+1的实时观测证据具体步骤如下
(a)构造观测噪声函数向量R(w)关于观测向量zk+1元素rl,k+1的初始证据其中 表示关于k时刻四旋翼飞行器观测向量zk中元素rl,k的观测噪声区间集合,为中的第j个取值为区间的元素,j=0,1,…,p-1,p∈[3,5],为它的左端点,为它的右端点,且有
其中αj=j/p,则有
(b)对步骤(a)获取的初始证据分别进行折扣计算,获得观测噪声向量函数R(w)关于观测向量zk+1元素rl,k+1的证据其中关于rl,k+1的观测噪声区间集合为
其中
这里折扣率εR∈[0.03,0.08];
为便于理解,这里举例说明,设定p=3,l=1,元素取值分别为 则 令εR=0.05,λ=10,则由式(26-29)得如表3所示:
表3的区间元素及信度赋值
(c)当从陀螺仪观测到向量 则构建元素rl,k+1的观测证据 为
亦即:
(5-2)将得到的观测证据与观测预测证据利用Dempster组合规则进行证据融合,得到观测域的融合证据
令 其信度赋值为 令 其信度赋值为
利用Dempster组合规则将和组合后得到
其中ζ=1,2,…,n,n≤2·(p+1)表示通过Ag∩Ch共计生成了n个不同的区间 则观测域的融合证据 为
为便于理解,这里举例说明,具体数据见表4-6所示:
表4观测预测证据的区间元素及信度赋值
表5观测证据的区间元素及信度赋值
Dempster组合的具体方式如下:
X1=[-0.0574,0.0798]∩[-0.1853,0.1648]=[-0.0574,0.0798],
X2=[-0.0574,0.0798]∩[-0.0628,0.0423]=[-0.0574,0.0423],
X3=[-0.0574,0.0798]∩[-0.0453,0.0248]=[-0.0453,0.0248],
X4=[-0.0574,0.0798]∩[-0.0278,0.0073]=[-0.0278,0.0073],
X5=[-0.2936,0.3204]∩[-0.1853,0.1648]=[-0.1853,0.1648],
X6=[-0.2936,0.3204]∩[-0.0628,0.0423]=[-0.0628,0.0423],
X7=[-0.2936,0.3204]∩[-0.0453,0.0248]=[-0.0453,0.0248],
X8=[-0.2936,0.3204]∩[-0.0278,0.0073]=[-0.0278,0.0073],
这里X3=X7,X4=X8故
由此可得观测域的融合证据见表6所示:
表6观测域融合证据的区间元素及信度赋值
(6)通过观测方程的逆运算获得k+1时刻关于向量xk+1的元素qi,k+1的状态域的新证据具体步骤如下:
(6-1)由步骤(5)得到的观测域的融合证据之后,分别抽取中的一个元素进行排列组合,共计产生n3个三元区间组,将这些三元区间组分别作为式(7-10)的输入量,利用MATLAB2010a软件中的工具箱intlab,得到关于qi,k+1的n3个区间,它们组成的区间集合为
并可得到式(33)中每个区间的信度赋值组成的信度集合为
其中的为所对应式(7-10)输入的三元区间组中每个区间信度赋值的乘积,由式(33)和(34)可以构成关于qi,k+1的状态域的初始新证据
(6-2)将步骤(6-1)所得的进行简化后得到关于元素qi,k+1的状态域的初始新证据其中
式(35)中的为中信度赋值最大的那个区间,则式(36)中该区间的信度赋值若有多个区间信度赋值相等且最大,则将它们取并后构成将它们的信度相加后构成式(35)中的为剩余各区间取并后得到的区间,且这些区间的信度相加后构成
(7)利用Dempster组合规则求出k+1时刻状态估计的融合证据
将步骤(6)中得到的关于向量xk+1的元素qi,k+1的状态域的新证据和步骤(3)中得到的关于向量xk的元素qi,k的预测证据利用Dempster组合规则进行证据融合,得到k+1时刻状态估计的融合证据
令 其信度赋值为 令 其信度赋值为
利用Dempster组合规则将组合后得到
其中ξ=1,2,…,m,m≤2·(p+1)表示通过Gg∩Hh共计生成m个不同的区间,则状态估计的融合证据为
中对m个不同区间的信度赋值,可以由式(37)得到;
(8)获得k+1时刻状态估计向量的元素的状态估计值:
其中
将获得的k+1时刻状态估计向量 带入步骤(3)进行下一时刻算法迭代,便可递推得出每一时刻的状态估计。
为便于理解,这里举例说明,获得的k+1时刻qi状态估计值如表7所示
表7关于qi融合证据的区间元素及信度赋值
经式(39)得:
以下结合附图,详细介绍本发明方法的实施例:
本发明方法的流程图如图1所示,核心部分是:对于状态噪声与观测噪声证据的构建,以及通过利用MATLAB2010a软件中的工具箱intlab中区间映射实现证据的传递,以及利用Dempster组合规则进行证据融合,然后将获得的融合证据进行加权求和后得到最终的状态估计值,整个算法可以迭代运行。
以下结合四旋翼飞行器模型及采集到的观测数据,详细介绍本发明方法的各个步骤,并通过实验结果验证本发明提出的基于证据融合的四旋翼飞行器姿态估计方法优于GhaliaNassreddine提出的经典的基于置信函数的前反向传播算法。
1、建立四旋翼飞行器姿态的状态方程与观测方程
xk+1=Axk+Q(v)
zk+1=h(xk+1)+R(w)k=0,1,2,3…
这里,xk从k=0时刻状态开始,Q(v)与R(w)函数向量中的元素分别可以表示为以下形式的三角形可能性分布函数:
2、构造状态噪声函数向量Q(v)关于四元数状态向量xk中元素qi,k的证据
这里p=3,分别取值 即 令εQ=0.05,λ=10,则由式(13-14)得如表8所示:
表8的区间元素及信度赋值
3、通过状态转移矩阵A获取k时刻关于向量xk的元素qi,k的预测证据这里给出k=0时刻开始的迭代过程。
当k=0时,由式(15-16)分别可以得到各元素的状态估计证据如表9所示:
表90时刻状态变量证据的区间元素及信度赋值
3-2)通过状态转移矩阵A获取向量xk的元素qi,k的预测证据当k=0时,由式(19-21)我们分别可以得到xk中各元素的状态预测证据如表10所示:
表100时刻状态预测证据的区间元素及信度赋值
4、通过观测方程获取k时刻关于四旋翼飞行器的观测向量zk的元素rl,k的观测预测证据
经式(22-25)简化后,得到观测预测证据如表11所示:
表11观测预测证据的区间元素及信度赋值
5、利用Dempster组合规则求出k+1时刻观测域的融合证据
这里p=3,分别取值 即 令εR=0.05,λ=10,则由式(26-29)得如表12所示:
表12的区间元素及信度赋值
当k=0时,经式(30-31)可以得到元素rl,k+1的观测证据如表13所示:
表13观测证据的区间元素及信度赋值
经式(32)利用Dempster组合规则将观测证据与观测预测证据进行证据融合可以得到观测域的融合证据如表14-16所示:
表14关于r1的观测域融合证据的区间元素及信度赋值
表15关于r2的观测域融合证据的区间元素及信度赋值
表16关于r3的观测域融合证据的区间元素及信度赋值
6、通过观测方程的逆运算获得k+1时刻关于向量xk+1的元素qi,k+1的状态域的新证据
经式(33-36)简化后,得到状态域的新证据如表17所示:
表17状态域新证据的区间元素及信度赋值
7、利用Dempster组合规则求出k+1时刻状态估计的融合证据
经式(37)利用Dempster组合规则将状态域新证据与状态预测证据进行证据融合可以得到状态域的融合证据如表18-21所示:
表18关于q0的状态域融合证据的区间元素及信度赋值
表19关于q1的状态域融合证据的区间元素及信度赋值
表20关于q2的状态域融合证据的区间元素及信度赋值
表21关于q3的状态域融合证据的区间元素及信度赋值
8、由表18-21经式(38-39)获得k+1时刻状态估计向量的元素的状态估计值如表22所示:
表22状态变量估计值
按照此方法,将获得的k+1时刻状态估计向量 带入步骤(3)进行下面每一时刻的算法迭代,最终得到每个时刻的状态估计值这里将得到的每个时刻的状态估计值代入式(4)得到用于四旋翼姿态控制的各姿态角的估计值如图2所示,其中理想的真实值在每个时刻表现在四旋翼飞行器姿态的角度值均为0,用“—”表示(k=0,1,2,…,20),本发明方法给出的估计值用“--*--”表示,GhaliaNassreddine经典算法给出的估计值用“-◇-”表示,陀螺仪测得的观测值用“-+-”表示。图3给出本发明方法、GhaliaNassreddine经典算法和直接观测之间绝对误差的比较。表23给出了三种方法在二十一步的估计中绝对误差的均值,可以看出本发明方法的估计精度要高于其他方法。
表23四旋翼飞行器姿态各角度估计的绝对误差
Claims (1)
1.基于证据融合的四旋翼飞行器姿态估计方法,其特征在于该方法包括以下各步骤:
(1)建立四旋翼飞行器姿态的状态方程与观测方程,具体是:
(1-1)建立四旋翼飞行器姿态的状态方程如式(1)所示:
xk+1=Axk+Q(v)(1)
式(1)中,k为自然数,表示时刻,xk=[q0,kq1,kq2,kq3,k]T表示k时刻四旋翼飞行器姿态的四元数状态向量,A为状态转移矩阵
为状态噪声函数向量,为四元数状态的噪声变化向量,并有
其为一个三角形可能性分布函数,其中,
(1-2)建立四旋翼飞行器姿态的观测方程如式(3)所示:
zk+1=h(xk+1)+R(w)(3)
其中,表示k+1时刻四旋翼飞行器姿态的观测向量,θk+1和ψk+1分别为四旋翼飞行器k+1时刻姿态的横滚角、俯仰角、偏航角的取值,状态向量xk+1到观测向量zk+1的转换函数为:
记r1、r2和r3分别表示横滚角、俯仰角、偏航角,则为观测噪声函数向量,是四旋翼飞行器姿态的观测噪声变化向量,并有
其为一个三角形可能性分布函数,其中
四旋翼飞行器姿态的四元数状态向量xk=[q0,kq1,kq2,kq3,k]T中的各元素与观测向量zk=[r1,kr2,kr3,k]T各元素之间的转换关系函数如式(6)所示
其中
(2)构造状态噪声函数向量Q(v)关于四元数状态向量xk中元素qi,k的证据具体是:
(2-1)构造状态噪声函数向量Q(v)关于四元数状态向量xk元素qi,k的初始证据其中表示关于k时刻四旋翼飞行器姿态的四元数状态向量xk=[q0,kq1,kq2,kq3,k]T中元素qi,k的状态噪声区间集合,为中的第j个取值为区间的元素,j=0,1,…,p-1,p∈[3,5],为它的左端点,为它的右端点,且有
其中αj=j/p,则有
是中各区间元素的信度组成的集合,并有
其元素的取值如式(12)所示
(2-2)对步骤(2-1)获取的初始证据分别进行折扣计算,获得状态噪声函数向量Q(v)关于四元数状态向量xk元素qi,k的证据其中关于qi,k的状态噪声区间集合为
为中各区间元素的信度组成的集合
其中
这里折扣率εQ∈[0.03,0.08];
(3)通过状态转移矩阵A获取k时刻关于向量xk的元素qi,k的预测证据其中下标k+1|k表示利用k时刻的状态对k+1时刻的状态进行预测,具体是:
(3-1)构建xk元素qi,k的状态估计证据具体步骤如下:
(a)当k=0时,取zk的元素r1,kr2,kr3,k分别带入式(7)-(10)得到
再利用式(6)得到
由此得到状态估计向量则状态估计证据中的
亦即
(b)k≥1时,由递归计算得到状态估计向量则元素qi,k的状态估计证据为
亦即
(3-2)通过状态转移矩阵A获取向量xk的元素qi,k的预测证据其中
这里,Ai+1,i+1表示状态转移矩阵A中,第i+1行i+1列的元素,其中i=0,1,2,3,并有Ai+1,i+1=1,故并有即:
(4)通过观测方程获取k+1时刻关于四旋翼飞行器姿态的观测向量zk+1的元素rl,k+1的观测预测证据具体是:
(4-1)通过步骤(3)得到状态预测证据之后,分别抽取中的一个元素进行排列组合,共计产生(p+1)4个四元区间组,将这些四元区间组作为式(4)的输入量,利用MATLAB2010a软件中的工具箱intlab,分别得到关于rl,k+1的(p+1)4个区间,它们组成的区间集合为
并得到式(22)中每个区间的信度赋值组成的信度集合为
其中的为所对应式(4)输入的四元区间组中每个区间信度赋值的乘积,由式(22)和(23)可以构成关于rl,k+1的初始观测预测证据
(4-2)将步骤(4-1)所得的进行简化后得到元素rl,k+1的观测预测证据其中
式(24)中的为中信度赋值最大的那个区间,则式(25)中该区间的信度赋值若有多个区间信度赋值相等且最大,则将它们取并后构成将它们的信度相加后构成式(24)中的为剩余各区间取并后得到的区间,且这些区间的信度相加后构成
(5)利用Dempster组合规则求出k+1时刻观测域的融合证据具体是:
(5-1)构建zk+1元素rl,k+1的实时观测证据具体步骤如下:
(a)构造观测噪声函数向量R(w)关于观测向量zk+1元素rl,k+1的初始证据其中表示关于k时刻四旋翼飞行器姿态观测向量zk中元素rl,k的观测噪声区间集合,为中的第j个取值为区间的元素,j=0,1,…,p-1,p∈[3,5],为它的左端点,为它的右端点,且有
其中αj=j/p,则有
是中各区间元素的信度组成的集合,并有
其元素的取值如式(27)所示
(b)对步骤(a)获取的初始证据分别进行折扣计算,获得观测噪声向量函数R(w)关于观测向量zk+1元素rl,k+1的证据其中关于rl,k+1的观测噪声区间集合为
为中各区间元素的信度赋值组成的集合
其中
这里折扣率εR∈[0.03,0.08];
(c)当从陀螺仪观测到向量zk+1=[r1,k+1r2,k+1r3,k+1]T,则构建元素rl,k+1的观测证据为
亦即:
(5-2)将得到的观测证据与观测预测证据利用Dempster组合规则进行证据融合,得到观测域的融合证据
令其信度赋值为令 其信度赋值为
利用Dempster组合规则将和组合后得到
其中表示通过Ag∩Ch共计生成了n个不同的区间则观测域的融合证据为
中对n个不同区间的信度赋值,由式(32)得到;
(6)通过观测方程的逆运算获得k+1时刻关于向量xk+1的元素qi,k+1的状态域的新证据具体是:
(6-1)由步骤(5)得到的观测域的融合证据之后,分别抽取中的一个元素进行排列组合,共计产生n3个三元区间组,将这些三元区间组分别作为式(7)-(10)的输入量,利用MATLAB2010a软件中的工具箱intlab,得到关于qi,k+1的n3个区间,它们组成的区间集合为
并可得到式(33)中每个区间的信度赋值组成的信度集合为
其中的为所对应式(7)-(10)输入的三元区间组中每个区间信度赋值的乘积,由式(33)和(34)可以构成关于qi,k+1的状态域的初始新证据
(6-2)将步骤(6-1)所得的进行简化后得到关于元素qi,k+1的状态域的初始新证据其中
式(35)中的为中信度赋值最大的那个区间,则式(36)中该区间的信度赋值若有多个区间信度赋值相等且最大,则将它们取并后构成将它们的信度相加后构成式(35)中的为剩余各区间取并后得到的区间,且这些区间的信度相加后构成
(7)利用Dempster组合规则求出k+1时刻状态估计的融合证据
将步骤(6)中得到的关于向量xk+1的元素qi,k+1的状态域的新证据和步骤(3)中得到的关于向量xk的元素qi,k的预测证据利用Dempster组合规则进行证据融合,得到k+1时刻状态估计的融合证据
令其信度赋值为令 其信度赋值为
利用Dempster组合规则将和组合后得到
其中ξ=1,2,…,m,m≤2·(p+1)表示通过Gg∩Hh共计生成m个不同的区间,则状态估计的融合证据为
中对m个不同区间的信度赋值,由式(37)得到;
(8)获得k+1时刻状态估计向量的元素的状态估计值:
其中,
然后将获得的k+1时刻状态估计向量带入步骤(3)进行下一时刻算法迭代,便可递推得出每一时刻的状态估计。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310641284.8A CN103697890B (zh) | 2013-12-03 | 2013-12-03 | 基于证据融合的四旋翼飞行器姿态估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201310641284.8A CN103697890B (zh) | 2013-12-03 | 2013-12-03 | 基于证据融合的四旋翼飞行器姿态估计方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN103697890A CN103697890A (zh) | 2014-04-02 |
CN103697890B true CN103697890B (zh) | 2016-06-22 |
Family
ID=50359502
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201310641284.8A Expired - Fee Related CN103697890B (zh) | 2013-12-03 | 2013-12-03 | 基于证据融合的四旋翼飞行器姿态估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103697890B (zh) |
Families Citing this family (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109871397A (zh) * | 2019-02-28 | 2019-06-11 | 重庆零壹空间航天科技有限公司 | 一种运载火箭测发控测试实时监判方法、*** |
Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7047161B1 (en) * | 2003-06-10 | 2006-05-16 | Lockheed Martin Corporation | Virtual sensor for data and sensor fusion |
CN102830622A (zh) * | 2012-09-05 | 2012-12-19 | 北京理工大学 | 一种四旋翼飞行器自抗扰自动飞行控制方法 |
-
2013
- 2013-12-03 CN CN201310641284.8A patent/CN103697890B/zh not_active Expired - Fee Related
Patent Citations (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7047161B1 (en) * | 2003-06-10 | 2006-05-16 | Lockheed Martin Corporation | Virtual sensor for data and sensor fusion |
CN102830622A (zh) * | 2012-09-05 | 2012-12-19 | 北京理工大学 | 一种四旋翼飞行器自抗扰自动飞行控制方法 |
Non-Patent Citations (4)
Title |
---|
基于证据折扣度修正和层次聚类的冲突证据合成;周峰;《中国优秀硕士学位论文全文数据库 信息科技辑》;20130615(第6期);I140-125 * |
基于证据理论的动态融合方法研究;史健;《中国优秀硕士学位论文全文数据库 信息科技辑 》;20131116(第S1期);I140-102 * |
基于证据理论的状态估计方法及其在液位估计中的应用;徐晓滨,史健,文成林;《计算机与应用化学》;20120728(第7期);第801-806页 * |
微小型四旋翼飞行器多信息非线性融合导航方法及实现;刘建业,贾文峰,赖际舟,吕品;《南京航空航天大学学报》;20131031;第45卷(第5期);第575-581页 * |
Also Published As
Publication number | Publication date |
---|---|
CN103697890A (zh) | 2014-04-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
Spence et al. | Large scale reliability-based design optimization of wind excited tall buildings | |
CN103900610B (zh) | 基于灰色小波神经网络的mems陀螺随机误差预测方法 | |
CN102322861B (zh) | 一种航迹融合方法 | |
CN107451619A (zh) | 一种基于感知生成对抗网络的小目标检测方法 | |
CN103235877B (zh) | 机器人控制软件模块划分方法 | |
CN106197408A (zh) | 一种基于因子图的多源导航信息融合方法 | |
CN101232180A (zh) | 一种配电***负荷模糊建模装置及方法 | |
CN103065037B (zh) | 非线性***基于分散式容积信息滤波的目标跟踪方法 | |
CN105635963A (zh) | 多智能体分布式协同定位方法 | |
CN105260786A (zh) | 一种电力推进***仿真可信度评估模型综合优化方法 | |
CN110347932A (zh) | 一种基于深度学习的跨网络用户对齐方法 | |
CN108763377A (zh) | 基于卫星故障诊断多源遥测大数据特征提取预处理方法 | |
CN115577436B (zh) | 一种求解不确定结构风致振动响应的组合深度学习方法 | |
CN104008304B (zh) | 一种乏信息多传感器神经网络‑熵测量不确定度评定方法 | |
Huang et al. | SDARE: A stacked denoising autoencoder method for game dynamics network structure reconstruction | |
CN109325128A (zh) | 一种机动目标的跟踪方法及*** | |
CN107978152A (zh) | 一种用于交通子网络出行矩阵估算的最大熵方法 | |
Liu et al. | Atvio: Attention guided visual-inertial odometry | |
CN110532665A (zh) | 一种固定航线任务下的移动对象动态轨迹预测方法 | |
CN103296995B (zh) | 任意维高阶(≥4阶)无味变换与无味卡尔曼滤波方法 | |
CN106249590A (zh) | 一体化自适应纳卫星姿态确定的方法 | |
Winkler et al. | High-fidelity molecular dynamics trajectory reconstruction with bi-directional neural networks | |
CN103697890B (zh) | 基于证据融合的四旋翼飞行器姿态估计方法 | |
Han et al. | Two new Voronoi cell finite element models for fracture simulation in porous material under inner pressure | |
Butter et al. | Kicking it Off (-shell) with Direct Diffusion |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
C14 | Grant of patent or utility model | ||
GR01 | Patent grant | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20160622 Termination date: 20161203 |