A two-dimensional turntable obstacle avoidance topology relationship modeling and planning method

Abstract

The application discloses a two-dimensional rotary table obstacle avoidance topology relationship modeling and planning method and belongs to the technical field of satellite task planning. The method comprises the following steps: according to intersection information of an original task planning trajectory and a sun circle and a rugged limiting curve, an obstacle avoidance ellipse trajectory curve of a two-dimensional rotary table is planned; a plurality of equally spaced key path points are selected on three curves, namely the obstacle avoidance ellipse trajectory curve, a fitting curve of an original task planning trajectory starting point and an obstacle avoidance ellipse trajectory curve starting point and a fitting curve of an original task planning trajectory ending point and an obstacle avoidance ellipse trajectory curve ending point; and according to a minimum jerk algorithm, a path segment composed of all adjacent two key path points is fitted and calculated to obtain an optimal task planning trajectory of the two-dimensional rotary table. The application can realize trajectory planning of the two-dimensional rotary table for simultaneously avoiding sunlight and limiting ruggedness, so that a task can be smoothly executed.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of satellite mission planning, in particular to a two-dimensional turntable obstacle avoidance topology relationship modeling and planning method. BACKGROUND

[0002] A wide-area surveillance satellite uses a precision tracking camera installed on a two-dimensional turntable to achieve rapid maneuvering tracking of space targets. However, the rotation of the two-dimensional turntable is subject to strong nonlinear geometric constraints caused by the position limit of the on-board sensor and the sunlight avoidance of the precision tracking camera, resulting in rugged limit problems. Therefore, when planning the tracking target trajectory on the satellite, it is necessary to consider whether the rotation angle of the turntable will cause the precision tracking camera to point to the sun or the precision tracking camera to collide with other sensors.

[0003] In related technologies, it is usually impossible to simultaneously perform trajectory planning for sunlight avoidance and rugged limit, which results in the inability to complete many tracking tasks of space targets in actual application processes.

[0004] Therefore, there is an urgent need for a two-dimensional turntable obstacle avoidance topology relationship modeling and planning method to solve the above technical problems. SUMMARY

[0005] The present application provides a two-dimensional turntable obstacle avoidance topology relationship modeling and planning method, which can realize simultaneous trajectory planning for sunlight avoidance and rugged limit of the on-board turntable. The technical solution is as follows:

[0006] On the one hand, a two-dimensional turntable obstacle avoidance topology relationship modeling and planning method is provided, which includes:

[0007] According to the intersection information of the original mission planning trajectory and the sun circle and the rugged limit curve, an obstacle avoidance elliptical trajectory curve of the two-dimensional turntable is planned;

[0008] On the three curves of the obstacle avoidance elliptical trajectory curve, the fitting curve of the original mission planning trajectory starting point and the obstacle avoidance elliptical trajectory curve starting point, and the fitting curve of the original mission planning trajectory ending point and the obstacle avoidance elliptical trajectory curve ending point, a plurality of equally spaced key path points are selected respectively;

[0009] According to the minimum jerk algorithm, the path segment composed of all adjacent two key path points is fitted and calculated to obtain the optimal mission planning trajectory of the two-dimensional turntable.

[0010] On the other hand, a two-dimensional turntable obstacle avoidance topology relationship modeling and planning device is provided, which includes:

[0011] A planning module is configured to plan an obstacle avoidance elliptical trajectory curve of the two-dimensional turntable according to the intersection information of the original mission planning trajectory and the sun circle and the rugged limit curve;

[0012] A selection module is configured to select a plurality of equally spaced key path points on each of the three curves, i.e., the obstacle avoidance ellipse trajectory curve, the fitting curve of the original task planning trajectory starting point and the obstacle avoidance ellipse trajectory curve starting point, and the fitting curve of the original task planning trajectory ending point and the obstacle avoidance ellipse trajectory curve ending point.

[0013] A calculation module is configured to perform fitting calculation on a path segment composed of two adjacent key path points according to a minimum jerk algorithm to obtain an optimal task planning trajectory of the two-dimensional turntable.

[0014] In another aspect, a computer device is provided, which includes a memory and a processor, the memory is configured to store a computer program, and the processor is configured to execute the computer program stored in the memory to implement the steps of the above-mentioned two-dimensional turntable obstacle avoidance topological relationship modeling and planning method.

[0015] In another aspect, a computer readable storage medium is provided, which stores a computer program, and the computer program is executed by a processor to implement the steps of the above-mentioned two-dimensional turntable obstacle avoidance topological relationship modeling and planning method.

[0016] In another aspect, a computer program product is provided, which includes a computer program, and the computer program is executed by a processor to implement the steps of the above-mentioned two-dimensional turntable obstacle avoidance topological relationship modeling and planning method.

[0017] The technical solution provided by the present application can bring at least the following beneficial effects: first, the sun avoidance trajectory curve of the two-dimensional turntable is planned according to the intersection relationship between the original task planning trajectory and the sun circle, then a plurality of key points are selected at equal intervals on each of the sun avoidance trajectory curve, the fitting curve of the original task planning trajectory starting point and the new task planning trajectory starting point, and the fitting curve of the original task planning trajectory ending point and the new task planning trajectory ending point, and finally the motion trajectory between any two key points is re-planned based on the minimum jerk algorithm to obtain the optimal task planning trajectory of the two-dimensional turntable. Through the method, the trajectory planning of the two-dimensional turntable for sunlight avoidance and rugged limitation at the same time is realized, and the task can be successfully executed. BRIEF DESCRIPTION OF DRAWINGS

[0018] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings needed to be used in the embodiments or prior art description. Obviously, the drawings in the following description are some embodiments of the present application, and other drawings can also be obtained by those skilled in the art without creative labor.

[0019] Figure 1 is a flow chart of the two-dimensional turntable obstacle avoidance topological relationship modeling and planning method provided by an embodiment of the present application.

[0020] Figure 2 is a schematic diagram of the sun circle and the rugged limit curve not having intersection points provided by an embodiment of the present application;

[0021] Figure 3 is a schematic diagram of the sun circle and the rugged limit curve having intersection points provided by an embodiment of the present application;

[0022] Figure 4 is a schematic diagram of the original task planning trajectory only having intersection points with the rugged limit curve provided by an embodiment of the present application;

[0023] Figure 5 is a schematic diagram of the intersection topology structure of the sun circle and the turntable limit frame provided by an embodiment of the present application;

[0024] Figure 6 is a schematic diagram of the full three-segment trajectory key path point selection provided by an embodiment of the present application;

[0025] Figure 7 is a schematic diagram of the turntable obstacle avoidance planning trajectory provided by an embodiment of the present application;

[0026] Figure 8 is a schematic diagram of the turn angle, angular velocity, angular acceleration and angular jerk of the turntable obstacle avoidance planning trajectory provided by an embodiment of the present application;

[0027] Figure 9 is a structure diagram of the two-dimensional turntable obstacle avoidance topology relationship modeling and planning device provided by an embodiment of the present application;

[0028] Figure 10 is a hardware architecture diagram of a computer device provided by an embodiment of the present application. DETAILED DESCRIPTION

[0029] In order to make the purpose, technical scheme and advantages of the embodiments of the present application clearer, the technical scheme in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are some embodiments of the present application, rather than all the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the protection scope of the present application.

[0030] As described above, the existing two-dimensional turntable on the satellite is often affected by the strong nonlinear geometric constraints caused by the limit of the on-satellite sensor and the sunlight avoidance of the precision tracking camera, resulting in the rugged limit problem. The existing technology cannot simultaneously plan the sunlight avoidance and the rugged limit.

[0031] Based on this, the concept of the present application is to re-plan the task trajectory according to the intersection information of the sun projection and the path curve of the rugged limit in the turntable coordinate system and the original task planning trajectory, to realize the planning of sun avoidance and rugged limit at the same time.

[0032] The specific implementation of the above concept is described below.

[0033] Please refer to Figure 1 The two-dimensional turntable obstacle avoidance topology relationship modeling and planning method provided by the embodiment of the present application comprises:

[0034] Step 100, according to the intersection information of the original task planning trajectory and the sun circle and the rugged limit curve, an obstacle avoidance ellipse trajectory curve of the two-dimensional turntable is planned;

[0035] Step 102, on the obstacle avoidance ellipse trajectory curve, the fitting curve of the original task planning trajectory starting point and the obstacle avoidance ellipse trajectory curve starting point, and the fitting curve of the original task planning trajectory ending point and the obstacle avoidance ellipse trajectory curve ending point, a plurality of equally spaced key path points are selected respectively;

[0036] Step 104, according to the minimum jerk algorithm, the path segment composed of all adjacent two key path points is fitted and calculated to obtain the optimal task planning trajectory of the two-dimensional turntable.

[0037] In the embodiment of the present application, first, the sun avoidance trajectory curve of the two-dimensional turntable is planned according to the intersection relationship between the original task planning trajectory and the sun circle, then a plurality of key points are selected at equal intervals on each curve of the sun avoidance trajectory curve, the fitting curve of the original task planning trajectory starting point and the new task planning trajectory starting point, and the fitting curve of the original task planning trajectory ending point and the new task planning trajectory ending point, and finally the motion trajectory between any two key points is re-planned based on the minimum jerk algorithm to obtain the optimal task planning trajectory of the two-dimensional turntable. Through this method, the trajectory planning of the two-dimensional turntable for sun avoidance and rugged limit at the same time is realized, so that the task can be successfully executed.

[0038] The execution mode of each step shown in the following description Figure 1

[0039] First, for step 100, an obstacle avoidance ellipse trajectory curve of the two-dimensional turntable is planned according to the intersection information of the original task planning trajectory and the sun circle and the rugged limit curve.

[0040] In the embodiment of the present application, the planning of the obstacle avoidance ellipse trajectory curve comprises the following processes:

[0041] ​When the original task planning trajectory intersects the sun circle at points A and B, the perpendicular bisector of the line segment connecting points A and B intersects the first perpendicular line of the sun circle at points C1 and C2, and a first elliptical path trajectory is constructed with points C1 and C2 as the endpoints of the minor axis and with a semi-major axis greater than the radius of the sun circle;

[0042] When the original task planning trajectory does not intersect the sun circle but intersects the rugged limit curve at points A' and B', an obstacle avoidance circle is constructed with points A', B' and the inflection point of the rugged limit curve, the perpendicular bisector of the line segment connecting points A' and B' intersects the second perpendicular line of the obstacle avoidance circle at points C1' and C2', and a second elliptical path trajectory is constructed with points C1' and C2' as the endpoints of the minor axis and with a semi-major axis greater than the radius of the obstacle avoidance circle;

[0043] Further, the obstacle avoidance elliptical trajectory curve is selected from the first elliptical path trajectory or the second elliptical path trajectory according to a predetermined selection criterion, comprising:

[0044] When the first elliptical path trajectory is selected, if the sun circle intersects the rugged limit curve, the first elliptical path trajectory is selected as the obstacle avoidance elliptical trajectory curve with the first perpendicular line point above the line segment connecting points A and B as the center of symmetry, and the target angle being the included angle between points A and B and the center of the sun circle.

[0045] If the sun circle does not intersect the rugged limit curve, the target angle is the inferior arc included angle between points A and B and the center of the sun circle, and the elliptical arc on the first elliptical path trajectory corresponding to the target angle is selected as the obstacle avoidance elliptical trajectory curve.

[0046] When the second elliptical path trajectory is selected, the second elliptical path trajectory is selected as the obstacle avoidance elliptical trajectory curve with the second perpendicular line point above the rugged limit curve as the center of symmetry, and the target angle being the included angle between points A' and B' and the center of the sun circle.

[0047] Specifically, the sun circle is a representation of the sun projection on the turntable coordinate system, as shown in Figure 2 , Figure 2 The circular curve in the figure is the sun circle, and the first elliptical path trajectory shown in the ellipse in Figure 2 can be planned according to the intersection points A and B of the original task planning trajectory and the sun circle. At this time, there is no intersection between the sun circle and the rugged limit, and in order to reduce the length of the turntable trajectory, the elliptical arc corresponding to the inferior arc included angle between points A and B and the center of the sun circle is selected as the obstacle avoidance elliptical trajectory curve.

[0048] As shown in Figure 3 , Figure 3This is a schematic diagram when the sun circle intersects with both the rugged limit and the original mission planning trajectory. In this case, the first perpendicular line intersection point C1 located above the line connecting intersection points A and B is taken as the center of symmetry, and the angle between the major arc of intersection points A and B and the center of the sun circle is taken as the target angle. The arc on the ellipse corresponding to this angle is selected as the obstacle avoidance elliptical trajectory curve.

[0049] It is worth noting that, Figure 3 It only provides one possibility when the sun circle intersects with both the rugged limit and the original mission planning trajectory. In actual application, regardless of whether the included angle corresponds to the superior or inferior arc, the elliptical arc corresponding to the target angle and located above the rugged limit curve is selected as the obstacle avoidance elliptical trajectory curve.

[0050] like Figure 4 As shown, Figure 4 In the case where the original mission trajectory does not intersect with the sun circle but does intersect with the rugged limit curve, an obstacle avoidance circle can be established using the intersection points A' and B' and the inflection point of the rugged limit curve. Then, a second elliptical path trajectory is established through the above process. Next, taking the intersection point C1' of the second perpendicular line above the rugged limit curve as the center of symmetry, and the angle between the intersection points A' and B' and the center of the sun circle as the target angle, the arc above the rugged limit curve corresponding to the target angle on the elliptical path trajectory is selected as the obstacle avoidance elliptical trajectory curve.

[0051] It is understandable that when part of the arc corresponding to the target angle is located below the rugged limiting curve, the part of the arc above the rugged limiting curve is preferentially selected with the intersection of the second vertical line located above the rugged limiting curve as the center of symmetry.

[0052] Then, for step 102, multiple equally spaced critical path points are selected on each of the three curves: the obstacle avoidance elliptical trajectory curve, the fitting curve between the starting point of the original task planning trajectory and the starting point of the obstacle avoidance elliptical trajectory curve, and the fitting curve between the ending point of the original task planning trajectory and the ending point of the obstacle avoidance elliptical trajectory curve.

[0053] like Figure 5 , Figure 6 and Figure 7 As shown, firstly, 10 equally spaced points are selected on the determined obstacle avoidance elliptical trajectory curve to represent the critical path points [R]. 21 ,R 22 ,…,R 2n ], n∈[1,10];

[0054] Next, based on the recorded positions of the starting point S0 and the ending point S1 of the original task planning trajectory, a curve R from the starting point S0 to the starting point R of the obstacle avoidance elliptical trajectory is fitted. 21 The B-spline curve is used to select 10 critical path points at equal intervals [R]. 11 ,R 12 ,…,R1n ], n e [1, 10]; fitting end point S1 to end point R of the obstacle-avoiding elliptical trajectory curve 210 , a B-spline curve, taking 10 key path points [R 31 , R 32 , …, R 3n ], n e [1, 10] at equal intervals.

[0055] Thus, 30 key path points for characterizing the optimal task planning trajectory are obtained, and it is worth noting that, in order to enable the two-dimensional turntable to run smoothly on the newly planned trajectory, the speed and acceleration of the obstacle-avoiding elliptical trajectory curve start point R 21 and the previous key path point adjacent thereto need to be consistent, and the speed and acceleration of the obstacle-avoiding elliptical trajectory curve end point R 210 and the next key path point adjacent thereto also need to be consistent.

[0056] For step 104, the path segment composed of all adjacent two key path points is calculated according to a minimum jerk algorithm to obtain the optimal task planning trajectory of the two-dimensional turntable.

[0057] In the embodiment of the application, the process of fitting calculation includes time allocation of each path segment according to a preset total time, and adjustment of the allocated time according to a preset maximum angular velocity threshold and a maximum angular acceleration threshold to obtain an optimal time allocation result; the trajectory of each path segment is fitted by using a fifth-order polynomial according to the optimal time allocation result; and the trajectory parameters of the fitting are solved by using a minimum jerk algorithm to obtain the optimal task planning trajectory.

[0058] Specifically, the total planning time needs to be input in advance before the fitting calculation is performed to allocate the time of each trajectory segment, and it is necessary to ensure that the maximum angular velocity and the maximum angular acceleration of the segmented trajectory are less than the set threshold. Therefore, in the embodiment, the original task planning total time can be used preferentially, and if the planning angular velocity and angular acceleration are out of limit, the time limit is gradually released, and finally the result of the optimal time of the current task curve is achieved.

[0059] Further, the trajectory of each path segment is fitted by using a fifth-order polynomial, and the mth trajectory is:

[0060]

[0061] In the formula, m is the serial number of the path segment; p m,0 to p m,5 are trajectory parameters of the mth trajectory fitting; and t is the optimal time of each path segment.

[0062] Further, the fourth derivative of the trajectory of each path segment is calculated as

[0063]

[0064] Finally, the fourth derivative is obtained:

[0065]

[0066] Then the cost function J of the mth trajectory segment is constructed m :

[0067]

[0068] The total cost function J of all trajectory segments is constructed:

[0069]

[0070] In the formula, J m is the cost function corresponding to the mth trajectory segment; Y m-1 is the m-1th key path point, i.e. the starting point of the mth trajectory segment; Y m is the mth key path point, i.e. the ending point of the mth trajectory segment; Q m is the positive definite matrix of the mth trajectory segment; P m is the fourth derivative of the mth trajectory segment.

[0071] Finally, according to the position, velocity, acceleration and other constraint conditions of each key path point, the cost function can be represented by the following formula:

[0072] J = min d T A -T QA -1 d

[0073] Since the intermediate key path points only have position information determined, without speed and acceleration information, d m is split into the variable d mF (i.e. the variable fixed by the constraint condition) and the variable d mP (i.e. the variable to be optimized, such as the high-order derivative of each path endpoint), which is represented as Substitute it into the cost function and combine the constraint conditions to obtain:

[0074]

[0075] In the formula, C is the selection matrix; d F is the variable fixed by the constraint condition; d P is the variable to be optimized; and the matrix A is the mapping matrix of the fifth-order polynomial coefficient to each derivative d.

[0076] Please refer to Figure 9The embodiment of the present application provides a two-dimensional rotary table obstacle avoidance topology relationship modeling and planning device, the device comprises:

[0077] A planning module 900 is configured to plan an obstacle avoidance ellipse trajectory curve of the two-dimensional rotary table according to intersection information of the original task planning trajectory and the sun circle and the rugged limiting curve.

[0078] A selection module 902 is configured to select a plurality of equally spaced key path points on the obstacle avoidance ellipse trajectory curve, a fitting curve of an original task planning trajectory starting point and an obstacle avoidance ellipse trajectory curve starting point, and a fitting curve of an original task planning trajectory ending point and an obstacle avoidance ellipse trajectory curve ending point.

[0079] A calculation module 904 is configured to perform fitting calculation on a path segment composed of all adjacent two key path points according to a minimum jerk algorithm, and obtain an optimal task planning trajectory of the two-dimensional rotary table.

[0080] In the embodiment of the present application, when the planning module 900 performs planning to obtain the obstacle avoidance ellipse trajectory curve of the two-dimensional rotary table according to the intersection information of the original task planning trajectory and the sun circle and the rugged limiting curve, the following operations are specifically performed: when the original task planning trajectory and the sun circle have intersection points A and B, the first vertical line intersection points C1 and C2 of the perpendicular bisector of the line connecting the intersection points A and B and the sun circle are calculated, and the first ellipse path trajectory with the half major axis greater than the radius of the sun circle is constructed with the intersection points C1 and C2 as the short axis vertices; when the original task planning trajectory and the sun circle do not have intersection points, and the rugged limiting curve has intersection points A' and B', an obstacle avoidance circle is established with the intersection points A', B' and the inflection point of the rugged limiting curve, the second vertical line intersection points C1' and C2' of the perpendicular bisector of the line connecting the intersection points A' and B' and the obstacle avoidance circle are calculated, and the second ellipse path trajectory with the half major axis greater than the radius of the obstacle avoidance circle is constructed with the intersection points C1' and C2' as the short axis vertices; and the obstacle avoidance ellipse trajectory curve is selected from the first ellipse path trajectory or the second ellipse path trajectory according to a preset selection standard.

[0081] In the embodiment of the present application, when the planning module 900 performs selection according to the preset selection standard from the first elliptical path trajectory or the second elliptical path trajectory to obtain the obstacle-avoiding elliptical trajectory curve, the following operations are specifically performed: when the first elliptical path, if the sun circle and the rugged limiting curve have intersection points, the first perpendicular intersection point located above the line connecting the intersection points A and B is taken as the center of symmetry, the included angle between the intersection points A and B and the sun center is taken as the target angle, and the circular arc corresponding to the target angle on the first elliptical path trajectory and located above the rugged limiting curve is selected as the obstacle-avoiding elliptical trajectory curve; if the sun circle and the rugged limiting curve have no intersection points, the inferior arc included angle between the intersection points A and B and the sun center is taken as the target angle, and the circular arc corresponding to the target angle on the first elliptical path trajectory is selected as the obstacle-avoiding elliptical trajectory curve; when the second elliptical path, the second perpendicular intersection point located above the rugged limiting curve is taken as the center of symmetry, the included angle between the intersection points A' and B' and the sun center is taken as the target angle, and the circular arc corresponding to the target angle on the second elliptical path trajectory and located above the rugged limiting curve is selected as the obstacle-avoiding elliptical trajectory curve.

[0082] In the embodiment of the present application, when the calculation module 904 performs fitting calculation on the path segment composed of all adjacent two key path points according to the minimum jerk algorithm to obtain the optimal task planning trajectory of the two-dimensional turntable, the following operations are specifically performed: time distribution is performed on each path segment according to the preset total time, and the distributed time is adjusted according to the preset maximum angular velocity threshold and the maximum angular acceleration threshold to obtain an optimal time distribution result; the trajectory of each path segment is fitted by using a five-order polynomial according to the optimal time distribution result; and the fitted trajectory parameters are solved by using the minimum jerk algorithm to obtain the optimal task planning trajectory.

[0083] In the embodiment of the present application, the trajectory of the path segment is fitted by the following formula:

[0084]

[0085] In the formula, m is the serial number of the path segment; p m,0 p m,5 are the trajectory parameters of the mth trajectory fitting; and t is the optimal time of each path segment.

[0086] In the embodiment of the present application, when the calculation module 904 performs solving of the fitted trajectory parameters by using the minimum jerk algorithm to obtain the optimal task planning trajectory, the following operations are specifically performed:

[0087] The fourth derivative of the trajectory of each path segment is calculated

[0088]

[0089] According to the calculation result, a total cost function J of all path segment trajectories is constructed:

[0090]

[0091] In the formula, J m is the cost function corresponding to the mth trajectory; Y m-1 is the m-1th key path point; Y m is the mth key path point; Q m is a positive definite matrix of the mth trajectory.

[0092] According to the constraint condition of each key path point, the trajectory parameters are calculated to obtain the optimal task planning trajectory:

[0093]

[0094] In the formula, C is a selection matrix; d F is a variable fixed by the constraint condition; d P is a variable to be optimized; and the matrix A is a mapping matrix in which the coefficients of the fifth order polynomial are mapped to each derivative d.

[0095] It should be noted that the two-dimensional turntable obstacle avoidance topology relationship modeling and planning device provided in the above embodiment is only exemplified by the division of the above functional modules. In actual application, the above functions can be distributed to be completed by different functional modules according to needs, that is, the internal structure of the device is divided into different functional modules to complete all or part of the functions described above. In addition, the two-dimensional turntable obstacle avoidance topology relationship modeling and planning device and the two-dimensional turntable obstacle avoidance topology relationship modeling and planning method provided in the above embodiment belong to the same concept, and the specific implementation process is detailed in the method embodiment, which will not be repeated here.

[0096] Embodiments of the present application also provide a computer device, which refers to Figure 10 The computer device includes a processor and a memory, and the memory stores at least one instruction, at least one program, a code set or an instruction set. The at least one instruction, at least one program, code set or instruction set is loaded and executed by the processor to implement the two-dimensional turntable obstacle avoidance topology relationship modeling and planning method provided by each method embodiment.

[0097] Embodiments of the present application also provide a computer readable storage medium, which stores at least one instruction, at least one program, a code set or an instruction set. The at least one instruction, at least one program, code set or instruction set is loaded and executed by the processor to implement the two-dimensional turntable obstacle avoidance topology relationship modeling and planning method provided by each method embodiment.

[0098] The embodiment of the application further provides a computer program product, which comprises a computer program, and a processor of a computer device reads the computer program from a computer readable storage medium, and the processor executes the computer program, so that the computer device executes the two-dimensional turntable obstacle avoidance topology relationship modeling and planning method in any of the above embodiments.

[0099] For the convenience of description, the above system or device is described in various modules or units in terms of functions. Of course, in the implementation of the application, the functions of each unit can be implemented in the same or multiple software and / or hardware.

[0100] From the above description of the embodiments, those skilled in the art can clearly understand that the application can be implemented by means of software and the necessary general hardware platform. Based on this understanding, the technical solutions of the application can be embodied in the form of a software product, which can be stored in a storage medium, such as a ROM / RAM, a magnetic disk, an optical disk, etc., and includes a number of instructions to make a computer device (which can be a personal computer, a server, or a network device, etc.) execute the methods described in various embodiments or some parts of the embodiments.

[0101] Finally, it should be noted that in this document, relational terms such as first and second and third and fourth, and the like can only be used to distinguish one entity or action from another entity or action, without necessarily requiring or implying any such actual relationship or order between such entities or actions. Moreover, the terms "comprises", "comprising", or any other variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but can include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without more limitations, an element defined by the phrase "comprising a" does not exclude the existence of additional identical elements in the process, method, article, or apparatus that includes the element.

[0102] The above description is only the preferred embodiment of the application, and it should be pointed out that for those skilled in the art, without departing from the principles of the application, a number of improvements and refinements can be made, which should be regarded as the protection scope of the application.

Claims

1. A method for modeling and planning of obstacle avoidance topological relations of a two-dimensional rotary table, characterized in that, The method comprises: According to the intersection information of the original task planning trajectory and the sun circle and the rugged limiting curve, an obstacle-avoiding elliptical trajectory curve of the two-dimensional turntable is planned; On the three curves of the obstacle-avoiding elliptical trajectory curve, the fitting curve of the original task planning trajectory starting point and the obstacle-avoiding elliptical trajectory curve starting point, and the fitting curve of the original task planning trajectory ending point and the obstacle-avoiding elliptical trajectory curve ending point, a plurality of equally spaced key path points are selected respectively; According to the minimum jerk algorithm, the path segment composed of all adjacent two key path points is fitted and calculated to obtain the optimal task planning trajectory of the two-dimensional turntable; According to the intersection information of the original task planning trajectory and the sun circle and the rugged limiting curve, an obstacle-avoiding elliptical trajectory curve of the two-dimensional turntable is planned, comprising: When the original task planning trajectory intersects with the sun circle A and B , the intersection points A and B are calculated, and the middle perpendicular of the connecting line intersects with the first perpendicular of the sun circle at the intersection points and , and a first elliptical path trajectory with a semi-major axis greater than the radius of the sun circle is constructed with the intersection points and as the short axis vertexes; When the original mission trajectory does not intersect with the sun circle, but does intersect with the rugged limiting curve. and At that time, by intersection, and Establish obstacle avoidance circles at the inflection points of the rugged limit curves and calculate the intersection points. and The intersection of the perpendicular bisector of the line connecting the two obstruction points with the second perpendicular bisector of the obstacle avoidance circle. and , with intersection and Construct a second elliptical path trajectory for the minor axis vertex with a semi-major axis greater than the radius of the obstacle avoidance circle; According to the preset selection standard, the obstacle-avoiding elliptical trajectory curve is selected from the first elliptical path trajectory or the second elliptical path trajectory; According to the preset selection standard, the obstacle-avoiding elliptical trajectory curve is selected from the first elliptical path trajectory or the second elliptical path trajectory, comprising: When the first elliptical path is selected, if the sun circle intersects with the rugged limiting curve, a first vertical line passing through the intersection point of the sun circle and the rugged limiting curve is selected as a symmetric center, and an intersection point of the first vertical line and the first elliptical path is selected as a symmetric center. An angle between the intersection point and the sun center is selected as a target angle. A circular arc corresponding to the target angle on the first elliptical path and located above the rugged limiting curve is selected as the obstacle-avoiding elliptical trajectory curve. A and B A When the first elliptical path is selected, if the sun circle intersects with the rugged limiting curve, a first vertical line passing through the intersection point of the sun circle and the rugged limiting curve is selected as a symmetric center, and an intersection point of the first vertical line and the first elliptical path is selected as a symmetric center. An angle between the intersection point and the sun center is selected as a target angle. A circular arc corresponding to the target angle on the first elliptical path and located above the rugged limiting curve is selected as the obstacle-avoiding elliptical trajectory curve.​​ If the sun circle and the rugged limiting curve have no intersection point, the intersection point A and B The target angle is the acute angle between the target angle and the sun center, and the circular arc corresponding to the target angle on the first elliptical path trajectory is selected as the obstacle avoidance elliptical trajectory curve. When being the second elliptical path, a second vertical intersection point located above the rugged limiting curve is taken as a symmetric center, and a first vertical intersection point located below the rugged limiting curve is taken as a symmetric center and The target angle is an angle between the sun center and the target angle, and a circular arc corresponding to the target angle on the second elliptical path trajectory and located above the rugged limiting curve is taken as the obstacle-avoiding elliptical trajectory curve.

2. The method of claim 1, wherein, According to the minimum jerk algorithm, the path segment composed of all adjacent two key path points is fitted and calculated to obtain the optimal task planning trajectory of the two-dimensional turntable, comprising: According to the preset total time, time is allocated to each path segment, and the allocated time is adjusted according to the preset maximum angular velocity threshold and the maximum angular acceleration threshold to obtain an optimal time allocation result; According to the optimal time allocation result, a five-order polynomial is used to fit the trajectory of each path segment; The trajectory parameters of the fitted trajectory are solved by using the minimum jerk algorithm to obtain the optimal task planning trajectory.

3. The method of claim 2, wherein, The trajectory of the path segment is fitted by the following formula: In the formula, m is the serial number of the path segment; to are the trajectory parameters of the mth trajectory fitting; t is the optimal time of each path segment.

4. The method of claim 3, wherein, The trajectory parameters of the fitted trajectory are solved by using the minimum jerk algorithm to obtain the optimal task planning trajectory, comprising: computing fourth derivative of each path segment trajectory : constructing a total cost function of all path segment trajectories from the calculation results : wherein is the cost function corresponding to the mth segment of trajectory; is the m-1th key path point; is the mth key path point; Q m is the positive definite matrix of the mth segment of trajectory; P m is the fourth derivative of the mth segment of trajectory; According to the constraint condition of each key path point, the trajectory parameters are calculated to obtain the optimal task planning trajectory: wherein C is a selection matrix; is a variable fixed by a constraint; is a variable to be optimized; matrix A is a mapping matrix of the fifth order polynomial coefficients to each derivative d .

5. A two-dimensional rotary table obstacle avoidance topology relationship modeling and planning device, characterized in that, The device is applied to the method of any one of claims 1-4, and the device comprises: A planning module is configured to plan an obstacle-avoiding elliptical trajectory curve of a two-dimensional turntable according to intersection information of an original task planning trajectory and a sun circle and a rugged limiting curve; A selection module is configured to select a plurality of equally spaced key path points on each of three curves, i.e., the obstacle-avoiding elliptical trajectory curve, a fitting curve of an original task planning trajectory starting point and an obstacle-avoiding elliptical trajectory curve starting point, and a fitting curve of an original task planning trajectory ending point and an obstacle-avoiding elliptical trajectory curve ending point; A calculation module is configured to perform fitting calculation on a path segment composed of all adjacent two key path points according to a minimum jerk algorithm to obtain an optimal task planning trajectory of a two-dimensional turntable.

6. A computer device, comprising: The computer device comprises a memory and a processor, the memory is configured to store a computer program, and the processor is configured to execute the computer program stored on the memory to implement the steps of the method of any one of claims 1-4.

7. A computer readable storage medium characterized in that, The storage medium stores a computer program, and the computer program is executed by the processor to implement the steps of the method of any one of claims 1-4.

8. A computer program product, characterised in that, A computer program comprising computer program elements which, when executed by a processor, implement the steps of the method according to any one of claims 1 to 4.

Images