Speed-controllable artificial potential field method based on estimated position

Abstract

The invention provides a speed-controllable artificial potential field method based on an estimated position, and the method comprises the steps: 1, building a two-dimensional map coordinate system Oxy, and collecting the information of a moving object, the information of a target, and the information of an obstacle through a sensor system of the moving object; step 2, calculating a distance D0 from the moving object to the target point, and if D0 is not less than a set threshold, entering step 3; 3, calculating an estimated position coordinate X 'G of the target; 4, calculating the gravitation Fa suffered by the moving object; 5, the repulsive force Fr borne by the moving object is calculated; 6, synthesizing the attractive force Fa and the repulsive force Fr into a resultant force F = Fa + Fr borne by the moving object; 7, according to the resultant force F borne by the moving object, the speed direction angle theta U (t + 1) of the next movement of the moving object is calculated; 8, calculating the speed UU (t + 1) of the next movement of the moving object; and 9, updating the position XU (t + 1) of the moving object at the next moment according to XU (t), theta U (t + 1) and UU (t + 1), and then returning to the step 2.

Description

Technical Field

[0001] This invention relates to the field of motion control, specifically to a velocity-controllable artificial potential field method based on predicted position. Background Technology

[0002] The artificial potential field method is a widely used approach for path planning and motion control, applied in the motion control of moving objects such as intelligent robots, autonomous vehicles, and unmanned aerial vehicles. The core idea is to model the environment of the moving object as a potential field, where the target point is assigned a gravitational field and obstacles are assigned a repulsive field. During its movement, the moving object is subjected to both gravitational and repulsive forces. By calculating the direction and magnitude of the resultant force, its trajectory is determined, thus achieving motion control and path planning from the starting point to the target point.

[0003] Traditional artificial potential field methods offer several advantages. First, their calculation process is relatively simple, making them suitable for real-time path planning. Because they are based on local information and do not require a global search of the entire environment, they are computationally efficient. Second, the method's principle is intuitive, its implementation is simple, and it is easy to understand and program, making it suitable for various path planning systems. Finally, artificial potential field methods are highly adaptable and can quickly respond to environmental changes. When obstacle positions change, the method can rapidly adjust the path to avoid collisions.

[0004] Despite the advantages mentioned above, the artificial potential field method also has some significant shortcomings in practical applications. First, it exhibits a lag when dealing with dynamic targets. Traditional artificial potential field methods rely on the target's current position to plan its next movement. Therefore, when the target is in motion, the path planned by traditional methods lacks foresight and cannot predict the target's future position, potentially resulting in a longer planned path. Second, traditional artificial potential field methods guide the movement of the object solely through the gradient of the potential field, lacking control over the object's velocity. This can lead to the object moving too fast near obstacles, failing to avoid them effectively and potentially colliding with them.

[0005] In dynamic environments and complex tasks, the shortcomings of traditional artificial potential field methods are becoming increasingly apparent. To improve the accuracy and reliability of path planning and enhance the algorithm's adaptability to dynamic targets, it is essential to improve the traditional artificial potential field method. By introducing a target position prediction function, the target's movement trend can be predicted in advance, enabling the algorithm to better handle dynamic targets. Simultaneously, adding a speed control function allows for dynamic adjustment of the speed, preventing collisions with obstacles due to maneuver constraints. Summary of the Invention

[0006] The purpose of this invention is to propose a velocity-controllable artificial potential field method based on predicted position, which solves the problems of traditional artificial potential field methods not being able to predict target motion and control the magnitude of velocity.

[0007] The technical solution of this invention is as follows:

[0008] To achieve the above objectives, the present invention adopts the following technical solution:

[0009] A velocity-controllable artificial potential field method based on predicted position, the method includes:

[0010] Step 1: Establish a two-dimensional map coordinate system Oxy, and collect information about the moving object, target, and obstacles through the moving object's own sensor system; where the origin O is the initial position of the moving object, the Ox axis points to the right, and the Oy axis is perpendicular to the Ox axis and points upward;

[0011] Step 2: Calculate the distance D0 from the moving object to the target point. If D0 is less than the set threshold, the moving object is considered to have reached the target point, the path planning is completed, and the process ends. If D0 is not less than the set threshold, proceed to Step 3.

[0012] Step 3: Based on the moving object information, target information, and obstacle information, calculate the estimated position coordinates X′ of the target. G ;

[0013] Step 4: Based on the target's estimated position coordinates X′ G and the X coordinates of the moved object U Calculate the gravitational force F acting on the moving object. a ;

[0014] Step 5: Based on the target's estimated position coordinates X′ G and the center position coordinates of the obstacle X T Calculate the repulsive force F experienced by the moving object. r ;

[0015] Step 6: Apply gravity F a and repulsive force F r The resultant force F = F on the composite moving object a +F r ;

[0016] Step 7: Based on the net force F acting on the moving object, calculate the velocity direction angle θ of the moving object's next motion. U (t+1);

[0017] Step 8: Calculate the magnitude U of the velocity of the moving object's next movement. U (t+1);

[0018] Step 9: Based on the current position X of the moving objectU (t), the velocity and direction angle θ of the moving object's next movement U (t+1) and speed magnitude U U (t+1), update the position X of the moved object at the next moment. U (t+1), then return to step 2.

[0019] Specifically, the information about the moved object includes the X coordinates of its position. U The speed U of the moving object U and velocity direction angle θ U ;

[0020] Target information includes the target point's location coordinates (X). G The magnitude of the target's velocity U G and velocity direction angle θ G ;

[0021] Obstacle information includes the X coordinates of the obstacle's center position. T .

[0022] Specifically, step 3 includes:

[0023] Step 31: Calculate the estimated time Δt = ||X G -X U ‖ / U U Where, ‖·‖ is the symbol for calculating the modulus;

[0024] Step 32: Based on the estimated time Δt and the target point coordinates X G The magnitude of the target's velocity U G and velocity direction angle θ G Calculate the estimated position X′ of the target point when the moving object reaches the target point. G :

[0025] X′ G =X G +U G ·[cos(θ G ),sin(θ G )]·Δt.

[0026] Specifically, step 4 includes:

[0027] Based on the target's estimated position coordinates X′ G and the X coordinates of the moved object U Calculate the gravitational force F acting on the moving object. a ,

[0028] F a (X U )=k a ·(X′ G -XU )

[0029] Where, k a For the set gravitational coefficient, F a The direction is determined by the position X of the moving object. U Pointing to the estimated position coordinates X′ of the target G .

[0030] Specifically, step 5 includes:

[0031] Based on the target's estimated location coordinates X′ G and the center position coordinates of the obstacle X T Calculate the repulsive force F experienced by the moving object. r ,

[0032]

[0033] Where, k r X is the set repulsion coefficient. Ti R is the coordinate of the center of the i-th obstacle within the moving object's field of view. i Let be the radius of the i-th obstacle.

[0034] Specifically, step 7 includes:

[0035] Step 71: Calculate the direction angle θ of the resultant force F. F ;

[0036] Step 72: Based on the direction angle θ of the resultant force F F Maximum angular velocity θ of the moving object max and the direction angle θ of the moving object's velocity U (t), calculate the turning angle θ of the moving object. a :

[0037] θ a =max(min(θ) F -θ U (t),θ max ),-θ max );

[0038] Step 73: Based on the steering angle θ a and the velocity, direction, and angle θ of the moving object at the current time t. U (t), calculate the velocity direction angle θU(t+1) of the moving object's next motion:

[0039] θ U (t+1)=θ U (t)+θ a .

[0040] Specifically, step 8 includes:

[0041] Step 81: Based on the net force F(t) acting on the moving object at the current time t and the net force F(t-1) acting on the moving object at the previous time (t-1), calculate the change in the magnitude of the moving object's velocity, a:

[0042] a=-sign(‖F(t)‖-‖F(t-1)‖)·‖F(t)-F(t-1)‖·k s

[0043] Where sign is used to determine the sign of the sign, and k s It is the set speed adjustment coefficient;

[0044] Step 82: Based on the speed U of the moving object at the current time t U Given (t) and the change in the velocity of the moving object a, calculate the velocity U of the moving object's next motion. U (t+1):

[0045] U U (t+1)=U U (t)+a

[0046] Among them, if U U (t+1) is greater than the maximum speed of the moving object, so U U (t+1) equals the maximum speed limit of the moving object.

[0047] Specifically, step 9 includes:

[0048] The next position of the moved object is X. U (t+1):

[0049] X U (t+1)=X U (t)+U U (t+1)·[cos(θ U (t+1)),sin(θ U (t+1))].

[0050] In summary, this invention proposes a velocity-controllable artificial potential field method based on predicted position, with the following beneficial effects:

[0051] 1. Improve the accuracy of path planning. The improved artificial potential field method, through target position prediction, can predict the movement trend of the target in advance, enabling moving objects to better cope with dynamic targets during the path planning process;

[0052] 2. Enhanced adaptability of path planning. The improved artificial potential field method, through speed control, can dynamically adjust the speed of the moving object based on its distance from obstacles, avoiding collisions and improving the safety and reliability of path planning. Attached Figure Description

[0053] Figure 1 This is a flowchart illustrating a velocity-controllable artificial potential field method based on predicted position, as provided in this application.

[0054] Figure 2 This paper compares the path planning performance of the method described in this application with that of the traditional artificial potential field method. Detailed Implementation

[0055] Example 1

[0056] like Figure 1-2 As shown, this application provides a velocity-controllable artificial potential field method based on predicted position, including the following steps:

[0057] Step 1: Establish a two-dimensional map coordinate system Oxy, and collect information about the moving object, target, and obstacles through the moving object's own sensor system;

[0058] Where the origin O is the initial position of the moving object, the Ox axis points to the right in the positive direction, and the Oy axis is perpendicular to the Ox axis and points upward in the positive direction.

[0059] The information about the moved object includes the X coordinates of its position. U The speed U of the moving object U and velocity direction angle θ U .

[0060] Target information includes the target point's location coordinates (X). G The magnitude of the target's velocity U G and velocity direction angle θ G .

[0061] Obstacle information includes the X coordinates of the obstacle's center position. T .

[0062] Step 2: Calculate the distance D0 from the moving object to the target point. If D0 is less than the set threshold, the moving object is considered to have reached the target point, the path planning is completed, and the process ends. If D0 is not less than the set threshold, proceed to Step 3.

[0063] Where, distance D0 = ||X G -X U ‖,‖·‖ are the symbols for calculating the modulus.

[0064] Step 3: Based on the moving object information, target information, and obstacle information, calculate the estimated position coordinates X′ of the target. G ;

[0065] Specifically, step 3 includes:

[0066] Step 31: Calculate the estimated time Δt = ||XG -X U ‖ / U U , where ‖·‖ is the symbol for calculating the modulus.

[0067] Step 32: Based on the estimated time Δt and the target point coordinates X G The magnitude of the target's velocity U G and velocity direction angle θ G Calculate the estimated position X′ of the target point when the moving object reaches the target point. G :

[0068] X′ G =X G +U G ·[cos(θ G ),sin(θ G )]·Δt.

[0069] Step 4: Based on the target's estimated position coordinates X′ G and the X coordinates of the moved object U Calculate the gravitational force F acting on the moving object. a ,

[0070] F a (X U )=k a ·(X′ G -X U (3)

[0071] Where, k a For the set gravitational coefficient, F a The direction is determined by the position X of the moving object. U Pointing to the estimated position coordinates X′ of the target G .

[0072] Step 5: Based on the target's estimated position coordinates X′ G and the center position coordinates of the obstacle X T Calculate the repulsive force F experienced by the moving object. r ,

[0073]

[0074] Where, k r X is the set repulsion coefficient. Ti R is the coordinate of the center of the i-th obstacle within the moving object's field of view. i Let be the radius of the i-th obstacle.

[0075] Step 6: Apply gravity F a and repulsive force F r The resultant force F = F on the composite moving object a +Fr Step 7: Based on the net force F acting on the moving object, calculate the velocity direction angle θ of the moving object's next motion. U (t+1);

[0076] Specifically, step 7 includes:

[0077] Step 71: Calculate the direction angle θ of the resultant force F. F ;

[0078] Step 72: Based on the direction angle θ of the resultant force F f Maximum angular velocity θ of the moving object max and the direction angle θ of the moving object's velocity U (t), calculate the turning angle θ of the moving object. a :

[0079] θ a =max(min(θ) F -θ U (t),θ max ),-θ max );

[0080] Step 73: Based on the steering angle θ a and the velocity, direction, and angle θ of the moving object at the current time t. U (t), calculate the velocity direction angle θ of the moving object's next motion. U (t+1):

[0081] θ U (t+1)=θ U (t)+θ a .

[0082] Step 8: Calculate the magnitude U of the velocity of the moving object's next movement. U (t+1);

[0083] Specifically, step 8 includes:

[0084] Step 81: Based on the net force F(t) acting on the moving object at the current time t and the net force F(t-1) acting on the moving object at the previous time (t-1), calculate the change in the magnitude of the moving object's velocity, a:

[0085] a=-sign(‖F(t)‖-‖F(t-1)‖)·‖F(t)-F(t-1)‖·k s ;

[0086] Where sign is used to determine the sign of the sign, and k s It is the set speed adjustment coefficient.

[0087] Step 82: Based on the speed U of the moving object at the current time tU Given (t) and the change in the velocity of the moving object a, calculate the velocity U of the moving object's next motion. U (t+1):

[0088] U U (t+1)=U U (t)+a

[0089] Among them, if U U (t+1) is greater than the maximum speed of the moving object, so U U (t+1) equals the maximum speed limit of the moving object.

[0090] Step 9: Based on the current position X of the moving object U (t), the velocity and direction angle θ of the moving object's next movement U (t+1) and speed magnitude U U (t+1), update the position X of the moved object at the next moment. U (t+1), then return to step 2.

[0091] Specifically, step 9 includes: moving the object's next position as X. U (t+1):

[0092] X U (t+1)=X U (t)+U U (t+1)·[cos(θ U (t+1)),sin(θ U (t+1))].

[0093] In summary, this invention proposes a velocity-controllable artificial potential field method based on predicted position, with the following beneficial effects:

[0094] 1. Improve the accuracy of path planning. The improved artificial potential field method, through target position prediction, can predict the movement trend of the target in advance, enabling moving objects to better cope with dynamic targets during the path planning process;

[0095] 2. Enhanced adaptability of path planning. The improved artificial potential field method, through speed control, can dynamically adjust the speed of the moving object based on its distance from obstacles, avoiding collisions and improving the safety and reliability of path planning.

[0096] Example 2

[0097] like Figure 1 As shown, this application provides a velocity-controllable artificial potential field method based on predicted position, including the following steps:

[0098] Step 1: Establish a two-dimensional map coordinate system Oxy, and collect information about the moving object, target, and obstacles through the moving object's own sensor system;

[0099] Where the origin O is the initial position of the moving object, the Ox axis points to the right in the positive direction, and the Oy axis is perpendicular to the Ox axis and points upward in the positive direction.

[0100] The information about the moved object includes the X coordinates of its position. U The speed U of the moving object U and velocity direction angle θ U .

[0101] Target information includes the target point's location coordinates (X). G The magnitude of the target's velocity U G and velocity direction angle θ G .

[0102] Obstacle information includes the X coordinates of the obstacle's center position. T .

[0103] Step 2: Calculate the distance D0 from the moving object to the target point. If D0 is less than the set threshold of 200, the moving object is considered to have reached the target point, the path planning is completed, and the process ends. If D0 is not less than the set threshold, proceed to Step 3.

[0104] Where, distance D0 = ||X G -X U ‖,‖·‖ are the symbols for calculating the modulus.

[0105] Step 3: Based on the moving object information, target information, and obstacle information, calculate the estimated position coordinates X′ of the target. G ;

[0106] Specifically, step 3 includes:

[0107] Step 31: Calculate the estimated time Δt = ||X G -X U ‖ / U U , where ‖·‖ is the symbol for calculating the modulus.

[0108] Step 32: Based on the estimated time Δt and the target point coordinates X G The magnitude of the target's velocity U G and velocity direction angle θ G Calculate the estimated position X′ of the target point when the moving object reaches the target point. G :

[0109] X′ G =X G +U G ·[cos(θ G ),sin(θG )]·Δt

[0110] Step 4: Based on the target's estimated position coordinates X′ G and the X coordinates of the moved object U Calculate the gravitational force F acting on the moving object. a ,

[0111] F a (X U )=k a ·(X′ G -X U (3)

[0112] Where, k a For the set gravitational coefficient, k a =1,F a The direction is determined by the position X of the moving object. U Pointing to the estimated position coordinates X′ of the target G .

[0113] Step 5: Based on the target's estimated position coordinates X′ G and the center position coordinates of the obstacle X T Calculate the repulsive force F experienced by the moving object. r ,

[0114]

[0115] Where, k r k is the set repulsion coefficient. r =1,000,000, X Ti R is the coordinate of the center of the i-th obstacle within the moving object's field of view. i Let be the radius of the i-th obstacle.

[0116] Step 6: Apply gravity F a and repulsive force F r The resultant force F = F on the composite moving object a +F r Step 7: Based on the net force F acting on the moving object, calculate the velocity direction angle θ of the moving object's next motion. U (t+1);

[0117] Specifically, step 7 includes:

[0118] Step 71: Calculate the direction angle θ of the resultant force F. F ;

[0119] Step 72: Based on the direction angle θ of the resultant force F F Maximum angular velocity θ of the moving object max and the direction angle θ of the moving object's velocity U(t), calculate the turning angle θ of the moving object. a :

[0120] θ a =max(min(θ) F -θ U (t),θ max ),-θ max );

[0121] Step 73: Based on the steering angle θ a and the velocity, direction, and angle θ of the moving object at the current time t. U (t), calculate the velocity direction angle θ of the moving object's next motion. U (t+1):

[0122] θ U (t+1)=θ U (t)+θ a .

[0123] Step 8: Calculate the magnitude U of the velocity of the moving object's next movement. U (t+1);

[0124] Specifically, step 8 includes:

[0125] Step 81: Based on the net force F(t) acting on the moving object at the current time t and the net force F(t-1) acting on the moving object at the previous time (t-1), calculate the change in the magnitude of the moving object's velocity, a:

[0126] a=-sign(‖F(t)‖-‖F(t-1)‖)·‖F(t)-F(t-1)‖·k s ;

[0127] Where sign is used to determine the sign of the sign, and k s It is the set speed adjustment coefficient, k s =0.1. Step 82: Based on the speed U of the moving object at the current time t. U Given (t) and the change in the velocity of the moving object a, calculate the velocity U of the moving object's next motion. U (t+1):

[0128] U U (t+1)=U U (t)+a

[0129] Among them, if U U (t+1) is greater than the maximum speed of the moving object (80), so U U (t+1) equals the maximum speed of the moving object, which is 80.

[0130] Step 9: Based on the current position X of the moving objectU (t), the velocity and direction angle θ of the moving object's next movement U (t+1) and speed magnitude U U (t+1), update the position X of the moved object at the next moment. U (t+1), then return to step 2.

[0131] Specifically, step 9 includes: moving the object's next position as X. U (t+1):

[0132] X U (t+1)=X U (t)+U U (t+1)·[cos(θ U (t+1)),sin(θ U (t+1))];

[0133] The above steps were simulated using MATLAB to obtain the results. Figure 2 The path planning results are shown and compared with those of the traditional artificial potential field method. The path length of the traditional artificial potential field method is 11888, while the path length planned by the method described in this application is 10848. It can be seen that the method described in this application can adapt to the moving target and reduce the planned distance compared with the traditional artificial potential field method.

[0134] The specific embodiments of the present invention have been described above. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention. These improvements and modifications should also be within the scope of protection of the present invention.

Claims

1. A velocity-controllable artificial potential field method based on estimated position, characterized in that, The method comprises: Step 1: establishing a two-dimensional map coordinate system Oxy, and collecting mobile object information, target information and obstacle information through a sensor system of the mobile object itself; wherein the coordinate origin O is the initial position of the mobile object, the Ox axis positive direction is to the right, and the Oy axis is perpendicular to the Ox axis and the positive direction is upward; Step 2: calculating the distance D0 of the mobile object to the target point, if D0 is less than the set threshold value, it is considered that the mobile object reaches the target point, the path planning is completed, and the process is ended; if D0 is not less than the set threshold value, step 3 is entered; Step 3: Calculate the estimated position coordinate X of the target according to the mobile object information, the target information and the obstacle information ′ G ; Step 4: Calculate the gravitational force F experienced by the moving object based on the estimated position coordinates X of the target ′ G and the position coordinates X of the moving object U a ;​ Step 5: Calculate repulsive force F ′ G and obstacle center position coordinate X T , received by the moving object r ; Step 6: The resultant force F experienced by the synthetic moving object is the sum of the attractive force F a and the repulsive force F r F a +F r ; Step 7: Calculate the speed direction angle θ of the next step of the moving object according to the resultant force F suffered by the moving object U (t+1); Step 8: Calculate the speed magnitude U of the next movement of the moving object U (t+1); Step 9: Update the position X of the mobile object at the next time instant according to the current position X U (t), the velocity direction angle θ of the next step of the mobile object U (t+1) and the velocity magnitude U U (t+1) of the next step of the mobile object U (t+1), and then return to Step 2.

2. The method of claim 1, wherein, The mobile object information includes a mobile object position coordinate X U , a mobile object speed size U U , and a speed direction angle θ U ; Target information includes target point position coordinates X G , target speed size U G , and speed direction angle θ G ; The obstacle information includes obstacle center position coordinates X T .

3. The method of claim 2, wherein, Step 3 comprises: Step 31: Calculate the estimated time Δt = ‖X G - X U ‖ / U U where ‖·‖ is the modulus calculation symbol; Step 32: Calculate the estimated position X G of the target point when the moving object reaches the target point according to the estimated time Δt, the target point position coordinate X G , the target speed size U G , and the speed direction angle θ ′ G : X ′ G = X G + U G · [cos(θ G ), sin(θ G )] · Δt.

4. The method of claim 2, wherein, Step 4 comprises: According to the estimated position coordinates X of the target ′ G And the position coordinates X of the moving object U , the gravitational force F received by the moving object is calculated a , F a (X U )=k a ·(X ′ G -X U ) where k a is a set gravitational coefficient, F a is directed from the mobile object position X U to the target estimated position coordinate X ′ G .

5. The method of claim 2, wherein, Step 5 comprises: According to the target estimated position coordinate X ′ G and the obstacle center position coordinate X T , the repulsive force F r experienced by the moving object is calculated where k r is a set repulsion coefficient, X Ti is the position coordinate of the center of the i-th obstacle in the field of view of the moving object, and R i is the radius of the i-th obstacle.

6. The method of claim 2, wherein, Step 7 comprises: Step 71 : Calculate the direction angle Θ of the resultant force F F ; Step 72: Calculate the steering angle θ F of the mobile object based on the direction angle θ max of the resultant force F U (t) and the maximum angular velocity θ a of the mobile object. θ a = max(min(θ F - θ U (t), θ max ), -θ max ); Step 73: Calculate the speed direction angle θU(t+1) of the moving object at the next step according to the steering angle θ(t) and the speed direction angle θ(t) of the moving object at the current time t: a and the speed direction angle θ(t) of the moving object at the current time t U (t), calculate the speed direction angle θU(t+1) of the moving object at the next step: θ U (t+1) = θ U (t) + θ a .

7. The method of claim 2, wherein, Step 8 comprises: Step 81: calculating the speed size change a of the mobile object according to the resultant force F(t) suffered by the mobile object at the current time t and the resultant force F(t-1) suffered by the mobile object at the previous time (t-1): a = -sign(||F(t) || - ||F(t-1) ||) · ||F(t) - F(t-1) || · k s wherein sign is a judgment of positive or negative sign, k s is a set speed adjustment coefficient; Step 82: According to the speed size U of the mobile object at the current time t U (t) and the speed size change amount a of the mobile object, the speed size U of the next step of the mobile object is calculated U (t+1): U U (t+1) = U U (t) + a wherein if U U (t+1) is greater than the upper limit of the speed of the moving object, U U (t+1) is equal to the upper limit of the speed of the moving object.

8. The method of claim 2, wherein, Step 9 comprises: The position of the mobile object at the next time is X U (t+1): X U (t+1) = X U (t) + U U (t+1) · [cos(θ U (t+1)), sin(θ U (t+1))].

Images