Laser forklift pickup path re-planning and self-adaptive optimization method thereof

Abstract

The invention relates to the technical field of path planning, in particular to a laser forklift goods taking path re-planning and self-adaptive optimization method. The method comprises the following steps: setting all parameter values required in calculation; the cargo tray position and the inclination angle recognized by the camera are obtained; according to the position and angle information of the tray, a third-order Bezier curve type goods taking route with the current position of the AGV as a starting point is planned; and according to the length and width of the fork of the forklift and the width of the tray, the planned route is adaptively optimized and adjusted, so that the forklift does not collide with the tray when running on the route. By comprehensively considering the current position and orientation of the AGV and the conversion relation among multiple coordinate systems, an accurate third-order Bezier curve type goods taking route is planned, adaptive optimization of the route is carried out, and the distance between control points is flexibly adjusted so as to balance the collision risk and the route curvature change, so that the accuracy of goods taking is improved. The safe distance between the forklift and the tray in the driving process is guaranteed, and collision is avoided.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a laser forklift cargo picking path replanning and an adaptive optimization method thereof. Background Art

[0002] In the application of forklifts automatically picking up goods, forklifts often follow preset routes to perform tasks. After the task is started, the forklift travels along the road to the designated location to pick up the goods. However, if the shelves are not placed straight, the forklift will collide with the shelves or trigger the safety mechanism, making it difficult to reach accurately. To solve this problem, technicians have proposed a visual recognition picking method such as Chinese patent CN115924798A. This method uses a visual module to identify the coordinates of the goods and plan the picking path. However, this method still requires customized installation of fixtures, which increases cost and complexity and limits the flexibility of the forklift; at the same time, the planned route is not smooth enough and requires turning on the spot, which reduces efficiency and increases the risk of collision; in addition, for situations where the shelves are not straight or the goods are not neatly placed, this method lacks adaptive adjustment, resulting in the forklift colliding with the pallet when picking up goods, reducing safety. Summary of the invention

[0003] The technical problem to be solved by the present invention is to overcome the deficiencies of the prior art and provide a laser forklift cargo picking path replanning and an adaptive optimization method thereof.

[0004] The technical solution adopted by the present invention is as follows: A laser forklift cargo picking path replanning and adaptive optimization method thereof comprises the following steps: S1. Parameter setting: set all parameter values ​​required for calculation; S2, camera data acquisition and processing: obtaining the position and tilt angle of the cargo pallet recognized by the camera; S3, end point calculation and path planning: According to the pallet position and angle information, a third-order Bezier curve-type pickup route is planned with the current position of the AGV as the starting point; S4. Adaptive path optimization: According to the length and width of the forklift forks and the width of the pallet, the planned route is adaptively optimized to prevent the forklift from colliding with the pallet when driving on the path.

[0005] This technical solution initializes all the basic values ​​required for calculation through parameter setting, and then uses the camera data acquisition and processing steps to obtain the specific position of the cargo pallet and its inclination angle information; based on these position and angle information, through the end point calculation and path planning steps, a pickup route starting from the current position of the AGV and using a third-order Bezier curve is planned; in the path adaptive optimization stage, the planned route is adaptively adjusted and optimized according to the length and width of the forklift and the width of the pallet to ensure that the forklift can maintain a safe distance from the pallet during driving to avoid collision.

[0006] In addition, the laser forklift pickup path replanning and adaptive optimization method proposed in the present invention also has the following additional technical features: According to an embodiment of the present invention, in the parameter setting of step S1, the setting method calculates the required parameter value, including: Represents the number of frames of camera data acquisition; Represents the camera's installation position in the X direction of the vehicle coordinate system; Represents the camera installation position in the Y direction of the vehicle coordinate system; Represents the installation angle of the camera in the vehicle coordinate system; Represents the collision model length; Represents the collision model width; Represents the X-direction distance between the AGV motion center and the model center; Represents the Y-direction distance between the AGV motion center and the model center; Represents the width of the collision model for safe passage, i.e. the distance between the inner edges of the two sides of the pallet; Represents the path sampling distance during path adaptive optimization; Represents the control point distance adjustment step size during path adaptive optimization; According to the characteristics of forklift picking, the collision model used to calculate whether the vehicle body has a collision risk with the pallet uses the rectangle occupied by the fork part; to ensure safety, before the forklift picks up the pallet, it detects whether the pallet has reached the detection sensor at the root of the fork and the working depth of the fork movement center entering the bottom of the pallet. The calculation formula is: (1).

[0007] This technical solution supports the operation of the entire laser forklift picking path replanning and its adaptive optimization method by setting key parameter values. The parameters include the number of camera data acquisition frames, which is used to determine the frequency of data updates; the installation position of the camera in the X and Y directions of the vehicle coordinate system, and the installation angle of the camera, which together define the spatial position relationship of the camera relative to the forklift, thereby ensuring that the position and tilt angle of the pallet can be accurately obtained; setting parameters representing the length and width of the collision model, and the distance in the X and Y directions of the AGV motion center relative to the center of the collision model, which is used to construct a rectangular collision model that can simulate the fork part of the forklift, so as to provide a reference when calculating whether the forklift collides with the pallet. Reference; Consider the inner edge spacing on both sides of the pallet as the safe passage width of the collision model to ensure that the forklift can maintain a safe distance from the pallet during driving; In the path adaptive optimization stage, set two parameters, the path sampling distance and the control point distance adjustment step length, to guide the fine adjustment and optimization process of the path; According to the characteristics of forklift picking up goods, when calculating whether the forklift collides with the pallet, the rectangle occupied by the fork part is used as the collision model, and a calculation formula is used to determine the working depth of the fork movement center entering the bottom of the pallet; In order to ensure that the forklift can accurately pick up the pallet, a detection sensor is also set to detect whether the pallet has reached the root of the fork, thereby improving the reliability of the picking process.

[0008] According to an embodiment of the present invention, in the camera data acquisition and processing of step S2, the camera installed on the forklift detects the position and angle information of the pallet in the camera coordinate system (XCOCYC); wherein the detected position Refers to the center point of the pallet near the forklift. The detection angle refers to the rotation angle of the pallet around the measured center point, with counterclockwise being positive. In order to reduce the impact of data fluctuations on the detection results, After the frame detection data is obtained, the average value is applied again. The calculation formula is: (2).

[0009] This technical solution achieves reliable acquisition of the position and angle information of the pallet. Specifically, in order to effectively reduce the error caused by data fluctuations to the detection results, multiple frames of detection data are first collected continuously, and then the data are averaged to be used as the final detection result; the formula (2) is used to calculate the average value, thereby ensuring the stability of the obtained result.

[0010] According to an embodiment of the present invention, in the end point calculation and path planning of step S3, it is assumed that a third-order Bezier curve-type pickup route is planned with the current position of the AGV as the starting point. The coordinate position of , facing ; The calculation process involves the conversion between the navigation map coordinate system (XMOMYM), the vehicle coordinate system (XAOAYA) and the camera coordinate system (XCOCYC).

[0011] This technical solution plans a third-order Bezier curve-type pickup route starting from the current position of the AGV by comprehensively considering the current position and orientation of the AGV, as well as the conversion relationship between the navigation map coordinate system, the vehicle coordinate system, and the camera coordinate system. During the planning process, the coordinate position and orientation of the end point are determined, which is the target point of the path planning; using coordinate conversion technology, the pallet position and angle information detected by the camera are converted from the camera coordinate system to the vehicle coordinate system, and then to the navigation map coordinate system to ensure the consistency and accuracy of the path planning; by applying the mathematical characteristics of the third-order Bezier curve, a smooth pickup route that conforms to the motion characteristics of the AGV is generated, thereby effectively guiding the AGV to reach the target pallet position safely and efficiently from the current position.

[0012] According to an embodiment of the present invention, the endpoint calculation and path planning in step S3 include the following specific steps: S31, coordinate transformation method: According to the relationship between the navigation map coordinate system (XMOMYM), the vehicle coordinate system (XAOAYA) and the camera coordinate system (XCOCYC), a transformation method from the camera coordinate system to the navigation map coordinate system is derived, including the following steps: S311. Assuming the camera coordinate system, the points that need to be transformed The coordinates are (x0, y0). First, translate the origin of the camera coordinate system (XCOCYC) to obtain a new coordinate system (XOY_1) with the center of the vehicle as the origin. Point coordinates (x1, y1): (3) S312, Coordinate system (XOY_1) rotation degrees, so that the direction of its coordinate axis is consistent with the navigation map coordinate system, and a new coordinate system (XOY_2) is obtained. Point coordinates (x2, y2): (4) S313, the coordinate system (XOY_2) is translated to the navigation map coordinate system (XMOMYM), and the Final coordinates of the point (x3, y3): (5) S32, end point coordinate calculation: working depth Represents the final parking position of the forklift The measured point of the pallet The distance between the two points is that both points are on the center line of the pallet. After processing the camera detection data, the position of the pallet in the camera coordinate system (XCOCYC) and angle It has been calculated, so calculate the coordinate position of the end point in this coordinate system: (6) Substituting the calculation result into step S31, the navigation map coordinate system is obtained. The coordinate position of S33, control point calculation: control point distance Set to half the distance between the start and end points: (7) S331, calculate the position of the starting control point: the starting control point is As a starting point, Distance in the opposite direction point , the calculation formula is: (8) S332, calculate the position of the end control point: the end control point is As the starting point, facing the measured point on the pallet Upper distance point ; Will Substitute the position in the camera coordinate system into step S31 to obtain the position in the navigation map coordinate system The coordinate position of ; Calculate the coordinates of the end control point as follows: (9) After the above calculation, a third-order Bezier curve-type pickup route is obtained, and the characteristic points are , , and .

[0013] This technical solution realizes the conversion from the camera coordinate system to the navigation map coordinate system, and accordingly plans a third-order Bezier curve-type pickup route. Specifically, in the coordinate transformation method stage, according to the relationship between the camera coordinate system, the vehicle coordinate system and the navigation map coordinate system, the transformation formula from the camera coordinate system to the navigation map coordinate system is derived, including translating the origin of the camera coordinate system to the center of the vehicle body, rotating the coordinate system so that the direction of its coordinate axis is consistent with the navigation map coordinate system, and then translating the coordinate system to the navigation map coordinate system, so as to obtain the accurate position of the point detected by the camera in the navigation map coordinate system; in the terminal coordinate calculation stage, the pallet position and angle information detected by the camera, as well as the distance between the final parking position of the forklift and the measured point of the pallet, are used to calculate the coordinate of the terminal in the camera coordinate system. The coordinate position of the endpoint is obtained by substituting the result into the coordinate transformation method to obtain the coordinate position of the end point in the navigation map coordinate system; in the control point calculation stage, the control point distance is set according to the distance between the starting point and the end point, and the positions of the starting control point and the end control point in the camera coordinate system are calculated accordingly, and the positions are substituted into the coordinate transformation method to obtain the coordinate positions of the control points in the navigation map coordinate system; a third-order Bezier curve-type pickup route is finally determined through the control points, and the characteristic points of the route are the starting point, the starting control point, the end control point and the end point, which not only improves the accuracy of path planning, but also ensures that the forklift can reach the target pallet position safely and efficiently according to the predetermined route.

[0014] According to an embodiment of the present invention, in the path adaptive optimization of step S4, the characteristic points of the third-order Bezier curve are , , and ,exist and Fixed case, adjust and To affect the shape of the route: Assumptions is the control point distance, is the distance from the starting point to the end point, then the influence of the control point distance on the route shape is: increasing To reduce the risk of collision and To reduce the curvature change of the route.

[0015] In this technical solution, during the path adaptive optimization process, it is necessary to weigh and choose between increasing the control point distance to reduce the risk of collision and reducing the control point distance to reduce the change in route curvature based on actual conditions and needs, so as to obtain a safe and efficient pickup route.

[0016] According to an embodiment of the present invention, the path adaptive optimization in step S4 includes the following specific steps: S41. Forklift collision model related data , , and ; According to the position of the pallet in the camera coordinate system ,angle And the inner distance between the two sides of the pallet , calculate the two corners of the pallet and Coordinate information: (10) calculate The distance from the origin coordinates is substituted into step S31 to obtain the collision risk point in the navigation map coordinate system. The coordinate position of S42, assuming that the initial Bezier path obtained in step 3 is , set the current optimization times ; S43, will Start / end control points shortened / lengthened respectively ,get ; The calculation method of the new coordinates after the control point changes is the same as step S33; S44, using the binary method to determine the sampling step : The equation of the third-order Bezier curve is as follows: (11) and Represents four control points The coordinates of is the sampling point parameter, ranging from ,when hour, ,when hour, ; Sampling step The calculation method is as follows: (12) is a positive integer with an initial value of 1. superior and Distance, if the distance is greater than ,but , and continue to calculate and distance, until the distance is less than , that is, determine The value of Set the initial value of the sampling point parameter t to 0; S45, calculation Sampling point Coordinate values ​​and tangent angles on : Set the current Substituting the value into formula (11) we can get the coordinate value of the point ; Calculate the tangent angle of the point and differentiate the curve equation. Substituting the value into the derivative equation, the tangent vector is calculated ; (13) Then the tangent angle is calculated: (14) S46. Determine if the collision model's motion center is at the sampling point , angle towards When A collision occurs; S461, calculate the coordinate position of the model center in the state; calculate The center coordinates of the model when the rotation occurs ; (15) The calculation formula for the new coordinate C of the known point A after rotating θ around point B is: (16) According to the above formula, calculate around Rotation , get the model center coordinates ; S462, Calculation and Will there be a collision? First, calculate the coordinate system of the navigation map. and The coordinate difference of (17) Then calculate and Vertical distance: (18) According to the following judgment formula, judge the collision situation: (19) If all conditions are met, then exist There is a risk of collision within the range. , return to step S43; otherwise, it indicates that there is no collision risk at present, and proceed to the next step; S47, known sampling point parameters The range is ,make ; like , then return to step S45 to determine the next sampling point; otherwise, it means that the forklift is traveling on the current route, and there is no collision analysis between the forklift and the pallet. The adaptive optimization process ends and a safe Bezier curve route is obtained. .

[0017] This technical solution realizes adaptive optimization of the path to ensure that there is no collision between the forklift and the pallet during driving. Specifically, based on the relevant data of the forklift collision model and the position, angle and inner edge spacing of the pallet on both sides in the camera coordinate system, the coordinate information of the two key corners on the pallet is calculated, and the possible collision risk points in the navigation map coordinate system are determined; based on the initial Bezier route, the shape of the route is changed by adjusting the positions of the start / end control points. In each iteration, the binary search method is used to determine a suitable sampling step size so that the sampling points are evenly distributed on the curve; the coordinate value and tangent angle at each sampling point are calculated, and the information will be used for subsequent collision judgment; in the collision judgment stage, the coordinates of the center of the collision model at the sampling point position are first calculated, and the Taking into account the possible rotation of the model center, the coordinate difference between the model center and the key angle of the pallet is calculated, and the vertical distance between them is calculated; by comparing this vertical distance with the preset safety distance threshold, it is determined whether there is a collision risk at the sampling point position; if there is a collision risk, it will return to the step of adjusting the control point and continue to iteratively optimize the route shape until a solution is found in which there is no collision risk between the forklift and the pallet when driving on the current route; after multiple iterations and optimizations, a Bezier curve route that meets driving requirements and ensures safety can be obtained, which not only improves the flexibility of path planning, but also significantly enhances the forklift's navigation ability in complex environments.

[0018] Compared with the prior art, the present invention has the following beneficial effects: (1) Improving the accuracy and safety of path planning: By comprehensively considering the current position and orientation of the AGV and the conversion relationship between multiple coordinate systems, an accurate third-order Bezier curve-type pickup route is planned; the path is adaptively optimized based on the length and width of the forklift, the width of the pallet, and the collision model to ensure that the forklift maintains a safe distance from the pallet during driving to avoid collision, thereby improving the reliability and safety of the pickup process; (2) Enhance the navigation capability of forklifts in complex environments: The path adaptive optimization process in the technical solution can flexibly adjust the control point distance according to actual conditions and needs to balance the collision risk and route curvature changes, thereby obtaining a safe and efficient pickup route, enhancing the navigation capability and adaptability of forklifts in complex environments and improving pickup efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 It is a structural schematic diagram of the present invention.

[0020] Figure 2 It is a schematic diagram of the collision model and the center of motion.

[0021] Figure 3 It is a schematic diagram of the camera coordinate system and detection.

[0022] Figure 4 It is a path planning diagram.

[0023] Figure 5 It is a schematic diagram of the coordinate system relationship.

[0024] Figure 6 This is a schematic diagram of the status of the laser forklift pickup parking point.

[0025] Figure 7 This is a schematic diagram of a collision between a laser forklift and a pallet.

[0026] Figure 8 This is a schematic diagram of the effect of control point distance on route shape.

[0027] Fig. 9 It is the flow chart of path adaptive optimization.

[0028] Fig.10 It is the result effect diagram of the present invention. DETAILED DESCRIPTION

[0029] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0030] Example 1 like Figure 1 As shown, this embodiment provides a laser forklift cargo picking path replanning and adaptive optimization method thereof, comprising the following steps: S1. Parameter setting: set all parameter values ​​required for calculation; S2, camera data acquisition and processing: obtaining the position and tilt angle of the cargo pallet recognized by the camera; S3, end point calculation and path planning: According to the pallet position and angle information, a third-order Bezier curve-type pickup route is planned with the current position of the AGV as the starting point; S4. Adaptive path optimization: According to the length and width of the forklift forks and the width of the pallet, the planned route is adaptively optimized to prevent the forklift from colliding with the pallet when driving on the path.

[0031] This technical solution initializes all the basic values ​​required for calculation through parameter setting, and then uses the camera data acquisition and processing steps to obtain the specific position of the cargo pallet and its inclination angle information; based on these position and angle information, through the end point calculation and path planning steps, a pickup route starting from the current position of the AGV and using a third-order Bezier curve is planned; in the path adaptive optimization stage, the planned route is adaptively adjusted and optimized according to the length and width of the forklift and the width of the pallet to ensure that the forklift can maintain a safe distance from the pallet during driving to avoid collision.

[0032] In addition, the laser forklift pickup path replanning and adaptive optimization method proposed in the present invention also has the following additional technical features: According to an embodiment of the present invention, in the parameter setting of step S1, the setting method calculates the required parameter values, and the parameter details are shown in the following table: Table 1

[0033] like Figure 2 As shown in the figure, according to the characteristics of forklift picking, the collision model used to calculate whether the vehicle body has a collision risk with the pallet uses the rectangle occupied by the fork part; to ensure safety, before the forklift picks up the pallet, it detects whether the pallet has reached the detection sensor at the root of the fork and the working depth of the fork movement center entering the bottom of the pallet. The calculation formula is: (1).

[0034] This technical solution supports the operation of the entire laser forklift picking path replanning and its adaptive optimization method by setting key parameter values. The parameters include the number of camera data acquisition frames, which is used to determine the frequency of data updates; the installation position of the camera in the X and Y directions of the vehicle coordinate system, and the installation angle of the camera, which together define the spatial position relationship of the camera relative to the forklift, thereby ensuring that the position and tilt angle of the pallet can be accurately obtained; setting parameters representing the length and width of the collision model, and the distance in the X and Y directions of the AGV motion center relative to the center of the collision model, which is used to construct a rectangular collision model that can simulate the fork part of the forklift, so as to provide a reference when calculating whether the forklift collides with the pallet. Reference; Consider the inner edge spacing on both sides of the pallet as the safe passage width of the collision model to ensure that the forklift can maintain a safe distance from the pallet during driving; In the path adaptive optimization stage, set two parameters, the path sampling distance and the control point distance adjustment step length, to guide the fine adjustment and optimization process of the path; According to the characteristics of forklift picking up goods, when calculating whether the forklift collides with the pallet, the rectangle occupied by the fork part is used as the collision model, and a calculation formula is used to determine the working depth of the fork movement center entering the bottom of the pallet; In order to ensure that the forklift can accurately pick up the pallet, a detection sensor is also set to detect whether the pallet has reached the root of the fork, thereby improving the reliability of the picking process.

[0035] like Figure 3 As shown, in the camera data acquisition and processing of step S2, the camera installed on the forklift detects the position and angle information of the pallet in the camera coordinate system (XCOCYC); wherein the detected position Refers to the center point of the pallet near the forklift. The detection angle refers to the rotation angle of the pallet around the measured center point, with counterclockwise being positive. In order to reduce the impact of data fluctuations on the detection results, After the frame detection data is obtained, the average value is applied again. The calculation formula is: (2).

[0036] This technical solution achieves reliable acquisition of the position and angle information of the pallet. Specifically, in order to effectively reduce the error caused by data fluctuations to the detection results, multiple frames of detection data are first collected continuously, and then the data are averaged to be used as the final detection result; the formula (2) is used to calculate the average value, thereby ensuring the stability of the obtained result.

[0037] like Figure 4 As shown, in the end point calculation and path planning of step S3, it is assumed that a third-order Bezier curve-type pickup route is planned with the current position of the AGV as the starting point. The coordinate position of , facing ; The calculation process involves the conversion between the navigation map coordinate system (XMOMYM), the vehicle coordinate system (XAOAYA) and the camera coordinate system (XCOCYC).

[0038] This technical solution plans a third-order Bezier curve-type pickup route starting from the current position of the AGV by comprehensively considering the current position and orientation of the AGV, as well as the conversion relationship between the navigation map coordinate system, the vehicle coordinate system, and the camera coordinate system. During the planning process, the coordinate position and orientation of the end point are determined, which is the target point of the path planning; using coordinate conversion technology, the pallet position and angle information detected by the camera are converted from the camera coordinate system to the vehicle coordinate system, and then to the navigation map coordinate system to ensure the consistency and accuracy of the path planning; by applying the mathematical characteristics of the third-order Bezier curve, a smooth pickup route that conforms to the motion characteristics of the AGV is generated, thereby effectively guiding the AGV to reach the target pallet position safely and efficiently from the current position.

[0039] The end point calculation and path planning of step S3 include the following specific steps: S31, coordinate transformation method: Figure 5 As shown, according to the relationship between the navigation map coordinate system (XMOMYM), the vehicle coordinate system (XAOAYA) and the camera coordinate system (XCOCYC), the transformation method from the camera coordinate system to the navigation map coordinate system is derived, including the following small steps: S311. Assuming the camera coordinate system, the points that need to be transformed The coordinates are (x0, y0). First, translate the origin of the camera coordinate system (XCOCYC) to obtain a new coordinate system (XOY_1) with the center of the vehicle as the origin. Point coordinates (x1, y1): (3) S312, Coordinate system (XOY_1) rotation degrees, so that the direction of its coordinate axis is consistent with the navigation map coordinate system, and a new coordinate system (XOY_2) is obtained. Point coordinates (x2, y2): (4) S313, the coordinate system (XOY_2) is translated to the navigation map coordinate system (XMOMYM), and the Final coordinates of the point (x3, y3): (5) S32, end point coordinate calculation: Figure 6 As shown, the working depth Represents the final parking position of the forklift The measured point of the pallet The distance between the two points is that both points are on the center line of the pallet. After processing the camera detection data, the position of the pallet in the camera coordinate system (XCOCYC) and angle It has been calculated, so calculate the coordinate position of the end point in this coordinate system: (6) Substituting the calculation result into step S31, the navigation map coordinate system is obtained. The coordinate position of S33, control point calculation: control point distance Set to half the distance between the start and end points: (7) S331, calculate the position of the starting control point: the starting control point is As a starting point, Distance in the opposite direction point , the calculation formula is: (8) S332, calculate the position of the end control point: the end control point is As the starting point, facing the measured point on the pallet Upper distance point ; Will Substitute the position in the camera coordinate system into step S31 to obtain the position in the navigation map coordinate system The coordinate position of ; Calculate the coordinates of the end control point as follows: (9) After the above calculation, a third-order Bezier curve-type pickup route is obtained, and the characteristic points are , , and .

[0040] This technical solution realizes the conversion from the camera coordinate system to the navigation map coordinate system, and accordingly plans a third-order Bezier curve-type pickup route. Specifically, in the coordinate transformation method stage, according to the relationship between the camera coordinate system, the vehicle coordinate system and the navigation map coordinate system, the transformation formula from the camera coordinate system to the navigation map coordinate system is derived, including translating the origin of the camera coordinate system to the center of the vehicle body, rotating the coordinate system so that the direction of its coordinate axis is consistent with the navigation map coordinate system, and then translating the coordinate system to the navigation map coordinate system, so as to obtain the accurate position of the point detected by the camera in the navigation map coordinate system; in the terminal coordinate calculation stage, the pallet position and angle information detected by the camera, as well as the distance between the final parking position of the forklift and the measured point of the pallet, are used to calculate the coordinate of the terminal in the camera coordinate system. The coordinate position of the endpoint is obtained by substituting the result into the coordinate transformation method to obtain the coordinate position of the end point in the navigation map coordinate system; in the control point calculation stage, the control point distance is set according to the distance between the starting point and the end point, and the positions of the starting control point and the end control point in the camera coordinate system are calculated accordingly, and the positions are substituted into the coordinate transformation method to obtain the coordinate positions of the control points in the navigation map coordinate system; a third-order Bezier curve-type pickup route is finally determined through the control points, and the characteristic points of the route are the starting point, the starting control point, the end control point and the end point, which not only improves the accuracy of path planning, but also ensures that the forklift can reach the target pallet position safely and efficiently according to the predetermined route.

[0041] like Figure 7 As shown, in the path adaptive optimization of step S4, the characteristic points of the third-order Bezier curve are , , and ,exist and Fixed case, adjust and To affect the shape of the route: Assumptions is the control point distance, is the distance from the starting point to the end point, then the influence of the control point distance on the route shape is: increasing To reduce the risk of collision and To reduce the curvature change of the route.

[0042] like Figure 8 As shown in the figure, when the distance of the terminal control point increases, the route section where the fork enters the pallet to pick up the goods becomes smoother, and the distance between the fork and the two sides of the pallet increases, thereby reducing the risk of collision. However, the increase in the distance of the control points will also increase the curvature change of the route, affecting the control accuracy of the vehicle. In summary, the path adaptive optimization can be carried out around the idea of ​​"increasing the distance of the terminal control point to reduce the risk of collision, and reducing the distance of the starting control point to reduce the change of route curvature".

[0043] like Fig. 9 As shown, the path adaptive optimization in step S4 includes the following specific steps: S41. Forklift collision model related data , , and ; According to the position of the pallet in the camera coordinate system ,angle And the inner distance between the two sides of the pallet , calculate the two corners of the pallet and Coordinate information: (10) calculate The distance from the origin coordinates is substituted into step S31 to obtain the collision risk point in the navigation map coordinate system. The coordinate position of S42, assuming that the initial Bezier path obtained in step 3 is , set the current optimization times ; S43, will Start / end control points shortened / lengthened respectively ,get ; The calculation method of the new coordinates after the control point changes is the same as step S33; S44, using the binary method to determine the sampling step : The equation of the third-order Bezier curve is as follows: (11) and Represents four control points The coordinates of is the sampling point parameter, ranging from ,when hour, ,when hour, ; Sampling step The calculation method is as follows: (12) is a positive integer with an initial value of 1. superior and Distance, if the distance is greater than ,but , and continue to calculate and distance, until the distance is less than , that is, determine The value of Set the initial value of the sampling point parameter t to 0; S45, calculation Sampling point Coordinate values ​​and tangent angles on : Set the current Substituting the value into formula (11) we can get the coordinate value of the point ; Calculate the tangent angle of the point and differentiate the curve equation. Substituting the value into the derivative equation, the tangent vector is calculated ; (13) Then the tangent angle is calculated: (14) S46. Determine the collision model The center of motion is at the sampling point , angle towards When A collision occurs; S461, calculate the coordinate position of the model center in the state; calculate The center coordinates of the model when the rotation occurs ; (15) The calculation formula for the new coordinate C of the known point A after rotating θ around point B is: (16) According to the above formula, calculate around Rotation , get the model center coordinates ; S462, Calculation and Will there be a collision? First, calculate the coordinate system of the navigation map. and The coordinate difference of (17) Then calculate and Vertical distance: (18) According to the following judgment formula, judge the collision situation: (19) If all conditions are met, then exist There is a risk of collision within the range. , return to step S43; otherwise, it indicates that there is no collision risk at present, and proceed to the next step; S47, known sampling point parameters The range is ,make ; like , then return to step S45 to determine the next sampling point; otherwise, it means that the forklift is traveling on the current route, and there is no collision analysis between the forklift and the pallet. The adaptive optimization process ends and a safe Bezier curve route is obtained. .

[0044] This technical solution realizes adaptive optimization of the path to ensure that there is no collision between the forklift and the pallet during driving. Specifically, based on the relevant data of the forklift collision model and the position, angle and inner edge spacing of the pallet on both sides in the camera coordinate system, the coordinate information of the two key corners on the pallet is calculated, and the possible collision risk points in the navigation map coordinate system are determined; based on the initial Bezier route, the shape of the route is changed by adjusting the positions of the start / end control points. In each iteration, the binary search method is used to determine a suitable sampling step size so that the sampling points are evenly distributed on the curve; the coordinate value and tangent angle at each sampling point are calculated, and the information will be used for subsequent collision judgment; in the collision judgment stage, the coordinates of the center of the collision model at the sampling point position are first calculated, and the Taking into account the possible rotation of the model center, the coordinate difference between the model center and the key angle of the pallet is calculated, and the vertical distance between them is calculated; by comparing this vertical distance with the preset safety distance threshold, it is determined whether there is a collision risk at the sampling point position; if there is a collision risk, it will return to the step of adjusting the control point and continue to iteratively optimize the route shape until a solution is found in which there is no collision risk between the forklift and the pallet when driving on the current route; after multiple iterations and optimizations, a Bezier curve route that meets driving requirements and ensures safety can be obtained, which not only improves the flexibility of path planning, but also significantly enhances the forklift's navigation ability in complex environments.

[0045] Example 2 Based on Example 1, the setting method of this embodiment calculates the required parameter values, and the parameter details are shown in the following table: Table 2

[0046] The current position data of the laser forklift in the navigation map coordinate system is: Location =(13.349, 10.634), the direction is =-2.7925; Collect camera detection data and process it to get the position data of the pallet in the camera coordinate system: Location =(0.921, 3.341), the direction is =0.3489; After calculation by the path planning method, a third-order Bezier route is initially obtained, and its characteristic points are: =(13.349000, 10.634000) =(14.948952, 11.216383) =(15.411963, 10.047229) =(16.716240, 11.141706).

[0047] like Fig.10 As shown in the figure, after collision risk detection, it is determined that the forklift will collide with the pallet when driving on this route, so the adaptive optimization step is automatically started. After 2 iterations, the distance between the start / end control points of the route is shortened / extended by 0.1 respectively. The final route feature points are: =(13.349000, 10.634000) =(14.854984, 11.182179) =(15.258758, 9.918667) =(16.716240, 11.141706).

[0048] Although the present invention is described in detail by referring to the accompanying drawings and in combination with the preferred embodiments, the present invention is not limited thereto. Without departing from the spirit and essence of the present invention, a person of ordinary skill in the art may make various equivalent modifications or substitutions to the embodiments of the present invention, and these modifications or substitutions shall be within the scope of the present invention. Any person of ordinary skill in the art may easily think of changes or substitutions within the technical scope disclosed by the present invention, and these shall be within the scope of protection of the present invention. Therefore, the scope of protection of the present invention shall be subject to the scope of protection of the claims.

Claims

1. A laser forklift cargo picking path replanning and adaptive optimization method, characterized in that: The steps include: S1. Parameter setting: set all parameter values ​​required for calculation; S2, camera data acquisition and processing: obtaining the position and tilt angle of the cargo pallet recognized by the camera; S3, end point calculation and path planning: According to the pallet position and angle information, a third-order Bezier curve-type pickup route is planned with the current position of the AGV as the starting point; S4. Adaptive path optimization: According to the length and width of the forklift forks and the width of the pallet, the planned route is adaptively optimized to prevent the forklift from colliding with the pallet when driving on the path.

2. The laser forklift cargo picking path replanning and adaptive optimization method as claimed in claim 1, characterized in that: In the parameter setting of step S1, the setting method calculates the required parameter values, including: Represents the number of frames of camera data acquisition; Represents the camera's installation position in the X direction of the vehicle coordinate system; Represents the camera installation position in the Y direction of the vehicle coordinate system; Represents the installation angle of the camera in the vehicle coordinate system; Represents the collision model length; Represents the collision model width; Represents the X-direction distance between the AGV motion center and the model center; Represents the Y-direction distance between the AGV motion center and the model center; Represents the width of the collision model for safe passage, i.e. the distance between the inner edges of the two sides of the pallet; Represents the path sampling distance during path adaptive optimization; Represents the control point distance adjustment step size during path adaptive optimization; According to the characteristics of forklift picking, the collision model used to calculate whether the vehicle body has a collision risk with the pallet uses the rectangle occupied by the fork part; to ensure safety, before the forklift picks up the pallet, it detects whether the pallet has reached the detection sensor at the root of the fork and the working depth of the fork movement center entering the bottom of the pallet. The calculation formula is: （1）。 3. The laser forklift cargo picking path replanning and adaptive optimization method as claimed in claim 2, characterized in that: In the camera data acquisition and processing of step S2, the camera installed on the forklift detects the position and angle information of the pallet in the camera coordinate system (XCOCYC); wherein the detected position Refers to the center point of the pallet near the forklift. The detection angle refers to the rotation angle of the pallet around the measured center point, with counterclockwise being positive. In order to reduce the impact of data fluctuations on the detection results, After the frame detection data is obtained, the average value is applied again. The calculation formula is: （2）。 4. The laser forklift cargo picking path replanning and adaptive optimization method as claimed in claim 3, characterized in that: In the end point calculation and path planning of step S3, it is assumed that a third-order Bezier curve-type pickup route is planned with the current position of the AGV as the starting point. The coordinate position of , facing ; The calculation process involves the conversion between the navigation map coordinate system (XMOMYM), the vehicle coordinate system (XAOAYA) and the camera coordinate system (XCOCYC).

5. The laser forklift cargo picking path replanning and adaptive optimization method as claimed in claim 4, characterized in that: The end point calculation and path planning of step S3 include the following specific steps: S31, coordinate transformation method: According to the relationship between the navigation map coordinate system (XMOMYM), the vehicle coordinate system (XAOAYA) and the camera coordinate system (XCOCYC), a transformation method from the camera coordinate system to the navigation map coordinate system is derived, including the following steps: S311. Assuming the camera coordinate system, the points that need to be transformed The coordinates are (x0, y0). First, translate the origin of the camera coordinate system (XCOCYC) to obtain a new coordinate system (XOY_1) with the center of the vehicle as the origin. Point coordinates (x1, y1): (3) S312, Coordinate system (XOY_1) rotation degrees, so that the direction of its coordinate axis is consistent with the navigation map coordinate system, and a new coordinate system (XOY_2) is obtained. Point coordinates (x2, y2): (4) S313, the coordinate system (XOY_2) is translated to the navigation map coordinate system (XMOMYM), and the Final coordinates of the point (x3, y3): (5) S32, end point coordinate calculation: working depth Represents the final parking position of the forklift The measured point of the pallet The distance between the two points is that both points are on the center line of the pallet. After processing the camera detection data, the position of the pallet in the camera coordinate system (XCOCYC) and angle It has been calculated, so calculate the coordinate position of the end point in this coordinate system: (6) Substituting the calculation result into step S31, the navigation map coordinate system is obtained. The coordinate position of S33, control point calculation: control point distance Set to half the distance between the start and end points: (7) S331, calculate the position of the starting control point: the starting control point is As a starting point, Distance in the opposite direction point , the calculation formula is: (8) S332, calculate the position of the end control point: the end control point is As the starting point, facing the measured point on the pallet Upper distance point ; Will Substitute the position in the camera coordinate system into step S31 to obtain the position in the navigation map coordinate system The coordinate position of ; Calculate the coordinates of the end control point as follows: (9) After the above calculation, a third-order Bezier curve-type pickup route is obtained, and the characteristic points are , , and .

6. The laser forklift cargo picking path replanning and adaptive optimization method as claimed in claim 5, characterized in that: In the path adaptive optimization of step S4, the characteristic points of the third-order Bezier curve are , , and ,exist and Fixed case, adjust and To affect the shape of the route: Assumptions is the control point distance, is the distance from the starting point to the end point, then the influence of the control point distance on the route shape is: increasing To reduce the risk of collision and To reduce the curvature change of the route.

7. The laser forklift cargo picking path replanning and adaptive optimization method as claimed in claim 6, characterized in that: The path adaptive optimization in step S4 includes the following specific steps: S41. Forklift collision model related data , , and ; According to the position of the pallet in the camera coordinate system ,angle And the inner distance between the two sides of the pallet , calculate the two corners of the pallet and Coordinate information: (10) calculate The distance from the origin coordinates is substituted into step S31 to obtain the collision risk point in the navigation map coordinate system. The coordinate position of S42, assuming that the initial Bezier path obtained in step 3 is , set the current optimization times ; S43, will Start / end control points shortened / lengthened respectively ,get ; The calculation method of the new coordinates after the control point changes is the same as step S33; S44, using the binary method to determine the sampling step : The equation of the third-order Bezier curve is as follows: (11) and Represents four control points The coordinates of is the sampling point parameter, ranging from ,when hour, ,when hour, ; Sampling step The calculation method is as follows: (12) is a positive integer with an initial value of 1. superior and Distance, if the distance is greater than ,but , and continue to calculate and distance, until the distance is less than , that is, determine The value of Set the initial value of the sampling point parameter t to 0; S45, calculation Sampling point Coordinate values ​​and tangent angles on : Set the current Substituting the value into formula (11) we can get the coordinate value of the point ; Calculate the tangent angle of the point and differentiate the curve equation. Substituting the value into the derivative equation, the tangent vector is calculated ; (13) Then the tangent angle is calculated: (14) S46. Determine if the collision model's motion center is at the sampling point , angle towards When A collision occurs; S461, calculate the coordinate position of the model center in the state; calculate The center coordinates of the model when the rotation occurs ; (15) The calculation formula for the new coordinate C of the known point A after rotating θ around point B is: (16) According to the above formula, calculate around Rotation , get the model center coordinates ; S462, Calculation and Will there be a collision? First, calculate the coordinate system of the navigation map. and The coordinate difference of (17) Then calculate and Vertical distance: (18) According to the following judgment formula, judge the collision situation: (19) If all conditions are met, then exist There is a risk of collision within the range. , return to step S43; Otherwise, it means there is no collision risk at present, and proceed to the next step; S47, known sampling point parameters The range is ,make ; like , then return to step S45 to determine the next sampling point; otherwise, it means that the forklift is traveling on the current route, and there is no collision analysis between the forklift and the pallet. The adaptive optimization process ends and a safe Bezier curve route is obtained. .

Images