How to create a oscillating trajectory using the kinematicTrajectory Object?

Question

zial_hka 2021-11-11

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1584419-how-to-create-a-oscillating-trajectory-using-the-kinematictrajectory-object

回答： Ryan Salvo 2021-11-11

Hello there,

currently I'm trying to generate a oscillating trajectory to simulate the movement of a walking person carrying my sensor platform.

I'm trying to generate the trajectory using the "kinematicTrajectory" System Object from the Sensor Fusion/Navigation Toolbox.

The inputs should be the acceleration and angular velocity in the body coordinate system. Outputs should be using the NED coordinate system. I'm currently simulating the oscillation using a simple sine wave function.

I'd like to achieve something like this (top-down-view on the x,y-plane):

black line: "optimal" trajectory without the oscillation

red line: trajectory with oscillation

(please excuse my poor MSPaint drawing...)

Unfortunately my current approach doesn't look similar...

For some reason the trajectory seems to "drift" away without a (for me) visible reason (top-down-view on the x,y-plane):

I'm using this code at the moment:

%%% startup
initSpeed = 1;
samples = 10000;
fs = 10;
ft = 10;
%%%
bodyAcc = zeros(samples, 3);
bodyAngVel = zeros(samples, 3);
for i = 1:samples
    bodyAcc(i, 2) = 5 * sin((i-1) * 2*pi / fs);
end
traj = kinematicTrajectory('SampleRate', ft, ...
    'Velocity', [ initSpeed 0 0 ], ...
    'Orientation', quaternion( [ 0 0 0 ], 'eulerd', 'XYZ', 'frame' ));
[ pos, ~, ~, ~, ~ ] = traj(bodyAcc, bodyAngVel);
figure('Name', 'traj out')
plot3(pos(:,1), pos(:,2), pos(:,3))
xlabel('x (m)')
ylabel('y (m)')
zlabel('z (m)')
title('Position')
axis([0 10 -5 5]) % axis setting
grid on

Maybe someone knows a solution or is having the same problem as me...

Thank you in advance!

0 个评论
显示 -2更早的评论隐藏 -2更早的评论

请先登录，再进行评论。

请先登录，再回答此问题。

Answer 1

Ryan Salvo 2021-11-11

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1584419-how-to-create-a-oscillating-trajectory-using-the-kinematictrajectory-object#answer_829604

在 MATLAB Online 中打开

Hello,

You'll want to generate your sine wave for the body acceleration using your specifed sampling frequency ft and set a non-zero initial y-velocity so that the oscillation occurs:

%%% startup
samples = 1000;
fs = 10;
ft = 10;
initSpeed = 1; %5/(2*pi/fs);
%%%
bodyAcc = zeros(samples, 3);
bodyAngVel = zeros(samples, 3);
for i = 1:samples
    t = (i-1)/ft; % Set "t" based on sampling rate "ft".
    bodyAcc(i, 2) = 5 * sin(t * 2*pi / fs);
end
traj = kinematicTrajectory('SampleRate', ft, ...
    'Velocity', [ initSpeed -5/(2*pi/fs) 0 ], ... % Set y-velocity.
    'Orientation', quaternion( [ 0 0 0 ], 'eulerd', 'XYZ', 'frame' ));
[ pos, ~, ~, ~, ~ ] = traj(bodyAcc, bodyAngVel);
figure('Name', 'traj out')
plot3(pos(:,1), pos(:,2), pos(:,3))
xlabel('x (m)')
ylabel('y (m)')
zlabel('z (m)')
title('Position')
%axis([0 10 -5 5]) % axis setting
grid on
view(2)