Sensor-based local path planning method for obstacle crossing of self-balancing electric vehicle
Through multi -sensor fusion technology and path planning algorithm, the problems of two -wheeled electric vehicles in complex environments have been solved, and high -precision navigation and stable control have been achieved, which significantly improves obstacles.
Patent Information
- Application Number
- CN202411390871.9
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-10-08
- Publication Date
- 2025-01-28
AI Technical Summary
Existing two-wheeled electric vehicles are difficult to achieve high-precision navigation and stable control in complex environments, especially when facing multiple obstacles, the path planning is unreasonable and the strategy is unclear.
Using multi -sensor fusion technology, through radar, ultrasonic sensor, 360 -degree camera, and SLAM algorithm to build a network map of the surrounding environment, determine the position of obstacles, and use the RL curve improved by DUBINS curve to generate crossing paths. Best.
The accurate perception and real -time update of the position of the two -wheeled electric vehicle position has improved the navigation accuracy and stability, enhanced the navigation and stable control capabilities of electric vehicles in a complex environment, and significantly improved the ability to avoid obstacles.
Smart Images
Figure CN119374591A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the technical field of self-balancing electric vehicles, and in particular to a local path planning method for a self-balancing electric vehicle to cross obstacles based on a sensor. Background Art
[0002] At present, two-wheeled electric vehicles have been widely developed and applied around the world, especially in urban transportation. Due to the increase in environmental protection and energy efficiency requirements and the improvement of battery technology, the endurance of electric vehicles has been greatly improved, making them a more reliable and practical means of transportation.
[0003] With the development of robotics technology, two-wheeled electric vehicles have been widely used in various scenarios. However, due to their own dynamic characteristics, two-wheeled electric vehicles require complex stability control systems to ensure the stability of their autonomous driving. At the same time, in order to achieve efficient and accurate autonomous navigation, it is necessary to have accurate perception and understanding of the surrounding environment. Traditional single-sensor assisted electric vehicles often find it difficult to meet this demand. First, a single sensor cannot provide sufficient precision and accuracy in a complex dynamic environment; second, if you only rely on a single sensor, once the only sensor fails, it will affect the entire control system.
[0004] For path planning, the obstacle avoidance measures of a mobile vehicle when facing obstacles are complex, but the basics are nothing more than turning left, going straight, and turning right. Therefore, they can be classified according to the relative positions of obstacles. One is path planning to avoid a single obstacle, and the other is path planning to avoid multiple obstacles, such as Figure 1 However, there are problems such as unreasonable path planning and unclear planning strategy. Summary of the invention
[0005] The present invention discloses a local path planning method for a self-balancing electric vehicle to cross obstacles based on a sensor. The specific method is as follows:
[0006] Determine the location of primary obstacles;
[0007] Construct a kinematic model of a self-balancing electric vehicle;
[0008] Generate a primary obstacle crossing path;
[0009] Control the self-balancing electric vehicle to execute the first-level obstacle crossing path, and use the cameras on both sides of the self-balancing electric vehicle to capture the road conditions in the first-level obstacle crossing path in real time;
[0010] If there is a secondary obstacle in the primary obstacle crossing path, the primary obstacle crossing path is adjusted to generate a secondary obstacle crossing path, and the secondary obstacle crossing path is executed.
[0011] Furthermore, the location of the first-level obstacle is determined as follows:
[0012] Using radar and ultrasonic sensors to detect and identify a first-level obstacle in front of the self-balancing electric vehicle, the size of the first-level obstacle is greater than a first preset threshold;
[0013] The image information is obtained through the 360-degree concentration camera, and the SLAM algorithm is used to build a network map of the surrounding environment;
[0014] Construct a network map of the surrounding environment to construct two-dimensional coordinate information and determine the location of primary obstacles;
[0015] The first-level obstacle is expanded, and the expansion radius is half the width of the self-balancing electric vehicle body.
[0016] Furthermore, a kinematic model of a self-balancing electric vehicle is constructed, and the specific method is as follows:
[0017] Calculate the turning radius of a self-balancing electric vehicle:
[0018]
[0019] In the formula, L is the distance between the front and rear axles, δ f is the maximum turning angle of the front wheel;
[0020] Simplified kinematic model equations:
[0021]
[0022] Where θ is the yaw angle, v is the longitudinal velocity, and ω is the circular motion angular velocity;
[0023] Discretize the simplified kinematic model equation and let Δs=vdt, then:
[0024] x new =x prev +Δs·cos(θ)
[0025] y new =y prev +Δs·sin(θ)
[0026]
[0027] Where dt is the sampling time, Δs is the displacement within the sampling time, (x prev ,y prev ),θ prev is the position coordinate and yaw angle before sampling time, (x new ,y new ),θ new are the position coordinates and yaw angle after sampling time.
[0028] Furthermore, a primary obstacle crossing path is generated using an RL curve improved from the Dubins curve. The specific method is as follows:
[0029] The coordinates of the two nearest level 1 obstacles A and B are obtained through the 360 depth camera, which are: (x a ,y a )、(x b ,y b );
[0030] Get the current position coordinates of the electric vehicle: (x0, y0);
[0031] Draw two identical circumscribed circles with the centers of the two primary obstacles A and B. The radius of the two identical circumscribed circles is:
[0032]
[0033] Among them, (x t1 ,y t1 ) is the coordinate a of the first tangent point;
[0034] In the RL curve, when crossing the primary obstacle A, a right turn is implemented, and the constraints are as follows:
[0035]
[0036] Among them, (x t1 ,y t1 ) is the coordinate of the first tangent point;
[0037] When crossing the first-level obstacle B, turn left, and the constraints are as follows:
[0038]
[0039] Among them, (x t2 ,y t2 ) is the coordinate b of the second tangent point;
[0040] By obtaining the two tangent point coordinates, when the electric vehicle reaches the first entry point coordinate, the servo motor located under the handlebars of the vehicle is controlled to deflect and pass through the obstacle.
[0041] Furthermore, the method for obtaining the secondary obstacle is as follows:
[0042] When executing the first-level obstacle crossing path, two cameras located on both sides of the front wheel are used to observe the second-level obstacle in front from different angles. When the size of the second-level obstacle is larger than the second preset size, it is identified as a second-level obstacle.
[0043] Furthermore, adjust the path for crossing the first-level obstacle. The specific method is as follows:
[0044] Two images are obtained simultaneously through two cameras located on both sides of the front wheel;
[0045] After epipolar correction and matching, the disparity of the image pixels is calculated, and the target third-dimensional coordinate depth Z is obtained by the following formula:
[0046]
[0047] Where d is the disparity of the image pixels, f is the focal length of the two cameras, and B is the distance between the optical centers of the two cameras;
[0048] According to the calculated depth value of the secondary obstacle, determine whether it affects the self-balancing performance;
[0049] If there is an impact, the first path is refined, the primary obstacle crossing path is regarded as the normal driving path, and the secondary obstacle is regarded as the primary obstacle. The Dubins algorithm is continued to be executed to generate the secondary obstacle crossing path.
[0050] Furthermore, to determine whether the self-balancing performance is affected, the specific method is as follows:
[0051] Based on the tire size and the control limit of the self-balancing control algorithm, the maximum depth value is set to k;
[0052] When the absolute value of the depth of the secondary obstacle is less than the k value, it is judged that it has no effect on the crossing path of the primary obstacle and the self-balancing of the vehicle body;
[0053] When the absolute value of the depth of the obstacle is greater than the k value, it is judged that it will affect the path of crossing the first-level obstacle.
[0054] Due to the adoption of the above technical solution, the present invention has the following beneficial effects:
[0055] 1. Through the fusion of multi-sensor data, the position and posture of two-wheeled electric vehicles can be accurately perceived and updated in real time, which not only improves the navigation accuracy and stability, but also enhances the navigation and stability control capabilities of electric vehicles in complex environments.
[0056] 2. In the self-balancing electric vehicle, the present invention can realize the precise control of the posture, speed, and position of the electric vehicle through multi-sensor fusion technology. Using different sensors from different directions can realize the role of information supplement, thereby improving the accuracy of data and the comprehensiveness of control. In addition, the fusion of multiple sensors not only improves the control accuracy, but also enhances the robustness and anti-interference ability of the system, significantly improving the user's driving experience and safety performance.
[0057] 3. The present invention proposes a path generation and optimization method for crossing obstacles, which can be optimized in real time during the execution process, thereby significantly improving the obstacle avoidance capability of the self-balancing electric vehicle.
[0058] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be achieved and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS
[0059] The accompanying drawings of the present invention are as follows.
[0060] Figure 1 This is a schematic diagram of the existing obstacle avoidance path planning.
[0061] Figure 2 Schematic diagram of the obstacle after expansion processing.
[0062] Figure 3 Schematic diagram of the obstacle crossing path.
[0063] Figure 4 Simplified diagram for obstacle traversal.
[0064] Figure 5 Schematic diagram of path optimization.
[0065] Figure 6 It is a schematic diagram of the structure of a self-balancing electric vehicle.
[0066] Figure 7 It is a schematic diagram of the process of the present invention. DETAILED DESCRIPTION
[0067] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.
[0068] A local path planning method for a self-balancing electric vehicle to cross obstacles based on a sensor, the structure of the self-balancing electric vehicle is as follows Figure 6 The specific steps are as shown in Figure 7 As shown:
[0069] S1. Determine the location of the primary obstacle.
[0070] Step S1 needs to determine the location of larger obstacles and the safe buffer distance. The radar and ultrasonic sensors installed on the front of the self-balancing electric vehicle can quickly detect larger identifiable obstacles at a distance in front, and then obtain accurate image information through the 360 depth camera to construct a surrounding environment grid map with the help of the SLAM algorithm, and at the same time construct two-dimensional coordinate information. Electric vehicles need to maintain a certain safe buffer distance from obstacles when driving, so it is necessary to optimize the map in the constructed surrounding environment grid map and generate a cost map with a certain expansion coefficient around the obstacle, such as Figure 2The blue ring around the obstacle is a safety zone set to ensure that the electric vehicle avoids collision when passing through. The expansion radius is generally designed according to the size of the designed electric vehicle. The expansion radius selected by the present invention is half the width of the vehicle body.
[0071] S2. Construct a kinematic model of a self-balancing electric vehicle.
[0072] In step S2, the turning radius of the electric vehicle is first determined as: Where L is the distance between the front and rear axles, δ f is the maximum turning angle of the front wheels.
[0073] The simplified kinematic model equation is:
[0074]
[0075] Where θ is the yaw angle, v is the longitudinal velocity, and ω is the circular motion angular velocity.
[0076] The above kinematic equation is discretized and the sampling time is taken as dt. The smaller dt is, the more discrete coordinates are obtained.
[0077] Let Δs = vdt, and we get
[0078] x new =x prev +Δs·cos(θ)
[0079] y new =y prev +Δs·sin(θ)
[0080]
[0081] S3. Generate a primary obstacle crossing path.
[0082] In step S3, the RL curve improved by the Dubins curve is used, such as Figure 3 shown.
[0083] Obstacle A and B coordinates obtained by 360 depth camera are: (x a ,y a )、(x b ,y b ), the current position coordinates of the electric vehicle are: (x0, y0), as shown in the diagram Figure 4 shown.
[0084] Then the radius of the two identical circumscribed circles in the RL path is
[0085] Through the following formula
[0086]
[0087] Because the RL path in the Dubins algorithm is used, the right turn strategy is implemented after crossing the first obstacle, so there are constraints:
[0088]
[0089] Where (x t1 ,y t1 ) is the coordinate of the first tangent point a
[0090] Similarly, using the formula
[0091]
[0092] Crossing the second obstacle implements a left turn strategy based on completing the first crossing, so other constraints are not considered.
[0093] Where (x t2 ,y t2 ) is the coordinate b of the second tangent point.
[0094] By obtaining the two tangent point coordinates, when the electric vehicle reaches the first entry point coordinate, the servo motor located under the vehicle handle is controlled to achieve a certain angle of deflection to achieve the purpose of turning and crossing obstacles.
[0095] S4. Control the self-balancing electric vehicle to execute the first-level obstacle crossing path, and use cameras on both sides of the self-balancing electric vehicle to capture the road conditions in the first-level obstacle crossing path in real time.
[0096] S5. If there is a secondary obstacle in the primary obstacle crossing path, the primary obstacle crossing path is adjusted to generate a secondary obstacle crossing path, and the secondary obstacle crossing path is executed.
[0097] In step S5, when driving through larger identifiable obstacles, smaller obstacles that cannot be directly detected in advance may appear, requiring cameras located on both sides of the vehicle to capture the road conditions in the path in real time.
[0098] Two cameras located on both sides of the front wheel are used to observe small obstacles from different angles and obtain two images at the same time. After epipolar correction and matching, the parallax of the image pixels can be calculated, and then the third-dimensional coordinate depth Z of the target can be obtained by the following formula:
[0099]
[0100] Among them, d is the parallax of the image pixels, f is the focal length of the two cameras, and B is the distance between the optical centers of the two cameras.
[0101] According to the calculated depth value of the smaller obstacle, determine whether it affects the self-balancing performance. Based on the tire size and the control limit of the self-balancing control algorithm, set the maximum depth value to k. When the absolute value of the depth of the obstacle is less than the k value, it has no effect on the first planned path and the self-balancing of the vehicle body, and the electric vehicle can strictly follow the first planned path; when the absolute value of the depth of the obstacle is greater than the k value, the newly acquired smaller obstacle will affect the self-balancing of the electric vehicle during driving, and the electric vehicle cannot continue to drive strictly according to the first planned path. At this time, it is necessary to adjust the first planned path to pass safely and smoothly.
[0102] The first planned path is adjusted by refining the first path, treating the first planned path as a normal driving path, treating the new smaller obstacles that will affect the balance of the electric vehicle as larger obstacles, and continuing to execute the Dubins algorithm to cross the smaller obstacles in the first path, such as Figure 5 shown.
[0103] Among them, C1 to Cn are the smaller obstacles detected by the camera when the electric vehicle executes the first planned path, which are sorted according to the first path. When driving close to the C series obstacles, the Dubins algorithm is executed, and the RL-C1 to RL-Cn curves are planned in sequence based on the first planned path to ensure that the vehicle can cross smaller obstacles without deviation and drive stably and safely.
[0104] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the above embodiments, ordinary technicians in the relevant field should understand that the specific implementation methods of the present invention can still be modified or replaced by equivalents. Any modification or equivalent replacement that does not depart from the spirit and scope of the present invention should be covered within the scope of protection of the claims of the present invention.
Claims
1. A local path planning method for a sensor-based self-balancing electric vehicle to cross obstacles, characterized in that: The specific method is as follows: Determine the location of primary obstacles; Construct a kinematic model of a self-balancing electric vehicle; Generate a primary obstacle crossing path; Control the self-balancing electric vehicle to execute the first-level obstacle crossing path, and use the cameras on both sides of the self-balancing electric vehicle to capture the road conditions in the first-level obstacle crossing path in real time; If there is a secondary obstacle in the primary obstacle crossing path, the primary obstacle crossing path is adjusted to generate a secondary obstacle crossing path, and the secondary obstacle crossing path is executed.
2. The local path planning method for a sensor-based self-balancing electric vehicle to cross obstacles as claimed in claim 1, characterized in that: Determine the location of the first-level obstacle. The specific method is as follows: Using radar and ultrasonic sensors to detect and identify a first-level obstacle in front of the self-balancing electric vehicle, the size of the first-level obstacle is greater than a first preset threshold; The image information is obtained through the 360-degree concentration camera, and the SLAM algorithm is used to build a network map of the surrounding environment; Construct a network map of the surrounding environment to construct two-dimensional coordinate information and determine the location of primary obstacles; The first-level obstacle is expanded, and the expansion radius is half the width of the self-balancing electric vehicle body.
3. The local path planning method for a sensor-based self-balancing electric vehicle to cross obstacles as claimed in claim 1, characterized in that: Construct a kinematic model of a self-balancing electric vehicle. The specific method is as follows: Calculate the turning radius of a self-balancing electric vehicle: In the formula, L is the distance between the front and rear axles, δ f is the maximum turning angle of the front wheel; Simplified kinematic model equations: Where θ is the yaw angle, v is the longitudinal velocity, and ω is the circular motion angular velocity; Discretize the simplified kinematic model equation and let Δs=vdt, then: x new =x prev +Δs·cos(θ) and new =and prev +Δs·sin(θ) Where dt is the sampling time, Δs is the displacement within the sampling time, (x prev ,y prev ),θ prev is the position coordinate and yaw angle before sampling time, (x new ,y new ),θ new are the position coordinates and yaw angle after sampling time.
4. The local path planning method for a sensor-based self-balancing electric vehicle to cross obstacles as claimed in claim 1, characterized in that: Generate a primary obstacle crossing path using the RL curve improved from the Dubins curve. The specific method is as follows: The coordinates of the two nearest level 1 obstacles A and B are obtained through the 360 depth camera, which are: (x a ,y a )、(x b ,y b ); Get the current position coordinates of the electric vehicle: (x0, y0); Draw two identical circumscribed circles with the centers of the two primary obstacles A and B. The radius of the two identical circumscribed circles is: Among them, (x t1 ,y t1 ) is the coordinate a of the first tangent point; In the RL curve, when crossing the primary obstacle A, a right turn is implemented, and the constraints are as follows: Among them, (x t1 ,y t1 ) is the coordinate of the first tangent point; When crossing the first-level obstacle B, turn left, and the constraints are as follows: Among them, (x t2 ,y t2 ) is the coordinate b of the second tangent point; By obtaining the two tangent point coordinates, when the electric vehicle reaches the first entry point coordinate, the servo motor located under the handlebars of the vehicle is controlled to deflect and pass through the obstacle.
5. The local path planning method for a sensor-based self-balancing electric vehicle to cross obstacles as claimed in claim 4, characterized in that: The method of obtaining secondary obstacles is as follows: When executing the first-level obstacle crossing path, two cameras located on both sides of the front wheel are used to observe the second-level obstacle in front from different angles. When the size of the second-level obstacle is larger than the second preset size, it is identified as a second-level obstacle.
6. The local path planning method for a sensor-based self-balancing electric vehicle to cross obstacles as claimed in claim 5, characterized in that: Adjust the path for crossing the first-level obstacle. The specific method is as follows: Two images are obtained simultaneously through two cameras located on both sides of the front wheel; After epipolar correction and matching, the disparity of the image pixels is calculated, and the target third-dimensional coordinate depth Z is obtained by the following formula: Where d is the disparity of the image pixels, f is the focal length of the two cameras, and B is the distance between the optical centers of the two cameras; According to the calculated depth value of the secondary obstacle, determine whether it affects the self-balancing performance; If there is an impact, the first path is refined, the primary obstacle crossing path is regarded as the normal driving path, and the secondary obstacle is regarded as the primary obstacle. The Dubins algorithm is continued to be executed to generate the secondary obstacle crossing path.
7. The local path planning method for a sensor-based self-balancing electric vehicle to cross obstacles as claimed in claim 6, characterized in that: To determine whether the self-balancing performance is affected, the specific method is as follows: Based on the tire size and the control limit of the self-balancing control algorithm, the maximum depth value is set to k; When the absolute value of the depth of the secondary obstacle is less than the k value, it is judged that it has no effect on the path of crossing the primary obstacle and the self-balancing of the vehicle body; When the absolute value of the depth of the obstacle is greater than the k value, it is judged that it will affect the path of crossing the first-level obstacle.