内容介绍

项目介绍

本项目基于RP2040 Game Kit和TMF8821 dTOF传感器，设计并实现了一个空间测量系统，能够测算出板卡前方平面与传感器的夹角以及最近的垂直距离。系统通过传感器采集环境中的点云数据，将测量的距离转换为空间坐标系下的xyz坐标，并利用最小二乘法对平面进行拟合。最终通过平面法向量计算夹角和垂直距离，实现了对环境平面的几何信息测量与分析。

简短的使用到的硬件介绍

1. RP2040 Game Kit

RP2040 Game Kit是基于树莓派RP2040的嵌入式系统学习平台，USB Type-C供电，采用RP2040作为主控，支持MicroPython、C/C++编程，性能强大。

板上功能：

240*240分辨率的彩色IPS LCD，SPI接口，控制器为ST7789
四向摇杆 + 2个轻触按键 + 一个三轴姿态传感器MMA7660用做输入控制
板上外扩2MB Flash，预刷MicroPython的UF2固件
一个红外接收管 + 一个红外发射管
一个三轴姿态传感器MMA7660
一个蜂鸣器
双排16Pin连接器，有SPI、I2C以及2路模拟信号输入

2. TMF8821 dTOF传感器

dToF模块是基于 TMF8821 设计的直接飞行时间（dToF）传感器模块，TMF8821采用单个模块化封装，带有相关的 VCSEL（垂直腔面发射激光器）。dToF 设备基于 SPAD、TDC 和直方图技术，可实现 5000 mm 的检测范围。由于它的镜头位于 SPAD 上，它支持 3x3、4x4 和 3x6 多区域输出数据以及宽广的、动态可调的视野。VCSEL 上方的封装内的多透镜阵列（MLA）拓宽了 FoI（照明场）。原始数据的所有处理都在片上进行，TMF8821在其 I2C 接口上提供距离信息和置信度值。

方案框图和项目设计思路介绍

系统方案框图

项目设计思路

数据采集：使用TMF8821传感器获取目标平面上的多个距离点，每个点的距离数据通过I2C传输至RP2040。
坐标转换：将测量距离结合传感器的几何位置和视场角，换算为三维空间中的(x, y, z)坐标。
平面拟合：利用最小二乘法对多组(x, y, z)数据进行平面拟合，计算平面方程以及法向量。
几何计算：根据传感器平面与目标平面的法向量计算夹角，并利用平面方程计算传感器到目标平面的垂直距离。
结果显示：通过串口输出夹角和垂直距离，便于观察和验证效果。

软件流程图和关键代码介绍

软件流程图

关键代码介绍

1. 数据采集及坐标转换

if (myTMF882X.startMeasuring(myResults)) {
        // print out results
        Serial.println("Measurement:");
        Serial.print("     Result Number: ");
        Serial.print(myResults.result_num);
        Serial.print("  Number of Results: ");
        Serial.println(myResults.num_results);


        for (int i = 0; i < myResults.num_results; ++i) {
            Serial.print("       conf: ");
            Serial.print(myResults.results[i].confidence);
            Serial.print(" distance mm: ");
            Serial.print(myResults.results[i].distance_mm);
            Serial.print(" channel: ");
            Serial.print(myResults.results[i].channel);
            Serial.print(" sub_capture: ");
            Serial.println(myResults.results[i].sub_capture);
        }
        Serial.print("     photon: ");
        Serial.print(myResults.photon_count);
        Serial.print(" ref photon: ");
        Serial.print(myResults.ref_photon_count);
        Serial.print(" ALS: ");
        Serial.println(myResults.ambient_light);
        Serial.println();


        float angle = calculatePlaneAngle(myResults);


        Serial.print("平面与传感器的夹角（度）: ");
        Serial.println(angle);


        // 可以添加移动平均滤波使结果更稳定
        //   static float filteredAngle = 0;
        //   filteredAngle              = filteredAngle * 0.8 + angle * 0.2;  // 简单的低通滤波
        //   Serial.print("平滑后的夹角（度）: ");
        //   Serial.println(filteredAngle);
    }

2. 角度和垂直距离计算

float calculatePlaneAngle(struct tmf882x_msg_meas_results results) {
    Serial.println("\n--- 平面角度计算过程 ---");
    
    // 1. 更新历史数据并进行均值滤波
    for (int i = 0; i < results.num_results; i++) {
        int channel = results.results[i].channel - 1;  // 将通道号1-9映射到0-8
        if (channel < 0 || channel >= CHANNEL_COUNT) continue;
        distanceHistory[channel][historyIndex] = results.results[i].distance_mm;
    }
    
    historyIndex = (historyIndex + 1) % FILTER_SIZE;
    
    // 计算并打印滤波后的距离
    Serial.println("滤波后的距离值(mm):");
    float filteredDistances[CHANNEL_COUNT] = {0};
    for (int channel = 0; channel < CHANNEL_COUNT; channel++) {
        float sum = 0;
        int validCount = 0;
        for (int j = 0; j < FILTER_SIZE; j++) {
            if (distanceHistory[channel][j] > 0) {
                sum += distanceHistory[channel][j];
                validCount++;
            }
        }
        filteredDistances[channel] = validCount > 0 ? sum / validCount : 0;
        Serial.print("通道 ");
        Serial.print(channel);
        Serial.print(": ");
        Serial.println(filteredDistances[channel]);
    }


    // 2. 转换为3D点坐标
    const int MAX_POINTS = CHANNEL_COUNT;
    Point3D points[MAX_POINTS];
    int validPoints = 0;


    // FOV角度（弧度）
    const float HFOV = 33.0f * PI / 180.0f;  // 水平FOV 33度
    const float VFOV = 32.0f * PI / 180.0f;  // 垂直FOV 32度
    
    // 计算每个像素的角度增量
    const float h_step = HFOV / 2.0f;  // 水平方向总范围的一半
    const float v_step = VFOV / 2.0f;  // 垂直方向总范围的一半


    Serial.println("\n3D点坐标(米):");
    for (int i = 0; i < CHANNEL_COUNT; i++) {
        if (filteredDistances[i] <= 0) continue;


        float distance = filteredDistances[i] / 1000.0f;  // 转换为米
        
        // 计算该通道的水平和垂直角度
        // 将3x3网格映射到FOV范围内
        // 将0,1,2位置映射到-1,0,1，然后乘以步长得到角度
        float h_angle = (CHANNEL_GRID[i][0] - 1) * (h_step);  // -h_step, 0, h_step
        float v_angle = (1 - CHANNEL_GRID[i][1]) * (v_step);  // v_step, 0, -v_step (注意y轴方向)


        // 使用球坐标系转换为笛卡尔坐标系
        points[validPoints].z = distance * cos(v_angle) * cos(h_angle);  // 深度
        points[validPoints].x = distance * cos(v_angle) * sin(h_angle);  // 水平偏移
        points[validPoints].y = distance * sin(v_angle);                 // 垂直偏移
        
        Serial.print("点");
        Serial.print(validPoints);
        Serial.print(": (");
        Serial.print(points[validPoints].x, 3);
        Serial.print(", ");
        Serial.print(points[validPoints].y, 3);
        Serial.print(", ");
        Serial.print(points[validPoints].z, 3);
        Serial.print(") 角度(度) h:");
        Serial.print(h_angle * 180.0f / PI);
        Serial.print(" v:");
        Serial.println(v_angle * 180.0f / PI);
        
        validPoints++;
    }


    if (validPoints < 3) {
        Serial.println("有效点数不足3个，无法拟合平面");
        return 0.0f;
    }


    // 3. 使用更稳定的平面拟合方法
    // 首先计算质心
    Point3D centroid = {0, 0, 0};
    for (int i = 0; i < validPoints; i++) {
        centroid.x += points[i].x;
        centroid.y += points[i].y;
        centroid.z += points[i].z;
    }
    centroid.x /= validPoints;
    centroid.y /= validPoints;
    centroid.z /= validPoints;


    // 构建协方差矩阵
    float xx = 0, xy = 0, xz = 0, yy = 0, yz = 0, zz = 0;
    for (int i = 0; i < validPoints; i++) {
        float dx = points[i].x - centroid.x;
        float dy = points[i].y - centroid.y;
        float dz = points[i].z - centroid.z;
        
        xx += dx * dx;
        xy += dx * dy;
        xz += dx * dz;
        yy += dy * dy;
        yz += dy * dz;
        zz += dz * dz;
    }


    // 找到最小特征值对应的特征向量（法向量）
    float det_x = yy*zz - yz*yz;
    float det_y = xx*zz - xz*xz;
    float det_z = xx*yy - xy*xy;
    
    // 选择最大行列式对应的分量作为法向量
    float nx, ny, nz;
    if (det_x >= det_y && det_x >= det_z) {
        nx = det_x;
        ny = xz*yz - xy*zz;
        nz = xy*yz - xz*yy;
    } else if (det_y >= det_x && det_y >= det_z) {
        nx = xz*yz - xy*zz;
        ny = det_y;
        nz = xy*xz - yz*xx;
    } else {
        nx = xy*yz - xz*yy;
        ny = xy*xz - yz*xx;
        nz = det_z;
    }


    // 归一化法向量
    float norm = sqrt(nx*nx + ny*ny + nz*nz);
    nx /= norm;
    ny /= norm;
    nz /= norm;


    // 确保法向量指向z轴正方向
    if (nz < 0) {
        nx = -nx;
        ny = -ny;
        nz = -nz;
    }


    // 计算与z轴的夹角
    float angle = acos(nz) * 180.0f / PI;


    // 计算平面方程 ax + by + cz + d = 0 的系数
    float a = nx;
    float b = ny;
    float c = nz;
    float d = -(a*centroid.x + b*centroid.y + c*centroid.z);


    // 打印调试信息
    Serial.println("\n平面拟合结果:");
    Serial.print("法向量: (");
    Serial.print(nx, 4); Serial.print(", ");
    Serial.print(ny, 4); Serial.print(", ");
    Serial.print(nz, 4); Serial.println(")");
    
    Serial.print("平面方程: ");
    Serial.print(a, 4); Serial.print("x + ");
    Serial.print(b, 4); Serial.print("y + ");
    Serial.print(c, 4); Serial.print("z + ");
    Serial.print(d, 4); Serial.println(" = 0");


    float distance_to_origin = abs(d) / sqrt(a*a + b*b + c*c);
    Serial.print("原点到平面的距离(米): ");
    Serial.println(distance_to_origin, 4);


    Serial.print("与z轴夹角(度): ");
    Serial.println(angle);


    return angle;
}

功能展示图及说明

项目中遇到的难题和解决方法

传感器数据波动
- 问题：dTOF传感器数据波动较大。
- 解决方法：对数据进行多次采样并取均值。