A method and device for trajectory learning and tracking control of a digging robot

Abstract

The application provides a kind of excavating robot trajectory learning and tracking control method and device, it is related to intelligent robot control.The method comprises: in response to the trajectory planning operation of excavating robot, the sensor acquisition data corresponding to excavating robot is obtained;With the terrain point cloud data of the area to be excavated and the excavation trajectory data as input features, a trajectory planning model is constructed;With the inclination data as input features, a force analysis model is constructed;According to the trajectory planning model, the predicted excavation trajectory corresponding to the excavating robot is output, and the predicted external force received by the excavating robot is output according to the force analysis model;The predicted excavation trajectory and the predicted external force are input, and the excavating robot is controlled by a trajectory tracking controller.The application solves the problem that the traditional rule-based excavating robot performs excavation action according to fixed trajectory and path, which is difficult to adapt to different excavation terrains.

Description

TECHNICAL FIELD

[0001] The present application relates to intelligent robot control, and in particular to a method and device for trajectory learning and tracking control of a digging robot. BACKGROUND

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

[0003] Traditional rule-based digging robots perform digging actions according to fixed trajectories and paths, thereby achieving uninterrupted operation. However, since the above-mentioned movement trajectories are simple and fixed, the influence of soil interaction dynamics is not considered, i.e., there is a problem of being difficult to adapt to different digging terrains.

[0004] Therefore, there is an urgent need for a method and device for trajectory learning and tracking control of a digging robot. SUMMARY

[0005] The present application provides a method and device for trajectory learning and tracking control of a digging robot, which solves the problem of traditional rule-based digging robots performing digging actions according to fixed trajectories and paths, which is difficult to adapt to different digging terrains.

[0006] In a first aspect of the present application, a method for trajectory learning and tracking control of a digging robot is provided, the method comprising: in response to a trajectory planning operation of the digging robot, acquiring sensor acquisition data corresponding to the digging robot, the sensor acquisition data including terrain point cloud data of a to-be-dug region, inclination data, and digging trajectory data; taking the terrain point cloud data of the to-be-dug region and the digging trajectory data as input features, and constructing a trajectory planning model; taking the inclination data as an input feature, and constructing a force analysis model; outputting a predicted digging trajectory corresponding to the digging robot according to the trajectory planning model, and outputting a predicted external force received by the digging robot according to the force analysis model; taking the predicted digging trajectory and the predicted external force as inputs, and controlling the digging robot through a trajectory tracking controller.

[0007] Optionally, the sensor data of the excavating robot is acquired, specifically including: acquiring terrain point cloud data of the region to be excavated by a first sensor device, the first sensor device including a laser radar device and a depth camera device; acquiring inclination data by a second sensor device, the second sensor device including an inclination sensor device and an inertial measurement unit sensor device; acquiring mechanical arm geometric parameter data of the excavating robot by a third sensor; and calculating the excavating trajectory data under a preset excavating operation condition by the mechanical arm geometric parameter data and the inclination data, the preset excavating operation condition being that the swing angle of the excavating robot is unchanged during the excavating operation.

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

[0009]

[0010] wherein, represents the coordinates of the end of the bucket of the excavating robot, L1, L2 and L3 are the mechanical arm geometric parameter data, L1 is the length data of the connecting rod of the boom of the excavating robot, L2 is the length data of the connecting rod of the stick of the excavating robot, and L3 is the length data of the connecting rod of the bucket of the excavating robot, θ arm , θ boom and θ bucket are the inclination data, θ arm is the joint angle data of the boom of the excavating robot and the base, θ boom is the joint angle data of the stick of the excavating robot and the boom, and θ bucket is the joint angle data of the bucket of the excavating robot and the stick; and the end-of-bucket trajectory of the excavating robot in the target time period is constructed according to the coordinates of the end of the bucket and by the following formula:

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

[0012] wherein, y demo is the end-of-bucket trajectory, y0, y1, …, y g respectively represent the coordinates of the end of the bucket corresponding to g+1 different time points in the target time period, and satisfy The end-of-bucket trajectory is taken as the excavating trajectory data.

[0013] Optionally, the terrain point cloud data of the region to be excavated and the excavation trajectory data are taken as input features, and a trajectory planning model is constructed, specifically including: obtaining slope information data and concave-convex degree data from the terrain point cloud data of the region to be excavated; learning the excavation trajectory data by using a dynamic motion primitive method based on the slope information data and the concave-convex degree data, and constructing the trajectory planning model.

[0014] Optionally, the inclination data are taken as input features, and a force analysis model is constructed, specifically including: calculating joint angular velocity data and joint angular acceleration data corresponding to the excavating robot by performing Kalman filtering on the inclination data; and constructing the force analysis model based on the joint angular velocity data and the joint angular acceleration data.

[0015] Optionally, the force analysis model is constructed based on the joint angular velocity data and the joint angular acceleration data, specifically including: constructing the force analysis model by the following formula:

[0016]

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

[0018]

[0019] wherein τ ext is an external torque acting on the excavating robot, p(t) is a generalized momentum at time t, p(0) is a generalized momentum at time 0, τ μ is a friction force acting on a joint of the excavating robot, T is a geometric feature of the terrain of the region to be excavated, and K0 is used to adjust the dynamic response speed of the disturbance observer.

[0020] Optionally, the excavating robot is controlled by a trajectory tracking controller, specifically including: controlling the excavating 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 k is the external torque acting on the excavating robot. f For τ ext The feedback gain, e(k), represents the error between the first and second joint angular velocities at time k, where the first joint angular velocity is the desired joint angular velocity, and the second joint angular velocity is the joint angular velocity estimated by Kalman filtering. p K i K d These are the proportional, derivative, and integral coefficients of the PID controller, respectively.

[0023] A second aspect of this application provides a trajectory learning and tracking control device for a mining robot. The device includes an acquisition module, a processing module, a generation module, and a control module, wherein...

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

[0025] The modeling module is used to construct a trajectory planning model by taking the topographic point cloud data of the area to be excavated and the excavation trajectory data as input features; and to construct a force analysis model by taking the dip angle data as input features.

[0026] The generation module is used to output the predicted digging trajectory for the excavating robot based on the trajectory planning model, and to output the predicted external forces acting on the excavating robot based on the force analysis model.

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

[0028] A third aspect of this application provides an electronic device 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 to cause the electronic device to perform any of the methods described above.

[0029] A fourth aspect of this application provides a computer-readable storage medium storing a computer program, which is executed by a processor using the method described in any of the foregoing descriptions.

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

[0031] 1. When performing trajectory planning on an excavating robot, the system acquires topographic point cloud data, tilt angle data, and excavation trajectory data of the area to be excavated. The topographic point cloud data and excavation trajectory data are used as input features to construct a trajectory planning model, while the tilt angle data is used as input features to construct a force analysis model. Based on the trajectory planning model, a predicted excavation trajectory is output, and based on the force analysis model, the predicted external forces acting on the excavating robot are output. These predicted excavation trajectory and predicted external forces are used as inputs, and the excavating robot is controlled by a trajectory tracking controller. This achieves efficient control of the excavating robot under complex soil conditions, solving the problem that traditional rule-based excavating robots, which execute excavation actions according to fixed trajectories and paths, are difficult to adapt to different excavation terrains.

[0032] 2. Obtain slope information and unevenness data based on the topographic point cloud data of the area to be excavated; use the slope information and unevenness data to learn the excavation trajectory data using the dynamic motion primitive method, and construct a trajectory planning model, thereby achieving accurate adaptation to complex terrain and optimized path planning.

[0033] 3. Using tilt angle data as input features, a force analysis model is constructed. Specifically, this includes: performing Kalman filtering on the tilt angle data to calculate the joint angular velocity and joint angular acceleration data of the excavating robot; constructing a force analysis model based on the joint angular velocity and joint angular acceleration data to accurately calculate the external torque and internal force experienced by the excavating robot under different working conditions, ensuring stability and safety during the excavation process. Attached Figure Description

[0034] Figure 1 This is a flowchart illustrating a method for trajectory learning and tracking control of an excavating robot provided in an embodiment of this application;

[0035] Figure 2 This is a schematic diagram of the structure and coordinate system configuration of an excavating robot provided in an embodiment of this application;

[0036] Figure 3 This is a schematic diagram of a module for a mining robot trajectory learning and tracking control device provided in an embodiment of this application;

[0037] Figure 4 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application.

[0038] Explanation of reference numerals in the attached figures: 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 Implementation

[0039] To enable those skilled in the art 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 with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments.

[0040] The terminology used in the following embodiments of this application is for the purpose of describing particular embodiments only and is not intended to be limiting of this application. As used in the specification of this application, the singular expressions “a,” “an,” “the,” “the,” “the,” and “this” are intended to include the plural expressions as well, unless the context clearly indicates otherwise. It should also be understood that the term “and / or” as used in this application refers to and includes any or all possible combinations of one or more of the listed items.

[0041] Hereinafter, the terms "first" and "second" are used for descriptive purposes only and should not be construed as implying or suggesting relative importance or implicitly indicating the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature, and in the description of the embodiments of this application, unless otherwise stated, "multiple" means two or more.

[0042] 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 The diagram illustrates a flowchart of a method for trajectory learning and tracking control of a mining robot provided in an embodiment of this application. The flowchart mainly includes the following steps: S101 to S105.

[0044] Step S101: In response to the trajectory planning operation of the excavating robot, acquire the sensor data collected by the excavating robot. The sensor data includes topographic point cloud data of the area to be excavated, tilt angle data, and excavation trajectory data.

[0045] Specifically, when the excavating robot is performing excavation work in the target area, its excavation trajectory is tracked in real time and planned. The target area refers to the specific geographical location or work area where the excavation task is scheduled to take place, including all places where excavation work needs to be performed. At this time, sensor data corresponding to the excavating robot is acquired through various sensors. The sensor data includes, but is not limited to, topographic point cloud data of the area to be excavated, tilt angle data, and excavation trajectory data. The area to be excavated is the actual working area within the target area, which usually refers to the specific terrain or area that has been determined and is ready to be excavated.

[0046] In one possible implementation, step S101 further includes: acquiring topographic 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 tilt angle data through a second sensor device, the second sensor device including a tilt angle sensor device and an inertial measurement unit sensor device; acquiring the robotic arm geometric parameters of the excavating robot through a third sensor; and calculating the excavation trajectory data under preset excavation operation conditions using the robotic arm geometric parameter data and the tilt angle data, the preset excavation operation conditions being that the rotation angle of the excavating robot remains unchanged during excavation operations.

[0047] Specifically, the topographic point cloud data of the area to be excavated can be obtained through lidar equipment and depth camera equipment. The specific steps are as follows: Assuming that the terrain of the area to be excavated is a sloping 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. The slope information of the terrain is then extracted using the following formula:

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

[0049] Where y(x) represents the height information of the point cloud data in the x-coordinate system, x1 and x2 represent the starting and ending points of the terrain slope, respectively, and a and b represent the slope and offset of the terrain, respectively; W(x) is a step function 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 tilt angle data can be obtained through tilt angle sensor devices and inertial measurement unit sensor devices. Assuming θ is the tilt angle data, then... Where, θ arm θ represents the joint angle data between the boom and the base of the excavating robot. boom θ represents the joint angle data of the boom and stick of the excavating robot.bucket This refers to the joint angle data between the bucket and the stick of the excavating robot. Let θ represent a three-dimensional vector.

[0052] The geometric parameters of the excavating robot's arm can be obtained through a third sensor. Using the arm's geometric parameters and the tilt angle data, the excavation trajectory data is calculated under preset excavation conditions. These preset conditions ensure that the excavating robot's rotation angle remains constant during excavation. The specific calculation method for the excavation trajectory data is as follows: First, the bucket end coordinates of the excavating robot in Cartesian space are obtained using the following formula:

[0053] The coordinates of the bucket end of the excavating robot are calculated using the following formula:

[0054]

[0055] in, Let L1, L2, and L3 represent the bucket end-effector coordinates of the excavating robot. L1, L2, and L3 are all geometric parameters of the robotic arm, with L1 representing the link length of the excavating robot's boom, L2 representing the link length of the excavating robot's stick, and L3 representing the link length of the excavating robot's bucket. Then, based on the bucket end-effector coordinates, the bucket end-effector trajectory of the excavating robot during the target time period is constructed using the following formula:

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

[0057] Among them, y demo Let y0, y1, ..., y be the trajectory of the bucket end point. g Let g and g represent the coordinates of the bucket end at different times within the target time period, and satisfy the following conditions: The trajectory of the bucket end is used as the excavation trajectory data. Please refer to... Figure 2 The document presents a schematic diagram of the structure and coordinate system configuration of an excavating robot provided in an embodiment of this application.

[0058] Step S102: Use the topographic point cloud data of the area to be excavated and the excavation trajectory data as input features, and construct a trajectory planning model.

[0059] Specifically, slope information and unevenness data are obtained from the topographic point cloud data of the area to be excavated; using the slope information data and the unevenness data, the excavation trajectory data is learned using the dynamic motion primitive method, and the trajectory planning model is constructed. Specifically, for the unevenness data, a Gaussian function can be used to fit the degree of unevenness of the terrain.

[0060]

[0061] Where d represents the depth parameter, and x0 represents the coordinates of the deepest point in the area to be excavated; furthermore, based on the terrain slope parameter, c is a parameter that controls the range of influence of depth, affecting the distribution pattern of depth changes in the area to be excavated. Further, based on the terrain slope parameter, the tangent angle of the slope can be obtained:

[0062] θ slope =tan -1 a;

[0063] Where 'a' is the terrain slope parameter, used to represent the change in vertical height per unit horizontal distance, and θ... slope Let θ be the tangent angle of the slope. Assuming the geometric characteristics of the terrain in the area to be excavated, i.e., the degree of concavity / convexity, are T, then T = (θ...). slope ,d).

[0064] Based on the excavation trajectory data obtained in step S101, which is [y0, y1, ..., y], g The dynamic motion primitive method is used to learn the mining trajectory data and construct a trajectory planning model:

[0065]

[0066] in, Let τ represent a regular system used to construct a time-independent quantity s, where τ is a scaling factor and α is a variable. x For constants in a regularized system; Let represent a second-order system with a nonlinear function, used to construct the expert demonstration trajectory, where y represents the bucket end trajectory, defined as y demo same, and The first and second derivatives of y are respectively, and α is the first and second derivative of y. y and β y All are constants, f(s) is a nonlinear function used to adjust the endpoint and shape of the expert demonstration trajectory, and satisfies (s) = h(s)(y g -y0)s, h(s) are expressions for the normalized weighted superposition of multiple nonlinear basis functions, and satisfy the following: Ψ i (s) is the Gaussian function, and its specific expression is: i represents the i-th nonlinear function, ω i Here, σ represents the weight parameters corresponding to the basis functions, N represents the number of basis functions, and σ represents the weight parameters corresponding to the basis functions. i and c i These represent the width and center position of the basis functions, respectively.

[0067] The formula ω can be determined using the locally weighted regression method. i First, construct the objective function, assuming the objective function is f. target Its expression is as follows:

[0068]

[0069] Further combining the objective function and Gaussian function mentioned above, we construct a loss function, assuming the loss function is J. i Its expression is as follows:

[0070]

[0071] Where P represents the total number of time steps for the expert demonstration trajectory, and t represents each discrete time point in the trajectory planning process. Furthermore, optimization methods are used to solve... Thus, ω is obtained i The expression is as follows:

[0072]

[0073] in, T represents the geometric features of the terrain in the area to be excavated, i.e., the degree of concavity and convexity.

[0074] Step S103: Use the tilt angle data as input features and construct a force analysis model.

[0075] Specifically, Kalman filtering is applied to the tilt angle data to calculate the joint angular velocity and joint angular acceleration data corresponding to the excavating robot; based on the joint angular velocity and joint angular acceleration data, the force analysis model is constructed. Kalman filtering is used to calculate the joint angular velocity and joint angular acceleration data. For convenience, a single joint angle θ is used as an example, with the joint angular velocity ω and the joint angular acceleration α. ​​The state vector is assumed to be x. Therefore... The state estimation model can then be constructed using the following formula, which is used to accurately estimate the motion state of the excavating robot:

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

[0077] Where, x k w is the state vector at time k. k Let F be the process noise, and F be the state transition matrix. Assuming the sampling period is dt, then... G is the noise influence 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 It is the observation value at time k, v k H is the measurement noise at time k, H is the observation matrix, and H = [1 00].

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

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

[0082]

[0083] in, This is the predicted state value at time k. This is the predicted prior state value at time k+1.

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

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

[0086] Among them, P k Let P be the state covariance matrix at time k. k+1|k Let Q be the state covariance matrix at time k+1, Q be the process noise covariance matrix, and T be the geometric features of the terrain to be excavated in step S012, i.e., the degree of concavity and convexity.

[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 Let K be the Kalman gain at time k, and R be the covariance matrix of the measurement noise, which is usually set according to the accuracy of the sensor.

[0090] S4. Update the state for the next moment:

[0091]

[0092] Among them, z k+1 It is the observation value at time k+1.

[0093] S5. Update the state covariance matrix:

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

[0095] Where I is the identity matrix. The force analysis model is constructed using the following formula:

[0096]

[0097] Where M(θ)∈R 3 Let R be the positive definite inertia matrix, R be the set of real numbers, and θ be the tilt angle data. Represents the centrifugal force vector and the Coriolis force vector. The first derivative of the tilt angle data. Let g(θ) be the second derivative of the tilt angle data, where g(θ) ∈ R. 3 F is the gravity vector. s ∈R 3×3 Let B be a Coulomb matrix, and B ∈ R. 3×3 Let τ be the viscous friction matrix. u ∈R 3 The driving torque of the excavating robot, τ, can be measured using a hydraulic pressure sensor. ext ∈R 3 The external torque acting on the excavating robot is denoted as .

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

[0099]

[0100] Where, τ ext Let p(t) be the external torque acting on the excavating robot, p(t) be the generalized momentum at time t, and (0) be the generalized momentum at time 0. μ Let be the frictional force experienced by the joints of the excavating robot, and T represents the geometric features of the terrain in the area to be excavated, and K0 is used to adjust the dynamic response speed of the disturbance observer.

[0101] Step S104: Output the predicted excavation trajectory corresponding to the excavating robot according to the trajectory planning model, and output the predicted external force on the excavating robot according to the force analysis model.

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

[0103] Step S105: The predicted excavation trajectory and the predicted external force are used as inputs, and the excavation robot is controlled by the trajectory tracking controller.

[0104] Specifically, the digging trajectory in Cartesian space is first converted into joint angular velocity in joint space using inverse kinematics formulas. An incremental PID controller is then used to track the joint angular velocity of the excavator robot. Based on the external torque calculated in step S103, this external torque is used as a feedforward term of the PID controller to compensate for the external torque acting on the excavator. Thus, the excavator robot is controlled according to the following formula:

[0105]

[0106] 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 k is the external torque acting on the excavating robot. f For τ ext The feedback gain, e(k), represents the error between the first and second joint angular velocities at time k, where the first joint angular velocity is the desired joint angular velocity, and the second joint angular velocity is the joint angular velocity estimated by Kalman filtering. p K i K d These are the proportional, derivative, and integral coefficients of the PID controller, respectively.

[0107] This application adopts the above-described method.

[0108] Please refer to Figure 3 The diagram illustrates a module schematic of a mining robot trajectory learning and tracking control device according to an embodiment of this application. The device includes an acquisition module 31, a processing module 32, a generation module 33, and a control module 34.

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

[0110] Modeling module 32 is used to construct a trajectory planning model by taking the topographic point cloud data of the area to be excavated and the excavation trajectory data as input features; and to construct a force analysis model by taking the dip angle data as input features.

[0111] The generation module 33 is used to output the predicted digging trajectory of the digging robot according to the trajectory planning model and to output the predicted external force on the digging robot according to the force analysis model.

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

[0113] In one possible implementation, the acquisition module 31 is used to acquire sensor data collected by the excavating robot, specifically including: acquiring topographic 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 tilt angle data through a second sensor device, the second sensor device including a tilt angle sensor device and an inertial measurement unit sensor device; acquiring the geometric parameters of the excavating robot's robotic arm through a third sensor; and calculating the excavation trajectory data under preset excavation operation conditions using the robotic arm geometric parameter data and the tilt angle data, the preset excavation operation conditions being that the rotation angle of the excavating robot remains unchanged during the excavation operation.

[0114] In one possible implementation, the acquisition module 31 is used to calculate excavation trajectory data under preset excavation operation conditions using the robotic arm's geometric parameter data and tilt angle data, specifically including: calculating the bucket end coordinates of the excavating robot according to the following formula:

[0115]

[0116] in, This represents the coordinates of the excavator's bucket end effector. L1, L2, and L3 are all geometric parameters of the robotic arm, with L1 being the link length of the excavator's boom, L2 being the link length of the excavator's stick, and L3 being the link length of the excavator's bucket. θ arm θ boom θ bucket All are tilt angle data, and θ arm To extract the joint angle data between the robot arm and the base, θ boom To extract joint angle data of the robot's stick and boom, θ bucket To obtain the joint angle data of the excavator's bucket and stick, and based on the bucket end-effector coordinates, the excavator's bucket end-effector trajectory during the target time period is constructed using the following formula:

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

[0118] Among them, y demo Let y0, y1, ..., y be the trajectory of the bucket end point.g Let g and g represent the bucket end coordinates at g+1 different times within the target time period, and satisfy the following conditions: The trajectory of the bucket end is used as the excavation trajectory data.

[0119] In one possible implementation, the modeling module 32 is used to take the topographic point cloud data of the area to be excavated and the excavation trajectory data as input features and construct a trajectory planning model. Specifically, it includes: obtaining slope information data and concavity / convexity data based on the topographic point cloud data of the area to be excavated; using the slope information data and concavity / convexity data, learning the excavation trajectory data using the dynamic motion primitive method, and constructing a trajectory planning model.

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

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

[0122]

[0123] Where M(θ)∈R 3 Let R be the positive definite inertia matrix, R be the set of real numbers, and θ be the tilt angle data. Represents the centrifugal force vector and the Coriolis force vector. The first derivative of the dip angle data. Let g(θ) be the second derivative of the tilt angle data, ∈ R. 3 F is the gravity vector. s ∈R 3×3 Let B be a Coulomb matrix, and B ∈ R. 3×3 Let τ be the viscous friction matrix. u ∈R 3 To provide the driving torque for the excavator robot, τ ext ∈R 3 The external torque acting on the excavating robot; the external torque acting on the excavating robot is calculated using a perturbation observer:

[0124]

[0125] Where, τ ext The external torque acting on the excavating robot, p(t) is the generalized momentum at time t, (0) is the generalized momentum at time 0, τ μTo explore the frictional forces acting on the robot's joints, T represents the geometric features 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 one possible implementation, the control module 34 is used to control the excavating robot via a trajectory tracking controller, specifically by controlling the excavating 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 k is the external torque acting on the excavating robot. f For τ ext The feedback gain, e(k), represents the error between the first and second joint angular velocities at time k, where the first joint angular velocity is the desired joint angular velocity, and the second joint angular velocity is the joint angular velocity estimated by Kalman filtering. p K i K d These are the proportional, derivative, and integral coefficients of the PID controller, respectively.

[0129] It should be noted that the above embodiments of the apparatus are only illustrated by the division of the above functional modules. In practical applications, the above functions can be assigned to different functional modules as needed, that is, the internal structure of the device can be divided into different functional modules to complete all or part of the functions described above. In addition, the apparatus and method embodiments provided in the above embodiments belong to the same concept, and the specific implementation process can be found in the method embodiments, which will not be repeated here.

[0130] This application also provides an electronic device. (See reference...) Figure 4 , Figure 4 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this 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 enable communication between these components.

[0132] The user interface 403 may include a display screen and a camera. Optionally, the 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] The processor 401 may include one or more processing cores. The processor 401 connects to various parts of the server using various interfaces and lines, and performs various server functions and processes data by running or executing instructions, programs, code sets, or instruction sets stored in memory 405, and by calling data stored in memory 405. Optionally, the processor 401 may be implemented using at least one hardware form of Digital Signal Processing (DSP), Field-Programmable Gate Array (FPGA), or Programmable Logic Array (PLA). The processor 401 may integrate one or a combination of several of the following: Central Processing Unit (CPU), Graphics Processing Unit (GPU), and modem. The CPU primarily handles the operating system, user interface, and applications; the GPU is responsible for rendering and drawing the content required for display; and the modem handles wireless communication. It is understood that the modem may also not be integrated into the processor 401 and may be implemented as a separate chip.

[0135] The memory 405 may include random access memory (RAM) or read-only memory. Optionally, the memory 405 may include a non-transitory computer-readable storage medium. The memory 405 may be used to store instructions, programs, code, 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 touch function, sound playback function, image playback function, etc.), instructions for implementing the above-described method embodiments, etc.; the data storage area may store data involved in the above-described method embodiments, etc. Optionally, the memory 405 may also be at least one storage device located remotely from the aforementioned processor 401. (Refer to...) Figure 4 The memory 405, which serves as a computer storage medium, may include an operating system, a network communication module, a user interface module, and an application program for the excavator robot trajectory learning and tracking control device.

[0136] existFigure 4 In the illustrated electronic device, the user interface 403 is primarily used to provide an input interface for the user and acquire user input data; while the processor 401 can be used to call the application program of the excavator robot trajectory learning and tracking control device stored in the memory 405. When executed by one or more processors 401, the electronic device performs one or more of the methods described in the above embodiments. It should be noted that, for the foregoing method embodiments, for the sake of simplicity, they are all described as a series of actions. However, those skilled in the art should understand that this application is not limited to the described order of actions, because according to this application, some steps can be performed in other orders or simultaneously. Secondly, those skilled in the art should also understand that the embodiments described in the specification are all preferred embodiments, and the actions and modules involved are not necessarily essential to this application.

[0137] This application also provides a computer-readable storage medium storing instructions. When executed by one or more processors, these instructions cause an electronic device to perform one or more of the methods described in the above embodiments.

[0138] In the above embodiments, the descriptions of each embodiment have different focuses. For parts not described in detail in a certain embodiment, please refer to the relevant descriptions in other embodiments.

[0139] In the various embodiments provided in this application, it should be understood that the disclosed apparatus can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some service interface; the indirect coupling or communication connection between apparatuses or units may be electrical or other forms.

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

[0141] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0142] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage device (CMD). Based on this understanding, the technical solution of this application, in essence, 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. This computer software product is stored in a memory and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods of the various embodiments of this application. The aforementioned memory includes various media capable of storing program code, such as USB flash drives, portable hard drives, magnetic disks, or optical disks.

[0143] The above description is merely an exemplary embodiment disclosed in this application and should not be construed as limiting the scope of this application. Any equivalent changes and modifications made in accordance with the teachings of this application shall still fall within the scope of this application. Those skilled in the art, upon considering the disclosure of the specification and practical truths, will readily conceive of other embodiments disclosed in this application.

[0144] This application is intended to cover any variations, uses, or adaptations disclosed herein that follow the general principles disclosed herein and include common knowledge or customary technical means in the art that are not described in this application.

Claims

1. A method for trajectory learning and tracking control of an excavating robot, characterized in that, The method includes: In response to the trajectory planning operation of the excavating robot, the sensor data collected by the excavating robot is acquired, including topographic point cloud data of the area to be excavated, tilt angle data, and excavation trajectory data. The topographic point cloud data of the area to be excavated and the excavation trajectory data are used as input features to construct a trajectory planning model; The tilt angle data is used as input features to construct a force analysis model; The predicted excavation trajectory corresponding to the excavating robot is output according to the trajectory planning model, and the predicted external force on the excavating robot is output 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. The step of using the topographic point cloud data of the area to be excavated and the excavation trajectory data as input features to construct a trajectory planning model specifically includes: obtaining slope information data and unevenness data based on the topographic point cloud data of the area to be excavated; learning the excavation trajectory data using the dynamic motion primitive method based on the slope information data and the unevenness data, and constructing the trajectory planning model; the step of using the tilt angle data as input features to construct a force analysis model specifically includes: calculating the joint angular velocity data and joint angular acceleration data corresponding to the excavating robot by performing Kalman filtering on the tilt angle data; and constructing the force analysis model based on the joint angular velocity data and joint angular acceleration data.

2. The method according to claim 1, characterized in that, The acquisition of sensor data corresponding to the excavating robot specifically includes: The topographic point cloud data of the area to be excavated is acquired by a first sensor device, which includes a lidar device and a depth camera device. The tilt angle data is acquired through a second sensor device, which includes a tilt angle sensor device and an inertial measurement unit sensor device. The excavation robot acquires its robotic arm geometric parameters using a third sensor. Based on the robotic arm geometric parameter data and the tilt angle data, the excavation trajectory data is calculated under preset excavation operation conditions, whereby the excavation robot maintains a constant rotation angle during excavation.

3. The method according to claim 2, characterized in that, The step of calculating the excavation trajectory data under preset excavation operation conditions using the robotic arm's geometric parameter data and the tilt angle data specifically includes: The coordinates of the bucket end of the excavating robot are calculated using the following formula: in, Let L1, L2, and L3 represent the coordinates of the bucket end of the excavating robot. L1, L2, and L3 are all geometric parameters of the robotic arm, where L1 is the link length of the excavating robot boom, L2 is the link length of the excavating robot stick, and L3 is the link length of the excavating robot bucket. θ arm θ boom θ bucket All are the tilt angle data, and θ arm θ represents the joint angle data between the boom and the base of the excavating robot. boom θ represents the joint angle data of the boom and stick of the excavating robot. bucket The joint angle data between the bucket and the stick of the excavating robot; Based on the bucket end coordinates, the bucket end trajectory of the excavating robot during the target time period is constructed using the following formula: y demo =[y0,y1,…,y g ]; Among them, y demo Let y0, y1, ..., y be the trajectory of the bucket end point. g Let g and g represent the coordinates of the bucket end at different times within the target time period, and satisfy the following conditions: (i = 0, 1, 2, ..., g); The trajectory of the bucket end is used as the excavation trajectory data.

4. The method according to claim 1, characterized in that, The construction of the force analysis model based on the joint angular velocity data and the joint angular acceleration data specifically includes: The force analysis model is constructed using the following formula: Where M(θ)∈R 3 Let R be the positive definite inertia matrix, R be the set of real numbers, and θ be the tilt angle data. Represents the centrifugal force vector and the Coriolis force vector. The first derivative of the tilt angle data. Let g(θ) be the second derivative of the tilt angle data, where g(θ) ∈ R. 3 F is the gravity vector. s ∈R 3×3 Let B be a Coulomb matrix, and B ∈ R. 3×3 Let τ be the viscous friction matrix. u ∈R 3 τ is the driving torque of the excavating robot. ext ∈R 3 The external torque acting on the excavating robot; The external torque acting on the excavating robot is calculated using a perturbation observer: Where, τ ext Let p(t) be the external torque acting on the excavating robot, p(t) be the generalized momentum at time t, p(0) be the generalized momentum at time 0, and τ be the external torque acting on the excavating robot. μ The frictional force experienced by the joints of the excavating robot. T is the transpose matrix, and K0 is used to adjust the dynamic response speed of the disturbance observer.

5. The method according to claim 4, characterized in that, The control of the excavating robot via a trajectory tracking controller specifically includes: The excavating robot is controlled according to the following formula: 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 k is the external torque acting on the excavating robot. f For τ ext The feedback gain, e(k), represents the error between the first and second joint angular velocities at time k, where the first joint angular velocity is the desired joint angular velocity, and the second joint angular velocity is the joint angular velocity estimated by Kalman filtering. p K i K d These are the proportional, derivative, and integral coefficients of the PID controller, respectively.

6. 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 excavating robot in response to the trajectory planning operation of the excavating robot. The sensor data includes topographic point cloud data of the area to be excavated, tilt angle data, and excavation trajectory data. The modeling module is used to construct a trajectory planning model by using the topographic point cloud data of the area to be excavated and the excavation trajectory data as input features; and to construct a force analysis model by using the tilt angle data as input features. Specifically, constructing the trajectory planning model by using the topographic point cloud data of the area to be excavated and the excavation trajectory data as input features includes: obtaining slope information data and unevenness data based on the topographic point cloud data of the area to be excavated; learning the excavation trajectory data using the dynamic motion primitive method based on the slope information data and the unevenness data, and constructing the trajectory planning model. Constructing the force analysis model by using the tilt angle data as input features specifically includes: calculating the joint angular velocity data and joint angular acceleration data corresponding to the excavating robot by performing Kalman filtering on the tilt angle data; and constructing the force analysis model based on the joint angular velocity data and joint angular acceleration data. The generation module is used to output the predicted excavation trajectory corresponding to the excavation robot according to the trajectory planning model, and to output the predicted external force on the excavation robot according to the force analysis model. The control module is used to take the predicted excavation trajectory and the predicted external force as inputs, and control the excavation robot through the trajectory tracking controller.

7. An electronic device, characterized in that, The device includes a processor, a communication bus, a user interface, a network interface, and a memory. The memory is used to store instructions. The user interface and the network interface are used to communicate with other devices. The processor is used to execute the instructions stored in the memory to cause the electronic device to perform the method as described in any one of claims 1 to 5.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores instructions that, when executed, perform the method as described in any one of claims 1 to 5.

Images