如何计算不是适当函数的轨迹的逐点曲率半径

Question

我正在尝试使用 Matlab 计算使用投影到本地笛卡尔平面的 GPS 数据获得的轨迹的“曲率半径”信号。 第 n 点上的信号值是与该点上的轨迹相切的密切圆之一。 按照惯例，与左转相关的信号幅度必须为负，反之亦然。

使用具有适当功能的轨迹作为图形，我正在构建“符号”信号，评估密切圆中心的 y 坐标之间的数值差异：

for i=1:length(yCenter) -1
    aux=Y_m(closestIndex_head:closestIndex_tail );

    if yCenter(i) - aux(i) > 0
        sign(i)=-1;
    else
        sign(i)=+1;
        
    end
end

• yCenter 包含与轨迹的每个点相关的所有密切圆的 x 坐标；
• Y_m 包含轨迹中每个点的 y 坐标。

只要轨迹图是一个适当的函数，上述简单方法就可以工作（对于每个 x，只有一个 y）。

我正在研究的轨迹是这样的：

符号信号出现异常：

标志似乎在一圈内改变。

我尝试使用切线向量和轨迹之间的角度的正弦、角度的切线的符号和其他类似的东西来纠正符号，但我仍然在查看一些异常情况：

我很确定这些异常来自于图形不是一个正确的函数并且解决方案取决于切向量的角度，但仍然缺少一些东西。

任何建议将不胜感激， 谢谢。

亚历山德罗

Answer 1

要跟踪 2D 曲线，您应该使用适用于一般参数化 2D 函数的 curvature 表达式。

在实现equation from Wikipedia 时，您可以使用离散差来近似导数。给定x 和y 坐标，可以按如下方式实现：

% approximate 1st derivatives of x & y with discrete differences
dx  = 0.5*(x(3:end)-x(1:end-2))
dy  = 0.5*(y(3:end)-y(1:end-2))
dl  = sqrt(dx.^2 + dy.^2)
xp  = dx./dl
yp  = dy./dl
% approximate 2nd derivatives of x & y with discrete differences
xpp = (x(3:end)-2*x(2:end-1)+x(1:end-2))./(dl.^2)
ypp = (y(3:end)-2*y(2:end-1)+y(1:end-2))./(dl.^2)

% Compute the curvature
curvature = (xp.*ypp - yp.*xpp) ./ ((xp.^2 + yp.^2).^(1.5))

出于演示目的，我还构建了一个合成测试信号（可用于重新创建相同的条件），但您显然可以改用自己的数据：

z1 = linspace(2,1,N).*exp(1i*linspace(0.75*pi,-0.25*pi,N))
z2 = 2*exp(-1i*0.25*pi) + linspace(1,2,N)*exp(1i*linspace(0.75*pi,2.25*pi,N))
z = cat(1,z1,z2)
x = real(z)
y = imag(z)

对应的曲率结果：