A loader auxiliary control system based on machine vision

Abstract

The present invention belongs to the field of loader auxiliary control, and specifically discloses a loader auxiliary control system based on machine vision, the system includes: a data acquisition module, a target detection module, an intelligent movement module and a loading path planning module. The present invention classifies obstacles and loads through a classification method based on semantic segmentation, fuses the segmentation results with the depth image, obtains a marked point cloud, and maps obstacles and loads in a 2D grid of the site. The loader movement path is planned through the DWA algorithm and the FGM algorithm, which can more accurately identify and classify different objects in the site, plan a path to avoid collision, and improve operation safety; a mathematical model is established for the loading cost during the loading process, and a parallel genetic algorithm is used to optimize the cost problem, reduce unnecessary movement and energy consumption, and reduce fuel consumption and equipment wear.

Description

Technical Field

[0001] The present invention relates to the field of loader auxiliary control, and in particular to a loader auxiliary control system based on machine vision. Background Art

[0002] Loader auxiliary control is a system that uses machine learning technology to automate the operation of the loader. It can provide operating instructions or perform automatic control based on the loader's control, reduce the loader's labor intensity and improve operation safety. It is a challenge for the loader to distinguish between obstacles and loads during operation. When the loader detects an object in front, if it cannot accurately distinguish between obstacles and loads, it will cause the loader to stop frequently and reduce operation efficiency. At the same time, the loading arm of an ordinary loader may not be precise enough, resulting in inaccurate positioning of the loaded material, requiring multiple adjustments. Unplanned random movements may cause unnecessary stress on the joints and connecting parts of the loading arm, accelerating wear. Summary of the invention

[0003] In view of the above situation, in order to overcome the defects of the prior art, the present invention provides a loader auxiliary control system based on machine vision. In view of the technical problem that it is difficult for the loader to distinguish obstacles and loads during operation, when the loader detects an object in front, if the loader control system cannot accurately distinguish obstacles and loads, the loader will frequently stop working and reduce the operation efficiency. The present invention classifies obstacles and loads through a classification method based on semantic segmentation, fuses the segmentation results with the depth image to obtain a marked point cloud, and maps the obstacles and loads in the 2D grid of the site. Through the DWA (Dynamic Window Approach) algorithm and FGM (Follow the Gap) The invention adopts a method to plan the moving path of the loader, which can more accurately identify and classify different objects in the field, plan a path to avoid collision, and improve the safety of operation. The loading arm of the ordinary loader may not be precise enough, resulting in inaccurate position of the loaded material, which requires multiple adjustments, and the unplanned random movement may cause the joints and connecting parts of the loading arm to bear unnecessary stress and accelerate wear. The invention establishes a mathematical model for the loading cost in the loading process, uses a parallel genetic algorithm to optimize the cost problem, reduces unnecessary movement and energy consumption, reduces fuel consumption and equipment wear, and thus reduces maintenance costs.

[0004] The technical solution adopted by the present invention is as follows: The present invention provides a loader auxiliary control system based on machine vision, and the loader auxiliary control system based on machine vision includes a data acquisition module, a target detection module, an intelligent movement module and a loading path planning module;

[0005] The data acquisition module collects RGB images and depth images by four RGB-D cameras installed on the loader, and the depth frames of the depth images are aligned with the RGB-D camera coordinates;

[0006] The target detection module classifies obstacles and loads to obtain segmentation results;

[0007] The intelligent mobile module fuses the segmentation results with the depth image to obtain a marked point cloud, and maps obstacles and loads in the 2D grid of the site, and plans the loader's mobile path through the DWA algorithm and the FGM algorithm;

[0008] The loading path planning module plans the movement path of the loading arm of the loader to minimize the loading cost during the loading process.

[0009] Furthermore, the target detection module classifies obstacles and loads by a classification method based on semantic segmentation, and the classification method based on semantic segmentation specifically includes the following steps:

[0010] Step A1: collecting a target object image set, wherein the target object image set includes historical target object images collected by an RGB-D camera and labels corresponding to the historical target object images, wherein the labels include obstacles, loads, and people;

[0011] Step A2: Establish and initialize two semantic segmentation models, use the DeepLabv3+ network as the model architecture, and the pre-trained ResNet model as the feature extractor. The two semantic segmentation models are the obstacle detection model and the load detection model. The obstacle detection model uses the Sigmoid function as the activation function, and the load detection model uses the SoftMax function as the activation function.

[0012] Step A3: Input the target image set into the semantic segmentation model for training, perform semantic segmentation on the target image, obtain a single-channel 448×448 mask image, use pixel values ​​as segmentation labels to distinguish obstacles, loads, and people, upsample the mask image to match the size of the depth image, and obtain the segmentation result.

[0013] Furthermore, the intelligent mobile module maps obstacles and loads in a 2D grid of the site by a point cloud reconstruction method, wherein the point cloud reconstruction method specifically comprises the following steps:

[0014] Step B1: Fuse the segmentation result with the depth image to obtain a marked point cloud. The marked point cloud consists of XYZL points. The XYZL points are the coordinate information of the points and the segmentation labels assigned in the semantic segmentation. The point coordinates are calculated using the following formula:

[0015] ;

[0016] In the formula, is the real-world coordinate, are the coordinates of the depth image, is the depth image in The depth value at the point, is the intrinsic matrix of the RGB-D camera;

[0017] Step B2: Use the point cloud library to filter and downsample the generated labeled point cloud to obtain the reconstructed point cloud;

[0018] Step B3: Use the 2D grid to represent the loader's workspace as a discrete grid, update the 2D grid, and reconstruct the points in the point cloud. The coordinates are projected onto the coordinates of the 2D grid using the following formula:

[0019] ;

[0020] In the formula, yes The coordinates are projected onto the 2D grid. and yes The offset of the coordinates relative to the origin and size of the 2D grid, is the map resolution in meters;

[0021] Step B4: The loader plans the moving path according to the updated 2D grid using a path planning algorithm that combines the DWA algorithm and the FGM algorithm;

[0022] Furthermore, in step B4, a path planning method combining the DWA algorithm and the IFGM algorithm is used to plan the moving path. The path planning method combining the DWA algorithm and the IFGM algorithm specifically includes the following steps:

[0023] Step B41: Mark obstacles and people as obstacles. Assume that the loader and the obstacle are both circular objects. The loader is regarded as a point in Cartesian space. The Pythagorean theorem is used to calculate the actual distance between the loader and the obstacle. The safe distance between the edge of the loader and the obstacle is calculated. The formula used is as follows:

[0024] ;

[0025] In the formula, is the safe distance between the loader and the obstacle, is the maximum speed of the loader, is the maximum acceleration of the loader, Indicates time;

[0026] Step B42: Construct a gap array to calculate the heading angle to guide the loader to move. The heading angle is composed of the gap angle and the target angle. Calculate the utility function of each gap and select a feasible gap for the loader. The formula used is as follows:

[0027] ;

[0028] In the formula, is the utility function of each gap, It is The size of the gap, It is The center of the gap, It is the load. and are the weight coefficients of gap size and angle, respectively;

[0029] Step B43: Use a mathematical formula to represent the movement of the loader moving in a straight line toward the load. When the loader finds an obstacle in the straight path, the loader increases the heading angle and reduces the speed until it avoids the obstacle. The formula used is as follows:

[0030] ;

[0031] In the formula, and are the horizontal and vertical coordinates of the loader, The loader is The speed at the moment is the linear speed, The loader is The angular velocity at time, is the heading angle of the loader, ;

[0032] Step B44: The loader continuously calculates the distance and updates the position to determine the optimal speed of the loader as the objective function of path planning. The formula used is as follows:

[0033] ;

[0034] In the formula, It is the optimal speed for the loader.

[0035] Furthermore, the loading path planning module plans the loading arm motion path using a parallel genetic algorithm, and the parallel genetic algorithm specifically includes the following steps:

[0036] Step C1: Assume that the loader is not allowed to move during loading. The loading cost includes loading time, fuel consumption, safety risk and operating difficulty of the loader. A mathematical model is established for the action of the loading arm, and the loading path is defined as a structure. The structure provides an independent configuration for the genetic algorithm search. The loading path is optimized and the optimization problem is mathematically expressed. The formula used is as follows;

[0037] ;

[0038] In the formula, is a string representing the mount path, yes The structure, yes The Strip edge, yes Length, is the angle between the loading arm and the ground, is the rotation angle of the loading arm, is the loading length of the loading arm, is the rotation angle of the load;

[0039] Step C2: Define two indicators of the loading path, namely the total number of units moving in different directions and the number of different parameters between two configurations, and obtain the evaluation function of the loading path. The formula used is as follows:

[0040] ;

[0041] ;

[0042] In the formula, and They are two indicator functions, and They are two different structures. is the scaling factor, is a piecewise function, When it is 0, is 0, otherwise, is 1, is the evaluation function, is the distance cost, is the loading switching cost, and is a constant scaling factor;

[0043] Step C3: Calculate the maximum optimization problem of path planning to minimize the loading cost, and add the hard constraints in the maximum optimization problem as the loss function to the fitness function. The formula used is as follows;

[0044] ;

[0045] In the formula, is the fitness function, It is A string representing the mount path, is the cost of movement, is a custom collision violation number;

[0046] Step C4: Use simulation software to simulate the loading path and test the feasibility of the loading path. If it is feasible, it is output as the optimal loading path. Otherwise, re-planning is performed.

[0047] The beneficial effects achieved by the present invention using the above scheme are as follows:

[0048] (1) In view of the technical problem that it is difficult for a loader to distinguish between obstacles and loads during operation, when the loader detects an object in front, if the loader control system cannot accurately distinguish between obstacles and loads, the loader will frequently stop working, thereby reducing the operating efficiency. The present invention classifies obstacles and loads through a classification method based on semantic segmentation, fuses the segmentation results with the depth image to obtain a marked point cloud, and maps the obstacles and loads in a 2D grid of the site. The loader movement path is planned through the DWA algorithm and the FGM algorithm, which can more accurately identify and classify different objects in the site, plan a path to avoid collisions, and improve the safety of operation;

[0049] (2) In view of the technical problem that the loading arm of an ordinary loader may not be precise enough, resulting in inaccurate positioning of the loaded materials and requiring multiple adjustments, and that unplanned random movements may cause the joints and connecting parts of the loading arm to bear unnecessary stress and accelerate wear, the present invention establishes a mathematical model for the loading cost during the loading process and uses a parallel genetic algorithm to optimize the cost issue, thereby reducing unnecessary movement and energy consumption, reducing fuel consumption and equipment wear, and thus reducing maintenance costs. BRIEF DESCRIPTION OF THE DRAWINGS

[0050] Figure 1 A module connection diagram of a machine vision-based loader auxiliary control system provided by the present invention.

[0051] The accompanying drawings are used to provide further understanding of the present invention and constitute a part of the specification. They are used to explain the present invention together with the embodiments of the present invention and do not constitute a limitation of the present invention. DETAILED DESCRIPTION

[0052] The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, rather than all the embodiments; based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0053] Example 1: See Figure 1 , this embodiment provides a loader auxiliary control system based on machine vision, the loader auxiliary control system based on machine vision includes a data acquisition module, a target detection module, an intelligent movement module and a loading path planning module;

[0054] The data acquisition module collects RGB images and depth images by four RGB-D cameras installed on the loader, and the depth frames of the depth images are aligned with the RGB-D camera coordinates;

[0055] The target detection module classifies obstacles and loads to obtain segmentation results;

[0056] The intelligent mobile module fuses the segmentation results with the depth image to obtain a marked point cloud, and maps obstacles and loads in the 2D grid of the site, and plans the loader's mobile path through the DWA algorithm and the FGM algorithm;

[0057] The loading path planning module plans the movement path of the loading arm of the loader to minimize the loading cost during the loading process.

[0058] Example 2: See Figure 1 This embodiment is based on the above embodiment. The target detection module classifies obstacles and loads by a classification method based on semantic segmentation. The classification method based on semantic segmentation specifically includes the following steps:

[0059] Step A1: collecting a target object image set, wherein the target object image set includes historical target object images collected by an RGB-D camera and labels corresponding to the historical target object images, wherein the labels include obstacles, loads, and people;

[0060] Step A2: Establish and initialize two semantic segmentation models, use the DeepLabv3+ network as the model architecture, and the pre-trained ResNet model as the feature extractor. The two semantic segmentation models are the obstacle detection model and the load detection model. The obstacle detection model uses the Sigmoid function as the activation function, and the load detection model uses the SoftMax function as the activation function.

[0061] Step A3: Input the target image set into the semantic segmentation model for training, perform semantic segmentation on the target image, obtain a single-channel 448×448 mask image, use pixel values ​​as segmentation labels to distinguish obstacles, loads, and people, upsample the mask image to match the size of the depth image, and obtain the segmentation result.

[0062] Example 3: See Figure 1 This embodiment is based on the above embodiment. The intelligent mobile module maps obstacles and loads in a 2D grid of the site through a point cloud reconstruction method. The point cloud reconstruction method specifically includes the following steps:

[0063] Step B1: Fuse the segmentation result with the depth image to obtain a labeled point cloud. The point cloud consists of XYZL points. The XYZL points are the coordinate information of the points in the three-dimensional world and the segmentation labels assigned in the semantic segmentation. The point coordinates are calculated using the following formula:

[0064] ;

[0065] In the formula, is the real-world coordinate, is the segmentation result coordinate, is the depth image in The depth value at the point, is the intrinsic matrix of the RGB-D camera;

[0066] Step B2: Use the constant 1000 to obtain the coordinate result in meters, and use the point cloud library to filter and downsample the generated point cloud to obtain the marked point cloud;

[0067] Step B3: Use the 2D grid to represent the loader's workspace as a discrete grid, update the 2D grid, and mark the points in the point cloud. The coordinates are projected onto the coordinates of the map unit using the following formula:

[0068] ;

[0069] In the formula, is the offset relative to the map origin and size, is the map resolution in meters;

[0070] Step B4: The loader plans the moving path according to the updated 2D grid using a path planning algorithm that combines the DWA algorithm and the FGM algorithm;

[0071] Furthermore, in step B4, a path planning method combining the DWA algorithm and the IFGM algorithm is used to plan the moving path. The path planning method combining the DWA algorithm and the IFGM algorithm specifically includes the following steps:

[0072] Step B41: Mark obstacles and people as obstacles. Assume that the loader and the obstacle are both circular objects. The loader is regarded as a point in Cartesian space. The Pythagorean theorem is used to calculate the actual distance between the loader and the obstacle. The safe distance between the edge of the loader and the obstacle is calculated. The formula used is as follows:

[0073] ;

[0074] In the formula, is the safe distance between the loader and the obstacle, is the maximum speed of the loader, is the maximum acceleration of the loader, Indicates time;

[0075] Step B42: Construct a gap array to calculate the heading angle to guide the loader to move. The heading angle is composed of the gap angle and the target angle. Calculate the utility function of each gap and select a feasible gap for the loader. The formula used is as follows:

[0076] ;

[0077] In the formula, is the utility function of each gap, It is The size of the gap, It is The center of the gap, It is the load. and are the weight coefficients of gap size and angle, respectively;

[0078] Step B43: Use a mathematical formula to represent the movement of the loader moving in a straight line toward the load. When the loader finds an obstacle in the straight path, the loader increases the heading angle and reduces the speed until it avoids the obstacle. The formula used is as follows:

[0079] ;

[0080] In the formula, and are the horizontal and vertical coordinates of the loader, The loader is The speed at the moment is the linear speed, The loader is The angular velocity at time, is the heading angle of the loader, ;

[0081] Step B44: The loader continuously calculates the distance and updates the position to determine the optimal speed of the loader as the objective function of path planning. The formula used is as follows:

[0082] ;

[0083] In the formula, It is the optimal speed for the loader.

[0084] Through the above operations, it is difficult for the loader to distinguish between obstacles and loads during operation. When the loader detects an object in front, if the loader control system cannot accurately distinguish between obstacles and loads, the loader will frequently stop working, reducing the operating efficiency. The present invention classifies obstacles and loads through a classification method based on semantic segmentation, fuses the segmentation results with the depth image to obtain a marked point cloud, and maps the obstacles and loads in a 2D grid of the site. The loader movement path is planned through the DWA algorithm and the FGM algorithm, which can more accurately identify and classify different objects in the site, plan a path to avoid collisions, and improve operation safety.

[0085] Example 4: See Figure 1 This embodiment is based on the above embodiment. The loading path planning module plans the loading arm motion path using a parallel genetic algorithm. The parallel genetic algorithm specifically includes the following steps:

[0086] Step C1: Assume that the loader is not allowed to move during loading. The loading cost includes loading time, fuel consumption, safety risk and operating difficulty of the loader. A mathematical model is established for the action of the loading arm, and the loading path is defined as a structure. The structure provides an independent configuration for the genetic algorithm search. The loading path is optimized and the optimization problem is mathematically expressed. The formula used is as follows;

[0087] ;

[0088] In the formula, is a string representing the mount path, yes The structure, yes The Strip edge, yes Length, is the angle between the loading arm and the ground, is the rotation angle of the loading arm, is the loading length of the loading arm, is the rotation angle of the load;

[0089] Step C2: Define two indicators of the loading path, namely the total number of units moving in different directions and the number of different parameters between two configurations, and obtain the evaluation function of the loading path. The formula used is as follows:

[0090] ;

[0091] ;

[0092] In the formula, and They are two indicator functions, and They are two different structures. is the scaling factor, is a piecewise function, When it is 0, is 0, otherwise, is 1, is the evaluation function, is the distance cost, is the loading switching cost, and is a constant scaling factor;

[0093] Step C3: Calculate the maximum optimization problem of path planning to minimize the loading cost, and add the hard constraints in the maximum optimization problem as the loss function to the fitness function. The formula used is as follows;

[0094] ;

[0095] In the formula, is the fitness function, It is A string representing the mount path, is the cost of movement, is a custom collision violation number;

[0096] Step C4: Use simulation software to simulate the loading path and test the feasibility of the loading path. If it is feasible, it is output as the optimal loading path. Otherwise, re-planning is performed.

[0097] Through the above operations, the loading arm of an ordinary loader may not be precise enough, resulting in inaccurate positioning of the loaded materials and requiring multiple adjustments. Unplanned random movement may cause the joints and connecting parts of the loading arm to bear unnecessary stress and accelerate wear. The present invention establishes a mathematical model for the loading cost during the loading process, uses a parallel genetic algorithm to optimize the cost issue, reduces unnecessary movement and energy consumption, reduces fuel consumption and equipment wear, and thus reduces maintenance costs.

[0098] Embodiment 5: This embodiment is based on the above embodiment. Embodiment 2 uses pixel values ​​as segmentation labels to distinguish obstacles, loads and people, wherein a pixel value of 0 represents an obstacle, a pixel value of 1 represents a load, and a pixel value of 2 represents a person.

[0099] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device.

[0100] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

[0101] The present invention and its embodiments are described above, and such description is not restrictive. The drawings show only one embodiment of the present invention, and the actual structure is not limited thereto. In short, if ordinary technicians in the field are inspired by it, without departing from the purpose of the invention, they can design a structure and embodiment similar to the technical solution without creativity, which should belong to the protection scope of the present invention.

Claims

1. A loader auxiliary control system based on machine vision, characterized in that: It includes data acquisition module, target detection module, intelligent movement module and loading path planning module; The data acquisition module collects RGB images and depth images by four RGB-D cameras installed on the loader, and the depth frames of the depth images are aligned with the RGB-D camera coordinates; The target detection module classifies obstacles and loads to obtain segmentation results; The intelligent mobile module fuses the segmentation results with the depth image to obtain a marked point cloud, and maps obstacles and loads in the 2D grid of the site, and plans the loader's mobile path through the DWA algorithm and IFGM algorithm; The loading path planning module plans the movement path of the loading arm of the loader to minimize the loading cost during the loading process; The intelligent mobile module maps obstacles and loads in the 2D grid of the site through a point cloud reconstruction method, wherein the point cloud reconstruction method specifically includes the following steps: Step B1: Fuse the segmentation result with the depth image to obtain a marked point cloud. The marked point cloud consists of XYZL points. The XYZL points are the coordinate information of the points and the segmentation labels assigned in the semantic segmentation. The point coordinates are calculated using the following formula: ； In the formula, is the real-world coordinate, are the coordinates of the depth image, is the depth image in The depth value at the point, is the intrinsic matrix of the RGB-D camera; Step B2: Use the point cloud library to filter and downsample the generated labeled point cloud to obtain the reconstructed point cloud; Step B3: Use the 2D grid to represent the loader's workspace as a discrete grid, update the 2D grid, and reconstruct the points in the point cloud. The coordinates are projected onto the coordinates of the 2D grid using the following formula: ； In the formula, yes The coordinates are projected onto the 2D grid. and yes The offset of the coordinates relative to the origin and size of the 2D grid, is the map resolution in meters; Step B4: The loader plans the moving path according to the updated 2D grid using a path planning algorithm that combines the DWA algorithm and the IFGM algorithm; The loading path planning module plans the loading arm motion path using a parallel genetic algorithm, and the parallel genetic algorithm specifically includes the following steps: Step C1: Assume that the loader is not allowed to move during loading. The loading cost includes loading time, fuel consumption, safety risk and operating difficulty of the loader. A mathematical model is established for the action of the loading arm, and the loading path is defined as a structure. The structure provides an independent configuration for the genetic algorithm search. The loading path is optimized and the optimization problem is mathematically expressed. The formula used is as follows; ； In the formula, is a string representing the mount path, yes The structure, yes The Strip edge, yes Length, is the angle between the loading arm and the ground, is the rotation angle of the loading arm, is the loading length of the loading arm, is the rotation angle of the load; Step C2: Define two indicators of the loading path, namely the total number of units moving in different directions and the number of different parameters between two configurations, and obtain the evaluation function of the loading path. The formula used is as follows: ； ； In the formula, and They are two indicator functions, and They are two different structures. is the scaling factor, is a piecewise function, When it is 0, is 0, otherwise, is 1, is the evaluation function, is the distance cost, is the loading switching cost, and is a constant scaling factor; Step C3: Calculate the maximum optimization problem of path planning to minimize the loading cost, and add the hard constraints in the maximum optimization problem as the loss function to the fitness function. The formula used is as follows; ； In the formula, is the fitness function, It is A string representing the mount path, is the cost of movement, is a custom collision violation number; Step C4: Use simulation software to simulate the loading path and test the feasibility of the loading path. If it is feasible, it is output as the optimal loading path. Otherwise, re-planning is performed.

2. The machine vision-based loader auxiliary control system according to claim 1, characterized in that: The target detection module classifies obstacles and loads by a classification method based on semantic segmentation. The classification method based on semantic segmentation specifically includes the following steps: Step A1: collecting a target object image set, wherein the target object image set includes historical target object images collected by an RGB-D camera and labels corresponding to the historical target object images, wherein the labels include obstacles, loads, and people; Step A2: Establish and initialize two semantic segmentation models, use the DeepLabv3+ network as the model architecture, and the pre-trained ResNet model as the feature extractor. The two semantic segmentation models are the obstacle detection model and the load detection model. The obstacle detection model uses the Sigmoid function as the activation function, and the load detection model uses the SoftMax function as the activation function. Step A3: Input the target image set into the semantic segmentation model for training, perform semantic segmentation on the target image, obtain a single-channel 448×448 mask image, use pixel values ​​as segmentation labels to distinguish obstacles, loads, and people, upsample the mask image to match the size of the depth image, and obtain the segmentation result.

3. The machine vision-based loader auxiliary control system according to claim 2, characterized in that: Step B4, using a path planning method combining the DWA algorithm and the IFGM algorithm to plan the moving path, the path planning method combining the DWA algorithm and the IFGM algorithm specifically includes the following steps: Step B41: Mark obstacles and people as obstacles. Assume that the loader and the obstacle are both circular objects. The loader is regarded as a point in Cartesian space. The Pythagorean theorem is used to calculate the actual distance between the loader and the obstacle. The safe distance between the edge of the loader and the obstacle is calculated. The formula used is as follows: ； In the formula, is the safe distance between the loader and the obstacle, is the maximum speed of the loader, is the maximum acceleration of the loader, Indicates time; Step B42: Construct a gap array to calculate the heading angle to guide the loader to move. The heading angle is composed of the gap angle and the target angle. Calculate the utility function of each gap and select a feasible gap for the loader. The formula used is as follows: ； In the formula, is the utility function of each gap, It is The size of the gap, It is The center of the gap, It is the load. and are the weight coefficients of gap size and angle, respectively; Step B43: Use a mathematical formula to represent the movement of the loader moving in a straight line toward the load. When the loader finds an obstacle in the straight path, the loader increases the heading angle and reduces the speed until it avoids the obstacle. The formula used is as follows: ； In the formula, and are the horizontal and vertical coordinates of the loader, The loader is The speed at the moment is the linear speed, The loader is The angular velocity at time, is the heading angle of the loader, ; Step B44: The loader continuously calculates the distance and updates the position to determine the optimal speed of the loader as the objective function of path planning. The formula used is as follows: ； In the formula, It is the optimal speed for the loader.

Images