Parking assisting method and system based on laser radar coordinate positioning

Abstract

The invention relates to the technical field of vehicle auxiliary driving, in particular to a parking auxiliary method and system based on laser radar coordinate positioning. Comprising the following steps: S1, obtaining a preset coordinate symbol of a parking space, and obtaining a first coordinate of the parking space based on the coordinate symbol; s2, acquiring a second coordinate of the preset coordinate symbol relative to the vehicle at the next moment based on the laser radar, and acquiring the pose of the vehicle at the next moment based on the first coordinate and the second coordinate; s3, acquiring environment information around the vehicle based on an ultrasonic radar or a 360-degree imaging system; and judging whether the pose of the vehicle at the next moment is overlapped with the environment information or not, and if yes, giving an alarm. Through the dynamic fusion of the pose prediction of the laser radar and the environmental information, the collision pre-judgment is realized, and the pain points of frequent alarm and lack of predictability during parking in narrow parking spaces are solved. The safety and the comfort are improved, the parking efficiency is optimized, and the system is particularly suitable for the narrow indoor environment.

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 position;

[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] The state variables at time k are represented as follows:

[0019] in, It is the rotation matrix from the lidar coordinate system L at time k to the lidar odometer coordinate system LO. LO p k , LO v k , LO ω k These represent the position, velocity, and angular velocity of the lidar in the lidar odometry coordinate system LO at time k; n v and n ω 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, Let k be the set of all planes fitted to the point cloud at time k; For the j-th point in the plane set, the surface ICP residual is... Let j be the information matrix of the ICP at point j. Let the prior residual be at time k. Let be the information matrix of the prior constraints at time k;

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

[0024]

[0025] in, Let k be the set of all planes fitted to the point cloud at time k; For the j-th point in the plane set, the surface ICP residual is... Let j be the information matrix of the ICP at point j. Let the prior residual be at time k. Let be the information matrix of the prior constraints at time k;

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

[0027]

[0028] in, and Let F be the prior state covariance matrix at time k and time k-1, respectively, and let Q be the noise covariance matrix. k-1 The definition of G is as follows:

[0029]

[0030] Where Exp(·) is the exponential mapping in Lie algebras; Δt = t k -t k-1 J represents the time interval from time k-1 to time k; r (·) represents the right Jacobian function;

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

[0032]

[0033] Where Log(·) is the logarithmic mapping that converts the rotation matrix into a Lie algebra;

[0034] The specific ICP residuals are as follows:

[0035]

[0036] Where q j For the j-th feature point, n j and d jThese are the normal vector and intercept of the fitted plane, respectively;

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

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

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

[0040] Step S222: Extract planar features based on the local point cloud map, extracting planar feature γ. j (x);

[0041] Among them, the planar feature γ j The definition of (x) is as follows:

[0042]

[0043] x is the DH parameter, γ j (x) represents the j-th planar feature, λ min (·) indicates taking the smallest eigenvalue of the matrix; Let represent the point cloud set of the j-th plane; This represents the number of point clouds in the point cloud set of the j-th plane. Let j be the center of the point cloud on the j-th plane. M p i (x) represents the point cloud set of the j-th plane. The i-th point cloud in;

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

[0045]

[0046] x is the DH parameter. The state variables at time k include the rotation matrix at time k. Translation at time k LO p k and speed LO v k The zero bias estimate of the gyroscope in the inertial navigation system at time k and accelerometer zero bias estimate LO g represents the gravity in the LO frame of the lidar odometer. γ is the transformation matrix from the motor coordinate system M to the inertial navigation coordinate system I; m is the number of planes fitted from the local point cloud map; j(x)2 represents the second plane feature of the j-th plane, and n is the number of pose sequences output by the lidar odometry. The information matrix for inertial navigation pre-integration constraints. The inertial navigation pre-integration constraint is used to define the state from time k-1 to time k.

[0047] Step S224: Use the Levenberg-Marquardt algorithm to solve the objective function and obtain the transformation matrix between the lidar and the first coordinate position.

[0048] 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;

[0049] 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;

[0050] 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.

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

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

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

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

[0055] 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 symbol, and uses the Levenberg-Marquardt algorithm to solve the objective function to obtain the transformation matrix between the lidar and the first coordinate position.

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

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

[0058] 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;

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

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

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

[0062] 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.

[0063] 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;

[0064] 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.

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

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

[0067] 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 position.

[0068] 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

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

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

[0071] 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.

[0072] 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.

[0073] 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.

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

[0075] 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;

[0076] 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;

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

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

[0079] 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.

[0080] 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.

[0081] 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.

[0082] 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.

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

[0084] 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.

[0085] 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.

[0086] 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.

[0087] 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.

[0088] 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:

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

[0090] 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 position;

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

[0092] 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.

[0093] 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.

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

[0095] 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;

[0096] The state variables at time k are represented as follows:

[0097] in, It is the rotation matrix from the lidar coordinate system L at time k to the lidar odometer coordinate system LO. LO p k , LO vk , LO ω k These represent the position, velocity, and angular velocity of the lidar in the lidar odometry coordinate system LO at time k; n v and n ω These are Gaussian white noise corresponding to the velocity and rotation, respectively.

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

[0099]

[0100] in, Let k be the set of all planes fitted to the point cloud at time k; For the j-th point in the plane set, the surface ICP residual is... Let j be the information matrix of the ICP at point j. Let the prior residual be at time k. Let be the information matrix of the prior constraints at time k;

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

[0102]

[0103] in, Let k be the set of all planes fitted to the point cloud at time k; For the j-th point in the plane set, the surface ICP residual is... Let j be the information matrix of the ICP at point j. Let the prior residual be at time k. Let be the information matrix of the prior constraints at time k;

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

[0105]

[0106] in, and Let F be the prior state covariance matrix at time k and time k-1, respectively, and let Q be the noise covariance matrix. k-1 The definition of G is as follows:

[0107]

[0108] Where Exp(·) is the exponential mapping in Lie algebras; Δt = t k -t k-1 J represents the time interval from time k-1 to time k;r (·) represents the right Jacobian function;

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

[0110]

[0111] Where Log(·) is the logarithmic mapping that converts the rotation matrix into a Lie algebra;

[0112] The specific ICP residuals are as follows:

[0113]

[0114] Where q j For the j-th feature point, n j and d j These are the normal vector and intercept of the fitted plane, respectively;

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

[0116] 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.

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

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

[0119] Step S222: Extract planar features based on the local point cloud map, extracting planar feature γ. j (x);

[0120] Among them, the planar feature γ j The definition of (x) is as follows:

[0121]

[0122] x is the DH parameter, γ j (x) represents the j-th planar feature, λ min (·) indicates taking the smallest eigenvalue of the matrix; Let represent the point cloud set of the j-th plane; This represents the number of point clouds in the point cloud set of the j-th plane. Let j be the center of the point cloud on the j-th plane. M p i (x) represents the point cloud set of the j-th plane. The i-th point cloud in;

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

[0124]

[0125] x is the DH parameter. The state variables at time k include the rotation matrix at time k. Translation at time k LO p k and speed LO v k The zero bias estimate of the gyroscope in the inertial navigation system at time k and accelerometer zero bias estimate LO g represents the gravity in the LO frame of the lidar odometer. γ is the transformation matrix from the motor coordinate system M to the inertial navigation coordinate system I; m is the number of planes fitted from the local point cloud map; j (x)2 represents the second plane feature of the j-th plane, and n is the number of pose sequences output by the lidar odometry. The information matrix for inertial navigation pre-integration constraints. The inertial navigation pre-integration constraint is used to define the state from time k-1 to time k.

[0126] When constructing the objective function, the state at time k is constrained by the state at time k-1 to obtain 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.

[0127] Step S224: Use the Levenberg-Marquardt algorithm to solve the objective function and obtain the transformation matrix between the lidar and the first coordinate position.

[0128] 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;

[0129] 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;

[0130] 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.

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

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

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

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

[0135] 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 symbol, and uses the Levenberg-Marquardt algorithm to solve the objective function to obtain the transformation matrix between the lidar and the first coordinate position.

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

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

[0138] 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;

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

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

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

[0142] 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.

[0143] 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;

[0144] 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.

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

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

[0147] 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 position.

[0148] 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.

[0149] 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 laser radar coordinate-based parking assistance method characterized by comprising: The method comprises the following steps: Step S1: obtaining a preset coordinate symbol of a parking space, and obtaining a first coordinate of the parking space based on the coordinate symbol; Step S2: obtaining a second coordinate of the preset coordinate symbol relative to the vehicle at a next time based on a laser radar, and obtaining a vehicle pose at the next time based on the first coordinate and the second coordinate; Step S3: obtaining environmental information around the vehicle based on an ultrasonic radar or a 360 imaging system; determining whether the vehicle pose at the next time and the environmental information overlap, and issuing an alarm if they do.

2. The laser radar coordinate positioning based parking assistance method according to claim 1, wherein The preset coordinate symbol in step S1 is printed on a wall or a car stopper; The preset coordinate symbol is a square plane, and a red mark is arranged around the plane, and yellow and black coordinate information is marked on the left and right sides of the plane.

3. The laser radar coordinate positioning based parking assistance method according to claim 1, wherein In step S2, the second coordinate of the preset coordinate symbol relative to the vehicle at the next time is obtained as follows: Step S21: obtaining an initial pose sequence through a laser radar odometry; Step S22: constructing a target function based on pre-integrated information of inertial navigation and a plane constraint of the preset coordinate symbol, solving the target function by using a Levenberg-Marquardt algorithm, and obtaining a transformation matrix of the laser radar and the first coordinate; Step S23: converting the first coordinate into the second coordinate through the transformation matrix.

4. The laser radar coordinate-based parking assistance method according to claim 3, characterized by, In step S21, the initial pose sequence is obtained as follows: Step S211: receiving a set of laser point cloud data, and outputting a state variable corresponding to point cloud data at each time; where the state variable at the kth time instant is represented as follows: wherein, is the rotation matrix from the laser radar coordinate system L to the laser radar odometry coordinate system L0 at time k, LO p k , LO v k , LO ω k are the position, velocity, and angular velocity of the laser radar in the laser radar odometry coordinate system L0 at time k, respectively; n v and n ω are Gaussian white noises corresponding to the velocity and rotation, respectively. Step S212: constructing a laser radar odometry problem, wherein the laser radar odometry problem is as follows: wherein, is a set of all planes fitted from the point cloud at time k; is the jth point-plane ICP residual in the set of planes, is the information matrix for the jth point-plane ICP, is the prior residual at time k, is the information matrix for the prior constraint at time k; Step S32: constructing a laser radar odometry problem, wherein the laser radar odometry problem is as follows: wherein, is a set of all planes fitted from the point cloud at time k; is the jthpoint-plane ICP residual in the set of planes, is the information matrix for the jthpoint-plane ICP, is the prior residual at time k, is the information matrix for the prior constraint at time k; Step S213: performing forward propagation on a covariance matrix, and the covariance matrix is mathematically expressed as follows: wherein and are the prior state covariance matrices at time k and k-1, respectively, Q is the noise covariance matrix, and F k-1 are defined as follows: where Exp(·) is the exponential map in the Lie algebra; At = t k -t k-1 is the time interval from time k-1 to time k; J r (@) is the right Jacobian function; Step S214: constructing a priori residual and an ICP residual, wherein the a priori residual is specifically as follows: wherein Log(·) is a logarithmic mapping of a rotation matrix into Lie algebra; The ICP residual is specifically as follows: where q j is the jth feature point, n j and d j are the corresponding normal vector and intercept of the fitted plane, respectively. Step S35: substituting the obtained covariance matrix, the a priori residual, and the ICP residual into the laser radar odometry problem, and obtaining the initial pose sequence.

5. The laser radar coordinate-based parking assistance method according to claim 4, characterized by, The specific steps of step S22 are as follows: Step S221: projecting the initial pose sequence into a laser radar coordinate system to obtain a local point cloud map; Step S222: Extracting a planar feature based on the local point cloud map, extracting a planar feature γ j (x); wherein the planar feature γ j (x) is defined as follows: x is a DH parameter, γ j (x) represents the jth plane feature, λ min (·) represents taking the smallest eigenvalue of a matrix; represents the set of point clouds of the jth plane; represents the number of point clouds in the set of point clouds of the jth plane, is the center of the point clouds of the jth plane, M p i (x) is the ith point cloud in the set of point clouds of the jth plane ; Step S223: constructing a target function for obtaining the transformation matrix through a factor graph, wherein the target function is as follows: x is a DH parameter, is the state at time k, including the rotation matrix is the translation at time k LO p k and velocity LO v k is the gyroscope bias estimate at time k in the inertial navigation system is the accelerometer bias estimate at time k LO g is the gravity in the laser radar odometry coordinate system LO; is the transformation matrix from the motor coordinate system M to the inertial navigation coordinate system I; m is the number of planes fitted in the local point cloud map; γ j (x)2 represents the second plane feature of the jth plane, and n is the number of pose sequences output by the laser radar odometry; is the information matrix of the inertial navigation pre-integration constraint, is the inertial navigation pre-integration constraint from the state at time k-1 to the state at time k Step S224: solving the target function by using a Levenberg-Marquardt algorithm, and obtaining the transformation matrix of the laser radar and the first coordinate.

6. A laser radar coordinate-based parking assistance system characterized by comprising: The parking assistance method based on laser radar coordinate positioning according to any one of claims 1-5 comprises a first coordinate obtaining module, a second coordinate obtaining module, and a prediction module; The first coordinate obtaining module is used to obtain a preset coordinate symbol of a parking space, and obtain a 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.

7. A laser radar coordinate-based parking assistance system according to claim 6, wherein 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 symbol, and uses the Levenberg-Marquardt algorithm to solve the objective function to obtain the transformation matrix between the lidar and the first coordinate position. The transformation unit is used to transform the first coordinate into the second coordinate through a transformation matrix.

8. A laser radar coordinate-based parking assistance system according to claim 7, wherein 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.

9. A laser radar coordinate-based parking assist system according to claim 8, wherein 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 position.

Images