[发明专利]紧耦合轨迹修复方法、装置、设备、介质及产品

权利要求书

1.一种紧耦合轨迹修复方法，其特征在于，包括：

获取由紧耦合位姿点组成的紧耦合轨迹和由松耦合位姿点组成的松耦合轨迹；

确定所述紧耦合轨迹中的异常轨迹段和所述松耦合轨迹中与异常轨迹段匹配的补偿轨迹段；所述异常轨迹段包括多个异常位姿点；

根据所述紧耦合轨迹、所述异常轨迹段和所述补偿轨迹段修复所述异常轨迹段；

所述确定所述紧耦合轨迹中的异常轨迹段和所述松耦合轨迹中与异常轨迹段匹配的补偿轨迹段，包括：

根据轨迹平滑性检测算法确定所述紧耦合轨迹中的异常位姿点；

确定各所述异常位姿点之间的相对距离；

将所述异常位姿点和以该异常位姿点为中心间隔预设距离内的其他异常位姿点确定为异常轨迹段；其中，在异常轨迹段中该异常位姿点和其他异常位姿点之间的相对距离小于或等于预设距离；

从所述松耦合轨迹中确定与所述异常轨迹段匹配的补偿轨迹段。

2.根据权利要求1所述的方法，其特征在于，所述异常位姿点包括位姿参数；

所述确定各所述异常位姿点之间的相对距离，包括：

以所述紧耦合位姿点中处于中心位置的中心位姿点为坐标原点建立局部坐标系；

将各异常位姿点的坐标转换到局部坐标系下得到各个异常位姿点在局部坐标系的相对坐标；

根据各所述异常位姿点的相对坐标确定所述相对距离。

3.根据权利要求2所述的方法，其特征在于，所述根据所述紧耦合轨迹、所述异常轨迹段和所述补偿轨迹段修复所述异常轨迹段，包括：

根据所述紧耦合位姿点和所述异常轨迹段确定所述异常轨迹段的两端对应的两个延伸轨迹段；其中一个延伸轨迹段的起点为所述异常轨迹段的两端中第一端的异常位姿点，终点为所述第一端的异常位姿点沿紧耦合轨迹延伸设定距离后的一紧耦合位姿点；另一个延伸轨迹段的起点为所述异常轨迹段的两端中第二端的异常位姿点，终点为所述第二端的异常位姿点沿着紧耦合轨迹延伸设定距离后的一紧耦合位姿点；

根据所述异常轨迹段、两个延伸轨迹段、所述补偿轨迹段以及预设的非线性优化算法修复所述异常轨迹段。

4.根据权利要求3所述的方法，其特征在于，所述异常轨迹段包括异常位姿点的位姿参数；所述延伸轨迹段包括紧耦合位姿点的位姿参数；所述补偿轨迹段包括松耦合位姿点的位姿参数；

所述根据所述异常轨迹段、两个延伸轨迹段、所述补偿轨迹段以及预设的非线性优化算法修复所述异常轨迹段，包括：

根据所述异常轨迹段的各异常位姿点的位姿参数构建待优化位姿矩阵集合；

根据所述两个延伸轨迹段的各紧耦合位姿点的位姿参数构建单边约束矩阵集合；

根据所述待优化位姿矩阵集合和所述补偿轨迹段的各松耦合位姿点的位姿参数构建双边约束矩阵集合；

将所述待优化位姿矩阵集合、单边约束矩阵集合和双边约束矩阵集合输入预设的非线性优化算法；

采用所述预设的非线性优化算法，以单边约束矩阵集合和双边约束矩阵集合为约束，对所述待优化位姿矩阵集合进行优化，以获得优化后的位姿矩阵集合；

根据所述优化后的位姿矩阵集合，确定并输出修复后的异常轨迹段。

5.根据权利要求1至4任一所述的方法，其特征在于，所述紧耦合位姿点和松耦合位姿点都包括以下位姿参数：

纬度、经度、高度、横滚角、俯仰角和航向角。

6.一种紧耦合轨迹修复装置，其特征在于，包括：

获取模块，用于获取由紧耦合位姿点组成的紧耦合轨迹和由松耦合位姿点组成的松耦合轨迹；

确定模块，用于确定所述紧耦合轨迹中的异常轨迹段和所述松耦合轨迹中与异常轨迹段匹配的补偿轨迹段；所述异常轨迹段包括多个异常位姿点；

修复模块，用于根据所述紧耦合轨迹、所述异常轨迹段和所述补偿轨迹段修复所述异常轨迹段；

所述确定模块具体用于：

根据轨迹平滑性检测算法确定紧耦合轨迹中的异常位姿点；确定各异常位姿点之间的相对距离；将异常位姿点和以该异常位姿点为中心间隔预设距离内的其他异常位姿点确定为异常轨迹段；其中，在异常轨迹段中该异常位姿点和其他异常位姿点之间的相对距离小于或等于预设距离；从松耦合轨迹中确定与异常轨迹段匹配的补偿轨迹段。