matlab练习程序（扩展曲线边界）

这里有这样一个问题，比如我们有一条曲线，要求出曲线的左右一定范围的边界。

方法是：

1. 取曲线前后帧局部点对，计算点对得到的直线方程L1。

2. 点法式计算垂直于L1的直线L2。

3. 根据拓展半径d以点对中后一个点画圆，计算圆和L2的交点。

4. 连接所有交点得到曲线的边界。 

matlab代码如下：

clear all;
close all;
clc;

d=10;

t = 1:0.1:20;
x = 80*t - 5*t.*sin(t) ;
y = t.^2 + cos(t);

plot(x,y)
hold on;

line1=[];
line2=[];
for i=2:length(t)
    
    k = (y(i)-y(i-1))/(x(i)-x(i-1));
    
    k1 = -1/k;
    b1 = y(i)-k1*x(i);
    
    tmp = sqrt(-b1^2 + d^2 + d^2*k1^2 - 2*b1*k1*x(i) - k1^2*x(i)^2 + 2*b1*y(i) + 2*k1*x(i)*y(i) - y(i)^2);
    
    x1 = (-b1*k1 + x(i) + k1*y(i) - tmp)/(1 + k1^2);
    y1 = b1 - (b1*k1^2)/(1 + k1^2) + (k1*x(i))/(1 + k1^2) + (k1^2*y(i))/(1 + k1^2) - (k1*tmp)/(1 + k1^2);
    
    x2 = (-b1*k1 + x(i) + k1*y(i) + tmp)/(1 + k1^2);
    y2 = b1 - (b1*k1^2)/(1 + k1^2) + (k1*x(i))/(1 + k1^2) + (k1^2*y(i))/(1 + k1^2) + (k1*tmp)/(1 + k1^2);
    
    line1=[line1;x1 y1];
    line2=[line2;x2 y2];
end
plot(line1(:,1),line1(:,2),'r');
plot(line2(:,1),line2(:,2),'r');

axis equal

结果如下：

半径10米的结果：

半径50米的结果：

可以看出半径大的时候在曲线曲率大的地方会形成毛刺，要解决毛刺问题其实可以先求得中心曲线的方程，再用上述方法得到边界离散点，最后用最小二乘以曲线的模型对离散点进行拟合。

应该就可以解决毛刺了，我这里就不实验了，应该是可以的。

~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

上面的方法又是求圆的又是解方程的，似乎很麻烦，下面再提供一种方法。

首先根据路径前后点计算航向角。

然后利用航向角将当前点坐标转换为航向角为0的坐标系，这样就能直接对y方向做加减操作了。

将转换后的点y方向加减两个数，就能得到扩展后的边界。

将扩展后的边界再通过航向角转回原坐标系即可。

matlab代码如下：

clear all;close all;clc;

x = 0:0.1:10;
y = cos(x./5);

yaw = atan2(diff(y),diff(x));
yaw = [yaw yaw(end)];
plot(x,y,'r.')
hold on;

for i=1:length(x)
    R = [cos(yaw(i)) -sin(yaw(i));sin(yaw(i)) cos(yaw(i))];  
    newx = x(i);
    newy = y(i);
    newp = [newx newy]*R;
    p1 = [newp(1,1) newp(1,2)+0.5]*inv(R);
    p2 = [newp(1,1) newp(1,2)-0.5]*inv(R);
    
    plot(p1(1),p1(2),'m.')
    plot(p2(1),p2(2),'m.')  
end
axis equal;

结果如下：