线速度、线加速度与参数化路径标量的速度、加速度之间的转换

最新推荐文章于 2025-10-21 16:27:57 发布

原创最新推荐文章于 2025-10-21 16:27:57 发布 · 2k 阅读

8 ·

CC 4.0 BY-SA版权

文章标签：

#路径标量速度，加速度 #参数速度与加速度

速度规划专栏收录该内容

5 篇文章

订阅专栏

本文详细介绍了如何将线速度和线加速度转化为参数化路径标量的速度和加速度，特别是在三维空间的贝塞尔曲线路径上。通过数学推导得到速度和加速度的表达式，并提供了MATLAB代码示例，演示了如何计算和可视化这些值的变化过程。

文章目录

一、问题描述
二、简单推导
三、matlab代码

一、问题描述

对于笛卡尔空间的参数化路径(如贝塞尔曲线、B样条曲线等)的速度规划问题，由于通常给定的是线速度 $v$ 、线加速度 $a$ 等，因此，若对参数化路径标量 $s\in[s_{min},s_{max}]$ 做速度规划，则首先必须将线速度、线加速度分别转化为参数化路径标量的速度、加速度。

二、简单推导

设三维空间任一参数化路径为:
$\ y(s), \ z(s)]^T, s\in[s_{min},s_{max}]\tag{1}$
式(1)对时间 $t$ 求导得到分速度:
$\begin{cases} \frac{dx}{dt}= \frac{dx}{ds} \frac{ds}{dt}= \frac{dx}{ds}\dot{s}\\ \frac{dy}{dt}=\frac{dy}{ds}\dot{s} \\ \frac{dz}{dt}=\frac{dz}{ds}\dot{s} \\ \tag 2 \end{cases}$
线速度:
$v=\sqrt{(\frac{dx}{dt})^2 +(\frac{dy}{dt})^2 + (\frac{dz}{dt})^2 }=\dot{s}\sqrt{(\frac{dx}{ds})^2 +(\frac{dy}{ds})^2 + (\frac{dz}{ds})^2 }\tag{3}$
参数化路径标量的速度:
$\dot{s}=\frac{v}{||N||}\tag{4}$
其中， $N=[\frac{dx}{ds}\ ,\frac{dy}{ds}\ ,\frac{dz}{ds}]^T$ 。
式(3)对时间求一阶导数，得到：
$a=\ddot{s}\sqrt{(\frac{dx}{ds})^2 +(\frac{dy}{ds})^2 + (\frac{dz}{ds})^2 } + \dot{s}\frac{2\frac{dx}{ds}\frac{d^2x}{ds^2} \dot{s} + 2\frac{dy}{ds}\frac{d^2y}{ds^2} \dot{s} + 2\frac{dz}{ds}\frac{d^2z}{ds^2} \dot{s}}{2\sqrt{(\frac{dx}{ds})^2 +(\frac{dy}{ds})^2 + (\frac{dz}{ds})^2 }} \\ =\ddot{s} \ ||N|| + (\dot{s})^2\frac{N\cdot M }{||N||} \tag{5}$
其中， $M=[\frac{d^2x}{ds^2}\ ,\frac{d^2y}{ds^2}\ ,\frac{d^2z}{ds^2}]^T$ 。
参数化路径标量的加速度:
$\ddot{s}=\frac{a-(\dot{s})^2\frac{N\cdot M }{||N||}}{||N||}\tag{6}$

三、matlab代码

clc
clear
close all

T = 2;
n = 100;
t = linspace(0, T, n)';
s = 3 * (t / T).^2 - 2 * (t / T).^3;  %归一化三次多项式
ds = (6 * (t / T) - 6 * (t / T).^2) / T;
dds = (6 - 12 * (t / T)) / T^2;

P0 = [0, 0, 0];
P1 = [10, 20, 10];
P2 = [20, 10, 50];
P3 = [40, 30, 0];
p = zeros(n, 3);
dp = zeros(n, 3);
ddp = zeros(n, 3);
for i = 1 : n   
    p(i, :) = (3*P1 - P0 - 3*P2 + P3)*s(i)^3 + (3*P0 - 6*P1 + 3*P2)*s(i)^2 + (3*P1 - 3*P0)*s(i) + P0;  %三次贝塞尔曲线
    dp(i, :) = 3 * (3*P1 - P0 - 3*P2 + P3)*s(i)^2 + 2*(3*P0 - 6*P1 + 3*P2)*s(i) + (3*P1 - 3*P0);
    ddp(i, :) = 6 * (3*P1 - P0 - 3*P2 + P3)*s(i) + 2*(3*P0 - 6*P1 + 3*P2);
end

dx_s = dp(:, 1);
dy_s = dp(:, 2);
dz_s = dp(:, 3);

ddx_s = ddp(:, 1);
ddy_s = ddp(:, 2);
ddz_s = ddp(:, 3);

dx_t = dx_s .* ds;
dy_t = dy_s .* ds;
dz_t = dz_s .* ds;

ddx_t = ddx_s .* ds.^2 + dx_s .* dds;
ddy_t = ddy_s .* ds.^2 + dy_s .* dds;
ddz_t = ddz_s .* ds.^2 + dz_s .* dds;

v = sqrt(dx_t.^2 + dy_t.^2 + dz_t.^2);
a = zeros(n, 1);
for i = 1 : n
    a(i) = dot([ddx_t(i), ddy_t(i), ddz_t(i)], [dx_s(i), dy_s(i), dz_s(i)]) / sqrt(dx_s(i)^2 + dy_s(i)^2 + dz_s(i)^2);  %加速度往切矢量方向投影，得到切向加速度
end

ds2 = v ./ sqrt(dx_s.^2 + dy_s.^2 + dz_s.^2);
dds2 = zeros(n, 1);
for i = 1 : n
    N = [dx_s(i), dy_s(i), dz_s(i)];
    M = [ddx_s(i), ddy_s(i), ddz_s(i)];
    dds2(i) = (a(i) - ds2(i)^2 * dot(N, M) / norm(N, 2)) / norm(N, 2);
end

figure
plot(t, ds)
hold on
plot(t, ds2, '--r')
xlabel('t')
ylabel('ds')

figure
plot(t, dds)
hold on
plot(t, dds2, '--r')
xlabel('t')
ylabel('dds')