Motion planning method and related device

Abstract

The invention provides a motion planning method which can be applied to a motion planning scene. The method comprises the steps that the first position of a movable mechanical arm, the second position of an obstacle and the third position of a terminal point are obtained, and the movable mechanical arm comprises a chassis and a mechanical arm body; and a first path is planned, and the first path is used for indicating the movable mechanical arm to move from the first position to the third position and avoid the second position in the moving process. And constructing a trajectory optimization problem for the first path based on the differential flatness characteristic, and solving the trajectory optimization problem to obtain a second path. According to the method and the device, the trajectory optimization problem is constructed based on the differential flatness characteristic, so that partial differential constraint conditions can be eliminated, and the trajectory optimization problem is effectively subjected to dimensionality reduction, thereby simplifying the solving process, improving the solving speed, and further improving the trajectory optimization efficiency and success rate.

Description

Technical Field

[0001] This application relates to the field of motion planning, and more particularly to a motion planning method and related apparatus. Background Art

[0002] "Mobile + Manipulation Technology" is a key technological direction in the evolution of robotics. Traditional fixed robotic arms have greatly improved the operational efficiency of various production workshops, but they require customized implementation in structured environments and can only handle point-to-point operations within limited operating spaces. Traditional mobile robots, on the other hand, possess the ability to move in large spaces, but they cannot complete various operational tasks. In recent years, mobile manipulation robots (i.e., mobile robotic arms) have emerged, attracting increasing attention due to their combination of manipulation and mobility capabilities, enabling them to provide operational services in large-scale environments.

[0003] However, the obstacle avoidance capabilities of commonly used mobile robotic arms are clearly insufficient, which seriously affects their commercial application. Summary of the Invention

[0004] This application provides a motion planning method and related apparatus. Based on the differential flatness property, a trajectory optimization problem is constructed, which can effectively reduce the dimensionality of the trajectory optimization problem, thereby simplifying the solution process, reducing the solution difficulty, improving the solution speed, and thus improving the efficiency and success rate of trajectory optimization.

[0005] The first aspect of this application provides a motion planning method applicable to a motion planning device. The method includes: obtaining a first position of a mobile robotic arm, a second position of an obstacle, and a third position of a destination, wherein the mobile robotic arm includes a chassis and a robotic arm; planning a first path, the first path being used to instruct the mobile robotic arm to move from the first position to the third position, and to avoid the second position during the movement; constructing a trajectory optimization problem for the first path based on the differential flatness property; and solving the trajectory optimization problem to obtain a second path.

[0006] The motion planning device acquires position information through sensing devices, including the position of the mobile robotic arm and the positions of surrounding obstacles. The motion planning device can be a standalone device, a module of the mobile robotic arm, or the mobile robotic arm itself. The sensing devices can be part of the motion planning device or independent of it. These devices include odometry and sensors. The motion planning device determines a first position using the odometry and acquires the second position of obstacles using the sensors. The odometry can be a wheeled odometry, laser odometry, or visual odometry, and the sensors can be lidar, ultrasonic radar, or infrared radar. In addition to sensors, the sensing devices can also be cameras, which the motion planning device uses to determine the second position of obstacles. When performing a movement operation, the motion planning device receives the third position of the target destination. It is understood that the mobile robotic arm includes a chassis and a robotic arm; therefore, the first position includes the current position of the chassis and the position of the robotic arm, and the third position includes the endpoint position of the chassis and the end effector position of the robotic arm.

[0007] A first path is planned based on the first, second, and third positions. This first path allows the mobile robotic arm to move from the first position to the third position while avoiding the second position. The first path is a preliminary plan, mainly to quickly obtain a feasible (i.e., a path that reaches the destination without collision) front-end path. Therefore, the first path may have problems such as being tortuous, having a long route, taking a lot of time, and not meeting kinematic constraints.

[0008] After obtaining the first path, it is optimized. Specifically, a trajectory optimization problem is constructed based on the differential flatness property of the first path. The differential flatness property means that the original state and input variables can be algebraically represented by the flat output and its derivative, thereby simplifying complex motion systems. After constructing the trajectory optimization problem, solving the trajectory optimization problem yields the optimized second path.

[0009] In the first aspect of this application, the trajectory optimization problem is constructed based on the differential flatness property, which can eliminate some differential constraints, effectively reduce the dimensionality of the trajectory optimization problem, thereby simplifying the solution process, reducing the solution difficulty, and improving the solution speed, thereby improving the trajectory optimization efficiency and success rate, and improving the response speed of the mobile robotic arm in the trajectory optimization process.

[0010] In one possible implementation of the first aspect, the above steps include: constructing a trajectory optimization problem for the first path based on the differential flatness property, including:

[0011] With z = [σ T ,q T ] T To achieve a flat output, the trajectory optimization problem is constructed using the following formula:

[0012]

[0013] Where σ represents the chassis position information, q represents the joint angle of the robotic arm, T represents the time allocation, and J represents the time distribution. s J is a smoothness penalty term used to improve the smoothness of the first path. T J is a time penalty term used to shorten the time taken by the first path. d J is a dynamic feasibility penalty term used to ensure that the mobile robotic arm meets dynamic constraints. c This is a safety penalty measure used to prevent the mobile robotic arm from colliding with obstacles and with the chassis. λ T For J T The weighting coefficient, λ d For J d The weighting coefficient, λ c For J c The weighting coefficients.

[0014] This possible implementation method limits the way the trajectory optimization problem is constructed, improving the feasibility of the solution. Furthermore, specific penalty terms are proposed: a smoothness penalty term makes the obstacle avoidance path smoother and more coherent; a time penalty term shortens the trajectory time; a dynamic feasibility penalty term ensures the mobile robotic arm meets dynamic constraints; and a safety penalty term prevents collisions between the mobile robotic arm and obstacles after trajectory optimization, as well as collisions between the mobile robotic arm and itself.

[0015] In one possible implementation of the first aspect, J d This includes linear acceleration penalties for the chassis, angular acceleration penalties, and joint angular acceleration penalties for the robotic arm.

[0016] The solution process can be reduced in dimensionality by using the differential flatness property, and higher-order quantities can be represented as nonlinear combinations of flat spaces. Therefore, compared with conventional trajectory optimization methods, this embodiment can use higher-order constraint terms, such as linear acceleration penalty terms, angular acceleration penalty terms, and joint angular acceleration penalty terms of the robotic arm. Higher-order penalty terms can better constrain the motion state of the chassis and the robotic arm.

[0017] In one possible implementation of the first aspect, the above steps: planning the first path include: planning an obstacle avoidance path for the chassis, the obstacle avoidance path of the chassis being used to instruct the chassis to move from a first position to a third position, and to avoid a second position during the movement; planning an obstacle avoidance path for the robotic arm based on the obstacle avoidance path of the chassis, the obstacle avoidance path of the robotic arm being used to instruct the robotic arm to move from a first position to a third position, and to avoid a second position during the movement; and obtaining the first path based on the obstacle avoidance paths of the chassis and the robotic arm.

[0018] In this possible implementation, a hierarchical planning approach is used to plan the first path, that is, first plan the obstacle avoidance path of the chassis, and then plan the obstacle avoidance path of the robotic arm based on the obstacle avoidance path of the chassis, which can quickly realize the overall trajectory planning.

[0019] In one possible implementation of the first aspect, the above steps: obtaining a first path based on the obstacle avoidance paths of the chassis and the robotic arm include: determining whether the obstacle avoidance paths of the chassis and the robotic arm have been successfully planned; if so, obtaining a first path based on the obstacle avoidance paths of the chassis and the robotic arm; if not, modeling the chassis and the robotic arm to obtain a first model; and using a random sampling algorithm to search for a path from the first position to the third position by the first model, avoiding the second position, to obtain the first path.

[0020] In this possible implementation, when the hierarchical planning method fails, the first path is planned using full-state sampling, that is, the entire mobile robotic arm is sampled and feasible trajectories are searched, thus combining the efficiency of the hierarchical planning method and the completeness of the full-state sampling method.

[0021] In one possible implementation of the first aspect, the above steps, namely solving the trajectory optimization problem, include: solving the trajectory optimization problem using a memory-constrained quasi-Newton method LBFGS optimizer.

[0022] Optionally, based on the differential flatness property, some differential constraints can be eliminated. Therefore, a more efficient LBFGS optimizer can be used to solve the trajectory optimization problem, thereby further accelerating the solution speed, which in turn speeds up the response speed of the mobile robotic arm and improves the real-time performance of the solution.

[0023] In one possible implementation of the first aspect, the chassis is a two-wheel differential chassis and the robotic arm is a 6-DOF lightweight robotic arm.

[0024] Two-wheel differential chassis are widely used chassis in daily life, and 6-DOF lightweight robotic arms are also common robotic arms in industrial production. Using two-wheel differential chassis and 6-DOF lightweight robotic arms as examples is more representative and has more practical application significance.

[0025] A second aspect of this application provides a motion planning device, including an acquisition unit, a planning unit, a construction unit, and a solution unit. The acquisition unit is used to acquire a first position of a mobile robotic arm, a second position of an obstacle, and a third position of a destination, wherein the mobile robotic arm includes a chassis and a robotic arm itself. The planning unit is used to plan a first path, which instructs the mobile robotic arm to move from the first position to the third position, avoiding the second position during the movement. The construction unit is used to construct a trajectory optimization problem based on the differential flatness property of the first path. The solution unit is used to solve the trajectory optimization problem to obtain a second path.

[0026] In one possible implementation of the second aspect, the building unit is specifically used for: with z = [σ T ,q T ] T To achieve a flat output, the trajectory optimization problem is constructed using the following formula:

[0027]

[0028] Where σ represents the chassis position information, q represents the joint angle of the robotic arm, T represents the time allocation, and J represents the time distribution. s J is a smoothness penalty term used to improve the smoothness of the first path. T J is a time penalty term used to shorten the time taken by the first path. d J is a dynamic feasibility penalty term used to ensure that the mobile robotic arm meets dynamic constraints. c This is a safety penalty measure used to prevent the mobile robotic arm from colliding with obstacles and with the chassis. λ T For J T The weighting coefficient, λ d For J d The weighting coefficient, λ c For J c The weighting coefficients.

[0029] In one possible implementation of the second aspect, J d This includes linear acceleration penalties for the chassis, angular acceleration penalties, and joint angular acceleration penalties for the robotic arm.

[0030] In one possible implementation of the second aspect, the planning unit is specifically used to: plan an obstacle avoidance path for the chassis, the obstacle avoidance path of the chassis being used to instruct the chassis to move from a first position to a third position, and to avoid a second position during the movement; plan an obstacle avoidance path for the robotic arm based on the obstacle avoidance path of the chassis, the obstacle avoidance path of the robotic arm being used to instruct the robotic arm to move from a first position to a third position, and to avoid a second position during the movement; and obtain a first path based on the obstacle avoidance paths of the chassis and the robotic arm.

[0031] In one possible implementation of the second aspect, the planning unit is specifically used to: determine whether the obstacle avoidance path of the chassis and the obstacle avoidance path of the robotic arm are successfully planned; if so, obtain a first path based on the obstacle avoidance path of the chassis and the obstacle avoidance path of the robotic arm; if not, model the chassis and the robotic arm to obtain a first model; and use a random sampling algorithm to search for a path from the first position to the third position by avoiding the second position, thereby obtaining the first path.

[0032] In one possible implementation of the second aspect, the solving unit is specifically used to solve the trajectory optimization problem using a memory-constrained quasi-Newton method LBFGS optimizer.

[0033] In one possible implementation of the second aspect, the chassis is a two-wheel differential chassis, and the robotic arm is a 6-DOF lightweight robotic arm.

[0034] The mobile robotic arm provided in the second aspect of this application is used to perform the methods described in the first aspect or any possible implementation thereof.

[0035] A third aspect of this application provides a motion planning device, including a processor and a memory. The memory is used to store instructions, and the processor is used to retrieve the instructions stored in the memory to execute the method described in the first aspect or any possible implementation thereof.

[0036] A fourth aspect of this application provides a computer-readable storage medium comprising instructions that, when executed on a computer, cause the computer to perform the method described in the first aspect or any possible implementation thereof.

[0037] The fifth aspect of this application provides a computer program product containing instructions that, when the computer program product is run on a computer, cause the computer to perform the method described in the first aspect or any possible implementation thereof.

[0038] The sixth aspect of this application provides a chip system including at least one processor and a communication interface, the communication interface and the at least one processor being interconnected via a line, the at least one processor being configured to run a computer program or instructions to perform the method described in the first aspect or any possible implementation thereof. Attached Figure Description

[0039] Figure 1 This is a schematic diagram illustrating an application scenario of the motion planning method provided in an embodiment of this application;

[0040] Figure 2 A schematic diagram of an embodiment of the motion planning method provided in this application;

[0041] Figure 3 This is a schematic diagram of the first path in an embodiment of this application;

[0042] Figure 4 This is a schematic diagram of the second path in an embodiment of this application;

[0043] Figure 5 A schematic diagram of another embodiment of the motion planning method provided in this application;

[0044] Figure 6 A schematic diagram of the motion planning device provided in the embodiments of this application;

[0045] Figure 7 This is another structural schematic diagram of the motion planning device provided in an embodiment of this application. Detailed Implementation

[0046] This application provides a motion planning method that can improve the efficiency and success rate of trajectory optimization. This application also provides corresponding devices, computer-readable storage media, and computer program products. These will be described separately below.

[0047] The embodiments of this application are described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of them. As those skilled in the art will understand, with the development of technology and the emergence of new scenarios, the technical solutions provided by the embodiments of this application are also applicable to similar technical problems.

[0048] The terms “path” and “trajectory,” “movement” and “motion” used in the specification, claims, and accompanying drawings of this application are interchangeable. Unless otherwise specified, ordinal numbers such as “first,” “second,” etc., are used to distinguish multiple objects and are not used to define the order, sequence, priority, or importance of the multiple objects. It should be understood that such terms are interchangeable where appropriate so that the embodiments described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms “comprising” and “having,” and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that comprises a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0049] To facilitate understanding, the relevant terms and concepts mainly involved in the embodiments of this application will be introduced below.

[0050] 1. Differential flatness

[0051] In differentially flat systems, the original state and input variables can be algebraically represented by flat outputs and their derivatives, thus simplifying complex motion systems. This representation can be achieved using a set of flat outputs and their derivatives. Based on the property of differential flatness, dimensionality reduction can be effectively achieved in motion planning, thereby simplifying the solution of trajectory optimization problems.

[0052] 2. Fast Search Random Tree

[0053] Rapidly-exploring random tree (RRT) is a random sampling algorithm that uses incremental growth to solve high-dimensional space problems with algebraic and differential constraints. The advantages of RRT are that it does not require system modeling or geometric partitioning of the search region, offers high coverage of the search space, and allows for a wide search range, exploring unknown regions as much as possible.

[0054] 3.LBFGS

[0055] The Limited-memory Broyden-Fletcher-Goldfarb-Shanno (LBFGS) algorithm is a method for solving nonlinear optimization problems, proposed by Broyden, Fletcher, Goldfarb, and Shanno. LBFGS is a quasi-Newton method, and therefore can also be called a memory-constrained quasi-Newton method. A solver using the LBFGS algorithm is called an LBFGS optimizer. LBFGS has advantages such as fast convergence speed and low memory overhead, and is often used to solve unconstrained nonlinear programming problems.

[0056] 4. A* Algorithm

[0057] The A* (A* star) algorithm is a heuristic search algorithm for solving path planning problems. It performs particularly well on two-dimensional grid maps and is widely used in indoor robot path search, game animation path search, and other fields. During the search process, the A* algorithm uses a heuristic function that combines the cost of already searched paths with the estimated cost from the current state to the target state, helping the algorithm find the target solution more efficiently. The estimated cost in the heuristic function is usually calculated based on distance formulas, such as Manhattan distance or Euclidean distance. The A* algorithm's workflow starts from the starting point and searches all possible states in a certain order, evaluating them each time. When the target state is found, the algorithm stops and outputs the optimal solution from the starting point to the target state. The advantages of the A* algorithm are that it not only guarantees that the found solution is optimal but also effectively prunes the path during the search process, avoiding the search for invalid states.

[0058] Please refer to the following: Figure 1This is a schematic diagram of an application scenario of the motion planning method provided in the embodiments of this application.

[0059] like Figure 1 As shown, this application scenario includes a mobile robotic arm and several obstacles. The mobile robotic arm consists of a chassis and a robotic arm, where the chassis provides mobility and the robotic arm provides operational capabilities, enabling the mobile robotic arm to perform movement tasks. It is understandable that... Figure 1 The number of mobile robotic arms and obstacles is for illustrative purposes only, and no specific number is specified here.

[0060] A task execution process is defined as a mobile robotic arm moving from its current position A to a designated target position B. Target position B includes the endpoints of both the chassis and the robotic arm. During this journey, the robotic arm must avoid collisions with obstacles, requiring advance planning of an obstacle avoidance path. Currently, path planning methods primarily include hierarchical planning and full-state sampling. Hierarchical planning first uses a trajectory search algorithm to find feasible paths for the chassis, then determines the robotic arm's path based on these feasible paths. While hierarchical planning can quickly plan obstacle avoidance paths, situations arise where the chassis can safely pass through, but no feasible path exists for the robotic arm. For example, the chassis might safely pass under a cabinet, but the robotic arm, even retracted to its lowest height, would still collide with the cabinet, rendering hierarchical planning ineffective. Full-state sampling, on the other hand, involves modeling both the chassis and the robotic arm uniformly, then using a random sampling algorithm to search for a path where both can safely avoid obstacles. Full-state sampling yields high-quality obstacle avoidance paths but requires more planning time.

[0061] Furthermore, after planning the initial path of the mobile robotic arm, the initial path can be optimized to obtain a coherent, smooth trajectory that satisfies kinematic constraints. However, due to the high degree of freedom and strong nonlinearity of the mobile robotic arm, the optimization problem of constructing the initial path is often very complex, leading to optimization failures and requiring a long time to solve, resulting in a slow response speed.

[0062] In view of this, embodiments of this application provide a motion planning method that uses the differential flatness property to perform differential parameterization on the constructed trajectory optimization problem, eliminates some differential constraints, thereby effectively reducing the dimensionality of the complex trajectory optimization problem, simplifying the solution process of the trajectory optimization problem, accelerating the solution speed, and thus improving the success rate of trajectory optimization and the response speed of the mobile robotic arm.

[0063] Furthermore, in the search for the initial feasible path of the mobile robotic arm, the embodiments of this application combine hierarchical planning and full-state sampling methods, which not only enables fast search but also ensures the success rate of trajectory planning, making the solution both efficient and complete.

[0064] The motion planning method provided in this application can be applied to scenarios such as home mobile operation service robots providing services (e.g., grasping, cleaning), material loading and unloading in manufacturing plants, data center node loading and unloading, and mobile charging robots charging electric vehicles. No specific limitations are made here regarding the application scenarios.

[0065] Please refer to the following: Figure 2 , Figure 2 This is a schematic diagram of one embodiment of the motion planning method provided in this application. Figure 2 As shown, this embodiment includes steps 201 to 204.

[0066] 201. Obtain the first position of the mobile robotic arm, the second position of the obstacle, and the third position of the endpoint, wherein the mobile robotic arm includes a chassis and a robotic arm.

[0067] The motion planning device acquires position information through sensing devices, including the location of the mobile robotic arm and the locations of surrounding obstacles. The motion planning device can be a standalone device, a module of the mobile robotic arm, or the mobile robotic arm itself. The sensing devices can be part of the motion planning device or separate from it. These sensing devices include odometry and sensors. The motion planning device uses the odometry to determine the initial position of the mobile robotic arm and the sensors to acquire the secondary positions of obstacles. The odometry can be a wheeled odometry, laser odometry, or visual odometry, etc., and the sensors can be lidar, ultrasonic radar, or infrared radar, etc. In addition to sensors, the sensing devices can also be cameras; the mobile robotic arm uses the camera to determine the secondary positions of obstacles.

[0068] When performing a movement operation task, the motion planning device receives the third position of the given destination target. After obtaining the third position, the motion planning device begins to plan an obstacle avoidance path from the first position to the third position.

[0069] It is understandable that the mobile robotic arm includes a chassis and a robotic arm. Therefore, the first position includes the current position of the chassis and the position of the robotic arm, and the third position includes the target position of the chassis and the end position of the robotic arm.

[0070] In one possible design, the chassis is a two-wheel differential chassis, and the robotic arm is a lightweight robotic arm with 6 degrees of freedom. A two-wheel differential chassis and a 6-DOF lightweight robotic arm have a wide range of applications. Furthermore, the differential flatness characteristic provides a greater boost to the differential chassis. Of course, the chassis could also be a four-wheel differential chassis or an Ackermann chassis, and the robotic arm could be 3-DOF or 5-DOF; the specific design is not limited here.

[0071] 202. Plan the first path, which is used to instruct the mobile robotic arm to move from the first position to the third position, and to avoid the second position during the movement.

[0072] Based on the first, second, and third positions, a first path is planned. Moving the robotic arm along this first path allows it to reach the third position without colliding with obstacles at the second position. It is understood that this first path is a preliminary plan; its purpose is simply to reach the third position without colliding with obstacles.

[0073] After obtaining the location of obstacles, a three-dimensional (3D) occupancy grid map can be generated based on these locations. The 3D occupancy grid map divides the environment in which the mobile robotic arm resides into numerous grids. Occupied grids indicate the presence of obstacles, while empty grids indicate the absence of obstacles. For example, a grid with a value of 1 indicates the presence of an obstacle, while a grid with a value of 0 indicates the absence of an obstacle. Using a 3D occupancy grid map can improve path planning efficiency.

[0074] In one possible approach, the first path is planned using a hierarchical planning method, combining the aforementioned 3D occupancy grid map. Specifically, the obstacle avoidance path for the chassis is planned first, instructing the chassis to move from the first position to the third position while avoiding collisions with obstacles at the second position. Then, based on the chassis's obstacle avoidance path, the obstacle avoidance path for the robotic arm is planned, instructing the robotic arm to move from the first position to the third position while avoiding collisions with obstacles at the second position. In other words, a feasible path for the robotic arm to move from the first to the third position is found from the chassis's obstacle avoidance path. Since the moving robotic arm includes both the chassis and the arm itself, the combination of the chassis's obstacle avoidance path and the robotic arm's obstacle avoidance path constitutes the first path. The hierarchical planning method will be explained in detail below.

[0075] Specifically, a trajectory search algorithm is first used to generate the chassis obstacle avoidance path in a 3D occupancy grid map. The chassis obstacle avoidance path includes n segments of forward and reverse paths, including... There are path points, namely: Among them, M j p is the segment number of the j-th path in n paths. i =[σ i,x ,σ i,y ,ψ i ] T Let i = 0, ..., N be the planning variables at chassis path point i, and σ be the variable at path point i. i,x Let σ be the position of the chassis on the x-axis of the two-dimensional coordinate system. i,y Let ψ be the position of the chassis along the y-axis in a two-dimensional coordinate system. iLet p0 be the chassis's orientation angle and T be the time allocation. p0 represents the initial state of the chassis in its first position. N This represents the final state of the chassis at the third position. The trajectory search algorithm can be A* (A star) or hybrid A* (hybrid A star), etc.

[0076] After obtaining the path points for the chassis obstacle avoidance path, a suitable robotic arm state is found at each path point, and a state grid diagram is constructed. In the hierarchical planning method, the robotic arm's state is attached to the chassis path points. Therefore, the state is attached to the chassis path point p. i The state of the robotic arm can only be determined by the chassis path point p. i；1 (i.e. p) i The robotic arm moves from the previous path point (p), and if it continues to move, it will reach the chassis path point p. i:1 (i.e. p) i The state of the robotic arm is the next path point. Therefore, the state grid diagram is a hierarchical structure, where the moving chassis path point p i The corresponding robotic arm state is referred to as being in the i-th layer. The purpose of layering the robotic arm motion states is to find the corresponding robotic arm state for each moving chassis path point, given the initial state (i.e., the current state at the first position) and the final state (i.e., the end state at the set third position) of the robotic arm.

[0077] Optionally, the obstacle avoidance path of the robotic arm can be planned based on a bidirectional rapidly-exploring random tree (RRT*-connect) algorithm. RRT*-connect uses two rapidly-exploring random trees (RRTs), growing bidirectionally from the start and end points for each sample, thereby improving the convergence rate, accelerating path planning, and ultimately shortening the response time of the moving robotic arm. Of course, other algorithms can also be used to plan the obstacle avoidance path of the robotic arm, such as RRT, in which case only one random tree is used. The specific algorithm is not limited here.

[0078] The two random trees are the starting random tree and the ending random tree, respectively. The first position is set as the root node q0 of the starting random tree, and the third position is set as the root node q of the ending random tree. N Each node in the random tree represents the state of the robotic arm at a chassis path point. A robotic arm state q is randomly sampled. rand and number of layers h rand Then, at the starting point, the h-th random tree rand Searching for distance q within the layer rand The nearest internal node q near Record from q near to qrand direction Let q near Growing one step in this direction yields q. new q new =q near +dir×step, where step is a custom step size. q new The number of layers is defined as q near The next level below the current level, if q near and q new If the connection condition is met, then q new and edge (q) near q new Add it to the starting random tree. This step is called q. near To q rand directional growth.

[0079] If q new If the node is at the maximum or minimum level of the starting random tree, then one growth cycle of the starting random tree ends. If q new If it is not at the maximum or minimum level of the starting random tree, then for q new Perform rerouting. Specifically, consider q. new The next level node q next If from the starting node through q new Then q next The path dissipation is greater than that from the current starting node to q. next If the path dissipation is small, then modify q. next The parent node is q new .

[0080] Determine q new Then, search for q in the endpoint random tree. new The nearest neighbor, denoted as q near；1 Let q near；1 To q new directional growth yields q new；1 and for q new；1 Perform rewiring. Let q near；1 For q new；1 and repeat for q new；1 Perform rewiring until q new；1 =q new At this point, q is on the random tree at the destination. near；1 q successfully grows to the starting random tree new In other words, the starting random tree and the ending random tree were successfully connected, and the location of the robotic arm at point h was found. rand A feasible path for layer q. new；1 =q newIf this happens, growth fails; in other words, the robotic arm's obstacle avoidance path cannot be planned based on the chassis's obstacle avoidance path. This requires finding the robotic arm's obstacle avoidance path at h... rand After establishing an obstacle avoidance path in the layer, swap the starting random tree and the ending random tree, and find the distance q from the ending random tree. rand The nearest node, the starting point of the random tree grows towards that node, and then the robotic arm is checked at h. rand Are there better obstacle avoidance paths, such as shorter paths? After comparison, select the better path for the robotic arm at h. rand Obstacle avoidance paths at each layer. By swapping two random trees, we can increase diversity, improve the success rate of path planning, and obtain higher quality paths.

[0081] Subsequently, the obstacle avoidance paths of the robotic arm on other layers were planned using the same method. After finding the obstacle avoidance paths for the robotic arm on each layer, the obstacle avoidance path for the robotic arm was obtained. in If an obstacle avoidance path for the robotic arm cannot be found on a certain layer, it means that the obstacle avoidance path for the robotic arm cannot be planned based on the obstacle avoidance path of the chassis, and the layered planning method fails.

[0082] After obtaining the obstacle avoidance paths of the chassis and the robotic arm, combining them yields the obstacle avoidance path of the mobile robotic arm (i.e., the first path).

[0083] Hierarchical planning methods can quickly plan the first path, but they may fail in some scenarios. For example, they may be unable to plan the obstacle avoidance path of the robotic arm based on the obstacle avoidance path of the chassis, or even the obstacle avoidance path of the chassis itself. Therefore, this embodiment combines hierarchical planning and full-state sampling methods to ensure the success rate of path planning while achieving rapid obstacle avoidance path planning. A detailed explanation follows.

[0084] In one possible approach, the obstacle avoidance path for the chassis is first planned. Then, it's determined whether the chassis obstacle avoidance path planning is successful. If the planning fails, a full-state sampling method is used to plan the first path. If the planning is successful, the obstacle avoidance path for the robotic arm is planned based on the chassis's path. The success of the robotic arm's obstacle avoidance path planning is then determined. If the robotic arm's path planning is successful, the first path is obtained, and the initial obstacle avoidance path planning process ends. If the robotic arm's obstacle avoidance path planning fails, a full-state sampling method is used to plan the first path. Full-state sampling involves sampling the degrees of freedom of the chassis and robotic arm, creating a unified model for both, resulting in a first model. Then, a random sampling algorithm is used to search for a path that allows the first model to move from its current first position to a second position, avoiding obstacles, and reach a third position. This path is the first path. The random sampling algorithm can be a bidirectional heuristic RRT* (inform-RRT*-connect), RRT, or RRT*, etc., which are not specifically limited here. The following uses inform-RRT*-connect as an example to illustrate the process of planning the first path through full-state sampling.

[0085] First, set up two random trees: a starting random tree and an ending random tree. The root node of the starting random tree represents the state of the mobile robot in its first position, and the root node of the ending random tree represents the state in its designated third position. The states of the mobile robot include the position of the chassis, the orientation angle, and the robot's configuration. Let the planning variable s = 0σ. x ,σ y ,ψ,q1,…,q61 T , where σ x σ represents the position of the chassis along the x-axis in a two-dimensional coordinate system. y This represents the position of the chassis on the y-axis in a two-dimensional coordinate system, ψ is the orientation angle of the chassis, and q1 to q6 are random tree nodes.

[0086] Randomly sample a state S rand Find the distance S within the random tree at the starting point. rand The nearest in-tree node S near Then let S near To S rand Growth yields S new Next, search for S within the endpoint random tree. new nearest neighbor S near1 Then let S near1 To S new Growth yields S new1 Repeat S within the endpoint random tree. near1 To S new The growth process continues until S near1 =S new1At this point, a feasible path is found. If the cost of this feasible path is less than the cost of the current optimal path, then this feasible path is adopted as the optimal path. Then, the starting random tree and the ending random tree are swapped, and the path with distance S is searched from the ending random tree. rand For the nearest node, grow the starting random tree towards that node to see if there's a better path. Swapping two random trees increases diversity, improves the success rate of path planning, and yields higher-quality paths. Repeat this process until the set termination condition is reached. If a collision-free path from the first to the third position is found, the planning is successful, and a feasible path is returned. Among them, s i =[σ i,x ,σ i,y ,ψ i ,q i,1 ,…,q i,6 ] T =[s i,1 ,s i,2 ,s i,3 ,s i,4 ,…,s i,9 ] T .

[0087] The first path obtained from the planning can be combined Figure 3 To understand. For example Figure 3 As shown, 301 to 307 are schematic diagrams of the mobile robotic arm at different positions and configurations during movement. 301 is the starting point of the mobile robotic arm (position one), and 307 is the target point of the mobile robotic arm (position three). The first path for the mobile robotic arm moving from 301 to 307 is the trajectory from 301 to 307. 308 represents the obstacle table (position two). To avoid collision with the table during movement, the mobile robotic arm passes under it, retracting downwards during this process. 304 to 306 illustrate the process of passing under the table.

[0088] 203. Constructing a trajectory optimization problem for the first path based on the differential flatness property.

[0089] The first path is a preliminary planned path, mainly to quickly obtain a feasible (i.e., a collision-free path that reaches the destination) initial path. Therefore, the first path may have problems such as being tortuous, long, time-consuming, and not meeting kinematic constraints. After obtaining the first path, it can be optimized. However, due to the high degree of freedom of the mobile robotic arm, the constructed optimization problem is often quite complex and the solution speed is slow. In view of this, this embodiment constructs a trajectory optimization problem for the initial path based on the differential flatness property, eliminates some constraints of the motion system, reduces the dimensionality of the trajectory optimization problem, thereby simplifying the solution process, accelerating the solution speed, and thus improving the response speed of the mobile robotic arm.

[0090] In one possible solution, z = [σ] T ,q T ] T To achieve a flat output, the trajectory optimization problem is constructed using the following formula:

[0091]

[0092] Where σ represents the position information of the chassis, q represents the joint angle of the robotic arm, T represents the time allocation, and J represents the position information of the robotic arm. s J is a smoothness penalty term used to improve the smoothness of the first path. T J is a time penalty term used to shorten the time taken by the first path. d J is a dynamic feasibility penalty term used to ensure that the mobile robotic arm meets dynamic constraints. c This is a safety penalty measure used to prevent the mobile robotic arm from colliding with obstacles and with the chassis. λ T For J T The weighting coefficient, λ d For J d The weighting coefficient, λ c For J c The weight coefficients are denoted by min, which represents the minimum value of the above penalty term.

[0093] Of course, not all four penalty terms mentioned above need to be used, or they can be combined with other penalty terms to construct the trajectory optimization formula; specifics are not limited here. For example, without the time penalty term, the formula would be: Alternatively, without incorporating a smoothness penalty term, the formula is: Without a time penalty, the process might take longer, for example, causing the robotic arm to take a roundabout route. Without a smoothness penalty, issues such as a more winding path and disjointed movements may occur.

[0094] The dimensionality of the solution process can be reduced by leveraging the differential flatness property, representing higher-order quantities as nonlinear combinations of flat spaces. Therefore, compared to conventional trajectory optimization methods, this embodiment can use higher-order constraint terms. For example, in addition to the linear velocity and angular velocity of the chassis and the joint angular velocity of the robotic arm, the dynamic feasibility penalty term can also include higher-order penalty terms such as the linear acceleration and angular acceleration penalty terms of the chassis and the joint angular acceleration of the robotic arm. These higher-order penalty terms can better constrain the motion of the chassis and the robotic arm.

[0095] The following section uses a trajectory optimization problem constructed with smoothness penalty, time penalty, dynamic feasibility penalty, and safety penalty as an example to illustrate the specific calculation process. The chassis is a two-wheel differential chassis, and the robotic arm is a lightweight robotic arm with 6 degrees of freedom.

[0096] Let the optimization variable of the trajectory optimization problem be its flat output. And time allocation T, This indicates that the mobile robotic arm includes two degrees of freedom for the chassis and six degrees of freedom for the robotic arm, totaling eight degrees of freedom (i.e., 8-dimensional). This indicates the position of the chassis in two-dimensional space. This represents the six joint angles of the robotic arm, and T represents the time at which the chassis position and joint angles are allocated. By combining chassis position, joint angles, and time allocation, both temporal and spatial dimensions can be considered simultaneously, achieving spatiotemporal joint optimization.

[0097] The state of the moving robotic arm can be obtained from the flat output z and its derivatives: Where ψ is the chassis orientation angle, v is the chassis linear velocity, and a l Let ω be the linear acceleration of the chassis, ω be the angular velocity of the chassis, and α be the angular acceleration of the chassis. Let be the joint angular velocity of the robotic arm. This refers to the joint angular acceleration of the robotic arm.

[0098] The first path obtained in step 202 includes n forward and reverse routes. Each route is defined by M... i The path consists of 8-dimensional 7th-degree polynomial functions, i ∈ {1,…,n}. Alternatively, it can be represented using 6th-degree or 5th-degree polynomial functions; the specific representation is not limited. The polynomial coefficients of the i-th polynomial path are... The corresponding time allocation is

[0099] Let β(t) = [1, t, t 2 ,…,t 7 ] T Then the j-th polynomial trajectory of the i-th segment of the moving robotic arm's trajectory can be expressed as:

[0100]

[0101] make

[0102]

[0103] The i-th segment of the trajectory of the moving robotic arm is represented as:

[0104]

[0105] make

[0106]

[0107] in The path of the moving robotic arm can then be represented as:

[0108]

[0109] Then, construct a trajectory optimization problem based on z(t). Where, λ T ,λ d ,λ c These are the weighting coefficients for the time penalty, dynamic feasibility penalty, and safety penalty, respectively. The weighting coefficients are set based on actual conditions and expert experience; for example, to move further away from the obstacle, λ is set accordingly. c Set it to a larger value. To reduce the time required, increase λ. T Set it to a larger value.

[0110] For the smoothness penalty term J s The desired outcome is to minimize the path's acceleration.

[0111]

[0112] in, Indicate z ij The fourth derivative of (t), express The norm of .

[0113] For the time penalty item J T The desired outcome is to minimize the trajectory time.

[0114]

[0115] Dynamic feasibility penalty term J d Ensure that the chassis and robotic arm meet the dynamic constraints:

[0116]

[0117]

[0118] in, κ represents the number of constraint points in each polynomial trajectory segment.

[0119]

[0120] This is the set of dimensions for the dynamic constraints of the mobile robotic arm, comprising both the chassis and the robotic arm. Where v, a r ,a l ,ω r ,ω l ,α r ,α l These represent the maximum linear velocity, minimum linear velocity, maximum linear acceleration, minimum linear acceleration, maximum angular velocity, minimum angular velocity, maximum angular acceleration, and minimum angular acceleration of the two-wheel differential chassis. θ , q ω ,q α These are the maximum joint angle, minimum joint angle, maximum joint angular velocity, and maximum joint angular acceleration of the robotic arm, respectively. The specific dynamic feasibility penalty for the chassis is as follows:

[0121]

[0122]

[0123]

[0124]

[0125]

[0126]

[0127]

[0128] The specific penalty for the dynamic feasibility of the robotic arm is as follows:

[0129]

[0130]

[0131]

[0132]

[0133] Security penalty item J c Ensure the mobile robotic arm avoids collisions with obstacles and with the chassis.c The formula for the penalty term is:

[0134] J c =J c,o +J c,s

[0135] Among them, J c,o J c,s This is a safety penalty for the mobile robotic arm itself.

[0136] Optionally, the overall shape of the mobile robotic arm is non-convex, meaning it cannot be compactly enclosed by a cuboid. For motion planning problems, collision detection on non-convex shapes is very complex. Therefore, the mobile robotic arm is decomposed using spheres, i.e., it is enclosed by numerous spheres of different radii. Collision detection then simply checks whether a sphere will collide with an obstacle; that is, it compares the sphere's radius with the distance from the robotic arm to the obstacle. If the sphere's radius is greater than the distance to the obstacle, a collision will not occur.

[0137] Optionally, based on the obstacle information obtained in step 201 and the 3D occupancy grid map established in step 202, a Euclidean Signed Distance Field (ESDF) can be further generated. The value of each grid cell in the ESDF represents the Euclidean distance between that cell and the nearest obstacle. Euclidean distance refers to straight-line distance; for example, the distance between coordinates [0,0] and [3,4] in a coordinate system is 5. Through the ESDF, the distance between each surrounding location and the nearest obstacle can be determined, meaning the positions of obstacles in the surrounding environment can be known.

[0138] After decomposing the mobile robotic arm into a sphere, the pose of the chassis and robotic arm joints in three-dimensional space can be obtained through the forward kinematics of the mobile robotic arm:

[0139]

[0140] in, w T b (σ,ψ) represents the chassis pose in the world frame. This represents the center positions of n0 spheres covering the chassis in the chassis coordinate system. Let represent the coordinates of the center of the j-th sphere on the i-th arm of the robotic arm in the coordinate system of the i-th joint of the robotic arm. Then, the positions of the centers of all spheres covering the moving robotic arm in the world frame can be represented as:

[0141]

[0142] The safety penalty for the mobile robotic arm and obstacles in the ESDF map is as follows:

[0143]

[0144] in,

[0145]

[0146] E(p) is ESDF value at d s For a safe distance.

[0147] make

[0148]

[0149] The safety penalty for the mobile robotic arm itself is as follows:

[0150]

[0151] in,

[0152] The first path obtained in step 202 includes n forward or backward paths, specifically: Let T = {T1, ..., T} N} represents the initial time allocation for each path segment, where T i =max{∥[Δs i,1 ,Δs i,2 ]∥ / v max ,|Δs i,4 | / q 1,max ,…,|Δs i,9 | / q 6,max}, Δs i,j =s i,j -s i；1,j ,i∈{1,…,N}.

[0153] After obtaining the trajectory optimization formula based on the flat output representation, the initial value of the trajectory optimization formula is... By finding T and then solving the trajectory optimization formula, the optimized obstacle avoidance path can be obtained.

[0154] 204. Solve the trajectory optimization problem to obtain the second path.

[0155] Solve the construction optimization problem based on the differential flatness property in step 203, or in other words, solve the trajectory optimization formula based on the flat output representation, to obtain the second path.

[0156] Optionally, based on the differential flatness property, some differential constraints can be eliminated. Therefore, an efficient LBFGS optimizer can be used to solve the trajectory optimization problem, thereby further accelerating the solution speed and thus speeding up the response speed of the mobile robotic arm and improving the real-time performance of the solution. Alternatively, the trajectory optimization problem can also be solved using an interior point optimizer (IPOPT), etc., without further limitation here.

[0157] After obtaining the second path, the mobile robotic arm follows the second path to reach the set endpoint (i.e., the third position) and executes the operation task. Since the surrounding environment and the positions of obstacles may change, the mobile robotic arm periodically checks whether the second path still meets various penalty conditions during movement, such as whether it still satisfies collision-free and dynamic constraints. If it does, it continues moving along the second path; if not, it returns to step 201 to replan the obstacle avoidance path.

[0158] The optimized second path can be combined Figure 4 To understand. For example Figure 4 As shown, the trajectory from 401 to 402 is the second path. 401 represents the initial state of the moving robotic arm, and 402 represents its final state. The robotic arm moves from 401 to 402. The process from 401 to 402 includes schematic diagrams of the robotic arm's different configurations at different positions. 403 represents an obstacle table; the robotic arm passes under the table during its journey from 401 to 402, thus avoiding a collision. Figure 3 compared to, Figure 4 The obstacle avoidance path is significantly more coherent and smoother, and the trajectory is shorter, thus requiring less time. For example, Figure 3 Lines 302 and 306 involve taking an unsmooth turn, resulting in a more convoluted and longer path. The convoluted path affects movement speed and safety, and the longer trajectory further increases the time required.

[0159] In this embodiment, the trajectory optimization problem is constructed based on the differential flatness property, which can eliminate some differential constraints, effectively reduce the dimensionality of the trajectory optimization problem, thereby simplifying the solution process, reducing the solution difficulty, and improving the solution speed. This, in turn, improves the trajectory optimization efficiency and success rate, and enhances the response speed of the mobile robotic arm in the trajectory optimization process.

[0160] Optionally, the front-end path (i.e. the first path) can be planned by combining hierarchical planning and full-state sampling. That is, when hierarchical planning fails, the first path can be planned by full-state sampling, so as to balance efficiency and completeness. It can achieve rapid planning of obstacle avoidance path while ensuring the success rate of path planning.

[0161] In summary, the following is a combination of... Figure 5 The motion planning method provided in the embodiments of this application will be described by way of example.

[0162] like Figure 5 As shown, the motion planning device first obtains the position of the mobile robotic arm and the positions of surrounding obstacles through sensing devices. For example, the position of the mobile robotic arm is sensed by an odometer, and the positions of obstacles are sensed by sensors or cameras. After obtaining the position information, localization mapping is performed to construct a 3D occupancy grid map and an ESDF (Electrical Safety Datasheet). Then, based on the 3D occupancy grid map, a front-end path (i.e., the first path) is planned using two methods: hierarchical planning and full-state sampling. Specifically, the obstacle avoidance path of the chassis is first searched based on the 3D occupancy grid map, and then the obstacle avoidance path of the robotic arm is planned based on the obstacle avoidance path of the chassis. If the planning fails, for example, if a path that the robotic arm can avoid obstacles cannot be found based on the obstacle avoidance path of the chassis, joint modeling and sampling of the chassis and the robotic arm are performed to search for the obstacle avoidance path of the entire mobile robotic arm. After obtaining the first path through hierarchical planning or full-state sampling, a trajectory optimization problem is constructed for the first path based on the differential flatness property, combined with the whole-body kinematic model. The trajectory optimization problem can include multiple cost terms such as safety penalty, dynamic feasibility penalty, smoothness penalty, and time penalty. The safety penalty term ensures the mobile robotic arm avoids collisions with obstacles and itself. Obstacle avoidance conditions are obtained using ESDF (Electrical Safety Factor). Specifically, the mobile robotic arm is decomposed into a sphere; if the ESDF value of a grid cell is greater than the radius of the sphere, the cell will not collide with an obstacle. The dynamic feasibility penalty term ensures the mobile robotic arm meets dynamic constraints. The smoothness penalty term makes the obstacle avoidance path more coherent and smooth, while the time penalty term minimizes path travel time. After constructing the trajectory optimization problem, a nonlinear optimizer, such as LBFGS, is used to solve it, resulting in the optimized obstacle avoidance path (i.e., the second path). After obtaining the second path, the overall controller, combined with its own position obtained from the odometry, outputs an obstacle avoidance strategy, which is executed by the actuator, enabling the mobile robotic arm to move from the first position to the third position without collision.

[0163] It is understood that the motion planning method provided in this application embodiment can be applied not only to mobile robotic arms but also to drones, without specific limitations on the application scenario. When applied to a drone, the first position is the drone's position, and the first path is used to instruct the drone to avoid the second position and reach the third position from the first position.

[0164] Correspondingly, the motion planning device provided in this application embodiment can be applied to mobile robotic arms or devices such as drones, and is not specifically limited here.

[0165] The embodiments of this application have been described above from the perspective of methodology. The relevant devices in the embodiments of this application will be introduced below from the perspective of specific device implementation.

[0166] Please see Figure 6 This application provides a schematic diagram of a motion planning device 600. The motion planning device 600 includes an acquisition unit 601, a planning unit 602, a construction unit 603, and a solution unit 604.

[0167] The acquisition unit 601 is used to acquire the first position of the mobile robotic arm, the second position of the obstacle, and the third position of the endpoint, wherein the mobile robotic arm includes a chassis and a robotic arm.

[0168] Planning unit 602 is used to plan a first path, which instructs the mobile robotic arm to move from a first position to a third position and avoid a second position during the movement.

[0169] Construction unit 603 is used to construct a trajectory optimization problem for the first path based on the differential flatness property.

[0170] Solver 604 is used to solve the trajectory optimization problem and obtain the second path.

[0171] Optionally, building unit 603 is specifically used for z = [σ T ,q T ] T To achieve a flat output, the trajectory optimization problem is constructed using the following formula:

[0172]

[0173] Where σ represents the chassis position information, q represents the joint angle of the robotic arm, T represents the time allocation, and J represents the time distribution. s J is a smoothness penalty term used to improve the smoothness of the first path. T J is a time penalty term used to shorten the time taken by the first path. d J is a dynamic feasibility penalty term used to ensure that the mobile robotic arm meets dynamic constraints. c This is a safety penalty measure used to prevent the mobile robotic arm from colliding with obstacles and with the chassis. λ T For J T The weighting coefficient, λ d For J d The weighting coefficient, λ c For J c The weighting coefficients.

[0174] Optional, J d This includes linear acceleration penalties for the chassis, angular acceleration penalties, and joint angular acceleration penalties for the robotic arm.

[0175] Optionally, the planning unit 602 is specifically used to plan the obstacle avoidance path of the chassis, which is used to instruct the chassis to move from the first position to the third position and avoid the second position during the movement; based on the obstacle avoidance path of the chassis, the obstacle avoidance path of the robotic arm is planned, which is used to instruct the robotic arm to move from the first position to the third position and avoid the second position during the movement; a first path is obtained based on the obstacle avoidance path of the chassis and the obstacle avoidance path of the robotic arm.

[0176] Optionally, the planning unit 602 is specifically used to determine whether the obstacle avoidance path of the chassis and the obstacle avoidance path of the robotic arm have been successfully planned; if so, a first path is obtained based on the obstacle avoidance path of the chassis and the obstacle avoidance path of the robotic arm; if not, the chassis and the robotic arm are modeled to obtain a first model; a random sampling algorithm is used to search for a path of the first model from the first position to avoid the second position and reach the third position to obtain the first path.

[0177] Optionally, the solver unit 604 is specifically used to solve the trajectory optimization problem using the memory-constrained quasi-Newton method LBFGS optimizer.

[0178] Optionally, the chassis is a two-wheel differential chassis, and the robotic arm is a 6-DOF lightweight robotic arm.

[0179] The modules in the motion planning device 600 perform as described above. Figure 2 and Figure 5 The operation of the motion planning device in the illustrated embodiment will not be described in detail here.

[0180] Please refer to the following: Figure 7 This is a possible structural diagram of a motion planning device 700 provided in an embodiment of this application, including a processor 701, a communication interface 702, a memory 703, and a bus 704. The processor 701, communication interface 702, and memory 703 are interconnected via the bus 704. In the embodiments of this application, the processor 701 is used to control and manage the actions of the data processing device; for example, the processor 701 is used to execute... Figure 2 The steps performed by the motion planning device in the illustrated method embodiment are shown. Communication interface 702 is used to support communication by the motion planning device. Memory 703 is used to store the program code and data of the motion planning device.

[0181] The processor 701 can be a central processing unit, a general-purpose processor, a digital signal processor, an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), or other programmable logic devices, transistor logic devices, hardware components, or any combination thereof. It can implement or execute various exemplary logic blocks, modules, and circuits described in conjunction with the disclosure of this application. The processor can also be a combination that implements computing functions, such as a combination of one or more microprocessors, a combination of a digital signal processor and a microprocessor, etc. The bus 704 can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. The bus can be divided into address bus, data bus, control bus, etc. For ease of representation, Figure 7 The bus is represented by a single thick line, but this does not mean that there is only one bus or one type of bus.

[0182] This application also provides a computer-readable storage medium, which includes instructions that, when executed on a computer, cause the computer to perform the aforementioned actions. Figure 2 and Figure 5 The method in the illustrated embodiment.

[0183] This application also provides a computer program product containing instructions, which, when run on a computer, causes the computer to execute the aforementioned... Figure 2 and Figure 5 The method in the illustrated embodiment.

[0184] This application also provides a chip system, which includes at least one processor and a communication interface. The communication interface and the at least one processor are interconnected via a circuit. The at least one processor is used to run computer programs or instructions to perform the aforementioned... Figure 2 and Figure 5 The method in the illustrated embodiment.

[0185] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0186] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems, devices, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here.

[0187] In the several embodiments provided in this application, it should be understood that the disclosed systems, apparatuses, and methods 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 an indirect coupling or communication connection between apparatuses or units through some interfaces, and may be electrical, mechanical, or other forms.

[0188] 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.

[0189] In addition, the functional units in the various embodiments of the present application may be integrated into a single processing unit, or each unit may exist physically separately, or two or more units may be integrated into a single unit. The aforementioned integrated units may be implemented in the form of hardware or software functional units.

[0190] 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 medium. 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 storage medium 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 described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

Claims

1. A motion planning method, characterized in that, The method includes: The first position of the mobile robotic arm, the second position of the obstacle, and the third position of the endpoint are obtained, wherein the mobile robotic arm includes a chassis and a robotic arm; A first path is planned, which is used to instruct the mobile robotic arm to move from the first position to the third position, and to avoid the second position during the movement; A trajectory optimization problem is constructed for the first path based on the differential flatness property; The trajectory optimization problem is solved to obtain the second path.

2. The method according to claim 1, characterized in that, The trajectory optimization problem based on the differential flatness property for the first path includes: With z = [σ T ,q T ] T To achieve a flat output, the trajectory optimization problem is constructed using the following formula: Wherein, σ represents the position information of the chassis, q represents the joint angle of the robotic arm, T represents the time allocation, and J represents the position information of the robotic arm. s J is a smoothness penalty term used to improve the smoothness of the first path. T J is a time penalty term used to shorten the time taken by the first path. d J is a dynamic feasibility penalty term used to ensure that the mobile robotic arm meets dynamic constraints. c As a safety penalty, it is used to prevent the mobile robotic arm from colliding with the obstacle and to prevent the robotic arm from colliding with the chassis. The λ T For the J T The weighting coefficient, λ d For the J d The weighting coefficient, λ c For the J c The weighting coefficients.

3. The method according to claim 2, characterized in that, The J d This includes the linear acceleration penalty term and angular acceleration penalty term of the chassis, as well as the joint angular acceleration penalty term of the robotic arm.

4. The method according to any one of claims 1 to 3, characterized in that, The first planned path includes: Plan the obstacle avoidance path of the chassis, which is used to instruct the chassis to move from the first position to the third position, and to avoid the second position during the movement; The obstacle avoidance path of the robotic arm is planned based on the obstacle avoidance path of the chassis. The obstacle avoidance path of the robotic arm is used to indicate that the robotic arm moves from the first position to the third position and avoids the second position during the movement. The first path is obtained based on the obstacle avoidance path of the chassis and the obstacle avoidance path of the robotic arm.

5. The method according to claim 4, characterized in that, The first path is obtained by combining the obstacle avoidance path based on the chassis and the obstacle avoidance path based on the robotic arm, including: Determine whether the obstacle avoidance paths of the chassis and the robotic arm have been successfully planned; If so, the first path is obtained based on the obstacle avoidance path of the chassis and the obstacle avoidance path of the robotic arm; If not, then the chassis and the robotic arm are modeled to obtain the first model; The first path is obtained by using a random sampling algorithm to search for a path from the first position to the third position, bypassing the second position.

6. The method according to any one of claims 1 to 5, characterized in that, Solving the trajectory optimization problem includes: The trajectory optimization problem was solved using the memory-constrained quasi-Newton method LBFGS optimizer.

7. The method according to any one of claims 1 to 6, characterized in that, The chassis is a two-wheel differential chassis, and the robotic arm is a 6-DOF lightweight robotic arm.

8. A motion planning device, characterized in that, include: An acquisition unit is used to acquire the first position of the mobile robotic arm, the second position of the obstacle, and the third position of the endpoint, wherein the mobile robotic arm includes a chassis and a robotic arm; A planning unit is used to plan a first path, which instructs the mobile robotic arm to move from the first position to the third position, and to avoid the second position during the movement. The construction unit is used to construct a trajectory optimization problem for the first path based on the differential flatness property; The solving unit is used to solve the trajectory optimization problem and obtain the second path.

9. The motion planning device according to claim 8, characterized in that, The building unit is specifically used for: With z = [σ T ,q T ] T To achieve a flat output, the trajectory optimization problem is constructed using the following formula: Wherein, σ represents the position information of the chassis, q represents the joint angle of the robotic arm, T represents the time allocation, and J represents the position information of the robotic arm. s J is a smoothness penalty term used to improve the smoothness of the first path. T J is a time penalty term used to shorten the time taken by the first path. d J is a dynamic feasibility penalty term used to ensure that the mobile robotic arm meets dynamic constraints. c As a safety penalty, it is used to prevent the mobile robotic arm from colliding with the obstacle and to prevent the robotic arm from colliding with the chassis. The λ T For the J T The weighting coefficient, λ d For the J d The weighting coefficient, λ c For the J c The weighting coefficients.

10. The motion planning device according to claim 9, characterized in that, The J d This includes the linear acceleration penalty term and angular acceleration penalty term of the chassis, as well as the joint angular acceleration penalty term of the robotic arm.

11. The motion planning device according to any one of claims 8 to 10, characterized in that, The planning unit is specifically used for: Plan the obstacle avoidance path of the chassis, which is used to instruct the chassis to move from the first position to the third position, and to avoid the second position during the movement; The obstacle avoidance path of the robotic arm is planned based on the obstacle avoidance path of the chassis. The obstacle avoidance path of the robotic arm is used to indicate that the robotic arm moves from the first position to the third position and avoids the second position during the movement. The first path is obtained based on the obstacle avoidance path of the chassis and the obstacle avoidance path of the robotic arm.

12. The motion planning device according to claim 11, characterized in that, The planning unit is specifically used for: Determine whether the obstacle avoidance paths of the chassis and the robotic arm have been successfully planned; If so, the first path is obtained based on the obstacle avoidance path of the chassis and the obstacle avoidance path of the robotic arm; If not, then the chassis and the robotic arm are modeled to obtain the first model; The first path is obtained by using a random sampling algorithm to search for a path from the first position to the third position, bypassing the second position.

13. The motion planning device according to any one of claims 8 to 12, characterized in that, The solution unit is specifically used for: The trajectory optimization problem was solved using the memory-constrained quasi-Newton method LBFGS optimizer.

14. The motion planning device according to any one of claims 8 to 13, characterized in that, The chassis is a two-wheel differential chassis, and the robotic arm is a 6-DOF lightweight robotic arm.

15. A motion planning device, characterized in that, include: Processor and memory; The memory is used to store instructions; The processor is configured to execute instructions stored in the memory to implement the method according to any one of claims 1 to 7.

16. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by one or more processors, it implements the method as described in any one of claims 1 to 7.

17. A computer program product containing instructions, characterized in that, When the computer program product is run on a computer, it causes the computer to perform the method as described in any one of claims 1 to 7.

Images