Disclosure of Invention
The invention aims to solve the problems in the prior art and provides a space combination obstacle avoidance method combining natural force and artificial potential field, which can effectively get rid of the restriction of the obstacle and successfully reach the target position under the condition that the obstacle is large in size or complex in structure.
In order to achieve the purpose, the invention is realized by adopting the following technical scheme:
a space combination obstacle avoidance method combining natural force and artificial potential field comprises the following steps:
Acquiring a dynamic model, an initial position, a target position, a structure of an obstacle B and various parameter values of the robot, and decomposing the obstacle B into a union of a plurality of basic geometric units Bi;
Constructing a repulsive potential function for the obstacle B based on the basic geometric unit Bi of the obstacle B, the classical artificial potential function form and the distance from the point to the basic geometric unit Bi, and solving a gradient of the repulsive potential function of the obstacle B to obtain an obstacle repulsive potential field;
Designing a target position occlusion detection algorithm for the robot based on the geometry of the obstacle B and the current and target positions of the robot;
and combining the repulsive potential field of the obstacle B and a target position shielding detection algorithm to design a motion control law for the robot.
Further, the kinetic model of the robot is:
wherein ζ (t) is the robot position vector at the time t, ζ (t) is the robot speed vector at the time t, f (ζ (t), ζ (t)) is the robot open loop dynamics function, and U (t) is the robot control amount at the time t.
Further, obstacle B is decomposed into a union of nb basic geometric units:
Wherein each basic geometric unit Bi is a convex geometric body, nb is the number of basic geometric units, i=1, 2,..nb, the j-th face Fij of which is expressed in the following general form,The number of faces of the basic geometric unit Bi:
Wherein the first equation is the curved surface equation where Fij is located, the kth inequality is the constraint generated by the kth edge of Fij,The number of sides of the face Fij.
Further, the repulsive potential function of the obstacle B may be expressed as follows:
Wherein Vi is the repulsive potential function of the obstacle basic geometry unit Bi, nb is the number of basic geometry units, i=1, 2.
Further, the rejection potential function of the barrier basic geometry unit Bi is:
Where Di (x, y, z) is the distance from point P (x, y, z) to base geometry unit Bi, Do is the collision avoidance detection distance threshold, and is calculated as follows when the nearest neighbor point PNearesti coordinates (xi,yi,zi) of point P (x, y, z) on base geometry unit Bi are known:
further, the barrier repulsive potential field is:
Wherein Fi (x, y, z) is the repulsive potential field of the obstacle basic geometry unit Bi, nb is the number of basic geometry units, i=1, 2,...
Further, the Fi (x, y, z) is obtained by taking a negative gradient from the point P coordinate by the corresponding potential function Vi, and is in the form of:
Where Di (x, y, z) is the distance from point P (x, y, z) to basic geometry unit Bi, α is the obstacle rejection potential coefficient, Do is the collision avoidance detection distance threshold, and the gradient of distance Di from point P to basic geometry unit Bi at point P is:
knowing the coordinates (xi,yi,zi) of the nearest neighbor PNearesti on the basic geometric unit Bi,The method is calculated according to the following formula:
further, the target position occlusion detection algorithm is as follows:
Wherein δi (x, y, z) is an occlusion detection function of the obstacle basic geometric unit Bi to the target position, nb is the number of basic geometric units, i=1, 2.
Further, the δi (x, y, z) can be expressed as:
Wherein,As an occlusion detection function of the j-th surface Fij of the obstacle basic geometry unit Bi to the target position,The number of faces of the basic geometric unit Bi,Fetching when the target position is blocked by the surface FijOtherwise take
Further, the motion control law is:
U(t)=δ(ξ(t))(-f(ξ(t),ζ(t))-kp(ξ(t)-ξr)-kvζ(t))+Uo(t)
Where kp is a position feedback coefficient, kv is a speed feedback coefficient, ζr is a desired position of the robot, δ (ζ (t)) is a target position occlusion determination function, Uo (t) =f (ζ (t)) is a value of the obstacle repulsive potential field at the current position ζ (t) of the robot, i.e. x, y, z are replaced by three components of ζ (t), respectively.
Compared with the prior art, the invention has the following beneficial effects:
The invention provides a space combination obstacle avoidance method combining natural force and artificial potential field, which designs an obstacle occlusion detection algorithm for a target position of a robot based on the specific structural characteristics of the obstacle and the basic geometric unit composition of the space combination obstacle, and further constructs a switching control law related to the target position occlusion, so that the robot can still effectively get rid of the obstacle constraint and successfully reach the target position under the action of a nearly natural dynamics model (only performing obstacle avoidance control) under the condition of larger obstacle size or more complex structure. The invention fully utilizes the natural dynamics model to design the obstacle avoidance control law, when the target position is visible to the robot, the robot approaches to the target position under the control law of the common artificial potential field, and when the target position of the robot is shielded by an obstacle, only the obstacle avoidance item in the control law of the robot is reserved. Because the local extremum area is distributed in the obstacle shielding area, the robot can jump out the local extremum point with smaller control quantity under the action of the near natural dynamics model in the obstacle shielding area. Meanwhile, by selecting a proper coefficient, the control quantity amplitude value when the target position is not shielded can be smaller. The method is suitable for practical three-dimensional environments such as a near-earth space track and the like, and can effectively avoid movement of the obstacle with larger size or more complex structure with smaller control quantity.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments of the present invention. The components of the embodiments of the present invention generally described and illustrated in the figures herein may be arranged and designed in a wide variety of different configurations.
Thus, the following detailed description of the embodiments of the invention, as presented in the figures, is not intended to limit the scope of the invention, as claimed, but is merely representative of selected embodiments of the invention. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The invention is described in further detail below with reference to the attached drawing figures:
referring to fig. 1, the present invention provides a spatial assembly obstacle avoidance method combining natural forces and artificial potential fields, comprising the steps of:
And acquiring information such as a dynamics model, an initial position, a target position, a geometric structure of the obstacle B, a collision avoidance detection distance threshold Do, an obstacle rejection potential coefficient alpha and the like of the robot. Wherein the kinetic model of the robot is given as follows:
wherein ζ (t) is the robot position vector at the time t, ζ (t) is the robot speed vector at the time t, f (ζ (t), ζ (t)) is the robot open loop dynamics function, and U (t) is the robot control amount at the time t.
Decomposing the obstacle B into a union of nb basic geometric units
Wherein each basic geometric unit Bi(i＝1,2,...,nb) is a convex geometric body, and the j-th surface Fij%The number of faces of Bi) can be expressed in the following general form (the first equation is the surface equation where Fij is located, the kth #Number of edges for Fij) the constraint created by the kth edge of Fij):
And the distances of the points to the basic geometric units can be analyzed or calculated numerically.
Based on the basic geometric unit composition of the obstacle, classical artificial potential function form and the distance analysis formula/numerical calculation from the point to the basic geometric unit, a repulsive potential function of the following form is constructed for the obstacle:
Wherein Vi is the repulsive potential function of the obstacle basic geometry unit Bi in the form of
Where di is the distance from point P (x, y, z) to base geometry unit Bi.
Calculating the gradient of the barrier rejection potential function to obtain a barrier rejection potential field in the following form as a barrier rejection term in the robot control law:
Wherein Fi (x, y, z) is the repulsive potential field of the obstacle basic geometry Bi in the form of
Wherein,For the gradient of the distance di of point P to base geometric unit Bi at point P, the nearest neighbor point PNearesti coordinate (xi,yi,zi) of point P on base geometric unit Bi can be calculated as follows:
Based on the basic geometric unit composition of the obstacle, the current position coordinate of the robot and the target position coordinate of the robot, a target position occlusion detection algorithm with the following form is designed:
Where δi (x, y, z) is the occlusion detection function of the obstacle base geometry unit Bi(i＝1,2,...,nb) for the target position (actually representing the target visibility), further expressed as:
Wherein,Fij which is the j-th surface of the basic geometrical unit Bi of the obstacleThe number of faces of the basic geometric unit i) to the target position, the value is taken according to the following rule:
If the intersection point (available vector) of the robot current position P (x, y, z) and the target position Pr(xr,yr,zr and Fij is connectedRepresentation) satisfies the boundary constraint of Fij, i.e., a system of equations
If there is a solution, the target position of the robot is blocked by the basic geometrical unit Bi of the obstacle, and there is a corresponding solution
If fij(x+λ(xr-x),y+λ(yr-y),z+λ(zr -z) in the above equation set (11) =0 has no solution, or the solution does not satisfy the boundary constraint shown by the inequality in equation set (11), the robot target position is not occluded by the obstacle basic geometry unit Bi, and accordingly,
Delta (x, y, z) =1, i.e. the target position is visible, if and only if the robot target position Pr(xr,yr,zr) is not occluded by any face Fij of any basic geometrical unit Bi of the obstacle.
To facilitate the calculation of the occlusion of the target location, an envelope ellipsoid may be constructed for the obstacle or its basic geometric element Bi(Including the obstacle or basic geometric element Bi entirely) if a system of equations about λWith a solution, the obstacle occlusion detection function δ (x, y, z) or the occlusion detection function δi (x, y, z) of the basic geometric unit Bi is taken as 0, otherwise 1.
By taking (x, y, z) in the obstacle repulsive potential field formula (6) as the current position coordinate ζ (t) of the robot, an obstacle repulsive term Uo (t) =f (ζ (t)) of the robot is obtained, so that a motion control law of the following form is designed for the robot:
U(t)=δ(ξ(t))(-f(ξ(t),ζ(t))-kp(ξ(t)-ξr)-kvζ(t))+Uo(t) (12)
Where kp is a position feedback coefficient, kv is a speed feedback coefficient, ζr is a desired position of the robot, and δ (ζ (t)) is a target position occlusion determination function shown in formula (9). So that the robot can cross the barrier of the obstacle and successfully reach the target position xir while effectively avoiding collision with the obstacle.
The meaning of the control law formula (12) is shown in fig. 6, wherein the dynamics compensation term is-f (ζ (t)), the target attraction term is-kp(ξ(t)-ξr), the damping term is-kv ζ (t), and the obstacle rejection term is Uo (t).
And (3) carrying out robot motion simulation under a dynamic model shown in a formula (1) by using a designed control law formula (12) and various parameter settings. And if the simulation result meets the user requirement, ending, otherwise, adjusting parameters such as a collision avoidance detection distance threshold Do, an obstacle rejection potential coefficient alpha and the like, and then carrying out simulation again until the requirement is met.
Example 1:
The geometry of the spacecraft is regular and can be approximately the combination of basic geometry such as a sphere, a cuboid and the like, and the space station structure has certain complexity, so that the embodiment takes the obstacle avoidance movement of the flying spacecraft (robot) near the space station (reference spacecraft) as a research object, and the specific implementation method of the invention is described.
S1, acquiring a dynamic model, an initial position, a target position, a geometric structure of an obstacle B and other parameter values of the robot. The basic geometrical unit parameters of the assembled obstacle are given here in reference to the simplified geometry of the chinese space station as shown in table 1 (where the capsule body is a cylinder with two end faces replaced by hemispheres of the same radius):
TABLE 1 basic geometry composition of simplified geometry of Chinese space station and parameters thereof
Collision avoidance detection distance threshold Do =10m, obstacle rejection potential coefficient α=1m4/s2, position feedback coefficient kp＝10-5s-2, speed feedback coefficient kv＝10-2s-1. Robot initial position ζ (0) = (-12.1 m,0,5.1 m)T, target position ζr＝(12.1m,0,5.1m)T, dynamics model is of the form:
Wherein the method comprises the steps ofΩ is the angular velocity of the orbit of the reference spacecraft around the earth (taken as 1.131×10-3 rad/s in the case of an orbit altitude of 400 m), and U (t) is the control quantity of the desired design.
S2, decomposing the obstacle into a union of basic geometric units. Since the obstacle geometry has been given in the form of basic geometrical units, as shown in fig. 3, fig. 3 (a) is a sphere, fig. 3 (b) is a capsule, fig. 3 (c) is a cuboid, and each basic geometrical unit is a convex body, the geometrical decomposition is no longer performed. Only the distance formula of the point to each basic geometric unit is given as shown in table 2:
TABLE 2 distance formula for point P (x, y, z) to several simple geometries
The surface constraint formula for each basic geometry unit is shown in table 3:
TABLE 3 surface element equations for several simple geometries
The distance from the point P (x, y, z) to each basic geometric unit can be formulated as follows (the subscript number corresponds to the basic geometric unit number) taking into account the actual orientation of each basic geometric unit:
Capsule body 1:
Capsule body 2:
cuboid 3, 4, 7:
Cuboid 5, 6:
The face constraint formula for each basic geometric element is expressed as follows (subscript number corresponds to basic geometric element number):
Capsule body 1:
Side F11:
Bottom surface F12、F13 (complete sphere instead): Capsule body 2:
Side surface
Bottom surface(Complete sphere substitution): cuboid 3, 4, 7 (×replacing basic geometric unit numbers):
Front and backLeft and right sides
Upper and lower surfaces
Cuboid 5, 6 (×instead of basic geometric unit number):
Front and backLeft and right sides
Upper and lower surfaces
S3, constructing a repulsive potential function for the obstacle based on basic geometric unit composition of the obstacle, classical artificial potential function form and a point-to-basic geometric unit distance analysis formula/numerical calculation:
Wherein Vi is the repulsive potential function of the obstacle basic geometry unit Bi, which is in the form of:
Where di is the distance from point P (x, y, z) to base geometry Bi, which can be calculated from the obstacle base geometry parameters shown in Table 1 and the point-to-base geometry distance formulas given in Table 2. The specific formula of di for different geometric units is (subscript corresponds to basic geometric unit number):
In order to visually display the barrier rejection potential function, fig. 4 shows a cloud of potential function value distributions for the space around the barrier.
S4, calculating the gradient of the barrier rejection potential function to obtain a barrier rejection potential field in the following form to serve as a barrier rejection term in the robot control law:
wherein Fi (x, y, z) is the repulsive potential field of the obstacle basic geometry unit Bi, which is in the form of:
Wherein the method comprises the steps ofFor the gradient of the distance di of point P (x, y, z) to base geometric unit Bi at point P, the following formula may be calculated for each base geometric unit (subscript corresponds to base geometric unit number):
S5, designing a target position occlusion detection algorithm in the following form based on the basic geometric unit composition of the barrier, the current position coordinate of the accompanying spacecraft and the target position coordinate of the accompanying spacecraft:
where δi (x, y, z) is the occlusion detection function (real object visibility) of the obstacle base geometry unit Bi (i=1, 2,..7) for the object position, which can be further expressed as:
Fij which is the j-th surface of the basic geometrical unit Bi of the obstacleThe number of faces of the basic geometric unit i) to the target position (real target visibility), the value is taken according to the following rule:
Capsule body 1:
When (when)In the case of a solution, the solution,Otherwise
When (when)In the case of a solution, the solution,Otherwise
When (when)In the case of a solution, the solution,Otherwise
Capsule body 2:
When (when)In the case of a solution, the solution,Otherwise
When (when)In the case of a solution, the solution,Otherwise
When (when)In the case of a solution, the solution,Otherwise
Cuboid 3, 4, 7 (×replacing basic geometric unit numbers):
When (when)In the case of a solution, the solution,Otherwise
When (when)In the case of a solution, the solution,Otherwise
When (when)In the case of a solution, the solution,Otherwise
When (when)In the case of a solution, the solution,Otherwise
When (when)In the case of a solution, the solution,Otherwise
When (when)In the case of a solution, the solution,Otherwise
Cuboid 5, 6 (×instead of basic geometric unit number):
When (when)In the case of a solution, the solution,Otherwise
When (when)In the case of a solution, the solution,Otherwise
When (when)In the case of a solution, the solution,Otherwise
When (when)In the case of a solution, the solution,Otherwise
When (when)In the case of a solution, the solution,Otherwise
When (when)In the case of a solution, the solution,Otherwise
Delta (x, y, z) =1, i.e. the target position is visible, if and only if the spacecraft target position Pr(xr,yr,zr) is not occluded by any face Fij of any basic geometrical unit Bi of the obstacle. Thus, δ (x, y, z) =0 can be taken whenever it is detected that the target position is occluded by one surface without traversing each surface of each basic geometric unit.
S6, taking (x, y, z) in the barrier rejection potential field formula (24) as a current position coordinate xi (t) of the robot to obtain a barrier rejection term Uo (t) = F (xi (t)) of the robot, so as to design a motion control law (point P (x, y, z) of the robot is taken as the current position coordinate xi (t)) of the robot:
U(t)=δ(ξ(t))(-D21ξ(t)-D22ζ(t)-kpξ(t)-kvζ(t))+Uo(t) (32)
here, δ (ζ (t)) is a target position occlusion determination function shown in formula (30).
And S7, carrying out robot obstacle avoidance motion simulation under a dynamic model shown in a formula (13) based on the control law formula (32) and the parameter values set in the S1, wherein the simulation time is 40000S, and the obtained results are shown in fig. 6-9. It has been found that under the influence of the designed control law, the robot can successfully surmount the obstacle, converge to the target position with a high degree of accuracy, and the magnitude of the control quantity is also in a suitable magnitude. In contrast, simulation results without considering the target position shielding property (i.e., δ (ζ (t)) is constant 1) are shown in fig. 10 to 11. It was found that the robot was trapped in a local area near the obstacle and failed to reach the target position.
The above is only a preferred embodiment of the present invention, and is not intended to limit the present invention, but various modifications and variations can be made to the present invention by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.