2026国赛-测量设备姿态角估计

一、原始数据管理

1、设计思路

六个数据文件都包含大量观测数据。如果将所有原始文本直接显示在 Qt 界面中，QTextEdit 需要保存、排版和绘制巨量文本，容易造成界面卡顿。

本程序不在界面中展示全部原始数据，而是先保存文件路径，再由 Python 在后台逐行读取和处理。界面只显示文件数量、处理状态和最终结果。

程序中的两个核心字典如下：

字典	键	值	作用
`file_paths`	数据名	文件绝对路径	记录用户选择的文件位置
`sensor_data`	数据名	`DataPoint` 对象列表	保存共同时间段内的观测数据

两个字典均以 acc_x、acc_y、acc_z、gyro_x、gyro_y、gyro_z 等数据名作为键。

2、读取并存储数据文件路径到file_paths

def open_file(self):
    global file_paths

    paths, _ = QFileDialog.getOpenFileNames(
        self,
        "选择六轴数据文件",
        "",
        "文本文件(*.txt)"
    )

    for path in paths:
        name = os.path.splitext(os.path.basename(path))[0]
        file_paths[name] = path

    self.original_text_area.setText(
        f"后台已成功读取{len(file_paths)}个数据文件。"
    )
    self.result_text_area.setText("等待处理数据...")
    self.status_label.setText("数据文件读取完成。")

该函数只记录文件路径到file_paths，不读取和显示全部文件内容，因此运行速度较快。

3、数据点类

class DataPoint:
    __slots__ = ("t", "v")

    def __init__(self, t, v):
        self.t = t
        self.v = v

t：观测时刻。
v：对应轴的观测值。
__slots__：减少大量 DataPoint 对象的内存开销。

不使用 __slots__ 时，类的属性更加灵活，但每个对象会占用更多内存；使用 __slots__ 后，实例属性固定，内存占用更低。需要添加新属性时，应先在 __slots__ 中声明，例如：

__slots__ = ("t", "v", "name")

4、读取并存储共同时间段数据到sensor_data

def read_(path, start_time, end_time):
    points = []
    start_ = start_time
    end_ = end_time

    with open(path, "r", encoding="utf-8") as file:
        for line_number, line in enumerate(file, 1):
            line = line.strip()
            if not line:
                continue

            parts = line.split(",", 1)
            time_value = float(parts[0])
            time_ = time_value

            if time_ < start_:
                continue
            if time_ > end_:
                break

            points.append(
                DataPoint(time_value, float(parts[1]))
            )

    return points

读取时采用：

for name, file_path in file_paths.items():
    self.status_label.setText(
        f"正在读取与计算{name}相关数据..."
    )
    QApplication.processEvents()

    sensor_data[name] = read_(
        file_path,
        start_time,
        end_time
    )

此时所有数据都存储到了sensor_data字典中，键为数据名，值为列表数据。（最难的部分完成了）

二、数据预处理与统计

1、共同时间段

六个数据文件的时间轴不同，共同时间段应满足：

共同起始时刻 = 六个文件起始时刻的最大值
共同终止时刻 = 六个文件终止时刻的最小值

本题数据的共同时间段为：220657.870 ～ 221860.555

2、单位转换

题目规定后续算法必须使用转换后的单位，因此可以直接覆盖 DataPoint.v，不必额外保存原始值。

def convert_(data):
    for name, points in data.items():
        if name.startswith("acc_"):
            for point in points:
                point.v /= interval
        else:
            for point in points:
                point.v = (
                    pi / 180
                ) * (point.v / interval)

3、异常点判断与剔除

重复点需要统计并剔除。
中断点只统计，不剔除已有的有效数据。
粗差点需要统计并剔除。

（1）重复点

连续数据的时间戳和观测值完全相同时，后续记录视为重复点。每组重复数据保留第一条。

def remove_duplicate_(points):
    normal_points = [points[0]]
    count = 0

    for point in points[1:]:
        previous = normal_points[-1]

        if point.t == previous.t and point.v == previous.v:
            count += 1
        else:
            normal_points.append(point)
	# 原地替换
    points[:] = normal_points
    return count

（2）中断点

相邻时间间隔超过采样间隔的 1.5 倍时，记为一个中断点。

def cnt_interrupt(points):
    count = 0
    threshold = interval * 1.5

    for i in range(1, len(points)):
        previous = points[i - 1]
        current = points[i]

        if current.t - previous.t > threshold:
            count += 1

    return count

（3）粗差点

陀螺各轴分别计算均值和总体标准差。当数据满足 |x - mean| > 3 * standard 时，判定为粗差点。

def remove_error(points):
    mean_ = sum(point.v for point in points) / len(points)

    standard_ = math.sqrt(
        sum(
            (point.v - mean_) ** 2
            for point in points
        ) / len(points)
    )

    normal_points = []
    count = 0

    for point in points:
        if abs(point.v - mean_) > 3 * standard_:
            count += 1
        else:
            normal_points.append(point)
	# 原地替换
    points[:] = normal_points
    return count

4、数据均值

异常数据处理完成后，计算各轴正常时间序列的均值：

def calculate_mean(points):
    return sum(point.v for point in points) / len(points)

陀螺均值在算法中必须保持 rad/s，仅在结果输出时乘以 1E6。

三、姿态角估计

姿态角采用方法 A 和方法 B 分别计算。两种方法都需要计算：

共同起始时刻单个数据的姿态角。
异常处理后共同时段均值的姿态角。

三角函数的输入和中间结果使用弧度，最终输出时转换为度。

1、方法A：加计调平和陀螺寻北

def method_a(ax, ay, az, wx, wy, wz):
    # 加计调平
    pitch = math.atan(
        -ay / math.sqrt(ax ** 2 + az ** 2)
    )
    roll = math.atan2(ax, az)

    # 陀螺调平
    wx_level = (
        wx * math.cos(roll)
        - wz * math.sin(roll)
    )

    wy_level = (
        wx * math.sin(pitch) * math.sin(roll)
        + wy * math.cos(pitch)
        + wz * math.sin(pitch) * math.cos(roll)
    )

    # 陀螺寻北
    yaw = math.atan2(-wx_level, wy_level)

    rad_to_deg = 180 / pi

    return (
        pitch * rad_to_deg,
        roll * rad_to_deg,
        yaw * rad_to_deg
    )

2、方法B：旋转矩阵转姿态角

def method_b(ax, ay, az, wx, wy, wz):
    # 已知参数及单位转换
    gravity = 9.794
    latitude = 30.5 * pi / 180
    earth_rate = 15.041 * pi / 180 / 3600

    cos_b = math.cos(latitude)
    tan_b = math.tan(latitude)

    # v = a x omega
    vx = ay * wz - az * wy
    vy = az * wx - ax * wz
    vz = ax * wy - ay * wx

    # 计算旋转矩阵
    denominator1 = gravity * earth_rate * cos_b
    denominator2 = earth_rate * cos_b

    c11 = -vx / denominator1
    c12 = -vy / denominator1
    c13 = -vz / denominator1

    c21 = -ax * tan_b / gravity + wx / denominator2
    c22 = -ay * tan_b / gravity + wy / denominator2
    c23 = -az * tan_b / gravity + wz / denominator2

    c31 = ax / gravity
    c32 = ay / gravity
    c33 = az / gravity

    matrix = (
        (c11, c12, c13),
        (c21, c22, c23),
        (c31, c32, c33)
    )

    # 旋转矩阵转姿态角
    pitch = -math.asin(c32)
    roll = math.atan2(c31, c33)
    yaw = math.atan2(c12, c22)

    rad_to_deg = 180 / pi

    return (
        pitch * rad_to_deg,
        roll * rad_to_deg,
        yaw * rad_to_deg,
        matrix
    )

四、实现要点

不要将全部原始数据放入 Qt 文本框，界面只显示状态、统计信息和计算结果。
使用 file_paths 管理文件位置，使用 sensor_data 管理参与算法的数据。
使用 DataPoint 统一保存时间和观测值，并通过 __slots__ 降低内存开销。
读取文件时逐行处理，并在超过共同终止时刻后立即停止。
单位转换后再进行异常检测、均值计算和姿态角估计。
重复点和粗差点需要剔除，中断点只进行统计。
陀螺均值在算法中使用 rad/s，仅在指定结果输出时乘以 1E6。
atan()、atan2()、asin() 的输入和返回值均为弧度，最终结果需要转换为度。
针对该题目，中间结果可以使用字典管理（对后续算法仍有用途的数据必须保存；仅用于最终展示的结果也应统一组织，以便输出Qt界面。），例如：duplicate_count = {}interrupt_count = {}error_count = {}mean_results = {}

五、结果文件和源程序

1、result.txt

序号,说明,计算结果
1,数据共同时间段的起始时刻,220657.870
2,数据共同时间段的终止时刻,221860.555
3,起始时刻转化后的三轴加计数据模长,9.7938
4,起始时刻转化后的三轴陀螺数据模长,92.602
5,加计X轴数据重复点总数,114
6,加计Y轴数据重复点总数,112
7,加计Z轴数据重复点总数,111
8,加计Y轴数据中断点总数,105
9,加计Z轴数据中断点总数,99
10,陀螺Y轴数据粗差总数,758
11,陀螺Z轴数据粗差总数,691
12,陀螺Y轴的均值,-26.1474
13,陀螺Z轴的均值,54.1405
14,加计Y轴的均值,-6.9254
15,加计Z轴的均值,5.9976
16,方法A(起始时刻)横滚角,30.0183
17,方法A(起始时刻)航向角,85.7761
18,方法A(共同时段)横滚角,30.0001
19,方法A(共同时段)俯仰角,45.0001
20,方法A(共同时段)航向角,89.9402
21,方法B(起始时刻)横滚角,30.0183
22,方法B(起始时刻)航向角,92.1449
23,方法B(起始时刻)姿态矩阵第1行第1列元素,0.533202
24,方法B(起始时刻)姿态矩阵第2行第1列元素,-1.021525
25,方法B(起始时刻)姿态矩阵第1行第3列元素,0.734439
26,方法B(起始时刻)姿态矩阵第1行第2列元素,0.902237
27,方法B(起始时刻)姿态矩阵第3行第3列元素,0.612099
28,方法B(共同时段)横滚角,30.0001
29,方法B(共同时段)俯仰角,45.0000
30,方法B(共同时段)航向角,89.9707

2、源码文件

import os
import sys
from PyQt5.QtWidgets import *
import math

# 一、数据预处理部分
class DataPoint:
    __slots__ = ("t", "v")
    def __init__(self, t, v):
        self.t = t
        self.v = v

sensor_data = {}
file_paths = {}

def read_(path, start_time, end_time):
    points = []
    # （1）将起止时间转化为整数，避免浮点精度问题
    start_ = start_time
    end_ = end_time
    # （2）读取并存储共同时间段数据
    with open(path, "r", encoding="utf-8") as file:
        for line_number, line in enumerate(file, 1):
            # 空行判断与跳过
            line = line.strip()
            if not line:
                continue
            parts = line.split(",", 1)
            time_value = float(parts[0])
            time_ = time_value
            # 核心过滤
            if time_ < start_:
                continue
            if time_ > end_:
                break
            # 存储共同时间段数据
            points.append(DataPoint(time_value, float(parts[1])))
    return points

pi = 3.141592653589790
interval = 0.005

# 二、算法部分
# 1、单位转换函数（直接覆盖v属性）
def convert_(m_dict):
    for name, points in m_dict.items():
        if name.startswith("acc_"):
            for p in points:
                p.v /= interval
        else:
            for p in points:
                p.v  = (pi / 180) * (p.v / 0.005)

# 2、统计并剔除重复点
def remove_duplicate_(points):
    # （1）
    normal_points = [points[0]]
    cnt = 0
    # （2）
    for point in points[1:]:
        previous = normal_points[-1]
        if point.t == previous.t and point.v == previous.v:
            cnt += 1
        else:
            normal_points.append(point)
    # （3）原地修改列表
    points[:] = normal_points
    # （4）
    return cnt

# 3、统计中断点
def cnt_interrupt(points):
    cnt = 0
    flag = interval * 1.5
    for i in range(1, len(points)):
        previous = points[i - 1]
        current = points[i]
        t_d = current.t - previous.t
        if t_d > flag:
            cnt += 1
    return cnt

# 4、统计并剔除粗差点
def remove_error(points):
    mean_ = sum(p.v for p in points) / len(points)
    standard_ = math.sqrt(sum((p.v - mean_) ** 2 for p in points) / len(points))
    normal_points = []
    cnt = 0
    for p in points:
        if abs(p.v - mean_) > 3 * standard_:
            cnt += 1
        else:
            normal_points.append(p)
    # 原地剔除粗差点
    points[:] = normal_points
    return cnt

# 5、计算时间序列均值
def calculate_mean(datum):
    return sum(data.v for data in datum) / len(datum)

# 6、方法A
def method_a(ax, ay, az, wx, wy, wz):
    # （1）
    p = math.atan(-ay / math.sqrt(ax ** 2 + az ** 2))
    r = math.atan2(ax, az)
    # （2）
    wx_l = wx * math.cos(r) - wz * math.sin(r)
    wy_l = wx * math.sin(p) * math.sin(r) + wy * math.cos(p) + wz * math.sin(p) * math.cos(r)
    # （3）
    y = math.atan2(-wx_l, wy_l)
    # （4）
    flag = 180 / pi
    return p * flag, r * flag, y * flag

# 7、方法B
def method_b(ax, ay, az, wx, wy, wz):
    # （1）数据准备
    g = 9.794
    b = 30.5 * (pi / 180)
    w = 15.041 * (pi / 180) / 3600
    cos_b = math.cos(b)
    tan_b = math.tan(b)
    # （2）叉乘
    vx = ay * wz - az * wy
    vy = az * wx - ax * wz
    vz = ax * wy - ay * wx
    # （3）计算旋转矩阵
    flag1 = g * w * cos_b
    flag2 = w * cos_b
    c11 = -vx / flag1
    c12 = -vy / flag1
    c13 = -vz / flag1
    c21 = -ax * tan_b / g + wx / flag2
    c22 = -ay * tan_b / g + wy / flag2
    c23 = -az * tan_b / g + wz / flag2
    c31 = ax / g
    c32 = ay / g
    c33 = az / g
    matrix = [[c11, c12, c13], [c21, c22, c23], [c31, c32, c33]]
    # （4）旋转矩阵转姿态角
    p = -math.asin(c32)
    r = math.atan2(c31, c33)
    y = math.atan2(c12, c22)
    flag = 180 / pi
    return p * flag, r * flag, y * flag, matrix

class App(QMainWindow):
    def __init__(self):
        super().__init__()
        self.setGeometry(100,100,850,600)
        self.setWindowTitle("测量设备姿态角估计")
        self._init_ui()
    def _init_ui(self):
        menu_bar = self.menuBar()
        file_menu = menu_bar.addMenu("文件")
        file_menu.addAction("📂打开", self.open_file)
        file_menu.addAction("🗃️保存", self.save_file)
        file_menu.addSeparator()
        file_menu.addAction("❌关闭", self.close)
        show_menu = menu_bar.addMenu("显示")
        show_menu.addAction("🖥️显示结果", self.process_data)
        show_menu.addAction("🧹清除结果", self.clear)
        menu_bar.addMenu("算法").addAction("⛷️运行算法", self.process_data)
        toolbar = self.addToolBar("工具栏")
        toolbar.addAction("📂打开文件", self.open_file)
        toolbar.addAction("⛷️运行算法", self.process_data)
        toolbar.addAction("🗃️保存结果", self.save_file)
        self.status_label = QLabel("✅就绪")
        self.statusBar().addPermanentWidget(self.status_label, 1)
        self.statusBar().setStyleSheet("background:#e8f4fd;")
        tab = QWidget()
        layout = QVBoxLayout(tab)
        layout.setContentsMargins(5,5,5,5)
        splitter = QSplitter()
        splitter.addWidget(self._make_panel("数据处理"))
        splitter.addWidget(self._make_panel("运行算法"))
        splitter.setSizes([425,425])
        layout.addWidget(splitter)
        notebook = QTabWidget()
        notebook.addTab(tab, "数据处理")
        self.setCentralWidget(notebook)
        self.original_text_area = self.panels[0]
        self.result_text_area = self.panels[1]
    def _make_panel(self, title):
        frame = QFrame()
        frame.setFrameShape(QFrame.StyledPanel)
        layout = QVBoxLayout(frame)
        layout.addWidget(QLabel(title))
        text = QTextEdit()
        text.setReadOnly(True)
        text.setStyleSheet("font-family:Consolas;font-size:10pt;")
        layout.addWidget(text)
        if not hasattr(self, 'panels'):
            self.panels = []
        self.panels.append(text)
        return frame
    def open_file(self):
        global file_paths
        # （1）
        paths, _ = QFileDialog.getOpenFileNames(self, "选择六轴数据文件","","文本文件(*.txt)")
        # （2）
        for path in paths:
            name = os.path.splitext(os.path.basename(path))[0]
            file_paths[name] = path
        # （3）
        self.original_text_area.setText(f"后台已成功读取{len(file_paths)}个数据文件。")
        self.result_text_area.setText("⌛️等待处理数据...")
        self.status_label.setText("✅数据文件读取完成。")
    def save_file(self):
        path, _ = QFileDialog.getSaveFileName(self, "保存文件", "", "文本文件(*.txt)")
        with open(path, "w", encoding = "utf-8") as f:
            f.write(self.result_text_area.toPlainText())
        self.status_label.setText(f"✅已将结果保存至：{path}。")
    def clear(self):
        self.result_text_area.clear()
        self.status_label.setText("✅数据已清除。")
    def process_data(self):
        try:
            processed_lines = ["序号,说明,计算结果"]

            # 一、数据读取与存储
            # sensor_data = {} # 值为列表，列表元素为类对象
            # file_paths = {} # 值为绝对路径
            # 以上三个字典的键都是数据名
            # 1、观察得到共同时间段的起始时刻与终止时刻
            start_time = 220657.870
            end_time = 221860.555
            processed_lines.append("1,数据共同时间段的起始时刻,220657.870")
            processed_lines.append("2,数据共同时间段的终止时刻,221860.555")
            # 2、读取所有文件
            for name, file_path in file_paths.items():
                # （1）状态栏更新
                self.status_label.setText(f"⌛️正在读取与计算{name}相关数据...")
                QApplication.processEvents()
                # （2）读取并存储到sensor_data字典
                path = file_paths[name]
                sensor_data[name] = read_(path, start_time, end_time)
            # 3、现在的所有数据都存储到了sensor_data字典中，键为数据名，值为列表数据

            # 二、数据预处理与统计
            # 1、单位转换
            convert_(sensor_data)
            acc_start_norm = math.sqrt(sensor_data["acc_x"][0].v ** 2 + sensor_data["acc_y"][0].v ** 2 + sensor_data["acc_z"][0].v ** 2)
            gyro_start_norm = (math.sqrt(sensor_data["gyro_x"][0].v ** 2 + sensor_data["gyro_y"][0].v ** 2 + sensor_data["gyro_z"][0].v ** 2)) * 1e6
            processed_lines.append(f"3,起始时刻转化后的三轴加计数据模长,{acc_start_norm:.4f}")
            processed_lines.append(f"4,起始时刻转化后的三轴陀螺数据模长,{gyro_start_norm:.3f}")
            # 2、异常点判断与剔除
            # （1）重复点
            duplicate_cnt = {}
            for name in ["acc_x", "acc_y", "acc_z"]:
                duplicate_cnt[name] = remove_duplicate_(sensor_data[name])
            processed_lines.append(f"5,加计X轴数据重复点总数,{duplicate_cnt['acc_x']}")
            processed_lines.append(f"6,加计Y轴数据重复点总数,{duplicate_cnt['acc_y']}")
            processed_lines.append(f"7,加计Z轴数据重复点总数,{duplicate_cnt['acc_z']}")
            # （2）中断点
            interrupt_cnt = {}
            for name in ["acc_x", "acc_y", "acc_z"]:
                interrupt_cnt[name] = cnt_interrupt(sensor_data[name])
            processed_lines.append(f"8,加计Y轴数据中断点总数,{interrupt_cnt['acc_y']}")
            processed_lines.append(f"9,加计Z轴数据中断点总数,{interrupt_cnt['acc_z']}")
            # （3）粗差点
            error_cnt = {}
            for name in ["gyro_x", "gyro_y", "gyro_z"]:
                error_cnt[name] = remove_error(sensor_data[name])
            processed_lines.append(f"10,陀螺Y轴数据粗差总数,{error_cnt['gyro_y']}")
            processed_lines.append(f"11,陀螺Z轴数据粗差总数,{error_cnt['gyro_z']}")
            # 3、数据均值计算
            mean_results = {}
            for name in ["acc_x", "acc_y", "acc_z", "gyro_x", "gyro_y", "gyro_z"]:
                mean_results[name] = calculate_mean(sensor_data[name])
            processed_lines.append(f"12,陀螺Y轴的均值,{mean_results['gyro_y'] * 1e6:.4f}")
            processed_lines.append(f"13,陀螺Z轴的均值,{mean_results['gyro_z'] * 1e6:.4f}")
            processed_lines.append(f"14,加计Y轴的均值,{mean_results['acc_y']:.4f}")
            processed_lines.append(f"15,加计Z轴的均值,{mean_results['acc_z']:.4f}")

            # 三、姿态角估计
            # 1、方法A，解算起始时刻姿态角
            start_p, start_r, start_y = method_a(sensor_data["acc_x"][0].v, sensor_data["acc_y"][0].v, sensor_data["acc_z"][0].v, sensor_data["gyro_x"][0].v, sensor_data["gyro_y"][0].v, sensor_data["gyro_z"][0].v)
            processed_lines.append(f"16,方法A(起始时刻)横滚角,{start_r:.4f}")
            processed_lines.append(f"17,方法A(起始时刻)航向角,{start_y:.4f}")
            # 2、方法A，解算共同时刻姿态角
            mean_p, mean_r, mean_y = method_a(mean_results["acc_x"], mean_results["acc_y"], mean_results["acc_z"], mean_results["gyro_x"], mean_results["gyro_y"], mean_results["gyro_z"])
            processed_lines.append(f"18,方法A(共同时段)横滚角,{mean_r:.4f}")
            processed_lines.append(f"19,方法A(共同时段)俯仰角,{mean_p:.4f}")
            processed_lines.append(f"20,方法A(共同时段)航向角,{mean_y:.4f}")
            # 3、方法B，解算起始时刻姿态角
            start_b_p, start_b_r, start_b_y, start_matrix = method_b(sensor_data["acc_x"][0].v, sensor_data["acc_y"][0].v, sensor_data["acc_z"][0].v, sensor_data["gyro_x"][0].v, sensor_data["gyro_y"][0].v, sensor_data["gyro_z"][0].v)
            processed_lines.append(f"21,方法B(起始时刻)横滚角,{start_b_r:.4f}")
            processed_lines.append(f"22,方法B(起始时刻)航向角,{start_b_y:.4f}")
            processed_lines.append(f"23,方法B(起始时刻)姿态矩阵第1行第1列元素,{start_matrix[0][0]:.6f}")
            processed_lines.append(f"24,方法B(起始时刻)姿态矩阵第2行第1列元素,{start_matrix[1][0]:.6f}")
            processed_lines.append(f"25,方法B(起始时刻)姿态矩阵第1行第3列元素,{start_matrix[0][2]:.6f}")
            processed_lines.append(f"26,方法B(起始时刻)姿态矩阵第1行第2列元素,{start_matrix[0][1]:.6f}")
            processed_lines.append(f"27,方法B(起始时刻)姿态矩阵第3行第3列元素,{start_matrix[2][2]:.6f}")
            # 4、方法B，解算共同时刻姿态角
            mean_b_p, mean_b_r, mean_b_y, mean_matrix = method_b(mean_results["acc_x"], mean_results["acc_y"], mean_results["acc_z"], mean_results["gyro_x"], mean_results["gyro_y"], mean_results["gyro_z"])
            processed_lines.append(f"28,方法B(共同时段)横滚角,{mean_b_r:.4f}")
            processed_lines.append(f"29,方法B(共同时段)俯仰角,{mean_b_p:.4f}")
            processed_lines.append(f"30,方法B(共同时段)航向角,{mean_b_y:.4f}")

            self.result_text_area.setPlainText("\n".join(processed_lines))
            self.status_label.setText("✅数据处理成功！")
        except Exception as e:
            self.status_label.setText(f"❌数据处理失败：{e}。")
if __name__ == "__main__":
    app = QApplication(sys.argv)
    window = App()
    window.show()
    sys.exit(app.exec_())