本发明涉及一种多波束测速信息融合方法及系统，属于航天器制导导航控制领域。
背景技术：
对于着陆航天器来说，测速修正是提高导航系统精度的一个重要手段。为了获得完备的三维速度信息，一般需要安装三个非共面测速波束。为了提高系统的可靠性，有的着陆器上可能安装有不同体制的测速敏感器，而每个测速敏感器的波束至少有3个，这使得同一修正周期时可供使用的测速波束多于3个，测速信息出现了冗余，如何选择和使用测速波束就成为这类着陆器的一个新的问题。
嫦娥三号对于超过3个测速波束的处理方式是预先指定波束的优先级，使用时只挑选优先级最高的3个有效波束(嫦娥三号着陆器动力下降的自主导航，控制理论与应用，2014,12)。实际上各个测量波束方向的速度分量并不相同，而测速误差与该波束方向速度分量的大小有关，也和测速体制有关。因此在固定优先级策略下，选出波束的测量误差可能大，这造成最终融合出的速度精度并非最高。
技术实现要素：
本发明技术解决问题：克服现有技术的不足，提出了一种多波束测速信息融合方法及系统，将有效的测速波束每四个构成一个平衡方程，计算它们的测量误差，并根据误差的大小对这四个波束进行计分；当所有的四个波束组合均进行判断后，得到所有波束的最终评价值。然后从这些波束中按照一定逻辑选出三个波束参与测速信息融合。这个逻辑以根据测速波束指向、精度和测量体制事先制定的组合优先级为基础，再参考各波束评价值的高低进行波束选择。通过这样一套选择方法和逻辑，可以对测速波束进行优选，确保获得测速波束融合使用的最佳性能。
本发明所采用的技术方案为：
一种多波束测速信息融合方法，实现步骤如下：
(1)将惯导获得的对地速度投影到各个测速波束方向上，并与测速波束的实测值进行比对，获得波束测量残差；
(2)根据获得的波束测量残差，通过平衡方程计算每个波束的累计评价值；
(3)根据步骤(2)得到的波束的累计评价值，按照波束选择逻辑，选择出三个波束；
(4)波束融合：对选出的3个波束进行几何解算，获得三维速度误差矢量，然后滤波修正惯导速度。
所述步骤(1)将惯导获得的对地速度投影到各个测速波束方向上，并与测速波束的实测值进行比对，获得波束测量残差，具体为：
(1.1)获得第i个测速波束的测量值vi和对应测量时间ti，且有t-Δt<ti≤t，测量值vi即为着陆器对地速度在该波束上的分量，当前时刻为t，导航系统的更新周期为Δt，1≤i≤N，N为波束数量；
(1.2)计算导航系统外推ti时刻的对地速度vm(ti)：
其中，导航系统在t-Δt时刻经修正后的对地速度矢量为在[t-Δt,t]周期内获得对地速度增量计算值为ΔvINS(t,t-Δt)，
(1.3)计算ti时刻本体系相对导航系的姿态矩阵Cbn(ti)；
Cbn(ti)＝A(q(ti))；
A(q)是四元数的函数，设q＝[q1,q2,q3,q4]T，则
其中，||·||表示矢量的2范数，表示四元数乘法，导航系统在t-Δt时刻计算出的本体系相对导航系的姿态四元数为q(t-Δt)，[t-Δt,t]周期内获得对本体系相对导航系的角速度为ωINS(t,t-Δt)；
(1.4)计算导航系统外推ti时刻的对地速度在波束i上的分量
符号<·>表示两个向量的点积；Ui表示第i个波束在本体坐标系下的方向；
(1.5)计算第i个波束的测量残差
所述步骤(2)根据获得的波束测量残差，通过平衡方程计算每个波束的累计评价值，具体为；
(2.1)令每个波束的累计评价值赋初值为零，即Sm＝0，m＝1,2,…,N；
(2.2)取任意四个波束构成一组，四个波束的编号分别为i,j,k,l，计算出该组平衡方程的残差εijkl为
其中，
(2.3)计算评价阈值ε：
ε＝|Cijkl(1)|εi+|Cijkl(2)|εj+|Cijkl(3)|εk+εl；
其中，εi＝f(vm(ti))，εj＝f(vm(tj))，εk＝f(vm(tk))，εl＝f(vm(tl))；f(v)为速度误差的函数；
(2.4)根据评价阈值ε确定每个波束的累计评价值；
(2.5)遍历所有不同的四波束组合，执行步骤(2.2)-(2.4)，得到所有波束的最终累计评价值。
所述步骤(2.4)根据评价阈值ε确定每个波束的累计评价值，具体为：
若εijkl≥k3ε，则i,j,k,l波束的评价值Si,Sj,Sk,Sl各自减2，即：
将Sm-2赋值给Sm，m＝i,j,k,l；
若k2ε≤εijkl<k3ε，则i,j,k,l波束的评价值各自减1，即：
将Sm-1赋值给Sm，m＝i,j,k,l；
若k1ε≤εijkl<k2ε，则i,j,k,l波束的评价值不变；
若k1ε>εijkl，则i,j,k,l波束的评价值各自加1，即
将Sm+1赋值给Sm，m＝i,j,k,l；
系数k3>k2>k1≥1。
所述步骤(3)根据波束的累计评价值，按照波束选择逻辑，选择出三个波束，具体为：
(3.1)构造优先考虑的M个波束组合，每个组合中包括三个波束，根据预设的波束组合的优先级对波数组合进行排序，即组合1的优先级高于组合2，组合2的优先级高于组合3，…，组合M-1的优先级高度组合M。组合数量M少于所有可能的组合数目，即是N个波束中以三个不同波束为一组的所有组合的数量；
(3.2)若最高优先级的波束组合，即第1个波束组合内的三个波束的累计评价值均为最高或不全是最低，则选该波束组合内的三个波束参与速度修正；否则进入步骤(3.3)；
(3.3)若次高优先级的波束组合，即第2个波束组合内的三个波束的累计评价值均为最高或不全是最低，则选该波束组合内的三个波束参与速度修正；否则进入步骤(3.4)；
(3.4)以此类推，若优先级最低的波束组合，即第M个波束组合内的三个波束的累计评价值均为最高或不全是最低，则选该波束组合内的三个波束参与速度修正；否则进入步骤(3.5)；
(3.5)选出累计评价值前三名的波束参与速度修正。
所述步骤(4)波束融合：对选出的3个波束进行几何解算，获得修正量，然后滤波修正惯导速度，具体为：
(4.1)将导航值外推到当前时刻t，得到外推的速度矢量和四元数q(t)：
(4.2)根据选出的三个测速波束的测量残差计算修正量δv，选出的三个波束的编号是i,j,k，则
其中，ki、kj和kk分别是三个波束的滤波系数，为0～1之间的常数；
(4.3)最后对外推的速度进行修正
其中，Cbn(t)＝A(q(t))。
一种多波束测速信息融合系统，包括：
波束测量残差获取模块：用于将惯导获得的对地速度投影到各个测速波束方向上，并与测速波束的实测值进行比对，获得波束测量残差；
评价值计算模块：用于根据波束测量残差获取模块获得的波束测量残差，通过平衡方程计算每个波束的累计评价值；
波束选择模块：用于根据评价值计算模块得到的波束的累计评价值，按照波束选择逻辑，选择出三个波束；
波束融合模块：用于对波束选择模块选出的3个波束进行几何解算，获得三维速度误差矢量，然后滤波修正惯导速度。
所述波束测量残差获取模块将惯导获得的对地速度投影到各个测速波束方向上，并与测速波束的实测值进行比对，获得波束测量残差，具体为：
(1.1)获得第i个测速波束的测量值vi和对应测量时间ti，且有t-Δt<ti≤t，测量值vi即为着陆器对地速度在该波束上的分量，当前时刻为t，导航系统的更新周期为Δt，1≤i≤N，N为波束数量；
(1.2)计算导航系统外推ti时刻的对地速度vm(ti)：
其中，导航系统在t-Δt时刻经修正后的对地速度矢量为在[t-Δt,t]周期内获得对地速度增量计算值为ΔvINS(t,t-Δt)，
(1.3)计算ti时刻本体系相对导航系的姿态矩阵Cbn(ti)；
Cbn(ti)＝A(q(ti))；
A(q)是四元数的函数，设q＝[q1,q2,q3,q4]T，则
其中，||·||表示矢量的2范数，表示四元数乘法，导航系统在t-Δt时刻计算出的本体系相对导航系的姿态四元数为q(t-Δt)，[t-Δt,t]周期内获得对本体系相对导航系的角速度为ωINS(t,t-Δt)；
(1.4)计算导航系统外推ti时刻的对地速度在波束i上的分量
符号<·>表示两个向量的点积；Ui表示第i个波束在本体坐标系下的方向；
(1.5)计算第i个波束的测量残差
所述评价值计算模块：用于根据波束测量残差获取模块获得的波束测量残差，通过平衡方程计算每个波束的累计评价值，具体为；
(2.1)令每个波束的累计评价值赋初值为零，即Sm＝0，m＝1,2,…,N；
(2.2)取任意四个波束构成一组，四个波束的编号分别为i,j,k,l，计算出该组平衡方程的残差εijkl为
其中，
(2.3)计算评价阈值ε：
ε＝|Cijkl(1)|εi+|Cijkl(2)|εj+|Cijkl(3)|εk+εl；
其中，εi＝f(vm(ti))，εj＝f(vm(tj))，εk＝f(vm(tk))，εl＝f(vm(tl))；f(v)为速度误差的函数；
(2.4)根据评价阈值ε确定每个波束的累计评价值；
若εijkl≥k3ε，则i,j,k,l波束的评价值Si,Sj,Sk,Sl各自减2，即：
将Sm-2赋值给Sm，m＝i,j,k,l；
若k2ε≤εijkl<k3ε，则i,j,k,l波束的评价值各自减1，即：
将Sm-1赋值给Sm，m＝i,j,k,l；
若k1ε≤εijkl<k2ε，则i,j,k,l波束的评价值不变；
若k1ε>εijkl，则i,j,k,l波束的评价值各自加1，即
将Sm+1赋值给Sm，m＝i,j,k,l；
系数k3>k2>k1≥1；
(2.5)遍历所有不同的四波束组合，执行步骤(2.2)-(2.4)，得到所有波束的最终累计评价值。
所述波束选择模块：用于根据评价值计算模块得到的波束的累计评价值，按照波束选择逻辑，选择出三个波束，具体为：
(3.1)构造优先考虑的M个波束组合，每个组合中包括三个波束，根据预设的波束组合的优先级对波数组合进行排序，即组合1的优先级高于组合2，组合2的优先级高于组合3，…，组合M-1的优先级高度组合M。组合数量M少于所有可能的组合数目，即是N个波束中以三个不同波束为一组的所有组合的数量；
(3.2)若最高优先级的波束组合，即第1个波束组合内的三个波束的累计评价值均为最高或不全是最低，则选该波束组合内的三个波束参与速度修正；否则进入步骤(3.3)；
(3.3)若次高优先级的波束组合，即第2个波束组合内的三个波束的累计评价值均为最高或不全是最低，则选该波束组合内的三个波束参与速度修正；否则进入步骤(3.4)；
(3.4)以此类推，若优先级最低的波束组合，即第M个波束组合内的三个波束的累计评价值均为最高或不全是最低，则选该波束组合内的三个波束参与速度修正；否则进入步骤(3.5)；
(3.5)选出累计评价值前三名的波束参与速度修正；
所述波束融合模块：用于对波束选择模块选出的3个波束进行几何解算，获得三维速度误差矢量，然后滤波修正惯导速度，具体为：
(4.1)将导航值外推到当前时刻t，得到外推的速度矢量和四元数q(t)：
(4.2)根据选出的三个测速波束的测量残差计算修正量δv，选出的三个波束的编号是i,j,k，则
其中，ki、kj和kk分别是三个波束的滤波系数，为0～1之间的常数；
(4.3)最后对外推的速度进行修正
其中，Cbn(t)＝A(q(t))。
本发明与现有技术相比的有益效果是：
(1)本发明对所有有效波束进行评判，并根据误差大小进行评价值计算，评价值高低表明该波束数据的可用程度，可以避免测量误差较大的波束引入修正。
(2)本发明提出的波束选择策略综合考虑单波束的评价值大小以及波束组合时不同测量体制和构型的影响，降低波束组合时误差被放大的可能性。通过这样一种策略，能够增加测速修正的精度，提高组合导航的可靠性和安全性。
附图说明
图1多波束测速信息融合方法框图。
图2测速误差变化曲线。
图3测速波束选择曲线。
具体实施方式
测速敏感器是着陆任务导航系统的必备敏感器，它可以提供波束方向的速度测量，并用于修正惯导。为了提供完备的三维速度修正，一般测速敏感器至少具有3个测量波束。而为了保证可靠性，有的着陆器还安装有不同体制的测速敏感器以形成备份，这使得同一时间可供使用的测速波束数量大大超过三个，造成信息的冗余。因此，如何使用这些冗余的测速信息成为导航系统设计中的一个问题。
本发明提出了一种多波束测速信息融合方法及系统。适用与有效波束数量不少于5个的情形。其原理是将有效的测速波束每四个构成一组，使用平衡方程计算它们的测量误差，并根据误差的大小对这四个波束计算评价值；当所有的四个波束组合均进行判断后，得到所有波束的最终累计评价值。然后从这些波束中按照一定逻辑选出三个波束参与测速信息融合。这个逻辑以根据测速波束指向、精度和测量体制事先制定的组合优先级为基础，再参考各波束评价值的高低进行波束选择。通过这样一套选择方法和逻辑，可以对测速波束进行优选，确保获得测速波束融合使用的最佳性能。
如图1所示，本发明提出的一种多波束测速信息融合方法，实现步骤如下：
(1)将惯导获得的对地速度投影到各个测速波束方向上，并与测速波束的实测值进行比对，获得波束测量残差；
(2)根据获得的波束测量残差，通过平衡方程计算每个波束的累计评价值；
(3)根据步骤(2)得到的波束的累计评价值，按照波束选择逻辑，选择出三个波束；
(4)波束融合：对选出的3个波束进行几何解算，获得三维速度误差矢量，然后滤波修正惯导速度。
本发明提出的实施例具体为：
设着陆器安装有若干台测速敏感器，共有N个不同指向的测速波束。它们在着陆器本体系(固定在着陆器上，三个坐标轴指向着陆器特定方向，用b表示)下的指向用列向量Ui(i＝1,…,N)表示。
导航系统每Δt个时间周期进行一次导航修正。设当前时刻为t，此时获得N个测速波束的测量值vi(它是着陆器对地速度在该波束上的分量)和对应测量时间ti且有t-Δt<ti≤t。
1)波束测量残差计算
对每一测速波束进行残差计算。
设导航系统在t-Δt时刻经修正后的对地速度矢量为并在[t-Δt,t]周期内获得对地速度增量计算值为ΔvINS(t,t-Δt)(该速度增量已经含有重力影响)，速度和速度增量均表示在导航系下，导航系根据具体任务选择，用n表示。则可利用上一周期导航速度和本周期速度增量线性外推出波束i测量时刻导航外推的速度。
设导航系统在t-Δt时刻计算出的本体系相对导航系的姿态四元数为q(t-Δt)，[t-Δt,t]周期内获得对本体系相对导航系的角速度(表示在本体系下)为ωINS(t,t-Δt)。那么可以外推计算出该波束测量时刻本体系相对导航系的姿态四元数为
其中，||·||表示矢量的2范数，表示四元数乘法。进一步可以根据四元数求出对应时刻本体系相对导航系的姿态矩阵Cbn(ti)。
Cbn(ti)＝A(q(ti))
A(q)是四元数的函数，设q＝[q1,q2,q3,q4]T，则
然后将vm(ti)投影到波束方向，并与测速波束测量值vi做差得到测量残差
δvi＝vi-<Ui·[Cbn(ti)vm(ti)]>
符号<·>表示两个向量的点积。
2)平衡方程评价
对于N个测速波束，用Sm(m＝1,2,…,N)表示第m个波束的评价值(初值为0)。取任意四个波束构成一组，设这四个波束的编号分别为i,j,k,l，可以计算出该组平衡方程的残差εijkl为
通常测速波束的误差不是一成不变的，它是着陆器某个状态的函数。不失一般性，假设它是该波束方向速度分量的函数，用f(v)表示，那么可以进行如下评价值累计运算
εi＝f(vm(ti))
εj＝f(vm(tj))
εk＝f(vm(tk))
εl＝f(vm(tl))
ε＝|Cijkl(1)|εi+|Cijkl(2)|εj+|Cijkl(3)|εk+εl
若εijkl≥k3ε，则i,j,k,l波束的评价值Si,Sj,Sk,Sl各自减2，即：
将Sm-2赋值给Sm，m＝i,j,k,l；
若k2ε≤εijkl<k3ε，则i,j,k,l波束的评价值各自减1，即：
将Sm-1赋值给Sm，m＝i,j,k,l；
若k1ε≤εijkl<k2ε，则i,j,k,l波束的评价值不变；
若k1ε>εijkl，则i,j,k,l波束的评价值各自加1，即
将Sm+1赋值给Sm，m＝i,j,k,l；
系数k3>k2>k1≥1。
改变波束的选择重新计算该组下的平衡方程残差，并按上述方法进行波束评价值计算。这样的组合一共有种，是N个波束中以4个不同波束为一组的所有组合的数量。
3)波束选择逻辑
构造优先考虑的M个波束组合，每个组合中包括三个波束，根据预设的波束组合的优先级对波数组合进行排序，即组合1的优先级高于组合2，组合2的优先级高于组合3，…，组合M-1的优先级高于组合M；组合数量M少于所有可能的组合数目，即是N个波束中以三个不同波束为一组的所有组合的数量。例如组合1优先级高于组合2，组合2优先级高于组合3等等。
接下来结合组合优先级和各波束的评价值进行选择，方法如下：
(a)若最高优先级的波束组合，即第1个波束组合内的三个波束的累计评价值均为最高或不全是最低，则选该波束组合内的三个波束参与速度修正；否则进入步骤(b)；
(b)若次高优先级的波束组合，即第2个波束组合内的三个波束的累计评价值均为最高或不全是最低，则选该波束组合内的三个波束参与速度修正；否则进入步骤(c)；
(c)以此类推，若优先级最低的波束组合，即第M个波束组合内的三个波束的累计评价值均为最高或不全是最低，则选该波束组合内的三个波束参与速度修正；否则进入步骤(d)；
(d)选出累计评价值前三名的波束参与速度修正。
4)波束融合使用
首先将导航值外推到当前时刻t，得到外推的速度矢量和四元数q(t)
然后根据上一步选出的三个测速波束测量值计算速度修正量δv，设上一步选出的三个波束的编号是i,j,k，则
其中，ki、kj和kk分别是三个波束的滤波系数，它们为0～1之间的常数，根据实际任务特点选取。
最后对预报的速度进行修正
其中，Cbn(t)＝A(q(t))。
仿真分析：
以月球软着陆过程为例，着陆器上安装有6个不同指向的测速波束。根据本发明内容，首先对这6个波束进行平衡方程评价，一共有15个平衡方程。在波束选择上，设计了两个优先组合，即组合1(包含波束1、2、3)和组合2(包含波束4、5、6)。那么波束选择逻辑是：
若波束组合1内的三个波束(1、2、3)全是最高评价值或不全是最低评价值，则选组合1内的三个波束参与速度修正；
否则若波束组合2内的三个波束(4、5、6)全是最高评价值或不全是最低评价值，则选组合2内的三个波束参与速度修正；
否则按照计评价值高低选出三个评价值最高的波束参与速度修正。
下降过程从924.5s开始引入测速修正(如图2所示)，之后导航速度误差迅速下降到0附近；对应时间段内波束组合的选择如图3所示(纵坐标是选出的波束序号)。可以看到，从测速引入到961.4s之间，选择的是组合1三个波束进行速度修正；之后到1028.3s时自动切换为组合2三个波束进行速度修正；再之后直到落月前，则切换回组合1。切换过程中导航速度误差平稳，波动范围小于0.1m/s。这说明本发明提出的多波束测速信息融合方法是有效的。