“This is the 8th day of my participation in the First Challenge 2022. For details: First Challenge 2022”

---

0 foreword

This paper makes a kinematic application based on the differential wheeled robot model, that is, control the speed difference between the two wheels of the robot to change its movement trajectory, so that the robot can complete aBack into the TreasuryThe action.The simulation effect GIF is shown below. After reading this article, I believe you can also do it! (There are actual robot operation renderings at the end of the article!)

1 What is differential wheeled robot?

Differential wheeled robot is a kind of wheeled robot, as shown in the figure below. Its characteristics are: two wheels only around the axis of rotation, and no other speed components, so this robot can not lateral movement, imagine the home of the sweeper, the road to open the car, they are differential structure, so can not “cross open”! What kind of robot can drive sideways? This needs to be fitted with the McNum wheel, and we’ll talk about it later.

2. Kinematics equation of differential wheeled robot

The kinematic equation of differential wheeled robot is as follows:

This article does not derive arcane formulas; it is written here for programming purposes only.

3 Start programming

3.1 Robot trajectory control

First of all, we need to determine two positions, one is the parking position ref, the other is the current position P of the car

%Parking space
xRef = ref(1);
yRef = ref(2);
thetaRef = ref(3);

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

And then we have to calculate the error between the two

%error
xErr = cos(theta) * (xRef - x) + sin(theta) * (yRef - y);
yErr = -sin(theta) * (xRef - x) + cos(theta) * (yRef - y);
thetaErr = thetaRef - theta;
Copy the code

And then we want to reduce the error

%parameter
Kpx = 3
Kpt = 3

%The linear and angular velocities of the wheels
v = Kpx * sqrt(xErr^2 + yErr^2);
w = Kpt * thetaErr;
Copy the code

Those of you who are familiar with control theory will recognize that this is a P-feedback controller. And then we feed that increment back out

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

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

3.2 Draw parking Spaces and cars

Parking Spaces are simple, just three straight lines

The annotation (' line ', [0.53, 0.53], [0.35, 0.5], 'Color', 'k'); The annotation (' line ', [0.63, 0.63], [0.35, 0.5], 'Color', 'k'); The annotation (' line ', [0.53, 0.63], [0.35, 0.35], 'Color', 'k');Copy the code

Cars can be drawn in any way you like, and this article uses arrows to indicate the direction of the car.

arrow = quiver(x, y, endPt(1) - x, endPt(2) - y, ... 'MaxHeadSize',5.5,'AutoScaleFactor',1,'AutoScale','off', 'LineWidth', 1.5, 'color', color... 'Marker', 'o', 'MarkerSize', 4, 'MarkerFaceColor',color);Copy the code

Results the following

Add some difficulty, the parking space tilted over, also very good performance!

3.3 Making giFs

Then see how to make Matlab demo animation, the following is the complete code

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

%Draw parkingThe annotation (' line ', [0.5, 0.5], [0.2, 0.3], 'Color', 'k'); The annotation (' line ', [0.63, 0.63], [0.2, 0.3], 'Color', 'k'); The annotation (' line ', [0.5, 0.63], [0.2, 0.2], 'Color', 'k');
%Draw target position
plotPose(refPos);
grid on
hold on

%Draw the initial positionhandler = plotPose(initPos); 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(stableProcess, movie(:, i)); end hold off close(stableProcess);Copy the code

4 real car combat

Because the GIF size is limited to 5M, the following example abstracts most of the frames.

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