基于卡尔曼滤波器的导航辅助方法和设备与流程

背景技术：

1、本发明涉及使用传感器进行测量的一般领域，诸如载具的位置和速度。

2、更具体地说，本发明涉及使用卡尔曼滤波器监控载具的导航。该卡尔曼滤波器被用于估计实施该载具的导航操控所需的量级。该量级例如表示载具本身的特性(例如位置、速度、取向等)，或者也可以表示与环境相关的特性(载具附近的对象的存在、风速等)。随后，这些特性(无论是载具内部还是外部)将被称为“状态”。

3、注意，卡尔曼滤波器还以协方差矩阵的形式估计状态估计的不确定性。所估计的这些不确定性也可以用于实施操控。因此，术语“量级”也可以表示由卡尔曼滤波器估计的这种不确定性。

4、众所周知，在导航系统中，卡尔曼滤波器经常被用作状态估计器。卡尔曼滤波器在每个时刻n递归地评估状态xn，根据数值模型(特别是描述不同状态如何根据彼此而进行演变以及测量如何取决于状态的模型)和传感器收集的信息(以下称为“测量”)。

5、例如，在生成目标对象的位置、取向和速度的估计的导航系统中，该模型特别地包括根据载具的位置、载具的取向和载具的速度来管控载具轨迹的运动等式。该模型可以涉及一些参数，这些参数的值是先验已知的(例如重力加速度)和/或其值可能取决于时间步长n而变化。

6、在本技术的其余部分中，为了提及时间化n，将互换使用给定的表述“瞬时”、“步骤”和“时刻”。

7、这些连续时刻中的两个之间的时间间隔通常是用户可调整的参数。

8、在导航系统的领域中，传感器常常与惯性测量单元相关联。

9、例如，在导航中，通常使用耦合到gps接收器的惯性测量单元来估计载具的位置和取向。惯性测量单元(包括陀螺仪和加速度计)分别评估移动载体所受到的取向变化和连续力。通过简单的数学积分操作，这些测量使得可以估计载具在位置、速度和取向方面的状态。由gps传达的信息使得可以计算载具的位置。卡尔曼滤波器合并此信息以估计载具的状态。合并的质量是经由对滤波器参数进行良好的校准等方式来完成的。模型和数据合并方法是已知的，并且因此将不再赘述。

10、由卡尔曼滤波器所估计的(一个或多个)量级以及这些估计的使用取决于应用而变化。特别地：

11、-估计例如与载具的状态相关，并且该估计可以用于更新被显示给载具驾驶员的道路地图和/或决定向他给予哪些方向以达到其行程目的；

12、-在配备有雷达传感器的系统中，估计可以与载具附近的对象的状态相关，并且该估计可以用作这些对象的位置的指示，从而调配载具的轨迹；

13、-在自动驾驶汽车中，估计例如与载具相关，并且可以向驱动载具的控制模块提供信息，从而允许载具决定下一步操控等。

14、卡尔曼滤波器通过使用先前的估计以及由传感器在时刻n之前提供的测量，递归地确定目标对象的当前状态的估计xn|n。该递归计算在图1中被示意性地图示。

15、即使利用适当的数学模型，由(一个或多个)传感器提供的测量的质量也会对卡尔曼滤波器的估计的质量产生重大影响。

16、为了了解测量质量的重要性，首先应该回顾卡尔曼滤波器的经典操作。

17、卡尔曼滤波器在给定步骤n处所估计的状态将被记为xn。在许多应用中，状态以多个变量来表达，并且因此xn构成向量。

18、系统根据数字模型来演化，并且因此可以认为根据如下等式[公式1]，目标对象在给定时刻n处的状态xn取决于前一时刻n-1处的状态xn-1：

19、[公式1]

20、xn＝fnxn-1+bnun+wn

21、其中fn是已知的方形矩阵，un是控制输入向量(用户已知)，也就是说被应用于系统的动作，bn是表示将状态xn与该控制输入联系起来的模型的矩阵，并且wn是中心高斯随机变量，其表示由矩阵qn表示的协方差噪声。

22、在等式[公式1]和以下等式中，两个量值的并列表示法对应于：

23、-如果这两个量值是数字，则为简单乘积，

24、-如果其中一个量值是矩阵并且另一个量值是向量，则为矩阵与向量的乘积(例如等式[公式1]中的fn xn-1)，或者

25、-如果这两个量值是矩阵，则为矩阵乘积(例如以下等式[公式4]中的fn pn-1|n-1)。

26、(一个或多个)测量(特别是由(一个或多个)传感器在时刻n处提供的测量)将被记为yn，并根据以下等式[公式2]而取决于状态xn：

27、[公式2]

28、yn＝hnxn+vn

29、其中hn是已知的观测矩阵，并且vn是表示协方差噪声rn的中心高斯随机变量。同样，在许多应用中，在每个时刻n处都会向卡尔曼滤波器递送多个测量，并且在这种情况下，yn和vn将是向量，而rn是矩阵。

30、在卡尔曼滤波器的实际实现期间，需要选择噪声协方差矩阵qn和rn。这通常是通过事先研究这些协方差与相关领域中获得的数据之间的关系来完成的。估计协方差的典型方法在于应用als(自协方差最小二乘)算法。

31、噪声协方差矩阵qn和rn对应于由传感器进行的关于测量的不确定性。因此，在本技术中，为了简洁起见，这些量值将被称为术语：关于测量yn的不确定性。

32、卡尔曼滤波器在给定时刻n处维持对状态xn的估计，并且当该估计是根据在时刻n-1处可用的测量yn-1计算出的(预测出的)，则被记为xn|n-1；并且当计算对在时刻n处获得的测量yn进行积分时，则被记为xn|n。卡尔曼滤波器还估计表示状态xn的估计的不确定性的协方差矩阵pn，并且当该矩阵适用于估计xn|n-1时，则被记为pn|n-1；并且当该矩阵适用于估计xn|n时，则被记为pn|n。

33、在每个步骤n处，滤波器操作两次。

34、首先，根据如下等式[公式3]和[公式4]，卡尔曼滤波器根据状态xn-1|n-1和协方差矩阵pn-1|n-1的估计来预测目标对象的先验状态xn|n-1以及协方差矩阵pn|n-1的估计：

35、[公式3]

36、xn|n-1＝fnxn-1|n-1+un

37、[公式4]

38、pn|n-1＝fnpn-1|n-1fnt+qn

39、其中fnt是矩阵fn的转置。

40、其次，卡尔曼滤波器通过使用上一步骤中做出的预测并考虑当前测量yn，以由称为“卡尔曼增益”的系数kn所表示的一定权重来更新目标对象状态的先验估计。更新根据以下等式[公式5]和[公式6]进行：

41、[公式5]

42、xn|n＝xn|n-1+kn(yn-hnxn|n-1)＝xn|n-1+knzn

43、[公式6]

44、pn|n＝(i-knhn)pn|n-1

45、其中kn根据以下等式[公式7]计算出：

46、[公式7]

47、

48、其中sn表示测量yn的协方差，其根据以下等式[公式8]来计算：

49、[公式8]

50、sn＝hnpn|n-1hnt+rn

51、sn-1是sn的逆矩阵，并且zn根据以下等式[公式9]来定义：

52、[公式9]

53、zn＝yn-hnxn|n-1

54、卡尔曼滤波器输出当前状态的当前估计xn|n和与该估计相关联的不确定性pn|n，这些量级取决于应用而被递送到控制系统等。

55、只要卡尔曼滤波器假设来自gps的测量是可靠的，测量误差，即使很小，也会导致估计与实际情形或多或少的偏差。

56、因此，与估计相关联的不确定性的值的数据不足以评估卡尔曼滤波器的测量和估计的可靠性，也无法检测出哪个测量或哪个传感器导致了该估计的稳健性较差。

57、本发明旨在克服这些缺点。

技术实现思路

1、因此，根据第一方面，本发明提出了一种用于监控载具导航以实施导航操控的方法，该方法包括以下步骤：

2、-由卡尔曼滤波器根据参数、载具在其环境中的数字模型以及至少一个测量来估计量级，该至少一个测量由至少一个传感器进行并且与关于该测量的不确定性相关联，所述量级用于实施所述导航操控；

3、-确定所述量级相关于所述至少一个测量的至少一个偏导数值；

4、-从所述至少一个偏导数值获得用于所述估计的所述至少一个测量的至少一个临界值；

5、-发送表示所述至少一个临界值的至少一个临界信号；

6、-接收响应于所述发送步骤的至少一个指令；以及

7、-应用所述至少一个指令；

8、所述指令指示用于实施所述导航操控的监控控制。

9、相应地，本发明提出了一种用于监控载具导航以实施导航操控的系统，该系统包括：

10、-用于由卡尔曼滤波器根据参数、载具在其环境中的数字模型以及至少一个测量来估计量级的模块，该至少一个测量由至少一个传感器进行并且与关于该测量的不确定性相关联，所述量级用于实施所述导航操控；

11、-用于确定所述量级相关于所述至少一个测量的至少一个偏导数值的模块；

12、-用于从所述至少一个偏导数值获得用于所述估计的所述至少一个测量的至少一个临界值的模块；

13、-用于发送表示所述至少一个临界值的至少一个临界信号的模块；

14、-用于响应于所述发送步骤而接收至少一个指令的模块；以及

15、-用于应用所述指令的模块；

16、所述指令指示用于实施所述导航操控的监控控制。

17、根据监控方法的一种实现模式，所述指令指示将测量相加以用于所述量级的随后估计。

18、在这里规定了在此处使用术语偏导数涵盖梯度的概念。实际上，测量或不确定性可以包含若干量值(速度、位置等)，并且因此可以以向量的形式来表示。

19、在这种情况下，为了简洁起见，还将使用术语量级相关于量值的偏导数。换句话说，在本技术中，如果量值是向量的形式，则量级相关于该量值的偏导数指明根据表示该量值的向量的该量级的梯度。

20、同样，为了简洁起见，量级关于测量的偏导数的值将指代包括根据表示该测量的向量的量级梯度的值集合。

21、还规定，在本技术中，量级关于第一矩阵(例如，表示关于测量的不确定性的矩阵)的偏导数的值对应于第二矩阵，该第二矩阵的分量等于该量级相关于第一矩阵的每个分量的偏导数的值。

22、在本技术中，如果调用两个向量(例如，两个向量形式的偏导数)之间的和，则该和对应于向量，该向量等于这两个向量的元素对元素和。

23、对这些偏导数的评估有利地使得可以提供关于实施导航操控所需的量级估计的稳健性的重要信息，特别是通过使得可以量化该估计中的不同测量的关键性。

24、所获得的关于量级估计的稳健性的信息指示在所有可用测量之中将测量相机以估计量级是否相关，或者应用监控控制是否相关，这例如对应于取消所设想的操控。

25、事实上，一方面，相关于在给定时刻进行的测量或者相关于在给定时间范围期间进行的测量的量级的一个或多个偏导数使得可以评估测量对随时间进行的量级估计的影响。

26、例如，如果相关于测量的偏导数对于在量级的估计之前的时刻处进行的测量取大值，但是对于在更早时刻处进行的测量取接近零的值，则这意味着只有在估计之前的时刻时进行的测量才在该估计中被显著考虑。

27、在稳健估计需要考虑若干连续测量的情况下，这种场景可能指示估计缺乏稳健性。

28、在该示例中，可以通过增加随时间进行的测量的频率来改进估计。

29、因此，根据监控方法的一种实现模式：

30、-获得连续进行的测量，以及

31、-在所述确定步骤期间，对于这些测量中的每一个，确定相关于该测量的量级的偏导数值。

32、在该实施例中，偏导数的计算是后验的。换句话说，在获得相关于测量而计算量级的偏导数值的测量之后获得量级。

33、例如，在时刻n时的估计xn|n的偏导数是相关于在时刻0和n之间考虑的一系列测量而计算的；因此，在获得估计xn|n之后计算这些偏导数。

34、另一方面，相关于由给定传感器进行的测量或相关于由一组给定传感器进行的测量的量级的一个或多个偏导数使得可以评估传感器或传感器组对估计的影响。

35、例如，如果相关于测量的偏导数取低值，而本领域技术人员的知识指示所估计的量级通常受该测量的影响很大(例如，载具速度的估计受其加速度的测量的影响很大)，则这指示估计不稳健。

36、根据监控方法的一种实现模式，对于至少一个给定时刻，在所述确定步骤期间确定相关于测量的所述偏导数值，该测量包括在该给定时刻由单独的传感器进行的至少两个测量。

37、回想一下，如上面所解释的，测量可以对应于向量，该向量的分量是不同状态的测量。例如，在给定时刻的所述测量可以例如包括在该给定时刻的载具加速度的测量以及在该给定时刻的载具速度的测量。

38、还应注意，在这种实现模式中，量级相关于该测量的偏导数是向量，该向量的分量等于量级相关于不同状态的每个测量的偏导数。例如，量级相关于加速度和速度测量的偏导数包括量级相关于速度测量的偏导数以及量级相关于加速度测量的偏导数。

39、有利的是，由卡尔曼滤波器估计的量级的偏导数的计算在计算能力和存储容量方面是便宜的。

40、所估计的量级的偏导数的计算利用了卡尔曼滤波器作为状态估计器的操作期间已经计算出的一些值。结果，本发明的实现仅需要添加一些在时间和计算能力方面便宜的额外手段。

41、根据偏导数值，该方法使得可以获得用于表征测量的临界性的临界值，以用于量级的估计。换句话说，临界值量化一个或多个测量对量级的估计的影响。在第一示例中，一个或多个临界值可以等于偏导数值，因为这些值使得可以表征测量的临界性，以用于量级的估计。

42、因此，根据监控方法的一种实现模式，至少一个临界值等于在所述确定步骤期间确定的至少一个偏导数值。

43、特别是在配备有单个传感器的系统的情况下，该传感器每次仅给出一个测量，量级相关于该单个传感器随时间进行的测量的偏导数使得可以估计该量级估计的稳健性。

44、但是，取决于要被执行的导航操控，以不同方式定义(一个或多个)临界值可能是有利的。

45、例如，根据监控方法的一种特定模式，(一个或多个)临界值等于偏导数值的绝对值。

46、实际上，取决于要被实施的导航操控，测量的临界性的幅度对量级估计是相关的，这由相关于该测量的偏导数的绝对值来量化。

47、在另一个示例中，临界值等于归一化偏导数值的绝对值，使得这些临界值的总和等于1。

48、偏导数值的绝对值相关于不同测量的归一化使得可以特别地相对于被用于相同估计的其他测量的临界性来比较用于量级估计的测量的临界性。

49、在另一个示例中，其中相关于随时间连续进行的测量确定了若干偏导数值，临界值等于相关于连续进行的两个测量的两个偏导数值的差。这样的临界值使得可以检测到在给定时刻处用于量级估计的测量的临界性的可能过大的变化，并且因此检测到在该给定时刻时的估计的较差稳健性。

50、为了量级估计而表征一组测量的临界性可能是有利的，例如在给定时间范围期间连续进行的测量，或由一组传感器进行的测量。

51、为此目的，临界值可以等于一组测量之中相关于测量的偏导数值的总和。

52、因此，根据监控方法的一个特定实施例：

53、-在所述确定步骤期间确定若干偏导数值，

54、-所述至少一个临界值等于所述偏导数值的总和。

55、根据监控方法的另一个实施例：

56、-在所述确定步骤期间确定若干偏导数值；

57、-所述至少一个临界值等于以下各项之间的乘积和：

58、(i)相关于所述测量的所述偏导数值，以及

59、(ii)关于该测量的所述不确定性。

60、换句话说，在这种实现模式中，临界值是相关于测量的偏导数值的总和，该总和由关于这些测量的不确定性来加权。

61、该实施例有利地使得能够以简单的方式检测不确定测量是否对量级的估计具有高临界性。如果这样的临界值很高，则意味着量级的估计不稳健。

62、本发明还使得可以根据单个偏导数值来确定临界值，类似于先前的实施例。

63、因此，根据监控方法的一个实施例，所述至少一个临界值等于以下各项之间的乘积：

64、(i)相关于所述测量的所述偏导数值，以及

65、(ii)关于该测量的所述不确定性。

66、为了允许使用(一个或多个)临界值，监控方法指示发送表示这些临界值的一个或多个临界信号。在允许实施上述方法的导航监控系统中，这个/这些临界信号被发送到接收模块，从而使得可以实施该方法的接收步骤。

67、通过构造，这个/这些临界信号包含关于量级的估计的稳健性的一个或多个信息，该信息使得可以相关于导航操控的实施做出决策。

68、在本发明的一个实施例中，(一个或多个)临界信号包括等于临界值的值。

69、在另一个实施例中，可以通过将临界值与一个或多个阈值进行比较来确定(一个或多个)临界信号。

70、因此，根据监控方法的一个特定实现模式，发送步骤包括以下子步骤：

71、-在所述至少一个临界值与至少一个阈值之间进行比较，所述至少一个阈值是相关于所述操控而被定义的；

72、-根据所述比较的结果生成所述至少一个临界信号。

73、相关地，本发明提出了监控系统的一个特定实施例，其中所述发送模块包括用于以下操作的子模块：

74、-在所述至少一个临界值与至少一个阈值之间进行比较，所述至少一个阈值是相关于所述操控而被定义的；

75、-根据所述比较子模块的比较的结果生成所述至少一个临界信号。

76、在该特定实现模式中，本领域技术人员利用要被实施的导航操控的知识，特别是进行测量的传感器的操作的知识，使得可以定义一个或多个阈值。

77、将(一个或多个)临界值与这个/这些阈值进行比较而产生的(一个或多个)临界信号具有易于读取的量级估计稳健性指标的优点。

78、特别地，这种临界信号可以是二进制数，从而指示估计的稳健性好坏。

79、(一个或多个)临界信号可以自动地被处理，或者由(一个或多个)临界信号被呈现给的用户经由该接收模块进行处理。

80、随后，接收模块接收例如自动确定或由用户确定的一个或多个指令，然后应用这些指令。例如，(一个或多个)指令是根据临界信号来确定的。

81、指令可以涉及载具本身的监控。特别地，如果检测到量级估计的稳健性较差，则指令可以指示暂时停止导航操控的实施或取消导航操控。

82、这种暂时停止导航操控尤其使得可以确保导航安全，直到获得更稳健的量级估计。

83、另一方面，如果检测到量级估计的良好稳健性，则指令可以指示操控的延长。

84、在一个特定实施例中，指令还可以指示，为了执行更稳健的量级估计，期望在由传感器提供的所有可用测量之中考虑一个或多个附加测量。

85、本发明还提出了一种用于载具的导航系统，所述导航系统适于执行根据上述实施例之一的监控系统的功能，并且包括至少一个传感器，传感器旨在获得用于估计量级的至少一个测量，所述量级用于实施导航操控。

86、根据该导航系统的一个实施例，至少一个所述传感器是惯性测量单元。

87、在这样的系统中，传感器组例如可以包括与惯性测量单元相关联的至少一个gps传感器。这是通常安装在载具、无人机等上的传感器的配置。

88、本发明提出了一种计算机程序，包括用于执行根据上述任一实现模式的监控方法的步骤的指令。

89、应当注意的是，本公开中提及的计算机程序可以使用任何编程语言，并且可以是源代码、目标代码或源代码与目标代码之间的中间代码的形式，诸如部分编译的形式，或者任何其他期望的形式。

90、本发明提出了一种计算机设备可读的、计算机程序的记录介质，该计算机程序包括用于执行根据上述任一实施例的方法的步骤的指令。

91、本公开中提到的记录介质可以是任何能够存储程序并可被控制装置或任何计算机设备(特别是计算机)读取的实体或设备。

92、例如，该介质可以包括存储部件或磁记录部件，例如硬盘。

93、替代地，记录介质可以对应于集成到计算机或导航系统中的电路，该电路中合并有程序，并且该程序适于执行如上所述的方法或在执行该方法时被使用。