[go: up one dir, main page]

CN114115275A - An autonomous navigation correction method for unmanned vehicles - Google Patents

An autonomous navigation correction method for unmanned vehicles Download PDF

Info

Publication number
CN114115275A
CN114115275A CN202111421080.4A CN202111421080A CN114115275A CN 114115275 A CN114115275 A CN 114115275A CN 202111421080 A CN202111421080 A CN 202111421080A CN 114115275 A CN114115275 A CN 114115275A
Authority
CN
China
Prior art keywords
unmanned vehicle
navigation
road condition
offset
distance
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202111421080.4A
Other languages
Chinese (zh)
Other versions
CN114115275B (en
Inventor
刘义亭
随连杰
路红
张磊
李佩娟
郁汉琪
郭婧
高芳征
赵贤林
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing Institute of Technology
Original Assignee
Nanjing Institute of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing Institute of Technology filed Critical Nanjing Institute of Technology
Priority to CN202111421080.4A priority Critical patent/CN114115275B/en
Publication of CN114115275A publication Critical patent/CN114115275A/en
Application granted granted Critical
Publication of CN114115275B publication Critical patent/CN114115275B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种无人车自主导航纠偏方法,包括通过读取放置于导航无人车中心陀螺仪的信息,以及读取放置在导航无人车左右两边各两个激光测距仪的信息,分别对弯道,十字路口,以及正常道路下的弯道和十字路口的路况信息做出判断,并计算出横向偏移量和航向偏移量发送给主控单元,主控单元根据判断信息实时对导航无人车规划的路径进行纠偏。本发明可以有效的对自动导航过程中的路况信息进行判断,大大提高的导航无人车导航的准确性和稳定性。

Figure 202111421080

The invention discloses a deviation correction method for autonomous navigation of an unmanned vehicle. , respectively make judgments on the road condition information of curves, intersections, and curves and intersections under normal roads, and calculate the lateral offset and heading offset and send them to the main control unit. The main control unit judges the information according to the information. Correct the path planned by the navigation unmanned vehicle in real time. The present invention can effectively judge the road condition information in the automatic navigation process, and greatly improves the navigation accuracy and stability of the navigation unmanned vehicle.

Figure 202111421080

Description

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:
Figure BDA0003376674820000021
Figure BDA0003376674820000022
Figure BDA0003376674820000023
the rotating speed of the right driving wheel is set,
Figure BDA0003376674820000024
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:
Figure BDA0003376674820000071
Figure BDA0003376674820000072
Figure BDA0003376674820000073
the rotating speed of the right driving wheel is set,
Figure BDA0003376674820000074
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.

Claims (10)

1.一种无人车自主导航纠偏方法,其特征在于,包括:1. an unmanned vehicle autonomous navigation correction method, is characterized in that, comprises: 步骤S1:以导航无人车自身坐标系建立车身坐标系,导航无人车中心为原点,导航无人车前进方向为X方向,左右方向为Y方向,上下方向为Z方向;Step S1: establishing a body coordinate system based on the coordinate system of the navigation unmanned vehicle, the center of the navigation unmanned vehicle as the origin, the forward direction of the navigation unmanned vehicle as the X direction, the left and right direction as the Y direction, and the up and down direction as the Z direction; 步骤S2:安装陀螺仪和激光测距仪,通过陀螺仪和激光测距仪实时采集导航无人车位姿信息;Step S2: install a gyroscope and a laser range finder, and collect the position and attitude information of the navigation unmanned vehicle in real time through the gyroscope and the laser range finder; 步骤S3:根据陀螺仪和激光测距仪实时采集的数据,判断路况信息并计算横向偏移量DP和航向偏移量AP;Step S3: According to the real-time data collected by the gyroscope and the laser rangefinder, determine the road condition information and calculate the lateral offset DP and the heading offset AP; 步骤S4:主控单元根据路况信息和偏移量判断无人导航无人车是否需要进行纠偏,如需纠偏,则进一步计算纠偏所需要的调偏线速度Vt和调偏角速度Wt,结合导航角速度Wd和导航线速度Vd,得到目标线速度Vx和目标角速度W,进入步骤S5;Step S4: The main control unit determines whether the unmanned navigation unmanned vehicle needs to be corrected according to the road condition information and the offset. If correction is required, the linear speed Vt and the angular speed Wt required for the correction are further calculated, combined with the navigation Angular velocity Wd and navigation linear velocity Vd, obtain target linear velocity Vx and target angular velocity W, enter step S5; 步骤S5:通过移动控制节点将目标线速度Vx和目标角速度W发布至速度话题上;Step S5: publish the target linear velocity Vx and the target angular velocity W to the velocity topic through the mobile control node; 步骤S6:导航无人车根据速度话题上的速度,分别计算出左驱动轮和右驱动轮所需要的脉冲个数,进行导航无人车自主导航纠偏。Step S6: The navigation unmanned vehicle calculates the number of pulses required by the left driving wheel and the right driving wheel respectively according to the speed on the speed topic, and performs the autonomous navigation correction of the navigation unmanned vehicle. 2.根据权利要求1所述的一种无人车自主导航纠偏方法,其特征在于,所述导航无人车的驱动结构为两轮差速结构:2. The method for autonomous navigation and deviation correction of an unmanned vehicle according to claim 1, wherein the driving structure of the navigation unmanned vehicle is a two-wheel differential structure: 左右驱动轮线速度分别为:The linear speeds of the left and right drive wheels are:
Figure FDA0003376674810000011
Figure FDA0003376674810000011
Figure FDA0003376674810000012
Figure FDA0003376674810000012
Figure FDA0003376674810000013
为右驱动轮转速,
Figure FDA0003376674810000014
为左驱动轮转速,r为轮子半径;
Figure FDA0003376674810000013
is the rotational speed of the right drive wheel,
Figure FDA0003376674810000014
is the rotational speed of the left drive wheel, and r is the radius of the wheel;
则导航无人车的导航角速度和导航线速度分别为:Then the navigation angular velocity and navigation linear velocity of the navigation unmanned vehicle are: Vd=(Vr+Vl)/2L (10)Vd=(Vr+Vl)/2L (10) Wd=(Vr-Vl)/2L (11)Wd=(Vr-Vl)/2L (11) L为导航无人车中心到两轮的距离。L is the distance from the center of the navigation unmanned vehicle to the two wheels.
3.根据权利要求1所述的一种无人车自主导航纠偏方法,其特征在于,步骤S2所述安装陀螺仪和激光测距仪,具体为:3. a kind of unmanned vehicle autonomous navigation correction method according to claim 1, is characterized in that, described in step S2 to install gyroscope and laser rangefinder, specifically: 陀螺仪安装在导航无人车的前方中心,激光测距仪分别安装在导航无人车的右前、右后、左前、左后位置;The gyroscope is installed in the front center of the navigation unmanned vehicle, and the laser rangefinder is installed in the right front, right rear, left front and left rear positions of the navigation unmanned vehicle; 所述陀螺仪用于测量导航无人车的三轴角速度;The gyroscope is used to measure the three-axis angular velocity of the navigation unmanned vehicle; 所述激光测距仪用于测量导航无人车与两侧墙体的距离。The laser rangefinder is used to measure the distance between the navigation unmanned vehicle and the walls on both sides. 4.根据权利要求1所述的一种无人车自主导航纠偏方法,其特征在于,所述步骤S3中,对转弯情况的判断方法如下:4. a kind of unmanned vehicle autonomous navigation correction method according to claim 1, is characterized in that, in described step S3, the judgment method to turning situation is as follows: 当导航无人车转弯内侧前方的激光测距仪所测距离在减小,后方激光测距仪所测距离在增大,同时陀螺仪所测Z轴转角速率在不断增大,则判断该导航无人车正在进行转弯,路况为转弯。When the distance measured by the laser rangefinder in front of the turning inside of the navigation unmanned vehicle is decreasing, the distance measured by the rear laser rangefinder is increasing, and the Z-axis angle rate measured by the gyroscope is continuously increasing, it is judged that the navigation The unmanned vehicle is turning, and the road condition is turning. 5.根据权利要求1所述的一种无人车自主导航纠偏方法,其特征在于,所述步骤S3中,对十字路口情况的判断方法如下:5. A kind of unmanned vehicle autonomous navigation correction method according to claim 1, is characterized in that, in described step S3, the judging method to the intersection situation is as follows: 当两侧前方的激光测距仪所测距离为无限大,后方的激光测距仪所测距离为一定值,同时陀螺仪所测Z轴转角速率也为一定值,则判断该路况为十字路口。When the distance measured by the laser rangefinders in front of both sides is infinite, the distance measured by the laser rangefinders at the back is a certain value, and the Z-axis angular rate measured by the gyroscope is also a certain value, then the road condition is judged to be an intersection. . 6.根据权利要求1所述的一种无人车自主导航纠偏方法,其特征在于,所述步骤S3中,对十字路口路况下的转弯的判断方法如下:6. The method for autonomous navigation and deviation correction of an unmanned vehicle according to claim 1, wherein, in the step S3, the method for judging a turn under road conditions at a crossroad is as follows: 当导航无人车转弯内侧前方的激光测距仪所测距离在减小,后方激光测距仪所测距离在增大,外侧激光测距仪所测距离由无限大逐渐缩小为一定值,同时陀螺仪所测Z轴转角速率在不断增大,则判断该导航无人车正在进行十字路口路况下的转弯操作。When the navigation unmanned vehicle turns on the inside, the distance measured by the laser rangefinder in front of the vehicle is decreasing, the distance measured by the rear laser rangefinder is increasing, and the distance measured by the outer laser rangefinder is gradually reduced from infinity to a certain value. If the angular rate of the Z-axis measured by the gyroscope is increasing continuously, it is judged that the navigation unmanned vehicle is performing a turning operation under the road conditions at the intersection. 7.根据权利要求1所述的一种无人车自主导航纠偏方法,其特征在于,所述步骤S3中,所述横向偏移量DP通过同侧两个激光测距仪所测距离的差值与目标距离做差所得到;7 . The method for autonomous navigation and deviation correction of an unmanned vehicle according to claim 1 , wherein, in the step S3, the lateral offset DP is determined by the difference between the distances measured by the two laser rangefinders on the same side. 8 . The difference between the value and the target distance is obtained; 所述航向偏移量AP通过同侧两个激光测距仪所测距离的差值与同侧激光测距仪的距离相除后的反正切值得到。The heading offset AP is obtained by dividing the difference between the distances measured by the two laser rangefinders on the same side and the arctangent of the distance from the laser rangefinder on the same side. 8.根据权利要求7所述的一种无人车自主导航纠偏方法,其特征在于,所述步骤S4中,将计算所得到的偏移量DP与初始设定的距离标准值Dm相比较,同时根据路况信息对是否需要纠偏做出判断:8. The method for autonomous navigation and deviation correction of an unmanned vehicle according to claim 7, wherein in the step S4, the calculated offset DP is compared with the initially set distance standard value Dm, At the same time, according to the road condition information, it is necessary to judge whether it needs to be corrected: 若横向偏移量DP等于距离标准值Dm,同时路况信息判断该路况为非十字路口路段,则判定该导航无人车在非十字路口路况下进行直线前进,不需要在Y方向上进行纠偏,否则需要在Y方向上进行纠偏;If the lateral offset DP is equal to the distance standard value Dm, and the road condition information determines that the road condition is a non-intersection road section, then it is determined that the navigation unmanned vehicle is moving in a straight line under non-intersection road conditions, and does not need to be corrected in the Y direction. Otherwise, it needs to be corrected in the Y direction; 若横向偏移量DP大于或小于距离标准值Dm,同时路况信息判断该路况为十字路口路段,则判定该导航无人车在十字路口路段下进行直线前进,不需要在Y方向上进行纠偏;If the lateral offset DP is greater than or less than the distance standard value Dm, and the road condition information determines that the road condition is an intersection road section, it is determined that the navigation unmanned vehicle is going straight under the intersection road section, and does not need to be corrected in the Y direction; 若横向偏移量DP大于或小于距离标准值Dm,同时路况信息判断该路况为非十字路口路段,则判定该导航无人车未进行直线前进,需要在Y方向上进行纠偏;If the lateral offset DP is greater than or less than the distance standard value Dm, and the road condition information determines that the road condition is a non-intersection road section, it is determined that the navigation unmanned vehicle does not proceed in a straight line and needs to be corrected in the Y direction; 所述步骤S4中,将计算所得到的航向偏移量AP与初始设定的航向标准值Am进行比较,同时根据路况信息对是否需要纠偏做出判断:In the step S4, the calculated heading offset AP is compared with the initially set heading standard value Am, and at the same time, according to the road condition information, it is judged whether it is necessary to correct the deviation: 若航向偏移量AP小于或等于航向标准值Am,同时路况信息判断该路况为非转弯路段,则判定该导航无人车未偏航,不需要进行Z方向上的调偏,否则需要进行Z方向上的调偏;If the heading offset AP is less than or equal to the heading standard value Am, and the road condition information determines that the road condition is a non-turning road section, it is determined that the navigation unmanned vehicle is not yawed, and no adjustment in the Z direction is required, otherwise it is necessary to carry out Z deviation in direction; 若航向偏移量AP大于航向标准值Am,同时路况信息判断该路况为转弯路段,则判定该导航无人车未偏航,不需要进行Z方向上的调偏;If the heading offset AP is greater than the heading standard value Am, and the road condition information determines that the road condition is a turning road section, it is determined that the navigation unmanned vehicle is not yawed, and no yaw adjustment in the Z direction is required; 若航向偏移量AP大于航向标准值Am,同时路况信息判断该路况为非转弯路段,则判定该导航无人车已偏航,需要在Z方向上进行纠偏。If the heading offset AP is greater than the heading standard value Am, and the road condition information determines that the road condition is a non-turning road section, it is determined that the navigation unmanned vehicle has yawed and needs to be corrected in the Z direction. 9.根据权利要求7所述的一种无人车自主导航纠偏方法,其特征在于,所述步骤S4中,将计算所得到的横向偏移量DP与初始设定的距离标准值Dm相比较,航向偏移量AP与初始设定的航向标准值Am进行比较,同时根据路况信息对是否需要纠偏做出判断:9 . The method for autonomous navigation and deviation correction of an unmanned vehicle according to claim 7 , wherein in the step S4 , the calculated lateral offset DP is compared with the initially set distance standard value Dm 9 . , The heading offset AP is compared with the initial set heading standard value Am, and at the same time, it is judged whether it needs to be corrected according to the road condition information: 若横向偏移量DP大于或小于距离标准值Dm,航向偏移量AP大于航向标准值Am,同时路况信息判断该路况为十字路口下的转弯路段,则判定该导航无人车在进行十字路口下的转弯动作,不需要在Y、Z方向上进行调偏。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 the road condition information determines that the road condition is a turning section under the intersection, then it is determined that the navigation unmanned vehicle is at the intersection. It is not necessary to adjust the deflection in the Y and Z directions for the turning action in the lower direction. 10.根据权利要求2所述的一种无人车自主导航纠偏方法,其特征在于,所述步骤S4中,调偏线速度Vt、调偏角速度Wt、目标线速度Vx和目标角速度W的计算方式如下:10. The method for autonomous navigation and deviation correction of an unmanned vehicle according to claim 2, wherein in the step S4, the deviation of the deviation adjustment linear velocity Vt, the deviation adjustment angular velocity Wt, the target linear velocity Vx and the target angular velocity W are determined. It is calculated as follows: Y方向上调偏时:When the Y direction is adjusted upwards: Wt=Π/4NΔt (3)Wt=Π/4NΔt (3) ΔD=DP-Dm (4)ΔD=DP-Dm (4) Vt=ΔD/Δt (5)Vt=ΔD/Δt (5) Vx=Vd+Vt (12)Vx=Vd+Vt (12) W=Wd+Wt (13)W=Wd+Wt (13) Z方向上调偏时:When the Z direction is adjusted upwards: ΔA=AP-Am (6)ΔA=AP-Am (6) Wt’=ΔA/Δt’ (7)Wt’=ΔA/Δt’ (7) Vx=Vd (14)Vx=Vd (14) W=Wd+Wt’ (15)W=Wd+Wt’ (15) N为此次调偏采集激光测距仪数据的次数,Δt为相邻两次采集激光测距仪数据的时间差,Dm为初始设定的距离标准值;N is the number of times the laser rangefinder data is collected in this polarization adjustment, Δt is the time difference between two adjacent laser rangefinder data acquisitions, and Dm is the initial set distance standard value; ΔA为航向偏差量,Am为初始设定的航向标准值,Δt’为相邻两次采集陀螺仪数据的时间差。ΔA is the heading deviation, Am is the initial set heading standard value, and Δt' is the time difference between two adjacent gyroscope data acquisitions.
CN202111421080.4A 2021-11-26 2021-11-26 Unmanned vehicle autonomous navigation correction method Active CN114115275B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111421080.4A CN114115275B (en) 2021-11-26 2021-11-26 Unmanned vehicle autonomous navigation correction method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111421080.4A CN114115275B (en) 2021-11-26 2021-11-26 Unmanned vehicle autonomous navigation correction method

Publications (2)

Publication Number Publication Date
CN114115275A true CN114115275A (en) 2022-03-01
CN114115275B CN114115275B (en) 2024-07-12

Family

ID=80369982

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111421080.4A Active CN114115275B (en) 2021-11-26 2021-11-26 Unmanned vehicle autonomous navigation correction method

Country Status (1)

Country Link
CN (1) CN114115275B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115990880A (en) * 2023-01-04 2023-04-21 佛山市顺德区一拓电气有限公司 Robot course adjustment method, robot, device and computer storage medium

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106959689A (en) * 2017-03-16 2017-07-18 浙江大学 A kind of robot tracking device and method for intersection
JP2017199257A (en) * 2016-04-28 2017-11-02 株式会社豊田自動織機 Autonomous travel vehicle
CN107918391A (en) * 2017-11-17 2018-04-17 上海斐讯数据通信技术有限公司 A kind of Mobile Robotics Navigation method for correcting error and device
CN109887306A (en) * 2019-03-15 2019-06-14 南京工程学院 Traffic intersection control early warning system and control method based on RFID technology
EP3621035A1 (en) * 2018-09-08 2020-03-11 Diehl Defence GmbH & Co. KG Method for guiding a vehicle behind a vehicle in front

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2017199257A (en) * 2016-04-28 2017-11-02 株式会社豊田自動織機 Autonomous travel vehicle
CN106959689A (en) * 2017-03-16 2017-07-18 浙江大学 A kind of robot tracking device and method for intersection
CN107918391A (en) * 2017-11-17 2018-04-17 上海斐讯数据通信技术有限公司 A kind of Mobile Robotics Navigation method for correcting error and device
EP3621035A1 (en) * 2018-09-08 2020-03-11 Diehl Defence GmbH & Co. KG Method for guiding a vehicle behind a vehicle in front
CN109887306A (en) * 2019-03-15 2019-06-14 南京工程学院 Traffic intersection control early warning system and control method based on RFID technology

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
徐德生;: "巡线机器人路口教学策略的优化研究", 理科爱好者(教育教学), no. 03, 10 June 2020 (2020-06-10) *
黄德添: "基于KCF算法的地面移动平台目标跟踪", 自动化应用, no. 7, 31 December 2018 (2018-12-31), pages 81 - 83 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115990880A (en) * 2023-01-04 2023-04-21 佛山市顺德区一拓电气有限公司 Robot course adjustment method, robot, device and computer storage medium

Also Published As

Publication number Publication date
CN114115275B (en) 2024-07-12

Similar Documents

Publication Publication Date Title
Su et al. GR-LOAM: LiDAR-based sensor fusion SLAM for ground robots on complex terrain
CN111907516B (en) Full-automatic parking method and system
CN107390691B (en) AGV path tracking method
CN104571112B (en) Pilotless automobile lateral control method based on turning curvature estimation
CN110502009B (en) Unmanned vehicle path tracking control method based on course estimation
CN112014849B (en) Unmanned vehicle positioning correction method based on sensor information fusion
Gehrig et al. Dead reckoning and cartography using stereo vision for an autonomous car
CN113204236A (en) Intelligent agent path tracking control method
CN110307850A (en) Reckoning localization method and automated parking system
CN112129297A (en) Self-adaptive correction indoor positioning method for multi-sensor information fusion
CN112628524B (en) High-precision positioning method for small-diameter pipeline robot based on turning angle
CN111137298B (en) Vehicle automatic driving method, device, system and storage medium
CN112433531A (en) Trajectory tracking method and device for automatic driving vehicle and computer equipment
CN111487976B (en) Backing track tracking method
CN104615889A (en) Intelligent vehicle path tracking method and system based on clothoid following
CN113777589B (en) LIDAR and GPS/IMU combined calibration method based on point characteristics
Śmieszek et al. Application of Kalman filter in navigation process of automated guided vehicles
CN111661048B (en) Multi-articulated vehicle and track following control method and system thereof
CN113064430A (en) Quality inspection trolley obstacle avoidance and path planning algorithm based on Android mobile phone
CN114115275A (en) An autonomous navigation correction method for unmanned vehicles
CN112415516B (en) Method and device for sensing obstacle area in front of vehicle
Zhang et al. Learning end-to-end inertial-wheel odometry for vehicle ego-motion estimation
CN112731932B (en) Path tracking method of mobile robot
CN108709560A (en) Carrying robot high accuracy positioning air navigation aid based on straightway feature
KR20110109606A (en) Vehicle tilt angle measuring method and its measuring device

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant