Autonomous exploration and control method and device for swing arm type tracked robot

Abstract

The invention discloses an autonomous exploration and control method and device for a swing arm type tracked robot, and relates to the technical field of robot motion control. The method comprises the following steps: planning a local optimal path comprising a plurality of path points according to an initial position, a local target point and an RTAA * algorithm; predicting an initial swing arm angle based on the skeleton model and the local elevation map, and generating a path point set according to the path points and the initial swing arm angle; performing spline interpolation optimization on the discrete initial swing arm angle in the path point set; when path tracking is executed, according to the current speed and the optimized angle corresponding to the next path point, the swing arm type tracked robot is controlled to swing arms according to the swing arm advance, and the swing arm type tracked robot moves to the next path point; and the execution is repeated until the swing arm type tracked robot reaches the end point target and finally reaches the end point target. According to the invention, the stability of swing arm control of the robot can be ensured, and the accuracy of operation control of the robot is improved.

Description

Technical Field

[0001] The present invention relates to the technical field of robot motion control, and in particular to an autonomous exploration and control method and device for a swing-arm crawler robot. Background Art

[0002] In recent years, with the rapid development of robotic automation technology, more and more highly intelligent robots have emerged, and the role of robots in people's daily lives has become increasingly profound. Research on highly intelligent and widely applicable intelligent robots has attracted much attention. Currently, most robots on the market operate with a priori maps of the environment. However, map construction requires a large amount of manual labor to collect information about the environment in advance. The complex process leads to a heavy workload. In addition, in actual operating environments, there are many special environments that are difficult for humans to pass through or enter. Such environments pose a huge challenge to personnel safety and cause great difficulties in constructing prior maps. To solve these problems, autonomous exploration technology, which allows robots to complete target tasks through autonomous navigation in unknown environments without prior maps, has become a research focus both domestically and internationally.

[0003] While traditional mobile robots can achieve a certain degree of operational capability in challenging, unknown environments through autonomous exploration, they are unable to cope with unstructured environments that may contain numerous rocks, bricks, steps, and other difficult terrain. To address this issue, swing-arm tracked robots, with their multiple degrees of freedom and robust obstacle-crossing capabilities, have become a key research focus for autonomous exploration in challenging environments. These robots are capable of handling unstructured, challenging environments and performing tasks such as exploration, transportation, and rescue. They are a key tool for autonomous exploration in the wild. In the process of autonomous exploration in a complex unstructured unknown environment, if a swing-arm tracked robot wants to operate safely in an unknown unstructured environment, it must have the ability to understand the surrounding environment to ensure that it can autonomously perceive, plan and control in the environment. Perception refers to the perception of its own state and the surrounding environment through sensors and cameras, and planning is to autonomously explore a safe and collision-free path in the established environmental model. The control system is one of the key parts of the swing-arm tracked robot. Unlike traditional mobile robots that only need to consider their own overall movement, the swing-arm tracked robot needs to consider not only the overall movement, but also the swing angle of its own swing arm and the change in the robot's spatial morphology caused by the swing arm movement, which leads to changes in the passability. The current swing-arm tracked robot fails to consider the motion performance of the tracked robot, and does not give full play to the characteristics of the tracked robot climbing obstacles during the exploration process, resulting in low efficiency and accuracy of exploration and control. Summary of the Invention

[0004] In order to solve the technical problem of low efficiency and accuracy of exploration and control in the existing technology, the embodiment of the present invention provides an autonomous exploration and control method and device for a swing-arm crawler robot. The technical solution is as follows:

[0005] In one aspect, a method for autonomous exploration and control of a swing-arm crawler robot is provided. The method is implemented by an autonomous exploration and control device for the swing-arm crawler robot, and the method comprises:

[0006] S1, obtain the initial position and end target of the swing-arm crawler robot;

[0007] S2. Exploring based on the initial position and the RTAA* algorithm to obtain a local target point, and obtaining a local optimal path including multiple path points based on the local target point and the initial position;

[0008] S3. For each path point in the local optimal path, predict the initial swing arm angle based on the skeleton model and the local elevation map, and generate a path point set based on the path point and the corresponding initial swing arm angle;

[0009] S4, performing spline interpolation optimization on the discrete initial swing arm angles in the path point set to generate an optimized angle sequence;

[0010] S5. When executing path tracking, calculating a swing arm lead amount based on the current speed of the swing arm crawler robot, the position of the next path point, and the optimized angle corresponding to the next path point, and controlling the swing arm crawler robot to swing its arm according to the swing arm lead amount and move to the next path point;

[0011] S6, repeating S5 until the swing-arm crawler robot reaches the local target point;

[0012] S7, determining the local target point as a new initial position, and repeating S2-S6. When the local target point is the end target, the path planning and exploration task of the swing-arm crawler robot is completed.

[0013] On the other hand, an autonomous exploration and control device for a swing-arm crawler robot is provided, and the device is applied to an autonomous exploration and control method for a swing-arm crawler robot, and the device includes:

[0014] An acquisition unit, used to acquire the initial position and end point target of the swing-arm crawler robot;

[0015] The planning unit is used to explore based on the initial position and the RTAA* algorithm to obtain a local target point, and then obtain a local optimal path including multiple path points based on the local target point and the initial position;

[0016] a determination unit, configured to predict, for each path point in the local optimal path, an initial swing arm angle based on the skeleton model and the local elevation map, and generate a path point set according to the path point and the corresponding initial swing arm angle;

[0017] A generation unit, used to perform spline interpolation optimization on the discrete initial swing arm angles in the path point set to generate an optimized angle sequence;

[0018] a calculation unit for calculating a swing arm lead amount according to the current speed of the swing arm crawler robot, the position of the next path point, and the optimized angle corresponding to the next path point when performing path tracking, and controlling the swing arm crawler robot to swing its arm according to the swing arm lead amount and move to the next path point;

[0019] an exploration unit, configured to repeatedly execute the calculation unit until the swing-arm crawler robot reaches a local target point;

[0020] The cyclic execution unit is used to determine the local target point as a new initial position, repeatedly execute the planning unit to the exploration unit, and when the local target point is the end target, complete the path planning and exploration task of the swing-arm crawler robot.

[0021] On the other hand, an autonomous exploration and control device for a swing-arm tracked robot is provided, and the autonomous exploration and control device for a swing-arm tracked robot includes: a processor; a memory, on which computer-readable instructions are stored, and when the computer-readable instructions are executed by the processor, any one of the above-mentioned autonomous exploration and control methods for a swing-arm tracked robot is implemented.

[0022] On the other hand, a computer-readable storage medium is provided, in which at least one instruction is stored. The at least one instruction is loaded and executed by a processor to implement any one of the above-mentioned autonomous exploration and control methods for a swing-arm crawler robot.

[0023] The beneficial effects brought about by the technical solution provided by the embodiment of the present invention include at least:

[0024] The embodiment of the present invention, based on a comprehensive consideration of the kinematic characteristics and climbing ability of a swing-arm tracked robot, adopts a swing-arm control method based on the robot skeleton model to obtain the swing-arm angle, thereby constructing an autonomous exploration framework for the swing-arm tracked robot in an unknown environment. This framework implements phased path planning by introducing the RTAA* algorithm, and combines dynamic prediction and optimization of the swing-arm angle to achieve efficient path tracking and autonomous exploration control. Given a target point and the robot's current posture, the method of the present invention can be used to calculate the robot's complete path from the starting point to the target point, and optimize the continuity of the swing-arm angle to ensure smooth control, thereby improving the efficiency and accuracy of the swing-arm tracked robot's exploration and control. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0026] Figure 1-1 This is a flow chart of an autonomous exploration and control method for a swing-arm crawler robot provided by an embodiment of the present invention;

[0027] Figure 1-2 is a pseudo code schematic diagram for autonomous exploration and control of a swing-arm crawler robot provided by an embodiment of the present invention;

[0028] Figure 2 This is a block diagram of an autonomous exploration and control device for a swing-arm crawler robot provided by an embodiment of the present invention;

[0029] Figure 3 It is a structural schematic diagram of an autonomous exploration and control device for a swing-arm crawler robot provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0030] The technical solution of the present invention is described below in conjunction with the accompanying drawings.

[0031] In the embodiments of the present invention, words such as "exemplarily" and "for example" are used to indicate examples, illustrations, or explanations. Any embodiment or design described as an "exemplary" in the present invention should not be interpreted as being preferred or advantageous over other embodiments or designs. Rather, the use of the word "exemplary" is intended to present concepts in a concrete manner. Furthermore, in the embodiments of the present invention, "and / or" can mean both or either of the two.

[0032] In the embodiments of the present invention, the terms "image" and "picture" may be used interchangeably. It should be noted that, when the distinction between them is not emphasized, their intended meanings are the same. The terms "of," "corresponding," and "corresponding" may be used interchangeably. It should be noted that, when the distinction between them is not emphasized, their intended meanings are the same.

[0033] In the embodiments of the present invention, sometimes a subscript such as W1 may be written as a non-subscript such as W1. When the difference is not emphasized, the meanings to be expressed are the same.

[0034] In order to make the technical problems, technical solutions and advantages to be solved by the present invention clearer, a detailed description will be given below with reference to the accompanying drawings and specific embodiments.

[0035] The embodiment of the present invention provides an autonomous exploration and control method for a swing-arm crawler robot. The method can be implemented by an autonomous exploration and control device for a swing-arm crawler robot. The autonomous exploration and control device for a swing-arm crawler robot can be a terminal or a server. Figure 1-1 The flowchart of the autonomous exploration and control method for a swing-arm crawler robot is shown. The processing flow of the method may include the following steps:

[0036] S1. Obtain the initial position and end point of the swing-arm crawler robot.

[0037] In one feasible implementation, the first step in autonomous exploration is to specify a final exploration target (i.e., the final destination) for the robot. This target is typically input by the user or generated by higher-level task planning. Due to the numerous unpredictable obstacles and terrain challenges (such as gravel, steps, and slopes) in unknown environments, directly planning a global path may not meet real-time and planning accuracy requirements. Therefore, a phased exploration strategy is adopted, breaking down the final task into multiple sub-stages.

[0038] Each stage of the mission is planned and controlled based on the robot's current position and the current exploration target. This phased planning allows for rapid adaptation to environmental changes and allows for gradual approach to the optimal path through local optima.

[0039] S2. Explore based on the initial position and the RTAA* algorithm to obtain a local target point, and obtain a local optimal path including multiple path points based on the local target point and the initial position.

[0040] In a feasible implementation, after exploring the local environment, the explored point with the smallest heuristic value is selected as the local target point.

[0041] Path planning is one of the core steps to achieve autonomous exploration. In each stage, the RTAA* algorithm is used to plan a local optimal path {P1, P2, ..., P n The generation of waypoints is based on the local traversability map M(x, y), which is constructed in real time by the robot sensors.

[0042] Optionally, the specific operation steps of S2 may include the following S21-S28:

[0043] S21. Generate an elevation map based on the point cloud data or the depth map.

[0044] In one feasible implementation, an elevation map H(x,y) is generated from point cloud data or a depth map to provide a terrain basis for the planning algorithm.

[0045] S22. Calculate the roughness of the local area based on the elevation map.

[0046] Optionally, S22 calculates the roughness of the local area based on the elevation map, and the calculation formula is as follows (1):

[0047]

[0048] Among them, σ(x, y) represents the roughness of the local area, h i Indicates the elevation value corresponding to the i-th coordinate point in the elevation map H(x, y), is the average elevation of the local area, and N is the total number of coordinate points in the elevation map.

[0049] S23. Calculate the steepness of the path points based on the elevation map.

[0050] Optionally, the steepness of the path point is calculated based on the elevation map in S23, and the calculation formula is as follows (2):

[0051]

[0052] Where G(x, y) represents the steepness of the path point, and H(x, y) represents the elevation map. Using the elevation map gradient calculation formula, the steepness of the path point is evaluated, limiting the areas where the swing-arm crawler robot cannot climb.

[0053] S24. Determine the actual terrain cost from the initial position to each exploration point based on the roughness and steepness.

[0054] In a feasible implementation, the steepness, roughness and robot climbing ability are weightedly summed to obtain the final actual cost, comprehensively considering the robot climbing ability, roughness and gradient constraints.

[0055] S25. Calculate the initial heuristic value of each exploration point through the Euclidean distance between each exploration point and the end target.

[0056] Optionally, in S25, a heuristic estimation value from the initial position to the local target point is determined based on the initial position, the local target point, and the Euclidean distance, and the calculation formula is as follows (3):

[0057]

[0058] Among them, (x sub-goal ,y sub-goal ) represents the coordinates of the end target, (x n ,y n ) represents the coordinates of the n-th exploration point, and h(n) represents the heuristic value of the n-th exploration point.

[0059] In one feasible implementation, the adjusted heuristic value is more consistent with the actual heuristic value. Because Euclidean distance is calculated based on a straight line distance, which is the optimal case, but a straight line is not possible in reality, updating the heuristic value can make the heuristic value closer to the actual heuristic value.

[0060] S26. Determine the cost from the initial position to each exploration point based on the actual terrain cost, the initial heuristic value, and the RTAA* algorithm. Determine an intermediate target point among the multiple exploration points based on the cost of each exploration point.

[0061] In a feasible implementation, the cost function of S26 is as follows (4):

[0062] f(n)=g(n)+h(n)(4)

[0063] Among them, f(n) represents the cost value of the n-th exploration point, and g(n) represents the actual terrain cost.

[0064] After planning is completed, the output path point set is: {(P1, H1), (P2, H2), ..., (P n , H n )}, where each point carries elevation information H i , for subsequent swing arm angle prediction.

[0065] S27. Determine a local target point based on the initial heuristic value of each intermediate target point, and determine a local optimal path based on the local target point.

[0066] In one feasible implementation, after a phased exploration using the RTAA* algorithm at the initial position, a number of explored points are obtained. These points form a tree structure, with the starting point as the root node. The explored point with the smallest heuristic value is then selected as the local target point. The path is then traced back to the starting point, node by node, through this local target point. This backtracked path is ultimately considered the local optimal path.

[0067] S28. Dynamically adjust the heuristic value of each path point in the local optimal path of the RTAA* algorithm according to the actual terrain cost from the initial position to the local target point and the initial heuristic value.

[0068] In one feasible implementation, the adjusted heuristic value is more consistent with the actual heuristic value. Because Euclidean distance is calculated based on a straight line distance, which is the optimal case, but a straight line is not possible in reality, updating the heuristic value can make the heuristic value closer to the actual heuristic value.

[0069] S3. For each path point in the local optimal path, the initial swing arm angle is predicted based on the skeleton model and the local elevation map, and a path point set is generated according to the path point and the corresponding initial swing arm angle.

[0070] Optionally, the specific operations of S3 may include the following S31-S33:

[0071] S31. With the robot's starting position as a reference, a skeleton model is established, and the kinematic equation of the skeleton model is determined; the skeleton model is a combination of multiple joints and connecting rods.

[0072] Optionally, the robot's starting position is used as a reference to establish its skeleton model. Assuming that the robot consists of two tracks and two swing arms, the skeleton model can be simplified to a combination of multiple joints and links. The skeleton model is expressed as the following formula (5):

[0073] skeleton={J1,J2,...,J i ,...,J m} (5)

[0074] Among them, J i represents the i-th joint;

[0075] The kinematic equation of the skeleton model is expressed as follows (6):

[0076]

[0077] Among them, L i represents the length of the link between the joints, θ i represents the angle between the joints, (x i ,y i , zi ) represents the three-dimensional coordinates of the i-th joint.

[0078] S32. Based on the local elevation map and the kinematic equation, the terrain plane is fitted using the least squares method to obtain the fitting plane parameters.

[0079] Optionally, S42 uses the least square method to fit the terrain plane based on the local elevation map and the kinematic equation to obtain fitting plane parameters. The fitting formula is as follows (7):

[0080] z=ax+by+c(7)

[0081] By solving formula (7), the fitting plane parameters are obtained as follows (8):

[0082]

[0083] in,

[0084] S33. Based on the skeleton model and the fitting plane parameters, calculate the initial swing arm angle and generate a set of path points including the initial swing arm angle, wherein the initial swing arm angle is the angle between the swing arm and the ground.

[0085] Optionally, S33 calculates the initial swing arm angle based on the skeleton model and the fitting plane parameters, and the calculation formula is as follows (9):

[0086]

[0087] Among them, θ s represents the initial swing arm angle, x′ represents the height of the current path point, y′ represents the horizontal coordinate of the current path point, and z′ represents the vertical coordinate of the current path point; the generated discrete path point set containing the initial swing arm angle is represented as {θ1, θ2, ..., θ s ,...,θ n}.

[0088] S4. Perform spline interpolation optimization on the discrete initial swing arm angles in the path point set to generate an optimized angle sequence.

[0089] In one feasible implementation, although step S3 above can calculate the swing arm angle for each path point, these angles are discrete and cannot directly meet the continuity requirements of the robot's actual motion. In order to optimize the swing arm control performance, a continuous processing method for the swing arm angle is proposed.

[0090] Optionally, the specific operations of S4 may include the following S41-S42:

[0091] S41. Perform spline interpolation on the discrete initial swing arm angle sequence to obtain an interpolated swing arm angle sequence to generate a continuous angle curve.

[0092] Optionally, in S41, spline interpolation is performed on the discrete initial swing arm angle sequence to obtain an interpolated swing arm angle sequence to generate a continuous angle curve, as shown in the following formula (10):

[0093]

[0094] Each spline S i (t) needs to satisfy the following constraint (11):

[0095] S i (t) = a i (tt i ) 3 +b i (tt i ) 2 +c i (tt i )+d i (11)

[0096] S42. Construct an evaluation function for the interpolated swing arm angle sequence to minimize the discontinuity of the swing arm motion, and obtain a continuous and smooth optimized angle sequence by solving the evaluation function.

[0097] Optionally, the optimization objective function constructed in S42 is as follows (12-1):

[0098]

[0099] It should be noted that during the angle optimization process, the swing arm angle is corrected based on real-time environmental information to ensure smoothness while maximally adapting to the current terrain characteristics, ultimately improving the robot's motion stability and obstacle-crossing capabilities. Minimizing the angle change rate while ensuring the swing arm's adaptability to the terrain is achieved as shown in the following equation (12-2):

[0100]

[0101] Where E(θ i ) represents the cost function of the angle to terrain adaptability, and λ is the weight factor.

[0102] S5. When executing path tracking, the swing arm advance amount is calculated according to the current speed of the swing arm crawler robot, the position of the next path point and the optimized angle corresponding to the next path point, and the swing arm crawler robot is controlled to swing its arm according to the swing arm advance amount and move to the next path point.

[0103] In one feasible implementation, traditional pure path-tracking algorithms primarily track the path by adjusting the track speed and direction based on the positional error between the target point and the current point. However, in a swing-arm tracked robot, because the path points contain information about the swing arm's angle, and the swing arm has motion lag, controlling the robot solely based on the actual position and angle of the path points can result in jerky motion or mismatched postures.

[0104] Optionally, in step S51, when performing path tracking, the swing arm lead amount is calculated based on the following formula (13) according to the current speed of the swing arm crawler robot, the position of the next path point, the optimized angle corresponding to the next path point, and the response delay time of the swing arm:

[0105] θ preemptive =θ target +k·v·Δt (13)

[0106] Among them, θ preemptive Indicates the swing arm advance amount, θ target represents the optimized angle corresponding to the next path point, k represents the adjustment coefficient, v represents the current speed, and Δt represents the response delay time.

[0107] It should be noted that in order to improve the adaptability and accuracy of the swing-arm crawler robot to the environment, during the path tracking process, the lead parameters k and Δt are dynamically adjusted by real-time acquisition of robot speed, swing-arm angle feedback, and environmental conditions. The adjustment is shown in the following formula (14):

[0108] k=f(terrain,robot_state),Δt=g(robot speed )(14)

[0109] Where f and g are empirical formulas or data-driven models.

[0110] S6. Repeat S5 until the swing-arm crawler robot reaches the local target point.

[0111] S7. Determine the local target point as the new initial position and repeat S2-S6. When the local target point is the final target, the path planning and exploration task of the swing-arm crawler robot is completed.

[0112] In a feasible implementation manner, the path planning and exploration of the swing-arm tracked robot includes multiple loop processes. First, after determining the local target point, S5 is executed in a loop until the swing-arm tracked robot reaches the local target point. Then, in order to make the swing-arm tracked robot reach the next local target point, the current local target point is determined as the new initial position, and S2-S6 is executed in a loop. And so on. After executing S2-S6 in a loop multiple times, the swing-arm tracked robot reaches the local target points one by one, until a local target point is the end target, that is, the swing-arm tracked robot reaches the end target, that is, the path planning and exploration task of the swing-arm tracked robot is completed.

[0113] It should be noted that the above steps S1-S7 can be Figure 1-2 The pseudo code is shown.

[0114] The embodiment of the present invention, based on a comprehensive consideration of the kinematic characteristics and climbing ability of a swing-arm tracked robot, adopts a swing-arm control method based on the robot skeleton model to obtain the swing-arm angle, thereby constructing an autonomous exploration framework for the swing-arm tracked robot in an unknown environment. This framework implements phased path planning by introducing the RTAA* algorithm, and combines dynamic prediction and optimization of the swing-arm angle to achieve efficient path tracking and autonomous exploration control. Given a target point and the robot's current posture, the method of the present invention can be used to calculate the robot's complete path from the starting point to the target point, and optimize the continuity of the swing-arm angle to ensure smooth control, thereby improving the efficiency and accuracy of the swing-arm tracked robot's exploration and control.

[0115] Figure 2 This is a block diagram of an autonomous exploration and control device for a swing-arm crawler robot according to an exemplary embodiment, and the device is used for an autonomous exploration and control method for a swing-arm crawler robot. Figure 2 The device includes an acquisition unit 210, a planning unit 220, a determination unit 230, a generation unit 240, a calculation unit 250, an exploration unit 260, and a loop execution unit 270.

[0116] An acquisition unit 210 is used to acquire the initial position and the end target of the swing-arm crawler robot;

[0117] A planning unit 220 is configured to perform exploration based on the initial position and the RTAA* algorithm to obtain a local target point, and obtain a local optimal path including multiple path points based on the local target point and the initial position;

[0118] a determination unit 230 for predicting, for each path point in the local optimal path, an initial swing arm angle based on the skeleton model and the local elevation map, and generating a path point set based on the path point and the corresponding initial swing arm angle;

[0119] A generating unit 240 is configured to perform spline interpolation optimization on the discrete initial swing arm angles in the path point set to generate an optimized angle sequence;

[0120] a calculation unit 250 for calculating a swing arm lead amount based on the current speed of the swing arm crawler robot, the position of the next path point, and the optimized angle corresponding to the next path point when performing path tracking, and controlling the swing arm crawler robot to swing its arm according to the swing arm lead amount and move to the next path point;

[0121] an exploration unit 260, configured to repeatedly execute the calculation unit 250 until the swing-arm crawler robot reaches a local target point;

[0122] The loop execution unit 270 is used to determine the local target point as a new initial position, repeatedly execute the planning unit 220 to the exploration unit 260, and when the local target point is the end target, complete the path planning and exploration task of the swing-arm crawler robot.

[0123] The embodiment of the present invention, based on a comprehensive consideration of the kinematic characteristics and climbing ability of a swing-arm tracked robot, adopts a swing-arm control method based on the robot skeleton model to obtain the swing-arm angle, thereby constructing an autonomous exploration framework for the swing-arm tracked robot in an unknown environment. This framework implements phased path planning by introducing the RTAA* algorithm, and combines dynamic prediction and optimization of the swing-arm angle to achieve efficient path tracking and autonomous exploration control. Given a target point and the robot's current posture, the method of the present invention can be used to calculate the robot's complete path from the starting point to the target point, and optimize the continuity of the swing-arm angle to ensure smooth control, thereby improving the efficiency and accuracy of the swing-arm tracked robot's exploration and control.

[0124] Figure 3 FIG. 1 is a schematic structural diagram of an autonomous exploration and control device for a swing-arm crawler robot provided by an embodiment of the present invention. Figure 3 As shown, the autonomous exploration and control device for the swing arm crawler robot may include the above Figure 3 The autonomous exploration and control device for a swing-arm crawler robot shown in FIG. Optionally, the autonomous exploration and control device 310 for a swing-arm crawler robot may include a first processor 2001 .

[0125] Optionally, the autonomous exploration and control device 310 for the swing-arm crawler robot may further include a memory 2002 and a transceiver 2003 .

[0126] The first processor 2001, the memory 2002 and the transceiver 2003 may be connected via a communication bus.

[0127] The following combination Figure 3 The components of the autonomous exploration and control device 310 for the swing-arm crawler robot are described in detail:

[0128] The first processor 2001 is the control center of the autonomous exploration and control device 310 for the swing-arm crawler robot, and can be a single processor or a collective term for multiple processing elements. For example, the first processor 2001 can be one or more central processing units (CPUs), or an application-specific integrated circuit (ASIC), or one or more integrated circuits configured to implement an embodiment of the present invention, such as one or more digital signal processors (DSPs) or one or more field programmable gate arrays (FPGAs).

[0129] Optionally, the first processor 2001 can perform various functions of the autonomous exploration and control device 310 for the swing-arm crawler robot by running or executing a software program stored in the memory 2002 and calling data stored in the memory 2002.

[0130] In a specific implementation, as an embodiment, the first processor 2001 may include one or more CPUs, such as Figure 3 CPU0 and CPU1 are shown in FIG.

[0131] In a specific implementation, as an embodiment, the autonomous exploration and control device 310 for the swing-arm crawler robot may also include multiple processors, such as Figure 3 1 and 2. The first processor 2001 and the second processor 2004 are shown in FIG. Each of these processors can be a single-core processor (single-CPU) or a multi-core processor (multi-CPU). A processor herein can refer to one or more devices, circuits, and / or processing cores for processing data (e.g., computer program instructions).

[0132] The memory 2002 is used to store the software program for executing the solution of the present invention, and is controlled by the first processor 2001 for execution. The specific implementation method can refer to the above method embodiment and will not be repeated here.

[0133] Alternatively, the memory 2002 may be a read-only memory (ROM) or other type of static storage device that can store static information and instructions, a random access memory (RAM) or other type of dynamic storage device that can store information and instructions, or an electrically erasable programmable read-only memory (EEPROM), a compact disc read-only memory (CD-ROM) or other optical disc storage, optical disc storage (including compact disc, laser disc, optical disc, digital versatile disc, Blu-ray disc, etc.), a magnetic disk storage medium or other magnetic storage device, or any other medium that can be used to carry or store desired program code in the form of instructions or data structures and can be accessed by a computer, but is not limited thereto. The memory 2002 may be integrated with the first processor 2001 or exist independently and be accessed through the interface circuit ( Figure 3 (not shown) is coupled to the first processor 2001, which is not specifically limited in this embodiment of the present invention.

[0134] The transceiver 2003 is used to communicate with a network device or a terminal device.

[0135] Optionally, the transceiver 2003 may include a receiver and a transmitter ( Figure 3 (not shown separately in the figure). The receiver is used to implement a receiving function, and the transmitter is used to implement a sending function.

[0136] Optionally, the transceiver 2003 may be integrated with the first processor 2001 or may exist independently and communicate with the first processor 2001 through the interface circuit ( Figure 3 (not shown) is coupled to the first processor 2001, which is not specifically limited in this embodiment of the present invention.

[0137] It should be noted that Figure 3 The structure of the autonomous exploration and control device 310 for the swing-arm crawler robot shown in the figure does not constitute a limitation on the router. The actual knowledge structure recognition device may include more or fewer components than shown in the figure, or combine certain components, or arrange the components differently.

[0138] In addition, the technical effects of the autonomous exploration and control device 310 for a swing-arm tracked robot can refer to the technical effects of the autonomous exploration and control method for a swing-arm tracked robot described in the above method embodiment, and will not be repeated here.

[0139] It should be understood that the first processor 2001 in the embodiment of the present invention may be a central processing unit (CPU), and the processor may also be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or any conventional processor, etc.

[0140] It should also be understood that the memory in the embodiments of the present invention may be a volatile memory or a non-volatile memory, or may include both volatile and non-volatile memories. Among them, the non-volatile memory may be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable programmable read-only memory (EEPROM), or a flash memory. The volatile memory may be a random access memory (RAM), which is used as an external cache. By way of example and not limitation, many forms of random access memory (RAM) are available, such as static RAM (SRAM), dynamic random access memory (DRAM), synchronous DRAM (SDRAM), double data rate synchronous dynamic random access memory (DDR SDRAM), enhanced synchronous dynamic random access memory (ESDRAM), synchronous link DRAM (SLDRAM), and direct rambus RAM (DR RAM).

[0141] The above embodiments can be implemented in whole or in part through software, hardware (such as circuits), firmware, or any other combination. When implemented using software, the above embodiments can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions or computer programs. When the computer instructions or computer program are loaded or executed on a computer, the processes or functions described in accordance with the embodiments of the present invention are generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via a wired method (such as infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server or data center that contains a collection of one or more available media. The available medium can be a magnetic medium (such as a floppy disk, hard disk, or magnetic tape), an optical medium (such as a DVD), or a semiconductor medium. The semiconductor medium can be a solid-state drive.

[0142] It should be understood that the term "and / or" as used herein simply describes a relationship between associated objects, indicating that three possible relationships exist. For example, "A and / or B" can represent: A alone, A and B together, or B alone. A and B can be singular or plural. Furthermore, the character " / " as used herein generally indicates an "or" relationship between the associated objects, but it may also indicate an "and / or" relationship. For specific understanding, please refer to the context.

[0143] In this disclosure, "at least one" means one or more, and "plurality" means two or more. "At least one of the following" or similar expressions refers to any combination of these items, including any combination of single or plural items. For example, "at least one of a, b, or c" can mean: a, b, c, ab, ac, bc, or abc, where a, b, and c can be single or plural.

[0144] It should be understood that in various embodiments of the present invention, the size of the serial numbers of the above-mentioned processes does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0145] Those skilled in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professionals and technicians can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of the present invention.

[0146] Those skilled in the art will clearly understand that, for the convenience and brevity of description, the specific working processes of the above-described equipment, devices and units can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0147] In the several embodiments provided by the present invention, it should be understood that the disclosed devices, apparatuses and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For example, the division of the units is merely a logical function division. In actual implementation, there may be other division methods, such as multiple units or components can be combined or integrated into another device, 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 interface, indirect coupling or communication connection of the device or unit, which can be electrical, mechanical or other forms.

[0148] The units described as separate components may or may not be physically separate, and 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 these units may be selected to achieve the purpose of this embodiment according to actual needs.

[0149] In addition, each functional unit in each embodiment of the present invention 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.

[0150] If the functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0151] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any modifications or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in the present invention should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. An autonomous exploration and control method for a swing-arm crawler robot, characterized in that: The method comprises: S1, obtain the initial position and end target of the swing-arm crawler robot; S2. Exploring based on the initial position and the RTAA* algorithm to obtain a local target point, and obtaining a local optimal path including multiple path points based on the local target point and the initial position; S3. For each path point in the local optimal path, predict the initial swing arm angle based on the skeleton model and the local elevation map, and generate a path point set based on the path point and the corresponding initial swing arm angle; S4, performing spline interpolation optimization on the discrete initial swing arm angles in the path point set to generate an optimized angle sequence; S5. When executing path tracking, calculating a swing arm lead amount based on the current speed of the swing arm crawler robot, the position of the next path point, and the optimized angle corresponding to the next path point, and controlling the swing arm crawler robot to swing its arm according to the swing arm lead amount and move to the next path point; S6, repeating S5 until the swing-arm crawler robot reaches the local target point; S7, determining the local target point as a new initial position, and repeating S2-S6. When the local target point is the end target, the path planning and exploration task of the swing-arm crawler robot is completed.

2. The autonomous exploration and control method for a swing-arm crawler robot according to claim 1, characterized in that: The S2 performs exploration based on the initial position and the RTAA* algorithm to obtain a local target point, and obtains a local optimal path including multiple path points based on the local target point and the initial position, including: S21. Generate an elevation map based on the point cloud data or the depth map; S22. Calculate the roughness of the local area based on the elevation map; S23, calculating the steepness of the path points based on the elevation map; S24. Determine the actual terrain cost from the initial position to each exploration point based on the roughness and steepness; S25. Calculate the initial heuristic value of each exploration point by the Euclidean distance between each exploration point and the final target; S26, determining a cost from the initial position to each exploration point based on the actual terrain cost, the initial heuristic value, and the RTAA* algorithm, and determining an intermediate target point among the multiple exploration points based on the cost of each exploration point; S27, determining a local target point based on the initial heuristic value of each intermediate target point, and determining a local optimal path based on the local target point; S28. Dynamically adjust the heuristic value of each path point in the local optimal path of the RTAA* algorithm according to the actual terrain cost from the initial position to the local target point and the initial heuristic value.

3. The autonomous exploration and control method for a swing-arm crawler robot according to claim 2, characterized in that: The roughness of the local area is calculated based on the elevation map in S22, and the calculation formula is as follows (1): Among them, σ(x, y) represents the roughness of the local area, h i Indicates the elevation value corresponding to the i-th coordinate point in the elevation map H(x,y), is the average elevation of the local area, and N represents the total number of coordinate points in the elevation map; The step S23 calculates the steepness of the path points based on the elevation map, and the calculation formula is as follows (2): Among them, G(x, y) represents the steepness of the path point, and H(x, y) represents the elevation map; The S25 calculates the initial heuristic value of each exploration point by the Euclidean distance between each exploration point and the end target, and the calculation formula is as follows (3): Among them, (x sub-goal ,y sub-goal ) represents the coordinates of the end target, (x n ,y n ) represents the coordinates of the n-th exploration point, and h(n) represents the heuristic value of the n-th exploration point; The calculation formula of the cost value of each exploration point in S26 is as follows (4): f(n)=g(n)+h(n) (4) Among them, f(n) represents the cost value of the n-th exploration point, and g(n) represents the actual terrain cost.

4. The autonomous exploration and control method for a swing-arm crawler robot according to claim 1, characterized in that: The step S3 predicts an initial swing arm angle for each path point in the local optimal path based on the skeleton model and the local elevation map, and generates a path point set according to the path point and the corresponding initial swing arm angle, including: S31. Using the robot's starting position as a reference, establish a skeleton model and determine a kinematic equation for the skeleton model; the skeleton model is a combination of multiple joints and connecting rods; S32. Based on the local elevation map and the kinematic equation, the terrain plane is fitted using the least squares method to obtain the fitting plane parameters; S33. Based on the skeleton model and the fitting plane parameters, calculate the initial swing arm angle and generate a set of path points including the initial swing arm angle, wherein the initial swing arm angle is the angle between the swing arm and the ground.

5. The autonomous exploration and control method for a swing-arm crawler robot according to claim 4, characterized in that: The skeleton model is expressed as the following formula (5): skeleton={J1,J2,...,J i ,...,J m } (5) Among them, J i represents the i-th joint; The kinematic equation of the skeleton model is expressed as the following formula (6): Among them, L i represents the length of the link between the joints, θ i represents the angle between the joints, (x i ,y i , z i ) represents the three-dimensional coordinates of the i-th joint; The S32 uses the least square method to fit the terrain plane based on the local elevation map and the kinematic equation to obtain the fitting plane parameters. The fitting formula is as follows (7): z=ax+by+c (7) By solving formula (7), the fitting plane parameters are obtained as follows (8): in, The S33 calculates the initial swing arm angle based on the skeleton model and the fitting plane parameters, and the calculation formula is as follows (9): Among them, θ s represents the initial swing arm angle, x′ represents the height of the current path point, y′ represents the horizontal coordinate of the current path point, and z′ represents the vertical coordinate of the current path point.

6. The autonomous exploration and control method for a swing-arm crawler robot according to claim 4, characterized in that: The step S4 performs spline interpolation optimization on the discrete initial swing arm angles in the path point set to generate an optimized angle sequence, including: S41, performing spline interpolation on the discrete initial swing arm angle sequence to obtain an interpolated swing arm angle sequence to generate a continuous angle curve; S42. Construct an evaluation function for the interpolated swing arm angle sequence to minimize the discontinuity of the swing arm motion, and obtain a continuous and smooth optimized angle sequence by solving the evaluation function.

7. The autonomous exploration and control method for a swing-arm crawler robot according to claim 6, characterized in that: The evaluation function is as follows (10): Among them, θ(t) represents the interpolated swing arm angle sequence, t n Represents the total number of elements in the interpolated swing arm angle sequence; S42 obtains a continuous and smooth optimized angle sequence by solving the evaluation function, including: After the evaluation value J is calculated, the evaluation value J is compared with a preset threshold. If the evaluation value J is less than or equal to the preset threshold, the interpolated swing arm angle sequence is determined as the optimized angle sequence. If the evaluation value J is greater than the preset threshold, the evaluation value of the modified swing arm angle sequence is made less than or equal to the preset threshold by adding or subtracting the angles in the interpolated swing arm angle sequence.

8. The autonomous exploration and control method for a swing-arm crawler robot according to claim 1, characterized in that: The step S5, when executing path tracking, calculates the arm swing advance amount according to the current speed of the arm swing type crawler robot, the position of the next path point, and the optimized angle corresponding to the next path point, and controls the arm swing type crawler robot to swing according to the arm swing advance amount, including: S51. When executing path tracking, according to a certain period, based on the current speed of the swing-arm crawler robot, the position of the next path point, the optimized angle corresponding to the next path point, and the response delay time of the swing arm, the swing arm advance amount is calculated based on the following formula (11): i preemptive =θ target +k·v·Δt (11) Among them, θ preemptive Indicates the swing arm advance amount, θ target represents the optimized angle corresponding to the next path point, k represents the adjustment coefficient, v represents the current speed, and Δt represents the response delay time.

9. An autonomous exploration and control device for a swing-arm crawler robot, wherein the autonomous exploration and control device for a swing-arm crawler robot is used to implement the autonomous exploration and control method for a swing-arm crawler robot according to any one of claims 1 to 8, characterized in that: The device comprises: An acquisition unit, used to acquire the initial position and end point target of the swing-arm crawler robot; The planning unit is used to explore based on the initial position and the RTAA* algorithm to obtain a local target point, and then obtain a local optimal path including multiple path points based on the local target point and the initial position; a determination unit, configured to predict, for each path point in the local optimal path, an initial swing arm angle based on the skeleton model and the local elevation map, and generate a path point set according to the path point and the corresponding initial swing arm angle; A generation unit, used to perform spline interpolation optimization on the discrete initial swing arm angles in the path point set to generate an optimized angle sequence; a calculation unit for calculating a swing arm lead amount according to the current speed of the swing arm crawler robot, the position of the next path point, and the optimized angle corresponding to the next path point when performing path tracking, and controlling the swing arm crawler robot to swing its arm according to the swing arm lead amount and move to the next path point; an exploration unit, configured to repeatedly execute the calculation unit until the swing-arm crawler robot reaches a local target point; The cyclic execution unit is used to determine the local target point as a new initial position, repeatedly execute the planning unit to the exploration unit, and when the local target point is the end target, complete the path planning and exploration task of the swing-arm crawler robot.

10. An autonomous exploration and control device for a swing-arm crawler robot, characterized in that: The autonomous exploration and control device for the swing-arm crawler robot includes: processor; A memory having computer-readable instructions stored thereon, wherein when the computer-readable instructions are executed by the processor, the method according to any one of claims 1 to 8 is implemented.

Images