项目开发背景

随着物联网和智能制造的推进，室内环境对高精度定位技术的需求日益增长。全球定位系统（GPS）在室外应用中表现卓越，但在室内场景中，由于建筑结构的遮挡和多径效应，其信号衰减严重，无法提供可靠的位置信息。这促使了室内定位技术的发展，以解决仓库、工厂、医院等封闭空间中对人员、设备实时跟踪的迫切需求。超宽带（UWB）技术以其高时间分辨率、低功耗和强抗干扰能力脱颖而出，成为实现厘米级精度的关键，为本项目的开发提供了技术基础。

在工业自动化、智能仓储和应急救援等领域，精确的三维定位能够优化资源调度、提升运营效率并增强安全保障。传统室内定位方法如Wi-Fi或蓝牙定位，往往受限于精度不足或成本高昂，难以满足复杂环境下的动态跟踪要求。因此，本项目旨在利用UWB技术构建一个经济实用的三维定位跟踪系统，通过部署基站网络和便携标签，实现对移动目标的实时监控，并集成电量检测和报警功能，以应对实际应用中的多样化挑战。

本项目的开发还响应了技术集成与创新的趋势，通过结合DW1000芯片、STM32主控器和Python上位机软件，设计了一个从硬件到软件的全栈解决方案。这不仅降低了系统部署成本，还提高了可扩展性和用户友好性，为室内定位技术的普及和行业应用提供了有力支持。

设计实现的功能

（1）实现UWB测距功能，采用基于DW1000芯片的模块作为基站和标签进行双向测距。
（2）实现定位解算功能，采用STM32F407VET6单片机作为主控器进行数据汇聚和三维坐标解算。
（3）实现通信同步功能，通过ESP-Now或有线串口进行基站间时钟同步和数据回传。
（4）实现上位机显示功能，采用Python+PyQt5编写软件接收串口数据并可视化运动轨迹。
（5）实现电源管理功能，基站采用5V电源适配器供电，标签采用600mAh锂电池与TP4056充电电路供电。

项目硬件模块组成

（1）测距核心模块：采用基于DW1000芯片的UWB模块（如DWM1000）作为基站和标签。
（2）定位解算主控：采用STM32F407VET6单片机作为定位服务器，负责数据汇聚与坐标解算。
（3）通信同步模块：基站间通过ESP-Now或有线串口进行时钟同步和数据回传。
（4）上位机显示：采用Python+PyQt5编写上位机软件，接收串口数据并可视化。
（5）电源与结构：基站采用5V电源适配器供电，标签采用600mAh锂电池与TP4056充电电路。

设计意义

该系统实现了高精度室内定位，通过UWB技术提供厘米级的测距能力，显著提升了传统室内定位方法的准确性，适用于对位置精度要求严格的场景，如工业自动化或医疗设备跟踪。其实时坐标解算和动态轨迹显示功能，使操作人员能够直观监控人员或设备的移动，增强了现场管理的效率和响应速度，为安全监控和资源调度提供了可靠支持。

系统集成硬件模块如DW1000芯片和STM32主控器，确保了稳定高效的数据处理，而通信同步模块保障了基站间的协调运作，降低了误差累积，体现了技术实用性与成本效益的平衡。同时，标签电量检测和速度报警功能增强了系统的可靠性，有助于预防因设备故障或异常移动导致的安全风险，延长了设备使用寿命。

该设计在仓储物流、智能工厂或紧急救援等室内环境中具有广泛的应用价值，通过提供精确的三维位置信息，优化了工作流程和安全管理，推动了物联网技术在现实场景中的落地。其模块化架构也为后续维护和扩展奠定了基础，无需复杂改造即可适应不同规模的部署需求。

设计思路

系统设计以UWB技术为基础，通过部署至少四个基于DW1000芯片的UWB模块作为定位基站，构建一个覆盖特定室内区域的定位网络。这些基站固定在空间的关键位置，确保佩戴在人员或设备上的UWB标签能够与多个基站进行可靠通信，形成稳定的测距环境。标签同样采用DW1000模块，定时与各基站进行双向测距，以获取精确的距离数据，为后续坐标解算提供基础。

定位解算主控采用STM32F407VET6单片机作为核心处理器，负责汇聚所有基站传回的测距数据。该主控器运用三边定位算法或卡尔曼滤波算法来实时解算标签的三维坐标（X, Y, Z），其中卡尔曼滤波可用于优化动态跟踪中的精度和稳定性。这一过程确保了系统能够高精度地输出位置信息，满足室内定位的实时性要求。

基站之间的时钟同步和数据回传通过ESP-Now无线通信或有线串口实现，以保证测距数据的同步性和准确性，避免因时间偏差导致的定位误差。解算出的坐标数据通过串口实时发送至上位机，上位机软件由Python和PyQt5编写，负责接收串口数据并将其可视化在二维地图上，动态显示标签的运动轨迹，便于用户监控和分析。

系统还集成了辅助功能，包括标签电量检测和移动速度超限报警。电量检测通过监控600mAh锂电池的电压状态实现，而移动速度超限报警则在标签速度超过预设阈值时触发警示。电源方面，基站采用5V电源适配器供电以确保稳定运行，标签则使用600mAh锂电池配合TP4056充电电路，支持便携和可持续使用。

框架图

                          +-------------------+
                          |   UWB标签         |
                          |  (DW1000模块)     |
                          |  电源: 锂电池+TP4056|
                          |  电量检测和速度报警|
                          +-------------------+
                                  /|\
                                   | 双向测距
                                   |
          +-------------------+    |    +-------------------+    +-------------------+
          |   UWB基站1        |<---+--->|   UWB基站2        |    |   UWB基站3...N    |
          |  (DW1000模块)     |         |  (DW1000模块)     |    |  (至少4个基站)    |
          |   电源: 5V适配器  |         |   电源: 5V适配器  |    |   电源: 5V适配器  |
          +-------------------+         +-------------------+    +-------------------+
                  |                               |                         |
                  |  ESP-Now/串口同步和数据回传   |  ESP-Now/串口同步和数据回传   |
                  |                               |                         |
                  +-------------------------------+-------------------------+
                                                    |
                                            +-------------------+
                                            |  定位主控器       |
                                            |  STM32F407VET6   |
                                            |  收集测距数据并解算|
                                            |  三维坐标(X,Y,Z)  |
                                            +-------------------+
                                                    |
                                                    | 串口通信
                                                    |
                                            +-------------------+
                                            |  上位机软件       |
                                            |  Python+PyQt5    |
                                            |  实时接收坐标数据 |
                                            |  二维地图显示轨迹 |
                                            |  超限报警和电量显示|
                                            +-------------------+

系统总体设计

高精度室内UWB三维定位跟踪系统旨在通过部署UWB技术实现人员或设备在特定区域内的实时三维坐标跟踪。系统覆盖区域由至少四个UWB定位基站构成网络，确保测距和定位的准确性，核心目标是采集并解算数据以动态显示运动轨迹。

系统硬件模块以基于DW1000芯片的UWB模块（如DWM1000）作为测距核心，分别用于基站和佩戴标签；定位解算主控采用STM32F407VET6单片机作为定位服务器，负责数据汇聚与坐标计算。通信同步模块支持基站间通过ESP-Now或有线串口进行时钟同步和数据回传，以维持系统时间一致性。

系统工作原理依赖于UWB标签定时与各基站进行双向测距，生成距离数据；定位主控器收集所有测距信息后，采用三边定位或卡尔曼滤波算法解算出标签的三维坐标（X，Y，Z）。解算过程在单片机内实时完成，确保定位精度和响应速度。

坐标数据通过串口实时发送至上位机软件，该软件采用Python和PyQt5编写，接收串口数据并在二维地图上可视化动态运动轨迹。同时，系统集成了标签电量检测功能，以及基于移动速度的超限报警机制，以增强实用性和安全性。

电源与结构设计方面，基站采用5V电源适配器供电，保证稳定运行；标签则配备600mAh锂电池与TP4056充电电路，支持便携使用和续航。整个系统通过硬件协同和算法处理，实现从数据采集到显示的完整定位跟踪流程。

系统功能总结

设计的各个功能模块描述

测距核心模块采用基于DW1000芯片的UWB模块（如DWM1000）作为基站和标签，实现高精度双向测距功能。基站部署在定位区域内构成覆盖网络，标签佩戴在人员或设备上定时与各基站进行测距，获取距离数据为后续定位解算提供基础。

定位解算主控采用STM32F407VET6单片机作为定位服务器，负责汇聚来自所有基站的测距数据。通过三边定位或卡尔曼滤波算法处理数据，解算出标签的三维坐标（X，Y，Z），并管理系统的整体逻辑，包括坐标实时输出和功能控制。

通信同步模块通过ESP-Now无线协议或有线串口实现基站间的时钟同步和数据回传。这确保了测距过程的准确性和数据时效性，为定位解算提供可靠的协同基础，支持系统的稳定运行。

上位机显示采用Python和PyQt5编写的软件，通过串口接收来自定位主控器的实时坐标数据。软件将数据可视化在二维地图上，动态显示标签的运动轨迹，并提供用户界面进行实时监控和轨迹回放。

电源与结构中，基站采用5V电源适配器供电以保证持续运行；标签采用600mAh锂电池供电，并集成TP4056充电电路进行充电管理。系统还具备标签电量检测和移动速度超限报警功能，增强实用性和安全性。

上位机代码设计

// main.cpp - 高精度室内UWB三维定位跟踪系统上位机
#include <QApplication>
#include <QMainWindow>
#include <QWidget>
#include <QVBoxLayout>
#include <QHBoxLayout>
#include <QSerialPort>
#include <QSerialPortInfo>
#include <QPushButton>
#include <QComboBox>
#include <QLabel>
#include <QLineEdit>
#include <QTableWidget>
#include <QGroupBox>
#include <QStatusBar>
#include <QTimer>
#include <QMessageBox>
#include <QChart>
#include <QChartView>
#include <QLineSeries>
#include <QScatterSeries>
#include <QValueAxis>
#include <QDateTime>
#include <QFile>
#include <QTextStream>
#include <cmath>

using namespace QtCharts;

// 坐标数据结构
struct PositionData {
    int tagId;
    double x;
    double y;
    double z;
    double battery;  // 电池百分比
    double speed;    // 速度 m/s
    QDateTime timestamp;
};

// 基站信息
struct BaseStation {
    int id;
    double x;
    double y;
    double z;
};

// 主窗口类
class MainWindow : public QMainWindow {
    Q_OBJECT
    
public:
    MainWindow(QWidget *parent = nullptr) : QMainWindow(parent) {
        setupUI();
        setupSerial();
        setupChart();
        setupTimer();
        
        // 初始化基站位置（示例数据，实际应可配置）
        baseStations.append({1, 0.0, 0.0, 2.5});
        baseStations.append({2, 10.0, 0.0, 2.5});
        baseStations.append({3, 10.0, 10.0, 2.5});
        baseStations.append({4, 0.0, 10.0, 2.5});
        
        // 初始化历史数据存储
        maxHistoryPoints = 1000;
    }
    
    ~MainWindow() {
        if (serialPort->isOpen()) {
            serialPort->close();
        }
        delete dataTimer;
    }
    
private slots:
    void onConnectClicked() {
        if (!serialPort->isOpen()) {
            serialPort->setPortName(portComboBox->currentText());
            serialPort->setBaudRate(baudRateComboBox->currentText().toInt());
            serialPort->setDataBits(QSerialPort::Data8);
            serialPort->setParity(QSerialPort::NoParity);
            serialPort->setStopBits(QSerialPort::OneStop);
            serialPort->setFlowControl(QSerialPort::NoFlowControl);
            
            if (serialPort->open(QIODevice::ReadWrite)) {
                connectButton->setText("断开连接");
                statusBar()->showMessage("已连接到串口: " + serialPort->portName(), 3000);
            } else {
                QMessageBox::critical(this, "错误", "无法打开串口");
            }
        } else {
            serialPort->close();
            connectButton->setText("连接串口");
            statusBar()->showMessage("串口已断开", 3000);
        }
    }
    
    void onRefreshPortsClicked() {
        portComboBox->clear();
        QList<QSerialPortInfo> ports = QSerialPortInfo::availablePorts();
        for (const QSerialPortInfo &port : ports) {
            portComboBox->addItem(port.portName());
        }
    }
    
    void onSerialReadyRead() {
        QByteArray data = serialPort->readAll();
        serialBuffer.append(data);
        
        // 解析数据（假设数据以换行符结尾）
        while (serialBuffer.contains('\n')) {
            int endIndex = serialBuffer.indexOf('\n');
            QByteArray line = serialBuffer.left(endIndex).trimmed();
            serialBuffer = serialBuffer.mid(endIndex + 1);
            
            if (!line.isEmpty()) {
                processPositionData(line);
            }
        }
    }
    
    void onDataTimeout() {
        // 定时更新显示
        updateDisplay();
    }
    
    void onClearClicked() {
        tagPositions.clear();
        positionHistory.clear();
        updateChart();
        statusBar()->showMessage("轨迹已清除", 2000);
    }
    
    void onSaveDataClicked() {
        QFile file("position_data.csv");
        if (file.open(QIODevice::WriteOnly | QIODevice::Text)) {
            QTextStream out(&file);
            out << "时间,标签ID,X坐标,Y坐标,Z坐标,电量%,速度m/s\n";
            
            for (const auto& pos : positionHistory) {
                out << pos.timestamp.toString("yyyy-MM-dd hh:mm:ss.zzz") << ","
                    << pos.tagId << ","
                    << QString::number(pos.x, 'f', 3) << ","
                    << QString::number(pos.y, 'f', 3) << ","
                    << QString::number(pos.z, 'f', 3) << ","
                    << QString::number(pos.battery, 'f', 1) << ","
                    << QString::number(pos.speed, 'f', 2) << "\n";
            }
            
            file.close();
            statusBar()->showMessage("数据已保存到position_data.csv", 3000);
        }
    }
    
    void onSpeedLimitChanged() {
        speedLimit = speedLimitEdit->text().toDouble();
    }
    
private:
    void setupUI() {
        // 创建中心部件
        QWidget *centralWidget = new QWidget(this);
        setCentralWidget(centralWidget);
        QVBoxLayout *mainLayout = new QVBoxLayout(centralWidget);
        
        // 串口控制区域
        QGroupBox *serialGroup = new QGroupBox("串口设置");
        QHBoxLayout *serialLayout = new QHBoxLayout();
        
        portComboBox = new QComboBox();
        baudRateComboBox = new QComboBox();
        baudRateComboBox->addItems({"9600", "115200", "230400", "460800", "921600"});
        baudRateComboBox->setCurrentIndex(1); // 115200
        
        connectButton = new QPushButton("连接串口");
        refreshButton = new QPushButton("刷新串口");
        
        serialLayout->addWidget(new QLabel("端口:"));
        serialLayout->addWidget(portComboBox);
        serialLayout->addWidget(new QLabel("波特率:"));
        serialLayout->addWidget(baudRateComboBox);
        serialLayout->addWidget(connectButton);
        serialLayout->addWidget(refreshButton);
        serialGroup->setLayout(serialLayout);
        
        // 图表显示区域
        chartView = new QChartView();
        chartView->setRenderHint(QPainter::Antialiasing);
        
        // 控制区域
        QGroupBox *controlGroup = new QGroupBox("控制");
        QHBoxLayout *controlLayout = new QHBoxLayout();
        
        clearButton = new QPushButton("清除轨迹");
        saveButton = new QPushButton("保存数据");
        controlLayout->addWidget(clearButton);
        controlLayout->addWidget(saveButton);
        controlLayout->addStretch();
        
        controlLayout->addWidget(new QLabel("速度报警阈值(m/s):"));
        speedLimitEdit = new QLineEdit("2.0");
        speedLimitEdit->setMaximumWidth(60);
        controlLayout->addWidget(speedLimitEdit);
        
        controlGroup->setLayout(controlLayout);
        
        // 状态显示区域
        QGroupBox *statusGroup = new QGroupBox("标签状态");
        QGridLayout *statusLayout = new QGridLayout();
        
        statusLabels.resize(6);
        QStringList labels = {"标签ID:", "X坐标:", "Y坐标:", "Z坐标:", "电量:", "速度:"};
        for (int i = 0; i < 6; i++) {
            statusLayout->addWidget(new QLabel(labels[i]), i, 0);
            statusLabels[i] = new QLabel("---");
            statusLayout->addWidget(statusLabels[i], i, 1);
        }
        statusGroup->setLayout(statusLayout);
        
        // 基站信息区域
        QGroupBox *baseGroup = new QGroupBox("基站信息");
        baseTable = new QTableWidget(4, 4);
        QStringList headers = {"基站ID", "X", "Y", "Z"};
        baseTable->setHorizontalHeaderLabels(headers);
        baseTable->verticalHeader()->setVisible(false);
        baseTable->setEditTriggers(QAbstractItemView::NoEditTriggers);
        
        // 填充基站数据
        for (int row = 0; row < 4; row++) {
            baseTable->setItem(row, 0, new QTableWidgetItem(QString::number(row + 1)));
            baseTable->setItem(row, 1, new QTableWidgetItem("0.00"));
            baseTable->setItem(row, 2, new QTableWidgetItem("0.00"));
            baseTable->setItem(row, 3, new QTableWidgetItem("2.50"));
        }
        
        QVBoxLayout *baseLayout = new QVBoxLayout();
        baseLayout->addWidget(baseTable);
        baseGroup->setLayout(baseLayout);
        
        // 右侧布局
        QWidget *rightPanel = new QWidget();
        QVBoxLayout *rightLayout = new QVBoxLayout(rightPanel);
        rightLayout->addWidget(statusGroup);
        rightLayout->addWidget(baseGroup);
        rightLayout->addStretch();
        
        // 主布局
        QHBoxLayout *contentLayout = new QHBoxLayout();
        contentLayout->addWidget(chartView, 4);
        contentLayout->addWidget(rightPanel, 1);
        
        mainLayout->addWidget(serialGroup);
        mainLayout->addLayout(contentLayout, 4);
        mainLayout->addWidget(controlGroup);
        
        // 设置窗口
        setWindowTitle("高精度室内UWB三维定位跟踪系统");
        resize(1200, 800);
        
        // 连接信号槽
        connect(connectButton, &QPushButton::clicked, this, &MainWindow::onConnectClicked);
        connect(refreshButton, &QPushButton::clicked, this, &MainWindow::onRefreshPortsClicked);
        connect(clearButton, &QPushButton::clicked, this, &MainWindow::onClearClicked);
        connect(saveButton, &QPushButton::clicked, this, &MainWindow::onSaveDataClicked);
        connect(speedLimitEdit, &QLineEdit::editingFinished, this, &MainWindow::onSpeedLimitChanged);
        
        // 初始化串口列表
        onRefreshPortsClicked();
    }
    
    void setupSerial() {
        serialPort = new QSerialPort(this);
        connect(serialPort, &QSerialPort::readyRead, this, &MainWindow::onSerialReadyRead);
    }
    
    void setupChart() {
        chart = new QChart();
        chart->setTitle("UWB定位轨迹图");
        chart->setAnimationOptions(QChart::NoAnimation);
        
        // 坐标轴
        axisX = new QValueAxis();
        axisX->setTitleText("X坐标 (米)");
        axisX->setRange(-5, 15);
        axisX->setTickCount(11);
        axisX->setLabelFormat("%.1f");
        
        axisY = new QValueAxis();
        axisY->setTitleText("Y坐标 (米)");
        axisY->setRange(-5, 15);
        axisY->setTickCount(11);
        axisY->setLabelFormat("%.1f");
        
        // 基站位置系列
        baseSeries = new QScatterSeries();
        baseSeries->setName("基站");
        baseSeries->setMarkerShape(QScatterSeries::MarkerShapeCircle);
        baseSeries->setMarkerSize(15);
        baseSeries->setColor(Qt::red);
        baseSeries->setBorderColor(Qt::black);
        
        // 轨迹系列
        trajectorySeries = new QLineSeries();
        trajectorySeries->setName("运动轨迹");
        trajectorySeries->setColor(Qt::blue);
        trajectorySeries->setPointsVisible(true);
        
        // 当前点系列
        currentPointSeries = new QScatterSeries();
        currentPointSeries->setName("当前位置");
        currentPointSeries->setMarkerShape(QScatterSeries::MarkerShapeRectangle);
        currentPointSeries->setMarkerSize(20);
        currentPointSeries->setColor(Qt::green);
        currentPointSeries->setBorderColor(Qt::black);
        
        chart->addSeries(baseSeries);
        chart->addSeries(trajectorySeries);
        chart->addSeries(currentPointSeries);
        chart->addAxis(axisX, Qt::AlignBottom);
        chart->addAxis(axisY, Qt::AlignLeft);
        
        baseSeries->attachAxis(axisX);
        baseSeries->attachAxis(axisY);
        trajectorySeries->attachAxis(axisX);
        trajectorySeries->attachAxis(axisY);
        currentPointSeries->attachAxis(axisX);
        currentPointSeries->attachAxis(axisY);
        
        chartView->setChart(chart);
        
        // 初始显示基站位置
        updateBaseStationsOnChart();
    }
    
    void setupTimer() {
        dataTimer = new QTimer(this);
        dataTimer->setInterval(100); // 100ms更新一次
        connect(dataTimer, &QTimer::timeout, this, &MainWindow::onDataTimeout);
        dataTimer->start();
    }
    
    void processPositionData(const QByteArray &data) {
        // 数据格式: TagID,X,Y,Z,Battery,Speed
        // 示例: "1,2.345,3.456,1.234,85.5,1.23"
        QString strData = QString::fromUtf8(data);
        QStringList parts = strData.split(',');
        
        if (parts.size() >= 6) {
            PositionData pos;
            pos.tagId = parts[0].toInt();
            pos.x = parts[1].toDouble();
            pos.y = parts[2].toDouble();
            pos.z = parts[3].toDouble();
            pos.battery = parts[4].toDouble();
            pos.speed = parts[5].toDouble();
            pos.timestamp = QDateTime::currentDateTime();
            
            // 保存数据
            tagPositions[pos.tagId] = pos;
            positionHistory.append(pos);
            
            // 限制历史数据大小
            if (positionHistory.size() > maxHistoryPoints) {
                positionHistory.removeFirst();
            }
            
            // 速度超限报警
            if (pos.speed > speedLimit) {
                QString warning = QString("标签%1速度超限: %2 m/s").arg(pos.tagId).arg(pos.speed, 0, 'f', 2);
                statusBar()->showMessage(warning, 2000);
                QMessageBox::warning(this, "速度超限报警", warning);
            }
            
            // 电量低报警
            if (pos.battery < 20.0) {
                QString warning = QString("标签%1电量低: %2%").arg(pos.tagId).arg(pos.battery, 0, 'f', 1);
                statusBar()->showMessage(warning, 2000);
            }
        }
    }
    
    void updateDisplay() {
        if (!tagPositions.isEmpty()) {
            // 显示最新标签数据
            auto latest = tagPositions.begin().value();
            
            statusLabels[0]->setText(QString::number(latest.tagId));
            statusLabels[1]->setText(QString::number(latest.x, 'f', 3));
            statusLabels[2]->setText(QString::number(latest.y, 'f', 3));
            statusLabels[3]->setText(QString::number(latest.z, 'f', 3));
            statusLabels[4]->setText(QString::number(latest.battery, 'f', 1) + "%");
            statusLabels[5]->setText(QString::number(latest.speed, 'f', 2) + " m/s");
            
            // 更新图表
            updateChart();
        }
    }
    
    void updateChart() {
        // 清除系列数据
        trajectorySeries->clear();
        currentPointSeries->clear();
        
        if (!positionHistory.isEmpty()) {
            // 添加轨迹点
            for (const auto& pos : positionHistory) {
                trajectorySeries->append(pos.x, pos.y);
            }
            
            // 添加当前点
            auto latest = positionHistory.last();
            currentPointSeries->append(latest.x, latest.y);
            
            // 调整坐标轴范围
            adjustAxisRange();
        }
    }
    
    void updateBaseStationsOnChart() {
        baseSeries->clear();
        for (const auto& bs : baseStations) {
            baseSeries->append(bs.x, bs.y);
        }
    }
    
    void adjustAxisRange() {
        if (positionHistory.isEmpty()) return;
        
        double minX = positionHistory[0].x;
        double maxX = positionHistory[0].x;
        double minY = positionHistory[0].y;
        double maxY = positionHistory[0].y;
        
        for (const auto& pos : positionHistory) {
            minX = qMin(minX, pos.x);
            maxX = qMax(maxX, pos.x);
            minY = qMin(minY, pos.y);
            maxY = qMax(maxY, pos.y);
        }
        
        // 添加边距
        double margin = 2.0;
        axisX->setRange(minX - margin, maxX + margin);
        axisY->setRange(minY - margin, maxY + margin);
    }
    
    // 成员变量
    QSerialPort *serialPort;
    QByteArray serialBuffer;
    
    // UI组件
    QComboBox *portComboBox;
    QComboBox *baudRateComboBox;
    QPushButton *connectButton;
    QPushButton *refreshButton;
    QPushButton *clearButton;
    QPushButton *saveButton;
    QLineEdit *speedLimitEdit;
    QLabel *speedWarningLabel;
    
    QVector<QLabel*> statusLabels;
    QTableWidget *baseTable;
    
    // 图表相关
    QChartView *chartView;
    QChart *chart;
    QValueAxis *axisX;
    QValueAxis *axisY;
    QLineSeries *trajectorySeries;
    QScatterSeries *currentPointSeries;
    QScatterSeries *baseSeries;
    
    // 数据
    QMap<int, PositionData> tagPositions;
    QList<PositionData> positionHistory;
    QList<BaseStation> baseStations;
    
    QTimer *dataTimer;
    
    // 配置参数
    int maxHistoryPoints;
    double speedLimit = 2.0;
};

// main函数
int main(int argc, char *argv[]) {
    QApplication app(argc, argv);
    
    // 设置应用程序信息
    QApplication::setApplicationName("UWB定位系统");
    QApplication::setOrganizationName("定位实验室");
    
    MainWindow window;
    window.show();
    
    return app.exec();
}

#include "main.moc"

这个C++上位机程序使用Qt框架开发，具有以下功能：

1. 串口通信：

   • 自动检测可用串口
   • 支持常见波特率选择
   • 实时接收并解析定位数据

2. 数据可视化：

   • 二维平面显示标签运动轨迹
   • 基站位置显示（红色圆点）
   • 当前位置显示（绿色方点）
   • 轨迹历史记录（蓝色线条）

3. 状态监控：

   • 实时显示标签位置信息（X, Y, Z坐标）
   • 电池电量显示
   • 移动速度监控

4. 报警功能：

   • 速度超限报警（可设置阈值）
   • 低电量报警

5. 数据管理：

   • 轨迹清除功能
   • 数据导出到CSV文件
   • 基站信息配置显示

6. 用户界面：

   • 直观的控制面板
   • 实时状态显示
   • 响应式图表显示

程序使用以下数据格式：
TagID,X,Y,Z,Battery,Speed
例如：1,2.345,3.456,1.234,85.5,1.23

要编译此程序，需要安装Qt5及以上版本，并在.pro文件中添加以下模块：

QT += core gui serialport charts

程序可以扩展的功能包括：3D轨迹显示、多标签同时跟踪、区域围栏报警、数据回放等。

模块代码设计

/* 
 * UWB三维定位系统 - STM32F407VET6主控代码
 * 寄存器方式开发，需配合DW1000模块
 */

#include "stm32f4xx.h"

/* DW1000寄存器定义 */
#define DW1000_SPI           SPI1
#define DW1000_CS_PIN        GPIO_Pin_4
#define DW1000_CS_PORT       GPIOA
#define DW1000_IRQ_PIN       GPIO_Pin_5
#define DW1000_IRQ_PORT      GPIOA

/* 基站数量 */
#define ANCHOR_COUNT         4

/* 结构体定义 */
typedef struct {
    float x;
    float y;
    float z;
} Coordinate3D;

typedef struct {
    uint32_t id;
    float distance;
    uint8_t status;
} AnchorData;

/* 全局变量 */
AnchorData anchors[ANCHOR_COUNT];
Coordinate3D current_position;
uint8_t battery_level = 100;
float current_speed = 0.0f;

/* 函数声明 */
void System_Init(void);
void GPIO_Init(void);
void SPI1_Init(void);
void USART1_Init(uint32_t baudrate);
void TIM2_Init(void);
void ADC1_Init(void);
void DW1000_WriteReg(uint16_t reg, uint32_t data);
uint32_t DW1000_ReadReg(uint16_t reg);
void DW1000_Init(void);
void Get_Distances(void);
void Trilateration_3D(void);
void Kalman_Filter_Update(void);
void Send_To_PC(void);
void Check_Battery(void);
void Check_Speed(void);

/* 系统初始化 */
void System_Init(void) {
    /* 使能时钟 */
    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN | RCC_AHB1ENR_GPIOBEN;
    RCC->APB1ENR |= RCC_APB1ENR_TIM2EN;
    RCC->APB2ENR |= RCC_APB2ENR_SPI1EN | RCC_APB2ENR_ADC1EN | RCC_APB2ENR_USART1EN;
    
    /* 系统时钟配置为168MHz */
    FLASH->ACR |= FLASH_ACR_LATENCY_5WS;
    RCC->PLLCFGR = (8 << 0) | (336 << 6) | (2 << 16) | (7 << 24);
    RCC->CR |= RCC_CR_PLLON;
    while(!(RCC->CR & RCC_CR_PLLRDY));
    RCC->CFGR |= RCC_CFGR_SW_PLL;
    while((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL);
}

/* GPIO初始化 */
void GPIO_Init(void) {
    /* SPI引脚: PA5-SCK, PA6-MISO, PA7-MOSI */
    GPIOA->MODER |= GPIO_MODER_MODER5_1 | GPIO_MODER_MODER6_1 | GPIO_MODER_MODER7_1;
    GPIOA->AFR[0] |= (5 << 20) | (5 << 24) | (5 << 28);
    
    /* CS引脚输出 */
    GPIOA->MODER |= GPIO_MODER_MODER4_0;
    GPIOA->BSRR = DW1000_CS_PIN;
    
    /* 串口引脚: PA9-TX, PA10-RX */
    GPIOA->MODER |= GPIO_MODER_MODER9_1 | GPIO_MODER_MODER10_1;
    GPIOA->AFR[1] |= (7 << 4) | (7 << 8);
    
    /* 中断引脚输入 */
    GPIOA->MODER &= ~GPIO_MODER_MODER5;
    GPIOA->PUPDR |= GPIO_PUPDR_PUPDR5_0;
}

/* SPI1初始化 */
void SPI1_Init(void) {
    SPI1->CR1 = SPI_CR1_BR_0 | SPI_CR1_MSTR | SPI_CR1_SSM | SPI_CR1_SSI;
    SPI1->CR2 = SPI_CR2_DS_2 | SPI_CR2_DS_1 | SPI_CR2_DS_0;
    SPI1->CR1 |= SPI_CR1_SPE;
}

/* 串口初始化 */
void USART1_Init(uint32_t baudrate) {
    uint32_t usartdiv = 84000000 / baudrate;
    USART1->BRR = usartdiv;
    USART1->CR1 = USART_CR1_TE | USART_CR1_RE | USART_CR1_UE;
}

/* 定时器2初始化（用于定时测距） */
void TIM2_Init(void) {
    TIM2->PSC = 8400 - 1;     // 10kHz计数
    TIM2->ARR = 10000 - 1;    // 1秒中断
    TIM2->DIER = TIM_DIER_UIE;
    TIM2->CR1 = TIM_CR1_CEN;
    NVIC_EnableIRQ(TIM2_IRQn);
}

/* ADC1初始化（用于电量检测） */
void ADC1_Init(void) {
    ADC1->SQR3 = 0;          // 通道0
    ADC1->SMPR2 = ADC_SMPR2_SMP0_0 | ADC_SMPR2_SMP0_1 | ADC_SMPR2_SMP0_2;
    ADC1->CR2 = ADC_CR2_ADON;
}

/* DW1000寄存器写操作 */
void DW1000_WriteReg(uint16_t reg, uint32_t data) {
    uint8_t buffer[7];
    buffer[0] = 0x80 | (reg >> 8);
    buffer[1] = reg & 0xFF;
    buffer[2] = data >> 24;
    buffer[3] = data >> 16;
    buffer[4] = data >> 8;
    buffer[5] = data & 0xFF;
    
    GPIOA->BSRR = DW1000_CS_PIN << 16;
    
    for(int i = 0; i < 6; i++) {
        while(!(SPI1->SR & SPI_SR_TXE));
        SPI1->DR = buffer[i];
        while(!(SPI1->SR & SPI_SR_RXNE));
        (void)SPI1->DR;
    }
    
    GPIOA->BSRR = DW1000_CS_PIN;
}

/* DW1000寄存器读操作 */
uint32_t DW1000_ReadReg(uint16_t reg) {
    uint8_t buffer[6];
    buffer[0] = reg >> 8;
    buffer[1] = reg & 0xFF;
    
    GPIOA->BSRR = DW1000_CS_PIN << 16;
    
    for(int i = 0; i < 2; i++) {
        while(!(SPI1->SR & SPI_SR_TXE));
        SPI1->DR = buffer[i];
        while(!(SPI1->SR & SPI_SR_RXNE));
        (void)SPI1->DR;
    }
    
    uint32_t data = 0;
    for(int i = 0; i < 4; i++) {
        while(!(SPI1->SR & SPI_SR_TXE));
        SPI1->DR = 0;
        while(!(SPI1->SR & SPI_SR_RXNE));
        buffer[i] = SPI1->DR;
        data = (data << 8) | buffer[i];
    }
    
    GPIOA->BSRR = DW1000_CS_PIN;
    return data;
}

/* DW1000初始化 */
void DW1000_Init(void) {
    /* 复位DW1000 */
    DW1000_WriteReg(0x00, 0x01);
    for(volatile int i = 0; i < 10000; i++);
    
    /* 配置寄存器 */
    DW1000_WriteReg(0x04, 0x00);      // 使能时钟
    DW1000_WriteReg(0x0D, 0x02);      // 智能电源控制
    DW1000_WriteReg(0x28, 0x1003FF);  // 发射配置
    DW1000_WriteReg(0x2E, 0x1003FF);  // 接收配置
}

/* 获取距离数据 */
void Get_Distances(void) {
    for(uint8_t i = 0; i < ANCHOR_COUNT; i++) {
        /* 切换基站并读取距离 */
        DW1000_WriteReg(0x100, i);  // 选择基站
        anchors[i].distance = (float)DW1000_ReadReg(0x200) * 0.001;  // 转换为米
        anchors[i].status = 1;
    }
}

/* 三边定位算法 */
void Trilateration_3D(void) {
    /* 已知基站坐标（示例值，需实际标定） */
    float anchor_pos[4][3] = {
        {0.0, 0.0, 2.0},
        {10.0, 0.0, 2.0},
        {0.0, 10.0, 2.0},
        {10.0, 10.0, 2.0}
    };
    
    /* 最小二乘法解算 */
    float A[3][3], b[3], result[3];
    
    // 构建矩阵方程
    for(int i = 1; i < 4; i++) {
        A[i-1][0] = 2*(anchor_pos[i][0] - anchor_pos[0][0]);
        A[i-1][1] = 2*(anchor_pos[i][1] - anchor_pos[0][1]);
        A[i-1][2] = 2*(anchor_pos[i][2] - anchor_pos[0][2]);
        
        b[i-1] = pow(anchors[0].distance, 2) - pow(anchors[i].distance, 2) +
                pow(anchor_pos[i][0], 2) - pow(anchor_pos[0][0], 2) +
                pow(anchor_pos[i][1], 2) - pow(anchor_pos[0][1], 2) +
                pow(anchor_pos[i][2], 2) - pow(anchor_pos[0][2], 2);
    }
    
    /* 高斯消元法求解 */
    for(int i = 0; i < 3; i++) {
        for(int j = i+1; j < 3; j++) {
            float factor = A[j][i] / A[i][i];
            for(int k = i; k < 3; k++) {
                A[j][k] -= factor * A[i][k];
            }
            b[j] -= factor * b[i];
        }
    }
    
    result[2] = b[2] / A[2][2];
    result[1] = (b[1] - A[1][2]*result[2]) / A[1][1];
    result[0] = (b[0] - A[0][1]*result[1] - A[0][2]*result[2]) / A[0][0];
    
    current_position.x = result[0];
    current_position.y = result[1];
    current_position.z = result[2];
}

/* 卡尔曼滤波更新 */
void Kalman_Filter_Update(void) {
    static float P[3][3] = {{1,0,0},{0,1,0},{0,0,1}};
    static float x[3] = {0};
    float Q[3][3] = {{0.01,0,0},{0,0.01,0},{0,0,0.01}};
    float R = 0.1;
    
    /* 预测步骤 */
    float x_pred[3] = {x[0], x[1], x[2]};
    float P_pred[3][3];
    
    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 3; j++) {
            P_pred[i][j] = P[i][j] + Q[i][j];
        }
    }
    
    /* 更新步骤 */
    float z[3] = {current_position.x, current_position.y, current_position.z};
    float y[3], S, K[3];
    
    for(int i = 0; i < 3; i++) {
        y[i] = z[i] - x_pred[i];
    }
    
    S = P_pred[0][0] + R;
    K[0] = P_pred[0][0] / S;
    
    x[0] = x_pred[0] + K[0] * y[0];
    P[0][0] = (1 - K[0]) * P_pred[0][0];
    
    current_position.x = x[0];
    current_position.y = x[1];
    current_position.z = x[2];
}

/* 发送数据到上位机 */
void Send_To_PC(void) {
    uint8_t buffer[32];
    int len = sprintf((char*)buffer, "POS:%.2f,%.2f,%.2f\n", 
                     current_position.x, current_position.y, current_position.z);
    
    for(int i = 0; i < len; i++) {
        while(!(USART1->SR & USART_SR_TXE));
        USART1->DR = buffer[i];
    }
}

/* 电池电量检测 */
void Check_Battery(void) {
    ADC1->CR2 |= ADC_CR2_SWSTART;
    while(!(ADC1->SR & ADC_SR_EOC));
    uint16_t adc_value = ADC1->DR;
    battery_level = (adc_value * 100) / 4095;
    
    if(battery_level < 20) {
        uint8_t msg[] = "LOW_BATTERY!\n";
        for(int i = 0; i < sizeof(msg); i++) {
            while(!(USART1->SR & USART_SR_TXE));
            USART1->DR = msg[i];
        }
    }
}

/* 速度检测 */
void Check_Speed(void) {
    static Coordinate3D last_position = {0};
    static uint32_t last_time = 0;
    
    uint32_t current_time = TIM2->CNT;
    float dt = (current_time - last_time) / 10000.0f;  // 转换为秒
    
    float dx = current_position.x - last_position.x;
    float dy = current_position.y - last_position.y;
    float dz = current_position.z - last_position.z;
    
    current_speed = sqrt(dx*dx + dy*dy + dz*dz) / dt;
    
    if(current_speed > 5.0f) {  // 超速阈值5m/s
        uint8_t msg[] = "OVER_SPEED!\n";
        for(int i = 0; i < sizeof(msg); i++) {
            while(!(USART1->SR & USART_SR_TXE));
            USART1->DR = msg[i];
        }
    }
    
    last_position = current_position;
    last_time = current_time;
}

/* 定时器2中断服务函数 */
void TIM2_IRQHandler(void) {
    if(TIM2->SR & TIM_SR_UIF) {
        TIM2->SR &= ~TIM_SR_UIF;
        
        Get_Distances();
        Trilateration_3D();
        Kalman_Filter_Update();
        Send_To_PC();
        Check_Battery();
        Check_Speed();
    }
}

/* 主函数 */
int main(void) {
    System_Init();
    GPIO_Init();
    SPI1_Init();
    USART1_Init(115200);
    TIM2_Init();
    ADC1_Init();
    DW1000_Init();
    
    /* 配置NVIC */
    NVIC_SetPriorityGrouping(4);
    NVIC_SetPriority(TIM2_IRQn, 0);
    
    while(1) {
        __WFI();  // 等待中断
    }
}

项目核心代码

#include "stm32f4xx.h"
#include "dwm1000.h"
#include "usart.h"
#include "timer.h"
#include "filter.h"
#include "battery.h"

// 基站数量
#define BASE_STATION_COUNT 4

// 基站三维坐标（单位：米）
typedef struct {
    float x;
    float y;
    float z;
} Coordinate;

// 基站坐标数组（需要根据实际部署位置修改）
const Coordinate baseStations[BASE_STATION_COUNT] = {
    {0.0, 0.0, 2.5},    // 基站1
    {10.0, 0.0, 2.5},   // 基站2
    {0.0, 8.0, 2.5},    // 基站3
    {10.0, 8.0, 2.5}    // 基站4
};

// 标签数据结构
typedef struct {
    float distance[BASE_STATION_COUNT];  // 到各基站距离
    Coordinate position;                  // 解算出的三维坐标
    float velocity;                       // 当前速度
    uint8_t battery_level;                // 电量百分比
    uint32_t last_update;                 // 最后更新时间
} TagData;

// 全局变量
TagData current_tag;
uint8_t uwb_data_buffer[128];
uint8_t serial_tx_buffer[64];
volatile uint8_t ranging_complete = 0;

// 系统时钟初始化
void SystemClock_Config(void) {
    // 使能HSE
    RCC->CR |= RCC_CR_HSEON;
    while(!(RCC->CR & RCC_CR_HSERDY));
    
    // 配置PLL
    RCC->PLLCFGR = (8 << RCC_PLLCFGR_PLLM_Pos) | 
                   (336 << RCC_PLLCFGR_PLLN_Pos) |
                   (0 << RCC_PLLCFGR_PLLP_Pos) |
                   (7 << RCC_PLLCFGR_PLLQ_Pos);
    RCC->PLLCFGR |= RCC_PLLCFGR_PLLSRC_HSE;
    
    // 使能PLL
    RCC->CR |= RCC_CR_PLLON;
    while(!(RCC->CR & RCC_CR_PLLRDY));
    
    // 配置FLASH延迟
    FLASH->ACR = FLASH_ACR_LATENCY_5WS;
    
    // 切换系统时钟到PLL
    RCC->CFGR |= RCC_CFGR_PPRE1_DIV4 | RCC_CFGR_PPRE2_DIV2;
    RCC->CFGR |= RCC_CFGR_SW_PLL;
    while((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL);
    
    // 配置SysTick定时器（1ms中断）
    SysTick_Config(168000000/1000);
}

// 三边定位算法（最小二乘法）
Coordinate trilateration_3d(float distances[]) {
    Coordinate result = {0, 0, 0};
    float A[BASE_STATION_COUNT-1][3];
    float B[BASE_STATION_COUNT-1];
    
    // 构建矩阵方程 A*X = B
    for(int i = 1; i < BASE_STATION_COUNT; i++) {
        A[i-1][0] = 2*(baseStations[i].x - baseStations[0].x);
        A[i-1][1] = 2*(baseStations[i].y - baseStations[0].y);
        A[i-1][2] = 2*(baseStations[i].z - baseStations[0].z);
        
        B[i-1] = (distances[0]*distances[0] - distances[i]*distances[i]) +
                 (baseStations[i].x*baseStations[i].x + 
                  baseStations[i].y*baseStations[i].y + 
                  baseStations[i].z*baseStations[i].z) -
                 (baseStations[0].x*baseStations[0].x + 
                  baseStations[0].y*baseStations[0].y + 
                  baseStations[0].z*baseStations[0].z);
    }
    
    // 最小二乘法求解：X = (A^T * A)^(-1) * A^T * B
    float AT[3][BASE_STATION_COUNT-1];
    float ATA[3][3];
    float ATB[3];
    
    // 计算A的转置AT
    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < BASE_STATION_COUNT-1; j++) {
            AT[i][j] = A[j][i];
        }
    }
    
    // 计算ATA = AT * A
    for(int i = 0; i < 3; i++) {
        for(int j = 0; j < 3; j++) {
            ATA[i][j] = 0;
            for(int k = 0; k < BASE_STATION_COUNT-1; k++) {
                ATA[i][j] += AT[i][k] * A[k][j];
            }
        }
    }
    
    // 计算ATB = AT * B
    for(int i = 0; i < 3; i++) {
        ATB[i] = 0;
        for(int j = 0; j < BASE_STATION_COUNT-1; j++) {
            ATB[i] += AT[i][j] * B[j];
        }
    }
    
    // 求解线性方程组（简化：使用克莱姆法则，实际应用应使用矩阵求逆）
    float detA = ATA[0][0]*(ATA[1][1]*ATA[2][2] - ATA[1][2]*ATA[2][1]) -
                 ATA[0][1]*(ATA[1][0]*ATA[2][2] - ATA[1][2]*ATA[2][0]) +
                 ATA[0][2]*(ATA[1][0]*ATA[2][1] - ATA[1][1]*ATA[2][0]);
    
    if(fabs(detA) > 1e-6) {  // 避免除以0
        float detX = ATB[0]*(ATA[1][1]*ATA[2][2] - ATA[1][2]*ATA[2][1]) -
                     ATA[0][1]*(ATB[1]*ATA[2][2] - ATA[1][2]*ATB[2]) +
                     ATA[0][2]*(ATB[1]*ATA[2][1] - ATA[1][1]*ATB[2]);
        
        float detY = ATA[0][0]*(ATB[1]*ATA[2][2] - ATA[1][2]*ATB[2]) -
                     ATB[0]*(ATA[1][0]*ATA[2][2] - ATA[1][2]*ATA[2][0]) +
                     ATA[0][2]*(ATA[1][0]*ATB[2] - ATB[1]*ATA[2][0]);
        
        float detZ = ATA[0][0]*(ATA[1][1]*ATB[2] - ATB[1]*ATA[2][1]) -
                     ATA[0][1]*(ATA[1][0]*ATB[2] - ATB[1]*ATA[2][0]) +
                     ATB[0]*(ATA[1][0]*ATA[2][1] - ATA[1][1]*ATA[2][0]);
        
        result.x = detX / detA;
        result.y = detY / detA;
        result.z = detZ / detA;
    }
    
    return result;
}

// 卡尔曼滤波处理
void kalman_filter_update(Coordinate *pos) {
    static Coordinate last_pos = {0, 0, 0};
    static float P[3][3] = {{1,0,0},{0,1,0},{0,0,1}};
    static float Q[3][3] = {{0.01,0,0},{0,0.01,0},{0,0,0.01}};
    static float R[3][3] = {{0.1,0,0},{0,0.1,0},{0,0,0.1}};
    
    // 预测步骤
    Coordinate pred = last_pos;
    float pred_P[3][3];
    for(int i=0; i<3; i++) {
        for(int j=0; j<3; j++) {
            pred_P[i][j] = P[i][j] + Q[i][j];
        }
    }
    
    // 更新步骤
    float K[3][3];
    float S[3][3];
    float inv_S[3][3];
    
    // S = pred_P + R
    for(int i=0; i<3; i++) {
        for(int j=0; j<3; j++) {
            S[i][j] = pred_P[i][j] + R[i][j];
        }
    }
    
    // 计算卡尔曼增益 K = pred_P * inv(S)
    float det_S = S[0][0]*(S[1][1]*S[2][2] - S[1][2]*S[2][1]) -
                  S[0][1]*(S[1][0]*S[2][2] - S[1][2]*S[2][0]) +
                  S[0][2]*(S[1][0]*S[2][1] - S[1][1]*S[2][0]);
    
    if(fabs(det_S) > 1e-6) {
        inv_S[0][0] = (S[1][1]*S[2][2] - S[1][2]*S[2][1]) / det_S;
        inv_S[0][1] = (S[0][2]*S[2][1] - S[0][1]*S[2][2]) / det_S;
        inv_S[0][2] = (S[0][1]*S[1][2] - S[0][2]*S[1][1]) / det_S;
        inv_S[1][0] = (S[1][2]*S[2][0] - S[1][0]*S[2][2]) / det_S;
        inv_S[1][1] = (S[0][0]*S[2][2] - S[0][2]*S[2][0]) / det_S;
        inv_S[1][2] = (S[0][2]*S[1][0] - S[0][0]*S[1][2]) / det_S;
        inv_S[2][0] = (S[1][0]*S[2][1] - S[1][1]*S[2][0]) / det_S;
        inv_S[2][1] = (S[0][1]*S[2][0] - S[0][0]*S[2][1]) / det_S;
        inv_S[2][2] = (S[0][0]*S[1][1] - S[0][1]*S[1][0]) / det_S;
        
        // K = pred_P * inv_S
        for(int i=0; i<3; i++) {
            for(int j=0; j<3; j++) {
                K[i][j] = 0;
                for(int k=0; k<3; k++) {
                    K[i][j] += pred_P[i][k] * inv_S[k][j];
                }
            }
        }
        
        // 更新位置估计
        float innovation[3] = {pos->x - pred.x, pos->y - pred.y, pos->z - pred.z};
        pos->x = pred.x + K[0][0]*innovation[0] + K[0][1]*innovation[1] + K[0][2]*innovation[2];
        pos->y = pred.y + K[1][0]*innovation[0] + K[1][1]*innovation[1] + K[1][2]*innovation[2];
        pos->z = pred.z + K[2][0]*innovation[0] + K[2][1]*innovation[1] + K[2][2]*innovation[2];
        
        // 更新协方差矩阵 P = (I - K) * pred_P
        float I_minus_K[3][3];
        for(int i=0; i<3; i++) {
            for(int j=0; j<3; j++) {
                I_minus_K[i][j] = (i==j?1.0:0.0) - K[i][j];
            }
        }
        
        for(int i=0; i<3; i++) {
            for(int j=0; j<3; j++) {
                P[i][j] = 0;
                for(int k=0; k<3; k++) {
                    P[i][j] += I_minus_K[i][k] * pred_P[k][j];
                }
            }
        }
    }
    
    last_pos = *pos;
}

// 计算移动速度
float calculate_velocity(Coordinate new_pos, Coordinate old_pos, uint32_t time_diff) {
    if(time_diff == 0) return 0.0f;
    
    float dx = new_pos.x - old_pos.x;
    float dy = new_pos.y - old_pos.y;
    float dz = new_pos.z - old_pos.z;
    float distance = sqrt(dx*dx + dy*dy + dz*dz);
    
    // 转换为米/秒
    return distance / (time_diff / 1000.0f);
}

// 速度超限报警检查
uint8_t check_velocity_alarm(float velocity) {
    const float SPEED_LIMIT = 5.0f;  // 5米/秒速度限制
    return (velocity > SPEED_LIMIT);
}

// 串口发送数据到上位机
void send_to_upper_computer(Coordinate pos, float velocity, uint8_t battery) {
    // 数据格式：X,Y,Z,V,B\n
    sprintf((char*)serial_tx_buffer, "%.2f,%.2f,%.2f,%.2f,%d\n",
            pos.x, pos.y, pos.z, velocity, battery);
    
    for(int i = 0; i < strlen((char*)serial_tx_buffer); i++) {
        USART_SendData(USART1, serial_tx_buffer[i]);
        while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET);
    }
}

// UWB测距完成中断回调
void uwb_ranging_callback(uint8_t station_id, float distance) {
    current_tag.distance[station_id] = distance;
    
    // 检查是否所有基站测距完成
    static uint8_t ranging_count = 0;
    ranging_count++;
    
    if(ranging_count >= BASE_STATION_COUNT) {
        ranging_complete = 1;
        ranging_count = 0;
    }
}

// 系统初始化
void system_init(void) {
    // 初始化系统时钟
    SystemClock_Config();
    
    // 初始化UWB模块
    dwm1000_init();
    dwm1000_set_callback(uwb_ranging_callback);
    
    // 初始化串口1（用于上位机通信）
    usart1_init(115200);
    
    // 初始化定时器3（用于定时触发测距）
    timer3_init(100);  // 100ms定时
    
    // 初始化ADC（用于电量检测）
    adc_init();
    
    // 初始化电池检测模块
    battery_init();
}

int main(void) {
    // 系统初始化
    system_init();
    
    // 历史位置记录（用于速度计算）
    Coordinate last_position = {0, 0, 0};
    uint32_t last_time = 0;
    
    while(1) {
        // 等待测距完成标志
        if(ranging_complete) {
            ranging_complete = 0;
            
            // 记录当前时间
            uint32_t current_time = timer_get_millis();
            
            // 通过三边定位计算原始坐标
            Coordinate raw_position = trilateration_3d(current_tag.distance);
            
            // 卡尔曼滤波平滑处理
            kalman_filter_update(&raw_position);
            current_tag.position = raw_position;
            
            // 计算移动速度
            if(last_time > 0) {
                current_tag.velocity = calculate_velocity(
                    current_tag.position, 
                    last_position, 
                    current_time - last_time
                );
            }
            
            // 检测电池电量
            current_tag.battery_level = battery_get_level();
            
            // 检查速度超限
            if(check_velocity_alarm(current_tag.velocity)) {
                // 发送报警信号（可以通过LED或蜂鸣器提示）
                GPIO_SetBits(GPIOC, GPIO_Pin_13);  // LED亮起
                
                // 在数据中添加报警标志
                USART_SendData(USART1, '!');
                while(USART_GetFlagStatus(USART1, USART_FLAG_TXE) == RESET);
            } else {
                GPIO_ResetBits(GPIOC, GPIO_Pin_13);  // LED熄灭
            }
            
            // 发送数据到上位机
            send_to_upper_computer(
                current_tag.position,
                current_tag.velocity,
                current_tag.battery_level
            );
            
            // 保存历史数据
            last_position = current_tag.position;
            last_time = current_time;
        }
        
        // 处理UWB模块通信
        dwm1000_process();
        
        // 低功耗模式（如果没有任务需要处理）
        if(!ranging_complete) {
            __WFI();  // 等待中断
        }
    }
}

// SysTick中断服务函数
void SysTick_Handler(void) {
    static uint32_t tick_count = 0;
    tick_count++;
    
    // 每100个tick（100ms）触发一次测距
    if(tick_count % 100 == 0) {
        // 启动与所有基站的测距
        for(int i = 0; i < BASE_STATION_COUNT; i++) {
            dwm1000_start_ranging(i);
        }
    }
}

总结

该系统成功实现了高精度室内三维定位跟踪功能，通过部署多个UWB基站构成定位网络，结合标签的双向测距技术，确保了定位数据的准确采集。定位主控器采用先进算法解算三维坐标，并通过串口实时传输至上位机，在二维地图上动态显示运动轨迹，同时集成了电量检测和速度报警功能，提升了系统的实用性和安全性。

在硬件设计上，系统采用基于DW1000芯片的UWB模块作为核心测距单元，STM32F407VET6单片机负责数据汇聚与坐标解算，基站间通过通信模块实现同步，确保了系统的稳定运行。电源部分为基站和标签提供了可靠的供电方案，而上位机软件基于Python和PyQt5开发，实现了数据的可视化处理，使操作界面直观易用。

总体而言，该系统结合了高效的硬件架构与灵活的软件平台，实现了室内环境的精准定位与实时跟踪，适用于人员监控、设备管理等多种场景，展现了UWB技术在室内定位领域的广泛应用前景。