A formation adjustment method based on simulation deduction

Abstract

The present invention relates to a formation shape adjustment method based on simulation deduction, belonging to the technical field of formation control. The present invention constructs a three-dimensional scene and a vehicle model simulation to display the vehicle phalanx model in the three-dimensional scene; if an obstacle appears in the road to hinder the movement of the vehicle phalanx, the vehicle phalanx starts to automatically adjust its formation to pass through the obstacle, and after passing through, it resumes its original phalanx formation and continues to move forward according to the plan. Among them, when planning the walking route according to the position information of the obstacle, the simulated entity is assumed to be a point charge with the same kind of charge, and the obstacle is assumed to be a conductor with the same kind of charge to construct an artificial potential field. According to the force situation of the simulated entity in the artificial potential field, the walking route of the vehicle phalanx is planned. This method can accurately and quickly plan the transformation route, not only improving the efficiency of simulating the formation transformation and recovery of the vehicle phalanx, but also facilitating the handling of emergencies that occur during the movement of the vehicle phalanx.

Description

Technical Field

[0001] The present invention relates to a formation shape adjustment method based on simulation deduction, belonging to the technical field of formation control. Background Art

[0002] In order to complete the inspection task of the parade equipment column, the participants need to drive the vehicles to conduct field drills many times. However, when the vehicle formation has a breakdown or encounters an obstacle, it cannot move forward in the original formation, which is likely to cause chaos at the scene. It is necessary to command the vehicle formation to quickly change the formation and continue to move forward. Traditional field training is not only inefficient but also consumes a large amount of manpower and material resources, which is not conducive to the staff quickly finding out the rules of the vehicle formation shape change.

[0003] For the traditional vehicle formation shape change method, usually, the formation queue change method is designed according to the plan, and then verified through the actual drills of the participants. The efficiency is low and the training effect is not good. For this reason, someone proposed to simulate the column march and formation shape change of the vehicle formation in a three-dimensional virtual scene. This can not only reduce the cost required for the parade formation training, but also verify the vehicle formation shape change method quickly and efficiently for many times, effectively improving the staff's mastery of the column march and rapid formation shape change rules of the vehicle formation, and providing a basis for the staff to plan the parade formation shape change decision. However, the current simulation method does not consider how to change the formation after encountering an obstacle. Summary of the Invention

[0004] The purpose of the present invention is to provide a formation shape adjustment method based on simulation deduction to solve the problem that there is a lack of planning on how to deal with obstacles in the current simulation process.

[0005] The present invention provides a formation shape adjustment method based on simulation deduction to solve the above technical problems, which is characterized in that the adjustment method includes the following steps:

[0006] 1) Construct a three-dimensional scene according to the walking route of the vehicle formation;

[0007] 2) Obtain the position information and formation information of the vehicle formation; the position information includes longitude, latitude and altitude; the formation information includes formation shape and quantity;

[0008] 3) Obtain the road environment information of the vehicle formation driving, and extract the position information of the obstacles on the road that hinder the vehicle formation driving; the position information of the obstacles includes longitude, latitude, and the length and width of the obstacles;

[0009] 4) Judge whether the obstacle hinders the vehicle formation from moving forward according to the vehicle formation shape information and the obstacle information. If the obstacle hinders the vehicle formation from moving forward, plan the walking route according to the position information of the obstacle;

[0010] When planning the walking route according to the position information of the obstacle, the simulation entity is assumed to be a point charge with the same kind of charge, and the obstacle is assumed to be a conductor with the same kind of charge. An artificial potential field is constructed, and the walking route of the vehicle phalanx is planned according to the force condition of the simulation entity in the artificial potential field, so as to avoid the collision between the vehicle phalanx and the obstacle;

[0011] 5) Control the queue transformation of the vehicle phalanx according to the planned walking route. When the vehicle phalanx passes through the obstacle, control the vehicle phalanx to restore the original formation and plan the walking route to continue moving forward.

[0012] Through the construction of a three-dimensional scene and vehicle model simulation, the vehicle phalanx model is displayed in the three-dimensional scene in the present invention; if an obstacle appears in the road to hinder the movement of the vehicle phalanx, the vehicle phalanx starts to automatically adjust its formation to pass through the obstacle, and after passing through, it restores the original phalanx formation and continues to move forward according to the plan. Among them, when planning the walking route according to the position information of the obstacle, the simulation entity is assumed to be a point charge with the same kind of charge, and the obstacle is assumed to be a conductor with the same kind of charge. An artificial potential field is constructed, and the walking route of the vehicle phalanx is planned according to the force condition of the simulation entity in the artificial potential field. This method can accurately and quickly plan the transformation route, not only improving the efficiency of simulating the formation transformation and restoration of the vehicle phalanx, but also facilitating the handling of emergencies that occur during the movement of the vehicle phalanx.

[0013] Further, the function model of the constructed artificial potential field in step 4) is:

[0014]

[0015]

[0016] η represents the gravitational coefficient; X represents the position of the target point, X 1 represents the position of the simulation entity, ρ(X, X 1 ) 2 represents the relative position between the simulation entity and the target point, U att is the gravitational field generated by the target point on the simulation entity, K rep represents the repulsive force coefficient, X 0 represents the position of the obstacle, ρ(X 1 , X 0 ) represents the relative position between the simulation entity and the robot, ρ 0 represents the maximum influence distance of the repulsive force field, U rep represents the repulsive force field generated by the target point on the simulation entity.

[0017] The present invention simulates the process of a vehicle passing through an obstacle by using an artificial potential field. All simulation entities are assumed to be point charges with the same kind of charge, the obstacle is assumed to be a conductor with the same kind of charge, and the entire vehicle formation is assumed to be a formation composed of point charges. By constructing an artificial potential field, the motion of the point charges in the potential field is determined, providing a reliable basis for subsequent control.

[0018] Further, the force on the simulation entity in the artificial potential field in step 4) is:

[0019] F = F att + F rep

[0020] F att = ηρ(X, X 1 )

[0021]

[0022] where F represents the resultant force on the simulation entity in the artificial potential field, F att represents the gravitational force on the simulation entity in the artificial potential field, and F rep represents the repulsive force on the simulation entity in the artificial potential field.

[0023] In the artificial potential field constructed by the present invention, the simulation entity moves under the action of the repulsive force, the simulation entity will deviate from the position of the predetermined formation target point, and a gravitational force will be generated between the simulation entity and the target point; based on the sum of the gravitational force and the repulsive force received by the simulation entity as the resultant force received, the present invention can accurately simulate the motion of the simulation entity when encountering an obstacle.

[0024] Further, the simulation entity adopts the Leader - Follower mode to maintain the formation. The latter formation member takes the previous formation member as the Leader and follows its motion. The formation transformation in step 5) refers to that the position of the Leader in the team does not change, and the position of the Follower in the formation changes.

[0025] The Leader - Follower mode adopted by the present invention to control the operation of the simulation entity can accurately simulate the operation of real vehicle simulation.

[0026] Further, the motion model of the Follower is:

[0027]

[0028] where θ, θ 1 , θ′ 1respectively represent the angles between the velocity directions of the Leader, the Follower, and the virtual Follower and the horizontal direction. x(t) and y(t) respectively represent the components of the position of the Leader in the x-axis direction and the y-axis direction. x′ 1 (t) and y′ 1 (t) represent the components of the position of the virtual Follower in the x-axis direction and the y-axis direction. d 1 represents the relative distance between the Leader and the virtual Follower. θ 0 is the angle between the line connecting the positions of the Leader and the virtual Follower and the moving direction of the virtual Follower. The magnitudes of θ and θ′ i are equal.

[0029] In the present invention, by keeping the position of the Leader in the formation unchanged and changing the position of the Follower in the formation, a virtual Follower is introduced to represent the Follower at the new formation position. The angle between the velocity direction of the introduced virtual Follower and the horizontal direction is the same as that of the Leader. The Follower adjusts its own motion state according to the relative position relationship with the virtual Follower, so that the Follower moves towards the position of the virtual Follower to complete the formation transformation.

[0030] Furthermore, the calculation formula for the position deviation of the Follower relative to the virtual Follower per unit time is as follows:

[0031]

[0032] where ΔX is the position deviation of the Follower relative to the virtual Follower on the X-axis, ΔY is the position deviation of the Follower relative to the virtual Follower on the Y-axis, Δθ is the angle deviation of the Follower relative to the virtual Follower, and x 1 (t), y 1 (t) respectively represent the components of the position of the Follower in the x-axis direction and the y-axis direction.

[0033] In the coordinate system centered on the Follower in the present invention, the position deviation of the Follower relative to the virtual Follower is calculated based on the relative distance between the Leader and the virtual Follower and the angle between the velocity direction of the Follower and the horizontal direction, etc., which can accurately describe the position deviation between the two and provide reliable support for subsequent control of the Follower.

[0034] Further, if the obstacle does not hinder the vehicle phalanx from moving forward, the vehicle phalanx continues to move forward in the predetermined formation. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] Figure 1 is a flowchart of the formation adjustment method based on simulation deduction of the present invention;

[0036] Figure 2 is a schematic diagram of the formation structure adopted in the embodiment of the present invention;

[0037] Figure 3 is a schematic diagram of the Leader-Follower motion model adopted by the present invention;

[0038] Figure 4 is a schematic diagram of the artificial potential field constructed by the present invention;

[0039] Figure 5 is a schematic diagram of the formation transformation process in the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0040] The following further describes the specific embodiments of the present invention with reference to the accompanying drawings.

[0041] The present invention constructs a three-dimensional scene based on the vehicle model on the walking route of the vehicle phalanx. When the vehicle model phalanx moves forward according to the maneuvering route, it transmits the formation information of the vehicle phalanx in real time, and combines the formation information of the vehicle phalanx with the position and attitude of the vehicle phalanx model to quickly find the position of the vehicle phalanx model in the three-dimensional scene, display the vehicle phalanx model in the three-dimensional scene, and continuously update the road information according to the movement of the vehicle phalanx. If an obstacle appears in the road to hinder the movement of the vehicle phalanx, the vehicle phalanx starts to automatically adjust the formation to pass the obstacle, and resumes the original formation after passing and continues to move forward according to the plan. This method not only improves the efficiency of simulating the formation transformation and recovery of the vehicle phalanx, but also facilitates handling emergencies that occur during the movement of the vehicle phalanx. Its implementation process is as Figure 1 shown, and the specific implementation process is as follows.

[0042] 1. Construct a three-dimensional scene according to the walking route of the vehicle phalanx.

[0043] Construct a three-dimensional scene under the driving route according to the driving route of the vehicle phalanx. The three-dimensional scene includes a geographic information model under the driving route and a vehicle phalanx model.

[0044] 2. Obtain the position information and formation information of the vehicle phalanx during driving.

[0045] The vehicle travels actually according to the driving route to obtain driving data; the driving data includes the position information, attitude information of the parade formation and the driving route environment information, and the driving data is sent to the simulation system in real time. In this embodiment, the driving route of the vehicle formation is a straight line, and the start and end points of the driving route include the following parameters: the longitude x of the vehicle formation i , the latitude y of the vehicle formation i , the driving height z of the vehicle formation i , the driving speed v of the vehicle formation i , and the specific parameters are as follows:

[0046] Start point / / Waypoint name

[0047] 113.076250° / / Waypoint longitude

[0048] 34.458422° / / Waypoint latitude

[0049] 2 / / Waypoint height

[0050] 10 / / Waypoint speed

[0051] End point / / Waypoint name

[0052] 113.084751° / / Waypoint longitude

[0053] 34.458459° / / Waypoint latitude

[0054] 2 / / Waypoint height

[0055] 10 / / Waypoint speed

[0056] When driving according to the planned route, the vehicle formation travels along the specified route with a yaw angle of 0°, a pitch angle of 0°, and a roll angle of 0°; and according to the driving route and driving speed of the vehicle formation model, the position information of the vehicle formation in the geodetic coordinate system is obtained, and the position information includes longitude, latitude, and height.

[0057] 3. Obtain the road environment information of the vehicle formation driving, and extract the position information of the obstacles on the road that hinder the vehicle formation driving.

[0058] The obtained obstacle position information includes longitude, latitude, as well as the length, width, and height of the obstacle. In this embodiment, there is an obstacle on the driving route of the vehicle formation. The longitude of the center point of the obstacle is 113.080687; the latitude is 34.458416; the length is 80m; the width is 55m. Among them, the longitude of Obstacle 1 in the obstacle is 113.080506; the latitude is 34.458580; the length is 5m; the width is 3.8m; the longitude of Obstacle 2 is 113.080982; the latitude is 34.458366; the length is 6; the width is 1.5m; the longitude of Obstacle 3 is 113.080964; the latitude is 34.458502; the length is 3m; the width is 15m.

[0059] 4. Plan the formation transformation of the vehicle formation according to the vehicle formation information and obstacle information. If the obstacle does not block the forward movement of the vehicle formation queue, the vehicle formation continues to move forward in the predetermined formation.

[0060] In the vehicle formation simulation entity, the Leader-Follower mode is adopted to maintain the formation, that is, the latter formation member takes the previous formation member as the leader and follows its movement.

[0061] In this embodiment, taking five simulation entities as an example, as Figure 2 shown, each node in the figure represents a simulation entity, and the arrow indicates the following relationship. The formation can be divided into four groups according to the following relationship, which are F 1 F 2 , F 1 F 3 , F 2 F 4 , F 3 F 4 . F 1 F 2 means that F 1 is the Leader of F 2 . Its general matrix expression is as follows:

[0062]

[0063] In the general expression, F i represents the Leader of the i-th simulation entity, d i represents the relative distance between the i-th simulation entity and its Leader, represents the angle between the velocity vector sum of the i-th simulation entity and the position vector relative to its Leader.

[0064] After the formation is formed, the formation moves forward after the Leader makes a judgment based on the perceived information, and the Follower follows the Leader to move forward. Their movement relationship is asFigure 3 As shown, where l 1 and l 2 represent the path of movement, F 1 (x 1 , y 1 ) and F 2 (x 2 , y 2 ) represent the coordinate positions of the Leader and the Follower, d represents the relative position distance between F 1 and F 2 , Δx and Δy represent the relative position distances of d in the x-axis direction and the y-axis direction, and v ix and V iy represent the components of the velocity of the i-th simulation entity in the x-axis direction and the y-axis direction.

[0065] At any moment t, the position coordinates of the Leader can be expressed as follows:

[0066]

[0067] In the case of no interference, the entire team moves in a certain stable formation, that is, the relative position distance between F 1 and F 2 is fixed, so the coordinate position of the Follower can be deduced from the position of the Leader at any moment:

[0068]

[0069] 5. If an obstacle blocks the vehicle phalanx from moving forward, re-plan the driving route.

[0070] If an obstacle blocks the vehicle phalanx from moving forward, the formation of the vehicle phalanx is automatically adjusted when passing through the obstacle area; during the movement of the simulation entity, in order to avoid collisions among all members throughout the process, the present invention refers to the principle that there will be a repulsive force between a charged charge and a conductor with the same kind of charge in an electric potential field, so that the charged charge and the conductor will not collide. All simulation entities are assumed to be point charges with the same kind of charge, and the obstacle is assumed to be a conductor with the same kind of charge. The entire process can be assumed to be a formation composed of point charges moving in an electric potential field without colliding with the charged conductor. The schematic diagram of its potential field is as Figure 4 shown.

[0071] The artificial potential field perceives the surrounding environmental information through the simulation entity, constructs the gravitational field generated by the target point and the repulsive field generated by the obstacle respectively according to the positions of the target point and the obstacle, and the simulation entity moves according to the force situation in the artificial potential field. The gravitational field function model is expressed as follows:

[0072]

[0073] In the formula: η represents the gravitational coefficient; X represents the position of the target point, and X 1 represents the position of the simulation entity, and ρ(X, X 1 ) 2 represents the relative position between the simulation entity and the target point; U att is the gravitational field generated by the target point on the simulation entity. It can be obtained from the potential field function that the farther the simulation entity is from the target point, the greater the gravitational potential energy it receives, and vice versa. Therefore, the magnitude of its gravitational force can be expressed as:

[0074] F att = ηp(X, X 1 ) (5)

[0075] The repulsive force field is generated by obstacles in the environment. It has positive potential energy and there is a repulsive force on the simulation entity in the environment, and it increases as the distance between the simulation entity and the obstacle decreases. The direction is along the line connecting the simulation entity and the obstacle and away from the obstacle. When the simulation entity gets closer to the obstacle, the repulsive potential energy becomes larger. When it reaches the obstacle, its repulsive potential energy can be expressed as infinity, indicating that the simulation entity tries to avoid colliding with the obstacle. On the contrary, when the distance from the obstacle is far enough, the repulsive potential energy approaches 0. The function model of the repulsive force field is expressed as follows:

[0076]

[0077] where K rep represents the repulsive force coefficient, X 1 represents the position of the simulation entity, X 0 represents the position of the obstacle, and ρ(X 1 , X 0 ) represents the relative position between the simulation entity and the robot, and ρ 0 represents the maximum influence distance of the repulsive force field. It can be seen that when the simulation entity reaches a certain position and the distance from the obstacle exceeds the maximum influence distance ρ 0 , it will no longer be affected by the repulsive force, that is, there will be no collision. When the simulation entity enters the influence range of the obstacle, the closer it is to the obstacle, the greater the degree of its being affected by the repulsive force. Therefore, its repulsive force function can be expressed as:

[0078]

[0079] According to the above potential field function and force function, the combined potential field function for controlling the movement of the simulation entity and the resultant force received by the simulation entity can be obtained. The sum of the gravitational field and the repulsive force field function is expressed as:

[0080] S = U att + U rep (8)

[0081] The resultant force on the simulation entity is the sum of the gravitational force and the repulsive force, expressed as:

[0082] F = F att + F rep (9)

[0083] When the simulation entity passes through the artificial potential field, under the action of the gravitational field and the repulsive field, it moves according to the force situation. Since the repulsive force is infinite when the position of the simulation entity coincides with the obstacle, the simulation entity will not collide with the obstacle, and the moving direction is consistent with the direction of the resultant force F.

[0084] 6. Control the queue transformation of the vehicle phalanx according to the planned walking route. When the vehicle phalanx passes through the obstacle, control the vehicle phalanx to restore the original formation and plan the walking route to continue moving forward.

[0085] If the parade phalanx passes through the obstacle, the parade phalanx restores the original formation and plans the walking route to continue moving forward; during the whole process, the formation mainly experiences the process of formation reorganization. During this process, it is assumed that the position of the Leader in the team does not change, and the position of the Follower in the formation changes. The present invention introduces a virtual Follower to represent the Follower at the new formation position.

[0086] Figure 5 In F, F 1 , F′ 1 respectively represent the positions of the Leader, the Follower, and the virtual Follower, (x, y), (x 1 , y 1 ), (x′ 1 , y′ 1 ) respectively represent the coordinates of the Leader, the Follower, and the virtual Follower, θ, θ 1 , θ′ 1 respectively represent the angles between the velocity directions of the Leader, the Follower, and the virtual Follower and the horizontal direction. According to Figure 5 the relative position relationship shown in

[0087]

[0088] where d 1 is the distance value between the Leader and the virtual Follower, θ 0 is the angle between the line connecting the positions of the Leader and the virtual Follower and the moving direction of the virtual Follower, and the magnitudes of θ and θ′ 1 are equal, i.e., θ = θ′ 1 .

[0089] During the formation transformation process, the present invention assumes that the motion state of the Leader remains unchanged, and the Follower adjusts its own motion state according to the relative position relationship with the virtual Follower, so that the Follower moves towards the position of the virtual Follower to complete the formation transformation. Therefore, in the coordinate system centered on the Follower, the position deviation of the Follower relative to the virtual Follower per unit time is:

[0090]

[0091] Therefore, the motion model of the Follower during the formation change process can be obtained based on the change of the deviation, and the position of the Follower can be calculated to restore the original formation of the formation and continue to move forward.

[0092] In the above embodiment, for the convenience of description, the automatic adjustment method of the vehicle square formation is described in the form of steps. However, the simulation system is not limited to the above steps during the simulation process. Steps 4 and 5 can be carried out simultaneously as long as the position information of the square formation model is transmitted into the system.

[0093] The present invention displays the formation transformation process of the virtual vehicle square in real time in a three-dimensional scene when passing through an obstacle. The road information is continuously updated according to the movement of the vehicle square. If there is an obstacle in the road that hinders the movement of the vehicle square, the vehicle square starts to automatically adjust its formation to pass through the obstacle area, resumes the original square formation after passing through, and continues to move forward according to the plan, and is displayed in the three-dimensional scene. The present invention improves the efficiency of real-time simulation test of the formation transformation of the vehicle square and is more convenient for organizing the formation adjustment of the vehicle square.

Claims

1. A formation adjustment method based on simulation deduction, characterized in that, the adjustment method includes the following steps: 1) Construct a three-dimensional scene according to the walking route of the vehicle phalanx; 2) Obtain the position information and formation information of the vehicle phalanx; the position information includes longitude, latitude and altitude; the formation information includes formation and quantity; 3) Obtain the road environment information of the vehicle phalanx traveling, and extract the position information of the obstacles that hinder the vehicle phalanx from traveling on the road; the position information of the obstacles includes longitude, latitude, and the length and width of the obstacles; 4) Judge whether the obstacle hinders the vehicle phalanx from moving forward according to the vehicle phalanx formation information and obstacle information. If the obstacle hinders the vehicle phalanx from moving forward, plan the walking route according to the position information of the obstacle; When planning the walking route according to the position information of the obstacle, assume the simulation entity as a point charge with the same kind of charge, assume the obstacle as a conductor with the same kind of charge, construct an artificial potential field, and plan the walking route of the vehicle phalanx according to the force condition of the simulation entity in the artificial potential field to avoid the vehicle phalanx from colliding with the obstacle; 5) Control the queue transformation of the vehicle phalanx according to the planned walking route. When the vehicle phalanx passes through the obstacle, control the vehicle phalanx to restore the original formation and plan the walking route to continue moving forward.

2. The formation adjustment method based on simulation deduction according to claim 1, characterized in that, the function model of the artificial potential field constructed in step 4) is: η is expressed as the gravitational coefficient; X represents the position of the target point, X 1 represents the position of the simulation entity, p(X, X 1 ) 2 represents the relative position between the simulation entity and the target point, U att is the gravitational field generated by the target point on the simulation entity, K rep represents the repulsive coefficient, X 0 represents the position of the obstacle, ρ(X 1 , X 0 ) represents the relative position between the simulation entity and the robot, ρ 0 represents the maximum influence distance of the repulsive field, U rep represents the repulsive field generated by the target point on the simulation entity.

3. The formation adjustment method based on simulation deduction according to claim 2, characterized in that, the force on the simulation entity in the artificial potential field in step 4) is: F = F att +F rep F att = ηρ(X, X 1 ) Among them, F represents the resultant force on the simulated entity in the artificial potential field, F att represents the gravitational force on the simulated entity in the artificial potential field, F rep represents the repulsive force on the simulated entity in the artificial potential field.

4. The formation adjustment method based on simulation deduction according to any one of claims 1-3, characterized in that, the simulation entity adopts the Leader-Follower mode to maintain the formation. The latter formation member takes the previous formation member as the Leader and follows its movement. In step 5), the formation transformation refers to that the position of the Leader in the team does not change, and the position of the Follower in the formation changes.

5. The formation adjustment method based on simulation deduction according to claim 4, characterized in that, the motion model of the Follower is: where θ, θ 1 , θ 1 ′ respectively represent the angles between the velocity directions of the Leader, Follower, and virtual Follower and the horizontal direction, x(t) and y(t) respectively represent the components of the Leader's position in the x-axis and y-axis directions, x ′ 1 (t) and y ′ 1 (t) represent the components of the virtual Follower's position in the x-axis and y-axis directions, d 1 represents the relative distance between the Leader and the virtual Follower, θ 0 is the angle between the line connecting the positions of the Leader and the virtual Follower and the motion direction of the virtual Follower, and the magnitudes of θ and θ 1 ′ are equal.

6. The formation adjustment method based on simulation deduction according to claim 5, characterized in that, the calculation formula adopted for the position deviation of the Follower relative to the virtual Follower per unit time is: where ΔX is the position deviation of the Follower with respect to the X-axis of the virtual Follower, ΔY is the position deviation of the Follower with respect to the Y-axis of the virtual Follower, Δθ is the angular deviation of the Follower with respect to the virtual Follower, x 1 (t), y 1 (t) respectively represent the components of the position of the Follower in the x-axis direction and the y-axis direction.

7. The formation adjustment method based on simulation deduction according to any one of claims 1-3, characterized in that, if the obstacle does not hinder the vehicle phalanx from moving forward, the vehicle phalanx continues to move forward in the predetermined formation.

Images