A parking assistance method and system based on laser radar coordinate positioning

Abstract

The present application relates to the technical field of vehicle auxiliary driving, and particularly relates to a parking assistance method and system based on laser radar coordinate positioning. The method comprises the following steps: step S1: a preset coordinate symbol of a parking space is acquired, and a first coordinate of the parking space is acquired based on the coordinate symbol; step S2: a second coordinate of the preset coordinate symbol relative to the vehicle at a next moment is acquired based on laser radar, and a vehicle pose at the next moment is acquired based on the first coordinate and the second coordinate; step S3: environment information around the vehicle is acquired based on ultrasonic radar or a 360 imaging system; whether the vehicle pose at the next moment and the environment information overlap is judged, and if yes, an alarm is sent. Through pose prediction of the laser radar and dynamic fusion of the environment information, collision prediction is realized, and the pain points of frequent alarms and lack of foresight when parking in a narrow parking space are solved. This not only improves safety and comfort, but also optimizes parking efficiency, and is particularly suitable for indoor narrow environments.

Description

Technical Field

[0001] This invention relates to the technical field of vehicle driver assistance, and in particular to a parking assistance method and system based on lidar coordinate positioning. Background Technology

[0002] With the continuous development of technology, various detection devices installed on automobiles, such as lidar, cameras, and sensors, work together to assist users in driving and provide a better driving experience when the car is in motion or parked.

[0003] For example, during parking, ultrasonic radar or 360-degree imaging systems are typically used to assess obstacles or the surrounding environment near the parking space. When the vehicle is close to an obstacle, these systems can provide a warning to the driver. However, in indoor environments like shopping malls, parking spaces are often limited and narrow, and there may be nearby equipment such as fire hydrants. Drivers often need to park very close to adjacent vehicles or objects to secure a spot. During this process, the close proximity to other objects causes the ultrasonic radar or 360-degree imaging system to continuously issue warnings, significantly impacting the driver's operation and judgment. Furthermore, ultrasonic radar or 360-degree imaging systems can only provide warnings based on current information and cannot predict collisions. Therefore, finding ways to assist in parking in narrow spaces is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0004] To address the aforementioned shortcomings, the present invention aims to propose a parking assistance method and system based on lidar coordinate positioning to solve the parking problem in narrow parking spaces.

[0005] To achieve this objective, the present invention adopts the following technical solution: a parking assistance method based on lidar coordinate positioning, comprising the following steps:

[0006] Step S1: Obtain the preset coordinate symbol of the parking space, and obtain the first coordinate of the parking space based on the coordinate symbol;

[0007] Step S2: Based on the lidar, obtain the second coordinate of the preset coordinate symbol relative to the vehicle at the next moment, and obtain the vehicle's pose at the next moment based on the first coordinate and the second coordinate;

[0008] Step S3: Obtain environmental information around the vehicle based on ultrasonic radar or a 360-degree imaging system;

[0009] Determine if there is any overlap between the vehicle's position and the environmental information at the next moment; if so, issue an alarm.

[0010] Preferably, the preset coordinate symbols in step S1 are printed on the wall or the vehicle barrier;

[0011] The preset coordinate symbol is a square plane with red markings around its perimeter, and yellow and black markings for coordinate information on the left and right sides of the plane, respectively.

[0012] Preferably, in step S2, the step of obtaining the second coordinate of the preset coordinate symbol relative to the vehicle at the next moment is as follows:

[0013] Step S21: Obtain the initial pose sequence using lidar odometry;

[0014] Step S22: Construct an objective function based on the pre-integration information of the inertial navigation system and the plane constraint of the preset coordinate symbols, and solve the objective function using the Levenberg-Marquardt algorithm to obtain the transformation matrix between the lidar and the first coordinate.

[0015] Step S23: The first coordinate is transformed into the second coordinate through a transformation matrix.

[0016] Preferably, the step of obtaining the initial pose sequence in step S21 is as follows:

[0017] Step S211: Receive a set of laser point cloud data and output the state variables corresponding to the point cloud data at each time step;

[0018] Among them, the first The state variables at time t are represented as follows: ;

[0019] in, From Time-based lidar coordinate system To the lidar odometry coordinate system The rotation matrix, , , They are In the lidar odometry coordinate system Position, velocity, and angular velocity; and These are Gaussian white noise corresponding to the velocity and rotation, respectively.

[0020] Step S212: Construct the problem of lidar odometry, where the lidar odometry problem is as follows:

[0021] ;

[0022] in, for The set of all planes fitted to the point cloud at any given time; The first in the plane set ICP residuals at individual points, For the first Information matrix of point-to-surface ICP, for Prior residuals at all times for Information matrix of prior constraints at each time step;

[0023] Step S32: Construct the problem of lidar odometry, where the lidar odometry problem is as follows:

[0024] ;

[0025] in, for The set of all planes fitted to the point cloud at any given time; The first in the plane set ICP residuals at individual points, For the first Information matrix of point-to-surface ICP, for Prior residuals at all times for Information matrix of prior constraints at each time step;

[0026] Step S213: Perform forward propagation on the covariance matrix, the mathematical expression of which is as follows:

[0027] ;

[0028] in, and They are respectively Time and The prior state covariance matrix at time t. The noise covariance matrix is... , The definition is as follows:

[0029] ;

[0030] ;

[0031] in For exponential mappings in Lie algebras; for Time's up The time interval between moments; It is a right Jacobian function;

[0032] Step S214: Construct the prior residuals and ICP residuals, where the specific details of the prior residuals are as follows:

[0033] ;

[0034] in, It is a logarithmic mapping that converts a rotation matrix into a Lie algebra;

[0035] The specific ICP residuals are as follows:

[0036] ;

[0037] in For the first One feature point, and These are the normal vector and intercept of the fitted plane, respectively;

[0038] Step S35: Substitute the obtained covariance matrix, prior residual, and ICP residual into the problem of lidar odometry to obtain the initial pose sequence.

[0039] Preferably, the specific steps of step S22 are as follows:

[0040] Step S221: Project the initial pose sequence onto the lidar coordinate system to obtain a local point cloud map;

[0041] Step S222: Extract planar features based on the local point cloud map. ;

[0042] Among them, planar features The definition is as follows:

[0043] ;

[0044] ;

[0045] For DH parameters, Indicates the first A planar feature, This indicates taking the smallest eigenvalue of the matrix; Indicates the first A set of point clouds on a plane; Indicates the first The number of point clouds in a plane-shaped point cloud set. For the first The center of a point cloud on a plane, To represent the point cloud set of the j-th plane The i-th point cloud in;

[0046] Step S223: Construct the objective function for obtaining the transformation matrix using the factor graph, where the objective function is as follows:

[0047] ;

[0048] For DH parameters, The state variables at time k include Rotation matrix at time step Translation at time k and speed , Zero bias estimate of gyroscope in inertial navigation at any time and accelerometer zero bias estimate ; This represents the number of planes fitted from the local point cloud map. Indicates the first Features of a plane The number of pose sequences output by the lidar odometry; The information matrix for inertial navigation pre-integration constraints. for Real-time status Inertial navigation pre-integral constraints for the state at any given time;

[0049] Step S224: The Levenberg-Marquardt algorithm is used to solve the objective function to obtain the transformation matrix between the lidar and the first coordinate.

[0050] A parking assistance system based on lidar coordinate positioning, using the aforementioned parking assistance method based on lidar coordinate positioning, includes a first coordinate acquisition module, a second coordinate acquisition module, and a prediction module;

[0051] The first coordinate acquisition module is used to acquire the preset coordinate symbol of the parking space, and obtain the first coordinate of the parking space based on the coordinate symbol;

[0052] The second coordinate acquisition module is used to acquire the second coordinate of the vehicle relative to the preset coordinate symbol at the next moment based on the lidar, and to obtain the vehicle's pose at the next moment based on the first coordinate and the second coordinate.

[0053] The prediction module is used to acquire environmental information around the vehicle based on ultrasonic radar or a 360-degree imaging system.

[0054] Determine if there is any overlap between the vehicle's position and the environmental information at the next moment. If so, issue an alarm.

[0055] Preferably, the second coordinate acquisition module includes an input unit, a solution unit, and a transformation unit;

[0056] The input unit is used to obtain the initial pose sequence via a lidar odometry system.

[0057] The solving unit is used to construct an objective function based on the pre-integration information of the inertial navigation system and the plane constraint of the preset coordinate symbols, and uses the Levenberg-Marquardt algorithm to solve the objective function to obtain the transformation matrix between the lidar and the first coordinate.

[0058] The transformation unit is used to transform the first coordinate into the second coordinate through a transformation matrix.

[0059] Preferably, the input unit includes a receiving subunit, a question construction subunit, a propagation subunit, a residual word subunit, and a sequence acquisition subunit;

[0060] The receiving subunit is used to receive a set of laser point cloud data and output the state variables corresponding to the point cloud data at each time step;

[0061] The problem-building subunit is used to build the problem of lidar odometry;

[0062] The propagation subunit is used to perform forward propagation of the covariance matrix;

[0063] The residual word sub-unit is used to construct the prior residual and the ICP residual;

[0064] The sequence acquisition subunit is used to substitute the acquired covariance matrix, prior residual, and ICP residual into the problem of lidar odometry to obtain the initial pose sequence.

[0065] Preferably, the solution unit includes a local point cloud map acquisition subunit, a planar feature acquisition subunit, an objective function acquisition subunit, and a computation subunit;

[0066] The local point cloud map acquisition sub-unit is used to project the initial pose sequence onto the lidar coordinate system to obtain a local point cloud map.

[0067] The planar feature acquisition subunit is used to extract planar features based on a local point cloud map;

[0068] The objective function acquisition sub-unit is used to construct the objective function for obtaining the transformation matrix through the factor graph;

[0069] The computational subunit is used to solve the objective function using the Levenberg-Marquardt algorithm to obtain the transformation matrix between the lidar and the first coordinate.

[0070] One of the above technical solutions has the following advantages or beneficial effects: The technical solution proposed in this invention achieves collision prediction through the dynamic fusion of lidar pose prediction and environmental information, solving the pain points of frequent alarms and lack of foresight when parking in narrow spaces. This not only improves safety and comfort but also optimizes parking efficiency, and is particularly suitable for narrow indoor environments. Attached Figure Description

[0071] Figure 1 This is a flowchart of one embodiment of the method of the present invention.

[0072] Figure 2 This is a schematic diagram of the structure of one embodiment of the system of the present invention. Detailed Implementation

[0073] Embodiments of the present invention are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention.

[0074] In the description of embodiments of the present invention, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the stated features. In the description of embodiments of the present invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0075] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of this invention, unless otherwise stated, "a plurality of" means two or more. Those skilled in the art will understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0076] like Figures 1-2 As shown, a parking assistance method based on lidar coordinate positioning includes the following steps:

[0077] Step S1: Obtain the preset coordinate symbol of the parking space, and obtain the first coordinate of the parking space based on the coordinate symbol;

[0078] Step S2: Based on the lidar, obtain the second coordinate of the preset coordinate symbol relative to the vehicle at the next moment, and obtain the vehicle's pose at the next moment based on the first coordinate and the second coordinate;

[0079] Step S3: Obtain environmental information around the vehicle based on ultrasonic radar or a 360-degree imaging system;

[0080] Determine if there is any overlap between the vehicle's position and the environmental information at the next moment; if so, issue an alarm.

[0081] In assisted parking, continuous vehicle positioning is necessary. This positioning, combined with ultrasonic radar or 360-degree environmental information, determines the distance between the vehicle and other objects during movement. However, in shopping mall parking spaces, due to their narrowness, ultrasonic radar or 360-degree imaging systems constantly issue warnings while the driver is parking, significantly impacting their operation and judgment. Furthermore, weak signal strength and difficulty in GPS signal reception within parking lots render traditional positioning methods ineffective, leading to inaccurate vehicle location and increasing the risk of collisions.

[0082] To address the aforementioned issues, one solution of this invention involves collaborating with shopping malls or parking lots to attach preset coordinate symbols with accompanying coordinate information to the corresponding parking spaces. When a vehicle is parked, the rear-facing reversing camera or a 360-degree imaging camera acquires the preset coordinate symbols. By parsing these symbols, the first coordinates of the current parking space can be obtained, providing a rough location. Then, by combining LiDAR and inertial navigation, the second coordinates of the vehicle relative to the preset coordinate symbols at the next moment are predicted. In this process, pose prediction is performed using the fixed position of the preset coordinate symbols, which is unaffected by noise and angle, greatly improving the accuracy of positioning.

[0083] Ultrasonic radar or 360-degree imaging systems typically acquire unchanging environmental information about the vehicle's surroundings. By predicting the second coordinates for the next moment, and based on these second coordinates and the vehicle's model, we can output the pose for the next moment. When the pose overlaps with the environmental information, it indicates that the vehicle may be scraping against its surroundings, and only then will an alarm be issued. This is unlike existing alarms that rely on distance from the surrounding environment, which can affect the driver's operation and judgment.

[0084] The technical solution proposed in this invention achieves collision prediction through the dynamic fusion of lidar pose prediction and environmental information, solving the pain points of frequent alarms and lack of foresight when parking in narrow spaces. This not only improves safety (reducing collision risk) and comfort (reducing false alarms), but also optimizes parking efficiency, making it particularly suitable for narrow indoor environments.

[0085] Preferably, the preset coordinate symbols in step S1 are printed on the wall or the vehicle barrier;

[0086] The preset coordinate symbol is a square plane with red markings around its perimeter, and yellow and black markings for coordinate information on the left and right sides of the plane, respectively.

[0087] In existing parking lots, there are usually parking barriers. Setting the preset coordinate symbol in the parking barrier can effectively locate the parking space. For parking spaces near the wall, the symbol can be set on the wall to better obtain the preset coordinate symbol.

[0088] When the preset coordinate symbol is set on the barrier, its image is small. In order to better resolve the first coordinate of the preset coordinate symbol at the barrier, in this invention, a red mark is set around the preset coordinate symbol. The red mark can be a red frame, because in computer image vision, red is the easiest to be recognized by image vision, while yellow and black are next. Then, the latitude and longitude are marked with yellow and black respectively.

[0089] It is worth mentioning that when reversing, if there are no vehicles near the parking space, image recognition may identify more than one preset coordinate symbol. In this case, the corresponding image can be sent to the vehicle's screen or the driver's mobile phone, allowing the driver to select the corresponding preset coordinate symbol.

[0090] Preferably, in step S2, the step of obtaining the second coordinate of the preset coordinate symbol relative to the vehicle at the next moment is as follows:

[0091] Step S21: Obtain the initial pose sequence using lidar odometry;

[0092] Step S22: Construct an objective function based on the pre-integration information of the inertial navigation system and the plane constraint of the preset coordinate symbols, and solve the objective function using the Levenberg-Marquardt algorithm to obtain the transformation matrix between the lidar and the first coordinate.

[0093] Step S23: The first coordinate is transformed into the second coordinate through a transformation matrix.

[0094] By using lidar odometry to obtain the initial pose sequence, and combining inertial navigation pre-integrated data with the planar constraints of preset coordinate symbols, the pose solution is optimized in real time using the Levenberg-Marquardt algorithm. In indoor environments without GPS, the relative positioning accuracy between the vehicle and the parking space is improved to the centimeter level, significantly reducing the risk of collisions in narrow parking spaces.

[0095] Furthermore, multi-source data fusion (LiDAR + inertial navigation + planar constraints) suppresses the limitations of single sensors (such as interference from metal objects or inertial navigation drift). Simultaneously, planar constraints reduce pose optimization from 6 degrees of freedom to 3 degrees of freedom (x, y, yaw angle), improving computational efficiency by more than 5 times and enabling high-frequency real-time pose updates of ≥50Hz, meeting the immediate decision-making needs of parking scenarios. Under limited computing power, it achieves centimeter-level positioning and millisecond-level prediction indoors, completely solving the dual challenges of alarm interference and prediction deficiencies in narrow parking spaces.

[0096] Preferably, the step of obtaining the initial pose sequence in step S21 is as follows:

[0097] Step S211: Receive a set of laser point cloud data and output the state variables corresponding to the point cloud data at each time step;

[0098] Among them, the first The state variables at time t are represented as follows: ;

[0099] in, From Time-based lidar coordinate system To the lidar odometry coordinate system The rotation matrix, , , They are In the lidar odometry coordinate system Position, velocity, and angular velocity; and These are Gaussian white noise corresponding to the velocity and rotation, respectively.

[0100] Step S212: Construct the problem of lidar odometry, where the lidar odometry problem is as follows:

[0101] ;

[0102] in, for The set of all planes fitted to the point cloud at any given time; The first in the plane set ICP residuals at individual points, For the first Information matrix of point-to-surface ICP, for Prior residuals at all times for Information matrix of prior constraints at each time step;

[0103] Step S32: Construct the problem of lidar odometry, where the lidar odometry problem is as follows:

[0104] ;

[0105] in, for The set of all planes fitted to the point cloud at any given time; The first in the plane set ICP residuals at individual points, For the first Information matrix of point-to-surface ICP, for Prior residuals at all times for Information matrix of prior constraints at each time step;

[0106] Step S213: Perform forward propagation on the covariance matrix, the mathematical expression of which is as follows:

[0107] ;

[0108] in, and They are respectively Time and The prior state covariance matrix at time t. The noise covariance matrix is... , The definition is as follows:

[0109] ;

[0110] ;

[0111] in For exponential mappings in Lie algebras; for Time's up The time interval between moments; It is a right Jacobian function;

[0112] Step S214: Construct the prior residuals and ICP residuals, where the specific details of the prior residuals are as follows:

[0113] ;

[0114] in, It is a logarithmic mapping that converts a rotation matrix into a Lie algebra;

[0115] The specific ICP residuals are as follows:

[0116] ;

[0117] in For the first One feature point, and These are the normal vector and intercept of the fitted plane, respectively;

[0118] Step S35: Substitute the obtained covariance matrix, prior residual, and ICP residual into the problem of lidar odometry to obtain the initial pose sequence.

[0119] Due to the problem of ICP information matrix in lidar odometer Since the covariance matrix is ​​given by humans and can be obtained directly, when solving this problem, we only need to perform forward propagation on the covariance matrix to obtain it. The corresponding prior residuals and ICP residuals are constructed and then re-introduced into the problem of lidar odometry to calculate the solution to the lidar odometry problem. The solution to the lidar odometry problem is then used as the initial pose sequence.

[0120] Preferably, the specific steps of step S22 are as follows:

[0121] Step S221: Project the initial pose sequence onto the lidar coordinate system to obtain a local point cloud map;

[0122] Step S222: Extract planar features based on the local point cloud map. ;

[0123] Among them, planar features The definition is as follows:

[0124] ;

[0125] ;

[0126] For DH parameters, Indicates the first A planar feature, This indicates taking the smallest eigenvalue of the matrix; Indicates the first A set of point clouds on a plane; Indicates the first The number of point clouds in a plane-shaped point cloud set. For the first The center of a point cloud on a plane, To represent the point cloud set of the j-th plane The i-th point cloud in;

[0127] Step S223: Construct the objective function for obtaining the transformation matrix using the factor graph, where the objective function is as follows:

[0128] ;

[0129] For DH parameters, The state variables at time k include Rotation matrix at time step Translation at time k and speed , Zero bias estimate of gyroscope in inertial navigation at any time and accelerometer zero bias estimate ; This represents the number of planes fitted from the local point cloud map. Indicates the first Features of a plane The number of pose sequences output by the lidar odometry; The information matrix for inertial navigation pre-integration constraints. for Real-time status Inertial navigation pre-integral constraints for the state at any given time;

[0130] When constructing the objective function, it is done by... The state at time k is constrained to the state at time k, thereby obtaining the objective function at time k. In practical applications, the state at time k-1 needs to be modified to the state at time k. At this time, the inertial navigation constraints at time k+1 can be obtained, thus obtaining the objective function at time k+1 for vehicle attitude prediction.

[0131] Step S224: The Levenberg-Marquardt algorithm is used to solve the objective function to obtain the transformation matrix between the lidar and the first coordinate.

[0132] A parking assistance system based on lidar coordinate positioning, using the aforementioned parking assistance method based on lidar coordinate positioning, includes a first coordinate acquisition module, a second coordinate acquisition module, and a prediction module;

[0133] The first coordinate acquisition module is used to acquire the preset coordinate symbol of the parking space, and obtain the first coordinate of the parking space based on the coordinate symbol;

[0134] The second coordinate acquisition module is used to acquire the second coordinate of the vehicle relative to the preset coordinate symbol at the next moment based on the lidar, and to obtain the vehicle's pose at the next moment based on the first coordinate and the second coordinate.

[0135] The prediction module is used to acquire environmental information around the vehicle based on ultrasonic radar or a 360-degree imaging system.

[0136] Determine if there is any overlap between the vehicle's position and the environmental information at the next moment. If so, issue an alarm.

[0137] Preferably, the second coordinate acquisition module includes an input unit, a solution unit, and a transformation unit;

[0138] The input unit is used to obtain the initial pose sequence via a lidar odometry system.

[0139] The solving unit is used to construct an objective function based on the pre-integration information of the inertial navigation system and the plane constraint of the preset coordinate symbols, and uses the Levenberg-Marquardt algorithm to solve the objective function to obtain the transformation matrix between the lidar and the first coordinate.

[0140] The transformation unit is used to transform the first coordinate into the second coordinate through a transformation matrix.

[0141] Preferably, the input unit includes a receiving subunit, a question construction subunit, a propagation subunit, a residual word subunit, and a sequence acquisition subunit;

[0142] The receiving subunit is used to receive a set of laser point cloud data and output the state variables corresponding to the point cloud data at each time step;

[0143] The problem-building subunit is used to build the problem of lidar odometry;

[0144] The propagation subunit is used to perform forward propagation of the covariance matrix;

[0145] The residual word sub-unit is used to construct the prior residual and the ICP residual;

[0146] The sequence acquisition subunit is used to substitute the acquired covariance matrix, prior residual, and ICP residual into the problem of lidar odometry to obtain the initial pose sequence.

[0147] Preferably, the solution unit includes a local point cloud map acquisition subunit, a planar feature acquisition subunit, an objective function acquisition subunit, and a computation subunit;

[0148] The local point cloud map acquisition sub-unit is used to project the initial pose sequence onto the lidar coordinate system to obtain a local point cloud map.

[0149] The planar feature acquisition subunit is used to extract planar features based on a local point cloud map;

[0150] The objective function acquisition sub-unit is used to construct the objective function for obtaining the transformation matrix through the factor graph;

[0151] The computational subunit is used to solve the objective function using the Levenberg-Marquardt algorithm to obtain the transformation matrix between the lidar and the first coordinate.

[0152] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "illustrative embodiment," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples.

[0153] Although embodiments of the invention have been shown and described, those skilled in the art will understand that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the claims and their equivalents.

Claims

1. A parking assistance method based on lidar coordinate positioning, characterized in that, Includes the following steps: Step S1: Obtain the preset coordinate symbol of the parking space, and obtain the first coordinate of the parking space based on the coordinate symbol; Step S2: Based on the lidar, obtain the second coordinate of the preset coordinate symbol relative to the vehicle at the next moment, and obtain the vehicle's pose at the next moment based on the first coordinate and the second coordinate; Step S3: Obtain environmental information around the vehicle based on ultrasonic radar or a 360-degree imaging system; Determine whether there is any overlap between the vehicle's position and the environmental information at the next moment; if so, issue an alarm. In step S2, the step of obtaining the second coordinate of the preset coordinate symbol relative to the vehicle at the next moment is as follows: Step S21: Obtain the initial pose sequence using lidar odometry; Step S22: Construct an objective function based on the pre-integration information of the inertial navigation system and the plane constraint of the preset coordinate symbols, and solve the objective function using the Levenberg-Marquardt algorithm to obtain the transformation matrix between the lidar and the first coordinate. Step S23: The first coordinate is transformed into the second coordinate using a transformation matrix; The steps for obtaining the initial pose sequence in step S21 are as follows: Step S211: Receive a set of laser point cloud data and output the state variables corresponding to the point cloud data at each time step; Among them, the first The state variables at time t are represented as follows: ; in, From Time-based lidar coordinate system To the lidar odometry coordinate system The rotation matrix, , , They are In the lidar odometry coordinate system Position, velocity, and angular velocity; and These are Gaussian white noise corresponding to the velocity and rotation, respectively. Step S212: Construct the problem of lidar odometry, where the lidar odometry problem is as follows: ； in, for The set of all planes fitted to the point cloud at any given time; The first in the plane set ICP residuals at individual points, For the first Information matrix of point-to-surface ICP, for Prior residuals at all times for Information matrix of prior constraints at each time step; Step S32: Construct the problem of lidar odometry, where the lidar odometry problem is as follows: ； in, for The set of all planes fitted to the point cloud at any given time; The first in the plane set ICP residuals at individual points, For the first Information matrix of point-to-surface ICP, for Prior residuals at all times for Information matrix of prior constraints at each time step; Step S213: Perform forward propagation on the covariance matrix, the mathematical expression of which is as follows: ； in, and They are respectively Time and The prior state covariance matrix at time t. The noise covariance matrix is... , The definition is as follows: ； ； in For exponential mappings in Lie algebras; for Time's up The time interval between moments; It is a right Jacobian function; Step S214: Construct the prior residuals and ICP residuals, where the specific details of the prior residuals are as follows: ； in, It is a logarithmic mapping that converts a rotation matrix into a Lie algebra; The specific ICP residuals are as follows: ； in For the first One feature point, and These are the normal vector and intercept of the fitted plane, respectively; Step S35: Substitute the obtained covariance matrix, prior residual, and ICP residual into the problem of lidar odometry to obtain the initial pose sequence; The specific steps of step S22 are as follows: Step S221: Project the initial pose sequence onto the lidar coordinate system to obtain a local point cloud map; Step S222: Extract planar features based on the local point cloud map. ; Among them, planar features The definition is as follows: ； ； For DH parameters, Indicates the first A planar feature, This indicates taking the smallest eigenvalue of the matrix; Indicates the first A set of point clouds on a plane; Indicates the first The number of point clouds in a plane-shaped point cloud set. For the first The center of a point cloud on a plane, To represent the point cloud set of the j-th plane The i-th point cloud in; Step S223: Construct the objective function for obtaining the transformation matrix using the factor graph, where the objective function is as follows: ； For DH parameters, The state variables at time k include Rotation matrix at time step Translation at time k and speed , Zero bias estimate of gyroscope in inertial navigation at any time and accelerometer zero bias estimate ; This represents the number of planes fitted from the local point cloud map. The number of pose sequences output by the lidar odometry; The information matrix for inertial navigation pre-integration constraints. for Real-time status Inertial navigation pre-integral constraints for the state at any given time; Step S224: The Levenberg-Marquardt algorithm is used to solve the objective function to obtain the transformation matrix between the lidar and the first coordinate.

2. The parking assistance method based on lidar coordinate positioning according to claim 1, characterized in that, The preset coordinate symbols mentioned in step S1 are printed on the wall or on the vehicle barrier. The preset coordinate symbol is a square plane with red markings around its perimeter, and yellow and black markings for coordinate information on the left and right sides of the plane, respectively.

3. A parking assistance system based on lidar coordinate positioning, characterized in that, The parking assistance method based on lidar coordinate positioning according to any one of claims 1 to 2 includes a first coordinate acquisition module, a second coordinate acquisition module, and a prediction module; The first coordinate acquisition module is used to acquire the preset coordinate symbol of the parking space, and obtain the first coordinate of the parking space based on the coordinate symbol; The second coordinate acquisition module is used to acquire the second coordinate of the vehicle relative to the preset coordinate symbol at the next moment based on the lidar, and to obtain the vehicle's pose at the next moment based on the first coordinate and the second coordinate. The prediction module is used to acquire environmental information around the vehicle based on ultrasonic radar or a 360-degree imaging system. Determine if there is any overlap between the vehicle's position and the environmental information at the next moment. If so, issue an alarm.

4. A parking assistance system based on lidar coordinate positioning according to claim 3, characterized in that, The second coordinate acquisition module includes an input unit, a solution unit, and a transformation unit; The input unit is used to obtain the initial pose sequence via a lidar odometry system. The solving unit is used to construct an objective function based on the pre-integration information of the inertial navigation system and the plane constraint of the preset coordinate symbols, and uses the Levenberg-Marquardt algorithm to solve the objective function to obtain the transformation matrix between the lidar and the first coordinate. The transformation unit is used to transform the first coordinate into the second coordinate through a transformation matrix.

5. A parking assistance system based on lidar coordinate positioning according to claim 4, characterized in that, The input unit includes a receiving subunit, a question construction subunit, a propagation subunit, a residual word subunit, and a sequence acquisition subunit; The receiving subunit is used to receive a set of laser point cloud data and output the state variables corresponding to the point cloud data at each time step; The problem-building subunit is used to build the problem of lidar odometry; The propagation subunit is used to perform forward propagation of the covariance matrix; The residual word sub-unit is used to construct the prior residual and the ICP residual; The sequence acquisition subunit is used to substitute the acquired covariance matrix, prior residual, and ICP residual into the problem of lidar odometry to obtain the initial pose sequence.

6. A parking assistance system based on lidar coordinate positioning according to claim 5, characterized in that, The solution unit includes a local point cloud map acquisition subunit, a planar feature acquisition subunit, an objective function acquisition subunit, and a computation subunit; The local point cloud map acquisition sub-unit is used to project the initial pose sequence onto the lidar coordinate system to obtain a local point cloud map. The planar feature acquisition subunit is used to extract planar features based on a local point cloud map; The objective function acquisition sub-unit is used to construct the objective function for obtaining the transformation matrix through the factor graph; The computational subunit is used to solve the objective function using the Levenberg-Marquardt algorithm to obtain the transformation matrix between the lidar and the first coordinate.

Images