带测距误差的高精度点定位最优方法咨询

一、定位算法优化 #

1. 加权最小二乘法（WLS） #

普通3-distance方程求解默认所有测距等权，但你的测距误差在1-5mm区间波动，用WLS给误差小的测距值更高权重，能有效降低角度偏差。核心是根据多次测量的标准差构造权重，最小化加权后的残差平方和。

MATLAB实现示例：

% 已知三点坐标
P = [x1 y1; x2 y2; x3 y3];
% 多次测量的距离均值
d_meas = [mean(d1_samples); mean(d2_samples); mean(d3_samples)];
% 各测距序列的标准差（作为误差估计）
sigma = [std(d1_samples); std(d2_samples); std(d3_samples)];

% 定义加权残差函数
function res = weighted_residuals(xy, P, d, sigma)
    x = xy(1); y = xy(2);
    res(1) = (sqrt((x-P(1,1))^2 + (y-P(1,2))^2) - d(1)) / sigma(1);
    res(2) = (sqrt((x-P(2,1))^2 + (y-P(2,2))^2) - d(2)) / sigma(2);
    res(3) = (sqrt((x-P(3,1))^2 + (y-P(3,2))^2) - d(3)) / sigma(3);
end

% 初始估计（可用普通最小二乘结果）
xy0 = [0; 0];
options = optimoptions('lsqnonlin', 'Display', 'off');
xy_est = lsqnonlin(@(xy) weighted_residuals(xy, P, d_meas, sigma), xy0);

2. 非线性鲁棒估计 #

若存在个别测距误差达到5mm的异常值，L2范数会被异常值主导，改用L1范数或Huber损失函数的鲁棒估计，能抑制异常值对角度的影响。MATLAB的lsqrobust直接支持配置：

% 定义基础残差函数
function res = basic_residuals(xy, P, d)
    x = xy(1); y = xy(2);
    res(1) = sqrt((x-P(1,1))^2 + (y-P(1,2))^2) - d(1);
    res(2) = sqrt((x-P(2,1))^2 + (y-P(2,2))^2) - d(2);
    res(3) = sqrt((x-P(3,1))^2 + (y-P(3,2))^2) - d(3);
end

% 使用Huber损失进行鲁棒估计
robust_opts = robustfitoptions('LossFunction', 'Huber');
xy_robust = lsqrobust(@(xy) basic_residuals(xy, P, d_meas), xy0, robust_opts);

3. 带几何约束的优化 #

三个已知点呈三角形，未知点必然在三角形外接圆的合理邻域内，加入坐标约束可避免求解结果偏离真实区域，直接降低角度偏差：

% 设置坐标约束：留100mm余量
lb = [min(P(:,1))-0.1; min(P(:,2))-0.1];
ub = [max(P(:,1))+0.1; max(P(:,2))+0.1];
xy_constrained = lsqnonlin(@(xy) weighted_residuals(xy, P, d_meas, sigma), xy0, lb, ub);

二、测距误差处理优化 #

1. 加权平均代替简单平均 #

不要直接取算术平均，而是根据单次测量的波动分配权重，波动越小的测量值权重越高：

% 以d1_samples为例计算加权平均
d1_mean = mean(d1_samples);
weights = 1 ./ (abs(d1_samples - d1_mean) + eps); % eps避免除以0
d1_weighted_mean = sum(d1_samples .* weights) / sum(weights);

2. 基于均匀分布的贝叶斯估计 #

你的测距误差是1-5mm的均匀分布，可将此作为先验分布，用马尔可夫链蒙特卡洛（MCMC）采样计算未知点的后验概率分布，取概率最大的位置作为估计值：

% 定义后验概率函数
function log_posterior = posterior(xy, P, d_samples)
    x = xy(1); y = xy(2);
    log_prob = 0;
    for i = 1:length(d_samples)
        d1 = d_samples(i,1); d2 = d_samples(i,2); d3 = d_samples(i,3);
        % 计算理论距离与测量值的偏差，需在1-5mm范围内
        err1 = abs(sqrt((x-P(1,1))^2 + (y-P(1,2))^2) - d1);
        err2 = abs(sqrt((x-P(2,1))^2 + (y-P(2,2))^2) - d2);
        err3 = abs(sqrt((x-P(3,1))^2 + (y-P(3,2))^2) - d3);
        if err1 < 0.001 || err1 > 0.005 || err2 < 0.001 || err2 > 0.005 || err3 < 0.001 || err3 > 0.005
            log_prob = -inf;
            break;
        end
        log_prob = log_prob + log(1/(0.004)); % 均匀分布的概率密度
    end
    log_posterior = log_prob;
end

% MCMC采样
start_point = [0;0];
nsamples = 10000;
samples = mhsample(start_point, nsamples, 'logpdf', @(xy) posterior(xy, P, all_d_samples), 'proposal', 'normal', 'proposalargs', [0.01 0.01]);
% 取样本均值作为最终估计
xy_bayes = mean(samples);

3. 静态卡尔曼滤波平滑 #

针对静态未知点的多组连续测量，用静态卡尔曼滤波融合数据，平滑误差积累：

% 初始化卡尔曼滤波
kf = kalmanFilter([1 0; 0 1], eye(2), eye(3), eye(2));
% 循环更新滤波
for i = 1:length(measurement_sets)
    d_set = measurement_sets(i,:);
    xy_current = kf.State;
    % 构造观测矩阵H
    H = [(xy_current(1)-P(1,1))/sqrt((xy_current(1)-P(1,1))^2 + (xy_current(2)-P(1,2))^2), ...
         (xy_current(2)-P(1,2))/sqrt((xy_current(1)-P(1,1))^2 + (xy_current(2)-P(1,2))^2);
         (xy_current(1)-P(2,1))/sqrt((xy_current(1)-P(2,1))^2 + (xy_current(2)-P(2,2))^2), ...
         (xy_current(2)-P(2,2))/sqrt((xy_current(1)-P(2,1))^2 + (xy_current(2)-P(2,2))^2);
         (xy_current(1)-P(3,1))/sqrt((xy_current(1)-P(3,1))^2 + (xy_current(2)-P(3,2))^2), ...
         (xy_current(2)-P(3,2))/sqrt((xy_current(1)-P(3,1))^2 + (xy_current(2)-P(3,2))^2)];
    kf.MeasurementMatrix = H;
    kf = correct(kf, d_set);
end
xy_final = kf.State;

关键说明 #

角度偏差大的核心原因是：普通最小二乘法对测距误差的方向敏感，当未知点靠近某一已知点时，小的测距误差会被几何关系放大为大的角度偏差。加权、鲁棒估计和约束优化能针对性削弱这种放大效应。

内容的提问来源于stack exchange，提问作者Masood Salik