Step obstacle passability prediction and climbing posture adjustment system and method

Abstract

The application discloses a kind of step-shaped obstacle passability prediction and climbing posture adjustment system and method, adjustment system includes perception layer, decision layer and execution layer, wherein the output end of perception layer is connected with the input end of decision layer, the output end of decision layer is connected with the input end of execution layer, APA ultrasonic radar is arranged in parallel in perception layer, RGB-D depth camera and CAN main line are arranged in parallel in perception layer, ECU control unit is arranged in decision layer, and central control screen, steering ware, electronic throttle pedal, electronic brake pedal and voice prompt module are arranged in parallel in execution layer, and its method is: first, arrange RGB-D depth camera;Second, arrange APA ultrasonic radar;Third, algorithm runs;Fourth, actuating mechanism operates;Beneficial effect: it avoids the climbing failure and the serious damage to wheel and other related components due to the too large height judgment error of step-shaped obstacle and the too small steering adjustment interval.

Description

Technical Field

[0001] This invention relates to a system and method for predicting traversability and adjusting posture, and more particularly to a system and method for predicting traversability and adjusting climbing posture for step-shaped obstacles. Background Technology

[0002] Currently, with the development of the national economy and the gradual improvement of people's living standards, automobiles have become widely popular, and the number of cars is growing rapidly. While driving, drivers sometimes encounter step-like obstacles of a certain height. Drivers may damage the car's chassis, bumpers, and other components due to inaccurate judgment of the obstacle's height or improper operation. Taking a side obstacle like a road shoulder as a typical case analysis, when encountering a low step or road shoulder, drivers can roughly estimate whether they can pass based on experience and choose whether to climb it. However, when encountering a high step or road shoulder, drivers may not be able to accurately judge whether they can pass. If the judgment is wrong, it will damage car parts. Therefore, a step-like obstacle passability prediction system is needed to help drivers make judgments. Besides road shoulders, step-like obstacles are also sometimes encountered on unstructured roads in rural areas, where a step-like obstacle passability prediction system is also needed.

[0003] Climbing step-like obstacles can cause damage to the wheels and other related components. In order to minimize the damage to the wheels and other related components, the driver needs to have rich experience in climbing. Simply relying on the driver to visually observe the height of the step-like obstacle and arbitrarily choose the climbing posture is obviously not enough to complete the climbing task efficiently while minimizing the damage to the wheels and other components. Summary of the Invention

[0004] The purpose of this invention is to solve the problem of how to achieve efficient climbing and minimize damage to tires and other related components during driving, and to provide a system and method for predicting the passability of step-shaped obstacles and adjusting climbing posture.

[0005] The step-shaped obstacle passability prediction and climbing posture adjustment system provided by this invention includes a perception layer, a decision layer, and an execution layer. The output of the perception layer is connected to the input of the decision layer, and the output of the decision layer is connected to the input of the execution layer. An APA ultrasonic radar, an RGB-D depth camera, and a CAN bus are arranged in parallel within the perception layer. An ECU control unit is located within the decision layer. A central control screen, a steering gear, an electronic throttle pedal, an electronic brake pedal, and a voice prompt module are arranged in parallel within the execution layer. The APA ultrasonic radar, RGB-D depth camera, and CAN bus are all connected to the ECU control unit. The APA ultrasonic radar, RGB-D depth camera, and CAN bus can transmit the collected data to the ECU control unit in real time. The ECU control unit is connected to the central control screen, steering gear, electronic throttle pedal, electronic brake pedal, and voice prompt module, respectively. The ECU control unit controls the operation of the central control screen, steering gear, electronic throttle pedal, electronic brake pedal, and voice prompt module based on the data transmitted from the APA ultrasonic radar, RGB-D depth camera, and CAN bus.

[0006] There are two APA ultrasonic radars, one located on the side of the vehicle. The model of the APA ultrasonic radar is AJ-SR04M ultrasonic radar. The APA ultrasonic radar is connected to the vehicle's central control screen via the vehicle bus. The APA ultrasonic radar can display the distance to step-like obstacles in real time and has a voice broadcast function. The detection range of the APA ultrasonic radar is between 30-500cm.

[0007] There are two RGB-D depth cameras. The model of the RGB-D depth camera is Intel RealSense D455RGB-D Depth Camera. The RGB-D depth camera can monitor the height of step-like obstacles in front and to the side in real time, and can also monitor the angle between the normal of the vehicle plane and the normal of the obstacle plane.

[0008] The CAN main line, ECU control unit, central control screen, steering gear, electronic throttle pedal, electronic brake pedal and voice prompt module mentioned above are all assemblies of existing equipment, therefore, the specific models and specifications are not described in detail.

[0009] The working principle of the step-shaped obstacle passability prediction and climbing posture adjustment system provided by this invention is as follows:

[0010] First, the height of the step-like obstacle on the side of the road is detected using an RGB-D depth camera. A passability prediction algorithm compares the height of the step-like obstacle to the vehicle's minimum ground clearance. If the height of the step-like obstacle is less than the vehicle's minimum ground clearance, the next step can be determined. Then, the height of the step is compared to the height that the vehicle's front wheels can overcome per unit diameter to determine the success rate of climbing. Next, APA ultrasonic radar detects the lateral distance between the vehicle and the step-like obstacle. If the distance is sufficient to allow the wheels to adjust their direction, the ECU control unit controls the wheel steering to decelerate the vehicle at approximately a 45° angle towards the step-like obstacle. The system detects obstacles and then uses an RGB-D depth camera to measure the angle between the vehicle's plane normal and the plane normal of the step-shaped obstacle. The sum of this angle and the front wheel steering angle is set to 90°, allowing the wheels to make perpendicular contact with the step-shaped obstacle as possible. When a single wheel makes perpendicular contact with the step-shaped obstacle, it sends a signal to the ECU control unit. The ECU control unit then controls the throttle to slowly accelerate and climb the obstacle. When the wheel's driving force reaches the set driving force just enough to lift it off the ground, it is determined that the driving force is sufficient for the wheel to climb, and the throttle is stabilized. Once the inner front wheel has successfully climbed, the speed is reduced to low. The outer front wheel climbs in the same way. The system sets the distance the outer front wheel climbs to be equal to the radius of the wheel itself as a signal that all front wheels have successfully climbed. At this point, the front wheels are raised by the height of the obstacle relative to their previous height. Once the RGB-D depth camera detects that all front wheels have successfully climbed, the system begins to adjust the rear wheel attitude. The system uses image recognition to control the front wheel angle for calibration, ensuring that the angle between the vehicle plane normal and the obstacle plane normal is 90°. After the rear wheels make perpendicular contact with the obstacle, the system maintains a stable throttle when the driving force for the rear wheels reaches the set value. Once the rear wheels have successfully climbed, the system terminates the step obstacle passability prediction and climbing attitude adjustment.

[0011] There are two APA ultrasonic radars in the perception layer. The model of the APA ultrasonic radar is AJ-SR04M ultrasonic radar. The APA ultrasonic radar is connected to the vehicle's central control screen through the vehicle bus. The APA ultrasonic radar can display the distance to the step-shaped obstacle in real time and has a voice broadcast function.

[0012] There are two RGB-D depth cameras in the perception layer. The model of the RGB-D depth camera is Intel RealSense D455RGB-D depth camera. The RGB-D depth camera can monitor the height of step-like obstacles in front and to the side in real time, and can also monitor the angle between the normal of the vehicle plane and the normal of the obstacle plane.

[0013] When the ECU control unit in the decision-making layer is working, it first processes the height information of the stepped obstacle transmitted by the RGB-D depth camera. If the height of the stepped obstacle is less than the minimum ground clearance and the success rate of the passability prediction exceeds the set critical success rate, it then processes the lateral distance information between the vehicle and the stepped obstacle transmitted by the APA ultrasonic radar. If the distance is too small, the voice prompt module will issue a voice alarm to remind the driver that the distance is too narrow; if the distance is within a suitable range, the ECU control unit controls the steering gear and electronic brake pedal in the execution layer to decelerate the vehicle at about 45° and approach the stepped obstacle. The maximum initial speed when the vehicle starts executing the system is 5 km / h. Then, based on the RGB-D depth camera, the angle between the vehicle's plane normal and the obstacle's plane normal is detected. The sum of the angle and the front wheel rotation angle is controlled to be 90°, so that the wheels make perpendicular contact with the stepped obstacle as much as possible. When the inner front wheel makes perpendicular contact with the stepped obstacle, it sends a signal to the ECU control unit. The ECU control unit controls the throttle to slowly accelerate and climb up. When the wheel driving force reaches the set driving force that is just enough to lift off the ground, it is determined that the driving force is sufficient for the wheel to climb. The throttle is stabilized. After the inner front wheel successfully climbs up, the speed is reduced to low. The outer front wheel climbs up in the same way. The system sets the distance the outer front wheel climbs to be equal to the radius of the wheel itself as a signal that all front wheels have successfully climbed. At this point, the front wheels are raised by the height of the obstacle relative to their previous height. Once the RGB-D depth camera detects that all front wheels have successfully climbed, the system begins to adjust the rear wheel attitude. The system uses image recognition to control the front wheel angle for calibration, ensuring that the angle between the vehicle plane normal and the obstacle plane normal is 90°. After the rear wheels make perpendicular contact with the obstacle, the system maintains a stable throttle when the driving force for the rear wheels reaches the set value. Once the rear wheels have successfully climbed, the system terminates the step obstacle passability prediction and climbing attitude adjustment.

[0014] The method for predicting the passability of stepped obstacles and adjusting climbing posture provided by the present invention includes the following steps:

[0015] The first step is to set up an RGB-D depth camera. The specific steps are as follows:

[0016] Step 1: Place the RGB-D depth camera on the rear side of the base plate, close to both sides of the vehicle body. Install two RGB-D depth cameras on the base plate to monitor the angle between the vehicle body plane normal and the obstacle plane normal, as well as the obstacle height.

[0017] Step 2: Connect the CAN bus interfaces of the two RGB-D depth cameras to the vehicle's bus and connect the power cables;

[0018] Step 3: The RGB-D depth camera transmits the height of the stepped obstacle and the angle between the vehicle's plane normal and the obstacle's plane normal to the perceived height of the stepped obstacle via the vehicle's CAN bus to the main vehicle and connects to the in-vehicle display screen. The main vehicle then obtains the height of the stepped obstacle on the side and the angle between the vehicle's plane normal and the obstacle's plane normal.

[0019] The second step is to deploy the APA ultrasonic radar, and the specific steps are as follows:

[0020] Step 1: Place the two APA ultrasonic radars on both sides of the vehicle body, symmetrically.

[0021] Step 2: Connect the CAN bus interfaces of the two APA ultrasonic radars to the vehicle's bus and connect the power cables;

[0022] Step 3: The APA ultrasonic radar transmits the distance information between the vehicle and the stepped obstacle on the side to the host vehicle via the vehicle's CAN bus. The host vehicle then obtains the relative status data between the vehicle and the stepped obstacle on the side.

[0023] The third step is to run the algorithm, and the specific steps are as follows:

[0024] The RGB-D depth camera uses OpenCV's YOLOv3 algorithm to detect objects and obtain bounding boxes, which are then processed to obtain the obstacle height. The RGB-D depth camera's plane detection algorithm uses the FAST algorithm to extract feature points through the OpenCV module, and the RANSAC algorithm to fit the plane. The angle between the front wheel rotation center plane and the obstacle plane is converted into the sum of the angle between the front wheel rotation angle and the normal of the vehicle plane and the normal of the obstacle plane. Therefore, the angle between the front wheel rotation center plane and the obstacle plane is g1.

[0025] g1 = g2 + α

[0026] In the above formula, g2 is the angle between the normal to the vehicle plane and the normal to the obstacle plane; α is the front wheel steering angle;

[0027] The RANSAC algorithm fits the plane equation as follows:

[0028] Ax + By + Cz + D = 0

[0029]

[0030] In the above formula, The normal vectors of the fitted plane; A, B, C, and D are the coefficients of the fitted plane equation obtained by the actual execution of the RANSAC algorithm;

[0031] The passability prediction algorithm initializes with the vehicle's minimum ground clearance, wheel diameter, distance from the center of gravity to the front and rear axles, wheelbase, and real-time road adhesion coefficient. The main vehicle ECU control unit processes the height and spacing data of the lateral step-like obstacles obtained in the first two steps according to the designed algorithm. The algorithm compares the vehicle's minimum ground clearance with the height of the step-like obstacle, then calculates the ratio of the step-like obstacle height to the diameter of the vehicle's front wheels, and then compares this ratio with the height of the step-like obstacle that can be overcome per unit wheel diameter to determine the success rate of climbing. The larger the difference between this ratio and the height of the step-like obstacle that can be overcome per unit wheel diameter, the higher the success rate. Critical success rates are set at 20% and 80%. Below 20%, climbing is abandoned; above 20% but below 80%, attitude adjustment is performed; above 80%, the vehicle passes directly without attitude adjustment. The height H of the step-like obstacle that can be overcome per unit wheel diameter of the front wheels is:

[0032]

[0033] h w D is the height of the stepped obstacle; D is the diameter of the front wheel. ρ is the adhesion coefficient; α is the distance from the center of mass to the front axle; L is the wheelbase.

[0034] The success rate of climbing is C:

[0035]

[0036] The steering angle α of a car refers to the angle formed by the centerline when the front wheels of a car are turned to their extreme left or right positions without deflection. The steering angle α of a car is between 30 and 40 degrees. The actual lateral clearance is compared with the minimum lateral clearance to determine if the clearance is appropriate. The theoretical minimum lateral clearance is s1, and the actual minimum lateral clearance is s:

[0037]

[0038] s = s1 + s2 + s3

[0039] In the above formula, v is the minimum vehicle speed when it contacts the stepped obstacle; t1 is the time it takes for the front wheels to reach perpendicular contact with the stepped obstacle; s1 is the theoretical minimum lateral clearance; s2 is the error distance affected by different vehicle wheel parameters; and s3 is the error distance affected by different initial vehicle speeds.

[0040] The front wheels begin climbing the obstacle. A critical driving force is set as the throttle control signal when the wheels just leave the ground. The critical driving force F1 at this point is calculated. At this point, the throttle is sufficient for the car to climb the stepped obstacle. The throttle is maintained to continue climbing. After successful climbing, the speed is reduced to low. The set front wheel contact surface pressure value is N, and the critical driving force F1 is:

[0041]

[0042]

[0043] In the above formula, m represents the load borne by a single wheel; The adhesion coefficient at the sharp corner of the step is denoted as , where Greater than the road adhesion coefficient β is the angle between the line connecting the point of contact between the wheel and the step-shaped obstacle and the center of the wheel and the line perpendicular to the ground passing through the center of the wheel;

[0044] The signal is set that the second front wheel crosses the stepped obstacle by one wheel radius as a successful climb signal for all front wheels, and the rear wheels begin to adjust their attitude. At this point, the lateral distances between the center and the foremost point of the inner rear wheel and the stepped obstacle are S′ and S′, respectively. r :

[0045] S′=L cos α2-L1 sin α2

[0046] S r =L cos α2-L1 sin α2-(R sin α2-L2 cos a2)tan α2

[0047] In the above formula, L is the wheelbase; L1 is the distance between the rear wheel centers; α2 is the steering angle of the outer front wheel; R is the wheel radius; and L2 is the wheel width.

[0048] When the rear wheel is climbing, the relationship between the change in the rear wheel center plane angle and the front wheel angle is as follows: (The front wheel is adjusted to a suitable turning angle and maintains a low-speed, uniform circular motion.)

[0049]

[0050] In the above formula, θ1 and θ2 are the angles that change after the inner front and rear wheels move, respectively; v f t is the speed of the inner front wheel; t is the time it takes for the rear wheel to adjust to be perpendicular to the obstacle section; α1 is the turning angle of the inner front wheel;

[0051] The rear wheels are adjusted to travel a lateral distance L3 perpendicular to the plane of the obstacle, while satisfying the following relationship to prevent insufficient lateral adjustment clearance of the rear wheels:

[0052] L3+R≤S′

[0053] Step 4: Operation of the implementing agency, the specific steps are as follows:

[0054] After data processing in steps one and three, based on the comparison results, the ECU control unit controls the voice prompt module or electronic brake pedal and steering gear to start working. The main vehicle uses an RGB-D depth camera to detect the height of the step-shaped obstacle. The data is processed by the ECU control unit to determine the success rate of climbing the step-shaped obstacle. If the height of the step-shaped obstacle is greater than or equal to the minimum ground clearance, the success rate is 0, and the voice prompt module reminds the driver to give up climbing. If the height of the step-shaped obstacle is less than the minimum ground clearance, the passability prediction algorithm calculates the climbing success rate. When the success rate is ≥20% and the lateral distance is appropriate, the steering gear and electronic brake pedal work together to approach the step-shaped obstacle. When the success rate is <20%, the voice prompt module reminds the driver to give up climbing. When the success rate is ≥20% and the lateral distance is too small, the voice prompt module reminds the driver to safely adjust the distance. When the success rate is ≥80%, no adjustment of the climbing posture is required.

[0055] Step Two: During the climbing posture adjustment, the angle between the vehicle's plane normal and the plane normal of the stepped obstacle, detected by the RGB-D depth camera, is controlled to be 90°, with the sum of the angle and the front wheel rotation angle. This ensures the wheels make as perpendicular contact as possible with the stepped obstacle. When a single wheel makes perpendicular contact with the obstacle, a signal is sent to the ECU control unit. The ECU control unit then controls the throttle to slowly accelerate and climb. When the wheel's driving force reaches the set point just enough to lift it off the ground, it is determined that the driving force is sufficient for the wheel to climb. The throttle is then maintained. Once the inner front wheel has successfully climbed, the speed is reduced to low, and the outer front wheel begins its climbing motion. The method is the same. The outer front wheel climbs a distance equal to the radius of the wheel as a signal that the front wheels have successfully climbed. At this time, the front wheels are raised by the height of the obstacle relative to the front wheels. When the RGB-D depth camera detects that the front wheels have successfully climbed, it starts to adjust the attitude of the rear wheels. The front wheel rotation angle is controlled by image recognition and calibrated so that the angle between the normal of the vehicle plane and the normal of the obstacle plane is 90°. After the rear wheels make perpendicular contact with the obstacle, when the driving force of the rear wheels reaches the set value, the throttle is kept stable. After the rear wheels successfully climb, the step obstacle passability prediction and climbing attitude adjustment system ends.

[0056] The beneficial effects of this invention are:

[0057] The system and method for predicting the passability of stepped obstacles and adjusting climbing posture provided by this invention utilizes an APA ultrasonic radar with an RGB-D depth camera installed on the vehicle to monitor the height of the stepped obstacle and the lateral distance between the obstacle and the vehicle in real time. This avoids climbing failures and serious damage to wheels and other related components caused by excessive errors in judging the height of the stepped obstacle or insufficient steering adjustment distance. By adjusting the climbing posture, the wheels contact the obstacle at a more ideal vertical angle, improving climbing efficiency and maximizing the protection of tires and other related components. The vehicle has a voice reminder function that prompts the driver to abandon climbing or adjust the climbing posture when the stepped obstacle is too high or the distance is too small. The electronic brake pedal and steering system installed in the vehicle are used to coordinate the adjustment of the high posture, aiming to keep the front wheels on one side of the stepped obstacle contacting the obstacle at the lowest possible speed and close to a 90° vertical angle, thereby increasing the contact area and minimizing impact. Attached Figure Description

[0058] Figure 1 This is a block diagram of the overall structure of the passability prediction and climbing posture adjustment system described in this invention.

[0059] Figure 2 This is a schematic diagram of the passability prediction and climbing posture adjustment system described in this invention.

[0060] Figure 3 This is a schematic diagram of the distribution structure of the passability prediction and climbing posture adjustment system described in this invention.

[0061] The annotations in the image above are as follows:

[0062] 1. Perception Layer 2. Decision Layer 3. Execution Layer 4. APA Ultrasonic Radar

[0063] 5. RGB-D depth camera; 6. CAN main line; 7. ECU control unit; 8. Central control screen.

[0064] 9. Steering gear; 10. Electronic throttle pedal; 11. Electronic brake pedal; 12. Voice prompt module. Detailed Implementation

[0065] Please see Figures 1 to 3 As shown:

[0066] The step-shaped obstacle passability prediction and climbing posture adjustment system provided by this invention includes a perception layer 1, a decision layer 2, and an execution layer 3. The output of the perception layer 1 is connected to the input of the decision layer 2, and the output of the decision layer 2 is connected to the input of the execution layer 3. The perception layer 1 contains an APA ultrasonic radar 4, an RGB-D depth camera 5, and a CAN bus 6 arranged in parallel. The decision layer 2 contains an ECU control unit 7. The execution layer 3 contains a central control screen 8, a steering gear 9, an electronic accelerator pedal 10, an electronic brake pedal 11, and a voice prompt module 12 arranged in parallel. The APA ultrasonic radar 4, RGB-D depth camera 5, and CAN bus 6 are also included. Camera 5 and CAN bus 6 are both connected to ECU control unit 7. APA ultrasonic radar 4, RGB-D depth camera 5 and CAN bus 6 can transmit the collected data to ECU control unit 7 in real time. ECU control unit 7 is connected to central control screen 8, steering gear 9, electronic throttle pedal 10, electronic brake pedal 11 and voice prompt module 12 respectively. ECU control unit 7 controls the operation of central control screen 8, steering gear 9, electronic throttle pedal 10, electronic brake pedal 11 and voice prompt module 12 according to the data transmitted by APA ultrasonic radar 4, RGB-D depth camera 5 and CAN bus 6.

[0067] There are two APA ultrasonic radars 4, located on the side of the vehicle. The model of the APA ultrasonic radar 4 is AJ-SR04M ultrasonic radar. The APA ultrasonic radar 4 is connected to the vehicle's central control screen 8 via the vehicle bus. The APA ultrasonic radar 4 can display the distance to step-shaped obstacles in real time and has a voice broadcast function. The detection range of the APA ultrasonic radar 4 is between 30-500cm.

[0068] There are two RGB-D depth cameras. The model of the RGB-D depth camera is Intel RealSense D455RGB-D depth camera. The RGB-D depth camera can monitor the height of step-like obstacles in front and to the side in real time, and can also monitor the angle between the normal of the vehicle plane and the normal of the obstacle plane.

[0069] The CAN main line 6, ECU control unit 7, central control screen 8, steering gear 9, electronic throttle pedal 10, electronic brake pedal 11 and voice prompt module 12 mentioned above are all assemblies of existing equipment, therefore, the specific models and specifications are not described in detail.

[0070] The working principle of the step-shaped obstacle passability prediction and climbing posture adjustment system provided by this invention is as follows:

[0071] First, the height of the step-like obstacle on the side of the road is detected by the RGB-D depth camera 5. A passability prediction algorithm compares the height of the step-like obstacle with the vehicle's minimum ground clearance. If the height of the step-like obstacle is less than the vehicle's minimum ground clearance, the next step can be determined. Then, the height of the step can be compared with the height that the vehicle's front wheels can overcome per unit diameter to determine the success rate of climbing. Next, the APA ultrasonic radar 4 detects the lateral distance between the vehicle and the step-like obstacle. If the distance is sufficient to allow the wheels to adjust their direction, the ECU control unit 7 controls the wheel steering to decelerate the vehicle at approximately a 45° angle towards the step-like obstacle. The system detects the obstacle and then uses the RGB-D depth camera 5 to detect the angle between the plane normal of the vehicle body and the plane normal of the stepped obstacle. The sum of the angle and the front wheel steering angle is 90°, so that the wheel can make perpendicular contact with the stepped obstacle as much as possible. When a single wheel makes perpendicular contact with the stepped obstacle, it sends a signal to the ECU control unit 7. The ECU control unit 7 controls the throttle to slowly accelerate and climb up. When the wheel driving force reaches the set driving force that is just enough to lift off the ground, it is determined that the driving force is sufficient for the wheel to climb. The throttle is stabilized. After the inner front wheel successfully climbs up, the speed is reduced to low. The outer front wheel climbs up in the same way. The system sets the distance the outer front wheel climbs to be equal to the radius of the wheel itself as a signal that all front wheels have successfully climbed. At this point, the front wheels are raised by the height of the obstacle relative to their previous height. Once the RGB-D depth camera 5 detects that all front wheels have successfully climbed, it begins to adjust the attitude of the rear wheels. The system uses image recognition to control the front wheel angle for calibration, ensuring that the angle between the vehicle plane normal and the obstacle plane normal is 90°. After the rear wheels make perpendicular contact with the obstacle, the system maintains a stable throttle when the driving force for the rear wheels reaches the set value. Once the rear wheels have successfully climbed, the system terminates the step obstacle passability prediction and climbing attitude adjustment.

[0072] There are two APA ultrasonic radars 4 in the perception layer 1. The model of APA ultrasonic radar 4 is AJ-SR04M ultrasonic radar. APA ultrasonic radar 4 is connected to the vehicle's central control screen 8 through the vehicle bus. APA ultrasonic radar 4 can display the distance to the step-shaped obstacle in real time and has a voice broadcast function.

[0073] There are two RGB-D depth cameras 5 in the perception layer 1. The model of the RGB-D depth camera 5 is Intel RealSense D455 RGB-D depth camera. The RGB-D depth camera 5 can monitor the height of step-like obstacles in front and to the side in real time, and can also monitor the angle between the plane normal of the vehicle body and the plane normal of the obstacle.

[0074] When the ECU control unit 7 in the decision layer 2 is working, it first processes the height information of the stepped obstacle transmitted by the RGB-D depth camera 5. If the height of the stepped obstacle is less than the minimum ground clearance and the success rate of the passability prediction exceeds the set critical success rate, it then processes the lateral distance information between the vehicle and the stepped obstacle transmitted by the APA ultrasonic radar 4. If the distance is too small, the voice prompt module 12 will issue a voice alarm to remind the driver that the distance is too narrow. If the distance is within a suitable range, the ECU control unit 7 controls the steering gear 9 and the electronic brake pedal 11 in the execution layer 3 to decelerate the vehicle at about 45° and approach the stepped obstacle. The maximum initial vehicle speed when the system starts is set to 5 km / h. Then, the angle between the vehicle plane normal and the obstacle plane normal is detected by the RGB-D depth camera 5. The sum of the angle and the front wheel rotation angle is controlled to be 90°, so that the wheels make perpendicular contact with the step-shaped obstacle as much as possible. When the inner front wheel makes perpendicular contact with the step-shaped obstacle, a signal is fed back to the ECU control unit 7. The ECU control unit 7 controls the throttle to slowly accelerate and climb up. When the wheel driving force reaches the set driving force that is just enough to lift off the ground, it is determined that the driving force is sufficient for the wheel to climb. The throttle is stabilized. After the inner front wheel successfully climbs up, the speed is reduced to low. The outer front wheel climbs up in the same way. The system sets the distance the outer front wheel climbs to be equal to the radius of the wheel itself as a signal that all front wheels have successfully climbed. At this point, the front wheels are raised by the height of the obstacle relative to their previous height. Once the RGB-D depth camera 5 detects that all front wheels have successfully climbed, it begins to adjust the attitude of the rear wheels. The system uses image recognition to control the front wheel angle for calibration, ensuring that the angle between the vehicle plane normal and the obstacle plane normal is 90°. After the rear wheels make perpendicular contact with the obstacle, the system maintains a stable throttle when the driving force for the rear wheels reaches the set value. Once the rear wheels have successfully climbed, the system terminates the step obstacle passability prediction and climbing attitude adjustment.

[0075] The method for predicting the passability of stepped obstacles and adjusting climbing posture provided by the present invention includes the following steps:

[0076] Step 1: Deploy RGB-D depth camera 5. The specific steps are as follows:

[0077] Step 1: Place the RGB-D depth camera 5 on the rear side of the base plate, close to both sides of the vehicle body. Two RGB-D depth cameras 5 are installed on the base plate to monitor the angle between the vehicle body plane normal and the obstacle plane normal and the obstacle height.

[0078] Step 2: Connect the CAN bus interfaces of the two RGB-D depth cameras 5 to the vehicle bus and connect the power cables;

[0079] Step 3: The RGB-D depth camera 5 transmits the height of the stepped obstacle and the angle between the vehicle plane normal and the obstacle plane normal to the sensed obstacle to the main vehicle via the car CAN bus and connects to the in-vehicle display screen. The main vehicle obtains the height of the stepped obstacle on the side and the angle between the vehicle plane normal and the obstacle plane normal.

[0080] Step 2: Deploy APA ultrasonic radar 4. The specific steps are as follows:

[0081] Step 1: Place the two APA ultrasonic radars 4 on both sides of the vehicle body, symmetrically.

[0082] Step 2: Connect the CAN bus interfaces of the two APA ultrasonic radars 4 to the vehicle bus and connect the power cord.

[0083] Step 3: The APA ultrasonic radar 4 transmits the distance information between the vehicle and the side step-shaped obstacle to the main vehicle via the vehicle's CAN bus. The main vehicle then obtains the relative status data between the vehicle and the side step-shaped obstacle.

[0084] Step 3: Algorithm execution, the specific steps are as follows:

[0085] The RGB-D depth camera 5 uses OpenCV's YOLOv3 algorithm to detect objects and obtain bounding boxes, which are then processed to obtain the obstacle height. The RGB-D depth camera 5 also uses a plane detection algorithm, which extracts feature points by running the FAST algorithm through the OpenCV module and fits the plane using the RANSAC algorithm. The angle between the front wheel rotation center plane and the obstacle plane is converted into the sum of the angle between the front wheel rotation angle and the normal of the vehicle plane and the normal of the obstacle plane. Therefore, the angle between the front wheel rotation center plane and the obstacle plane is g1.

[0086] g1 = g2 + α

[0087] In the above formula, g2 is the angle between the normal to the vehicle plane and the normal to the obstacle plane; α is the front wheel steering angle;

[0088] The RANSAC algorithm fits the plane equation as follows:

[0089] Ax + By + Cz + D = 0

[0090]

[0091] In the above formula, The normal vectors of the fitted plane; A, B, C, and D are the coefficients of the fitted plane equation obtained by the actual execution of the RANSAC algorithm;

[0092] The passability prediction algorithm initializes with the vehicle's minimum ground clearance, wheel diameter, distance from the center of gravity to the front and rear axles, wheelbase, and real-time road adhesion coefficient. The main vehicle ECU control unit 7 processes the height and spacing data of the lateral step-like obstacles obtained in the first two steps according to the designed algorithm. The algorithm compares the vehicle's minimum ground clearance with the height of the step-like obstacle, then calculates the ratio of the step-like obstacle height to the diameter of the vehicle's front wheels, and then compares this ratio with the height of the step-like obstacle that the front wheel can overcome per unit wheel diameter to determine the climbing success rate. The larger the difference between this ratio and the height of the step-like obstacle that the front wheel can overcome per unit wheel diameter, the higher the success rate. Critical success rates are set at 20% and 80%. Below 20%, climbing is abandoned; above 20% but below 80%, attitude adjustment is performed; above 80%, the vehicle passes directly without attitude adjustment. The height H of the step-like obstacle that the front wheel can overcome per unit wheel diameter is:

[0093]

[0094] h w D is the height of the stepped obstacle; D is the diameter of the front wheel. ρ is the adhesion coefficient; α is the distance from the center of mass to the front axle; L is the wheelbase.

[0095] The success rate of climbing is C:

[0096]

[0097] Compare the actual horizontal spacing with the minimum horizontal spacing to determine if the spacing is appropriate. The theoretical minimum horizontal spacing is s1, and the actual minimum horizontal spacing is s:

[0098]

[0099] s = s1 + s2 + s3

[0100] In the above formula, v is the minimum vehicle speed when it contacts the stepped obstacle; t1 is the time it takes for the front wheels to reach perpendicular contact with the stepped obstacle; s1 is the theoretical minimum lateral clearance; s2 is the error distance affected by different vehicle wheel parameters; and s3 is the error distance affected by different initial vehicle speeds.

[0101] The front wheels begin climbing the obstacle. A critical driving force is set as the throttle control signal when the wheels just leave the ground. The critical driving force F1 at this point is calculated. At this point, the throttle is sufficient for the car to climb the stepped obstacle. The throttle is maintained to continue climbing. After successful climbing, the speed is reduced to low. The set front wheel contact surface pressure value is N, and the critical driving force F1 is:

[0102]

[0103]

[0104] In the above formula, m represents the load borne by a single wheel; The adhesion coefficient at the sharp corner of the step is denoted as , where Greater than the road adhesion coefficient β is the angle between the line connecting the point of contact between the wheel and the step-shaped obstacle and the center of the wheel and the line perpendicular to the ground passing through the center of the wheel;

[0105] The signal is set that the second front wheel crosses the stepped obstacle by one wheel radius as a successful climb signal for all front wheels, and the rear wheels begin to adjust their attitude. At this point, the lateral distances between the center and the foremost point of the inner rear wheel and the stepped obstacle are S′ and S′, respectively. r :

[0106] S′=L cos α2-L1 sin α2

[0107] S r =L cos α2-L1 sin α2-(R sin α2-L2 cos a2)tan α2

[0108] In the above formula, L is the wheelbase; L1 is the distance between the rear wheel centers; α2 is the turning angle of the outer front wheel; R is the wheel radius; L2 is the wheel width;

[0109] When the rear wheel is climbing, the relationship between the change in the rear wheel center plane angle and the front wheel angle is as follows: (The front wheel is adjusted to a suitable turning angle and maintains a low-speed, uniform circular motion.)

[0110]

[0111] In the above formula, θ1 and θ2 are the angles that change after the inner front and rear wheels move, respectively; v f t is the speed of the inner front wheel; t is the time it takes for the rear wheel to adjust to be perpendicular to the obstacle section; α1 is the turning angle of the inner front wheel;

[0112] The rear wheels are adjusted to travel a lateral distance L3 perpendicular to the plane of the obstacle, while satisfying the following relationship to prevent insufficient lateral adjustment clearance of the rear wheels:

[0113] L3+R≤S′

[0114] Step 4: Operation of the implementing agency, the specific steps are as follows:

[0115] After data processing in steps one and three, based on the comparison results, the ECU control unit 7 controls the voice prompt module 12 or the electronic brake pedal 10 and steering gear 9 to start working. The main vehicle detects the height of the step-shaped obstacle through the RGB-D depth camera 5. The data is processed by the ECU control unit 7 to determine the success rate of climbing the step-shaped obstacle. If the height of the step-shaped obstacle is greater than or equal to the minimum ground clearance, the success rate is 0, and the voice prompt module reminds the driver to give up climbing. If the height of the step-shaped obstacle is less than the minimum ground clearance, the passability prediction algorithm calculates the climbing success rate. When the success rate is ≥20% and the lateral distance is appropriate, the steering gear 9 and the electronic brake pedal 11 work together to approach the step-shaped obstacle. When the success rate is <20%, the voice prompt module 12 reminds the driver to give up climbing. When the success rate is ≥20% and the lateral distance is too small, the voice prompt module 12 reminds the driver to safely adjust the distance. When the success rate is ≥80%, there is no need to adjust the climbing posture.

[0116] Step Two: During the climbing posture adjustment, the angle between the vehicle's plane normal and the plane normal of the stepped obstacle is detected by the RGB-D depth camera 5. The sum of this angle and the front wheel rotation angle is controlled to be 90°, ensuring that the wheels make as perpendicular contact as possible with the stepped obstacle. When a single wheel makes perpendicular contact with the stepped obstacle, a signal is sent to the ECU control unit 7. The ECU control unit 7 then controls the throttle to slowly accelerate and climb. When the wheel's driving force reaches the set point just enough to lift off the ground, it is determined that the driving force is sufficient for the wheel to climb. The throttle is then maintained. After the inner front wheel successfully climbs, the speed is reduced to low, and the outer front wheel begins climbing. The method is the same. The outer front wheel is set to climb one wheel radius distance as the signal that the front wheel has successfully climbed. At this time, the front wheel is raised by one obstacle height distance relative to the front wheel before climbing. When the RGB-D depth camera 5 detects that the front wheel has successfully climbed, the rear wheel attitude is adjusted. The front wheel angle is controlled by image recognition and calibrated so that the angle between the normal of the vehicle plane and the normal of the obstacle plane is 90°. After the rear wheel makes perpendicular contact with the obstacle, when the driving force of the rear wheel climbing reaches the set value, the throttle is kept stable. After the rear wheel climbs successfully, the step obstacle passability prediction and climbing attitude adjustment system ends.

Claims

1. A method for predicting the passability of stepped obstacles and adjusting climbing posture, characterized in that: The method includes the following steps: The first step is to set up an RGB-D depth camera. The specific steps are as follows: Step 1: Place the RGB-D depth camera on the rear side of the base plate, close to both sides of the vehicle body. Install two RGB-D depth cameras on the base plate to monitor the angle between the vehicle body plane normal and the obstacle plane normal, as well as the obstacle height. Step 2: Connect the CAN bus interfaces of the two RGB-D depth cameras to the vehicle's bus and connect the power cables; Step 3: The RGB-D depth camera transmits the height of the stepped obstacle and the angle between the vehicle's plane normal and the obstacle's plane normal to the perceived height of the stepped obstacle via the vehicle's CAN bus to the main vehicle and connects to the in-vehicle display screen. The main vehicle then obtains the height of the stepped obstacle on the side and the angle between the vehicle's plane normal and the obstacle's plane normal. The second step is to deploy the APA ultrasonic radar, and the specific steps are as follows: Step 1: Place the two APA ultrasonic radars on both sides of the vehicle body, symmetrically. Step 2: Connect the CAN bus interfaces of the two APA ultrasonic radars to the vehicle's bus and connect the power cables; Step 3: The APA ultrasonic radar transmits the distance information between the vehicle and the stepped obstacle on the side to the host vehicle via the vehicle's CAN bus. The host vehicle then obtains the relative status data between the vehicle and the stepped obstacle on the side. The third step is to run the algorithm, and the specific steps are as follows: The RGB-D depth camera uses OpenCV's YOLOv3 algorithm for object detection to obtain bounding boxes, and then processes the data to obtain the obstacle height. The RGB-D depth camera's plane detection algorithm uses the FAST algorithm run through an OpenCV module to extract feature points, and then uses the RANSAC algorithm to fit the plane. The angle between the front wheel rotation center plane and the obstacle plane is converted into the sum of the angles between the front wheel rotation angle and the normals to the vehicle plane and the obstacle plane. Therefore, the angle between the front wheel rotation center plane and the obstacle plane is... ; In the above formula, The angle between the normal to the vehicle's plane and the normal to the obstacle's plane; The steering angle of the front wheels; The RANSAC algorithm fits the plane equation as follows: In the above formula, To fit the plane normal vector; , , , These are the coefficients of the fitted plane equation obtained from the actual execution of the RANSAC algorithm; The passability prediction algorithm initializes with the vehicle's minimum ground clearance, wheel diameter, distance from the center of gravity to the front and rear axles, wheelbase, and real-time road adhesion coefficient. The main vehicle ECU control unit processes the height and spacing data of the lateral step-like obstacles obtained in the first two steps according to the designed algorithm. The algorithm compares the vehicle's minimum ground clearance with the height of the step-like obstacle, then calculates the ratio of the step-like obstacle height to the diameter of the vehicle's front wheels, and then compares this ratio with the height of the step-like obstacle that the front wheels can overcome per unit wheel diameter to determine the success rate of climbing. The larger the difference between this ratio and the height of the step-like obstacle that the front wheels can overcome per unit wheel diameter, the higher the success rate. Critical success rates are set at 20% and 80%. Below 20%, climbing is abandoned; above 20% but below 80%, attitude adjustment is performed; above 80%, the vehicle passes directly without attitude adjustment. The threshold success rate is defined as the height of the step-like obstacle that the front wheels can overcome per unit wheel diameter. : D is the height of the stepped obstacle; D is the diameter of the front wheel. ρ is the adhesion coefficient; α is the distance from the center of mass to the front axle; L is the wheelbase. The success rate of climbing is C: Car steering angle The steering angle refers to the angle formed by the center line of a car when the front wheels are turned to their extreme left or right positions without deflection. Between 30 and 40 degrees, compare the actual lateral spacing with the minimum lateral spacing to determine if the spacing is appropriate. The theoretical minimum lateral spacing is... The actual minimum horizontal spacing is : In the above formula, This is the minimum speed at which the vehicle will come into contact with the stepped obstacle. The time it takes for the front wheels to travel to make vertical contact with the stepped obstacle; This represents the theoretical minimum horizontal spacing. The error distance is affected by different car wheel parameters; The error distance is affected by different initial vehicle speeds; The front wheels begin to climb the obstacle. A critical driving force for the front wheels is set as the throttle control signal when the wheels just leave the ground. The critical driving force at this point is calculated. At this point, the throttle is sufficient for the car to climb the step-like obstacle. Maintain the throttle to continue climbing, and once successfully climbed, resume low speed, i.e., the set front wheel contact surface pressure value. Critical driving force : In the above formula, For a single wheel to bear the load; The adhesion coefficient at the sharp corner of the step is denoted as , where Greater than the road adhesion coefficient ; The angle between the line connecting the point of contact between the wheel and the step-shaped obstacle and the center of the wheel, and the line perpendicular to the ground passing through the center of the wheel; The signal is set that the second front wheel crosses the stepped obstacle by one wheel radius as a successful climb signal for all front wheels, and the rear wheels begin to adjust their attitude. At this point, the lateral distances between the center and the foremost point of the inner rear wheel and the stepped obstacle are respectively... and : In the above formula, This refers to the wheelbase; This is the distance between the centers of the rear wheels; The steering angle of the outer front wheel of the vehicle; The radius of the wheel; This refers to the width of the wheel; When the rear wheel is climbing, the relationship between the change in the rear wheel center plane angle and the front wheel angle is as follows: (The front wheel is adjusted to a suitable turning angle and maintains a low-speed, uniform circular motion.) In the above formula, , These represent the changes in angle after the front and rear inner wheels move; The speed of the inner front wheel; The time taken to align the rear wheels with the cross section of the obstacle; The inner front wheel steering angle; The rear wheel is adjusted to travel the lateral distance perpendicular to the plane of the obstacle. Furthermore, the following relationship must be met to prevent insufficient lateral adjustment clearance of the rear wheels: Step 4: Operation of the implementing agency, the specific steps are as follows: After data processing in steps one and three, based on the comparison results, the ECU control unit controls the voice prompt module or electronic brake pedal and steering gear to start working. The main vehicle uses an RGB-D depth camera to detect the height of the step-shaped obstacle. The data is processed by the ECU control unit to determine the success rate of climbing the step-shaped obstacle. If the height of the step-shaped obstacle is greater than or equal to the minimum ground clearance, the success rate is 0, and the voice prompt module reminds the driver to give up climbing. If the height of the step-shaped obstacle is less than the minimum ground clearance, the passability prediction algorithm calculates the climbing success rate. When the success rate is ≥20% and the lateral distance is appropriate, the steering gear and electronic brake pedal work together to approach the step-shaped obstacle. When the success rate is <20%, the voice prompt module reminds the driver to give up climbing. When the success rate is ≥20% and the lateral distance is too small, the voice prompt module reminds the driver to safely adjust the distance. When the success rate is ≥80%, no adjustment of the climbing posture is required. Step Two: During the climbing posture adjustment, the angle between the vehicle's plane normal and the plane normal of the stepped obstacle, detected by the RGB-D depth camera, is controlled to be 90°, with the sum of the angle and the front wheel rotation angle. This ensures the wheels make as perpendicular contact as possible with the stepped obstacle. When a single wheel makes perpendicular contact with the obstacle, a signal is sent to the ECU control unit. The ECU control unit then controls the throttle to slowly accelerate and climb. When the wheel's driving force reaches the set point just enough to lift it off the ground, it is determined that the driving force is sufficient for the wheel to climb. The throttle is then maintained. Once the inner front wheel has successfully climbed, the speed is reduced to low, and the outer front wheel begins its climbing motion. The method is the same. The outer front wheel climbs a distance equal to the radius of the wheel as a signal that the front wheels have successfully climbed. At this time, the front wheels are raised by the height of the obstacle relative to the front wheels. When the RGB-D depth camera detects that the front wheels have successfully climbed, it starts to adjust the attitude of the rear wheels. The front wheel rotation angle is controlled by image recognition and calibrated so that the angle between the normal of the vehicle plane and the normal of the obstacle plane is 90°. After the rear wheels make perpendicular contact with the obstacle, when the driving force of the rear wheels reaches the set value, the throttle is kept stable. After the rear wheels successfully climb, the step obstacle passability prediction and climbing attitude adjustment system ends.

Images