# How to create a oscillating trajectory using the kinematicTrajectory Object?

조회 수: 10(최근 30일)
zial_hka 2021년 11월 11일
답변: 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...

댓글을 달려면 로그인하십시오.

### 채택된 답변

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

댓글을 달려면 로그인하십시오.

### 범주

Find more on Trajectory and Scenario Generation in Help Center and File Exchange

R2021b

### Community Treasure Hunt

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

Start Hunting!

Translated by