Track learning and tracking control method and device for mining robot

Abstract

The invention provides a mining robot trajectory learning and tracking control method and device, and relates to intelligent robot control. The method comprises the following steps: in response to a trajectory planning operation on the mining robot, acquiring sensor acquisition data corresponding to the mining robot; taking the terrain point cloud data of the to-be-mined area and the mining trajectory data as input features, and constructing a trajectory planning model; taking the inclination angle data as an input feature, and constructing a stress analysis model; outputting a predicted excavation track corresponding to the excavation robot according to the track planning model, and outputting a predicted external force borne by the excavation robot according to the stress analysis model; and the predicted excavation track and the predicted external force serve as input, and the excavation robot is controlled through a track tracking controller. The problem that a traditional digging robot based on rules executes digging actions according to a fixed track and path and is difficult to adapt to different digging terrains is solved.

Description

Technical Field

[0001] The present application relates to intelligent robot control, and in particular to a mining robot trajectory learning and tracking control method and device. Background Art

[0002] As the most widely used heavy machinery on construction sites, excavators play an important role in many fields such as earthwork, mining, infrastructure construction, etc. In order to improve construction efficiency and achieve uninterrupted operation, the automation or robotization of excavators has become a research focus.

[0003] Traditional rule-based excavation robots achieve uninterrupted operation by performing excavation actions according to fixed trajectories and paths. However, since the above motion trajectories are simple and fixed, the influence of soil interaction dynamics is not considered, that is, there is a problem of difficulty in adapting to different excavation terrains.

[0004] Therefore, there is an urgent need for a mining robot trajectory learning and tracking control method and device. Summary of the invention

[0005] The present application provides a method and device for trajectory learning and tracking control of an excavation robot, which solves the problem that a traditional rule-based excavation robot performs excavation actions according to a fixed trajectory and path and is difficult to adapt to different excavation terrains.

[0006] In the first aspect of the present application, a method for trajectory learning and tracking control of an excavation robot is provided, the method comprising: in response to a trajectory planning operation on the excavation robot, obtaining sensor acquisition data corresponding to the excavation robot, the sensor acquisition data including terrain point cloud data, inclination data and excavation trajectory data of the area to be excavated; using the terrain point cloud data and the excavation trajectory data of the area to be excavated as input features, and constructing a trajectory planning model; using the inclination data as input features, and constructing a force analysis model; outputting a predicted excavation trajectory corresponding to the excavation robot according to the trajectory planning model, and outputting a predicted external force to which the excavation robot is subjected according to the force analysis model; using the predicted excavation trajectory and the predicted external force as input, and controlling the excavation robot through a trajectory tracking controller.

[0007] Optionally, sensor acquisition data corresponding to the excavation robot is obtained, specifically including: obtaining terrain point cloud data of the area to be excavated through a first sensor device, the first sensor device includes a lidar device and a depth camera device; obtaining inclination data through a second sensor device, the second sensor device includes an inclination sensor device and an inertial measurement unit sensor device; obtaining geometric parameters of the mechanical arm of the excavation robot through a third sensor, and calculating the excavation trajectory data under preset excavation operation conditions through the mechanical arm geometric parameter data and the inclination data, the preset excavation operation conditions being to satisfy the requirement that the rotation angle of the excavation robot remains unchanged during the excavation operation.

[0008] Optionally, the excavation trajectory data is calculated under preset excavation operation conditions through the mechanical arm geometric parameter data and the inclination angle data, specifically including: calculating the bucket end coordinates of the excavation robot according to the following formula:

[0009]

[0010] in, represents the bucket end coordinates of the excavator robot, L1, L2, and L3 are all mechanical arm geometric parameter data, and L1 is the connecting rod length data of the excavator robot's boom, L2 is the connecting rod length data of the excavator robot's bucket, and L3 is the connecting rod length data of the excavator robot's bucket. arm ,θ boom ,θ bucket All are inclination data, and θ arm is the joint angle data between the excavator arm and the base, θ boom is the joint angle data of the digging robot arm and boom, θ bucket is the joint angle data of the bucket and the dipper arm of the excavator robot; according to the bucket end coordinates, the bucket end trajectory of the excavator robot in the target time period is constructed by the following formula:

[0011] y demo =[y0,y1,…,y g ];

[0012] Among them, y demo is the bucket end trajectory, y0, y1, ..., y g They represent the bucket end coordinates corresponding to g+1 different moments in the target time period, and satisfy The bucket end trajectory is used as the excavation trajectory data.

[0013] Optionally, the terrain point cloud data of the area to be mined and the mining trajectory data are used as input features, and a trajectory planning model is constructed, specifically including: obtaining slope information data and concavity data according to the terrain point cloud data of the area to be mined; using the slope information data and concavity data, using the dynamic motion primitive method to learn the mining trajectory data, and constructing a trajectory planning model.

[0014] Optionally, the inclination data is used as an input feature and a force analysis model is constructed, specifically including: performing Kalman filtering on the inclination data to calculate the joint angular velocity data and joint angular acceleration data corresponding to the mining robot; and constructing a force analysis model based on the joint angular velocity data and the joint angular acceleration data.

[0015] Optionally, constructing a force analysis model according to the joint angular velocity data and the joint angular acceleration data specifically includes: constructing the force analysis model by the following formula:

[0016]

[0017] Where M(θ)∈R 3 is the positive definite inertia matrix, R is a real number set, θ is the inclination data, represents the centrifugal force vector and the Coriolis force vector, is the first-order derivative of the inclination data, is the second-order derivative of the inclination data, g(θ)∈R 3 is the gravity vector, F s ∈R 3×3 is the Coulomb matrix, B∈R 3×3 is the viscous friction matrix, τ u ∈R 3 is the driving torque of the excavator robot, τ ext ∈R 3 is the external torque on the excavation robot; the external torque on the excavation robot is calculated by the disturbance observer:

[0018]

[0019] Among them, τ ext is the external torque on the excavator robot, p(t) is the generalized momentum at time t, (0) is the generalized momentum at time 0, τ μ To determine the friction force on the joints of the mining robot, T is the geometric characteristics of the terrain in the area to be excavated, and K0 is used to adjust the dynamic response speed of the disturbance observer.

[0020] Optionally, controlling the excavation robot by a trajectory tracking controller specifically includes: controlling the excavation robot according to the following formula:

[0021] ;

[0022] Where u(k) is the control input of the trajectory tracking controller at time k, Δu(k) is the increment of the control input of the trajectory tracking controller at time k, and τ ext is the external torque on the excavator robot, k f is τ ext The feedback gain, e(k) represents the error between the first joint angular velocity and the second joint angular velocity at time k, the first joint angular velocity is the expected joint angular velocity, and the second joint angular velocity is the joint angular velocity estimated by Kalman filtering, K p , K i , K d They are the proportional, differential and integral coefficients of the PID controller respectively.

[0023] In a second aspect of the present application, a mining robot trajectory learning and tracking control device is provided, the device comprising an acquisition module, a processing module, a generation module and a control module, wherein:

[0024] The acquisition module is used to acquire sensor acquisition data corresponding to the excavation robot in response to the trajectory planning operation of the excavation robot, and the sensor acquisition data includes terrain point cloud data, inclination data and excavation trajectory data of the area to be excavated.

[0025] The modeling module is used to take the terrain point cloud data of the area to be excavated and the excavation trajectory data as input features and build a trajectory planning model; take the inclination data as input features and build a force analysis model.

[0026] The generation module is used to output the predicted excavation trajectory corresponding to the excavation robot according to the trajectory planning model, and output the predicted external force received by the excavation robot according to the force analysis model.

[0027] The control module is used to take the predicted excavation trajectory and the predicted external force as input and control the excavation robot through a trajectory tracking controller.

[0028] In the third aspect of the present application, an electronic device is provided, including a processor, a memory, a user interface and a network interface, the memory is used to store instructions, the user interface and the network interface are used to communicate with other devices, and the processor is used to execute the instructions stored in the memory so that the electronic device executes any one of the methods described above.

[0029] In a fourth aspect of the present application, a computer-readable storage medium is provided, wherein the computer-readable storage medium stores a computer program, and the computer program is executed by a processor to perform any of the above methods.

[0030] One or more technical solutions provided in the embodiments of the present application have at least the following technical effects or advantages:

[0031] 1. When performing trajectory planning operations on the excavation robot, the terrain point cloud data, inclination data and excavation trajectory data of the area to be excavated corresponding to the excavation robot are obtained, and the terrain point cloud data and excavation trajectory data of the area to be excavated are used as input features, and a trajectory planning model is constructed. The inclination data is used as input features, and a force analysis model is constructed, so as to output the predicted excavation trajectory corresponding to the excavation robot according to the trajectory planning model, and output the predicted external force on the excavation robot according to the force analysis model, so as to take the predicted excavation trajectory and the predicted external force as input, and control the excavation robot through a trajectory tracking controller, thereby realizing efficient control of the excavation robot under the action of complex soil, and solving the problem that traditional rule-based excavation robots perform excavation actions according to fixed trajectories and paths, and are difficult to adapt to different excavation terrains.

[0032] 2. Obtain slope information data and concavity data based on the terrain point cloud data of the area to be mined; use the slope information data and concavity data to learn the mining trajectory data using the dynamic motion primitive method, and build a trajectory planning model, thereby achieving accurate adaptation to complex terrain and optimizing path planning.

[0033] 3. Use the inclination angle data as input features and construct a force analysis model, specifically including: performing Kalman filtering on the inclination angle data to calculate the joint angular velocity data and joint angular acceleration data corresponding to the excavation robot; constructing a force analysis model based on the joint angular velocity data and joint angular acceleration data, so as to accurately calculate the external torque and internal force of the excavation robot under different working conditions, and ensure stability and safety during the excavation process. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 It is a flow chart of a method for trajectory learning and tracking control of an excavating robot provided in an embodiment of the present application;

[0035] Figure 2 It is a schematic diagram of an excavation robot structure and coordinate system configuration provided by an embodiment of the present application;

[0036] Figure 3 It is a module schematic diagram of a trajectory learning and tracking control device for an excavating robot provided in an embodiment of the present application;

[0037] Figure 4 It is a structural schematic diagram of an electronic device provided in an embodiment of the present application.

[0038] Explanation of the reference numerals: 31, acquisition module; 32, modeling module; 33, generation module; 34, control module; 401, processor; 402, communication bus; 403, user interface; 404, network interface; 405, memory. DETAILED DESCRIPTION

[0039] In order to enable technicians in this field to better understand the technical solutions in this specification, the technical solutions in the embodiments of this specification will be clearly and completely described below in conjunction with the drawings in the embodiments of this specification. Obviously, the described embodiments are only part of the embodiments of this application, not all of the embodiments.

[0040] The terms used in the following embodiments of the present application are only for the purpose of describing specific embodiments, and are not intended to be used as limitations to the present application. As used in the specification of the present application, the singular expressions "one", "a kind of", "said", "above", "the" and "this" are intended to also include plural expressions, unless there is a clear indication to the contrary in the context. It should also be understood that the term "and / or" used in the present application refers to and includes any or all possible combinations of one or more listed items.

[0041] In the following, the terms "first" and "second" are used for descriptive purposes only and are not to be understood as suggesting or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features, and in the description of the embodiments of the present application, unless otherwise specified, "plurality" means two or more.

[0042] In order to enable those skilled in the art to better understand the technical solution of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings.

[0043] Please refer to Figure 1 , which shows a flow chart of a mining robot trajectory learning and tracking control method provided in an embodiment of the present application, and the flow chart mainly includes the following steps: S101 to S105.

[0044] Step S101, in response to the trajectory planning operation of the mining robot, obtaining sensor collection data corresponding to the mining robot, wherein the sensor collection data includes terrain point cloud data, inclination data and mining trajectory data of the area to be mined.

[0045] Specifically, when the excavation robot performs excavation operations in the target area, the excavation trajectory of the excavator is tracked in real time and its excavation trajectory is planned. The target area refers to a specific geographical location or working area where the excavation task is scheduled to be carried out, which includes all places where the excavation operation needs to be performed. At this time, the sensor acquisition data corresponding to the excavation robot is obtained through a variety of different sensors. The sensor acquisition data includes but is not limited to: terrain point cloud data, inclination data, excavation trajectory data, etc. of the area to be excavated. The area to be excavated is the actual working area in the target area, which usually refers to the specific terrain or area that has been determined and is ready to be excavated.

[0046] In a possible implementation, step S101 also includes: acquiring terrain point cloud data of the area to be excavated through a first sensor device, the first sensor device including a lidar device and a depth camera device; acquiring the inclination data through a second sensor device, the second sensor device including an inclination sensor device and an inertial measurement unit sensor device; acquiring the geometric parameters of the mechanical arm of the excavation robot through a third sensor, and calculating the excavation trajectory data under preset excavation operation conditions through the mechanical arm geometric parameter data and the inclination data, the preset excavation operation conditions being to satisfy the requirement that the rotation angle of the excavation robot remains unchanged during the excavation operation.

[0047] Specifically, the terrain point cloud data of the area to be excavated can be obtained by a laser radar device and a depth camera device, and the specific steps are as follows: Assuming that the terrain of the area to be excavated is an inclined terrain with irregular pits, the point cloud data is flattened in the Z-axis direction, and the following fitting operation is performed in the XY plane, and the slope information of the terrain is extracted by the following formula:

[0048] y(x)=ax+b-aW(x1-x)(x-x1)-aW(x-x2)(x2-x);

[0049] Among them, y(x) is the height information of the point cloud data in the x coordinate system, x1 and x2 represent the starting point and end point of the terrain slope, a and b represent the slope and offset of the terrain, respectively; W(x) is a step function, which is used to characterize whether the x coordinate of the point cloud data is within the slope range, and W(x) satisfies the following formula:

[0050]

[0051] The inclination data can be obtained through an inclination sensor device and an inertial measurement unit sensor device. Assuming that θ is the inclination data, then Among them, θ arm is the joint angle data between the excavator arm and the base, θ boom is the joint angle data of the digging robot arm and boom, θbucket is the joint angle data of the bucket and the dipper arm of the excavator robot, Represents θ as a three-dimensional vector.

[0052] The geometric parameters of the mechanical arm of the excavating robot can be obtained through the third sensor. The excavation trajectory data can be calculated under the preset excavation operation conditions through the geometric parameter data of the mechanical arm and the inclination data. The preset excavation operation conditions are to satisfy the fact that the rotation angle of the excavating robot remains unchanged during the excavation operation. The specific calculation method of the excavation trajectory data is as follows: First, the bucket end coordinates of the excavating robot in Cartesian space are obtained through the following formula:

[0053] The bucket end coordinates of the excavator robot are calculated according to the following formula:

[0054]

[0055] in, represents the bucket end coordinates of the excavator robot, L1, L2, and L3 are all the mechanical arm geometric parameter data, and L1 is the connecting rod length data of the excavator robot arm, L2 is the connecting rod length data of the excavator robot bucket arm, and L3 is the connecting rod length data of the excavator robot bucket. Then, according to the bucket end coordinates, the bucket end trajectory of the excavator robot in the target time period is constructed by the following formula:

[0056] y demo =[y0,y1,…,y g ];

[0057] Among them, y demo is the bucket end trajectory, y0, y1, ..., y g Respectively represent the bucket end coordinates corresponding to g+1 different moments in the target time period, and satisfy The bucket end trajectory is used as the excavation trajectory data, please refer to Figure 2 , which shows a schematic diagram of an excavation robot structure and coordinate system configuration provided in an embodiment of the present application.

[0058] Step S102: taking the terrain point cloud data of the area to be mined and the mining trajectory data as input features, and constructing a trajectory planning model.

[0059] Specifically, the slope information data and the concavity data are obtained according to the terrain point cloud data of the area to be excavated; the excavation trajectory data is learned by the dynamic motion primitive method through the slope information data and the concavity data, and the trajectory planning model is constructed. For the concavity data, the Gaussian function can be used to fit the concavity of the terrain:

[0060]

[0061] Among them, d represents the depth parameter, x0 represents the coordinates of the deepest point in the area to be excavated; further, according to the terrain slope parameter, c is the parameter that controls the depth influence range, which affects the distribution form of the depth change in the area to be excavated. Further, according to the terrain slope parameter, the tangent angle of the slope can be obtained:

[0062] θ slope =tan -1 a;

[0063] Among them, a is the terrain slope parameter, which is used to represent the vertical height change per unit horizontal distance, θ slope is the tangent angle of the slope, then assuming that the geometric characteristics of the terrain in the area to be excavated, that is, the concave-convex degree data is T, then T=(θ slope , d).

[0064] Based on the mining trajectory data obtained in step S101, [y0, y1, ..., y g ], using the dynamic motion primitive method to learn the mining trajectory data and build a trajectory planning model:

[0065]

[0066] in, represents a canonical system used to construct a time-independent quantity s, τ is the scaling factor, α x is the constant of the canonical system; represents a second-order system with nonlinear functions, which is used to construct the expert demonstration trajectory. y represents the bucket end trajectory, which is defined in the same way as y demo same, and are the first and second derivatives of y, respectively, α y and β y are all constants, f(s) is a nonlinear function used to adjust the end point and trajectory shape of the expert demonstration trajectory, and satisfies (s) = h(s)(y g -y0)s, h(s) is the expression of normalized weighted superposition of multiple nonlinear basis functions, and satisfies Ψ i (s) is the Gaussian basis function, and the specific Gaussian basis function expression is: i represents the i-th nonlinear function, ω i is the weight parameter corresponding to the basis function, N represents the number of basis functions, σ i and c i Represent the width and center position of the basis function respectively.

[0067] The formula ω can be determined by the local weighted regression method i , first construct the objective function, assuming that the objective function is f target , which is expressed as follows:

[0068]

[0069] Further combine the above objective function and Gaussian basis function to construct the loss function. Assume that the loss function is J i , which is expressed as follows:

[0070]

[0071] Among them, P represents the total time steps of the expert demonstration trajectory, t is each discrete time point in the trajectory planning process, and further, the optimization method is used to solve So we get ω i The expression is as follows:

[0072]

[0073] in, T is the geometric features of the terrain in the area to be excavated, that is, the concavity data.

[0074] Step S103: using the inclination angle data as input features and constructing a force analysis model.

[0075] Specifically, the inclination angle data is subjected to Kalman filtering to calculate the joint angular velocity data and joint angular acceleration data corresponding to the excavator robot; and the force analysis model is constructed according to the joint angular velocity data and the joint angular acceleration data. The joint angular velocity data and the joint angular acceleration data are calculated by Kalman filtering. For convenience, a joint angle θ is taken as an example, the joint angular velocity is ω, the joint angular acceleration is α, and the state vector is assumed to be x, so Then, the state estimation model can be constructed by the following formula, which is used to accurately estimate the motion state of the excavator robot:

[0076] x k+1 =F·x k +Gw k ;

[0077] Among them, x k is the state vector at time k, w k is the process noise, F is the state transfer matrix, assuming the sampling period is dt, then G is the noise impact matrix, and The corresponding observation equation can be expressed by the following formula:

[0078] z k =H·x k +v k ;

[0079] Among them, z k is the observed value at the kth moment, v k is the measurement noise at the kth moment, H is the observation matrix, and H = [1 00].

[0080] The steps of Kalman filtering are as follows:

[0081] S1. Predict the next state using the following formula:

[0082]

[0083] in, is the state prediction value at the kth moment, is the prior state prediction value at the k+1th moment.

[0084] S2. The predicted state covariance matrix is:

[0085] P k+1|k =F·P k ·F T +Q;

[0086] Among them, P k is the state covariance matrix at the kth moment, P k+1|k is the state covariance matrix at the k+1th moment, Q is the process noise covariance matrix, and T is the geometric features of the terrain of the area to be mined in step S012, that is, the concavity data.

[0087] S3. Update Kalman gain:

[0088] K k =P k+1|k ·H T ·(H·P k+1|k ·H T +R) -1 ;

[0089] Among them, K k is the Kalman gain at the kth moment, and R is the covariance matrix of the measurement noise, which is usually set according to the accuracy of the sensor.

[0090] S4. Update the next moment status:

[0091]

[0092] Among them, z k+1 is the observed value at the k+1th moment.

[0093] S5. Update the state covariance matrix:

[0094] P k+1 =(IK k ·H)·P k+1|k

[0095] Wherein, I is the unit matrix. The force analysis model is constructed by the following formula:

[0096]

[0097] Where M(θ)∈R 3 is the positive definite inertia matrix, R is a real number set, θ is the inclination data, represents the centrifugal force vector and the Coriolis force vector, is the first-order derivative of the inclination data, is the second-order derivative of the inclination data, g(θ)∈R 3 is the gravity vector, F s ∈R 3×3 is the Coulomb matrix, B∈R 3×3 is the viscous friction matrix, τ u ∈R 3 is the driving torque of the excavator robot, which can be measured by a hydraulic pressure sensor, τ ext ∈R 3 is the external torque applied to the excavating robot.

[0098] Based on the force analysis model, a momentum-based disturbance observer is proposed to estimate the external torque acting on the excavator robot:

[0099]

[0100] Among them, τ ext is the external torque on the excavator robot, p(t) is the generalized momentum at time t, (0) is the generalized momentum at time 0, τ μ is the friction force on the joints of the excavating robot, and T is the geometric feature of the terrain of the area to be excavated, and K0 is used to adjust the dynamic response speed of the disturbance observer.

[0101] Step S104: outputting the predicted excavation trajectory corresponding to the excavation robot according to the trajectory planning model, and outputting the predicted external force received by the excavation robot according to the force analysis model.

[0102] Specifically, the predicted excavation trajectory corresponding to the excavation robot is output according to the trajectory planning model constructed in step S102, and the predicted external force received by the excavation robot is output according to the force analysis model constructed in step S103.

[0103] Step S105 , taking the predicted excavation trajectory and the predicted external force as input, and controlling the excavation robot through a trajectory tracking controller.

[0104] Specifically, the excavation trajectory in the Cartesian space is first converted into the joint angular velocity in the joint space through the inverse kinematics formula, and the incremental PID controller is used to track the joint angular velocity of the excavator robot. Based on the external torque calculated in step S103, it is used as the feedforward item of the PID controller to compensate for the external torque exerted on the excavator, so as to control the excavator robot according to the following formula:

[0105]

[0106] Wherein, u(k) is the control input of the trajectory tracking controller at time k, Δu(k) is the increment of the control input of the trajectory tracking controller at time k, τ ext is the external torque on the excavator robot, k f is τ ext The feedback gain, e(k) represents the error between the first joint angular velocity and the second joint angular velocity at time k, the first joint angular velocity is the expected joint angular velocity, and the second joint angular velocity is the joint angular velocity estimated by Kalman filtering, K p , K i , K d They are the proportional, differential and integral coefficients of the PID controller respectively.

[0107] This application adopts the above method.

[0108] Please refer to Figure 3 , which shows a module schematic diagram of a mining robot trajectory learning and tracking control device provided by an embodiment of the present application, the device includes an acquisition module 31, a processing module 32, a generation module 33 and a control module 34, wherein,

[0109] The acquisition module 31 is used to acquire sensor data corresponding to the excavation robot in response to the trajectory planning operation of the excavation robot, and the sensor data includes terrain point cloud data, inclination data and excavation trajectory data of the area to be excavated.

[0110] The modeling module 32 is used to take the terrain point cloud data of the area to be excavated and the excavation trajectory data as input features and build a trajectory planning model; take the inclination data as input features and build a force analysis model;

[0111] A generation module 33 is used to output a predicted excavation trajectory corresponding to the excavation robot according to the trajectory planning model, and to output a predicted external force on the excavation robot according to the force analysis model;

[0112] The control module 34 is used to take the predicted excavation trajectory and the predicted external force as input and control the excavation robot through a trajectory tracking controller.

[0113] In a possible implementation, the acquisition module 31 is used to acquire sensor data corresponding to the excavation robot, specifically including: acquiring terrain point cloud data of the area to be excavated through a first sensor device, the first sensor device includes a lidar device and a depth camera device; acquiring inclination data through a second sensor device, the second sensor device includes an inclination sensor device and an inertial measurement unit sensor device; acquiring the geometric parameters of the mechanical arm of the excavation robot through a third sensor, and calculating the excavation trajectory data under preset excavation operation conditions through the mechanical arm geometric parameter data and the inclination data, the preset excavation operation conditions being to satisfy the excavation robot's rotation angle remains unchanged during the excavation operation.

[0114] In a possible implementation, the acquisition module 31 is used to calculate the excavation trajectory data under preset excavation operation conditions through the mechanical arm geometric parameter data and the inclination angle data, specifically including: calculating the bucket end coordinates of the excavation robot according to the following formula:

[0115]

[0116] in, represents the bucket end coordinates of the excavator robot, L1, L2, and L3 are all mechanical arm geometric parameter data, and L1 is the connecting rod length data of the excavator robot's boom, L2 is the connecting rod length data of the excavator robot's bucket, and L3 is the connecting rod length data of the excavator robot's bucket. arm ,θ boom ,θ bucket All are inclination data, and θ arm is the joint angle data between the excavator arm and the base, θ boom is the joint angle data of the digging robot arm and boom, θ bucket is the joint angle data of the bucket and the dipper arm of the excavator robot; according to the bucket end coordinates, the bucket end trajectory of the excavator robot in the target time period is constructed by the following formula:

[0117] y demo =[y0,y1,…,y g ];

[0118] Among them, y demo is the bucket end trajectory, y0, y1, ..., yg They represent the bucket end coordinates corresponding to g+1 different moments in the target time period, and satisfy The bucket end trajectory is used as the excavation trajectory data.

[0119] In one possible implementation, the modeling module 32 is used to use the terrain point cloud data of the area to be mined and the mining trajectory data as input features and construct a trajectory planning model, specifically including: obtaining slope information data and concavity data based on the terrain point cloud data of the area to be mined; using the slope information data and concavity data, using the dynamic motion primitive method to learn the mining trajectory data, and constructing a trajectory planning model.

[0120] In one possible implementation, the modeling module 32 is used to use the inclination data as an input feature and construct a force analysis model, specifically including: performing Kalman filtering on the inclination data to calculate the joint angular velocity data and joint angular acceleration data corresponding to the excavation robot; and constructing a force analysis model based on the joint angular velocity data and the joint angular acceleration data.

[0121] In a possible implementation, the modeling module 32 is used to construct a force analysis model according to the joint angular velocity data and the joint angular acceleration data, specifically including: constructing the force analysis model by the following formula:

[0122]

[0123] Where M(θ)∈R 3 is the positive definite inertia matrix, R is a real number set, θ is the inclination data, represents the centrifugal force vector and the Coriolis force vector, is the first-order derivative of the inclination data, is the second-order derivative of the inclination data, g(θ)∈R 3 is the gravity vector, F s ∈R 3×3 is the Coulomb matrix, B∈R 3×3 is the viscous friction matrix, τ u ∈R 3 is the driving torque of the excavator robot, τ ext ∈R 3 is the external torque on the excavation robot; the external torque on the excavation robot is calculated by the disturbance observer:

[0124]

[0125] Among them, τ ext is the external torque on the excavator robot, p(t) is the generalized momentum at time t, (0) is the generalized momentum at time 0, τ μTo determine the friction force on the joints of the mining robot, T is the geometric characteristics of the terrain in the area to be excavated, and K0 is used to adjust the dynamic response speed of the disturbance observer.

[0126] In a possible implementation, the control module 34 is used to control the excavation robot through a trajectory tracking controller, specifically including: controlling the excavation robot according to the following formula:

[0127] ;

[0128] Where u(k) is the control input of the trajectory tracking controller at time k, Δu(k) is the increment of the control input of the trajectory tracking controller at time k, and τ ext is the external torque on the excavator robot, k f is τ ext The feedback gain, e(k) represents the error between the first joint angular velocity and the second joint angular velocity at time k, the first joint angular velocity is the expected joint angular velocity, and the second joint angular velocity is the joint angular velocity estimated by Kalman filtering, K p , K i , K d They are the proportional, differential and integral coefficients of the PID controller respectively.

[0129] It should be noted that: when the device provided in the above embodiment realizes its function, only the division of the above functional modules is used as an example. In actual application, the above functions can be assigned to different functional modules as needed, that is, the internal structure of the device is divided into different functional modules to complete all or part of the functions described above. In addition, the device and method embodiments provided in the above embodiment belong to the same concept, and the specific implementation process is detailed in the method embodiment, which will not be repeated here.

[0130] The present application also provides an electronic device. Figure 4 , Figure 4 4 is a schematic diagram of the structure of an electronic device provided in an embodiment of the present application. The electronic device may include: at least one processor 401, at least one communication bus 402, a user interface 403, at least one network interface 404, and a memory 405.

[0131] The communication bus 402 is used to realize the connection and communication between these components.

[0132] The user interface 403 may include a display screen (Display) and a camera (Camera), and the optional user interface 403 may also include a standard wired interface and a wireless interface.

[0133] The network interface 404 may optionally include a standard wired interface or a wireless interface (such as a WI-FI interface).

[0134] Among them, the processor 401 may include one or more processing cores. The processor 401 uses various interfaces and lines to connect various parts in the entire server, and executes various functions of the server and processes data by running or executing instructions, programs, code sets or instruction sets stored in the memory 405, and calling data stored in the memory 405. Optionally, the processor 401 can be implemented in at least one hardware form of digital signal processing (Digital Signal Processing, DSP), field programmable gate array (Field-Programmable Gate Array, FPGA), and programmable logic array (Programmable Logic Array, PLA). The processor 401 can integrate one or more combinations of a central processing unit (Central Processing Unit, CPU), a graphics processing unit (Graphics Processing Unit, GPU) and a modem. Among them, the CPU mainly processes the operating system, user interface and application programs; the GPU is responsible for rendering and drawing the content to be displayed on the display screen; the modem is used to process wireless communications. It can be understood that the above-mentioned modem may not be integrated into the processor 401, and it can be implemented by a single chip.

[0135] Among them, the memory 405 may include a random access memory (Random Access Memory, RAM) and may also include a read-only memory (Read-Only Memory). Optionally, the memory 405 includes a non-transitory computer-readable storage medium. The memory 405 can be used to store instructions, programs, codes, code sets or instruction sets. The memory 405 may include a program storage area and a data storage area, wherein the program storage area may store instructions for implementing an operating system, instructions for at least one function (such as a touch function, a sound playback function, an image playback function, etc.), instructions for implementing the above-mentioned various method embodiments, etc.; the data storage area may store data involved in the above-mentioned various method embodiments, etc. The memory 405 may also be optionally at least one storage device located away from the aforementioned processor 401. Refer to Figure 4 , the memory 405 as a computer storage medium may include an operating system, a network communication module, a user interface module, and an application program for a mining robot trajectory learning and tracking control device.

[0136] exist Figure 4 In the electronic device shown, the user interface 403 is mainly used to provide an input interface for the user and obtain the data input by the user; and the processor 401 can be used to call the mining robot trajectory learning and tracking control device application stored in the memory 405. When executed by one or more processors 401, the electronic device executes one or more of the methods described in the above embodiments. It should be noted that for the aforementioned method embodiments, for the sake of simple description, they are all expressed as a series of action combinations, but those skilled in the art should know that the present application is not limited by the described order of actions, because according to the present application, certain steps can be performed in other orders or simultaneously. Secondly, those skilled in the art should also know that the embodiments described in the specification are all preferred embodiments, and the actions and modules involved are not necessarily required for the present application.

[0137] The present application also provides a computer-readable storage medium, which stores instructions. When executed by one or more processors, the electronic device executes one or more of the methods described in the above embodiments.

[0138] In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0139] In the several embodiments provided in this application, it should be understood that the disclosed devices can be implemented in other ways. For example, the device embodiments described above are only schematic, such as the division of units, which is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some service interfaces, and the indirect coupling or communication connection of devices or units can be electrical or other forms.

[0140] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0141] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0142] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable memory. Based on this understanding, the technical solution of the present application, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, which is stored in a memory and includes several instructions for a computer device (which can be a personal computer, server or network device, etc.) to perform all or part of the steps of the various embodiments of the present application. The aforementioned memory includes: various media that can store program codes, such as USB flash drives, mobile hard drives, magnetic disks or optical disks.

[0143] The above are only exemplary embodiments disclosed in this application, and the scope of the disclosure of this application cannot be limited by this. That is, any equivalent changes and modifications made according to the teachings disclosed in this application are still within the scope of the disclosure of this application. After considering the disclosure of the specification and the truth of practice, those skilled in the art will easily think of other implementation schemes disclosed in this application.

[0144] This application is intended to cover any modifications, uses or adaptive changes disclosed in this application, which follow the general principles disclosed in this application and include common knowledge or customary technical means in the technical field not recorded in this application.

Claims

1. A mining robot trajectory learning and tracking control method, characterized in that: The method comprises: In response to the trajectory planning operation of the excavation robot, acquiring sensor data corresponding to the excavation robot, wherein the sensor data includes terrain point cloud data, inclination data and excavation trajectory data of the area to be excavated; Taking the terrain point cloud data of the area to be mined and the mining trajectory data as input features, and constructing a trajectory planning model; Using the inclination data as input features and constructing a force analysis model; Outputting the predicted excavation trajectory corresponding to the excavation robot according to the trajectory planning model, and outputting the predicted external force to which the excavation robot is subjected according to the force analysis model; The predicted excavation trajectory and the predicted external force are used as inputs, and the excavation robot is controlled by a trajectory tracking controller.

2. The method according to claim 1, characterized in that The step of obtaining sensor data corresponding to the mining robot specifically includes: Acquire the terrain point cloud data of the to-be-excavated area through a first sensor device, wherein the first sensor device includes a laser radar device and a depth camera device; Acquiring the inclination data by a second sensor device, wherein the second sensor device comprises an inclination sensor device and an inertial measurement unit sensor device; The geometric parameters of the mechanical arm of the excavating robot are obtained through a third sensor, and the excavation trajectory data is calculated under preset excavation operation conditions based on the mechanical arm geometric parameter data and the inclination angle data. The preset excavation operation conditions are to ensure that the rotation angle of the excavating robot remains unchanged during the excavation operation.

3. The method according to claim 2, characterized in that The calculating of the excavation trajectory data under preset excavation operation conditions by using the mechanical arm geometric parameter data and the inclination angle data specifically includes: The bucket end coordinates of the excavator robot are calculated according to the following formula: in, represents the bucket end coordinates of the excavator robot, L1, L2, and L3 are all the geometric parameter data of the mechanical arm, and L1 is the connecting rod length data of the excavator robot arm, L2 is the connecting rod length data of the excavator robot bucket, and L3 is the connecting rod length data of the excavator robot bucket, θ arm ,θ boom ,θ bucket are the inclination data, and θ arm is the joint angle data between the excavator arm and the base, θ boom is the joint angle data of the digging robot arm and boom, θ bucket The joint angle data of the bucket and the dipper arm of the excavating robot; According to the bucket end coordinates, the bucket end trajectory of the excavator robot in the target time period is constructed by the following formula: y demo =[y0,y1,…,y g ]; Among them, y demo is the bucket end trajectory, y0, y1, ..., y g Respectively represent the bucket end coordinates corresponding to g+1 different moments in the target time period, and satisfy The bucket end trajectory is used as the excavation trajectory data.

4. The method according to claim 1, characterized in that: The method of using the terrain point cloud data of the area to be mined and the mining trajectory data as input features and constructing a trajectory planning model specifically includes: Acquire slope information data and concavity data according to the terrain point cloud data of the area to be excavated; The mining trajectory data is learned by using the slope information data and the concavity data using a dynamic motion primitive method, and the trajectory planning model is constructed.

5. The method according to claim 1, characterized in that The method of using the inclination angle data as an input feature and constructing a force analysis model specifically includes: By performing Kalman filtering on the inclination angle data, the joint angular velocity data and the joint angular acceleration data corresponding to the excavating robot are calculated; The force analysis model is constructed according to the joint angular velocity data and the joint angular acceleration data.

6. The method according to claim 5, characterized in that The constructing the force analysis model according to the joint angular velocity data and the joint angular acceleration data specifically includes: The force analysis model is constructed by the following formula: Where M(θ)∈R 3 is the positive definite inertia matrix, R is a real number set, θ is the inclination data, represents the centrifugal force vector and the Coriolis force vector, is the first-order derivative of the inclination data, is the second-order derivative of the inclination data, g(θ)∈R 3 is the gravity vector, F s ∈R 3×3 is the Coulomb matrix, B∈R 3×3 is the viscous friction matrix, τ u ∈R 3 is the driving torque of the excavator robot, τ ext ∈R 3 is the external torque applied to the excavating robot; The external torque on the excavator robot is calculated by the disturbance observer: Among them, τ ext is the external torque on the excavator robot, p(t) is the generalized momentum at time t, (0) is the generalized momentum at time 0, τ μ is the friction force on the joints of the excavating robot, T is the geometric feature of the terrain of the area to be excavated, and K0 is used to adjust the dynamic response speed of the disturbance observer.

7. The method according to claim 6, characterized in that The controlling of the excavating robot by means of a trajectory tracking controller specifically includes: The excavation robot is controlled according to the following formula: Wherein, u(k) is the control input of the trajectory tracking controller at time k, Δu(k) is the increment of the control input of the trajectory tracking controller at time k, τ ext is the external torque on the excavator robot, k f is τ ext The feedback gain, e(k) represents the error between the first joint angular velocity and the second joint angular velocity at time k, the first joint angular velocity is the expected joint angular velocity, and the second joint angular velocity is the joint angular velocity estimated by Kalman filtering, K p , K i , K d They are the proportional, differential and integral coefficients of the PID controller respectively.

8. A trajectory learning and tracking control device for an excavating robot, characterized in that: The device includes an acquisition module, a modeling module, a generation module and a control module, wherein: The acquisition module is used to acquire sensor data corresponding to the excavation robot in response to the trajectory planning operation of the excavation robot, wherein the sensor data includes terrain point cloud data, inclination data and excavation trajectory data of the area to be excavated; The modeling module is used to take the terrain point cloud data of the area to be excavated and the excavation trajectory data as input features and construct a trajectory planning model; take the inclination data as input features and construct a force analysis model; A generating module, configured to output the predicted excavation trajectory corresponding to the excavation robot according to the trajectory planning model, and to output the predicted external force received by the excavation robot according to the force analysis model; A control module is used to take the predicted excavation trajectory and the predicted external force as input and control the excavation robot through a trajectory tracking controller.

9. An electronic device, characterized in that: It includes a processor, a communication bus, a user interface, a network interface and a memory, wherein the memory is used to store instructions, the user interface and the network interface are used to communicate with other devices, and the processor is used to execute the instructions stored in the memory so that the electronic device executes the method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores instructions, and when the instructions are executed, the method according to any one of claims 1 to 7 is performed.

Images