一、问题描述

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

二、简单推导

设三维空间任一参数化路径为:
p ( s ) = [ x ( s ) , y ( s ) , z ( s ) ] T , s ∈ [ s m i n , s m a x ] (1) p(s)=[x(s), \ y(s), \ z(s)]^T, s\in[s_{min},s_{max}]\tag{1} p(s)=[x(s), y(s), z(s)]T,s∈[smin,smax](1)
式(1)对时间 t t t求导得到分速度:
{ d x d t = d x d s d s d t = d x d s s ˙ d y d t = d y d s s ˙ d z d t = d z d s s ˙ (2) \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} ⎩⎪⎨⎪⎧dtdx=dsdxdtds=dsdxs˙dtdy=dsdys˙dtdz=dsdzs˙(2)
线速度:
v = ( d x d t ) 2 + ( d y d t ) 2 + ( d z d t ) 2 = s ˙ ( d x d s ) 2 + ( d y d s ) 2 + ( d z d s ) 2 (3) 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} v=(dtdx)2+(dtdy)2+(dtdz)2 =s˙(dsdx)2+(dsdy)2+(dsdz)2 (3)
参数化路径标量的速度:
s ˙ = v ∣ ∣ N ∣ ∣ (4) \dot{s}=\frac{v}{||N||}\tag{4} s˙=∣∣N∣∣v(4)
其中， N = [ d x d s , d y d s , d z d s ] T N=[\frac{dx}{ds}\ ,\frac{dy}{ds}\ ,\frac{dz}{ds}]^T N=[dsdx ,dsdy ,dsdz]T。
式(3)对时间求一阶导数，得到：
a = s ¨ ( d x d s ) 2 + ( d y d s ) 2 + ( d z d s ) 2 + s ˙ 2 d x d s d 2 x d s 2 s ˙ + 2 d y d s d 2 y d s 2 s ˙ + 2 d z d s d 2 z d s 2 s ˙ 2 ( d x d s ) 2 + ( d y d s ) 2 + ( d z d s ) 2 = s ¨ ∣ ∣ N ∣ ∣ + ( s ˙ ) 2 N ⋅ M ∣ ∣ N ∣ ∣ (5) 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} a=s¨(dsdx)2+(dsdy)2+(dsdz)2 +s˙2(dsdx)2+(dsdy)2+(dsdz)2 2dsdxds2d2xs˙+2dsdyds2d2ys˙+2dsdzds2d2zs˙=s¨ ∣∣N∣∣+(s˙)2∣∣N∣∣N⋅M(5)
其中， M = [ d 2 x d s 2 , d 2 y d s 2 , d 2 z d s 2 ] T M=[\frac{d^2x}{ds^2}\ ,\frac{d^2y}{ds^2}\ ,\frac{d^2z}{ds^2}]^T M=[ds2d2x ,ds2d2y ,ds2d2z]T。
参数化路径标量的加速度:
s ¨ = a − ( s ˙ ) 2 N ⋅ M ∣ ∣ N ∣ ∣ ∣ ∣ N ∣ ∣ (6) \ddot{s}=\frac{a-(\dot{s})^2\frac{N\cdot M }{||N||}}{||N||}\tag{6} s¨=∣∣N∣∣a−(s˙)2∣∣N∣∣N⋅M(6)

三、matlab代码

clc
clear
close allT = 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);
enddx_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 : na(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);  %加速度往切矢量方向投影，得到切向加速度
endds2 = v ./ sqrt(dx_s.^2 + dy_s.^2 + dz_s.^2);
dds2 = zeros(n, 1);
for i = 1 : nN = [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);
endfigure
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')

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

3dmax：3dmax的软件右边栏常用修改器(Cloth修改器、车削、倒角/剖面、对称、FFD长方体/圆柱体、Gizmo、规格化样条线、挤出、路径、扭曲、晶格、壳、拉伸）之详细攻略
3dmax:3dmax的软件右边栏常用修改器(Cloth修改器.车削.倒角/剖面.对称.FFD长方体/圆柱体.Gizmo.规格化样条线.挤出 .路径.扭曲.晶格.壳.拉伸.面挤出.网格/平滑.涡轮平滑 ...
R语言使用plot函数和lines函数可视化线图（line plot）时、图之间的主要区别是由选项type产生的、type参数常用参数说明、不同type生成的可视化图像对比
R语言使用plot函数和lines函数可视化线图(line plot)时.图之间的主要区别是由选项type产生的.type参数常用参数说明.不同type生成的可视化图像对比目录
Double S 曲线轨迹规划——不同速度加速度条件下综合
目前机械臂常用的轨迹曲线主要是S曲线,由于前后两端速度为抛物线形式,整体相对平滑,具有较好的过渡.程序涵盖了混合轨迹,加速轨迹和减速轨迹,通过设置flag和合理的速度加速度输入值获取双S曲线. 参数的 ...
python画点连线_python matplotlib 在指定的两个点之间连线方法
python matplotlib 在指定的两个点之间连线方法为了找到matplotlib在两个点之间连线的方法真是费了好大功夫,最后还是决定用简单的 plt.plot 来解决.如果有好多对点,则可 ...
利用stm32控制步进电机速度加速度控制
因为想申请 CSDN 博客认证需要一定的粉丝量,而我写了五年博客才 700 多粉丝,本文开启关注才可阅读全文,很抱歉影响您的阅读体验利用stm32控制步进电机尝试用42系步进电机做倒立摆,总结步进 ...
使用加速度传感器与GPS位移求速度
根据卡尔曼滤波来进行计算,刚开始时,不清楚为什么卡尔曼滤波能够预测物体的状态,看了几天后发现,采样率是很重要的一个点. 假设车辆做匀加速直线运动,要预测车辆的速度vt,在采样率确定的情况下,很明显时间 ...
hdu 5092 线裁剪(纵向连线最小和+输出路径)
http://acm.hdu.edu.cn/showproblem.php?pid=5092 给一个m*n的矩阵,找到一个纵向的"线"使得线上的和最小并输出这条线,线能向8个方向延 ...
vscode新手注意事项(字体间隔，报错提示波浪线，头文件路径，opencv头文件路径)
一.字体空格刚安装vscode,不设置字体的话,字体间的间隔会很难受,需要进行如下配置. 在设置->首选项选择文本编辑器->字体 ,将"FONT Family " ...
ABB机器人线速度_ABB机器人发生不一致路径精确性故障维修
ABB机器人发生不一致路径精确性故障维修描述:ABB机器人的TCP路径出现不一致,会导致其经常变化,并且伴有轴承.变速箱及其他位置的噪音发出,直接的后果就是导致机器人无法正常进行生产. ABB机器人发 ...

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

文章目录

一、问题描述

二、简单推导

三、matlab代码

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

最新文章

热门文章