基于角度量测序贯结构状态估计方法、系统及存储介质

本发明涉及目标自动检测跟踪，尤其涉及一种基于角度量测序贯结构状态估计方法、系统及存储介质。

背景技术：

1、在机器人环境感知中，获取环境中移动目标的状态和运动模型是至关重要的环节，因为这是确保机器人能够安全航行的先决条件。因此尽可能准确地估计移动目标的位置和速度是机器人自动安全航行必须具备的关键能力。

2、在专门针对移动目标的状态估计中，当其运动模型先验已知时，可以利用卡尔曼及其变体方法，粒子滤波等方法来估计障碍物的位置和速度。但是非结构化环境中包括运动模型在内的先验信息通常是完全未知或不完备的，因此目标的运动中存在不可预测的机动风险。现有的应对运动模型未知风险的方法主要是采用与模式匹配的方式，得到概率上近似的模型。根据对模型集的处理方法，现有方法可以大致分为两类：

3、(1)在固定的模型集中直接进行匹配：如包含了多个滤波器的交互式多模型算法(interacting multiple model，imm)。katrakazas等人则是对于整个运动路径建模成集，对路径参数进行匹配，方法用于有轨道约束的地面道路行车中，hiatt等人则是将路径匹配用于行人的运动预测中。路径匹配在已知目标特性和有约束环境下能够得到较好的估计预测结果，但是在一般非结构化环境中该方法有失效风险，同时大规模计算量使其不能实时应用。

4、(2)用有标签的模型集训练得到的函数对未知模型逼近：此类方法主要包括深度学习方法和概率学习方法，如farhad等人采用两个前馈神经网络对低速运动平台在一特定环境下的运动模式进行训练，然后来对运动平台自身的运动进行预测。marban则采用卷积神经网络对单目视频序列信息进行训练，实现对位置与速度的估计。此类方法中，训练集和参数的选择对其泛化能力影响重大，选择适当能够得到非常好的效果，但是过拟合现象是限制其使用的缺陷之一。

5、以上两类方法都存在计算量随问题规模、参数规模和模型集规模增大而大幅增长的问题，且模型集的使用显然对于先验信息非常依赖。根据上述分析，目前基于纯角度量测的，非结构化环境下先验信息缺失的运动估计问题仍然需要进一步研究。

技术实现思路

1、本申请实施例通过提供一种基于角度量测序贯结构状态估计方法，以序贯结构融合了粒子滤波与未知输入观测器两种状态估计器，在目标运动偏离原模型时，切换估计算法保证其不出现模型失配，克服了传统估计方法依赖目标先验运动模型的问题。

2、本申请实施例提供了一种基于角度量测序贯结构状态估计方法，包括以下步骤：

3、步骤s1,建立移动平台与运动目标的状态与角度量测方程；

4、步骤s2,构造未知输入估计器，以单位方向矢量作为量测估计目标位置；

5、步骤s3,基于估计出的目标位置，从高斯分布采样实现粒子滤波方法的粒子初始化；

6、步骤s4,基于先验运动模型，更新粒子滤波迭代过程的状态估计；

7、步骤s5,构建基于时间维度均方误差的离散判据，判断粒子滤波方法的发散趋势；

8、步骤s6,依据判据结果，若粒子滤波方法发散，则采用未知输入观测器进行当前时刻状态估计；否则保留粒子滤波方法迭代过程的状态估计。

9、优选的，所述步骤s5中具体包括：

10、步骤s51，建立离散判据，根据判据判断目标当前运动状态偏离先验模型分布的程度；

11、步骤s52，不满足离散判据表示运动状态服从先验分布，采用粒子滤波方法进行状态估计；

12、步骤s53，满足离散判据表示目标偏离先验运动模型，采用本发明中构建的未知输入观测器来估计目标的位置状态和速度状态。

13、优选的，所述步骤s5中离散判据具体为：

14、

15、优选的，所述步骤s6具体包括：

16、步骤s61,相关向量和方程即先验信息完备时,用卡尔曼系列方法进行估计；

17、步骤s62，相关向量和方程即先验信息不完备时，将未知信息建模为未知输入，采用构建观测器的方法进行状态估计。

18、优选的，所述步骤s62具体包括：

19、采用构建观测器的方法将未知信息对消后重构状态向量：

20、

21、其中是观测器的状态，是状态估计；n，l，j，h是根据系统动态方程设计出的构建矩阵。

22、本申请还提出了一种基于角度量测序贯结构状态估计系统，包括

23、角度测量模块,用于建立移动平台与运动目标的状态与角度量测方程；

24、估计器构造模块，用于构造未知输入估计器，以单位方向矢量作为量测估计目标位置；

25、离子初始化模块,基于估计出的目标位置，从高斯分布采样实现粒子滤波方法的粒子初始化；

26、例子迭代模块,基于先验运动模型，更新粒子滤波迭代过程的状态估计；

27、离散判断模块,用于构建基于时间维度均方误差的离散判据，判断粒子滤波方法的发散趋势；

28、状态估计模块,依据判据结果，若粒子滤波方法发散，则采用未知输入观测器进行当前时刻状态估计；否则保留粒子滤波方法迭代过程的状态估计。

29、本申请还提出了一种基于角度量测序贯结构状态估计的存储介质,其上存储有计算机程序,其特征在于,计算机程序被处理器执行时实现上述状态估计方法。

30、本申请实施例中提供的一个或多个技术方案，至少具有如下技术效果或优点：

31、1、本申请的估计算法能够利用纯角度量测构建未知输入观测器来估计目标的位置与速度，在目标运动模型未知的情况下，克服了传统估计方法依赖目标先验运动模型的问题，与同类方法相比增加了对速度信息的估计。与同类方法相比其估计精度、收敛速度等方面都得到了较大提高。

32、2、本申请以序贯结构融合了粒子滤波与未知输入观测器两种状态估计器，在目标运动偏离原模型时，切换估计算法保证其不出现模型失配，克服了传统估计方法依赖目标先验运动模型的问题。

技术特征：

1.一种基于角度量测序贯结构状态估计方法，其特征在于，包括以下步骤：

2.如权利要求1所述的状态估计方法，其特征在于，所述步骤s5中具体包括：

3.如权利要求2所述的状态估计方法，其特征在于，所述步骤s5中离散判据具体为：

4.如权利要求1所述的状态估计方法，其特征在于，所述步骤s6具体包括：

5.如权利要求4所述的状态估计方法，其特征在于，所述步骤s62具体包括：

6.一种基于角度量测序贯结构状态估计系统，其特征在于，包括

7.一种基于角度量测序贯结构状态估计的存储介质,其上存储有计算机程序,其特征在于,计算机程序被处理器执行时实现权利要求1至5中任一项所述状态估计方法。

技术总结
本发明公开了一种基于角度量测序贯结构状态估计方法，建立移动平台与运动目标的状态与角度量测方程；构造未知输入估计器，以单位方向矢量作为量测估计目标位置；基于估计出的目标位置，从高斯分布采样实现粒子滤波方法的粒子初始化；基于先验运动模型，更新粒子滤波迭代过程的状态估计；构建基于时间维度均方误差的离散判据，判断粒子滤波方法的发散趋势；依据判据结果进行状态估计；以序贯结构融合了粒子滤波与未知输入观测器两种状态估计器，在目标运动偏离原模型时，切换估计算法保证其不出现模型失配，克服了传统估计方法依赖目标先验运动模型的问题。

技术研发人员：王晓华,卢振利,丁卫,朱培逸
受保护的技术使用者：常熟理工学院
技术研发日：
技术公布日：2024/3/5