CN115342805A - High-precision robot positioning navigation system and navigation method - Google Patents

Abstract

The invention provides a high-precision robot positioning and navigation method and a high-precision robot positioning and navigation system. The robot chassis comprises a robot control system, a mobile communication module and a positioning navigation system. The positioning navigation system consists of a front camera, a rear camera and an inertial sensor, wherein the cameras acquire information of the ground auxiliary device, and the inertial sensor acquires position information of the robot. The front camera and the rear camera simultaneously follow the line to correct the left-right driving deviation of the robot; and at the position correction identification point in the ground auxiliary device, the robot performs vision-inertia information fusion to calibrate the position of the robot. The invention corrects the left and right deviation by the simultaneous line circulation of the front camera and the rear camera, and performs visual-inertial information fusion when a position correction identification point exists, thereby calibrating the position of the robot and improving the positioning precision of the robot.

Description

A high-precision robot positioning and navigation system and navigation method

technical field

The invention belongs to the field of robot positioning and navigation, and in particular relates to a high-precision robot positioning and navigation system and a navigation method.

Background technique

With the progress and development of society, it is the development trend of today's industrial production that robots replace manual handling of heavy items. Now the same type of robots on the market mainly use magnetic navigation, laser navigation, two-dimensional code navigation, inertial navigation and other navigation methods.

Magnetic navigation uses magnetic strips and some markers to complete the positioning and navigation of the robot, and the magnetic strips guide the robot along a fixed trajectory. In practical application, the laying of magnetic strips is difficult and the construction cost is high, which increases the production cost and reduces the profit of the enterprise.

Laser navigation is to install a laser reflector around the driving path of the robot. The robot emits a laser beam and collects the laser beam reflected by the reflector to determine its current position and direction. Although laser navigation has the advantages of high precision, its cost is high, and it has strict environmental requirements, so it is difficult to apply in complex environments.

Two-dimensional code navigation is to lay out two-dimensional codes discretely, and obtain real-time coordinates by scanning and analyzing the two-dimensional codes through the robot camera. Since the two-dimensional code is easy to wear and needs regular maintenance, it increases the cost of later maintenance, and this method is only suitable for warehouses with good environments, not for complex environments.

Inertial navigation is to install a gyroscope on the robot. The gyroscope can be used to obtain the three-axis angular velocity and acceleration of the robot, and the robot can be navigated and positioned by integral calculation. As the gyroscope itself grows with time, the error will accumulate and increase, or even be lost. location, causing serious losses, and cannot meet the needs of high-precision positioning and navigation.

The above navigation methods have high cost of scene laying, low positioning and navigation accuracy, high requirements for use scenarios, and are difficult to apply to complex use environments. Therefore, a positioning and navigation method with low cost, simple scene laying and high precision is needed. .

Contents of the invention

Purpose of the invention: The purpose of the present invention is to provide a high-precision robot positioning and navigation method and system. The present invention effectively solves the problems that the existing robot positioning and navigation accuracy is not high, and the high-precision robot positioning and navigation system requires high usage scenarios.

Technical solution: the high-precision robot positioning and navigation system of the present invention, the high-precision robot positioning and navigation system includes a robot chassis and a ground auxiliary device;

The robot chassis includes a robot control system, a positioning and navigation system, and a mobile communication module;

The ground auxiliary device includes tracking lines that are easy for the robot to recognize and position correction marking points that are different in color from the tracking lines, so as to provide effective detection information for the robot to obtain.

Further, the positioning and navigation system includes a front camera installed in the middle of the front of the chassis, a rear camera installed in the middle of the rear of the chassis, and an inertial sensor installed in the center of the chassis.

Further, the mobile communication module is used for data transmission between the control system and the background, the driving data of the robot is sent to the background through the mobile communication module, and the background receives operation instructions and work tasks from the user terminal, and the background data is synchronized in the user terminal show.

(l₂为机器人转弯半径)标定一个位置校正标识点。Further, for the position of the position correction mark point of the ground auxiliary device, a position correction mark point is calibrated at every interval distance of 0.5l ₁ to 3l ₁ (l ₁ is the length of the robot) for the straight part of the tracking line, and the curved part of the track line Each distance is

(l ₂ is the turning radius of the robot) to calibrate a position calibration mark point.

The invention also discloses a high-precision robot positioning and navigation method, which includes the following steps:

Step 1: Start the robot by sending an instruction through the user terminal, and the robot is initialized;

Step 2: Obtain the initial position data of the robot, where the initial position data includes: visual initialization data, inertial initialization data, visual-inertial information fusion data; visual data to estimate the position of the robot, inertial data to estimate the pose of the robot, and then perform visual -Inertial information fusion to further accurately estimate the position of the robot;

Step 3: Send the visual initialization data, inertial sensor initialization data, and visual-inertial information fusion data of the robot in step 2 to the background in real time through the mobile communication module;

Step 4: The robot enters the standby state, waiting for the user to release the robot's work tasks to the background through the user terminal;

Step 5: The robot detects whether there is a work task, and if not, jumps to step 4; if there is a work task, the robot enters the working state and performs the following steps;

Step 6: The visual sensor and the inertial sensor obtain the driving data of the robot in real time. The driving data includes: the key frame data of the ground tracking line obtained by the front and rear cameras and the key frame data of the robot position correction point obtained by the rear camera;

Step 7: After the camera detects whether the robot has traveled to the position correction mark point, it is judged whether visual-inertial information fusion is required to further refine the robot positioning information; if the robot travels to the position correction mark point, go to step 8, otherwise, go directly to step 9;

Step 8: Visual-inertial information fusion;

Step 9: The front and rear cameras follow the line at the same time and correct the driving error of the robot according to the position of the theoretical tracking line, and use the control algorithm to control the driving of the robot;

Step 10: Whether the task of the robot is completed, if not, go to step 6; if the task of the robot is completed, enter the next step;

Step 11: The robot task ends; if the user does not perform any operation, the robot enters step 2; if the user turns off the robot, the robot enters the shutdown state;

Step 12: End.

Further, in step 6, the front and back cameras acquire key frame data of ground tracking lines:

The front camera calculates the observed tracking data of the robot, including the abscissa x _a0 of the first row of the tracking line, the abscissa x _an of the tracking line every n lines, and calculates the robot offset angle θ _a observed by the front camera, where

The rear camera calculates the observed robot tracking data, which includes the abscissa x _b0 of the first row of the tracking line, and the abscissa x _bn of the tracking line every n rows, and calculates the robot offset angle α _b observed by the rear camera, where

The rear camera obtains key frame data of robot position correction points:

Step 1.1. If the rear camera detects the position correction point, then calculate the robot visual mileage d ₂ according to the known ground setting method of the ground position correction mark point, where

d ₂ =d′ ₂ +l

In the formula, d' ₂ is the visual mileage of the previous position correction point, and l is the distance of the position correction mark point set on the known ground;

Step 1.2, if the rear camera does not detect the position correction mark point, the robot visual mileage d ₂ remains unchanged;

Step 1.3, the inertial sensor is used to calculate the inertial mileage d ₁ of the robot, where d ₁ is calculated as follows:

In the formula, d ₀ is the inertia mileage of the previous cycle, v is the driving speed of the robot measured by the inertial sensor, a is the acceleration of the robot measured by the inertial sensor, and t is the sampling period of the inertial sensor.

Further, in step 8, the visual-inertial information fusion method is as follows:

Step 2.1, if the visual mileage d ₂ and the inertial mileage d ₁ do not exceed half of the distance l between the two known positioning marks on the ground, then the

Then the inertial mileage d ₁ is updated to the visual mileage d ₂ , that is, d ₁ =d ₂ at this time;

Step 2.2, if the visual mileage d ₂ and the inertial mileage d ₁ exceed the distance l between the two known positioning marks on the ground, then the

max{|d ₂ -d ₁ |, |d ₁ -d ₂ |}＞l+lk(3≥k≥0)

This situation is the lack of position correction marker points on the ground, and the processing method is as follows: when driving in a straight line, the inertia mileage d ₁ is updated to the distance of the positioning marker point d ₂ =d ₂ +lk, if k does not meet the constraint conditions, the robot will send abnormal data to the background , and wait for the user to re-calibrate the data. After the user corrects the data, go to step 9.

Further, the specific steps of step 9 are:

Step 3.1. Control the robot to follow the line according to the data of the front camera, and keep the tracking line in the middle of the front of the robot. The formula for calculating the output of the control system is as follows:

u ₁ ＝K _p1 e ₁ +K _i1 ∑e ₁ +K _d1 e ₁

e ₁ = θ _a - θ ₁

In the formula, K _p1 , K _i1 , K _d1 are the control coefficients, e ₁ is the error between the observed value θ _a of the front camera and the theoretical value θ ₁ , and u ₁ is the output of the control system;

Step 3.2. Control the robot to follow the line according to the data of the rear camera, and keep the tracking line in the middle of the rear of the robot. The output calculation formula of the control system is as follows:

u ₂ ＝K _p2 e ₂ +K _i2 ∑e ₂ +K _d2 e ₂

e ₂ = θ _b - θ ₂

In the formula, K _p2 , K _i2 , and K _d2 are control coefficients, e ₂ is the post-camera observation value θ _a and theoretical value θ ₂ , and u ₂ is the output of the control system.

Beneficial effect: compared with the prior art, the present invention has the following significant advantages:

(1) In the high-precision robot positioning and navigation system of the present invention, the price of the visual-inertial sensor positioning and navigation system is relatively low, and high-precision positioning and navigation can be realized.

(2) In the present invention, the front and rear cameras are used and located on the same straight line to ensure that the whole robot has no left-right deviation, and the algorithm is relatively simple, which can greatly reduce the calculation of the controller.

(3) The present invention relies on simple ground auxiliary devices, which is convenient for deployment in different scenes, and is easier to use in complex industrial scenes.

Description of drawings

Fig. 1 is the schematic diagram of high-precision robot system of the present invention;

Fig. 2 is a device diagram of the positioning and navigation system of the present invention;

Fig. 3 is a schematic diagram of the ground auxiliary device of the present invention;

Fig. 4 is the structural block diagram of robot system of the present invention;

Fig. 5 is a flow chart of the robot positioning and navigation method of the present invention.

Detailed ways

The technical solution of the present invention will be further described below in conjunction with the accompanying drawings.

Aiming at the problem of low positioning and navigation accuracy in related technologies, the embodiment of the present invention proposes a high-precision positioning and navigation system. The system uses a high-precision handling robot. Deviation, to meet the needs of high-precision robot positioning and navigation in practical applications.

A high-precision robot positioning and navigation system proposed according to an embodiment of the present invention will be described below with reference to the accompanying drawings.

As shown in Figure 1, the high-precision robot positioning and navigation system includes a robot chassis and ground auxiliary devices.

Robot chassis, including robot control system, mobile communication module and positioning and navigation system. The positioning and navigation system consists of a front camera, a rear camera, and an inertial sensor. The front camera and the rear camera are used to identify the tracking line, and the rear camera is also used to detect whether there is a position correction mark point. The user terminal, the user terminal communicates with the background in real time, and the user can conveniently view the robot's running status, driving position and control the robot through the user terminal. Ground auxiliary devices, including tracking lines that are easy for the robot to identify and position correction marking points that are different in color from the tracking lines, provide effective detection information for the robot to obtain.

According to the embodiment of the high-precision positioning and navigation system of the present invention, the user lays the ground auxiliary device in advance, which can provide the positioning and navigation information required by the robot when driving. After the user starts the robot through the user terminal, the robot starts to initialize the positioning and navigation system. After the initialization is completed, it waits for the control system to come from the background through the mobile communication module. When the robot performs work tasks, the inertial sensor calculates the inertial mileage of the robot. The front camera and the rear camera follow the line at the same time to ensure that the robot as a whole has no left-right deviation. , perform visual-inertial information fusion, and correct the positioning error of the robot.

As shown in FIG. 2 , the robot left front wheel 101 , left rear wheel 102 , right front wheel 103 , and right rear wheel 104 are placed as shown in the figure. The camera is embedded under the robot chassis, wherein the front camera 2111 is installed in the middle of the left and right front wheels of the robot chassis, and the rear camera 2112 is installed in the middle of the left and right front wheels of the robot chassis. Since the robot chassis is relatively close to the ground, the front and rear cameras are thin cameras. The inertial sensor 212 is placed at the geometric center of the robot chassis.

When the robot is running normally and straight, the front camera 2111 and the rear camera 2112 track and drive at the same time, keeping the front camera 2111, the rear camera 2112 and the ground auxiliary device on the same straight line. When driving in a curve, the front camera 2111 and the rear camera 2112 track at the same time, keep the tracking line of the front camera 2111, the rear camera 2112 and the ground auxiliary device on the same tangent, and the robot drives normally. When the rear camera 2112 detects the ground position calibration mark point, visual-inertial information fusion is performed to correct the positioning error.

(l₂为机器人转弯半径)标定一个位置校正标识点。。As shown in Figure 3, the position of the position correction mark point of the ground auxiliary device, the linear part of the tracking line is calibrated with a position correction mark point every interval distance of 0.5l ₁ ~ 3l ₁ (l ₁ is the length of the robot), and the tracking line The distance between the curve parts of the line is

(l ₂ is the turning radius of the robot) to calibrate a position calibration mark point. .

When a high-precision robot performs a task, the front camera and the rear camera track at the same time, which can ensure that the robot as a whole is directly above the tracking line, and avoid situations where only the front of the robot is directly above the tracking line, while other parts have deviated from the tracking line. Happening. At the same time, the rear camera detects whether the robot has traveled to the position correction mark point. When the position correction mark is detected, visual-inertial information fusion is performed to correct the positioning deviation of the robot.

As shown in Figure 4, the main control system of the robot in this embodiment is composed of a power supply module, a control system, a positioning and navigation system, and a mobile communication module. The positioning and navigation system is composed of a front camera, a rear camera, and an inertial sensor.

The power supply module provides electric energy for the work of the whole circuit. The mobile communication module receives the robot work tasks from the background, and the mobile communication module sends the work tasks to the main control system through the serial port; in turn, the main control system sends the robot running status to the mobile communication module through the serial port, and the mobile communication module sends the robot running status sent to the background. The inertial sensor calculates the inertial mileage, and the front camera and the rear camera track at the same time to correct the left-right deviation when the robot is driving, so as to ensure that the robot as a whole has no left-right deviation when driving. The rear camera also detects whether there is a position correction mark point at the same time. When the position correction mark point is detected, the visual-inertial information is fused to calibrate the positioning of the robot. The front camera, the rear camera and the main control system send and receive data through the serial port, and the front camera and the rear camera are ultra-thin cameras. Specifically, according to a specific embodiment of the present invention, the inertial sensor can use MPU6050, and the inertial sensor and the main control system can send and receive data through the IIC bus protocol.

FIG. 5 is a flow chart of the high-precision positioning and navigation method and system according to a specific embodiment of the present invention. Specific steps are as follows:

Step 1: Start the robot by sending an instruction through the user terminal, and the robot is initialized;

Step 2: Obtain the initial position data of the robot, where the initial position data includes: visual initialization data, inertial initialization data, and visual-inertial information fusion data. Visual data estimates the position of the robot, inertial data estimates the robot's pose, and then performs visual-inertial information fusion to further accurately estimate the robot's position;

Step 3: Send the visual initialization data, inertial sensor initialization data, and visual-inertial information fusion data of the robot in step 2 to the background in real time through the mobile communication module;

Step 4: The robot enters the standby state, waiting for the user to release the robot's work tasks to the background through the user terminal;

Step 5: The robot detects whether there is a work task, and if not, jumps to step 4; if there is a work task, the robot enters the working state and performs the following steps;

Step 6: The visual sensor and inertial sensor acquire the driving data of the robot in real time. Driving data includes,

(1) The front and rear cameras obtain the key frame data of the ground tracking line:

The front camera calculates the observed tracking data of the robot, including the abscissa x _a0 of the first row of the tracking line, the abscissa x _an of the tracking line every n lines, and calculates the robot offset angle θ _a observed by the front camera, where

The rear camera calculates the observed robot tracking data, including the abscissa x _b0 of the first line of the tracking line, the abscissa x _bn of the tracking line every n lines, and calculates the robot offset angle θ _b observed by the rear camera, where

(2) The rear camera obtains the key frame data of the robot position correction point:

1) If the rear camera detects the position correction point, then calculate the robot visual mileage d ₂ according to the known ground setting method of the ground position correction mark point, where

d ₂ =d′ ₂ +l

In the formula, d' ₂ is the visual mileage of the previous position correction point, and l is the distance of the position correction mark point set on the known ground;

2) If the rear camera does not detect the position correction mark point, the robot visual mileage _d2 remains unchanged;

(3) The inertial sensor is used to calculate the inertial mileage d ₁ of the robot, where d ₁ is calculated as follows:

In the formula, d ₀ is the inertia mileage of the previous cycle, v is the driving speed of the robot measured by the inertial sensor, a is the acceleration of the robot measured by the inertial sensor, and t is the sampling period of the inertial sensor;

Step 7: The rear camera detects whether the robot has traveled to the position correction mark point, and judges whether visual-inertial information fusion is required to further accurately position the robot. If the robot travels to the position correction mark point, then go to step 8, otherwise, go directly to step 9;

Step 8: The visual-inertial information fusion method is as follows:

(1) If the visual mileage d ₂ and the inertial mileage d ₁ do not exceed half of the distance l between two known positioning marker points on the ground, then the

Then the inertial mileage d ₁ is updated to the visual mileage d ₂ , that is, d ₁ =d ₂ at this time;

(2) If the visual mileage d ₂ and the inertial mileage d ₁ exceed the distance l between two known positioning marks on the ground, then the

max{|d ₂ -d ₁ |, |d ₁ -d ₂ |}＞l+lk(3≥k≥0)

This situation is that the ground is missing a position correction mark point, and the processing method is as follows: when driving straight, the inertia mileage d ₁ is updated to the distance of the positioning mark point d ₂ =d ₂ +lk. If k does not meet the constraints, the robot will send abnormal data to the background, waiting for the user to re-correct the data. After the user corrects the data, go to step 9;

Step 9: The front and rear cameras follow the line at the same time and correct the robot's driving error according to the position of the theoretical tracking line. The control algorithm is used to control the driving of the robot.

(1) According to the data of the front camera, the robot is controlled to follow the line, and the tracking line can be kept in the middle of the front of the robot. The calculation formula of the output of the control system is as follows:

u ₁ ＝K _p1 e ₁ +K _i1 ∑e ₁ +K _d1 e ₁

e ₁ = θ _a - θ ₁

In the formula, K _p1 , K _i1 , K _d1 are the control coefficients, e ₁ is the error between the observed value θ _a of the front camera and the theoretical value θ ₁ , and u ₁ is the output of the control system;

(2) According to the data of the rear camera, the robot is controlled to follow the line, and the tracking line can be kept in the middle of the rear of the robot. The calculation formula of the output of the control system is as follows:

u ₂ ＝K _p2 e ₂ +K _i2 Σe ₂ +K _d2 e ₂

e ₂ = θ _b - θ ₂

In the formula, K _p2 , K _i2 , K _d2 are control coefficients, e ₂ is the post-camera observation value θ _a and theoretical value θ ₂ , u ₂ is the output of the control system;

Step 10: Whether the task of the robot is completed, if not, go to step 6; if the task of the robot is completed, go to the next step.

Step 11: End of robot mission:

(1) If the user does not perform any operation, the robot will enter step 2;

(2) If the user turns off the robot, the robot enters the shutdown state.

Step 12: End.

Claims (8)

1. A high-precision robot positioning navigation system is characterized by comprising a robot chassis and a ground auxiliary device;

the robot chassis comprises a robot control system, a positioning navigation system and a mobile communication module;

the ground auxiliary device comprises a tracking line easy to recognize by the robot and a position correction identification point different from the color of the tracking line, and provides convenience for the robot to acquire effective detection information.

2. The high accuracy robot positioning navigation system of claim 1, wherein the positioning navigation system comprises a front camera mounted in the middle of the front of the chassis, a rear camera mounted in the middle of the rear of the chassis, and an inertial sensor mounted in the center of the chassis.

3. The high-precision robot positioning and navigation system according to claim 1, wherein the mobile communication module is used for controlling data transmission between the system and the background, the robot driving data is sent to the background through the mobile communication module, the background receives the operation instruction and the work task from the user terminal, and the background data is synchronously displayed on the user terminal.

4. The high accuracy robot positioning and navigating system according to claim 1, wherein the position of the ground assisting device corrects the position of the identification point, and the linear part of the tracking line is 0.5l per interval distance ₁ ～3l ₁ (l ₁ For the length of the robot) to calibrate an identification point for the position, the tracing curve part has the following distance

(l ₂ For the robot turning radius) to calibrate a position correction mark point.

5. A high-precision robot positioning and navigation method is characterized by comprising the following steps:

step 1: the robot is started by sending an instruction through a user terminal, and the robot is initialized;

step 2: acquiring initial position data of the robot, wherein the initial position data comprises: visual initialization data, inertial initialization data and visual-inertial information fusion data; the position of the robot is estimated through the visual data, the pose of the robot is estimated through the inertial data, then the visual-inertial information fusion is carried out, and the position of the robot is further accurately estimated;

and step 3: the vision initialization data, the inertial sensor initialization data and the vision-inertial information fusion data of the robot in the step 2 are sent to a background in real time through a mobile communication module;

and 4, step 4: the robot enters a standby state, and waits for a user to release a work task of the robot to a background through a user terminal;

and 5: the robot detects whether a work task exists, and if not, the robot jumps to the step 4; if the robot has the work task, the robot enters a working state and executes the following steps;

step 6: the vision sensor and the inertial sensor acquire the driving data of the robot in real time, and the driving data comprises: the method comprises the steps that a front camera and a rear camera acquire key frame data of ground tracking lines and key frame data of robot position correction points;

and 7: the rear camera detects whether the robot runs to the position correction identification point or not, judges whether visual-inertial information fusion is needed or not, and further accurately positions the robot; if the robot runs to the position correction identification point, the step 8 is carried out, otherwise, the step 9 is directly carried out;

and 8: visual-inertial information fusion;

and step 9: the front camera and the rear camera simultaneously run along the tracks, the running error of the robot is corrected according to the position of the theoretical tracking line, and the running of the robot is controlled by adopting a control algorithm;

step 10: whether the work task of the robot is finished or not is judged, and if not, the step 6 is skipped; if the task of the robot is completed, entering the next step;

step 11: the robot task is finished; if the user does not execute any operation, the robot enters the step 2; if the user closes the robot, the robot enters a shutdown state;

step 12: and (6) ending.

6. The method according to claim 5, wherein in step 6, the front and back cameras acquire the key frame data of ground tracking:

front camera calculates the robot cycle observedLine data, including the trace-following primary abscissa x _a0 Transverse axis x of the tracking line of every n rows _an Calculating the offset angle theta of the robot observed by the front camera _a Wherein

The rear camera calculates the observed robot tracking data, including the first row abscissa x of the tracking line _b0 Transverse axis x of the tracking line of every n rows _bn Calculating the offset angle theta of the robot observed by the rear camera _b Wherein

The rear camera acquires key frame data of a robot position correction point:

step 1.1, if the rear camera detects the position correction point, calculating the robot visual mileage d according to the known ground setting mode of the ground position correction identification point ₂ In which

d ₂ ＝d′ ₂ +l

In the formula (II), d' ₂ The visual mileage of the last position correction point is represented by l, which is the distance of the position correction identification point set on the known ground;

step 1.2, if the position correction identification point is not detected by the rear camera, the robot vision mileage d ₂ Keeping the original shape;

step 1.3, the inertial sensor is used for calculating the inertial mileage d of the robot ₁ In which d is ₁ The calculation is as follows:

in the formula (d) ₀ Is the inertial mileage of the last cycle, v is the robot running speed measured by the inertial sensor, and a is the machine measured by the inertial sensorHuman acceleration, t, is the sampling period of the inertial sensor.

7. The high-precision robot positioning and navigating method according to claim 5, wherein in step 8, the visual-inertial information fusion method comprises the following steps:

step 2.1, if vision mileage d ₂ And inertia mileage d ₁ When the distance between two known positioning mark points on the ground is not more than half l, the requirement is met

Inertia mileage d ₁ Updated to the visual mileage d ₂ At this time, i.e. d ₁ ＝d ₂ ；

Step 2.2, if vision mileage d ₂ And inertia mileage d ₁ When the distance l of the two known positioning mark points exceeds the ground, the requirement is met

max{|d ₂ -d ₁ |，|d ₁ -d ₂ |}＞l+lk(3≥k≥0)

In this case, the processing method is as follows: inertial distance d in straight line ₁ Updated to the positioning identification point distance d ₂ ＝d ₂ And + lk, if k does not meet the constraint condition, the robot sends abnormal data to a background, waits for the user to correct the data again, and enters step 9 after the user finishes correcting the data.

8. The high-precision robot positioning and navigating method according to claim 5, wherein the specific steps of step 9 are as follows:

step 3.1, controlling the robot to run in a tracking manner according to the data of the front camera, keeping the tracking line at the middle position of the front part of the robot, and controlling a system output quantity calculation formula as follows:

u ₁ ＝K _p1 e ₁ +K _i1 ∑e ₁ +K _d1 e ₁

e ₁ ＝θ _a -θ ₁

in the formula, K _p1 、K _i1 、K _d1 To control the coefficient, e ₁ As a front camera observation value theta _a And the theoretical value theta ₁ Error between u ₁ To control the output of the system;

and 3.2, controlling the robot to run in a tracking manner according to the data of the rear camera, keeping the tracking line at the middle position of the rear part of the robot, and controlling a system output quantity calculation formula as follows:

u ₂ ＝K _p2 e ₂ +K _i2 ∑e ₂ +K _d2 e ₂

e ₂ ＝θ _b -θ ₂

in the formula, K _p2 、K _i2 、K _d2 To control the coefficient, e ₂ As a rear camera observation value theta _a To the theoretical value theta ₂ ，u ₂ To control the output of the system.

Images