A navigation method based on road edge

Abstract

The present invention discloses a navigation method based on road sidelines, which relates to the field of robot navigation technology. Based on the observation of a set forward-looking camera, the present invention accurately extracts the road sidelines without prior marking, calculates the yaw angle and lateral deviation distance relative to the road surface, and then uses a factor graph model and a wheel odometer to obtain the motion trajectory of the robot. The method is simple, the navigation of the robot can be completed without redundant data, the required computing resources are small, the computing speed is fast, and it is applicable to a variety of scenarios.

Description

Technical Field

[0001] The present invention relates to the technical field of robot navigation, and in particular to a navigation method based on road sidelines. Background Art

[0002] High-precision positioning of robots in open areas outdoors or indoors has always been a difficult problem. For outdoor situations, the commonly used methods are GNSS positioning or differential positioning, but GNSS positioning has low accuracy and differential positioning is very expensive. For indoor situations, lidar positioning is usually used. However, for large and open indoor scenes, lidar usually cannot locate because it cannot detect enough collective features. Summary of the invention

[0003] The object of the present invention is to provide a navigation method based on road sidelines to improve the above technical problems.

[0004] In order to achieve the above-mentioned object of the invention, the embodiment of the present invention provides the following technical solutions:

[0005] A navigation method based on road edges includes:

[0006] S1. Using a camera mounted on a vehicle body to take real-time photos of the road to obtain a road image; determining a vehicle body coordinate system and a camera coordinate system;

[0007] S2. acquiring a yaw angle and a lateral deviation distance of the vehicle body relative to the road based on the vehicle body coordinate system, the camera coordinate system and the road image;

[0008] S3, obtaining wheel odometer information of the robot through the wheel odometer, and constructing a factor graph model;

[0009] S4. Obtain the motion trajectory of the robot through the factor graph model; operate the robot based on the motion trajectory to complete navigation of the robot.

[0010] Further, the acquiring the yaw angle and lateral deviation distance of the vehicle body relative to the road based on the vehicle body coordinate system, the camera coordinate system and the road image includes:

[0011] S2-1, obtaining a road width and setting an interval division value; based on the road width and the interval division value, setting first road virtual boundary line coordinate information in a vehicle body coordinate system; the first road virtual boundary line coordinate information includes coordinate information of a plurality of road virtual boundary lines; the coordinate information of each road virtual boundary line includes two corresponding coordinate points, and the corresponding road virtual boundary line is obtained by connecting the two coordinate points;

[0012] S2-2, based on the conversion matrix of the vehicle body coordinate system and the camera coordinate system, convert the first road virtual boundary coordinate information to obtain the second road virtual boundary coordinate information in the camera coordinate system;

[0013] S2-3, based on the coordinate information of the second road virtual boundary line, obtaining a third road virtual boundary line corresponding to a camera image plane; the camera image plane is an image plane corresponding to an image captured in real time by a camera;

[0014] S2-4, segmenting the road image by a SAM method to obtain corresponding road detection edges;

[0015] S2-5, determining an actual road edge line based on the third road virtual edge line and the road detection edge line;

[0016] S2-6. Based on the actual edge line of the road, calculate the yaw angle and lateral deviation distance of the vehicle body relative to the road.

[0017] Furthermore, the formula of the transformation matrix between the vehicle body coordinate system and the camera coordinate system is:

[0018] ;

[0019] ;

[0020] ;

[0021] in, Represents the transformation matrix between the vehicle coordinate system and the camera coordinate system, Represents the rotation transformation matrix from the vehicle coordinate system to the camera coordinate system, Represents the translation transformation matrix from the vehicle coordinate system to the camera coordinate system.

[0022] Further, based on the second road virtual edge line coordinate information, obtaining a third road virtual edge line of the corresponding camera image plane includes:

[0023] S2-3-1, projecting the second road virtual boundary coordinate information onto the camera image plane, and calculating the pixel point coordinates of the second road virtual boundary coordinate information on the camera image plane;

[0024] S2-3-2. Connect the pixel coordinates corresponding to all the second road virtual boundary lines in the second road virtual boundary line coordinate information, and project them to the camera image plane to obtain the third road virtual boundary line.

[0025] Furthermore, the road image is segmented by a SAM method to obtain corresponding road detection edges, including:

[0026] S2-4-1, segmenting the road image using a SAM2 method to determine each segmented area;

[0027] S2-4-2, extracting the external contour in each segmented area, processing it using Hough line transform, and obtaining the initial road detection edge line and its corresponding length information;

[0028] S2-4-3. Determine whether the length information corresponding to each of the initial road detection edges exceeds a threshold; if so, retain the initial road detection edge, otherwise remove the initial road detection edge to obtain the road detection edge.

[0029] Further, determining the actual road edgeline based on the third road virtual edgeline and the road detected edgeline includes:

[0030] S2-5-1, select a road detection edgeline, match it with the third road virtual edgelines in sequence, calculate the matching degree between the road detection edgeline and each third road virtual edgeline, and select the highest matching degree as the road surface edgeline probability of the road detection edgeline;

[0031] S2-5-2, repeat S2-5-1 until all road detection edges are matched to obtain the corresponding road edge probability;

[0032] S2-5-3, sorting all road edge probabilities from large to small, selecting the road detection edge corresponding to the top five road edge probabilities, and obtaining the selected road detection edge;

[0033] S2-5-4. Calculate the distances between the distance midlines corresponding to the left half image and the right half image in the road image and the selected road detection edge line respectively, and select the selected road detection edge line with the smallest distance as the two road edge lines, so as to obtain the actual road edge line.

[0034] Further, based on the actual edge line of the road, calculating the yaw angle and lateral deviation distance of the vehicle body relative to the road includes:

[0035] S2-6-1, projecting the actual road edge line to the vehicle body coordinate system to determine the relationship between the actual road edge line and the vehicle body from a bird's eye view;

[0036] S2-6-2, based on the relationship between the actual road edge line and the vehicle body, determining the middle point corresponding to the actual road edge line, and calculating the direction angle of the vehicle body relative to the road;

[0037] S2-6-3, calculating the yaw angle of the vehicle body relative to the road based on the direction angle of the vehicle body relative to the road;

[0038] S2-6-4. Calculate the lateral deviation distance based on the middle point corresponding to the actual edge line of the road.

[0039] Furthermore, the wheel odometer information of the robot is obtained through the wheel odometer, including:

[0040] S3-1, using the same method as S1 to S2, obtain the yaw angle and lateral deviation distance of the robot relative to the world coordinate system;

[0041] S3-2, taking the yaw angle and lateral deviation distance of S3-1 as observation factors;

[0042] S3-3, using the wheel odometer to obtain the continuous relative motion of the robot, accumulating the X-direction displacement, Y-direction displacement and relative rotation angle during each motion, and obtaining the corresponding wheel odometer information;

[0043] S3-4, using the wheel odometer information as a motion factor;

[0044] S3-5, obtain the robot's historical status node;

[0045] S3-6, constructing an initial factor graph model based on the motion factor and the observation factor;

[0046] S3-7, inputting the historical state node into the initial factor graph model, obtaining the corresponding historical motion trajectory, and constructing the corresponding objective function;

[0047] S3-8. Optimize the initial factor graph model based on the objective function to obtain an optimized factor graph model, thereby completing the construction of the factor graph model.

[0048] The effective effects of the present invention are:

[0049] The present invention is based on the observation of the set forward-looking camera, and can accurately extract the road edge line without advance marking, and calculate the yaw angle and lateral deviation distance relative to the road surface, and then use the factor graph model and the wheel odometer to obtain the motion trajectory of the robot. The method is simple, and the navigation of the robot can be completed without redundant data. It requires few computing resources, has a fast computing speed, and is applicable to a variety of scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0051] Figure 1 is a flow chart of a method in an embodiment of the present invention;

[0052] Figure 2 is a schematic diagram of a camera coordinate system and a vehicle coordinate system in an embodiment of the present invention;

[0053] Figure 3 is a schematic diagram of a first road virtual edge line in a vehicle body coordinate system in an embodiment of the present invention;

[0054] Figure 4 is a schematic diagram of pixel points of a camera image plane in an embodiment of the present invention;

[0055] Figure 5 is a schematic diagram of a third road virtual edge line of the camera image plane in an embodiment of the present invention;

[0056] Figure 6 is a schematic diagram of matching degree in an embodiment of the present invention;

[0057] Figure 7 A schematic diagram of actual road sidelines in an embodiment of the present invention;

[0058] Figure 8 is a schematic diagram of the positions of the camera and the vehicle body in an embodiment of the present invention;

[0059] Fig. 9 Schematic diagram of the relationship between the actual road edge and the vehicle body in an embodiment of the present invention. DETAILED DESCRIPTION

[0060] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. The components of the embodiments of the present invention generally described and represented in the drawings here can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the claimed invention, but merely represents the selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without making creative work belong to the scope of protection of the present invention.

[0061] See also Figure 1 , a navigation method based on road sidelines provided in this embodiment includes:

[0062] S1. Take real-time photos of the road through a camera installed on the vehicle to obtain a road image; determine the vehicle coordinate system and the camera coordinate system, such as Figure 2 As shown; in the figure, AGV is an automatic guided vehicle;

[0063] S2. acquiring a yaw angle and a lateral deviation distance of the vehicle body relative to the road based on the vehicle body coordinate system, the camera coordinate system and the road image;

[0064] The acquiring, based on the vehicle body coordinate system, the camera coordinate system and the road image, a yaw angle and a lateral deviation distance of the vehicle body relative to the road comprises:

[0065] S2-1, obtaining a road width and setting an interval division value; and setting first road virtual edge line coordinate information in a vehicle body coordinate system based on the road width and the interval division value;

[0066] The first road virtual boundary line coordinate information includes coordinate information of a plurality of road virtual boundary lines; the coordinate information of each road virtual boundary line includes two corresponding coordinate points, and the corresponding road virtual boundary line is obtained by connecting the two coordinate points;

[0067] like Figure 3 As shown in the figure, assuming that the road width is 2m and the interval division value is set to 10cm, a road virtual edge line is set every 10cm, so there are 21 road virtual edge lines, each road virtual edge line is represented by any two different points on the line, and each road virtual edge line is parallel to the vehicle body coordinate system. Direction, along Directions are evenly spaced. It represents the first virtual edge of the road in the vehicle coordinate system, and its expression is:

[0068] ;

[0069] in, , Represent the first road virtual edge For any two different points on , , Indicate point The three-dimensional coordinates in the vehicle body coordinate system, , , Indicate point The three-dimensional coordinates in the vehicle coordinate system.

[0070] exist Figure 3 middle, Indicates the second road virtual edge in the vehicle coordinate system.

[0071] S2-2, based on the conversion matrix of the vehicle body coordinate system and the camera coordinate system, convert the first road virtual boundary coordinate information to obtain the second road virtual boundary coordinate information in the camera coordinate system;

[0072] The second road virtual boundary line coordinate information is similar to the first road virtual boundary line coordinate information, and also includes coordinate information of a plurality of second road virtual boundary lines; the coordinate information of each second road virtual boundary line includes two corresponding coordinate points, and the corresponding second road virtual boundary line is obtained by connecting the two coordinate points;

[0073] The formula of the transformation matrix between the vehicle coordinate system and the camera coordinate system is:

[0074] ;

[0075] ;

[0076] ;

[0077] in, Represents the transformation matrix between the vehicle coordinate system and the camera coordinate system, Represents the rotation transformation matrix from the vehicle coordinate system to the camera coordinate system, which is a 3×3 matrix. Represents the translation transformation matrix from the vehicle coordinate system to the camera coordinate system, which is a 3×1 matrix.

[0078] The transformation matrix of the vehicle coordinate system and the camera coordinate system is obtained based on the vehicle body and camera in actual situations. Different vehicle bodies and different camera installation positions correspond to different transformation matrices, which must be specifically formulated based on the 3D structure diagram during vehicle body design.

[0079] For the first road virtual edge , the corresponding points and Point After conversion, we get:

[0080] ;

[0081] in, Indicate point The corresponding point in the camera coordinate system is, Indicate point The corresponding point in the camera coordinate system.

[0082] Connect the dots and Point , get the first road virtual edge The edge line in the camera coordinate system, that is, the coordinate information of the second road virtual edge line.

[0083] S2-3, based on the coordinate information of the second road virtual boundary line, obtaining a third road virtual boundary line corresponding to a camera image plane; the camera image plane is an image plane corresponding to an image captured in real time by a camera;

[0084] Based on the second road virtual boundary coordinate information, obtaining a third road virtual boundary of a corresponding camera image plane includes:

[0085] S2-3-1, projecting the second road virtual boundary coordinate information onto the camera image plane, and calculating the pixel point coordinates of the second road virtual boundary coordinate information on the camera image plane;

[0086] like Figure 4 As shown, the camera intrinsic parameter matrix is ​​obtained by camera calibration, and the point corresponding to the coordinate information of the second road virtual edge line is and Point Project it to the camera image plane and calculate the corresponding pixel points. The corresponding formula is:

[0087] ;

[0088] ;

[0089] in, represents the camera intrinsic parameter matrix, Indicate point At the pixel point corresponding to the camera image plane, Indicate point At the pixel point corresponding to the camera image plane, , Represents pixel points The column and row coordinates of , Represents pixel points Column and row coordinates of .

[0090] S2-3-2. Connect the pixel coordinates corresponding to all the second road virtual boundary lines in the second road virtual boundary line coordinate information, and project them to the camera image plane to obtain the third road virtual boundary line.

[0091] The pixel and pixels The first road virtual boundary line in the third road virtual boundary line is obtained by connecting them. Repeat the above calculation of the pixel point coordinates corresponding to all the second road virtual boundary lines in the second road virtual boundary line coordinate information and connect them to obtain the third road virtual boundary line, and project all the road virtual boundary lines in the third road virtual boundary line to the camera image plane, such as Figure 5 shown.

[0092] The third road virtual boundary is similar to the first road virtual boundary coordinate information, and also includes coordinate information of multiple road virtual boundaries and their corresponding two coordinate points;

[0093] S2-4, segmenting the road image by a SAM method to obtain corresponding road detection edges;

[0094] The step of segmenting the road image by a SAM method to obtain corresponding road detection edges includes:

[0095] S2-4-1, segmenting the road image using a SAM2 method to determine each segmented area;

[0096] S2-4-2, extracting the external contour in each segmented area, processing it using Hough line transform, and obtaining the initial road detection edge line and its corresponding length information;

[0097] S2-4-3, determine whether the length information corresponding to each of the initial road detection edges exceeds a threshold; if so, retain the initial road detection edge, otherwise remove the initial road detection edge to obtain the road detection edge, wherein the threshold is 50 pixels.

[0098] S2-5, determining an actual road edge line based on the third road virtual edge line and the road detection edge line;

[0099] Determining the actual road edge line based on the third road virtual edge line and the road detection edge line includes:

[0100] S2-5-1, select a road detection edgeline, match it with the third road virtual edgelines in sequence, calculate the matching degree between the road detection edgeline and each third road virtual edgeline, and select the highest matching degree as the road surface edgeline probability of the road detection edgeline;

[0101] The matching degree of two image lines is defined as the reciprocal of the distance between the intersection of the two lines and the upper and lower borders of the image. The corresponding formula is:

[0102] ;

[0103] in, Indicates The road detection edge and The matching degree between the third road virtual edges, Indicates The intersection of the road detection edge and the upper border, Indicates The intersection of the road detection edge and the lower border, Indicates The intersection of the third road virtual edge line and the upper border, Indicates The intersection of the third road virtual edge line and the lower border.

[0104] like Figure 6 As shown, the road detection edge and Taking the third road virtual boundary line as an example, the corresponding formula is:

[0105] ;

[0106] in, Indicates road detection edge and The matching degree between the third road virtual edges, Indicates road detection edge The intersection with the top border, Indicates road detection edge Intersection point with bottom border.

[0107] S2-5-2, repeat S2-5-1 until all road detection edges are matched to obtain the corresponding road edge probability;

[0108] S2-5-3, sorting all road edge probabilities from large to small, selecting the road detection edge corresponding to the top five road edge probabilities, and obtaining the selected road detection edge;

[0109] S2-5-4, respectively calculating the distances between the distance midlines corresponding to the left half image and the right half image in the road image and the selected road detection edgeline, and selecting the selected road detection edgeline with the smallest distance as the two road edgelines, that is, obtaining the actual road edgeline, such as Figure 7 shown.

[0110] S2-6. Based on the actual edge line of the road, calculate the yaw angle and lateral deviation distance of the vehicle body relative to the road.

[0111] Based on the actual edge line of the road, the yaw angle and lateral deviation distance of the vehicle body relative to the road are calculated, including:

[0112] S2-6-1, projecting the actual road edge line to the vehicle body coordinate system to determine the relationship between the actual road edge line and the vehicle body from a bird's eye view;

[0113] Convert the actual road edge to the vehicle coordinate system, such as Figure 8 As shown, the camera installation height is H, which is set to 0.7m.

[0114] Select an actual edge of the road and calculate a pixel point on the line, such as Figure 7 Pixels in , , Pixel Column and row coordinates of .

[0115] Based on the formula:

[0116] ;

[0117] Get pixel points Depth ;

[0118] According to the formula:

[0119] ;

[0120] Project it to the camera depth normalized coordinate system to get the corresponding point ;in, represents the inverse matrix of the camera intrinsic parameter matrix, , , Indicates the point corresponding to the camera depth normalized coordinate system The three-dimensional coordinates of .

[0121] According to the formula:

[0122] ;

[0123] Projected onto the ground in three-dimensional space, that is, the point in the camera coordinate system ;

[0124] Then use the rotation transformation matrix and translation transformation matrices Point Convert to the vehicle coordinate system, that is .

[0125] Through the above conversion process, four points on the actual sidelines of the two roads in the vehicle coordinate system are obtained.

[0126] Only consider the vehicle coordinate system Direction and Direction, based on four points on the actual edge of the two roads in the vehicle coordinate system , , , , the relationship between the actual edge of the road and the vehicle body in the top-down perspective is obtained, such as Fig. 9 shown.

[0127] S2-6-2, based on the relationship between the actual road edge and the vehicle body, determine the middle point corresponding to the actual road edge , , calculating the direction angle of the vehicle body relative to the road;

[0128] Midpoint for Fig. 9 midpoint and Point The middle point between for Fig. 9 midpoint and Point The midpoint between

[0129] According to the formula:

[0130] ;

[0131] Calculate the direction angle of the vehicle relative to the road ;in, , Indicates the direction angle of the vehicle relative to the road The coordinates of .

[0132] S2-6-3. Calculate the yaw angle of the vehicle body relative to the road based on the direction angle of the vehicle body relative to the road , the corresponding formula is:

[0133] ;

[0134] in, represents the forward vector of the vehicle body, and ;

[0135] S2-6-4. Calculate the lateral deviation distance of the vehicle body relative to the center line of the road surface based on the middle point corresponding to the actual edge line of the road , the corresponding formula is:

[0136] ;

[0137] , Indicates the middle point The coordinates of , Indicates the middle point The coordinates of .

[0138] S3, obtaining wheel odometer information of the robot through the wheel odometer, and constructing a factor graph model;

[0139] Since the lateral deviation distance and yaw angle obtained through vision cannot be directly used for robot navigation, it is chosen to fuse the lateral deviation distance and yaw angle with the wheel odometer to obtain accurate robot plane coordinates for robot navigation.

[0140] Obtain the robot's wheel odometer information through the wheel odometer, including:

[0141] S3-1, using the same method as S1 to S2, obtain the yaw angle and lateral deviation distance of the robot relative to the world coordinate system;

[0142] The origin of the world coordinate system is defined as the horizontal center point of the road surface where the robot is located when it starts. The direction facing the road is the X-axis of the world coordinate system; facing the X-axis direction, the left is the Y-axis of the world coordinate system, and the upward direction is the Z-axis of the world coordinate system.

[0143] S3-2, taking the yaw angle and lateral deviation distance of S3-1 as observation factors;

[0144] The yaw angle of the robot relative to the world coordinate system is , that is, at time The angle of the robot relative to the x-axis of the world coordinate system is , that is, at time The lateral deviation distance of the robot.

[0145] S3-3, using the wheel odometer to obtain the continuous relative motion of the robot, accumulating the X-direction displacement, Y-direction displacement and relative rotation angle during each motion, and obtaining the corresponding wheel odometer information;

[0146] The wheel odometer can obtain the continuous relative motion of the robot. Due to measurement errors and wheel slippage, the robot position accumulated by the wheel odometer will gradually drift. Define wheel odometer information:

[0147] For at the moment To time The X-direction displacement between

[0148] For at the moment To time The Y-direction displacement between

[0149] For at the moment To time The relative rotation angle between them.

[0150] S3-4, the wheel odometer information , , As a motion factor, it provides the relative motion information of the robot between adjacent time steps;

[0151] S3-5, obtain the robot's historical status node;

[0152] S3-6, based on the motion factor and the observation factor, determining constraint conditions and constructing an initial factor graph model;

[0153] Factor graph model is a probabilistic graph model suitable for dealing with multi-source sensor information fusion problems. Its main idea is to solve the optimal state estimation by constructing a graph structure composed of state variables and factors (constraints) and combining the observation results of different sensors. The state variable is the state node, which is the time The robot status , robot status Including the robot's position in the coordinate system .

[0154] The constraints are determined based on motion factors and observation factors, which include motion factor constraints and observation factor constraints, corresponding to the motion constraints of the wheel odometer and the measurement constraints of visual observation respectively;

[0155] The relative motion measured by the odometer is , then the motion factor constraint for:

[0156] ;

[0157] in, , Respectively indicate time The robot status and time The robot status .

[0158] At the moment When , the side deviation distance and yaw angle obtained by visual measurement are and , then the observation factor constraint for:

[0159] ;

[0160] in, Indicates the initial value of the robot's lateral distance, Indicates the initial value of the robot's lateral heading angle.

[0161] S3-7, input the historical state node into the initial factor graph model, obtain the corresponding historical motion trajectory, and construct the corresponding objective function , the corresponding formula is:

[0162] ;

[0163] in, represents the motion constraint factor, represents the measurement constraint factor;

[0164] Objective Function The optimization can be solved by common nonlinear optimization algorithms (such as Gauss-Newton or Levenberg-Marquardt) to obtain the optimal robot motion trajectory.

[0165] S3-8. Optimize the initial factor graph model based on the objective function to obtain an optimized factor graph model, thereby completing the construction of the factor graph model.

[0166] S4. Obtain the motion trajectory of the robot through the factor graph model; operate the robot based on the motion trajectory to complete navigation of the robot.

[0167] The trajectory output by the factor graph model optimization will replace the wheel odometry for robot navigation.

[0168] In summary, the present invention is based on the observation of the set forward-looking camera, and can accurately extract the edge of the road surface without prior marking, and calculate the yaw angle and lateral deviation distance relative to the road surface, and then use the factor graph model and the wheel odometer to obtain the motion trajectory of the robot. The method is simple, and the navigation of the robot can be completed without redundant data. It requires few computing resources, has a fast computing speed, and is applicable to a variety of scenarios.

[0169] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A navigation method based on road sidelines, characterized in that: include: S1. Using a camera mounted on a vehicle body to take real-time photos of the road to obtain a road image; determining a vehicle body coordinate system and a camera coordinate system; S2. acquiring a yaw angle and a lateral deviation distance of the vehicle body relative to the road based on the vehicle body coordinate system, the camera coordinate system and the road image; S3, obtaining wheel odometer information of the robot through the wheel odometer, and constructing a factor graph model; S4, obtaining the motion trajectory of the robot through the factor graph model; running the robot based on the motion trajectory to complete navigation of the robot; The acquiring, based on the vehicle body coordinate system, the camera coordinate system and the road image, a yaw angle and a lateral deviation distance of the vehicle body relative to the road comprises: S2-1, obtaining a road width and setting an interval division value; based on the road width and the interval division value, setting first road virtual boundary line coordinate information in a vehicle body coordinate system; the first road virtual boundary line coordinate information includes coordinate information of a plurality of road virtual boundary lines; the coordinate information of each road virtual boundary line includes two corresponding coordinate points, and the corresponding road virtual boundary line is obtained by connecting the two coordinate points; S2-2, based on the conversion matrix of the vehicle body coordinate system and the camera coordinate system, convert the first road virtual boundary coordinate information to obtain the second road virtual boundary coordinate information in the camera coordinate system; S2-3. Based on the coordinate information of the second road virtual boundary line, obtain a third road virtual boundary line corresponding to a camera image plane; the camera image plane is an image plane corresponding to an image captured by a camera in real time; S2-4, segmenting the road image by a SAM method to obtain corresponding road detection edges; S2-5, determining an actual road edge line based on the third road virtual edge line and the road detection edge line; S2-6. Calculating the yaw angle and lateral deviation distance of the vehicle body relative to the road based on the actual edge line of the road; The determining the actual road edge line based on the third road virtual edge line and the road detection edge line comprises: S2-5-1, select a road detection edgeline, match it with the third road virtual edgelines in sequence, calculate the matching degree between the road detection edgeline and each third road virtual edgeline, and select the highest matching degree as the road surface edgeline probability of the road detection edgeline; S2-5-2, repeat S2-5-1 until all road detection edges are matched to obtain the corresponding road edge probability; S2-5-3, sorting all road edge probabilities from large to small, selecting the road detection edge corresponding to the top five road edge probabilities, and obtaining the selected road detection edge; S2-5-4, respectively calculating the distances between the distance midlines corresponding to the left half image and the right half image in the road image and the selected road detection edgeline, and selecting the selected road detection edgeline with the smallest distance as the two road edgelines, that is, obtaining the actual road edgeline; The formula corresponding to the matching degree is: ； in, Indicates road detection edge and The matching degree between the third road virtual edges, Indicates road detection edge The intersection with the top border, Indicates road detection edge The intersection with the bottom border, Indicates The intersection of the third road virtual edge line and the upper border, Indicates The intersection of the third road virtual edge line and the lower border.

2. A road edge-based navigation method according to claim 1, characterized in that: The formula of the transformation matrix between the vehicle body coordinate system and the camera coordinate system is: ； ； ； in, Represents the transformation matrix between the vehicle coordinate system and the camera coordinate system, Represents the rotation transformation matrix from the vehicle coordinate system to the camera coordinate system, Represents the translation transformation matrix from the vehicle coordinate system to the camera coordinate system.

3. A road edge-based navigation method according to claim 1, characterized in that: The step of acquiring a third road virtual edge line corresponding to the camera image plane based on the second road virtual edge line coordinate information includes: S2-3-1. Projecting the second road virtual boundary coordinate information onto the camera image plane, and calculating the pixel point coordinates of the second road virtual boundary coordinate information on the camera image plane; S2-3-2. Connect the pixel point coordinates corresponding to all the second road virtual boundary lines in the second road virtual boundary line coordinate information to obtain the third road virtual boundary line.

4. A road edge-based navigation method according to claim 1, characterized in that: The step of segmenting the road image by a SAM method to obtain corresponding road detection edges includes: S2-4-1, segmenting the road image using a SAM2 method to determine each segmented area; S2-4-2, extracting the external contour in each segmented area, processing it using Hough line transform, and obtaining the initial road detection edge line and its corresponding length information; S2-4-3. Determine whether the length information corresponding to each of the initial road detection edges exceeds a threshold; if so, retain the initial road detection edge, otherwise remove the initial road detection edge to obtain the road detection edge.

5. A road edge-based navigation method according to claim 1, characterized in that: The calculating, based on the actual edge line of the road, the yaw angle and the lateral deviation distance of the vehicle body relative to the road comprises: S2-6-1, projecting the actual road edge line to the vehicle body coordinate system to determine the relationship between the actual road edge line and the vehicle body from a bird's eye view; S2-6-2, based on the relationship between the actual road edge line and the vehicle body, determining the middle point corresponding to the actual road edge line, and calculating the direction angle of the vehicle body relative to the road; S2-6-3, calculating the yaw angle of the vehicle body relative to the road based on the direction angle of the vehicle body relative to the road; S2-6-4. Calculate the lateral deviation distance based on the middle point corresponding to the actual edge line of the road.

6. A road edge-based navigation method according to claim 1, characterized in that: The obtaining of wheel odometer information of the robot through the wheel odometer includes: S3-1, using the same method as S1 to S2, obtain the yaw angle and lateral deviation distance of the robot relative to the world coordinate system; S3-2, taking the yaw angle and lateral deviation distance of S3-1 as observation factors; S3-3, using the wheel odometer to obtain the continuous relative motion of the robot, accumulating the X-direction displacement, Y-direction displacement and relative rotation angle during each motion, and obtaining the corresponding wheel odometer information; S3-4, using the wheel odometer information as a motion factor; S3-5, obtain the robot's historical status node; S3-6, constructing an initial factor graph model based on the motion factor and the observation factor; S3-7, inputting the historical state node into the initial factor graph model, obtaining the corresponding historical motion trajectory, and constructing the corresponding objective function; S3-8. Optimize the initial factor graph model based on the objective function to obtain an optimized factor graph model, thereby completing the construction of the factor graph model.

Images