CN115480571B

CN115480571B - A method for autonomous navigation and motion control of inspection robots

Info

Publication number: CN115480571B
Application number: CN202211132836.8A
Authority: CN
Inventors: 吴应; 张目华; 马磊; 沈楷; 孙永奎
Original assignee: Southwest Jiaotong University
Current assignee: Southwest Jiaotong University
Priority date: 2022-09-16
Filing date: 2022-09-16
Publication date: 2025-02-25
Anticipated expiration: 2042-09-16
Also published as: CN115480571A

Abstract

The present invention discloses an autonomous navigation motion control method for an inspection robot, including navigation motion control outside an inspection ditch and navigation motion control inside an inspection ditch; the navigation motion control outside the inspection ditch is as follows: determining the current initial posture and driving direction of the robot, rotating the inspection robot body to the average angle direction of the initial posture angle, using an improved dead reckoning algorithm to predict the trajectory, and moving to a specified position according to the calculated trajectory and speed; the navigation motion control inside the inspection ditch is as follows: obtaining the distance to the target point, precalculating the linear speed control amount by trapezoidal acceleration and deceleration, fitting the walls on both sides of the inspection ditch by laser radar, obtaining the positioning data of the movement distance in the ditch at the same time, dynamically adjusting the linear speed and angular speed of the inspection robot, and keeping it traveling in a straight line to the specified position in the inspection ditch. The present invention can realize precise control of the robot body both inside and outside the inspection ditch, thereby safely and efficiently completing the daily inspection operation of the vehicle bottom.

Description

Autonomous navigation motion control method for inspection robot

Technical Field

The invention belongs to the field of robots, and particularly relates to an autonomous navigation movement control method of a patrol robot.

Background

In order to ensure the safe operation of vehicles such as passenger and freight vehicles or trains, subway trains, urban rail trains and the like, the parts of the vehicle bottom are required to be comprehensively checked outside the operation time, including whether the bolt connection of the vehicle bottom is loose, whether the wheel pair parts are worn, whether the structural parts of the vehicle bottom have cracks and the like, so that the potential safety hazard of the vehicle in operation is reduced, and the normal operation of the vehicle is ensured. The traditional vehicle bottom inspection operation adopts a method of manually entering an inspection trench for inspection, but because the labor intensity of personnel is high, the inspection efficiency is low, and the problem of false inspection and missing inspection is easily caused, the vehicle bottom inspection robot is started to replace a manual inspection operator to finish the vehicle bottom inspection operation of the vehicle.

The work flow and the work scene of the vehicle bottom inspection robot are very special, and generally, when a new inspection task is received, the inspection robot starts from a ground charging station, enters an inspection trench through an elevator, automatically navigates to a fixed point according to requirements to acquire data, and after the acquisition is completed, the robot automatically rides on the elevator to return to the ground, automatically returns to the charging station to supplement electric quantity, and reenters a standby state. The biggest characteristic of the working scene is that the working space is very narrow, especially in the process of automatically entering and exiting the elevator and running in the inspection trench, the left and right sides of the inspection robot are only 3cm-5cm, the navigation control algorithm of the inspection robot is required to precisely control the inspection robot body with larger volume and mass on the centimeter scale, and in addition, the inspection robot must keep straight running in the trench in order to ensure the quality of the acquired image data. However, the conventional inspection robot generally works in a relatively open environment, and such precise control of the robot body is not required.

Therefore, it is necessary to study the motion control strategy of the inspection robot inside and outside the inspection trench for the scene to solve the above problems.

Disclosure of Invention

According to the unique working scene of the vehicle bottom inspection robot in and out of the inspection trench, precise control is carried out on the robot body with larger volume and mass in and out of the inspection trench, so that daily inspection operation of the vehicle bottom is completed safely and efficiently. The invention provides an autonomous navigation movement control method of a patrol robot.

The invention discloses an autonomous navigation motion control method of a patrol robot, which comprises an outside-patrol-ditch navigation motion control method and an inside-patrol-ditch navigation motion control method. When receiving a new inspection task, the inspection robot starts from a ground charging station and automatically navigates to a designated position to prepare for entering a trench for inspection by a lifting elevator, and the method for controlling the navigation movement outside the inspection trench comprises the following steps:

Step 1, acquiring a global navigation path through Dijkstra algorithm, extracting a current farthest tracking point according to set dead reckoning time length, taking the current farthest tracking point as a target point for current movement of the inspection robot, determining a current initial pose of the robot according to a request positioning data source, and judging whether the robot should be driven forwards or backwards according to an initial pose orientation angle and an angle from a final target point path.

And 2, rotating the inspection robot body to an average angle direction of the path reckoning section distance from the initial pose angle.

And 3, predicting a sampling track by using an improved dead reckoning algorithm, selecting a path with highest evaluation as a next running state of the inspection robot by an evaluation function, calculating a current speed instruction of the robot in real time, and performing dead reckoning and speed instruction calculation in a reciprocating manner, so that the inspection robot can be ensured to accurately and efficiently run to a final target point translation tolerance range.

And 4, rotating the inspection robot body to a final target point rotation tolerance range, and ending the navigation.

After the inspection robot enters the inspection trench through the lifting elevator, the inspection robot automatically navigates to a certain fixed position according to the instruction to carry out inspection operation, and the method for controlling navigation movement in the inspection trench comprises the following steps:

and step A, obtaining the distance between the target points, and pre-calculating the linear speed control quantity of trapezoidal acceleration and deceleration.

And B, fitting the wall surfaces on two sides of the inspection trench by using a laser radar to obtain geometric features of the wall surfaces, constructing two straight lines representing the left wall surface and the right wall surface according to the geometric features, and simultaneously obtaining the positioning data of the movement distance in the trench.

And C, dynamically adjusting the linear speed v and the angular speed w of the inspection robot, and keeping the inspection robot to linearly travel in the inspection trench.

And D, repeating the step C until the inspection robot reaches displacement tolerance, and ending the navigation.

Step 2 is to obtain the current course angle of the inspection robot, find the distance farthest point which can be calculated in the course calculation time in the global path, traverse and accumulate the straight angle from the inspection robot to each point, and calculate the average value of the angles in the distance, namely the angle which the inspection robot needs to rotate, wherein the calculation formula of the average value of the angles is as follows:

in the formula, For the average angle, p ₀ is the current point of the inspection robot in the global path, p ₁ is the most distant point of the distance which can be pre-calculated in the dead reckoning duration of the inspection robot in the global path, and x _i is the straight-going angle from the point p ₀ to the point i.

Further, the step 3 specifically includes:

step 3.1, determining a speed sampling spatial range, wherein the speed sampling spatial range is expressed as:

V_m＝{v∈[v_min,v_max],w∈[w_min,w_max]}

Wherein v _min、v_max is the minimum and maximum linear speeds of the inspection robot, and w _min、w_max is the minimum and maximum angular speeds of the inspection robot.

And 3.2, starting an improved dead reckoning algorithm to carry out dead reckoning and speed instruction calculation.

The inspection robot undergoes two stages of acceleration and uniform velocity in one dead reckoning, and the kinematic model expression at the moment is as follows:

Wherein x (k), y (k), x (k+1), y (k+1) represent position coordinate information of the robot at the kth time and the k+1 time, x _acc、y_acc represents a distance travelled by the acceleration stage, x _const、y_const represents a distance travelled by the constant speed stage, v _begin represents an initial speed of the robot in the dead reckoning, v _end represents a target search speed of the dead reckoning, t _sum represents total time of the dead reckoning, t _acc represents differential time, x _acc、ang_acc represents acceleration differential, ang _begin represents an initial angle of the dead reckoning of the inspection robot, ang _end represents a target search angle of the dead reckoning of the inspection robot, v _offset、ang_offset represents a difference value between the initial speed and the search speed, x _acc、ang_acc represents a speed change amount of the inspection robot in a differential process, and n represents the acceleration times required for accelerating the actual speed to the search speed.

Substituting the calculated track into a track evaluation function, selecting an optimal path according to evaluation, and then distributing a linear speed v and an angular speed w corresponding to the path to drive the inspection robot to move, wherein the track evaluation function scores according to two indexes of the degree of fitting of the deduced track with a tracking point and the degree of fitting with a global path based on a working scene of the inspection robot, and the mathematical expression is as follows:

Wherein score represents the score of the current deduction track, alpha represents the weight of the distance from the end point of the deduction track to the tracking point in the calculated score, beta represents the weight of the shortest distance from the deduction track to the global path in the calculated score, dist ₀ represents the distance from the end point of the deduction track to the tracking point, dist1 represents the shortest distance from the deduction track to the global path.

Repeating the steps 3.1-3.2 until the inspection robot reaches the displacement tolerance and then stopping.

Further, the step C specifically comprises the following steps:

According to the front radar and the rear radar, calculating the distance difference between the inspection robot and the wall bodies on two sides, taking the symmetrical geometric relationship of two straight lines as a target, and calculating the angular speed control quantity through a PID algorithm.

And (3) pre-calculating the linear speed control quantity by adopting a trapezoid acceleration and deceleration algorithm based on the current driving mileage, releasing the current pre-calculated linear speed v and angular speed w, and enabling the inspection robot body to move according to the released linear speed v and angular speed w, so that the inspection robot can be ensured to linearly drive in the inspection ditch.

The trapezoid acceleration and deceleration pre-linear speed calculation method comprises the following steps:

the constraints are as follows:

x₁＝x₃-x₂

Wherein x is more than or equal to 0 and less than or equal to x ₁, x ₁≤x≤x₂, x ₂≤x≤x₃, x ₃, max (a, b) and vel _min are respectively the maximum value of a and b, and ve _max is the minimum linear speed of the robot, x is the maximum linear speed of the robot, x is the current driving mileage of the robot, and acc _max is a constant.

The angular velocity control amount calculating method is as follows:

Wherein w _output (k) represents the angular velocity of the inspection robot at the moment k, k _p、k_i、k_d represents the coefficients of the proportional, integral and differential links, dis _diff (k) represents the distance difference between the inspection robot at the moment k and the wall bodies at the two sides, v _cur represents the solid line actual velocity of the inspection robot at the moment k, and v _max represents the maximum line velocity which the inspection robot can achieve.

Compared with the prior art, the invention has the advantages that:

1) In the autonomous navigation process of the inspection robot, the robot can be automatically judged to be driven forwards or backwards according to the initial pose orientation angle and the angle from the final target point path, so that the autonomous navigation efficiency is improved.

2) During the speed space searching stage, the process of accelerating the actual speed to the searching speed through the kinematic performance is considered in the dead reckoning, and the dead reckoning result is more accurate than the traditional dead reckoning result.

3) The in-ditch navigation provides a virtual tracking method, the laser radar is used for fitting the wall surfaces on two sides of the inspection ditch, two straight lines representing the left wall surface and the right wall surface are constructed, the geometric relationship of the two straight lines is kept to be symmetrical, the angular velocity control quantity is calculated through the PID algorithm, the geometric relationship of the two straight lines is kept to be symmetrical, the inspection robot can be guaranteed to run in the inspection ditch fast and accurately, compared with the existing AGV tracking method, the method does not need to lay a track or an electromagnetic tracking line, and the deployment cost of the robot is reduced.

Drawings

FIG. 1 is a flow chart of a method for controlling navigation movement outside a patrol trench.

FIG. 2 is a flow chart of a method for controlling navigation movement in a patrol trench.

Fig. 3 is a functional block diagram of a PID algorithm.

Detailed Description

The invention will be described in further detail with reference to the accompanying drawings and the detailed description.

The invention discloses an autonomous navigation motion control method of a patrol robot, which comprises an outside-patrol-ditch navigation motion control method and an inside-patrol-ditch navigation motion control method.

The inspection robot is used as a system actuator, is provided with a three-dimensional laser radar, an inertial measurement unit and a motor wheel speed meter for space positioning navigation, and realizes positioning and navigation of the inspection robot based on the ROS system. When receiving a new inspection task, the inspection robot starts from a ground charging station and automatically navigates to a designated position to prepare for entering a trench for inspection through a lifting elevator. The method for controlling the navigation movement outside the inspection ditch is shown in fig. 1, and specifically comprises the following steps:

Firstly, acquiring the current course angle of the inspection robot, then searching the most distant point of the distance which can be pre-calculated in the dead reckoning time length in the global path, traversing and accumulating the straight-going angle of the inspection robot to each point, and finally calculating the average value of the angles in the distance, namely the angle which the inspection robot needs to rotate, wherein the calculation formula of the average value of the angles is as follows:

In the embodiment, the highest speed of the hardware limit of the inspection robot is 1.0m/s, the travel distance of the robot is compared with the distance from the robot to each point in the global path to obtain the furthest point which can be calculated in advance, and the straight-through angle from the robot to each point is obtained by a method provided by an ROS built-in software package angles.

Step 3.1, determining a speed sampling space range according to the current pose of the inspection robot, the distance of a target point, the dynamic performance, the hardware performance of the robot, the operation scene of the inspection robot and other factors, wherein the speed sampling space range is expressed as:

V_m＝{v∈[v_min,v_max],w∈[w_min,w_max]}

The speed search space range of the inspection robot is known from step 3.1, and is subdivided into a plurality of groups of speeds, and each group of linear speed v and angular speed w corresponds to a feasible track. The present invention proposes an improved method in which the process of accelerating from the actual speed to the search speed via the kinematic performance is taken into account in dead reckoning, thereby achieving high-precision navigation. According to the dynamic performance, the differential time and acceleration differential concept is introduced, the process of accelerating the robot from the current speed to the search speed is divided into a plurality of differential stages according to time (the differential time and the differential times are determined according to the working requirement of the inspection robot), the inspection robot moves at different speeds in each differential stage until the actual speed of the inspection robot is accelerated to the search speed, and then the inspection robot keeps constant speed movement, so that the distance between the two stages of acceleration and constant speed is considered by the finally deduced flight path, and the navigation precision is improved.

Based on the working scenario of the robot in this embodiment, the test result shows that n=10 and t _sum＝3,t_acc =0.1 have the best effect.

In this embodiment, α=1, and β=1 is preferable.

And 4, rotating the inspection robot body to the rotation tolerance range of the final target point, and ending the navigation outside the inspection ditch.

After the inspection robot enters the inspection trench through the lifting elevator, the inspection robot automatically navigates to a certain fixed position according to the instruction to carry out inspection operation, wherein the control method of navigation movement in the inspection trench is shown in fig. 2 and is specifically as follows:

In order to consider the running stability and the running efficiency, the current running mileage is taken as a basis, the linear speed control quantity is pre-calculated by adopting a trapezoidal acceleration and deceleration algorithm, the current pre-calculated linear speed v and the current angular speed w are issued, and the inspection robot body moves according to the issued linear speed v and the issued angular speed w, so that the inspection robot can be ensured to run in a straight line in the inspection ditch.

the constraints are as follows:

x₁＝x₃-x₂

In the embodiment, the minimum linear speed, the maximum linear speed and the maximum acceleration of a plurality of groups of inspection robots are tested, and the running stability and the navigation efficiency of the inspection robots in the working scene are comprehensively considered, wherein the optimal combination is vel _min＝0.02,vel_max＝0.8,acc_max =0.6.

As shown in fig. 3, the functional block diagram of the PID algorithm is as follows:

The present embodiment tests a plurality of sets of PID coefficients, and the optimal combination of robot operation in this working scenario is k _p＝0.2,k_i＝0,k_d =0.05.

In order to verify the navigation accuracy of the motion control strategy of the vehicle bottom inspection robot, a laser pen is arranged on the side of a vehicle body, an adhesive tape is stuck on the ground, the current pose output by a positioning module of the inspection robot is used as a navigation target point, a navigation instruction is repeatedly issued to the inspection robot, the inspection robot repeatedly goes to the target point from other points, and a ball pen is used for recording a parking point on the adhesive tape. When a plurality of points are recorded, the absolute value of the error is taken and then averaged, and the smaller the value is, the higher the navigation precision is. Recording the navigation outside the inspection trench and the navigation inside the inspection trench, and respectively showing the results in table 1 and table 2.

TABLE 1 results of navigation test outside inspection trench

Sequence number	Target pose (m)	Reaching pose (m)	X error (m)	Y error (m)
					1	(10.0,10.0)	(9.993,9.992)	-0.007	-0.008
2	(10.0,10.0)	(10.008,10.007)	0.008	0.007
					3	(10.0,10.0)	(10.012,10.009)	0.012	0.009
4	(10.0,10.0)	(10.00,10.00)	0.00	0.00
					5	(10.0,10.0)	(10.01,10.007)	0.01	0.007
6	(10.0,10.0)	(9.989,9.99)	-0.011	-0.01
					7	(10.0,10.0)	(10.012,10.008)	0.012	0.008
8	(10.0,10.0)	(9.99,9.992)	-0.01	-0.008

TABLE 2 navigation test results in inspection ditch

Sequence number	Target pose (m)	Reaching pose (m)	X error (m)	Y error (m)
					1	(100.0,0)	(99.993,-0.003)	-0.007	-0.003
2	(100.0,0)	(100.008,0.002)	0.008	0.002
					3	(100.0,0)	(100.001,0.002)	0.01	0.002
4	(100.0,0)	(99.994,0.00)	-0.006	0.00
					5	(100.0,0)	(99.993,0.003)	-0.007	0.003
6	(100.0,0)	(99.004,-0.001)	0.004	-0.001
					7	(100.0,0)	(99.998,0.003)	-0.008	0.003
8	(100.0,0)	(100.01,-0.002)	0.01	-0.002

Table 1 shows the results of the precision test of the x direction and the y direction of the positioning navigation outside the inspection trench, the average error measured by dotting eight times is x direction +/-0.01 m, y direction +/-0.0085 m, and the condition that the robot cannot reach the point does not occur in the eight times of navigation, so that the precision and the reliability of the positioning navigation algorithm outside the trench are verified. Table 2 shows the results of the precision test of the positioning navigation x direction and y direction in the inspection trench, the average error measured by eight times of dotting is x direction plus or minus 0.0075m, y direction plus or minus 0.0025m, and the condition that the robot cannot reach the point does not occur in eight times of navigation, so that the precision and the reliability of the positioning navigation algorithm in the trench are verified.

Claims

1. The autonomous navigation movement control method of the inspection robot is characterized in that an autonomous navigation space of the vehicle bottom inspection robot comprises the outside of an inspection ditch and the inside of the inspection ditch, when a new inspection task is received, the inspection robot starts from a ground charging station and automatically navigates to a designated position to prepare for entering the ditch for inspection through a lifting elevator, and the external navigation movement control method of the inspection ditch comprises the following specific steps:

Step 1, acquiring a global navigation path through Dijkstra algorithm, extracting a current farthest tracking point according to set dead reckoning time length, taking the current farthest tracking point as a target point for current movement of a patrol robot, determining a current initial pose of the robot according to a request positioning data source, and judging whether the robot should be driven forwards or backwards according to an initial pose orientation angle and an angle from a final target point path;

step 2, the inspection robot body rotates to an average angle direction of the path reckoning section distance initial pose angle;

Step 3, predicting a sampling track by utilizing an improved dead reckoning algorithm, selecting a path with highest evaluation as a next running state of the inspection robot by an evaluation function, calculating a current speed instruction of the robot in real time, and performing dead reckoning and speed instruction calculation in a reciprocating manner in such a way, so that the inspection robot can be ensured to accurately and efficiently run to a final target point translation tolerance range;

step 4, the inspection robot body rotates to a final target point rotation tolerance range, and navigation is finished;

step A, obtaining the distance between target points, and pre-calculating the linear speed control quantity of trapezoid acceleration and deceleration;

Step B, fitting the wall surfaces on two sides of the inspection trench by using a laser radar to obtain geometrical characteristics of the wall surfaces, constructing two straight lines representing the left wall surface and the right wall surface according to the geometrical characteristics, and simultaneously obtaining positioning data of the movement distance in the trench;

Step C, dynamically adjusting the linear speed v and the angular speed w of the inspection robot, and keeping the inspection robot to linearly run in the inspection trench;

2. The autonomous navigation motion control method of the inspection robot according to claim 1, wherein the step 2 comprises the steps of firstly obtaining a current course angle of the inspection robot, then searching a distance furthest point which can be pre-calculated in a dead reckoning time in a global path, traversing and accumulating straight angles from the inspection robot to each point, and finally calculating an average value of angles in the distance, namely an angle required to be rotated by the inspection robot, wherein a calculation formula of the average value of the angles is as follows:

3. The autonomous navigation movement control method of the inspection robot according to claim 1, wherein the step 3 specifically comprises:

V_m＝{v∈[v_min,v_max],w∈[w_min,w_max]}

wherein v _min、v_max is the minimum and maximum linear speeds of the inspection robot, and w _min、w_max is the minimum and maximum angular speeds of the inspection robot;

step 3.2, starting an improved dead reckoning algorithm to calculate dead reckoning and speed instructions;

Wherein x (k), y (k), x (k+1), y (k+1) represent position coordinate information of the robot at the kth time and the k+1 time, x _acc、y_acc represents a distance travelled by the acceleration stage, x _const、y_const represents a distance travelled by the constant speed stage, v _begin represents an initial speed of the robot in the dead reckoning, v _end represents a target search speed of the dead reckoning, t _sum represents total time of the dead reckoning, t _acc represents differential time, x _acc、ang_acc represents acceleration differential, ang _begin represents an initial angle of the dead reckoning of the inspection robot, ang _end represents a target search angle of the dead reckoning of the inspection robot, v _offset、ang_offset represents a difference value between the initial speed and the search speed, x _acc、ang_acc represents a speed change amount of the inspection robot in a differential process, and n represents the acceleration times required for accelerating the actual speed to the search speed;

Wherein score represents the score of the current deduction track, alpha represents the weight of the distance from the end point of the deduction track to the tracking point in the calculated score, beta represents the weight of the shortest distance from the deduction track to the global path in the calculated score, dist ₀ represents the distance from the end point of the deduction track to the tracking point, dist1 represents the shortest distance from the deduction track to the global path;

4. The autonomous navigation movement control method of the inspection robot according to claim 1, wherein the step C is specifically:

Calculating the distance difference between the inspection robot and the wall bodies on two sides according to the front radar and the rear radar, taking the symmetrical geometric relationship of two straight lines as a target, and calculating the angular speed control quantity through a PID algorithm;

According to the current driving mileage, a trapezoidal acceleration and deceleration algorithm is adopted to pre-calculate the linear speed control quantity, the current pre-calculated linear speed v and the angular speed w are issued, and the inspection robot body moves according to the issued linear speed v and the issued angular speed w, so that the inspection robot can be ensured to linearly drive in the inspection ditch;

the constraints are as follows:

x₁＝x₃-x₂

Wherein x is more than or equal to 0 and less than or equal to x ₁, x ₁≤x≤x₂, x ₂≤x≤x₃, x ₃, max (a, b) and vel _min are respectively the maximum value of a and b, and vel _max is the maximum linear speed of the robot, x is the current driving mileage of the robot, and acc _max is a constant;

the angular velocity control amount calculating method is as follows: