Automatic equipment obstacle avoidance method and system based on trajectory planning, computer and medium

Abstract

The invention discloses an automatic equipment obstacle avoidance method and system based on trajectory planning, a computer and a medium, and the method comprises the following steps: constructing a coordinate system according to an obstacle vertex, an equipment starting point and an equipment terminal point; calculating coordinate values of a first vertex and a second vertex according to the coordinate values of the obstacle vertex, the equipment starting point and the equipment ending point in the coordinate system; according to the coordinate values of the equipment starting point, the equipment ending point, the first vertex and the second vertex, setting a motion track of the equipment on each single axis; according to the motion trails of the equipment on the single axes, collision detection is conducted when the equipment runs to the first vertex and the second vertex, the motion trails of the equipment on the single axes are adjusted according to collision detection results, and the adjustment trails of the equipment on the single axes are obtained. According to the invention, rapid obstacle avoidance of the equipment can be realized, the movement of the equipment can be disassembled into movement in each coordinate axis direction, and the movement limit of the equipment is fully utilized.

Description

Technical Field

[0001] The present invention relates to the field of automated production technology, and in particular to an automated equipment obstacle avoidance method, system, computer and medium based on trajectory planning. Background Art

[0002] Automated equipment obstacle avoidance technology is widely used in smart homes, autonomous driving, agricultural machinery, industrial manufacturing and other fields. Especially in industrial manufacturing, automated equipment obstacle avoidance technology can help robots, machine tools or other automated equipment avoid collisions with obstacles during work, thereby improving work efficiency. In traditional industrial manufacturing, automated equipment obstacle avoidance technology is mainly implemented through trajectory planning. Specifically, its implementation is mainly through manual operation to determine the trajectory operation coordinate system and motion trajectory coordinate points of robots, machine tools or other automated equipment, thereby determining their motion posture and motion trajectory. This process is also called manual teaching points for automated equipment.

[0003] Compared with the traditional method of manual operation and calibration of automation equipment, the manual teaching points of automation equipment are mainly achieved through the coordination of the motion trajectory and motion posture of the automation equipment on the spindle, which can significantly reduce human operation errors and improve work efficiency.

[0004] However, the above-mentioned manual teaching points of the automation equipment still have the following defects: 1. Since the manual teaching points need to be taught through a teach pendant or directly control the automation equipment to teach the points, it may slow down its deployment speed. 2. Due to manual teaching, the automatic equipment may plan the path unreasonably, resulting in excessive increase in movement time. 3. The processing of the teaching points will increase the complexity of the method. Summary of the invention

[0005] In view of the above-mentioned deficiencies in the prior art, the technical problem to be solved by the present invention is to provide an automated equipment obstacle avoidance method and device based on trajectory planning that can achieve rapid obstacle avoidance of the equipment and can fully utilize the movement limit of the equipment.

[0006] In order to solve the above technical problems, a technical solution adopted by the present invention is: to provide an automated equipment obstacle avoidance method based on trajectory planning, comprising the following steps:

[0007] S1: Construct a coordinate system based on the obstacle vertices and the device start point and device end point;

[0008] S2: Calculate the coordinate values ​​of the first vertex and the second vertex according to the coordinate values ​​of the obstacle vertex, the device starting point, and the device end point in the coordinate system;

[0009] S3: setting the motion trajectory of the device on each single axis according to the coordinate values ​​of the device starting point, the device end point, the first vertex and the second vertex;

[0010] S4: According to the motion trajectory of the device on each single axis, collision detection is performed when the device runs to the first vertex and the second vertex, and the motion trajectory of the device on each single axis is adjusted according to the collision detection result to obtain the adjusted trajectory of the device on each single axis.

[0011] Furthermore, after step S4, the method further includes the following steps:

[0012] S5: performing timed interpolation on the adjustment trajectory of the device on each single axis to obtain a smooth motion trajectory of the device on each single axis.

[0013] Furthermore, step S1 includes the following sub-steps:

[0014] S11: Identify an obstacle entity and construct an obstacle according to the obstacle entity; the obstacle entity is located on a line connecting the device start point and the device end point, and the obstacle is a cuboid circumscribing the obstacle entity;

[0015] S12: constructing a coordinate system with the starting point of the device as the origin, wherein three coordinate axes of the coordinate system are respectively parallel to the length, width and height of the obstacle.

[0016] Furthermore, in step S2, the following sub-steps are included:

[0017] S21: The coordinates of the device start point and the device end point are represented by (Xs, Ys, Zs) and (Xe, Ye, Ze) respectively;

[0018] S22: Take the two vertices with the smallest and largest coordinate values ​​among the obstacle vertices, and represent them as oN=(oNx, oNy, oNz) and oP=(oPx, oPy, oPz) respectively;

[0019] S23: Calculate the coordinate values ​​of the first vertex and the second vertex, and express them as v1 = (Xs, oNy, oPz) and v2 = (Xe, oPy, oPz) respectively.

[0020] Furthermore, in step S3, the following sub-steps are included:

[0021] S31: generating motion trajectories of the device on the X-axis and the Y-axis according to a preset speed curve according to the motion limits of the device on the X-axis and the Y-axis, and representing them as x(t) and y(t) respectively, wherein the motion limits include the speed, acceleration, and jerk of the device on the X-axis and the Y-axis;

[0022] S33: The motion trajectory of the device on the Z axis is divided into an ascending phase and a descending phase, which are represented by zup(t) and zdown(t) respectively. The starting point of the ascending phase is the starting point of the device, and the end point is defined as the ascending end point. The ascending end point is the highest point reached by the device for the first time in the ascending phase. The starting point of the descending phase is the ascending end point, and the end point is the device end point.

[0023] Furthermore, after step S31, the following steps are also included:

[0024] S32: Determine the input ranges of x(t) and y(t), and express them as [0, tx_end] and [0, ty_end] respectively, where tx_end represents the time when the device reaches the end point of the X axis, and ty_end represents the time when the device reaches the end point of the Y axis;

[0025] After step S33, the following steps are also included:

[0026] S34: Determine the input range of the zup(t) and express it as [0, tzup_end], where tzup_end represents the time when the device reaches the end of the rise;

[0027] S35: Synthesize the trajectories of the zdown(t) and zup(t), determine the input range of the zdown(t) according to the input range of the zup(t), and express it as [tzup_end, tzup_end+ΔtZdown], where ΔtZdown represents the time required for the device to reach the device end from the rising end.

[0028] Furthermore, in step S4, the following sub-steps are included:

[0029] S41: Taking the starting time of zup(t) as a reference, unifying the starting time of x(t) and y(t);

[0030] S42: Calculate the time when the device reaches the Y-axis coordinate of the first vertex according to the motion trajectory y(t) of the device on the Y-axis, and record it as tYv1; calculate the time when the device reaches the Z-axis coordinate of the first vertex according to the motion trajectory zup(t) of the device in the Z-axis ascending stage, and record it as tZv1;

[0031] S43: Compare the sizes of tYv1 and tZv1. If tYv1 is smaller than tZv1, the device will collide when running to the first vertex. At this time, adjust the input range of y(t) so that the starting time of y(t) is delayed by at least tZv1-tYv1.

[0032] S44: Calculate the time when the device reaches the Y-axis coordinate of the second vertex according to the motion trajectory y(t) of the device on the Y-axis, and record it as tYv2; Calculate the time when the device reaches the Z-axis coordinate of the second fixed point according to the motion trajectory zdown(t) of the device in the Z-axis descending stage, and record it as tZv2;

[0033] S45: Compare the sizes of tYv2 and tZv2. If tYv2 is greater than tZv2, the device will collide when it runs to the second vertex. At this time, adjust the input range of zdown(t) so that the starting time of zdown(t) is delayed by at least tYv2-tZv2.

[0034] In order to solve the above technical problems, the second technical solution adopted by the present invention is to provide an automatic equipment obstacle avoidance system based on trajectory planning, comprising:

[0035] A coordinate system building module is used to build a coordinate system according to the obstacle vertices and the device start point and the device end point;

[0036] A vertex coordinate calculation module, used to calculate the coordinate values ​​of the first vertex and the second vertex according to the coordinate values ​​of the obstacle vertex, the device starting point and the device end point in the coordinate system;

[0037] A trajectory setting module, used to set the motion trajectory of the device on each single axis according to the coordinate values ​​of the device starting point, the device end point, the first vertex and the second vertex;

[0038] The trajectory adjustment module is used to perform collision detection on the device when it runs to the first vertex and the second vertex according to the motion trajectory of the device on each single axis, and adjust the motion trajectory of the device on each single axis according to the collision detection result to obtain the adjusted trajectory of the device on each single axis.

[0039] To solve the above technical problems, the third technical solution adopted by the present invention is: a computer device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement any of the above-mentioned automated equipment obstacle avoidance methods.

[0040] In order to solve the above technical problems, the fourth technical solution adopted by the present invention is: a computer-readable storage medium, on which a computer program is stored, and when the computer program is executed by a processor, the automated equipment obstacle avoidance method described in any one of the above items is implemented.

[0041] The automated equipment obstacle avoidance method, system, computer and medium based on trajectory planning of the present invention have at least the following beneficial effects: the present invention realizes rapid obstacle avoidance by planning the trajectory of the equipment; the smallest circumscribed cuboid of the obstacle entity is defined as the obstacle, and the position of the obstacle can be intuitively described by the vertex coordinate values ​​of the obstacle in the coordinate system, without the need for manual teaching of points and direct generation of the middle points of the Cartesian space path; the equipment movement is decomposed into movement in the direction of each coordinate axis, which can make full use of the movement limit of the equipment in the direction of each axis; by comparing the corresponding time when the three coordinate axes of the equipment reach the vertex of the obstacle, the start movement time of one or two axes is delayed, and finally the execution interval of each axis of the equipment on the unified time line is determined, and interpolated according to the unified time, such a trajectory can avoid obstacles and quickly reach the end point. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0043] Figure 1 It is a flow chart of an implementation method of an automated equipment obstacle avoidance method based on trajectory planning of the present invention.

[0044] Figure 2 for Figure 1 Sub-flow chart of step S1 in .

[0045] Figure 3 for Figure 1 Sub-flow chart of step S2 in .

[0046] Figure 4 for Figure 1 Sub-flow chart of step S3 in .

[0047] Figure 5 for Figure 1 Sub-flow chart of step S4 in .

[0048] Figure 6 It is a structural block diagram of an implementation scheme of an automated equipment obstacle avoidance system of the present invention. DETAILED DESCRIPTION

[0049] The present invention will be further described below in conjunction with the accompanying drawings.

[0050] See also Figure 1 , is a flow chart of an implementation method of an automated equipment obstacle avoidance method based on trajectory planning of the present invention. This implementation method may specifically include the following steps:

[0051] S1: Construct a coordinate system based on the obstacle vertices and the device start point and device end point.

[0052] Specifically, constructing a coordinate system for the operating space of the device is the basis for planning the motion trajectory of the device. In the present embodiment, the construction of the coordinate system is based on the vertices of the obstacle and the starting point and end point of the device, unifying the standard for constructing the coordinate system, thereby ensuring the consistency of the coordinate system when the obstacle has different relative positions with the starting point and end point of the device, and facilitating the calculation of the coordinate values ​​of subsequent points.

[0053] See also Figure 2 , this step S1 may include the following sub-steps:

[0054] S11: Identify an obstacle entity and construct an obstacle according to the obstacle entity; the obstacle entity is located on a line connecting the device starting point and the device end point, and the obstacle is a cuboid circumscribing the obstacle entity.

[0055] Specifically, in the present embodiment, the recognition of obstacle entities can be achieved through visual perception or other sensors. Obviously, in actual industrial manufacturing scenarios, obstacle entities are often irregular entities, which may have multiple vertices or no vertices. It is difficult to describe the position of the obstacle entity through a coordinate system, which will bring difficulties to the subsequent path planning of the equipment. Therefore, in the present embodiment, the minimum circumscribed cuboid of the obstacle entity is defined as an obstacle. In this way, the position of the obstacle can be intuitively described by the vertex coordinate values ​​of the obstacle in the coordinate system, thereby facilitating the path planning of the equipment. When the equipment running path does not intersect with the obstacle, the equipment will definitely not collide with the obstacle entity, that is, automatic obstacle avoidance of the equipment is achieved.

[0056] S12: constructing a coordinate system with the starting point of the device as the origin, wherein three coordinate axes of the coordinate system are respectively parallel to the length, width and height of the obstacle.

[0057] Specifically, in order to further simplify the obstacle position description process, that is, the calculation process of the coordinate values ​​of each vertex in the coordinate system, in this embodiment, the device starting point is used as the origin to construct the coordinate system, and the XYZ three coordinate axes of the coordinate system are respectively parallel to the length, width and height of the obstacle.

[0058] S2: Calculate the coordinate values ​​of the first vertex and the second vertex according to the coordinate values ​​of the obstacle vertex, the device start point, and the device end point in the coordinate system.

[0059] Specifically, in this embodiment, based on the construction of the above-mentioned coordinate system, the coordinate values ​​of the obstacle vertex, the device starting point and the device end point can be directly obtained, and the above-mentioned first vertex and the second vertex are actually the two points where the device is most likely to collide with the obstacle during operation.

[0060] See also Figure 3, this step S2 may include the following sub-steps:

[0061] S21: The coordinates of the device start point and the device end point are represented by (Xs, Ys, Zs) and (Xe, Ye, Ze) respectively.

[0062] S22: Take the two vertices with the smallest and largest coordinate values ​​among the obstacle vertices, and represent them as oN=(oNx, oNy, oNz) and oP=(oPx, oPy, oPz) respectively.

[0063] Specifically, in this embodiment, according to the construction of the above-mentioned coordinate system, the coordinate values ​​of the obstacle vertices, the device starting point and the device end point can be directly obtained, and the coordinates of the device starting point and the device end point are further represented by (Xs, Ys, Zs) and (Xe, Ye, Ze), respectively. At the same time, the two vertices with the smallest and largest coordinate values ​​among the obstacle vertices are taken, and represented by oN=(oNx, oNy, oNz) and oP=(oPx, oPy, oPz), respectively.

[0064] S23: Calculate the coordinate values ​​of the first vertex and the second vertex, and express them as v1 = (Xs, oNy, oPz) and v2 = (Xe, oPy, oPz) respectively.

[0065] Specifically, in this implementation, based on the coordinate values ​​of the above-mentioned obstacle vertices, the device starting point and the device end point, the coordinate values ​​of the first vertex and the second vertex can be calculated, which are v1=(Xs, oNy, oNz) and v2=(Xe, oPy, oPz), respectively. According to the coordinate values ​​of the first vertex and the second vertex, it can be known that the X-axis coordinate of the first vertex is the same as the X-axis coordinate of the device starting point, the Y-axis coordinate is the same as the Y-axis coordinate of the point with the smallest coordinate value among the obstacle vertices, and the Z-axis coordinate is the same as the Z-axis coordinate of the point with the largest coordinate value among the obstacle vertices; the X-axis coordinate of the second vertex is the same as the X-axis coordinate of the device end point, and the Y-axis coordinate is the same as the Y-axis coordinate of the point with the largest coordinate value among the obstacle vertices. Since the XYZ three coordinate axes of the coordinate system are respectively parallel to the length, width and height of the obstacle, its Z-axis coordinate is also the same as the Z-axis coordinate of the point with the largest coordinate value among the obstacle vertices.

[0066] S3: Setting the motion trajectory of the device on each single axis according to the coordinate values ​​of the device starting point, the device end point, the first vertex, and the second vertex.

[0067] See also Figure 4 , this step S3 may include the following sub-steps:

[0068] S31: According to the motion limits of the device on the X-axis and the Y-axis, the motion trajectories of the device on the X-axis and the Y-axis are generated according to the preset speed curve, and are represented by x(t) and y(t) respectively. The motion limits include the speed, acceleration, and jerk of the device on the X-axis and the Y-axis.

[0069] Specifically, since the motion limits (including speed limit, acceleration limit, jerk limit, etc.) of the device on the X-axis and Y-axis may be different, in this embodiment, the motion trajectory of the device on the X-axis and Y-axis can be set according to its actual motion limit, and its speed curve can also adopt a trapezoidal speed curve or an S-shaped speed curve (or other speed curves) according to actual needs, so as to make full use of the motion limit of the device on each axis. The obtained motion trajectory of the device on the X-axis and Y-axis can be represented by x(t) and y(t) respectively. The motion trajectory represents the relationship between time and position of the device on the X-axis and Y-axis respectively, including the motion duration. Its input is time t, and the output is the position of the device on the X-axis and Y-axis. Assuming that they all start from time 0, the motion time period (i.e., the value range of t) is [0, tx_end] and [0, ty_end] respectively.

[0070] After the above step S31, the following steps are also included:

[0071] S32: Determine the input ranges of x(t) and y(t), and express them as [0, tx_end] and [0, ty_end] respectively, where tx_end represents the time when the device reaches the end point of the X axis, and ty_end represents the time when the device reaches the end point of the Y axis;

[0072] Specifically, since the motion limit of the device on each axis is used to determine its motion trajectory on the corresponding axis in this embodiment, the running time of the device on each axis may also be different. Therefore, in order to facilitate the synthesis of the trajectories of each axis and subsequent collision detection, it is also necessary to determine the input range of the device on each axis, that is, the time range of its operation. In this embodiment, the input range of x(t) and y(t) can be first determined and expressed as [0, tx_end] and [0, ty_end], respectively, where tx_end represents the moment when the device reaches the end point of the X-axis, and ty_end represents the moment when the device reaches the end point of the Y-axis.

[0073] S33: Divide the motion trajectory of the device on the Z axis into an ascending phase and a descending phase, which are represented by zup(t) and zdown(t) respectively. The starting point of the ascending phase is the device starting point, and the end point is defined as the ascending end point. The ascending end point is the highest point reached by the device for the first time in the ascending phase. The starting point of the descending phase is the ascending end point, and the end point is the device end point.

[0074] Specifically, after the motion trajectories of the X-axis and the Y-axis and their input ranges are determined, the motion trajectory of the device on the Z-axis and its input range need to be determined. However, unlike the motion trajectories of the device on the X-axis and the Y-axis, the starting point and end point of the device on the Z-axis are the same, that is, the device has an ascending and descending process on the Z-axis. Therefore, the motion trajectory of the device on the Z-axis needs to be divided into an ascending phase and a descending phase. In this embodiment, zup(t) and zdown(t) can be used to represent the ascending phase and the descending phase of the device on the Z-axis, respectively. In this embodiment, the starting point of the device in the ascending stage is the Z-axis coordinate of the starting point of the device, and the end point is defined as the ascending end point, which is the highest point reached by the device for the first time in the ascending stage, and the starting point of the descending stage is the ascending end point, and the end point is the Z-axis coordinate of the end point of the device. It should be noted here that after the device reaches the ascending end point, it will move along a fixed height hO above the top surface of the obstacle, that is, according to the above-mentioned determination of the motion trajectory of the device in the X-axis, Y-axis and Z-axis, the obstacle avoidance motion trajectory of the device relative to the obstacle can be roughly determined, that is, the device starts from the starting point of the device, and moves in the directions of the X-axis and Y-axis according to the motion trajectory x(t) and y(t), and moves in the Z-axis direction according to the motion trajectory zup(t) and zdown(t).

[0075] After the above step S33, the following steps are also included:

[0076] S34: Determine the input range of the zup(t) and express it as [0, tzup_end], where tzup_end represents the time when the device reaches the end of the rise;

[0077] S35: Synthesize the trajectories of the zdown(t) and zup(t), determine the input range of the zdown(t) according to the input range of the zup(t), and express it as [tzup_end, tzup_end+ΔtZdown], where ΔtZdown represents the time required for the device to reach the device end from the rising end.

[0078] Specifically, after determining the motion trajectories zup(t) and zdown(t) of the device on the Z axis, it is also necessary to determine its input range. In this implementation, the input range of zup(t) can be expressed as [0, tzup_end], where tzup_end represents the moment when the device reaches the rising end point, and the input range of zdown(t) can be expressed as [tzup_end, tzup_end+ΔtZdown], where ΔtZdown represents the time required for the device to reach the device end point from the rising end point.

[0079] S4: According to the motion trajectory of the device on each single axis, collision detection is performed when the device runs to the first vertex and the second vertex, and the motion trajectory of the device on each single axis is adjusted according to the collision detection result to obtain the adjusted trajectory of the device on each single axis.

[0080] Specifically, since the motion limits of the device on each axis may be different, according to the motion trajectory of the device on each single axis, it may happen that the device first reaches the Y coordinate value of the first vertex on the Y axis, while the device has not yet moved to the Z coordinate of the first vertex on the Z axis. In this case, the device may collide at the first vertex. Similarly, it may happen that the device has dropped to the Z coordinate of the second fixed point on the Z axis, while the device has not yet moved to the Y coordinate of the second fixed point on the Y axis. In this case, the device may collide at the second fixed point. Therefore, in order to ensure that the device can successfully avoid obstacles, it is also necessary to perform collision detection when the device moves to the first vertex and the second vertex, and adjust the motion trajectory of the device on each single axis according to the collision detection results.

[0081] See also Figure 5 , this step S4 may include the following sub-steps:

[0082] S41: Taking the starting time of zup(t) as a reference, unify the starting time of x(t) and y(t).

[0083] Specifically, after determining the starting time of the four trajectories, the length of the time period is guaranteed to remain unchanged, and the starting time or the ending time of the remaining three trajectories is adjusted as needed based on the time 0 at the beginning of zup(t) to achieve the purpose of avoiding the first vertex and the second fixed point.

[0084] S42: According to the motion trajectory y(t) of the device on the Y axis, calculate the moment when the device reaches the Y-axis coordinate of the first vertex, and record it as tYv1; according to the motion trajectory zup(t) of the device in the Z-axis ascending phase, calculate the moment when the device reaches the Z-axis coordinate of the first vertex, and record it as tZv1.

[0085] S43: Compare the sizes of tYv1 and tZv1. If tYv1 is smaller than tZv1, the device will collide when it runs to the first vertex. At this time, adjust the input range of y(t) so that the starting time of y(t) is delayed by at least tZv1-tYv1.

[0086] Specifically, for the collision detection of the device at the first vertex, the detection rule in this embodiment is set as follows: the moment when the device reaches the Z coordinate value of the first vertex on the Z axis must be before the moment when the device reaches the Y coordinate value of the first vertex on the Y axis. According to the detection rule, it is necessary to calculate the moment tZv1 when the device reaches the Z-axis coordinate of the first vertex and the moment tYv1 when the device reaches the Y-axis coordinate of the first vertex based on the motion trajectory zup(t) of the device in the Z-axis ascending phase and the motion trajectory y(t) of the device on the Y axis. Then compare tYv1 with tZv1. If tYv1 is less than tZv1, the device will collide when it runs to the first vertex. At this time, you only need to adjust the input range of y(t) and delay the input of y(t) by at least tZv1-tYv1 to ensure that the device reaches the Z coordinate value of the first vertex on the Z axis before the device reaches the Y coordinate value of the first vertex on the Y axis, thereby avoiding a collision of the device at the first vertex. In this embodiment, the input range of y(t) can be directly set to [tZv1-tYv1, ty_end+tZv1-tYv1].

[0087] S44: According to the motion trajectory y(t) of the device on the Y axis, calculate the moment when the device reaches the Y-axis coordinate of the second vertex, and record it as tYv2; according to the motion trajectory zdown(t) of the device in the Z-axis descending phase, calculate the moment when the device reaches the Z-axis coordinate of the second fixed point, and record it as tZv2.

[0088] S45: Compare the sizes of tYv2 and tZv2. If tYv2 is greater than tZv2, the device will collide when it runs to the second vertex. At this time, adjust the input range of zdown(t) so that the starting time of zdown(t) is delayed by at least tYv2-tZv2.

[0089] Specifically, for the collision detection of the device at the second vertex, the detection rule in this embodiment is set as follows: the moment when the device reaches the Z coordinate value of the second vertex on the Z axis must be after the moment when the device reaches the Y coordinate value of the second vertex on the Y axis. According to the detection rule, it is necessary to calculate the moment tZv2 when the device reaches the Z-axis coordinate of the second vertex and the moment tYv2 when the device reaches the Y-axis coordinate of the second vertex based on the motion trajectory zdown(t) of the device in the Z-axis descent phase and the motion trajectory y(t) of the device on the Y axis. Then tZv2 is compared with tYv2. If tYv2 is greater than tZv2, the device will collide when it runs to the second vertex. At this time, it is only necessary to adjust the input range of zdown(t) and delay the input of zdown(t) by at least tYv2-tZv2 to ensure that the moment when the device reaches the Z coordinate value of the second fixed point on the Z axis is after the moment when the device reaches the Y coordinate value of the second fixed point on the Y axis, thereby avoiding the device from colliding at the second fixed point. In this embodiment, the input range of zdown(t) can be directly set to [tzup_end+tYv2-tZv2, tzup_end+ΔtZdown+tYv2-tZv2].

[0090] In order to ensure the smoothness of the device motion trajectory, in this implementation, the following steps are also included:

[0091] S5: performing timed interpolation on the adjustment trajectory of the device on each single axis to obtain a smooth motion trajectory of the device on each single axis.

[0092] Specifically, the motion trajectory of the finally determined device on the X-axis, Y-axis and Z-axis is uniformly interpolated at a fixed time to ensure the smoothness of the motion trajectory. In this embodiment, the interpolation process is as follows: the program creates a time variable t, and t increases by Δt after each interpolation cycle Δt; in each cycle, x(t), y(t), z(t) (i.e., the synthetic motion trajectory of the adjusted zup(t) and zdown(t)) are interpolated respectively, and when t has not yet entered the respective input ranges, they are sent to the servo at their respective starting positions; when t enters the respective ranges, the new positions are calculated by interpolation according to the motion trajectories of x(t), y(t), and z(t), and sent to the servo; when t exceeds the respective ranges, they are sent to the servo at their respective end positions. When t exceeds the time ranges of the three axes x(t), y(t), and z(t), the end point is reached and interpolation stops.

[0093] See also Figure 5, is a structural block diagram of an embodiment of an automated equipment obstacle avoidance system based on trajectory planning of the present invention. The automated equipment obstacle avoidance system based on trajectory planning of this embodiment is used to implement the automated equipment obstacle avoidance method based on trajectory planning as described in the above embodiment. Specifically, the automated equipment obstacle avoidance system based on trajectory planning of this embodiment includes a data processing module 100, a model training module 200, a prediction module 300 and an adjustment control module 400. Among them:

[0094] A coordinate system construction module 100, for constructing a coordinate system according to the obstacle vertices and the device start point and the device end point;

[0095] A vertex coordinate calculation module 200, used to calculate the coordinate values ​​of the first vertex and the second vertex according to the coordinate values ​​of the obstacle vertex, the device start point and the device end point in the coordinate system;

[0096] The trajectory setting module 300 is used to set the motion trajectory of the device on each single axis according to the coordinate values ​​of the device starting point, the device end point, the first vertex and the second vertex;

[0097] The trajectory adjustment module 400 is used to perform collision detection on the device when it runs to the first vertex and the second vertex according to the motion trajectory of the device on each single axis, and adjust the motion trajectory of the device on each single axis according to the collision detection result to obtain the adjusted trajectory of the device on each single axis.

[0098] In order to ensure the smoothness of the device's motion trajectory, in this embodiment, a trajectory interpolation module 500 is further provided for performing timed interpolation on the adjustment trajectory of the device on each single axis to obtain the smooth motion trajectory of the device on each single axis to ensure the smoothness of the device's motion trajectory.

[0099] On the other hand, an embodiment of the present invention further provides a computer device, comprising: a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement any of the above-described automated equipment obstacle avoidance methods.

[0100] On the other hand, an embodiment of the present invention further provides a computer-readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the automated equipment obstacle avoidance method described in any one of the above items is implemented.

[0101] The present invention realizes rapid obstacle avoidance by planning the trajectory of the device; the smallest circumscribed cuboid of the obstacle entity is defined as the obstacle, and the position of the obstacle can be intuitively described by the vertex coordinate values ​​of the obstacle in the coordinate system, without the need for manual teaching of points and direct generation of the middle point of the Cartesian space path; the movement of the device is decomposed into movements in the directions of each coordinate axis, and the movement limit of the device in the direction of each axis can be fully utilized; by comparing the corresponding time when the device reaches the vertex of the obstacle on the three coordinate axes, the start movement time of one or two axes is delayed, and finally the execution interval of each axis of the device on the unified timeline is determined, and interpolation is performed according to the unified time, so that such a trajectory can avoid obstacles and quickly reach the end point.

[0102] The above content only expresses the preferred embodiments of the present invention, and its description is relatively specific and detailed, but it cannot be understood as limiting the scope of the invention patent. It should be pointed out that for ordinary technicians in this field, several modifications and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention shall be based on the attached claims.

Claims

1. An automated equipment obstacle avoidance method based on trajectory planning, characterized in that: The following steps are involved: S1: Construct a coordinate system based on the obstacle vertex, the device starting point, and the device end point; S2: Calculate the coordinate values ​​of the first vertex and the second vertex according to the coordinate values ​​of the obstacle vertex, the device starting point, and the device end point in the coordinate system; S3: setting the motion trajectory of the device on each single axis according to the coordinate values ​​of the device starting point, the device end point, the first vertex and the second vertex; S4: According to the motion trajectory of the device on each single axis, collision detection is performed when the device runs to the first vertex and the second vertex, and the motion trajectory of the device on each single axis is adjusted according to the collision detection result to obtain the adjusted trajectory of the device on each single axis.

2. The automated equipment obstacle avoidance method based on trajectory planning as claimed in claim 1, characterized in that: After step S4, the following steps are also included: S5: performing timed interpolation on the adjustment trajectory of the device on each single axis to obtain a smooth motion trajectory of the device on each single axis.

3. The automated equipment obstacle avoidance method based on trajectory planning as claimed in claim 1, characterized in that: The S1 step includes the following sub-steps: S11: Identify an obstacle entity and construct an obstacle according to the obstacle entity; the obstacle entity is located on a line connecting the device start point and the device end point, and the obstacle is a cuboid circumscribing the obstacle entity; S12: constructing a coordinate system with the starting point of the device as the origin, wherein three coordinate axes of the coordinate system are respectively parallel to the length, width and height of the obstacle.

4. The automated equipment obstacle avoidance method based on trajectory planning as claimed in claim 3, characterized in that: In step S2, the following sub-steps are included: S21: The coordinates of the device start point and the device end point are represented by (Xs, Ys, Zs) and (Xe, Ye, Ze) respectively; S22: Take the two vertices with the smallest and largest coordinate values ​​among the obstacle vertices, and represent them as oN=(oNx, oNy, oNz) and oP=(oPx, oPy, oPz) respectively; S23: Calculate the coordinate values ​​of the first vertex and the second vertex, and express them as v1 = (Xs, oNy, oPz) and v2 = (Xe, oPy, oPz) respectively.

5. The automated equipment obstacle avoidance method based on trajectory planning as claimed in claim 1, characterized in that: The S3 step includes the following sub-steps: S31: generating motion trajectories of the device on the X-axis and the Y-axis according to a preset speed curve according to the motion limits of the device on the X-axis and the Y-axis, and representing them as x(t) and y(t) respectively, wherein the motion limits include the speed, acceleration, and jerk of the device on the X-axis and the Y-axis; S33: The motion trajectory of the device on the Z axis is divided into an ascending phase and a descending phase, which are represented by zup(t) and zdown(t) respectively. The starting point of the ascending phase is the starting point of the device, and the end point is defined as the ascending end point. The ascending end point is the highest point reached by the device for the first time in the ascending phase. The starting point of the descending phase is the ascending end point, and the end point is the device end point.

6. The automated equipment obstacle avoidance method based on trajectory planning as claimed in claim 5, characterized in that: After step S31, the following steps are also included: S32: Determine the input ranges of x(t) and y(t), and express them as [0, tx_end] and [0, ty_end] respectively, where tx_end represents the time when the device reaches the end point of the X axis, and ty_end represents the time when the device reaches the end point of the Y axis; After step S33, the following steps are also included: S34: Determine the input range of the zup(t) and express it as [0, tzup_end], where tzup_end represents the time when the device reaches the end of the rise; S35: Synthesize the trajectories of the zdown(t) and zup(t), determine the input range of the zdown(t) according to the input range of the zup(t), and express it as [tzup_end, tzup_end+ΔtZdown], where ΔtZdown represents the time required for the device to reach the device end from the rising end.

7. The automated equipment obstacle avoidance method based on trajectory planning as claimed in claim 6, characterized in that: The S4 step includes the following sub-steps: S41: Taking the starting time of zup(t) as a reference, unifying the starting time of x(t) and y(t); S42: Calculate the time when the device reaches the Y-axis coordinate of the first vertex according to the motion trajectory y(t) of the device on the Y-axis, and record it as tYv1; calculate the time when the device reaches the Z-axis coordinate of the first vertex according to the motion trajectory zup(t) of the device in the Z-axis ascending stage, and record it as tZv1; S43: Compare the sizes of tYv1 and tZv1. If tYv1 is smaller than tZv1, the device will collide when running to the first vertex. At this time, adjust the input range of y(t) so that the starting time of y(t) is delayed by at least tZv1-tYv1. S44: Calculate the time when the device reaches the Y-axis coordinate of the second vertex according to the motion trajectory y(t) of the device on the Y-axis, and record it as tYv2; Calculate the time when the device reaches the Z-axis coordinate of the second fixed point according to the motion trajectory zdown(t) of the device in the Z-axis descending stage, and record it as tZv2; S45: Compare the sizes of tYv2 and tZv2. If tYv2 is greater than tZv2, the device will collide when it runs to the second vertex. At this time, adjust the input range of zdown(t) so that the starting time of zdown(t) is delayed by at least tYv2-tZv2.

8. An automated equipment obstacle avoidance system based on trajectory planning, characterized in that: include: A coordinate system building module is used to build a coordinate system according to the obstacle vertices and the device start point and the device end point; A vertex coordinate calculation module, used to calculate the coordinate values ​​of the first vertex and the second vertex according to the coordinate values ​​of the obstacle vertex, the device starting point and the device end point in the coordinate system; A trajectory setting module, used to set the motion trajectory of the device on each single axis according to the coordinate values ​​of the device starting point, the device end point, the first vertex and the second vertex; The trajectory adjustment module is used to perform collision detection on the device when it runs to the first vertex and the second vertex according to the motion trajectory of the device on each single axis, and adjust the motion trajectory of the device on each single axis according to the collision detection result to obtain the adjusted trajectory of the device on each single axis.

9. A computer device comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the automated equipment obstacle avoidance method according to any one of claims 1 to 7.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the automated equipment obstacle avoidance method described in any one of claims 1 to 7 is implemented.

Images