How to create a oscillating trajectory using the kinematicTrajectory Object?

조회 수: 10(최근 30일)
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!

채택된 답변

Ryan Salvo
Ryan Salvo 2021년 11월 11일
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)
Thanks,
Ryan

추가 답변(0개)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by