Simulation method for trajectory tracking of underground hinged type shoveling and transporting equipment

Through the ROS and Gazebo simulation platforms, combined with lidar and kinematic models, the trajectory tracking control of underground articulated shovel equipment is realized, solving the problem of insufficient visual and physical simulation effects of traditional simulation methods and improving the reliability of the simulation system and scalability.

CN119024719APending Publication Date: 2024-11-26UNIV OF SCI & TECH BEIJING
View PDF 0 Cites 3 Cited by

Patent Information

Application Number
CN202411120000.5
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-08-15
Publication Date
2024-11-26

AI Technical Summary

Technical Problem

The traditional simulation methods have poor visual effects, low simulation and unreal physical simulation effects, making it difficult to effectively realize the track tracking control of underground articulated shovel equipment.

Method used

Using ROS and Gazebo simulation platforms, we establish articulated shovel equipment models and real underground tunnel environments. Through lidar perception and kinematic models, combined with pure tracking algorithms for trajectory planning and control to ensure that articulated shovel equipment is in complex environments. Track tracking.

Benefits of technology

It improves the scalability and reliability of the simulation system, provides realistic physical and environmental effects, solves the shortcomings of traditional simulation methods, and achieves more efficient trajectory tracking control.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119024719A_ABST
    Figure CN119024719A_ABST
Patent Text Reader

Abstract

The invention belongs to the technical field of trajectory tracking control of underground hinged type shoveling and transporting equipment, and particularly relates to a trajectory tracking simulation method of the underground hinged type shoveling and transporting equipment, which comprises the following steps: S1, modeling the hinged type shoveling and transporting equipment; s2, planning and deciding a path model of the hinged type shoveling and transporting equipment; s21, acquiring radar information of a wall in the simulation environment; obtaining pose information in kinematics of the hinged type shoveling and transporting equipment model; s22, according to the radar information and the pose information of the wall acquired in the step S21, generating a reference path of a hinged type shoveling and transporting equipment model; and S3, adopting a pure tracking algorithm to plan motion control of the hinged type shoveling and transporting equipment model, and enabling a motion path to be consistent with the reference path. The problems that a traditional simulation mode is poor in simulation visual effect, low in simulation degree and unreal in physical simulation effect are solved, and the reliability of a simulation result can be improved.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the technical field of trajectory tracking control of underground articulated shovel-lifting equipment, and in particular relates to a simulation method for trajectory tracking of underground articulated shovel-lifting equipment. Background Art

[0002] Deep underground mining faces many safety hazards, such as high humidity, high temperature, landslides, water gushing and other harsh conditions, which not only seriously restrict the efficiency of resource mining, but also endanger the safety of underground workers. In addition, labor costs account for a large proportion of mining costs. However, the existing widely used mineral mining equipment has a low level of automation and low production efficiency, which makes it impossible for the underground mining industry to develop further. In order to break through this dilemma, it has become an urgent need to develop unmanned mining of deep resources, and unmanned and integrated intelligence have become the future trend of mineral mining equipment.

[0003] Among the many types of equipment used in mineral mining, articulated shovel loaders are mobile and flexible, and have high mining efficiency, making them an indispensable key piece of equipment in underground deep resource mining operations. Intelligent underground shovel loaders can not only achieve unmanned operation and autonomous driving in underground tunnels, but also improve the efficiency of underground mining with advanced technology and status monitoring. In addition, the unmanned operation of articulated shovel loaders can also reduce costs and further increase mining intensity. The application of this unmanned technology allows mining personnel to stay away from underground tunnel environments that pose a huge threat to their lives and health, which not only ensures the safety of on-site workers and reduces the harm of disasters and accidents to workers, but also improves the efficiency of mining operations and reduces mining costs.

[0004] However, when articulated shovel loaders are driving in underground tunnels, they face challenges brought by underground geological conditions and mineral vein factors, such as uneven and irregular tunnel wall dimensions, uneven wall surfaces, and large local undulations. At the same time, most underground shovel loaders are articulated vehicles, with a front and rear central articulated structure, and the steering hydraulic cylinders arranged on one or both sides of the articulation point are used to push the vehicle body to steer.

[0005] Since articulated shovel-loaders are different from conventional vehicle structures, they have more obvious changes in body posture when turning, that is, the lateral swing amplitude of the vehicle body is significantly greater than that of rigid vehicles. Although flexible steering can be achieved during driving, it will also lead to poor lateral stability. Due to its unique kinematic and dynamic characteristics and its complex working environment, trajectory tracking control has become a difficulty faced by articulated shovel-loaders when achieving autonomous driving. Therefore, it is of great strategic significance to study the trajectory tracking control of underground articulated shovel-loaders. This will not only promote the innovation of mining technology, but also lay a solid foundation for the future intelligent development of mining.

[0006] The trajectory tracking control was studied and debugged using ROS (Robot Operating System) and Gazebo simulation platform. ROS adopts a distributed architecture design with its modular and distributed characteristics, allowing developers to split the different functions of the robot into independent nodes. Each node is responsible for performing a specific task. The node is an executable file that performs different functions, and each node name is unique.

[0007] Gazebo is a powerful robot simulation platform that plays an important role in the ROS ecosystem. It uses a high-performance physics engine for accurate physical simulation, supports physical phenomena such as collision detection, gravity, and friction, and ensures that the robot behavior in the simulation environment is highly similar to the real world. Gazebo can simulate a variety of sensors, such as cameras, lidar, etc., allowing developers to verify and test various robot algorithms in a virtual environment. It can solve the problems of poor simulation visual effects, low fidelity, and unrealistic physical simulation effects in traditional simulation methods. Summary of the invention

[0008] In order to solve the above technical problems, namely, the problems of poor simulation visual effects, low realism and unrealistic physical simulation effects of traditional simulation methods.

[0009] The present invention provides the following technical solutions:

[0010] A simulation method for tracking the trajectory of underground articulated shovel equipment comprises the following steps:

[0011] S1. Modeling of articulated loader equipment; including:

[0012] Establish an articulated loader model under the GAZEBO simulation platform;

[0013] Establish a simulation environment of a real underground tunnel for the operation of an articulated shovel equipment model under the GAZEBO simulation platform;

[0014] Establish the kinematic model of articulated shovel equipment model;

[0015] S2. Path planning decision of articulated loader model: including:

[0016] S21. Select the laser radar in the GAZEBO simulation platform of the ROS system, mount the laser radar on the articulated scraper model, define the parameters of the laser radar to adapt to the simulation environment of the articulated scraper model, and obtain the laser radar information of the wall in the simulation environment; at the same time, obtain the kinematic posture information of the articulated scraper model in kinematics through the kinematic data of the articulated scraper model recorded in the GAZEBO simulation platform;

[0017] S22, performing path planning of the articulated shovel-lifting equipment model according to the laser radar information of the wall obtained in step S21 and the position information of the articulated shovel-lifting equipment model, and generating a reference path for trajectory tracking;

[0018] S3. Trajectory tracking control: A pure tracking algorithm is used to plan the motion control of the articulated shovel-lifting equipment model; the pure tracking algorithm calculates the speed and angle of rotation required by the articulated shovel-lifting equipment model, and sends them to the lower-level controller of the articulated shovel-lifting equipment model to control the motion path of the articulated shovel-lifting equipment model, so that the motion path of the articulated shovel-lifting equipment model is consistent with the reference path.

[0019] Furthermore, in step S1, establishing the articulated shovel equipment model under the GAZEBO simulation platform includes:

[0020] Step S11, defining an articulated scraper equipment model through a URDF model: using URDF to describe an articulated scraper equipment model under the GAZEBO simulation platform, the articulated scraper equipment model includes connecting rods, joints, kinematic / dynamic parameters, visualization and collision detection models;

[0021] Step S12, defining the joint framework of the articulated scraper model: the articulated scraper model is formed by hinged front body and rear body, the hinge point between the front body and the rear body is the hinge point, the joints between the four wheels and the body in the articulated scraper model are defined as continuous rotation joints, the joint between the front body and the hinge point is defined as revolute rotation joint; the remaining joints of the articulated scraper model are defined as fixed joints;

[0022] Step S13: After the joints are defined, physical simulation is performed in Gazebo to drive the movement of the robot, and a simulation of a motor driver is added to the continuous revolute joint and the revolute revolute joint defined in step S12.

[0023] Furthermore, a simulation environment of a real underground tunnel for the operation of an articulated shovel loader model under the GAZEBO simulation platform is established, including: preprocessing the acquired map: screening and simplifying the original underground tunnel map according to the simulation requirements, retaining the main tunnels, geometric shapes, dimensions and directions, intersections, and slope changes in the original underground tunnel map; performing preliminary modeling in solidworks based on the wall length and dimension direction information of the underground tunnel; saving the model in solidworks as an obj format file, then importing the obj format file into Blender to add wall details, and finally converting the file into a dae format and importing it into the GAZEBO simulation platform for simulation.

[0024] Furthermore, the kinematic model is established with the front vehicle body as a reference. The kinematic model of the articulated scraper equipment model is:

[0025]

[0026] (x f ,y f ) is the coordinate of the center point of the front vehicle body, is the speed of the front vehicle in the x-axis direction, is the velocity of the front vehicle in the y-axis direction, θ f is the heading angle of the front vehicle body, γ is the articulation angle, is the angular velocity of the articulation angle, L f Distance from the front body center to the hinge point, L r is the distance from the rear body center to the hinge point, v f is the longitudinal velocity of the front vehicle.

[0027] Furthermore, the state space equation of the kinematic model of the articulated loader model is:

[0028]

[0029] Furthermore, the kinematic constraints of the articulated loader model are: the longitudinal velocity increment Δv of the front vehicle body f , articulation angular velocity The range of articulation angle γ:

[0030]

[0031] Furthermore, in step S22, path planning of the articulated shovel equipment is performed to generate a reference path for trajectory tracking, which specifically includes the following steps:

[0032] 1) Initialization: including reference path initialization and articulated shovel equipment model position initialization;

[0033] 2) Receive LiDAR data;

[0034] 3) Traverse the lidar data;

[0035] 4) According to the symmetry of the laser radar, the laser radar value l on the left side of the wall is obtained z and the right laser radar value l y ;

[0036] 5) Using trigonometric functions, the coordinates of the left and right lane walls in the radar coordinate system are obtained: z and O y ;

[0037] 6) Check the coordinates O on both sides in turn z and Oy Take the midpoint as the discrete path point of the articulated shovel equipment model in the radar coordinate system;

[0038] 7) All discrete path points are added to a set of reference paths consisting of several discrete path points in the order of the laser radar;

[0039] 8) Convert the path points from the radar coordinate system to the global coordinate system;

[0040] 9) Path planning is completed.

[0041] Further, step 8) transforms the path points from the radar coordinate system to the global coordinate system through the transformation frame TF function, that is, the path data in the body coordinate system of the articulated shovel equipment is converted into the path data in the world coordinate system. The specific steps are as follows:

[0042] (1) Initialize the TF listener and broadcaster; the TF listener is used to receive and cache the transformation information between various coordinate systems; the TF broadcaster is used to publish the transformation information from the vehicle body coordinate system to the world coordinate system;

[0043] (2) Publishing the transformation of the vehicle body coordinate system: In each control cycle, publish the transformation of the vehicle body’s current position relative to the world coordinate system;

[0044] (3) Path data coordinate conversion: The transformation from the current vehicle body coordinate system to the world coordinate system is obtained by querying, and the path data is converted from the vehicle body coordinate system to the world coordinate system.

[0045] Furthermore, in step S3, the pure tracking algorithm selects a preview path point in front of the articulated shovel loader model. The position of the preview path point is determined based on the current vehicle speed, steering ability and path tracking accuracy requirements. Then, the optimal arc trajectory from the current position to the point is calculated. The radius of this arc has a trigonometric function relationship with the turning angle of the articulated shovel loader model, and the steering of the articulated shovel loader model is controlled based on this turning angle to achieve tracking of the target path.

[0046] Furthermore, when the trajectory tracking algorithm tracks the reference path, it sends a control quantity to the lower-level speed and torque controller. The Gazebo lower-level controller receives the control quantity sent by the trajectory tracking algorithm and controls the drive wheel speed and articulation angle position.

[0047] Compared with the prior art, the present invention has the following beneficial effects:

[0048] 1. Based on ROS-GAZEBO, the traditional articulated scraper equipment simulation is transformed into a modular open robot system simulation. Under the premise of ensuring the stability and robustness of the original simulation system, the scalability, versatility and code reusability of the simulation system are greatly improved. The distributed nature of the ROS system also provides the possibility of multi-machine simulation of articulated scraper equipment.

[0049] 2. The present invention is not limited to a certain articulated shovel equipment model and sensor model. By configuring the *.yaml file and modifying the parameters of each module, the purpose of simulating a general vehicle and utilizing various sensors can be achieved. The *.yaml file is mainly used to configure the type (control speed / position), torque, friction and related constraints and related PID parameters of each joint controller (lower-level controller).

[0050] 3. Based on ROS-GAZEBO, it provides realistic physical effects, environmental effects and simulation results in more dimensions, solving the problems of poor simulation visual effects, low realism and unrealistic physical simulation effects in traditional simulation methods, and can improve the reliability of simulation results.

[0051] 4. The simulation platform is highly customizable, and users can add or adjust simulation modules as needed. This highly customizable capability enables the platform to adapt to changing technical requirements and application scenarios, maintaining the foresight and flexibility of technology. It can support different types of shovel equipment and operating environments, and meet a variety of simulation test needs. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 It is a simulation schematic diagram of the front vehicle body in the articulated shovel equipment model of the present invention;

[0053] Figure 2 It is a schematic diagram of the simulation of the rear vehicle body in the articulated shovel equipment model of the present invention;

[0054] Figure 3 It is a joint frame diagram of the articulated shovel equipment model of the present invention;

[0055] Figure 4 A preliminary modeling map of the simulation environment of the present invention in solidworks;

[0056] Figure 5 It is a schematic diagram of the kinematic characteristic coordinates of the articulated scraper equipment model of the present invention;

[0057] Figure 6 It is a schematic diagram of the path planning decision process of the articulated shovel equipment model in the GAZEBO simulation test of the present invention;

[0058] Figure 7It is a node framework diagram of the present invention;

[0059] Figure 8 It is a schematic diagram of a scene when the articulated shovel equipment model of the present invention is in a simple road section;

[0060] Fig. 9 A schematic diagram of the planning path and radar data of the present invention;

[0061] Fig.10 A schematic diagram of selecting preview path points in the pure tracking algorithm of the present invention;

[0062] Fig.11 This is a schematic diagram of the pure tracking algorithm of the present invention;

[0063] Fig.12 It is a structural schematic diagram of the lower layer controller of the present invention. DETAILED DESCRIPTION

[0064] The technical solution of the present invention will be clearly described below in conjunction with the accompanying drawings. Obviously, the described embodiments are not all embodiments of the present invention, and all other embodiments obtained by ordinary technicians in the field without making creative work are within the protection scope of the present invention.

[0065] The present invention provides a simulation method for tracking the trajectory of underground articulated shovel equipment, comprising the following steps:

[0066] S1. Modeling of articulated loader equipment; including:

[0067] Establish an articulated loader model under the GAZEBO simulation platform;

[0068] Establish a simulation environment of a real underground tunnel for the operation of an articulated shovel equipment model under the GAZEBO simulation platform;

[0069] Establish the kinematic model of articulated shovel equipment model;

[0070] Based on the physical parameters, mechanical structure and motion characteristics of the actual articulated shovel-loader, a simulation model of the articulated shovel-loader is created on the simulation test platform of the articulated shovel-loader, which matches the actual articulated shovel-loader in key elements such as size, mass distribution, and power system, to ensure the authenticity of its dynamic performance.

[0071] A simulation model of an articulated shovel loader was created on the simulation platform, and a real underground tunnel environment was constructed for the operation of the articulated shovel loader model. The irregular terrain of the tunnel was replicated and its uneven details were simulated, so that the simulation test can be closer to the actual working environment. It is used to test the trajectory tracking effect of the articulated shovel loader in this environment and accurately evaluate its trajectory tracking ability.

[0072] The establishment of the articulated loader model under the GAZEBO simulation platform includes the following steps:

[0073] 1. Define the articulated shovel equipment model through URDF;

[0074] URDF is used to describe the articulated shovel equipment model under the GAZEBO simulation platform, covering the connecting rods, joints, kinematic / dynamic parameters, visualization and collision detection models, etc.

[0075] The robot model in GAZEBO is usually described by URDF (Universal Robot Description Format), which covers links, joints, kinematic / dynamic parameters, visualization and collision detection models, etc.

[0076] There is also xacro, which is the abbreviation of XMLMacros. Xacro is an XML macro language, which is programmable XML. By setting the vehicle model file to the Xacro format, the Xacro file can be converted to a urdf file or used directly in the launch file. Compared with directly writing a urdf file, a more concise and easy-to-read vehicle model file can be achieved, which improves writing efficiency.

[0077] The model under the GAZEBO simulation platform can also be imported from the 3D model of other platforms to complete the simulation of the vehicle model. Use solidworks to build a 3D model, and define the reference position and direction for the joints and connecting rods by adding reference points and coordinate systems. Configure URDF export in SolidWorks, including naming, selecting ranges, specifying reference axes and joint types, and configuring the properties of the connecting rod. After completing the configuration, you can export the URDF model file to achieve simulation and control in ROS, such as Figure 1 , Figure 2 shown.

[0078] 2. Define the joint frame of the articulated shovel equipment model;

[0079] The articulated scraper model is composed of a front body and a rear body, and the point where the front body and the rear body are hinged is the hinge point. The joints between the four wheels and the body of the articulated scraper model, that is, the two joints between the front body and the front wheels, and the two joints between the rear body and the rear wheels, are defined as continuous rotation joints, and the joint between the front body and the hinge point is defined as a revolute rotation joint, and the rotation angle limit is defined according to the kinematic limit; the remaining joints, such as the joint between the hinge point and the rear body, the joint between the front body and the support frame in the articulated scraper, and the joint between the support frame and the laser radar, are all defined as fixed joints.

[0080] After the joints are defined, in order to perform physical simulation in Gazebo and drive the movement of the robot, it is necessary to add a motor driver (actuator) simulation for the continuous revolute joint and revolute revolute joint defined in step S12.

[0081] Specifically,

[0082] After completing the definition of the articulated shovel equipment model, in order to achieve relative movement between the various components, it is necessary to add definitions for the joints. The type of joint determines the relative movement between the links.

[0083] There are six types of joints under ROS, among which,

[0084] Continuous revolute joints allow the connection to rotate infinitely around a single axis;

[0085] Planer joints are different from other types of joints in that they allow the link to perform compound translational and rotational motions in a two-dimensional plane.

[0086] A prismatic sliding joint is a joint that moves along a certain axis and can set its position limits;

[0087] The revolute joint is similar to the continuous revolute joint, but has angle limits, which can define the maximum and minimum rotation angles of the joint;

[0088] Floating joints allow the robot to freely translate and rotate in three-dimensional space. Floating joints are usually composed of one or more freely movable links, which are connected by special mechanisms to achieve full range of motion.

[0089] Fixed joints do not allow any form of movement. It permanently fixes two or more parts of the robot together to form a rigid connection. These joints will remain stationary and will not participate in any form of movement.

[0090] The four wheels of the articulated scraper model are all driving wheels. The articulated scraper model mainly relies on the middle articulation angle to control steering, so the joints between the four wheels and the vehicle body are defined as continuous rotation joints, and the joints between the front vehicle body and the hinge point are defined as revolute rotation joints, and the rotation angle limit is defined according to the kinematic limit. The remaining joints are defined as fixed joints. The joint framework of the articulated scraper model is as follows: Figure 3 shown.

[0091] After the joints are defined, in order to perform physical simulation in Gazebo and drive the robot's motion, it is necessary to add motor driver (actuator) simulation for the joints between the four wheels and the body, and the joints between the front body and the hinge. In Gazebo, motor drivers are usually implemented through controller plug-ins, which can simulate the motor behavior in the actual physical environment, including torque, speed, position control, etc.; for different types of joints, different types of controllers can be configured to meet specific needs.

[0092] Establish a simulation environment of a real underground tunnel for the operation of the articulated shovel equipment model under the Gazebo simulation platform: Construct the Gazebo simulation environment based on the actual underground tunnel map; including:

[0093] When building the Gazebo simulation environment, modeling is performed based on the actual underground tunnel map to ensure the authenticity and effectiveness of the simulation. The acquired map is preprocessed. Since the original underground tunnel map often contains a lot of details and redundant information, details such as the texture, roughness and color of the underground tunnel wall and some facilities that may exist in the underground tunnel, such as cables and lighting facilities, are omitted. Redundant information such as excessive map accuracy does not require centimeter level, and the tiny bumps on the wall are simplified, so some screening and simplification are required according to the simulation requirements. At the same time, key features such as the main tunnel are retained, retaining its basic geometry, size and direction, intersections, slope changes, etc.

[0094] According to the information of the wall length and size of the underground tunnel, preliminary modeling was carried out in SolidWorks, such as Figure 4 As shown in the figure. When adding wall details later, you need to strike a balance between details and simulation requirements. Overly complex details may increase the difficulty of modeling and the amount of simulation calculations, while overly simplified details may not fully reflect the characteristics of the actual environment.

[0095] Save the model in solidworks as obj format, then import this obj format file into Blender to add wall details. First, subdivide the wall surface so that more details can be added. Then select "SculptMode" in the mode bar of Blender, switch to sculpting mode, and use different tools of the Brush brush to sculpt on the wall to make the wall uneven and increase its realism and details. Finally, convert the file into dae format so that it can be imported into the GAZEBO simulation platform for simulation.

[0096] Finally, the *.urdf file of the constructed articulated shovel loader model, the simulation environment model of the real underground tunnel where the articulated shovel loader model runs, and the various functional nodes related to trajectory tracking are integrated into the *.launch file.

[0097] Among them, the launch file (LaunchFile) is a way to start multiple nodes at the same time in ROS. It can also automatically start the ROSMaster node manager and implement various configurations for each node, which greatly facilitates the operation of multiple nodes. The simulation can be started by running the *.launch file.

[0098] Establish the kinematic model of articulated shovel equipment model;

[0099] When applying vehicle models in trajectory tracking control algorithms, one of the most important indicators is real-time performance. The more complex the vehicle model is, the more computing resources and solution time are required. The corresponding trajectory tracking control algorithm has very high real-time requirements, so certain simplifications and approximations are required. Overly complex kinematic models are not the focus of research.

[0100] Therefore, the following reasonable assumptions are needed:

[0101] (1) In trajectory tracking control, the articulated shovel equipment model only performs planar motion in the lateral and longitudinal directions, and ignores vertical motion.

[0102] (2) The articulated shovel-loading equipment model is regarded as a rigid body, and the deformation and influence of the suspension are not considered.

[0103] (3) There is no lateral or longitudinal sliding of the tires on the front and rear axles, that is, there is pure rolling between the tires and the ground.

[0104] (4) The instantaneous turning radius is equal to the road curvature radius, simplifying the steering dynamics.

[0105] (5) The tire is not affected by the self-aligning torque, which simplifies the tire dynamics.

[0106] (6) The rear wheel turning angle is zero, that is, the rear wheels maintain a straight-ahead state.

[0107] In the process of kinematic modeling, the articulated loader model is regarded as two articulated rigid bodies, and the tire mechanical properties of the articulated loader model during movement are ignored.

[0108] Figure 5 is the kinematic characteristics of the articulated shovel-lift equipment model, where XOY is the inertial coordinate system. Table 1 shows the physical meaning of the parameters of the articulated shovel-lift equipment model.

[0109] Table 1 Physical meaning of various parameters of articulated shovel equipment model

[0110]

[0111]

[0112] Ignoring the vertical motion, the motion of the articulated scraper model can be equivalent to the plane motion of two articulated rigid bodies. At the articulation point, there is a relationship:

[0113]

[0114] The center point of the front vehicle body is selected as the reference point of the vehicle, and the control quantity is the longitudinal velocity v of the front vehicle body. f , from the above formula 1 we can get:

[0115]

[0116] Therefore, through equation 1 and equation 2, we can get:

[0117]

[0118] The articulation angle of the articulated scraper model usually has a limited range, -0.79rad≤γ≤0.79rad, at this time cosγ≠0, multiply both sides of formula 3 by cosγ, and we can get:

[0119]

[0120] Formula 4 is simplified to:

[0121]

[0122] according to Figure 5 We can get:

[0123] γ=θ f -θ r Formula 6,

[0124] Derivatives of both sides of equation 6 yield:

[0125]

[0126] Substituting equation 7 into equation 5 yields:

[0127]

[0128] Since the side slip of the tire is ignored, we can get:

[0129]

[0130] According to equations 8 and 9, the kinematic model of the articulated scraper equipment model can be obtained as follows:

[0131]

[0132] The state space equation of the kinematic model of the articulated shovel equipment model can be obtained from the matrix form of formula 10:

[0133]

[0134] in, is the state variable, is the control variable.

[0135] Since articulated shovel loaders mostly work in deep underground mines, they usually travel at a relatively low speed. At the same time, due to their special mechanical structure, various accelerations are also limited. Based on the kinematic model given above, the longitudinal velocity increment Δv of the front vehicle body needs to be calculated. f , angular velocity of the articulation angle The hinge angle γ is constrained.

[0136] Define the longitudinal velocity increment of the front vehicle as Δv f , defined as follows:

[0137]

[0138] Where T1 is the sampling period, is the longitudinal acceleration of the front vehicle body,

[0139] According to actual vehicle data, the maximum longitudinal acceleration of the front vehicle body is About 1.41m / s 2 , when the sampling period is 0.05s, the range of the longitudinal velocity increment of the front vehicle body can be obtained:

[0140] -0.07m / s≤Δv f ≤0.07m / s,

[0141] From the actual vehicle data, we know that the articulation angular velocity of the articulated shovel equipment is about 17° / s, and the range of the articulation angular velocity and articulation angle can be obtained:

[0142]

[0143] -0.73rad≤γ≤0.73rad,

[0144] In summary, the kinematic constraints of the articulated shovel equipment model are:

[0145]

[0146] S2. Path planning decision of articulated shovel-loader model; the path planning decision program for articulated shovel-loader includes a perception module and a planning module.

[0147] like Figure 6 As shown in the ROS-GAZEBO simulation test, the path planning decision-making process of the articulated shovel equipment model is divided into:

[0148] S21. Select the laser radar in the GAZEBO simulation platform of the ROS system. The laser radar is mounted on the articulated shovel equipment model. Define the parameters of the laser radar to adapt to the simulation environment of the articulated shovel equipment model. The perception module is used to obtain and process the laser radar information of the wall in the simulation environment. The perception module includes: 1) Laser radar parameter initialization: set the laser radar scanning range, resolution, scanning speed and other parameters. 2) The laser radar continuously emits laser beams and receives reflected signals, and receives distance and angle information. 3) De-noise and filter the laser radar data to improve data quality.

[0149] At the same time, through the kinematic data of the articulated shovel-lifting equipment model recorded in GAZEBO, the perception module obtains the kinematic posture information of the articulated shovel-lifting equipment model;

[0150] In step S21, the environmental perception technology of the laser radar mainly relies on its core components: the laser transmitter and the receiving system. These two components work together to detect and measure objects and obstacles in the surrounding environment by emitting laser pulses from the laser transmitter and receiving the signals reflected by the receiving system, so as to achieve accurate perception of the target position and its surrounding environment. The working principle of the laser radar mainly includes the time of flight (TOF) principle and the optical triangulation principle, which together constitute the technical basis for the laser radar to perform environmental perception.

[0151] In the GAZEBO simulation platform of the ROS system, there are various sensors to choose from to meet the different needs of various simulations; at the same time, the same type of sensors provide custom interfaces, which can be configured with the most suitable parameters for the current simulation environment according to actual needs.

[0152] For example, if you customize the resolution of the laser radar and adjust the resolution, you can optimize the simulation performance while ensuring the simulation accuracy. Table 2 shows the configuration parameters:

[0153] Table 2 LiDAR configuration parameters

[0154]

[0155]

[0156] In the present invention, the laser radar mainly focuses on trajectory tracking, so the information obtained by the laser radar is two-dimensional plane information, and the three-dimensional depth information is ignored. As shown in Table 3, it is the definition of laser radar data information.

[0157] Table 3 Lidar information interface

[0158] parameter Physical Description Data Types Angle_min Starting scanning angle Float32 angle_max End scanning angle Float32 Time_increment Time interval Float32 Angle_increment Angle interval Float32 Scan_time Scan interval Float32 Range_min Minimum effective distance Float32 range_max Maximum effective distance Float32 Ranges Scan data of one cycle Float32[] Intensities Scan intensity for one cycle Float32[]

[0159] S22, according to the laser radar information and posture information of the wall obtained in step S21, the planning module performs path model planning for the articulated shovel equipment and generates a reference path for trajectory tracking; the nodes are used for communication under the ROS framework to transmit the reference path data to the control module;

[0160] The planning modules include:

[0161] 1) Receive the posture and speed information of the current scraper equipment model and the posture information of the front vehicle body sent by the perception module;

[0162] 2) Based on the received posture information, determine the map section where the articulated shovel equipment model is located, and determine the applicable path planning strategy and reference path in combination with the lidar information;

[0163] 3) Use nodes to communicate under the ROS framework to transmit reference path data to the control module.

[0164] Nodes in ROS are the basic units for building robot applications. They interact through the communication mechanism of ROS to achieve data transmission and collaborative work. Each node is an independent process responsible for performing specific functions or tasks; in the trajectory tracking task, the message publishing and subscription rules include ROSMaster, publishers and subscribers as topic managers. Publishers and subscribers use RPC communication to register information in ROSMaster. ROSMaster matches the registration information of publishers and subscribers, and then sends the corresponding publisher RPC port number to subscribers. Subscribers connect to publishers through RPC port numbers, and then publishers publish TCP port numbers to subscribers. Subscribers use TCP communication to connect to publishers. Subscribers and publishers communicate through TCP to exchange data. Once the publisher and subscriber have established a connection, they can still communicate even if ROSMaster is closed.

[0165] like Figure 7As shown, it is a node framework diagram of the present invention. The / pure_presuit node is responsible for path tracking, receiving the planned path information from the / path_gene node, as well as the current pose and speed information and the pose information of the front vehicle body from the / lhd_pose_and_v_updater node. Subsequently, the / pure_presuit node uses this information to calculate the required speed and turning angle through the trajectory tracking algorithm, and publishes these cmd_vel information to the / cmdvel2gazebo node. This information includes the required speed and turning angle information of the articulated shovel equipment under the tracking trajectory, ensuring that the equipment can accurately travel along the planned path.

[0166] The / path_gene node is responsible for generating a feasible planning path. It receives the lidar information / scan from the scraper and the / tf information in the gazebo coordinate system. / tf records the pose change relationship in each coordinate system and the pose information of the front vehicle after / worldsendTF conversion. Based on the received pose information, the / path_gene node determines the map section where the scraper is located, and determines the applicable path planning strategy in combination with the lidar information. Finally, the / path_gene node publishes the planned path to the / pure_presuit node for the latter to track the trajectory.

[0167] The / cmdvel2gazebo node receives the cmd_vel information from the / pure_presuit node, and then sends the speed and angle information to the four wheels and the articulation angle of the middle articulation point of the lower-level controller according to the kinematic model and steering radius of the articulated scraper model, so as to achieve the position change of the articulated scraper model. Specifically, the / cmdvel2gazebo node splits the instructions and passes them to different controllers.

[0168] In addition, / gazebo / model_states is the model data in the simulation platform, and the / odom_pub node provides position information and motion state data to ensure information synchronization between nodes in the system. These data help the system maintain high-precision control and path tracking in different environments and working conditions. Through the collaborative work of these nodes, the entire system can achieve autonomous navigation and operation in complex environments.

[0169] The / lhd / controller_spawner node is responsible for initializing other necessary nodes and parameters when the system starts, ensuring that the system can quickly enter the working state after startup. In addition, the / robot_state_publisher node publishes joint state information, coordinates the movement of each joint, and optimizes the overall movement effect. The remaining nodes are generated by the system.

[0170] Each module communicates and transfers data through topic and service to realize the simulation of articulated shovel equipment. TCPROS, a message method based on TCP / IP, is used in ROS messages and services. TCP / IP transmission control protocol (TCP, Transmission Control Protocol) is a transmission control protocol called TCP / IP. From the perspective of the Internet protocol layer, it is based on IP (Internet Protocol) and uses the transmission control protocol TCP to ensure data transmission and send / receive in the order of sending. Finally, the whole system realizes efficient path tracking and precise control of the articulated shovel equipment model under various complex working conditions through close cooperation between various nodes. These nodes jointly process sensor data, path planning, control instructions and state feedback to form a closed-loop control system to ensure the stable operation and efficient operation of the articulated shovel equipment model.

[0171] Generally, the road sections where articulated loader equipment travel are very spacious. Using the underground roadway wall as a reference, the articulated loader equipment model can be driven in the center, which can adapt to various changes more quickly and maintain the continuity and efficiency of operation. At the same time, driving in the center is not only more in line with the driving habits of most drivers, but also effectively reduces the risk of collision with the roadway wall. Therefore, when planning the driving path of the articulated loader equipment, priority should be given to setting the path in the center of the roadway.

[0172] When the multi-line laser radar is fixed on the articulated shovel loader model, it will emit multiple laser rays, covering a fan-shaped area, and return to the articulated shovel loader model the distance between each ray and the tunnel wall or obstacle. The laser radar is fixed on the vehicle body, and the angle of each ray does not change when the vehicle moves. The control system receives the distance and angle data measured by the laser radar and can calculate the precise coordinates of the obstacle in the radar coordinate system through an algorithm. These coordinate data provide the articulated shovel loader model with real-time environmental perception information, ensuring safe and efficient operation in complex tunnels.

[0173] The steps of path planning for the bilateral centering strategy are described as follows:

[0174] 1) Initialization: including laser radar parameter initialization, path initialization, and articulated shovel loader model position initialization. It mainly initializes the laser radar, articulated shovel loader model, and various variables of the path, such as the angle increment of the laser radar. The path set is initialized to an empty set first, the position of the articulated shovel loader in the global coordinate system is initialized, and the end condition of the path planning is initialized.

[0175] 2) The laser radar emits laser to obtain laser radar data;

[0176] 3) Traverse the lidar data;

[0177] 4) According to the symmetry of the laser radar, the laser radar value l on the left side of the wall is obtained z and the right laser radar value l y ;

[0178] 5) Using trigonometric functions, the coordinates of the left and right lane walls in the radar coordinate system are obtained: z and O y ;

[0179] 6) Check the coordinates O on both sides in turn z and O y Take the midpoint as the discrete path point of the articulated shovel equipment model in the radar coordinate system;

[0180] 7) All discrete path points are added to a set of reference paths consisting of several discrete path points in the order of the laser radar;

[0181] 8) Convert the path points from the radar coordinate system to the global coordinate system;

[0182] 9) Path planning is completed.

[0183] When the articulated loader model is on a simple road section, it will execute the bilateral centering strategy, plan a straight path, and perform trajectory tracking. Figure 8 At this time, the center path planned according to the laser radar data and all the valid data reflected by the laser radar are as follows: Fig. 9 As shown, the dotted lines on both sides are the wall information obtained by the laser radar, and the continuous line in the middle is the reference path planned based on the wall information.

[0184] In the process of generating the planned path and trajectory tracking, the above generated path is usually expressed in the body coordinate system. However, in order to achieve accurate trajectory tracking control, the path data in the body coordinate system needs to be converted to the path data in the world coordinate system. This conversion ensures that the path information is aligned with the global reference frame, so that the control algorithm can calculate and perform path tracking more accurately.

[0185] In ROS, this conversion process can be achieved through the TF function package. TF (Transformation Framework) is a ROS library used to track the transformation relationship between different coordinate systems. The transformation from the body coordinate system to the world coordinate system is published through the TF function package, which includes the position and attitude of the body coordinate system. The position and attitude changes of the body coordinate system in the world coordinate system are tracked through TF monitoring.

[0186] The specific steps are as follows:

[0187] (1) Initialize the TF listener and broadcaster; the TF listener is used to receive and cache the transformation information between various coordinate systems; the TF broadcaster is used to publish the transformation information from the vehicle body coordinate system to the world coordinate system.

[0188] (2) Publishing the transformation of the vehicle body coordinate system: In each control cycle, publish the transformation of the vehicle body’s current posture (position and orientation) relative to the world coordinate system.

[0189] (3) Path data coordinate conversion: The transformation from the current vehicle body coordinate system to the world coordinate system is obtained by querying, and the path data is converted from the vehicle body coordinate system to the world coordinate system.

[0190] (4) Real-time update: Since the vehicle is moving in real time, the body coordinate system will change at every moment, so this process is repeated in each control cycle to ensure the continuity and accuracy of path tracking.

[0191] Step S3, trajectory tracking control: Through node communication, the control module obtains the reference path of the path planning of the articulated shovel-lifting equipment. The control module obtains the speed and angle information of the articulated shovel-lifting equipment required for tracking the trajectory based on the reference path, and sends these speed and angle information to the lower-level controller in the articulated shovel-lifting equipment model to obtain the articulation angles of the four wheels and the middle articulation point, ensuring that the articulated shovel-lifting equipment can accurately travel along the planned path.

[0192] The control module includes:

[0193] 1) Select a suitable preview distance L d ;

[0194] 2) Calculate the preview path point P on the expected planning path g ;

[0195] 3) According to the preview path point and preview distance, the turning angle is calculated as the control amount;

[0196] 4) Perform path tracking control of the articulated shovel equipment model and convert the calculated angle control amount into the control instruction of the lower-level controller;

[0197] 5) Update the posture information of the articulated shovel equipment model and repeat the cycle.

[0198] According to the reference path planned by the planning module for the articulated shovel-lift equipment model, the control module tracks the reference path of the articulated shovel-lift equipment model through a pure tracking algorithm, and sends it to the lower-level controller in the articulated shovel-lift equipment model to control the motion path, so that the motion path of the articulated shovel-lift equipment model is consistent with the reference path.

[0199] When the articulated loader model obtains the planned path, trajectory tracking control begins. The goal of trajectory tracking control is to make the articulated loader model follow the planned path while meeting the speed and posture requirements, thereby ensuring the accuracy and safety of the operation.

[0200] The PurePursuit algorithm is based on geometric principles for tracking. A preview path point is selected in front of the articulated shovel model. The location of the preview path point is usually determined based on factors such as the current vehicle speed, steering ability, and path tracking accuracy requirements. Then, the optimal arc trajectory from the current position to the point is calculated. The radius and curvature of this arc are directly related to the steering control amount of the robot, and the steering of the robot is controlled accordingly to achieve tracking of the target path.

[0201] Pure tracking algorithms have no strict restrictions on the curvature of the path and the steering ability of the robot, so they are highly adaptable.

[0202] The radius and curvature of the arc are directly related to the steering control of the robot. When the car turns at a fixed angle, there will be an arc trajectory. If we know the radius of the arc trajectory, we can infer how many angles the car needs to turn to drive out of this trajectory. The specific analysis is as follows:

[0203] like Fig.10 As shown, P c Set the preview distance L for the current position of the articulated loader model d . With the current position P c is the center of the circle, L d Draw a circle with radius , and the reference path B of the articulated loader model intersects with the circle. The intersection point is the preview path point P g .

[0204] like Fig.11 As shown, the preview distance L d The angle between the direction and the heading is α, α is the turning angle, according to α and L d The arc path of the articulated shovel equipment model can be obtained, where O is the center of the arc, and R is the radius of the arc (i.e., the turning radius).

[0205] According to geometry, we get the following formula:

[0206]

[0207] Based on the analysis of steering characteristics, the relationship between steering radius and turning angle can be obtained:

[0208]

[0209] C is the wheelbase and L is the wheelbase of the articulated loader model.

[0210] The specific steps of the traditional pure tracking algorithm are as follows:

[0211] 1) Select a suitable preview distance L d ;

[0212] 2) Calculate the preview path point P on the expected planning path g ;

[0213] 3) Calculate the angle control amount according to the preview path point and preview distance;

[0214] 4) Perform path tracking control of the articulated shovel equipment model;

[0215] 5) Update the posture information of the articulated shovel equipment model and repeat the cycle.

[0216] Lower-level controller: When the trajectory tracking algorithm tracks the reference path, it sends a control quantity to the lower-level speed and torque controller. The lower-level controllers in the Gazebo simulation platform are such as drive wheels, articulation angles, etc. Fig.12 A PID-based control strategy is used to perform the actions required by the upper layer. Fig.12 In, k p is the proportionality coefficient, k i is the integration coefficient, k d is the differential coefficient, T wheel is the wheel control amount, T hing is the articulation angle control variable, V is the speed of the articulated shovel-lifting equipment model, θ is the angular velocity of the articulated shovel-lifting equipment model, and s is the transfer function.

[0217] The specific steps to configure the lower-level controller are as follows:

[0218] 1) Configure the gazebo_ros_control plugin in URDF;

[0219] 2) Write for each joint <transmission>Tag, define the transmission device, type SimpleTransmission, and specify the hardware interface "EffortJointInterface" or "VelocityJointInterface" for each joint;

[0220] 3) Write <actuator>Tags, define the same hardware interface for each actuator and specify the mechanical transmission ratio;

[0221] 4) Create a new config.yaml file, write the type of each joint controller in the file and determine each PID parameter by trial and error;

[0222] 5) Load the yaml configuration file in roslaunch and start the controller_manager node to load each joint controller

[0223] The above technical features constitute the best embodiment of the present invention, which has strong adaptability and best implementation effect. Non-essential technical features can be added or reduced according to actual needs to meet the needs of different situations.

[0224] Finally, it should be noted that the above content is only used to illustrate the technical solution of the present invention, rather than to limit the scope of protection of the present invention. Simple modifications or equivalent substitutions of the technical solution of the present invention by ordinary technicians in this field do not deviate from the essence and scope of the technical solution of the present invention.< / actuator> < / transmission>

Claims

1. A simulation method for tracking the trajectory of underground articulated shovel equipment, characterized in that: The steps include: S1. Modeling of articulated shovel equipment: include, Establish an articulated loader model under the GAZEBO simulation platform; Establish a simulation environment of a real underground tunnel for the operation of an articulated shovel equipment model under the GAZEBO simulation platform; Establish the kinematic model of articulated shovel equipment model; S2. Path planning decision of articulated loader model: including: S21. Select the laser radar in the GAZEBO simulation platform of the ROS system, mount the laser radar on the articulated scraper model, define the parameters of the laser radar to adapt to the simulation environment of the articulated scraper model, and obtain the laser radar information of the wall in the simulation environment; at the same time, obtain the kinematic posture information of the articulated scraper model in kinematics through the kinematic data of the articulated scraper model recorded in the GAZEBO simulation platform; S22, performing path planning of the articulated shovel-lifting equipment model according to the laser radar information of the wall obtained in step S21 and the position information of the articulated shovel-lifting equipment model, and generating a reference path for trajectory tracking; S3, trajectory tracking control: using pure tracking algorithm to plan the motion control of the articulated shovel equipment model; The pure tracking algorithm calculates the speed and turning angle required by the articulated loader model and sends them to the lower-level controller of the articulated loader model to control the motion path of the articulated loader model so that the motion path of the articulated loader model is consistent with the reference path.

2. The simulation method according to claim 1, characterized in that: In step S1, the articulated shovel equipment model is established under the GAZEBO simulation platform, including: Step S11, defining an articulated scraper equipment model through a URDF model: using URDF to describe an articulated scraper equipment model under the GAZEBO simulation platform, the articulated scraper equipment model includes connecting rods, joints, kinematic / dynamic parameters, visualization and collision detection models; Step S12, defining the joint framework of the articulated scraper model: the articulated scraper model is formed by hinged front body and rear body, the hinge point between the front body and the rear body is the hinge point, the joints between the four wheels and the body in the articulated scraper model are defined as continuous rotation joints, the joint between the front body and the hinge point is defined as revolute rotation joint; the remaining joints of the articulated scraper model are defined as fixed joints; Step S13: After the joints are defined, physical simulation is performed in Gazebo to drive the movement of the robot, and a simulation of a motor driver is added to the continuous revolute joint and the revolute revolute joint defined in step S12.

3. The simulation method according to claim 1, characterized in that: A simulation environment of a real underground tunnel for the operation of an articulated shovel loader model under the GAZEBO simulation platform is established, including: preprocessing the acquired map: screening and simplifying the original underground tunnel map according to the simulation requirements, retaining the main tunnels, geometric shapes, dimensions and directions, intersections, and slope changes in the original underground tunnel map; performing preliminary modeling in solidworks based on the wall length and dimension direction information of the underground tunnel; saving the model in solidworks as an obj format file, then importing the obj format file into Blender to add wall details, and finally converting the file into dae format and importing it into the GAZEBO simulation platform for simulation.

4. The simulation method according to claim 2, characterized in that: The kinematic model is established with the vehicle body as a reference. The kinematic model of the articulated scraper equipment model is: (x f ,y f ) is the coordinate of the center point of the front vehicle body, is the velocity of the front vehicle in the x-axis direction, is the velocity of the front vehicle in the y-axis direction, θ f is the heading angle of the front vehicle body, γ is the articulation angle, is the angular velocity of the articulation angle, L f Distance from the front body center to the hinge point, L r is the distance from the rear body center to the hinge point, v f is the longitudinal velocity of the front vehicle.

5. The simulation method according to claim 4, characterized in that: The state space equations of the kinematic model of the articulated loader model:

6. The simulation method according to claim 5, characterized in that: The kinematic constraints of the articulated loader model are: the longitudinal velocity increment Δv of the front vehicle body f , articulation angular velocity The range of articulation angle γ:

7. The simulation method according to claim 1, characterized in that: In step S22, path planning of the articulated shovel equipment is performed to generate a reference path for trajectory tracking, which specifically includes the following steps: 1) Initialization: including reference path initialization and articulated shovel equipment model position initialization; 2) Receive LiDAR data; 3) Traverse the lidar data; 4) According to the symmetry of the laser radar, the laser radar value l on the left side of the wall is obtained z and the right laser radar value l y ; 5) Using trigonometric functions, the coordinates of the left and right lane walls in the radar coordinate system are obtained: z and O y ; 6) Check the coordinates O on both sides in turn z and O y Take the midpoint as the discrete path point of the articulated shovel equipment model in the radar coordinate system; 7) All discrete path points are added to a set of reference paths consisting of several discrete path points in the order of the laser radar; 8) Convert the path points from the radar coordinate system to the global coordinate system; 9) Path planning is completed.

8. The simulation method according to claim 7, characterized in that: Step 8) By using the transformation frame TF function, the path points are converted from the radar coordinate system to the global coordinate system, that is, the path data in the body coordinate system of the articulated loader model is converted to the path data in the world coordinate system. The specific steps are as follows: (1) Initialize the TF listener and broadcaster; the TF listener is used to receive and cache the transformation information between various coordinate systems; the TF broadcaster is used to publish the transformation information from the vehicle body coordinate system to the world coordinate system; (2) Publishing the transformation of the vehicle body coordinate system: In each control cycle, publish the transformation of the vehicle body’s current position relative to the world coordinate system; (3) Path data coordinate conversion: The transformation from the current vehicle body coordinate system to the world coordinate system is obtained by querying, and the path data is converted from the vehicle body coordinate system to the world coordinate system.

9. The simulation method according to claim 1, characterized in that: In step S3, the pure tracking algorithm selects a preview path point in front of the articulated shovel loader model. The position of the preview path point is determined based on the current vehicle speed, steering ability and path tracking accuracy requirements. Then, the optimal arc trajectory from the current position to the point is calculated. The radius of this arc has a trigonometric function relationship with the turning angle of the articulated shovel loader model, and the steering of the articulated shovel loader model is controlled based on this turning angle to achieve tracking of the target path.

10. The simulation method according to claim 9, characterized in that: When the trajectory tracking algorithm tracks the reference path, it sends a control variable to the lower-level speed and torque controller. The Gazebo lower-level controller receives the control variable sent by the trajectory tracking algorithm and controls the drive wheel speed and articulation angle position.

Citation Information

Cited By

  • Mining articulated vehicle path tracking method and system based on linear model prediction

    CN120909281A

  • Carry-scraper roadway collision detection and control method, device, equipment and medium

    CN122049063A

  • Shovel lane collision detection and control method, device, equipment and medium

    CN122049063B