Unmanned vehicle autonomous navigation deviation rectifying method
Technical Field
The invention belongs to the technical field of automatic navigation, and particularly relates to an autonomous navigation deviation rectifying method for an unmanned vehicle.
Background
With the continuous advance of internet technology, intelligent robot technology has been developed sufficiently. The automatic navigation unmanned vehicle is mainly used for replacing human beings to carry out work with strong repeatability or large labor capacity in the fields of industry, logistics, storage and the like. For example, loading and unloading, heavy object carrying in industrial production, and intelligent storage automatic transfer in logistics industry are widely applied to automatic navigation unmanned vehicles. Although the application scenes are different, the automatic navigation unmanned vehicle has higher requirements on the performances of accuracy, rapidity, continuity and the like of the automatic navigation unmanned vehicle motion.
At present, for the autonomous navigation design of a robot, a drawing is built mainly in a manual control mode, the drawing building precision cannot meet the requirement of an industrial level, and meanwhile, an existing autonomous robot system is weak in deviation rectification capacity of a traveling path under an unknown environment and poor in anti-collision effect.
The traditional deviation rectifying method for the automatic navigation unmanned vehicle adopts PID control, but the method has long parameter adjusting time and poor anti-interference capability, and the length of the adjusting distance cannot be determined during the deviation rectifying of the motion, so that the automatic navigation unmanned vehicle can not achieve the ideal effect.
Aiming at the problems that the automatic navigation robot in the prior art has poor navigation deviation adjustment capability and cannot accurately judge road condition information, an effective solution is not provided at present.
Disclosure of Invention
The invention aims to solve the technical problem of the prior art and provides an autonomous navigation deviation rectifying method for an unmanned vehicle, which judges road condition information of a curve, a crossroad and the curve and the crossroad under a normal road respectively by reading information of a gyroscope arranged at the center of the unmanned vehicle and information of two laser distance meters respectively arranged at the left side and the right side of the unmanned vehicle, calculates a transverse offset and a course offset and sends the transverse offset and the course offset to a main control unit, and the main control unit rectifies a planned path of the unmanned vehicle in real time according to the judgment information. The invention can effectively judge the road condition information in the automatic navigation process and greatly improve the accuracy and stability of the navigation of the unmanned vehicle.
In order to achieve the technical purpose, the technical scheme adopted by the invention is as follows:
an unmanned vehicle autonomous navigation deviation rectifying method comprises the following steps:
step S1: establishing a vehicle body coordinate system by using a self coordinate system of the navigation unmanned vehicle, wherein the center of the navigation unmanned vehicle is taken as an original point, the advancing direction of the navigation unmanned vehicle is the X direction, the left and right directions are the Y direction, and the up and down directions are the Z direction;
step S2: installing a gyroscope and a laser range finder, and acquiring the information of the navigation unmanned parking position in real time through the gyroscope and the laser range finder;
step S3: judging road condition information and calculating a transverse offset DP and a course offset AP according to data acquired by a gyroscope and a laser range finder in real time;
step S4: the main control unit judges whether the unmanned navigation vehicle needs to be corrected or not according to the road condition information and the offset, if the unmanned navigation vehicle needs to be corrected, further calculates a deviation adjusting linear speed Vt and a deviation adjusting angular speed Wt which are needed for correction, obtains a target linear speed Vx and a target angular speed W by combining the navigation angular speed Wd and the navigation line speed Vd, and enters step S5;
step S5: issuing the target linear velocity Vx and the target angular velocity W to a velocity topic through a mobile control node;
step S6: the number of pulses required by the left driving wheel and the right driving wheel is respectively calculated by the navigation unmanned vehicle according to the speed on the speed topic, and the autonomous navigation deviation correction of the navigation unmanned vehicle is carried out.
In order to optimize the technical scheme, the specific measures adopted further comprise:
the driving structure of the navigation unmanned vehicle is a two-wheel differential structure:
the linear speeds of the left driving wheel and the right driving wheel are respectively as follows:
the rotating speed of the right driving wheel is set,
the left driving wheel rotation speed, r is the wheel radius;
the navigation angular velocity and the navigation route velocity of the navigation unmanned vehicle are respectively as follows:
Vd=(Vr+Vl)/2L (10)
Wd=(Vr-Vl)/2L (11)
and L is the distance from the center of the navigation unmanned vehicle to two wheels.
The step S2 of installing the gyroscope and the laser range finder specifically includes:
the gyroscope is arranged in the front center of the navigation unmanned vehicle, and the laser range finders are respectively arranged at the right front position, the right rear position, the left front position and the left rear position of the navigation unmanned vehicle;
the gyroscope is used for measuring the three-axis angular speed of the navigation unmanned vehicle;
the laser range finder is used for measuring the distance between the navigation unmanned vehicle and the wall bodies on the two sides.
In step S3, the method for determining the turning state is as follows:
when the distance measured by the laser distance meter at the front of the inner side of the turn of the navigation unmanned vehicle is reduced, the distance measured by the laser distance meter at the rear is increased, and meanwhile, the angular velocity of the Z axis measured by the gyroscope is continuously increased, the navigation unmanned vehicle is judged to turn, and the road condition is the turn.
In step S3, the method for determining the intersection condition is as follows:
when the distance measured by the laser distance measuring instruments in front of the two sides is infinite, the distance measured by the laser distance measuring instruments in the rear is a certain value, and the rotating speed of the Z axis measured by the gyroscope is a certain value, the road condition is judged to be the crossroad.
In the step S3, the method for determining the turning under the road condition at the intersection is as follows:
when the distance measured by the laser distance meter at the front of the inner side of the navigation unmanned vehicle during turning is reduced, the distance measured by the laser distance meter at the rear is increased, the distance measured by the laser distance meter at the outer side is gradually reduced from infinity to a certain value, and meanwhile, the rotating speed of the Z axis measured by the gyroscope is continuously increased, the navigation unmanned vehicle is judged to be turning under the road condition of the crossroad.
In the step S3, the lateral offset DP is obtained by subtracting the target distance from the difference between the distances measured by the two laser range finders on the same side;
the course offset AP is obtained through an arctangent value obtained by dividing the difference value of the distances measured by the two laser range finders on the same side by the distance of the laser range finder on the same side.
In the step S4, the calculated offset DP is compared with the initially set distance standard Dm, and whether deviation correction is required is determined according to the traffic information:
if the transverse offset DP is equal to the distance standard value Dm and the road condition information judges that the road condition is a non-crossroad section, judging that the navigation unmanned vehicle linearly advances under the non-crossroad condition without correcting the position in the Y direction, otherwise, correcting the position in the Y direction;
if the transverse offset DP is larger than or smaller than the distance standard value Dm and the road condition information judges that the road condition is a crossroad section, judging that the navigation unmanned vehicle linearly advances under the crossroad section without correcting the deviation in the Y direction;
if the transverse offset DP is greater than or less than the distance standard value Dm and the road condition information judges that the road condition is a non-crossroad section, judging that the navigation unmanned vehicle does not go forward straight and needs to be rectified in the Y direction;
in the step S4, the calculated course offset AP is compared with the initially set course standard value Am, and meanwhile, it is determined whether deviation correction is required according to the road condition information:
if the course offset AP is smaller than or equal to the course standard value Am and the road condition information judges that the road condition is a non-turning road section, judging that the unmanned navigation vehicle does not yaw and does not need to adjust the offset in the Z direction, otherwise, adjusting the offset in the Z direction;
if the course offset AP is larger than the course standard value Am and the road condition information judges that the road condition is a turning road section, judging that the unmanned navigation vehicle does not yaw and does not need to adjust the offset in the Z direction;
and if the course offset AP is larger than the course standard value Am and the road condition information judges that the road condition is a non-turning road section, judging that the unmanned navigation vehicle has drifted and needs to be rectified in the Z direction.
In the step S4, the calculated lateral offset DP is compared with the initially set distance criterion Dm, the heading offset AP is compared with the initially set heading criterion Am, and meanwhile, according to the road condition information, it is determined whether deviation correction is required:
if the lateral offset DP is greater than or less than the distance standard value Dm, the heading offset AP is greater than the heading standard value Am, and meanwhile, the road condition information judges that the road condition is a turning road section under the crossroad, the navigation unmanned vehicle is judged to turn under the crossroad without adjusting the offset in the direction of Y, Z.
In the above step S4, the linear yaw rate Vt, the yaw angular rate Wt, the target linear rate Vx, and the target angular rate W are calculated as follows:
when the offset is adjusted in the Y direction:
Wt=Π/4NΔt (3)
ΔD=DP-Dm (4)
Vt=ΔD/Δt (5)
Vx=Vd+Vt (12)
W=Wd+Wt (13)
when the deviation is adjusted in the Z direction:
ΔA=AP-Am (6)
Wt’=ΔA/Δt’ (7)
Vx=Vd (14)
W=Wd+Wt’ (15)
n is the number of times of acquiring the laser distance meter data by the offset adjustment, delta t is the time difference of acquiring the laser distance meter data twice adjacently, and Dm is a distance standard value which is initially set;
and delta A is a course deviation value, Am is an initially set course standard value, and delta t' is a time difference between two adjacent acquired gyroscope data.
The invention has the following beneficial effects:
1. according to the invention, the road condition information at the moment can be accurately judged through the data of the two sensors, namely the gyroscope and the laser range finder, so that the accuracy of navigation deviation correction is improved.
2. The control method provided by the invention can not only comprehensively adjust the distance and the direction deviation, but also ensure that the adjustment is completed within the specified distance, greatly save the time for adjusting the parameters by the PID adjustment method, and provide an accurate, efficient and reliable control method for the deviation adjustment of the automatic navigation unmanned vehicle.
Drawings
FIG. 1 is a block diagram of the steps of the method of the present invention;
FIG. 2 is a schematic diagram of the hardware and coordinate system of the navigation unmanned vehicle according to the present invention;
fig. 3 is a flow chart of traffic information analysis according to the present invention.
Detailed Description
Embodiments of the present invention are described in further detail below with reference to the accompanying drawings.
As shown in fig. 1, an autonomous navigation deviation rectifying method for an unmanned vehicle includes:
step S1: establishing a vehicle body coordinate system by using a self coordinate system of the navigation unmanned vehicle, wherein the center of the navigation unmanned vehicle is taken as an original point, the advancing direction of the navigation unmanned vehicle is the X direction, the left and right directions are the Y direction, and the up and down directions are the Z direction;
step S2: and installing a gyroscope and a laser range finder, and acquiring the position and pose information of the unmanned navigation vehicle in real time through the gyroscope and the laser range finder.
Step S3: judging road condition information and calculating a transverse offset DP and a course offset AP according to data acquired by a gyroscope and a laser range finder in real time;
step S4: the main control unit judges whether the unmanned navigation vehicle needs to be corrected or not according to the road condition information and the offset, if the unmanned navigation vehicle needs to be corrected, the main control unit further calculates the deviation adjusting linear velocity Vt and the deviation adjusting angular velocity Wt which are needed by the correction, and the step S5 is carried out;
step S5: issuing the deviation correcting linear velocity Vx and the deviation correcting angular velocity W to a velocity topic through a mobile control node;
step S6: the number of pulses required by the left driving wheel and the right driving wheel is respectively calculated by the navigation unmanned vehicle according to the speed on the speed topic, and the autonomous navigation deviation correction of the navigation unmanned vehicle is carried out.
In an embodiment, the step S2 of installing the gyroscope and the laser range finder specifically includes:
as shown in fig. 2, the gyroscope is installed in the front center of the unmanned navigation vehicle, and the laser range finders are respectively installed at the front right position, the rear right position, the front left position and the rear left position of the unmanned navigation vehicle.
The gyroscope is used for measuring the three-axis angular speed of the navigation unmanned vehicle;
the laser range finder is used for measuring the distance between the navigation unmanned vehicle and the wall bodies on the two sides.
As shown in fig. 3, step S3 analyzes the data of the gyroscope and the laser range finder, so as to determine the road condition information of the unmanned vehicle.
In step S3, the method for determining the turning condition is as follows:
when the distance measured by the laser distance meter at the front of the inner side of the turn of the navigation unmanned vehicle is reduced, the distance measured by the laser distance meter at the rear is increased, and meanwhile, the angular velocity of the Z axis measured by the gyroscope is continuously increased, the navigation unmanned vehicle is judged to turn, and the road condition is the turn.
In step S3, the method for determining the intersection condition is as follows:
when the distance measured by the laser distance measuring instruments in front of the two sides is infinite, the distance measured by the laser distance measuring instruments in the rear is a certain value, and the rotating speed of the Z axis measured by the gyroscope is a certain value, the road condition is judged to be the crossroad.
In step S3, the method for determining a turn under the intersection road condition is as follows:
when the distance measured by the laser distance meter at the front of the inner side of the navigation unmanned vehicle during turning is reduced, the distance measured by the laser distance meter at the rear is increased, the distance measured by the laser distance meter at the outer side is gradually reduced from infinity to a certain value, and meanwhile, the rotating speed of the Z axis measured by the gyroscope is continuously increased, the navigation unmanned vehicle is judged to be turning under the road condition of the crossroad.
In an embodiment, in the step S3, the lateral offset DP is obtained by subtracting a difference between distances measured by two laser range finders on the same side and a target distance;
the lateral shift amount DP is calculated as follows:
DP=|C1-C2| (1)
in the formula (1), C1 and C2 are data measured by the same-side laser range finder.
The course offset AP is obtained through an arctangent value obtained by dividing the difference value of the distances measured by the two laser range finders on the same side by the distance of the laser range finder on the same side.
The calculation formula of the heading offset AP is as follows:
AP=arctan(|C1-C2|/pdist) (2)
in the formula (2), C1 and C2 represent data measured by the ipsilateral laser rangefinder, and pdist represents the distance between the ipsilateral laser rangefinder.
In step S4, the calculated lateral offset DP is compared with the initially set distance criterion Dm, and whether deviation correction is required is determined according to the road condition information.
Here, the distance criterion value Dm is given as 10 mm;
if the transverse offset DP is equal to the distance standard value Dm and the road condition information judges that the road condition is a non-crossroad section, judging that the navigation unmanned vehicle linearly advances under the non-crossroad condition without correcting the deviation in the Y direction;
if the transverse offset DP is larger than or smaller than the distance standard value Dm and the road condition information judges that the road condition is a crossroad section, judging that the navigation unmanned vehicle linearly advances under the crossroad section without correcting the deviation in the Y direction;
if the lateral offset DP is greater than or less than the distance standard value Dm, and the road condition information judges that the road condition is a non-crossroad section, the navigation unmanned vehicle is judged not to go straight ahead, and the lateral offset needs to be subjected to drift zero adjustment in the Y direction until the number of the lateral offsets of the measured data of 100 groups of laser range finders is equal to zero is greater than or equal to 50 groups.
When the deviation correction is carried out in the Y direction, the unmanned vehicle is given to carry out deviation correction in the direction inclined by 45 degrees, and the deviation adjusting angular velocity Wt at the moment is as follows:
Wt=Π/4NΔt (3)
and N is the number of times of acquiring the data of the laser range finder by the offset adjustment.
The calculation formula of the deviation adjusting linear velocity Vt required by deviation rectification is as follows:
ΔD=DP-Dm (4)
Vt=ΔD/Δt (5)
and delta D is the transverse deviation value, and delta t is the time difference between two adjacent times of collecting the data of the laser range finder.
Comparing the calculated course offset AP with an initially set course standard value Am, and judging whether deviation correction is needed according to road condition information;
here, the heading standard value Am is given as 5 °.
If the course offset AP is smaller than or equal to the course standard value Am and the road condition information judges that the road condition is a non-turning road section, judging that the unmanned vehicle for navigation does not yaw and does not need to adjust the offset in the Z direction;
if the course offset AP is larger than the course standard value Am and the road condition information judges that the road condition is a turning road section, judging that the unmanned navigation vehicle does not yaw and does not need to adjust the offset in the Z direction;
if the course offset AP is larger than the course standard value Am and the road condition information judges that the road condition is a non-turning road section, the navigation unmanned vehicle is judged to have drifted, and the course offset needs to be subjected to drift zero adjustment in the Z direction until the transverse offset of the data of the gyroscope returns to zero.
The calculation formula of the deviation correcting angular velocity Wt in the Z direction required by deviation correction is as follows:
ΔA=AP-Am (6)
Wt’=ΔA/Δt’ (7)
and delta A is a course deviation amount, delta t 'is a time difference between two adjacent gyroscope data acquisition processes, and Wt' is a deviation rectifying angular speed in the Z direction.
And comparing the calculated transverse offset DP with an initially set distance standard value Dm, and judging whether the deviation needs to be corrected according to road condition information, wherein the course offset AP and the initially set course standard value Am are obtained.
If the lateral offset DP is greater than or less than the distance standard value Dm, the heading offset AP is greater than the heading standard value Am, and meanwhile, the road condition information judges that the road condition is a turning road section under the crossroad, the navigation unmanned vehicle is judged to turn under the crossroad without adjusting the direction of Y, Z.
In an embodiment, the driving structure of the navigation unmanned vehicle is a two-wheel differential structure:
the linear speeds of the left driving wheel and the right driving wheel are respectively as follows:
the rotating speed of the right driving wheel is set,
the left driving wheel rotation speed, r is the wheel radius;
the navigation angular velocity and the navigation route velocity of the navigation unmanned vehicle are respectively as follows:
Vd=(Vr+Vl)/2L (10)
Wd=(Vr-Vl)/2L (11)
and L is the distance from the center of the navigation unmanned vehicle to two wheels.
After the autonomous deviation adjustment in the Y direction, the final target linear velocity and target angular velocity of the navigation unmanned vehicle are respectively as follows:
Vx=Vd+Vt (12)
W=Wd+Wt (13)
after the autonomous deviation adjustment in the Z direction, the final target linear velocity and target angular velocity of the navigation unmanned vehicle are respectively as follows:
Vx=Vd (14)
W=Wd+Wt’ (15)
according to the invention, the road condition information at the moment can be accurately judged through the data of the two sensors, namely the gyroscope and the laser range finder, so that the accuracy of navigation deviation correction is improved. The control method provided by the invention can not only comprehensively adjust the distance and the direction deviation, but also ensure that the adjustment is completed within the specified distance, greatly save the time for adjusting the parameters by the PID adjustment method, and provide an accurate, efficient and reliable control method for the deviation adjustment of the automatic navigation unmanned vehicle.
The above is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above-mentioned embodiments, and all technical solutions belonging to the idea of the present invention belong to the protection scope of the present invention. It should be noted that modifications and embellishments within the scope of the invention may be made by those skilled in the art without departing from the principle of the invention.