A wheel skidding detection and escape method and system for a wheeled chassis with multiple sensors

Abstract

The invention relates to the technical field of motor drive integrated control, and discloses a wheel skidding detection and escape method and system of a wheeled chassis with multiple sensors, wherein the compensation transformation matrix of an inertial measurement unit of a vehicle to be detected and a laser radar is calculated, the laser point cloud coordinates constructed by the laser radar are corrected according to the compensation transformation matrix, and a laser odometer is obtained based on the corrected laser point cloud; the inertial detection unit of the vehicle to be detected is corrected in combination with inertial pre-score calculation to obtain an inertial odometer, and then a wheel speed odometer is obtained based on a wheel speedometer combined with a navigation speed calculation; the laser odometer, the inertial odometer and the wheel speed odometer are fused in combination with a multi-sensor fusion positioning algorithm to obtain a fused odometer, and whether the vehicle is skidding is determined based on the wheel speed odometer and the fused odometer, and if skidding occurs, a differential is locked by a locker to get the vehicle out of trouble, thereby solving the problem that wheel skidding of existing unmanned vehicles cannot be accurately detected and corrected.

Description

Technical Field

[0001] The present invention relates to the technical field of motor drive integrated control, and in particular to a wheel slip detection and escape method and system. Background Art

[0002] Unmanned vehicles can navigate autonomously when driving outdoors, including self-positioning, route planning, surrounding perception, and body control. However, if the wheels of an unmanned vehicle slip during driving, the unmanned vehicle is prone to deviating or snaking if it cannot self-check and correct the error caused by the slip.

[0003] When the wheels of an unmanned vehicle slip, one wheel is suspended in the air or has little friction with the ground, while the other wheel cannot exert force. At this time, the differential needs to be locked to distribute synchronous speeds to the wheels on both sides, so that the vehicle still has good passing performance even if it slips. There are two existing solutions for detecting wheel slip:

[0004] 1. Use the gyroscope to detect the vehicle body status and use the gyroscope feedback signal to perform self-check and correction of errors;

[0005] 2. Analyze the speed difference between the driving wheel and the driven wheel based on the wheel speed meter of the unmanned vehicle's driven wheel, and based on this, determine whether it is slipping and make corrections.

[0006] However, the gyroscope solution, due to its own low accuracy, often has zero-point drift and is prone to cumulative errors, which require correction over a long period of time. A single gyroscope has a large random error, and a single gyroscope cannot effectively detect wheel slip, so multiple gyroscopes need to be installed. In the process of correction through a wheel speed meter, there is usually a differential on the chassis, which can easily cause inconsistent wheel speeds on the left and right sides of the driven wheel, bringing difficulties and errors to the accurate analysis of the rotational speed, and it is difficult to find a suitable position for the mechanical installation of the corresponding encoder on the axles or reducers of multiple wheels, and the reliability is not high. Summary of the invention

[0007] The present invention provides a wheel skidding detection and escape method and system of a wheeled chassis with multiple sensors, so as to solve the problem that wheel skidding of existing unmanned vehicles cannot be accurately detected and corrected.

[0008] In order to achieve the above object, the present invention is implemented by the following technical solutions:

[0009] In a first aspect, the present invention provides a wheel skidding detection and escape method for a wheeled chassis with multiple sensors, which is applied to a vehicle equipped with a gyroscope, a wheel speed meter, a differential, a locker for locking the differential, an inertial measurement unit (IMU) and a laser radar, and the method comprises the following steps:

[0010] Step 1: Calculate the compensation transformation matrix between the inertial measurement unit and the laser radar of the vehicle to be detected, correct the laser point cloud coordinates constructed by the laser radar according to the compensation transformation matrix, and obtain the laser odometer based on the corrected laser point cloud;

[0011] Step 2: Based on the inertial measurement data collected by the inertial measurement unit and the inertial estimated score calculation, the inertial detection unit of the vehicle to be detected is corrected to obtain the inertial odometer, and then the wheel speed odometer is obtained based on the wheel speedometer and the speed calculation;

[0012] Step 3: Combine the laser odometer, inertial odometer and wheel speed odometer with the multi-sensor fusion positioning algorithm to obtain a fused odometer. Determine whether the vehicle is skidding based on the wheel speed odometer and the fused odometer. If skidding occurs, lock the differential through the lock to free the vehicle.

[0013] Furthermore, before calculating the compensation transformation matrix in step 1, the inertial measurement unit and the laser radar need to be synchronized;

[0014] The synchronization includes: adjusting the timestamp of one frame of the laser point cloud of the laser radar and the timestamp of the inertial data acquired by the inertial measurement unit most recently to be less than a predetermined threshold.

[0015] Furthermore, the compensation transformation matrix is ​​calculated by the following steps:

[0016] Step 101: construct an inertial measurement coordinate system according to the inertial measurement data obtained by the inertial measurement unit, and obtain the inertial measurement coordinate system , , The Euler angles of the axes relative to the world coordinate system and , , The acceleration of the axis and removing the effect of gravity from the acceleration;

[0017] Step 102: Based on the Euler angles and acceleration, the displacement data and velocity data corresponding to the inertial measurement data in the world coordinate system are obtained, and the displacement data and velocity data are interpolated. Based on the displacement data and velocity data after the interpolation processing, each laser point in the laser point cloud of the laser radar is corrected, and the compensation transformation matrix of each laser point relative to the laser point at the starting point scanning time is calculated.

[0018] Furthermore, the coordinate correction of the laser point cloud constructed by the laser radar according to the compensation transformation matrix includes: multiplying the coordinates of each laser point in the laser point cloud by the compensation transformation matrix.

[0019] Furthermore, the step of obtaining the laser odometer based on the corrected laser point cloud includes the following steps:

[0020] Step 103: extracting features from the corrected laser point cloud to obtain point features and line features, and constructing a pose based on the point features and line features, and converting the pose into a global coordinate system to construct a local map;

[0021] Step 104: Match the point cloud data of the laser radar with the local map, calculate the distance between the line feature and the line feature in the local map by using the least squares method, and obtain the laser odometer.

[0022] Furthermore, the step of acquiring the inertial odometer specifically includes: constructing a deviation queue and an inertial measurement queue based on the inertial measurement data received by the inertial measurement unit, calculating the noise deviation of the deviation queue, adding the noise deviation to the inertial measurement queue, and acquiring the inertial odometer.

[0023] Furthermore, the method of obtaining a wheel speed odometer based on the wheel speedometer combined with the navigation speed includes: obtaining the radius of the vehicle tire and the cumulative number of pulses obtained by the differential when the vehicle tire rotates one circle, and calculating the pulse distance based on the tire radius and the cumulative number of pulses, differentiating the time required for the vehicle to rotate one circle to obtain the vehicle speed, and obtaining the wheel speed odometer based on the vehicle speed combined with the heading angle of the inertial measurement unit.

[0024] Furthermore, the multi-sensor fusion positioning algorithm includes a factor graph tight coupling algorithm, an EKF filtering algorithm and an ESKF loose coupling algorithm.

[0025] In a second aspect, the present invention provides a wheel slip detection and escape system for a wheeled chassis with multiple sensors, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of any of the above methods when executing the computer program.

[0026] Beneficial effects:

[0027] The present invention provides a wheel skidding detection and escape method and system for a wheeled chassis with multiple sensors. By combining a laser odometer, an inertial odometer and a wheel speed odometer with a positioning algorithm to obtain a fused odometer, a more stable and reliable odometer for vehicle body motion control can be obtained, so that the vehicle body can be better controlled during skidding, and the deviation and serpentine walking of the vehicle body can be effectively improved.

[0028] In addition, through the detection and control of slippage combined with the automatic control of the underlying differential lock linkage, the chassis can escape from trouble on its own in unmanned scenarios, and the bottom plate has stronger chassis adaptability. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] Figure 1 The flowchart of obtaining the fused odometer in the present invention. DETAILED DESCRIPTION

[0030] The technical solution of the present invention is described clearly and completely below. Obviously, the described embodiments are only a part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0031] Unless otherwise defined, the technical terms or scientific terms used in the present invention shall have the usual meanings understood by persons with ordinary skills in the field to which the present invention belongs. The words "first", "second" and similar words used in the present invention do not indicate any order, quantity or importance, but are only used to distinguish different components. Similarly, words such as "one" or "one" do not indicate quantity restrictions, but indicate the existence of at least one. Words such as "connect" or "connected" are not limited to physical or mechanical connections, but may include electrical connections, whether direct or indirect. "Up", "down", "left", "right" and the like are only used to indicate relative positional relationships. When the absolute position of the object being described changes, the relative positional relationship also changes accordingly.

[0032] See also Figure 1 ,exist Figure 1 In the figure, odometer 1 refers to the laser odometer, odometer 2 refers to the inertial odometer, and odometer 3 refers to the wheel speed odometer;

[0033] The embodiment of the present application provides a wheel skidding detection and escape method of a wheeled chassis with multiple sensors, which is applied to a vehicle equipped with a gyroscope, a wheel speed meter, a differential, a lock for locking the differential, an inertial measurement unit, and a laser radar. The method comprises the following steps:

[0034] Step 1: Synchronize the inertial measurement unit of the vehicle to be detected with the laser radar, calculate the compensation transformation matrix between the inertial measurement unit of the vehicle to be detected and the laser radar, correct the laser point cloud coordinates constructed by the laser radar according to the compensation transformation matrix, and obtain the laser odometer based on the corrected laser point cloud;

[0035] Since laser scanning usually takes one week , the interval time is The vehicle body will produce motion distortion during movement, and the frequency of the inertial measurement unit can reach Even higher, so an inertial measurement unit can be used to compensate for motion distortion of the laser point cloud;

[0036] Specifically, the timestamp of one frame of the laser point cloud of the laser radar is adjusted to be smaller than a predetermined threshold value with respect to the timestamp of the inertial data acquired by the most recent inertial measurement unit;

[0037] Step 1 specifically includes the following steps:

[0038] Step 101: Construct an inertial measurement coordinate system based on the inertial measurement data obtained by the inertial measurement unit, and obtain the inertial measurement coordinate system , , The Euler angles of the axes relative to the world coordinate system and , , The acceleration of the axis and remove the effect of gravity from the acceleration;

[0039] Step 102: based on the Euler angle and the acceleration, the displacement data and the velocity data corresponding to the inertial measurement data in the world coordinate system are obtained, the displacement data and the velocity data are interpolated, and each laser point in the laser point cloud of the laser radar is corrected based on the displacement data and the velocity data after the interpolation processing, and the compensation transformation matrix of each laser point relative to the laser point at the scanning time of the starting point is calculated;

[0040] Step 103: multiplying the coordinates of each laser point in the laser point cloud by the compensation transformation matrix to obtain a corrected laser point cloud;

[0041] Step 104: extracting features from the corrected laser point cloud to obtain point features and line features, and constructing a pose based on the point features and line features, and converting the pose into a global coordinate system to construct a local map;

[0042] Specifically, feature extraction is performed on each frame of point cloud, and the extracted features include line features and plane features. The frame point cloud line feature is , the surface feature is . Then it matches the Frame time, history exists The locally accumulated line features are The accumulated surface features are ; The corresponding accumulated posture is ;

[0043] For any historical point cloud frame and corresponding pose, the point cloud can be converted to the global coordinate system of the map:

[0044] ;

[0045] in, Refers to the coordinates in global coordinates, Refers to the map coordinate system; Refers to a point cloud of any history; and It's a posture The rotation and translation vectors in ;

[0046] Step 105: Match the point cloud data of the laser radar with the local map, calculate the distance between the line feature and the line feature in the local map by using the least squares method, and obtain the laser odometer.

[0047] Find the line feature corresponding to the line feature of the current frame in the local map. The line feature is represented by a point set. The first Frame to The relative pose of the frame can be The frame is converted to the historical map coordinate system. At this time, the inaccurate posture is obtained by inertial estimation, and the first The frame is in the odometer coordinate system, and the edge line feature set is calculated , point Recorded in The point at a moment belongs to the edge set at that moment One of the edge points.

[0048] The local map centerline feature set has been accumulated through the odometer ;by Points in the frame The center is searched in the map by nearest neighbor distance Find the closest point , recorded as , with a point Find the adjacent line bundles (taking 16-line LiDAR as an example) that belong to the point cloud set point l. Because point For the The point of the line feature in the frame, passing through the point The nearest distance to the search map is obtained and point l, so and If the same line feature is obtained by radar scanning at different times, then calculate arrive The distance of the line segment passing through the point , The lines that make up the line feature The corresponding straight line in can be transformed into a point To the passing point , The straight line distance.

[0049] The distance formula is as follows:

[0050] ;

[0051] A similar idea can be used to calculate the distance from a point in the feature surface to the plane in the local map. The calculation formula is as follows:

[0052] The formula for the unit normal vector of a plane

[0053] ;

[0054] ;

[0055] in, represents the unit normal vector of the plane;

[0056] The distance from the point to the extracted feature plane is:

[0057] ;

[0058] in, Represents the distance from the point to the extracted feature plane;

[0059] The distances from point to line and point to surface are obtained. The goal is to optimize these distances. There are currently 6 unknowns. ,in, Indicates that in the global coordinate system relative to The amount of translation of the axis; Indicates Frame In the global frame coordinate system, relative to The angle of the axis, that is, Frame to The relative pose of the frame;

[0060] Optimize the solution through the least squares method and optimize the use Algorithm or The algorithm is iteratively solved, and the unknown quantity is , the objective function is the distance from a point to a line or to a plane, and each step finds Make the goal Finally, the optimization solution is , that is, the pose of the current frame is obtained to obtain the laser odometer;

[0061] Step 2: Based on the inertial measurement data collected by the inertial measurement unit and the inertial estimated score calculation, the inertial detection unit of the vehicle to be detected is corrected to obtain the inertial odometer, and then the wheel speed odometer is obtained based on the wheel speedometer and the speed calculation;

[0062] Specifically, a deviation queue and an inertial measurement queue are constructed based on the inertial measurement data received by the inertial measurement unit, a noise deviation of the deviation queue is calculated, the noise deviation is added to the inertial measurement queue, and the inertial odometer is obtained;

[0063] The wheel speed odometer is obtained based on the wheel speed meter combined with the navigation speed, including: obtaining the radius of the vehicle tire and the cumulative number of pulses obtained by the differential when the vehicle tire rotates one circle, and calculating the pulse distance based on the tire radius and the cumulative number of pulses, differentiating the time required for the vehicle to rotate one circle to obtain the vehicle speed, and obtaining the wheel speed odometer based on the vehicle speed combined with the heading angle of the inertial measurement unit.

[0064] Step 3: Combine the laser odometer, inertial odometer and wheel speed odometer with the multi-sensor fusion positioning algorithm to obtain a fused odometer. Determine whether the vehicle is skidding based on the wheel speed odometer and the fused odometer. If skidding occurs, lock the differential through the lock to free the vehicle.

[0065] After obtaining the fused odometer, if the wheel speed odometer indicates that the wheels are rotating, but the fused odometer indicates that the vehicle is not moving, it is determined that slippage has occurred, and the differential is locked by the locker to free the vehicle.

[0066] In the present application, the EKE filtering algorithm is used to fuse the laser odometer, the inertial odometer and the wheel speed odometer to obtain a fused odometer. In other embodiments, the fused odometer can also be obtained through the factor graph tight coupling algorithm and the ESKF loose coupling algorithm.

[0067] An embodiment of the present invention also provides a wheel slip detection and escape system for a wheeled chassis with multiple sensors, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the steps of the method in the above embodiment are implemented.

[0068] The preferred specific embodiments of the present invention are described in detail above. It should be understood that a person skilled in the art can make many modifications and changes based on the concept of the present invention without creative work. Therefore, any technical solution that can be obtained by a person skilled in the art through logical analysis, reasoning or limited experiments based on the concept of the present invention on the basis of the prior art should be within the scope of protection determined by the claims.

Claims

1. A wheel skidding detection and escape method for a wheeled chassis with multiple sensors, characterized in that: Applied to a vehicle equipped with a gyroscope, a wheel speed meter, a differential, a locker for locking the differential, an inertial measurement unit, and a laser radar, the method comprises the following steps: Step 1: Calculate the compensation transformation matrix between the inertial measurement unit and the laser radar of the vehicle to be detected, correct the laser point cloud coordinates constructed by the laser radar according to the compensation transformation matrix, and obtain the laser odometer based on the corrected laser point cloud; The compensation transformation matrix is ​​calculated by the following steps: Step 101: construct an inertial measurement coordinate system according to the inertial measurement data obtained by the inertial measurement unit, and obtain the inertial measurement coordinate system , , The Euler angles of the axes relative to the world coordinate system and , , The acceleration of the axis and removing the effect of gravity from the acceleration; Step 102: based on the Euler angle and the acceleration, the displacement data and the velocity data corresponding to the inertial measurement data in the world coordinate system are obtained, the displacement data and the velocity data are interpolated, each laser point in the laser point cloud of the laser radar is corrected based on the displacement data and the velocity data after the interpolation processing, and the compensation transformation matrix of each laser point relative to the laser point at the scanning time of the starting point is calculated; Step 2: Based on the inertial measurement data collected by the inertial measurement unit and the inertial estimated score calculation, the inertial detection unit of the vehicle to be detected is corrected to obtain the inertial odometer, and then the wheel speed odometer is obtained based on the wheel speedometer and the speed calculation; Step 3: Combine the laser odometer, inertial odometer and wheel speed odometer with the multi-sensor fusion positioning algorithm to obtain a fused odometer. Determine whether the vehicle is skidding based on the wheel speed odometer and the fused odometer. If skidding occurs, lock the differential through the lock to free the vehicle.

2. The wheel skid detection and escape method of a wheeled chassis with multiple sensors according to claim 1, characterized in that: Before calculating the compensation transformation matrix in step 1, the inertial measurement unit and the laser radar need to be synchronized; The synchronization includes: adjusting the timestamp of one frame of the laser point cloud of the laser radar and the timestamp of the inertial data acquired by the inertial measurement unit most recently to be less than a predetermined threshold.

3. The wheel skid detection and escape method of a wheeled chassis with multiple sensors according to claim 1, characterized in that: The coordinate correction of the laser point cloud constructed by the laser radar according to the compensation transformation matrix includes: multiplying the coordinates of each laser point in the laser point cloud by the compensation transformation matrix.

4. The wheel skid detection and escape method of a wheeled chassis with multiple sensors according to claim 3, characterized in that: The method of obtaining the laser odometer based on the corrected laser point cloud comprises the following steps: Step 103: extracting features from the corrected laser point cloud to obtain point features and line features, and constructing a pose based on the point features and line features, and converting the pose into a global coordinate system to construct a local map; Step 104: Match the point cloud data of the laser radar with the local map, calculate the distance between the line feature and the line feature in the local map by using the least squares method, and obtain the laser odometer.

5. The wheel slip detection and escape method of a wheeled chassis with multiple sensors according to any one of claims 1 to 4, characterized in that: The step of acquiring the inertial odometer specifically includes: constructing a deviation queue and an inertial measurement queue based on inertial measurement data received by an inertial measurement unit, calculating a noise deviation of the deviation queue, adding the noise deviation to the inertial measurement queue, and acquiring the inertial odometer.

6. The wheel skid detection and escape method of a wheeled chassis with multiple sensors according to any one of claims 1 to 4, characterized in that: The method of obtaining the wheel speed odometer based on the wheel speedometer combined with the navigation speed includes: obtaining the radius of the vehicle tire and the cumulative number of pulses obtained by the differential when the vehicle tire rotates one circle, and calculating the pulse distance based on the tire radius and the cumulative number of pulses, differentiating the time required for the vehicle to rotate one circle to obtain the vehicle speed, and obtaining the wheel speed odometer based on the vehicle speed combined with the heading angle of the inertial measurement unit.

7. The wheel slip detection and escape method of a wheeled chassis with multiple sensors according to any one of claims 1 to 4, characterized in that: The multi-sensor fusion positioning algorithm includes a factor graph tight coupling algorithm, an EKF filtering algorithm and an ESKF loose coupling algorithm.

8. A wheel skid detection and escape system for a wheeled chassis with multiple sensors, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of the method described in any one of claims 1 to 7 are implemented.

Images