基于改进天牛搜索的无人机转弯速率和状态估计方法与流程

1.本发明涉及未知系统模型参数下的状态估计理论技术领域，特别是指一种基于改进天牛搜索的无人机转弯速率和状态估计方法。


背景技术：

2.最优状态估计理论在无人机导航及控制中具有重要作用。当无人机在固定水平面执行机动任务时，由于其转弯速率通常是随机未知的，使得未知转弯速率机动运动下无人机状态估计成为无人机姿态跟踪中的热点和难点问题。针对未知转弯速率的无人机状态估计问题，通常假设：(1)将无人机的转弯速率视为常量，即忽略无法测量的转弯率对系统状态估计的影响；(2)将无人机的转弯速率视为噪声干扰，即通过随机噪声干扰来表示转弯速率。上述假设对于具有高机动性或频繁转弯下的无人机状态估计问题，通常会使估计精度下降甚至发散，针对以上问题，据发明人所知，还没有有效的解决方案。


技术实现要素：

3.针对上述背景技术中存在的不足，本发明提出了一种基于改进天牛搜索的无人机转弯速率和状态估计方法，解决了高机动性或频繁转弯场景时，无人机在固定水平面以未知转弯率作业时的状态估计问题。
4.本发明的技术方案是这样实现的：
5.一种基于改进天牛搜索的无人机转弯速率和状态估计方法，基于无人机空中交通管制场景，无人机在固定水平面上以未知的转弯速率执行转弯运动，其步骤如下：
6.步骤一：利用离散非线性系统方程构建转弯运动系统的数学模型；
7.步骤二：定义转弯运动系统的初始状态估计值为x
k|k
，初始估计误差协方差为p
k|k
，初始转弯率ωk＝ω
k,j＝1
，k时刻的初始转弯速率表示为ω
k,1
，其中j＝1,2,3,......,j表示天牛寻优次数；
8.步骤三：基于步骤二中的初始状态估计值x
k|k
和初始估计误差协方差p
k|k
计算k 1时刻的转弯运动系统的状态预测值、测量预测值及状态预测误差协方差值；
9.步骤四：基于随机递减惯性权重天牛须搜索原理，将步骤二中的k时刻的初始转弯速率ω
k,1
经多次迭代得到k时刻的最优转弯速率ω
k,j
；
10.步骤五：根据步骤四中的k时刻的最优转弯率ω
k,j
得到优化更新后的k 1时刻的状态预测值x
k 1|k
、测量预测值y
k 1|k
及状态预测误差协方差p
k 1|k
；
11.步骤六：基于步骤五中的k 1时刻的状态预测值x
k 1|k
、测量预测值y
k 1|k
及状态预测误差协方差p
k 1|k
计算无人机在k 1时刻的状态估计更新值和状态估计误差协方差更新值；
12.步骤七：将步骤六中的无人机在k 1时刻的状态估计更新值和状态估计误差协方差更新值，作为下一时刻的初始条件，返回步骤二继续执行以完成下一时刻转弯速率和状态的估计更新过程。
13.所述转弯运动系统的数学模型为：
14.x
k 1
＝ξkxk wkꢀꢀꢀ
(1)；
15.yk＝h(xk) vkꢀꢀꢀ
(2)；
16.其中，x
k 1
表示下一时刻系统状态量，表示当前时刻系统状态量，表示系统状态转移矩阵，wk表示均值为0、方差为的高斯白噪声干扰，表示系统测量转移矩阵，k表示采样时刻，ωk为转弯速率，εk无人机北向位置，ηk表示无人机东向位置，无人机北向速度，表示无人机东向速度，δt表示采样周期，yk＝[rk，θk]
t
表示系统测量量，rk表示无人机距离观测点的距离，θk表示测量的目标角度，vk表示均值为0、方差为的高斯白噪声干扰，t表示转置。
[0017]
所述k 1时刻的转弯运动系统的状态预测值、测量预测值及状态预测误差协方差值的计算方法为：
[0018]
s3.1、基于cholesky分解，计算初始状态容积点
[0019][0020][0021]
其中，p
k|k
表示初始状态估计误差协方差，x
k|k
表示状态估计初始值，s
k|k
表示初始估计误差协方差的平方根因子，ξi表示单位初始容积点，2n表示容积点个数；
[0022]
单位初始容积点ξi表示为：
[0023][0024]
其中，[1]为单位矢量点；
[0025]
s3.2、计算k 1时刻的状态预测值x
k 1|k
、测量预测值y
k 1|k
及状态预测误差协方差p
k 1|k
：
[0026][0027][0028][0029]
其中，测量预测容积点表示为：
[0030]
[0031]
其中，表示k 1时刻的状态预测容积点，表示为：
[0032][0033]
其中，ω
k,j
为k时刻的最优转弯速率。
[0034]
所述k时刻的最优转弯速率ω
k,j
的获得方法为：
[0035]
s4.1、用k时刻第j次的单位随机向量表示天牛朝向：
[0036][0037]
其中，rnd(
·
)表示随机向量，s表示空间维数；
[0038]
s4.2、利用s4.1中随机天牛朝向及初始转弯率ω
k＝1,j＝1
，建立天牛左右须坐标，其中天牛的左须坐标和右须坐标分别为：
[0039][0040][0041]
其中，ω
k,j
为k时刻第j次天牛的质心坐标，d
k,j
为k时刻第j次迭代过程中两须之间的距离；
[0042]
两须之间的距离d
k,j
的更新方式为：
[0043]dk,j 1
＝d
edk,j
0.01
ꢀꢀꢀ
(14)；
[0044]
其中，de为d
k,j
的衰减因子，d
k,j 1
为k时刻第j 1次迭代过程中两须之间的距离；
[0045]
s3.3、引入随机递减惯性权重，建立转弯速率更新迭代模型：
[0046][0047]
其中，表示右须适应度函数的值，表示左须适应度函数的值，sign(
·
)表示符号函数，δ
k,j
表示k时刻第j次天牛搜索的步长，c表示随机递减惯性权重。
[0048]
所述随机递减惯性权重c的计算方法为：
[0049]
c＝μ
min
(μ
max-μ
min
)a σb
ꢀꢀꢀ
(16)；
[0050]
其中，μ
min
是随机递减惯性权重的最小值，μ
max
是随机递减惯性权重的最大值，a为服从[0，1]均匀分布的随机数，b为服从正态分布的随机数，σ用来衡量随机变量权重与数学期望之间的偏离程度；
[0051]
k时刻第j次天牛搜索的步长δ
k,j
的更新方法为：
[0052]
δ
k,j 1
＝δeδ
k,j
ꢀꢀꢀ
(17)；
[0053]
其中，δe表示步长δ
k,j
的衰减因子；
[0054]
适应度函数f(ω
k,j
)的计算方法为：
[0055]
f(ω
k,j
)＝(y
k 1-y
k 1|k
)(y
k 1-y
k 1|k
)
t
ꢀꢀꢀ
(18)；
[0056]
其中，y
k 1
为k时刻的测量值，y
k 1|k
为k时刻的测量预测值。
[0057]
所述根据步骤四中的k时刻的最优转弯率ω
k,j
得到优化更新后的k 1时刻的状态预测值x
k 1|k
、测量预测值y
k 1|k
及状态预测误差协方差p
k 1|k
的方法为：将k时刻最优转弯率ω
k,j
代入公式(10)得到状态预测传播容积点同时，基于公式(6)～(8)得到优化更新后的状态预测值x
k 1|k
、测量预测值y
k 1|k
及状态预测误差协方差p
k 1|k
。
[0058]
所述无人机在k 1时刻的状态估计更新值和状态估计误差协方差更新值的计算方法为：
[0059]
s6.1、基于cholesky分解，计算转弯运动系统的状态预测误差协方差的平方根因子s
k 1|k
：
[0060][0061]
s6.2、计算新息协方差和预测误差互协方差
[0062][0063][0064]
s6.3、基于kalman增益更新公式k
k 1
，计算得到状态估计更新值x
k 1|k 1
，以及状态估计误差协方差更新值p
k 1|k 1
：
[0065][0066]
x
k 1|k 1
＝x
k 1|k
k
k 1
(y
k 1-y
k 1|k
)
ꢀꢀꢀ
(23)；
[0067][0068]
与现有技术相比，本发明产生的有益效果为：
[0069]
1)通过设置适当的适应度函数，利用改进的基于随机递减惯性权重天牛搜索算法，实现对未知转弯速率的自适应寻优，进而获取更加准确的转弯率，实现对无人机未知转弯速率的优化估计。
[0070]
2)基于初始转弯率、初始状态和初始估计误差协方差，得到无人机初始的状态预测值、状态预测误差协方差值和测量预测值，基于cholesky分解得到的容积点计算方法克服了系统非线性的影响，具有较高的计算精度。
[0071]
3)通过随机递减惯性权重的引入，很好地解决了天牛搜索算法在应对高维复杂函数时易陷入局部最优的问题，极大地改善了天牛搜索算法的性能。提出基于随机递减惯性权重天牛搜索的无人机转弯速率优化估计策略，通过多次迭代优化，得到无人机转弯速率最优值。
[0072]
4)本发明可以解决无人机在固定水平面上高机动性或频繁转弯场景时，以未知转弯率机动运动下的状态估计问题。通过基于随机递减惯性权重的天牛搜索算法，克服传统天牛算法容易陷入局部最优的情况；同时克服传统目标跟踪算法无法测量转弯率和转弯率建模不准确造成的系统不确定问题，对于未知转弯率下机动转弯问题具有较好的估计效果。
附图说明
[0073]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0074]
图1为本发明的流程图。
[0075]
图2为本发明的数据流向图。
具体实施方式
[0076]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0077]
如图1和图2所示，本发明实施例提供了一种基于改进天牛搜索的无人机转弯速率和状态估计方法，基于一个典型的无人机空中交通管制场景，无人机在固定水平面上以未知的转弯速率执行转弯运动，其步骤如下：
[0078]
步骤一：利用离散非线性系统方程构建转弯运动系统的数学模型；北东坐标系下系统模型为：
[0079]
x
k 1
＝ξkxk wkꢀꢀꢀ
(1)；
[0080]
yk＝h(xk) vkꢀꢀꢀ
(2)；
[0081]
其中，x
k 1
表示下一时刻系统状态量，表示当前时刻系统状态量，表示系统状态转移矩阵，wk表示均值为0、方差为的高斯白噪声干扰，表示系统测量转移矩阵，k表示采样时刻，ωk为转弯速率，εk无人机北向位置，ηk表示无人机东向位置，无人机北向速度，表示无人机东向速度，δt表示采样周期，yk＝[rk，θk]
t
表示系统测量量，rk表示无人机距离观测点的距离，θk表示测量的目标角度，vk表示均值为0、方差为的高斯白噪声干扰，t表示转置。
[0082]
步骤二：定义转弯运动系统的初始状态估计值为x
k|k
，初始估计误差协方差为p
k|k
，初始转弯率ωk＝ω
k,j＝1
，k时刻的初始转弯速率表示为ω
k,1
，其中j＝1,2,3,......,j表示天牛寻优次数。
[0083]
步骤三：基于步骤二中的初始状态估计值x
k|k
和初始估计误差协方差p
k|k
计算k 1时刻的转弯运动系统的状态预测值、测量预测值及状态预测误差协方差值；具体计算方法为：
[0084]
s3.1、基于cholesky分解，计算初始状态容积点
[0085][0086][0087]
其中，p
k|k
表示初始状态估计误差协方差，x
k|k
表示状态估计初始值，s
k|k
表示初始估计误差协方差的平方根因子，ξi表示单位初始容积点，2n表示容积点个数。
[0088]
单位初始容积点ξi表示为：
[0089][0090]
其中，[1]为单位矢量点。
[0091]
s3.2、计算k 1时刻的状态预测值x
k 1|k
、测量预测值y
k 1|k
及状态预测误差协方差p
k 1|k
：
[0092][0093][0094][0095]
其中，测量预测容积点表示为：
[0096][0097]
其中，表示k 1时刻的状态预测容积点，表示为：
[0098][0099]
其中，ω
k,j
为k时刻的最优转弯速率。
[0100]
步骤四：基于随机递减惯性权重天牛须搜索原理，将步骤二中的k时刻的初始转弯速率ω
k,1
经多次迭代得到k时刻的最优转弯速率ω
k,j
。
[0101]
所述k时刻的最优转弯速率ω
k,j
的获得方法为：
[0102]
s4.1、用k时刻第j次的单位随机向量表示天牛朝向：
[0103][0104]
其中，rnd(
·
)表示随机向量，s表示空间维数。
[0105]
s4.2、利用s4.1中随机天牛朝向及初始转弯率ω
k＝1,j＝1
，建立天牛左右须坐标，其中天牛的左须坐标和右须坐标分别为：
[0106][0107][0108]
其中，ω
k,j
为k时刻第j次天牛的质心坐标，d
k,j
为k时刻第j次迭代过程中两须之间的距离。
[0109]
两须之间的距离d
k,j
的更新方式为：
[0110]dk,j 1
＝d
edk,j
0.01
ꢀꢀꢀ
(14)；
[0111]
其中，de为d
k,j
的衰减因子，d
k,j 1
为k时刻第j 1次迭代过程中两须之间的距离。
[0112]
s3.3、引入随机递减惯性权重，建立转弯速率更新迭代模型：
[0113][0114]
其中，表示右须适应度函数的值，表示左须适应度函数的值，sign(
·
)表示符号函数，δ
k,j
表示k时刻第j次天牛搜索的步长，c表示随机递减惯性权重。
[0115]
所述随机递减惯性权重c的计算方法为：
[0116]
c＝μ
min
(μ
max-μ
min
)a σb
ꢀꢀꢀ
(16)；
[0117]
其中，μ
min
是随机递减惯性权重的最小值，μ
max
是随机递减惯性权重的最大值，a为服从[0，1]均匀分布的随机数，b为服从正态分布的随机数，σ用来衡量随机变量权重w与数学期望之间的偏离程度，目的是为了控制权重的取值，使w的选取更有利于权重方向变化。
[0118]
k时刻第j次天牛搜索的步长δ
k,j
的更新方法为：
[0119]
δ
k,j 1
＝δeδ
k,j
ꢀꢀꢀ
(17)；
[0120]
其中，δe表示步长δ
k,j
的衰减因子。
[0121]
由最小均方根误差原理得到适应度函数f(ω
k,j
)的计算方法为：
[0122]
f(ω
k,j
)＝(y
k 1-y
k 1|k
)(y
k 1-y
k 1|k
)
t
ꢀꢀꢀ
(18)；
[0123]
其中，y
k 1
为k时刻的测量值，y
k 1|k
为k时刻的测量预测值。在天牛搜索算法实际运行的时候，当迭代60次左右，便能非常接近于全局最小值的点，所以基于实际天牛搜索经验，以迭代60次作为天牛搜索终止的指标，即令j＝60。若j＜60,则返回到s3继续进行迭代。若j＝60，即j＝j时,输出寻优后的转弯率，记为ω
k,j
。同时将k时刻的最优转弯速率ω
k,j
作为k 1时刻的初始转弯率。
[0124]
步骤五：根据步骤四中的k时刻的最优转弯率ω
k,j
得到优化更新后的k 1时刻的状态预测值x
k 1|k
、测量预测值y
k 1|k
及状态预测误差协方差p
k 1|k
；将k时刻最优转弯率ω
k,j
代入公式(10)得到状态预测传播容积点同时，基于公式(6)～(8)得到优化更新后的状态预测值x
k 1|k
、测量预测值y
k 1|k
及状态预测误差协方差p
k 1|k
。
[0125]
步骤六：基于步骤五中的k 1时刻的状态预测值x
k 1|k
、测量预测值y
k 1|k
及状态预测误差协方差p
k 1|k
计算无人机在k 1时刻的状态估计更新值和状态估计误差协方差更新值；具体计算方法为：
[0126]
s6.1、基于cholesky分解，计算转弯运动系统的状态预测误差协方差的平方根因子s
k 1|k
：
[0127][0128]
s6.2、计算新息协方差和预测误差互协方差
[0129][0130][0131]
s6.3、基于kalman增益更新公式k
k 1
，计算得到状态估计更新值x
k 1|k 1
，以及状态估计误差协方差更新值p
k 1|k 1
：
[0132][0133]
x
k 1|k 1
＝x
k 1|k
k
k 1
(y
k 1-y
k 1|k
)
ꢀꢀꢀ
(23)；
[0134][0135]
步骤七：将步骤六中的无人机在k 1时刻的状态估计更新值和状态估计误差协方差更新值，作为下一时刻的初始条件，返回步骤二继续执行以完成下一时刻转弯速率和状态的估计更新过程。
[0136]
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。