本帖最后由 是木子明呀 于 2026-4-28 17:55 编辑
#申请原创#
最近持续基于Geehy超声波雷达GURC21做超声波雷达定位基础算法及其应用方案，记录一下这个过程中自己的一些思考和总结，欢迎同行者一起交流学习。
接上一篇文章超声波雷达定位基础算法及其应用方案(二)——三边定位法理论篇 - 极海MCU极海MCU官方技术支持论坛
本帖主要介绍三边定位法基于C语言版本的算法实现。
为了更好地理解算法实现的原理，方便后续快速读懂C语言代码。
首先，我们来详细、逐步地推导两圆根轴方程中的系数。这是理解双探头定位算法从几何定义到可解方程的关键一步。
1.两个探头参数：
探头 A: 坐标为 (x₁, y₁)，探头 B: 坐标为 (x₂, y₂)，
测得障碍物 P (x, y) 到它们的距离（即半径）为：到探头 A 的距离：r₁，到探头 B 的距离：r₂
根据圆的定义，障碍物 P 必须同时位于两个圆上，因此满足两个方程：
圆 A 的方程：
(1) (x - x₁)² + (y - y₁)² = r₁²
圆 B 的方程：
(2) (x - x₂)² + (y - y₂)² = r₂²
我们的目标是找到一条直线，P 点必然位于这条直线上。这条直线就是两圆的根轴。
2.根轴方程系数的求解推导：
推导的核心是 “相减消元法”。我们通过将两个圆的方程相减来消去二次项 x² 和 y²，从而得到一个线性方程。
展开两个圆的方程：
将方程(1)和(2)展开：
(1) → x² - 2x₁*x + x₁² + y² - 2y₁*y + y₁² = r₁²
(2) → x² - 2x₂*x + x₂² + y² - 2y₂*y + y₂² = r₂²
方程(1)减去方程(2)：[x² - 2x₁*x + x₁² + y² - 2y₁*y + y₁²] - [x² - 2x₂*x + x₂² + y² - 2y₂*y + y₂²] = r₁² - r₂²
合并同类项，整理后得到：2(x₂ - x₁)x + 2(y₂ - y₁)y + (x₁² + y₁² - x₂² - y₂²) = r₁² - r₂²
整理成标准线性方程形式 Ax + By + C = 0，将上式的常数项移到右边：
2(x₂ - x₁)x + 2(y₂ - y₁)y = (r₁² - r₂²) - (x₁² + y₁² - x₂² - y₂²)
为了更清晰，我们调整右边常数项的符号。将 -(x₁² + y₁² - x₂² - y₂²) 改写为 +(x₂² + y₂² - x₁² - y₁²)：
两圆的根轴方程为：2(x₂ - x₁)x + 2(y₂ - y₁)y = (r₁² - r₂²) + (x₂² + y₂² - x₁² - y₁²)
现在方程已经是 Ax + By = D 的形式，其中：
一次项系数 A = 2(x₂ - x₁)
一次项系数 B = 2(y₂ - y₁)
常数项 D = (r₁² - r₂²) + (x₂² + y₂² - x₁² - y₁²)
为了方便后续求解 y = mx + c 或 x = my + c 的形式，我们也可以将常数项D除以2，得到更简洁的形式。令：K = [ (r₁² - r₂²) + (x₂² + y₂² - x₁² - y₁²) ] / 2
则方程简化为：(x₂ - x₁)x + (y₂ - y₁)y = K
这个形式就是C语言代码中推导所用的基础。
接下来，我们利用上述根轴方程及系数，从C语言代码的角度进行求解过程推导：
①从简化形式出发：(x₂ - x₁)x + (y₂ - y₁)y = K
②令 dx = x₂ - x₁, dy = y₂ - y₁，方程写作：dx * x + dy * y = K
③解出 y：
dy * y = K - dx * x
y = (-dx/dy) * x + (K/dy)
④因此，在代码中：
斜率 m_x = -dx / dy
截距 c_x = K / dy
其中 K 就是上面推导的 [(r₁² - r₂²) + (x₂² + y₂² - x₁² - y₁²)] / 2。
这个推导清晰地展示了：从纯粹的几何距离约束，如何通过代数运算转化为一个线性约束。正是这个线性约束，将二维交点问题降低为一维搜索问题（在一条直线上寻找满足圆方程的点），从而使求解成为可能。这也是双探头定位算法能够实现的核心数学基础。
接下来，结合实际汽车上超声波雷达具体实现，附上一个C语言代码实现示例，供大家参考：
/**
* @file ultrasonic_general_triangulation.c
* @brief 通用双探头超声波定位算法（探头可在任意2D位置）
*/
#include <stdio.h>
#include <math.h>
#include <float.h>
/**
* @brief 探头与障碍物结构体定义
*/
typedef struct { double x, y; } Probe;
typedef struct { double x, y; } Obstacle;
/**
* @brief 算法返回状态码
*/
typedef enum {
SUCCESS = 0,
ERROR_INVALID_RADIUS,
ERROR_NO_INTERSECTION,
ERROR_COINCIDENT_PROBES,
ERROR_PARALLEL_ROOT_AXIS // 新增：当两圆圆心连线与根轴垂直时可能出现的退化情况
} CalcStatus;
/**
* @brief 计算两点间距离的平方
*/
static inline double distance_squared(double x1, double y1, double x2, double y2) {
double dx = x1 - x2, dy = y1 - y2;
return dx * dx + dy * dy;
}
/**
* @brief 通用的双探头定位函数
*
* @param p1 探头1坐标
* @param p2 探头2坐标
* @param r1 到探头1的距离
* @param r2 到探头2的距离
* @param result 计算结果存储指针（输出有效解，通常y值较大的那个）
* @return CalcStatus
*/
CalcStatus locateObstacleGeneral(const Probe* p1, const Probe* p2,
double r1, double r2,
Obstacle* result) {
// 1. 输入有效性检查
if (r1 <= 0.0 || r2 <= 0.0) return ERROR_INVALID_RADIUS;
double dx = p2->x - p1->x, dy = p2->y - p1->y;
double dist2 = dx * dx + dy * dy; // 探头间距离平方
// 检查探头是否重合（或过近）
if (dist2 < DBL_EPSILON) return ERROR_COINCIDENT_PROBES;
// 2. 计算两圆的根轴（radical axis）线性方程：y = m*x + c
// 通过两圆方程相减得到：(x2-x1)x + (y2-y1)y = (r1^2 - r2^2 + d^2)/2
// 其中 d^2 = (x2-x1)^2 + (y2-y1)^2
double right = (r1 * r1 - r2 * r2 + distance_squared(p2->x, p2->y, p1->x, p1->y)) / 2.0;
// 3. 求解交点
double x_sol[2], y_sol[2];
int num_solutions = 0;
// 判断处理哪种情况更稳定：|dx| > |dy| 时，用y表示x；否则用x表示y
if (fabs(dx) < fabs(dy)) {
// 情况A: 用 x 表示 y， y = m_x * x + c_x
double m_x = -dx / dy;
double c_x = (right - p1->x * dx - p1->y * dy) / dy;
// 将 y = m_x * x + c_x 代入圆1方程，得到关于x的二次方程: A*x^2 + B*x + C = 0
double A = 1 + m_x * m_x;
double B = 2 * (m_x * (c_x - p1->y) - p1->x);
double C = p1->x * p1->x + (c_x - p1->y) * (c_x - p1->y) - r1 * r1;
double discriminant = B * B - 4 * A * C;
if (discriminant < -DBL_EPSILON) return ERROR_NO_INTERSECTION;
if (discriminant < 0) discriminant = 0; // 处理微小负值
double sqrt_disc = sqrt(discriminant);
x_sol[0] = (-B + sqrt_disc) / (2 * A);
x_sol[1] = (-B - sqrt_disc) / (2 * A);
y_sol[0] = m_x * x_sol[0] + c_x;
y_sol[1] = m_x * x_sol[1] + c_x;
num_solutions = (fabs(discriminant) > DBL_EPSILON) ? 2 : 1;
} else {
// 情况B: 用 y 表示 x， x = m_y * y + c_y
double m_y = -dy / dx;
double c_y = (right - p1->x * dx - p1->y * dy) / dx;
// 将 x = m_y * y + c_y 代入圆1方程，得到关于y的二次方程
double A = 1 + m_y * m_y;
double B = 2 * (m_y * (c_y - p1->x) - p1->y);
double C = p1->y * p1->y + (c_y - p1->x) * (c_y - p1->x) - r1 * r1;
double discriminant = B * B - 4 * A * C;
if (discriminant < -DBL_EPSILON) return ERROR_NO_INTERSECTION;
if (discriminant < 0) discriminant = 0;
double sqrt_disc = sqrt(discriminant);
y_sol[0] = (-B + sqrt_disc) / (2 * A);
y_sol[1] = (-B - sqrt_disc) / (2 * A);
x_sol[0] = m_y * y_sol[0] + c_y;
x_sol[1] = m_y * y_sol[1] + c_y;
num_solutions = (fabs(discriminant) > DBL_EPSILON) ? 2 : 1;
}
// 4. 从可能的解中选择最合理的（以选择y值较大的点（即“前方”的障碍物）为例）
int selected_index = 0;
if (num_solutions == 2) {
selected_index = (y_sol[0] > y_sol[1]) ? 0 : 1;
}
result->x = x_sol[selected_index];
result->y = y_sol[selected_index];
// 5. 验证解的有效性
double err1 = fabs(distance_squared(result->x, result->y, p1->x, p1->y) - r1 * r1);
double err2 = fabs(distance_squared(result->x, result->y, p2->x, p2->y) - r2 * r2);
if (err1 > 1e-6 || err2 > 1e-6) {
// 计算误差过大，可能数值不稳定
return ERROR_NO_INTERSECTION;
}
return SUCCESS;
}
以上C语言代码中，关于x和y的坐标轴表示。其核心准则是用差值较大的那个坐标轴来表示差值较小的坐标轴。这样，在求解表达式的斜率时，我们总是在除以一个较大的数，从而获得一个绝对值小于或等于1的斜率，最大程度减少舍入误差的放大效应。
最后，根据上述C语言代码示例，给出5个算例验证方案如下图所示：
C语言代码实现示例如下：
/**
* @brief 主函数：算例演示
*/
int main() {
printf("=== 通用双探头超声波定位算法（任意安装位置）===\n\n");
Obstacle obstacle;
CalcStatus status;
// 算例1: 经典水平情况（验证与之前算法一致性）
Probe p1_h = {0.0, 0.0};
Probe p2_h = {20.0, 0.0}; // 间距20cm
status = locateObstacleGeneral(&p1_h, &p2_h, 15.0, 15.5, &obstacle); // R_a=15.0, R_b=15.5
// 理论解应接近: x ≈ -1.8125, y ≈ 14.834 （单位：cm）
// 算例2: 探头垂直安装（一上一下）
Probe p1_v = {10.0, 0.0};
Probe p2_v = {10.0, 15.0}; // 垂直间距15cm
status = locateObstacleGeneral(&p1_v, &p2_v, 25.0, 20.0, &obstacle);
// 算例3: 探头倾斜安装（更一般的工程情况）
Probe p1_t = {5.0, 2.0}; // 左下
Probe p2_t = {25.0, 8.0}; // 右上，不在同一水平线
status = locateObstacleGeneral(&p1_t, &p2_t, 18.0, 12.0, &obstacle);
// 算例4: 无解情况（距离过短）
status = locateObstacleGeneral(&p1_t, &p2_t, 5.0, 5.0, &obstacle);
// 算例5: 对称情况，障碍物在两探头中垂面附近
// 调整距离使障碍物到两探头距离近似相等
status = locateObstacleGeneral(&p1_t, &p2_t, 16.0, 16.2, &obstacle);
printResult(status, &obstacle);
return 0;
}
超声波雷达定位基础算法及其应用方案三边定位法C语言算法实现篇内容如上。
以上所有，仅供参考，欢迎大家多多指教。
|
|
