空间曲线的曲率计算方法(附代码)

一、空间曲线的曲率计算方法

  方法一，参考张学东的论文《空间曲线的曲率计算方法》：

  方法二，参考博文：空间曲线的弧长与曲率

二、空间曲线的曲率计算matlab代码

%{
Function: calculate_curvature_of_spatial_curve
Description: 计算空间曲线的曲率
Input: 速度向量v,加速度向量a
Output: 空间曲线的曲率k,求解结果状态sta(sta = 0表示求解失败，sta = 1表示求解成功)
Author: Marc Pony(marc_pony@163.com)
%}
function [k, sta] = calculate_curvature_of_spatial_curve(v, a)

normV = norm(v, 2);
normA = norm(a, 2);
dotVA = dot(v, a);
temp = (normA * normV - dotVA) * (normA * normV + dotVA);
if temp >= 0.0 && normV > eps
    k = sqrt(temp) / normV / normV / normV;
    sta = 1;
else
    k = 0.0;
    sta = 0;
end
end

%{
Function: calculate_curvature_of_spatial_curve2
Description: 计算空间曲线的曲率
Input: 速度向量v,加速度向量a
Output: 空间曲线的曲率k,求解结果状态sta(sta = 0表示求解失败，sta = 1表示求解成功)
Author: Marc Pony(marc_pony@163.com)
%}
function [k, sta] = calculate_curvature_of_spatial_curve2(v, a)

normV = norm(v, 2);
if normV > eps
    k = norm(cross(v, a), 2) / normV / normV / normV;
    sta = 1;
else
    k = 0.0;
    sta = 0;
end

end

clc
clear
close all

%三次Bezier曲线C(u) = (1-u)^3 * P0 + 3*u*(1-u)^2*P1 + 3*u^2*(1-u)*P2 + u^3*P3
%                  = (3*P1 - P0 - 3*P2 + P3)*u^3 + (3*P0 - 6*P1 + 3*P2)*u^2 + (3*P1 - 3*P0)*u + P0

syms P0 P1 P2 P3 u
collect(expand((1-u)^3 * P0 + 3*u*(1-u)^2*P1 + 3*u^2*(1-u)*P2 + u^3*P3 ))

P0 = [0, 0, 0];
P1 = [10, 30, 0];
P2 = [20, 5, 0];
P3 = [30, 30, 0];

n = 300;
u = linspace(0, 1, n);
k = zeros(n, 1);
k2 = zeros(n, 1);
sta = zeros(n, 1);
sta2 = zeros(n, 1);
p = zeros(n, 3);
v = zeros(n, 3);
a = zeros(n, 3);
for i = 1 : n
    p(i, :) = (3*P1 - P0 - 3*P2 + P3)*u(i)^3 + (3*P0 - 6*P1 + 3*P2)*u(i)^2 + (3*P1 - 3*P0)*u(i) + P0;
    v(i, :) = 3 * (3*P1 - P0 - 3*P2 + P3)*u(i)^2 + 2*(3*P0 - 6*P1 + 3*P2)*u(i) + (3*P1 - 3*P0);
    a(i, :) = 6 * (3*P1 - P0 - 3*P2 + P3)*u(i) + 2*(3*P0 - 6*P1 + 3*P2);
    [k(i), sta(i)] = calculate_curvature_of_spatial_curve(v(i, :), a(i, :));
    [k2(i), sta2(i)] = calculate_curvature_of_spatial_curve2(v(i, :), a(i, :));
end
error = sum(abs(k-k2))

R = 1./ k;  %曲率半径

figure
subplot(3, 1, 1)
plot3(p(:, 1), p(:, 2), p(:, 3), '-')
hold on
plot3([P0(1), P1(1), P2(1), P3(1)],[P0(2), P1(2), P2(2), P3(2)],[P0(3), P1(3), P2(3), P3(3)], 'o-')
ylabel('Bezier曲线')
view([0 0 1])
subplot(3, 1, 2)
plot(u, k, '-')
ylabel('曲率')
subplot(3, 1, 3)
plot(u, R, '-')
ylabel('曲率半径')