Make writing a habit together! This is the 11th day of my participation in the “Gold Digging Day New Plan · April More text Challenge”. Click here for more details.

0 foreword

Hardcore, robots these days teach themselves to “back into a garage.” This article is essentially a stabilization problem for robots. In this paper, a trajectory tracking application is completed based on the differential wheeled robot model, so that the robot can track the trajectory of heart in as short a time as possible.

The simulation effect GIF is shown below. After reading this article, I believe you can also do it! (At the end of the article == actual robot operation effect picture ==!)

1 What is trajectory tracking?

With stability and rapidity as the core, trajectory tracking plans the motion states between path points based on time differential constraints (such as velocity and acceleration), and trajectory planning endows path planning with time information. Simply put: it is to control the robot with the shortest time to keep up with the desired trajectory.

In contrast with the stabilization problem in the previous phase, the stabilization problem is to design a control law to make the error decay to 0 in a finite time, and the current pose is stable at the reference pose.

The problems discussed in these two blog posts are easy and simple on the surface, but the application behind them is very extensive, which is commonly seen in intelligent car competitions such as tracking and balance. There are also quite a number of papers on these two problems in the academic circle. Interested students can continue to have a deeper understanding of these two problems, and I put the relevant reference papers at the end of the article.

2 Start programming implementation

2.1 Trajectory Selection

R = 5.0;
T = 40.0;
w = 2 * pi / T;

ref = @(t) [R*(2*sin(w*t)-sin(2*w*t)), R*(2*cos(w*t)-cos(2*w*t))];
Copy the code

Let’s print it out

2.2 Write trajectory tracking algorithm

function dpdt = wmrTrackOde45Func(t, p, ref)
Copy the code

Get the current state of the robot

% robot state
x = p(1);
y = p(2);
theta = p(3);
Copy the code

Get the reference pose information

% reference path
pRef = ref(t);
xRef = pRef(1);
yRef = pRef(2);
vRef = 1;
wRef = 0;
Copy the code

Get the target vector

% predict reference path
targetVec = [xRef - x;
             yRef - y]; 
if targetVec(2) > 0
    thetaRef = acos(targetVec(1) / sqrt(targetVec(1)^2 + targetVec(2)^2));
else
    thetaRef = - acos(targetVec(1) / sqrt(targetVec(1)^2 + targetVec(2)^2));
end
Copy the code

Controller Parameter Setting

% controller parameters
Kx = 15;
Ky = 25;
Kt = 10;
Copy the code

control

% controller
ex = cos(theta) * (xRef - x) + sin(theta) * (yRef - y);
ey = -sin(theta) * (xRef - x) + cos(theta) * (yRef - y);
u  = [ vRef * cos(thetaRef - theta) + Kx * ex;
       wRef + vRef*(Ky * ey + Kt * sin(thetaRef - theta))];

v = u(1);
w = u(2);

dpdt = [v*cos(theta); v*sin(theta); w];
end
Copy the code

Then encapsulate this function into oDE45 called function, let Matlab help us to calculate iteratively.

2.3 Test tracking performance

Green is the initial position of the robot, and blue is the actual trajectory of the robot.

Then take a look at the error curve. In 5s, all parameters can track the expected curve. This effect can be observed by adjusting the parameters of the controller, deepening the understanding of the control function.What about other trajectories? If you look at the tracking effect of the circle track below, there is no problem even if the initial position is very off.

2.4 Make giFs

figure(1)
trackProcess = VideoWriter('video/trackProcess.avi');
open(trackProcess);
movie = moviein(t);

%Draw a reference trajectory
plot(refPos(:,1),refPos(:,2), "--b", 'LineWidth', 1);
grid on
hold on

%Draw the initial positionHandler = plotPose(initPos, 0.8, 0.3, 'r', size); for i=1:length(t) delete(handler); handler = plotPose(actualPos(i,:)); Plot (actualPos (I, 1), actualPos (I, 2), 'Marker', '. ', 'color', [,0.5 0, 0]). movie(:, i) = getframe; writeVideo(trackProcess, movie(:, i)); end hold off close(trackProcess);Copy the code

3 Real car combat

Due to the limit of 5M, only part of the GIF is released, and the whole video can be watched by the public account.

4 References

Those who want to do related research can read the literature below

[1] Yang Shichao. Research on Feedback Stabilization and Trajectory Tracking Control of Nonholonomic Wheeled Mobile Robot [D]. Chang ‘an University. [2]Kanayama Y, Kimura Y, Miyazaki F, et al. A stable tracking control method for an autonomous mobile robot[C]// IEEE International Conference on Robotics & Automation. IEEE, 1991.

For more content, please follow my AI channel “AI Technology Club “.