Obstacle avoidance method for mobile mechanical arm and related device

Abstract

The invention discloses a mobile mechanical arm obstacle avoidance method and a related device, which are applied to a mobile mechanical arm obstacle avoidance scene. The method comprises the following steps: determining first environment information, wherein the first environment information indicates the position of an obstacle; and then a first track is planned according to the first environment information, and the movable mechanical arm does not collide with the obstacle under the first track. And updating the first environment information to obtain second environment information. And based on the second environment information, an optimal control problem is constructed by combining trajectory tracking cost and obstacle avoidance cost to optimize the first trajectory, the trajectory tracking cost is used for predicting whether the mobile mechanical arm will collide with the obstacle, and the obstacle avoidance cost is used for indicating the mobile mechanical arm to avoid the obstacle during movement. According to the method and the device, the feasibility of the first track can be analyzed according to the constructed optimal control problem, and whether collision occurs under the first track or not is predicted, so that obstacle avoidance measures can be taken in advance, and the obstacle avoidance capability of the mobile mechanical arm is improved.

Description

Technical Field

[0001] The present application relates to the technical field of mobile robot arm control, and in particular to a mobile robot arm obstacle avoidance method and related devices. Background Art

[0002] "Mobility + operation technology" is the key technical direction of the evolution of robotics technology. Traditional fixed robotic arm operations have greatly improved the operating efficiency of various production workshops, but they need to be customized in a structured environment and can only handle point-to-point operation problems within a limited operating space. Traditional mobile robots have the ability to move in large spaces, but they cannot complete various operation tasks. The mobile operating robots (i.e., mobile robotic arms) that have emerged in recent years have attracted more and more attention because they have both operational and mobility capabilities and can provide operating services in large scenes.

[0003] However, the obstacle avoidance capabilities of common mobile robotic arms are obviously insufficient, which seriously affects their commercial applications. Summary of the invention

[0004] The present application provides a mobile robotic arm obstacle avoidance method and related devices, which can improve the overall obstacle avoidance capability of the mobile robotic arm.

[0005] A first aspect of the present application provides a mobile robotic arm obstacle avoidance method, which can be applied to an obstacle avoidance device, wherein the mobile robotic arm includes a chassis and a robotic arm, and the method includes: determining first environmental information, the first environmental information is used to indicate the position of an obstacle; planning a first trajectory based on the first environmental information, the first trajectory is used to instruct the mobile robotic arm to avoid obstacles when moving; updating the first environmental information to obtain second environmental information; based on the second environmental information, combining the trajectory tracking cost and the obstacle avoidance cost to construct an optimal control problem to optimize the first trajectory, wherein the trajectory tracking cost is used to predict whether the mobile robotic arm will collide with an obstacle, and the obstacle avoidance cost is used to instruct the mobile robotic arm to avoid obstacles when moving.

[0006] In the first aspect of the present application, the feasibility of the first trajectory can be analyzed based on the constructed optimal control problem, and it is predicted whether a collision will occur under the first trajectory, so that obstacle avoidance measures can be taken in advance, thereby improving the obstacle avoidance capability of the mobile robot arm.

[0007] In a possible implementation manner of the first aspect, the step of optimizing the first trajectory by constructing an optimal control problem based on the second environment information and combining the trajectory tracking cost and the obstacle avoidance cost includes:

[0008] The optimal control problem is constructed based on the following formula:

[0009]

[0010] st

[0011] u min <u<u max

[0012] x min <x<x max

[0013] The parameters in the above formula are obtained based on the second environment information, where: is the penalty term at the end point of the first trajectory, x is the state parameter of the mobile robot, which includes the moving angle of the chassis and the joint angle of the robot, T is the time, and x T is the state parameter under T, x init is the first trajectory, Q is the penalty weight, o(x) is the obstacle avoidance cost, (x init ) T Q(xx init ) is the trajectory tracking cost, st is the constraint condition of the optimal control problem, and u is the control parameter of the mobile robot, which includes the linear velocity and angular velocity of the chassis and the joint velocity of the robot.

[0014] In this possible implementation, the method of constructing the optimal control problem is limited, which improves the feasibility of the solution.

[0015] In a possible implementation manner of the first aspect, the optimal control problem further includes a motion smoothing cost, and the motion smoothing cost is used to smooth the first trajectory.

[0016] In order to avoid the first trajectory being too tortuous, which makes the mobile robot arm very unsmooth during execution, the optimal control problem can be further constructed in combination with the motion smoothness cost. The motion smoothness cost is used to smooth the first trajectory, so that the mobile robot arm can achieve low-jerk agile movement.

[0017] In a possible implementation manner of the first aspect, the optimal control problem also includes an operability optimization cost, and the operability optimization cost is used to improve the operability of the robot arm.

[0018] In this possible implementation, the optimal control problem is constructed in combination with the maneuverability optimization cost. The maneuverability optimization cost is used to improve the maneuverability of the robot arm, thereby preventing the robot arm from falling into a singular state (if the robot arm is singular, it cannot exert force on the object) and further improving the overall dexterity of the mobile robot arm.

[0019] In a possible implementation manner of the first aspect, the method further includes: solving the optimal control problem through differential dynamic programming to obtain a second trajectory.

[0020] By solving the optimal control problem through differential dynamic programming, the high-dimensional optimal control problem (hundreds or thousands of dimensions) can be reduced to a series of low-dimensional optimal control problems (several or dozens of dimensions), thereby greatly improving the speed of solving the optimal control problem and enabling faster trajectory replanning, ensuring collision-free operation of the mobile robotic arm and further improving the obstacle avoidance capability of the mobile robotic arm.

[0021] In a possible implementation manner of the first aspect, the step of updating the first environment information includes updating the first environment information when the position of the obstacle changes.

[0022] In this possible implementation, an incremental update method is adopted, that is, the first environmental information is updated after the environmental information is perceived to have changed. Compared with the traditional method of re-acquiring environmental information in each frame update, this incremental update method is more efficient and faster, can save a lot of computing workload, and can also further improve the efficiency of trajectory replanning.

[0023] In a possible implementation of the first aspect, the above-mentioned step: determining the first environmental information includes: obtaining first point cloud information, the first point cloud information including information of the mobile robotic arm and information of the obstacle; filtering out the information of the mobile robotic arm in the first point cloud information to obtain second point cloud information; generating a Euclidean signed distance field of the obstacle based on the second point cloud information; and determining the first environmental information based on the Euclidean signed distance field.

[0024] First, obtain the first point cloud information, which can be obtained through a sensor or a camera, etc. The first point cloud information includes the point cloud information of the mobile robotic arm body and the point cloud information of the surrounding obstacles. Filter out the point cloud information of the mobile robotic arm in the first point cloud information to avoid treating the mobile robotic arm as an obstacle, so that the information of the obstacle can be obtained more clearly. After filtering out the point cloud information of the mobile robotic arm, the second point cloud information is obtained, and an occupancy grid map is generated based on the second point cloud information. According to the state of each grid in the occupancy grid map, it can be known which positions have obstacles. For example, a grid with a value of 1 indicates that there is an obstacle there, and a grid with a data of 0 indicates that there is no obstacle there.

[0025] After the occupancy grid map is generated, the Euclidean Signed Distance Field (ESDF) of the obstacle is further generated based on the second point cloud information. The value of each grid in the ESDF stores the Euclidean distance between this grid and the nearest obstacle. The Euclidean distance refers to the straight-line distance. For example, the distance between the [0,0] coordinate and the [3,4] coordinate in the coordinate system is 5. Through the ESDF, the distance between each surrounding position and the nearest obstacle to the position can be known, that is, the position of the obstacles in the surrounding environment can be known. Therefore, the first environmental information can be quickly determined through the ESDF.

[0026] In a possible implementation of the first aspect, the first trajectory includes a moving trajectory of a chassis and a moving trajectory of a robotic arm, and the above steps: planning the first trajectory based on the first environmental information, include: planning the moving trajectory of the chassis based on the first environmental information, the moving trajectory of the chassis is used to instruct the chassis to avoid obstacles when moving; planning the moving trajectory of the robotic arm based on the first environmental information and the moving trajectory of the chassis, the moving trajectory of the robotic arm is used to instruct the robotic arm to avoid obstacles when moving.

[0027] In this possible implementation, a topology-guided hierarchical whole-body trajectory search method is adopted, that is, the trajectory of the chassis is planned first, and the trajectory of the robotic arm is determined based on the trajectory of the chassis, so that the whole-body trajectory planning can be quickly performed in a high-dimensional space, ensuring the real-time and reliable trajectory planning.

[0028] In a possible implementation of the first aspect, the chassis is a chassis with non-holonomic constraints. Exemplarily, the chassis may be a differential chassis or an Ackerman chassis. In this possible implementation, the specific form of the chassis is limited, which improves the feasibility of the solution.

[0029] The second aspect of the present application provides an obstacle avoidance device, which is applied to a mobile robot arm, and the mobile robot arm includes a chassis and a robot arm. The obstacle avoidance device includes a determination unit, a planning unit, an update unit, and an optimization unit. The determination unit is used to determine the first environmental information, and the first environmental information is used to indicate the position of the obstacle; the planning unit is used to plan a first trajectory based on the first environmental information, and the first trajectory is used to instruct the mobile robot arm to avoid obstacles when moving; the update unit is used to update the first environmental information to obtain the second environmental information; the optimization unit is used to optimize the first trajectory based on the second environmental information, combining the trajectory tracking cost and the obstacle avoidance cost to construct an optimal control problem, wherein the trajectory tracking cost is used to predict whether the mobile robot arm will collide with the obstacle, and the obstacle avoidance cost is used to instruct the mobile robot arm to avoid the obstacle when moving.

[0030] In a possible implementation manner of the second aspect, the optimization unit is specifically configured to construct an optimal control problem based on the following formula:

[0031]

[0032] st

[0033] u min <u<u max

[0034] x min <x<x max

[0035] The parameters in the above formula are obtained based on the second environment information, where: is the penalty term at the end point of the first trajectory, x is the state parameter of the mobile robot, which includes the moving angle of the chassis and the joint angle of the robot, T is the time, and x T is the state parameter under T, x init is the first trajectory, Q is the penalty weight, o(x) is the obstacle avoidance cost, (x init ) T Q(xx init ) is the trajectory tracking cost, st is the constraint condition of the optimal control problem, and u is the control parameter of the mobile robot, which includes the linear velocity and angular velocity of the chassis and the joint velocity of the robot.

[0036] In a possible implementation manner of the second aspect, the optimal control problem further includes a motion smoothing cost, and the motion smoothing cost is used to smooth the first trajectory.

[0037] In a possible implementation manner of the second aspect, the optimal control problem also includes an operability optimization cost, and the operability optimization cost is used to improve the operability of the robot arm.

[0038] In a possible implementation manner of the second aspect, the device further includes a solving unit, configured to solve the optimal control problem through differential dynamic programming to obtain the second trajectory.

[0039] In a possible implementation manner of the second aspect, the updating unit is specifically configured to update the first environment information when a position of the obstacle changes.

[0040] In a possible implementation manner of the second aspect, the determination unit is specifically used to obtain first point cloud information, the first point cloud information including information of a mobile robotic arm and information of an obstacle; filter out the information of the mobile robotic arm in the first point cloud information to obtain second point cloud information; generate a Euclidean signed distance field of the obstacle based on the second point cloud information; and determine the first environmental information based on the Euclidean signed distance field.

[0041] In a possible implementation of the second aspect, the first trajectory includes a moving trajectory of the chassis and a moving trajectory of the robotic arm. The planning unit is specifically used to plan the moving trajectory of the chassis based on the first environmental information, and the moving trajectory of the chassis is used to instruct the chassis to avoid obstacles when moving; the moving trajectory of the robotic arm is planned based on the first environmental information and the moving trajectory of the chassis, and the moving trajectory of the robotic arm is used to instruct the robotic arm to avoid obstacles when moving.

[0042] In a possible implementation manner of the second aspect, the chassis is a chassis with non-holonomic constraints.

[0043] The obstacle avoidance device provided in the second aspect of the present application is used to execute the method described in the first aspect or any possible implementation manner of the first aspect.

[0044] The third aspect of the present application provides an obstacle avoidance device, comprising a processor and a memory, wherein the memory is used to store instructions, and the processor is used to obtain the instructions stored in the memory to execute the method described in the first aspect or any possible implementation of the first aspect.

[0045] A fourth aspect of the present application provides a computer-readable storage medium, which includes instructions. When the instructions are executed on a computer, the computer executes the method described in the first aspect or any possible implementation of the first aspect.

[0046] A fifth aspect of the present application provides a computer program product comprising instructions, which, when executed on a computer, enables the computer to execute the method described in the first aspect or any possible implementation of the first aspect.

[0047] In a sixth aspect, the present application provides a chip system, which includes at least one processor and a communication interface, wherein the communication interface and the at least one processor are interconnected via lines, and the at least one processor is used to run computer programs or instructions to execute the method described in the first aspect or any possible implementation of the first aspect. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 A schematic diagram of an application scenario of the mobile robot arm obstacle avoidance method provided in an embodiment of the present application;

[0049] Figure 2 A schematic diagram of an embodiment of a mobile robot arm obstacle avoidance method provided in an embodiment of the present application;

[0050] Figure 3 A schematic diagram of convex hull decomposition in an embodiment of the present application;

[0051] Figure 4 A schematic diagram of constructing an ESDF map in an embodiment of the present application;

[0052] Figure 5 A schematic diagram of an embodiment of planning the trajectory of a mobile robot arm in an embodiment of the present application;

[0053] Figure 6 A schematic diagram of planning the chassis trajectory in an embodiment of the present application;

[0054] Figure 7 A schematic diagram of planning the trajectory of a robotic arm in an embodiment of the present application;

[0055] Figure 8 This is a schematic diagram of an embodiment of updating the ESDF map in an embodiment of the present application;

[0056] Fig. 9 This is another schematic diagram of an embodiment of updating the ESDF map in an embodiment of the present application;

[0057] Fig.10 A schematic diagram of another embodiment of the mobile robot arm obstacle avoidance method provided in an embodiment of the present application;

[0058] Fig.11 A structural schematic diagram of a mobile mechanical arm provided in an embodiment of the present application;

[0059] Fig.12 Another structural schematic diagram of the mobile robotic arm provided in an embodiment of the present application. DETAILED DESCRIPTION

[0060] The following describes the embodiments of the present application in conjunction with the accompanying drawings. Obviously, the described embodiments are only embodiments of a part of the present application, rather than all embodiments. It is known to those skilled in the art that with the development of technology and the emergence of new scenarios, the technical solutions provided in the embodiments of the present application are also applicable to similar technical problems.

[0061] The terms "system" and "network" in the specification and claims of the present application and the above-mentioned drawings can be used interchangeably. Unless otherwise specified, ordinal numbers such as "first" and "second" are used to distinguish multiple objects and are not used to limit the order, timing, priority or importance of multiple objects. It should be understood that the terms used in this way can be interchangeable where appropriate, so that the embodiments described here can be implemented in an order other than the content illustrated or described here. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device comprising a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0062] To facilitate understanding, the relevant terms and concepts mainly involved in the embodiments of the present application are first introduced below.

[0063] 1. Jerk

[0064] An indicator that reflects the smoothness of a mobile robot's motion, defined as the derivative of acceleration. The larger the jerk, the less smooth the motion.

[0065] 2. Optimal control problem

[0066] Optimal control refers to seeking a control under given constraints to make a given system performance index reach a maximum value (or minimum value). Mathematically, the optimal control problem can be expressed as: under the constraints of the motion equation and the allowable control range, the performance index function (called functional) with the control function and the motion state as variables is used to find the extreme value (maximum or minimum).

[0067] 3. Non-integrity constraints

[0068] Non-holonomic constraints (NHC) mean that when the vehicle is driving, it is assumed that the lateral and vertical velocities of the vehicle are 0 if there is no side slip, drift, or bounce.

[0069] 4. Point Cloud

[0070] A point cloud is a collection of many 3D coordinate points, which consists of many discrete points. Each point has its own position coordinates and possible other attributes, such as color, normal vector, and intensity. Point clouds are usually captured by laser scanners, cameras, or other sensors and used to create 3D models, maps, or perform remote sensing analysis.

[0071] 5.URDF

[0072] Unified Robot Description Format (URDF) is an XML-based format for describing robots and their partial structures, joints, and degrees of freedom. From a mechanics perspective, robots are usually modeled as structures consisting of links and joints. Links are rigid bodies with mass properties, while joints are structures that connect and restrict the relative motion of two rigid bodies. Joints are also called kinematic pairs. Connecting links in sequence through joints forms a kinematic chain. A URDF document describes the relative relationship, inertial properties, geometric characteristics, and collision model of a series of joints and links.

[0073] 6. Positive kinematics

[0074] Forward kinematics is to determine the position and posture of the end effector based on the values ​​of the robot's joint variables.

[0075] 7. Breadth-First Search

[0076] Breadth First Search (BFS), also known as Breadth First Search, is a graph search algorithm that searches for a target node by traversing the nodes of a graph layer by layer. The algorithm starts from the starting node, visits the starting node first, then visits all the neighboring nodes of the starting node, then visits the neighboring nodes of these neighboring nodes, and so on, until all reachable nodes are traversed.

[0077] The embodiment of the present application provides a mobile robot arm obstacle avoidance method, which can improve the obstacle avoidance capability of the mobile robot arm. The embodiment of the present application also provides a corresponding device, a computer-readable storage medium, and a computer program product, etc. The following are respectively described.

[0078] See below Figure 1 , which is a schematic diagram of an application scenario of the mobile robotic arm obstacle avoidance method provided in an embodiment of the present application.

[0079] like Figure 1 As shown, the application scenario includes a mobile robot and several obstacles. The mobile robot includes a chassis and a robot, wherein the chassis provides mobility and the robot provides operation capability, so that the mobile robot can perform mobile operation tasks. It can be understood that Figure 1 The number of mobile robotic arms and obstacles is only for illustrative purposes, and the specific number is not limited here.

[0080] In the process of performing tasks, the mobile robot arm needs to avoid collision with obstacles. Specifically, both the chassis and the robot arm need to avoid obstacles. However, the obstacle avoidance capabilities of common mobile robot arms are obviously insufficient, which seriously affects the commercial application of mobile robot arms.

[0081] In view of this, an embodiment of the present application provides a method for obstacle avoidance of a mobile robotic arm, which optimizes the moving trajectory of the mobile robotic arm by constructing an optimal control problem, thereby improving the obstacle avoidance capability of the mobile robotic arm.

[0082] The mobile robotic arm obstacle avoidance method provided in the embodiment of the present application specifically involves application scenarios including household mobile operation service robots providing services (such as grasping and cleaning), loading and unloading materials in manufacturing plants, loading and unloading of nodes in data centers, and mobile charging robots charging electric vehicles.

[0083] See below Figure 2 , is a schematic diagram of an embodiment of the mobile robot arm obstacle avoidance method provided in the embodiment of the present application. Figure 2 As shown, this embodiment includes steps 201 to 204 .

[0084] 201. Determine first environmental information, where the first environmental information is used to indicate a location of an obstacle.

[0085] First, first environmental information around the mobile robot arm is determined, and the first environmental information indicates the location of the obstacle.

[0086] In a possible solution, the mobile robotic arm is equipped with multiple sensors, which may be laser radar, ultrasonic radar, infrared radar, etc. The sensors may be used to obtain first point cloud information of the mobile robotic arm under multiple viewing angles, and the first point cloud information includes point cloud information of surrounding obstacles and point cloud information of the mobile robotic arm itself. In addition, the first point cloud information may also be obtained through a camera, and the method for obtaining the point cloud information is not limited here.

[0087] Generally speaking, the first point cloud information acquired by the sensor has a large amount of data. In order to ensure the real-time performance of environmental perception, the first point cloud information is downsampled and outlier filtered. Downsampling is to reduce the density of point cloud data. Methods include voxel grid downsampling, nearest neighbor downsampling, and statistical filtering. Outlier filtering is to filter out outliers in point cloud data. Outliers mainly refer to sparse point clouds or small point clouds far away from the main body of the point cloud. This type of point cloud has little correlation with the main point cloud and contains less information.

[0088] A mobile robot arm is generally composed of a chassis and a robot arm, and the overall shape is non-convex, that is, it cannot be compactly enclosed by a rectangular parallelepiped. For motion planning problems, the complexity of collision detection for non-convex shapes is very high. Therefore, in this embodiment, sphere decomposition is performed on the mobile robot arm, that is, the mobile robot arm is wrapped with many spheres of different radii. In this way, when performing collision detection, it is only necessary to detect whether the sphere will collide with the obstacle, that is, compare the radius of the sphere with the distance from the mobile robot arm to the obstacle. If the radius of the sphere is greater than the distance to the obstacle, no collision will occur. The specific process of sphere decomposition processing can be combined with Figure 3 Understand. Figure 3 As shown in FIG. 1 , the mobile robot arm is firstly modeled by the Unified Robot Description Format (URDF), and then the volumetric hierarchical approximate convex decomposition (V-HACD) is performed according to the URDF model, and the model is wrapped with a sphere.

[0089] After downsampling, outlier filtering, and spherical decomposition processing, the point cloud information of the mobile robotic arm in the first point cloud information is filtered out to obtain the second point cloud information. The purpose of filtering out the point cloud information of the mobile robotic arm is to avoid interference when constructing the environmental map, that is, to avoid treating the mobile robotic arm as an obstacle, so that the information of the obstacle can be obtained more clearly. After obtaining the second point cloud information, an occupation grid map is generated according to the ray cast algorithm. According to the state of each grid in the occupation grid map, it can be known which positions have obstacles. For example, a grid with a value of 1 indicates that there is an obstacle there, and a grid with a data of 0 indicates that there is no obstacle there. Of course, in addition to the raycast algorithm, grid maps can also be generated by other commonly used algorithms, which are not limited here.

[0090] After the occupancy grid map is generated, the Euclidean Signed Distance Field (ESDF) of the obstacle is further generated based on the second point cloud information. The value of each grid in the ESDF stores the Euclidean distance between this grid and the nearest obstacle. Euclidean distance refers to the straight-line distance. For example, the distance between the [0,0] coordinate and the [3,4] coordinate in the coordinate system is 5. Through ESDF, the distance between each surrounding position and the nearest obstacle to the position can be known, that is, the position of the obstacles in the surrounding environment can be known. Therefore, the first environmental information can be determined through ESDF.

[0091] The spherical decomposition of the mobile manipulator can be combined with the construction of the ESDF map Figure 4 Understand. Figure 4 As shown in FIG. 1 , the mobile robot arm includes a chassis and a robot arm. The chassis is surrounded by two large circles, and the robot arm is surrounded by several small circles. The ESDF map constructed after filtering out the point cloud information of the mobile robot arm will not regard the mobile robot arm as an obstacle. Figure 4 A grid with a negative value indicates that there is an obstacle there. The smaller the value, the closer it is to the center of the obstacle. A grid with a value of 1 indicates that the distance to the nearest obstacle is 1, a grid with a value of 2 indicates that the distance to the nearest obstacle is 2, and so on. The ball wrapped around the mobile robot arm should not enter a grid with a negative value to avoid collision with obstacles.

[0092] With the ESDF map, the collision detection of the mobile robot can be mathematically simplified to:

[0093]

[0094] Among them, FK refers to the center position of the i-th sphere calculated according to the forward kinematics of the mobile robot, and r refers to the radius of the i-th sphere. Generally speaking, in order to ensure a certain safety margin, r will be larger than the actual robot model. The above formula can be understood vividly as follows: to ensure that the mobile robot does not collide with obstacles, it is necessary to ensure that all spheres in its sphere decomposition model are collision-free, and the collision-free condition is that the distance between the center of each sphere and the nearest obstacle must be greater than the radius of the sphere.

[0095] Of course, in addition to the above-mentioned method of determining the first environmental information by constructing ESDF based on point cloud information, there are many other methods for determining the first environmental information. For example, an environmental map is constructed by random sampling to determine the first environmental information. In this embodiment, the method for determining the first environmental information is not specifically limited.

[0096] 202. Plan a first trajectory based on the first environmental information, where the first trajectory is used to instruct the mobile robotic arm to avoid obstacles when moving.

[0097] The time and space complexity of traditional high-dimensional sampling motion planning are very high. For mobile operating robots in 3D dynamic environments, it is difficult to achieve real-time motion planning by relying solely on high-dimensional sampling, and reliable operation in complex environments cannot be guaranteed. Therefore, this embodiment proposes a hierarchical trajectory search method based on topology guidance. Specifically, the topology guidance trajectory of the chassis is first generated, and after the chassis trajectory is confirmed, the spatiotemporal sampling of the state space of the manipulator is performed, and finally the first feasible collision-free trajectory of the whole body is output.

[0098] In a possible solution, the chassis is a chassis with non-integrity constraints, such as a differential chassis or an Ackerman chassis, and the kinematic constraints (non-integrity constraints) of the chassis also need to be considered. That is, after generating the topology guidance trajectory of the chassis, the trajectory that satisfies the kinematic constraints of the chassis is selected according to the topology guidance trajectory, and then the motion trajectory of the robot arm is generated based on the chassis trajectory that satisfies the kinematic constraints.

[0099] The following is an example of a chassis with non-integrity constraints. Figure 5 The process of planning the first trajectory is described.

[0100] S501. Given the starting point and the ending point of the plan in the full state space based on the ESDF map, that is, x, y, θ, θ. Wherein, x, y represent the x, y position of the chassis in the coordinate system, θ represents the orientation angle of the chassis, and θ represents the joint angle of the manipulator. It can be understood that the starting point and the ending point include the starting position and the ending position of the chassis and the starting position and the ending position of the manipulator.

[0101] S502: Based on the position of each grid and obstacle indicated by the first environmental information, the collision-free tracks in the ESDF map are connected, and the collision-free edge composed of two nodes represents a feasible topological area in the map.

[0102] S503, remove the topological homotopy trajectory. In the same topological space, there may be many topological trajectories, where the topological homotopy is defined as:

[0103] No touch

[0104] Where τ1 and τ2 are two trajectories, t is the time of the trajectory, and line represents the line segment formed by the two trajectory points. In the intuitive sense, topological homology means that given two trajectories, if their connecting lines at the same time are collision-free, then the two trajectories are topologically completely equivalent and are in the same topological space. For multiple topological homology trajectories, the topological trajectory with the shortest trajectory is retained.

[0105] S504, short-circuit the topological trajectory. The sampled topological trajectory may be very long, and it will be far away from the obstacle. However, in fact, the mobile robot only needs to bypass it when avoiding obstacles, instead of bypassing several meters, because the longer trajectory will lead to lower operation efficiency. Therefore, the topological trajectory is short-circuited to make the topological trajectory as short as possible. Specifically, sample any two times t1 and t2 on the same trajectory. If the line (τ(t1), τ(t2)) is collision-free, connect the two points and remove the node in the middle, so as to ensure that the trajectory is collision-free and make the trajectory as short as possible.

[0106] S505: For a chassis with non-holonomic constraints, based on the topological trajectory obtained after short-circuiting, search for the chassis trajectory with kinematic constraints. For example, for a differential wheel chassis, its kinematic equation is:

[0107]

[0108] Where x and y are the positions of the chassis, θ is the orientation angle of the chassis, v is the linear velocity of the chassis, and w is the angular velocity of the chassis. When searching for a trajectory, v and w are searched, and by giving v and w to make the chassis move forward, a trajectory that satisfies the non-holonomic constraints can be obtained.

[0109] S506, calculating the cost and the heuristic cost according to the trajectory obtained in step S505. Exemplarily, the HybridAstar algorithm is used to perform heuristic trajectory search of the chassis, and the topological trajectory in S505 is used as a heuristic function, that is, the cost close to the topological trajectory is small, and the cost far from the topological trajectory is large, so a topological guiding effect can be formed to quickly guide the chassis away from obstacles.

[0110] S507, judging whether the chassis has reached the end point. If it has not reached the end point, steps 505 and 506 are executed repeatedly, and searching and integrating are continuously performed, and finally a complete movement trajectory of the chassis that satisfies the non-holonomic constraints and has a shorter trajectory can be obtained.

[0111] The process of planning the chassis movement trajectory can be combined with Figure 6 Understand. Figure 6 As shown, the circular boxes are the starting point and the end point, the rectangular boxes are obstacles, and the lines are the moving trajectories, which need to avoid obstacles.

[0112] S508. Plan the movement trajectory of the robot. In this embodiment, sampling is performed through the extended time-space state, and a rapid-exploring random tree (RRT) algorithm is used to generate a feasible trajectory. Specifically, the state of the robot is first sampled in time and space, and then the nearest neighbor on the tree is found in the time and space state (i.e., a comprehensive distance index, judging the node that is closer to the current sampling point in time and space), and then it is determined whether this connection is feasible. Feasibility has two meanings, temporal feasibility and spatial feasibility. Temporal feasibility means that it must be forward in time (a node position cannot move from the future to the past), and spatial feasibility means that it will not collide with obstacles, and secondly, it can be reached while satisfying the constraint of the maximum movement speed.

[0113] It can be understood that after the starting point and the end point are given in step S501, the movement trajectory of the robot arm can be planned, and then after the movement trajectory of the chassis is determined, the final trajectory is determined based on the movement trajectory of the chassis.

[0114] S509. Determine the moving trajectory of the robot arm in combination with the obtained chassis trajectory. Specifically, determine the entire position of the robot arm in the spatiotemporal sampling based on the chassis trajectory, and make a feasibility criterion based on this to determine whether the connection between the sampling point and the nearest neighbor on the tree is feasible. Feasibility has two meanings, temporal feasibility and spatial feasibility. Temporal feasibility means that it must be forward in time (a node position cannot move from the future to the past), and spatial feasibility means that it will not collide with obstacles, and secondly, it can be reached while satisfying the constraint of the maximum movement speed. When feasible, connect the two points.

[0115] S510, determine whether the whole state of the mobile robot has reached the end point, that is, whether the states of the chassis and the robot have reached the end point. If not, repeat steps S508 and S509 to perform sampling and connection until the whole state of the whole body reaches the end point, then the trajectory search part ends, and the first trajectory including the chassis movement trajectory and the robot movement trajectory is obtained.

[0116] The robot trajectory can be determined by combining Figure 7 To understand. Figure 7 Each point in the figure represents a configuration of the robot arm at a certain moment, and the positions of the six joints of the robot arm determine a configuration. Among them, the point labeled 0 is the starting point position, the point labeled 9 is the ending point position, the points labeled 1 and 4 are feasible points determined after avoiding the obstacle position, and 2 and 3 are points that are found to collide with obstacles after trying, so the points labeled 2 and 3 are abandoned. Determine the nearest node 5 within the range formed by the obstacle, and then determine the sample point 7, and determine the extended node 6 from 5 and 7. The point labeled 8 is a feasible point in space, but the time of 8 is later than the sample point 7, so 8 is not feasible in time, and other positions need to be found. In the process from the starting point 0 to the ending point 9, keep looking for feasible positions and configurations, and connect the feasible positions and configurations to get the movement trajectory of the robot arm.

[0117] In addition to the above-mentioned topology-guided hierarchical trajectory search method, other methods can also be used to plan the first trajectory. For example, a common method is to uniformly model and sample the chassis and the robotic arm, and then perform integrated path planning based on the overall model. In this embodiment, the method for planning the first trajectory is not specifically limited.

[0118] 203. Update the first environment information to obtain second environment information.

[0119] In a possible solution, the first environment information is updated only when the position of the obstacle changes.

[0120] The ESDF map includes the underlying occupied grid map, which can indicate whether each grid is occupied or idle. The occupied state means that there is an obstacle there, and the idle state means that there is no obstacle there. In other words, the environmental information is updated when the state of the grid changes. Specifically, when the grid changes from the idle state to the occupied state, the Euclidean distance values ​​of the grid and the surrounding grids should be reduced, and the information of the decrease is propagated to the surrounding grids according to the breadth-first search method. In simple terms, if the current Euclidean distance of the surrounding grids is greater than the distance between it and the newly added obstacle, then the value of the grid is updated. When the grid changes from the occupied state to the idle state, the Euclidean distance values ​​of the grid and the surrounding grids should be increased, and the information of the increase in distance is propagated to the surrounding grids according to the breadth-first search method, and the distance value of the grid is updated (because the nearest obstacle has been cleared, so a new nearest obstacle needs to be found). In simple terms, the ESDF distance value to be updated by the grid is set to the minimum distance of the obstacles with its neighbors.

[0121] The grids that change from the idle state to the occupied state are placed in the lower queue, and the grids that change from the occupied state to the idle state are placed in the raise queue.

[0122] Combine the following Figure 8 and Fig. 9 The update process is described in detail.

[0123] like Figure 8 , which is a schematic diagram of the updating process of the first environment information after the grid changes from an idle state to an occupied state (ie, an obstacle appears in the grid).

[0124] S801: Determine whether the descending queue is empty. If not, execute step S802.

[0125] S802: Take out the grid in the descending queue and traverse all surrounding grids.

[0126] S803: Determine whether the current ESDF record value of the surrounding grid is greater than the distance between it and the newly appeared obstacle. If yes, execute step S804; if no, execute step S806.

[0127] S804: Update the ESDF record value of the surrounding grid to the distance between it and the newly appeared obstacle.

[0128] S805, putting the surrounding grids into the descending queue, and re-execute step S801.

[0129] S806: Skip the neighbor grid and do not process it.

[0130] like Fig. 9 , which is a schematic diagram of the updating process of the first environment information after the grid changes from an occupied state to an idle state (ie, an obstacle appears in the grid).

[0131] S901, determine whether the ascending queue is empty. If not, execute step S902.

[0132] S902, taking out the grid in the ascending queue, clearing its value, setting it to infinity, and traversing all surrounding grids.

[0133] S903: Determine whether the distance between the grid and the nearby obstacle is less than the currently recorded value. If yes, execute step S904; if no, execute step S906.

[0134] S904. Update the ESDF record value of the grid to the distance between it and nearby obstacles until the nearest obstacle is found.

[0135] S905, putting the grids around the grid into the ascending queue, and re-execute step S901.

[0136] S906: Skip the grid and do not process it.

[0137] When both the ascending queue and the descending queue are cleared, the update of the first environment information is completed and the second environment information is obtained.

[0138] In addition to the incremental updating method described above (i.e., updating only when the position of the obstacle changes), other methods may be used to update the environment information. For example, the ESDF values ​​of all grids are recalculated in each frame. In this embodiment, the updating method of the first environment information is not specifically limited.

[0139] 204. Based on the second environment information, an optimal control problem is constructed in combination with a trajectory tracking cost and an obstacle avoidance cost to optimize the first trajectory.

[0140] When the environmental information is updated, the originally planned first trajectory also needs to be optimized. In this embodiment, the chassis and the robotic arm are jointly modeled, and their dynamic coupling equations are used as system equations. The optimal control problem is constructed in combination with the trajectory tracking cost and the obstacle avoidance cost to optimize the first trajectory. Among them, the trajectory tracking cost is used to predict whether the mobile robotic arm will collide with the obstacle. If it is predicted that there will be no collision, there is no need to update the first trajectory. If it is predicted that a collision will occur, the moving trajectory of the mobile robotic arm is changed. The obstacle avoidance cost is used to instruct the mobile robotic arm to avoid obstacles while moving. In this way, predictive obstacle avoidance can be achieved, and the feasibility of trajectory execution is continuously predicted based on the system model and planning results, and obstacle avoidance strategies are taken in advance. For example, the mobile robotic arm can make an obstacle avoidance response 3 meters away from the obstacle. Or the time to the obstacle is calculated based on the moving speed, and an obstacle avoidance response is made 3 seconds in advance.

[0141] The first trajectory obtained in step 202 is often tortuous, and the mobile robot arm will be very uneven during execution, and in serious cases, the mobile robot arm will be unstable. Therefore, the optimal control problem can be further constructed in combination with the motion smoothing cost, and the motion smoothing cost is used to smooth the first trajectory, so that the mobile robot arm can achieve low-jerk dexterous motion.

[0142] In addition, the maneuverability of the robot arm can also be restricted. That is, the optimal control problem is constructed in combination with the maneuverability optimization cost. The maneuverability optimization cost is used to improve the maneuverability of the robot arm and prevent the robot arm from falling into a singular state (if the robot arm is singular, it cannot exert force on the object), thereby further improving the overall dexterity of the mobile robot arm.

[0143] The following takes a chassis with non-holonomic constraints as an example, and describes in detail the process of constructing the optimal control problem by combining trajectory tracking cost, obstacle avoidance cost, motion smoothness cost, and maneuverability optimization cost. It can be understood that the motion smoothness cost and maneuverability optimization cost are optional, that is, these two cost items can be omitted when constructing the optimal control problem, and are not specifically limited here.

[0144] The trajectory optimization problem (i.e., optimal control problem) is constructed as:

[0145]

[0146] st

[0147] u min <u<u max

[0148] x min <x<x max

[0149] The parameters in the above formula are obtained based on the second environment information, where: is the penalty term for a given endpoint (i.e., the end point of the first trajectory), x is the state parameter of the mobile robot, including the moving angle of the chassis and the joint angle of the robot, T is the time, and x T is the state parameter under T. R and Q are diagonal matrices, is the penalty term weight, u is the control parameter of the mobile robot, including the linear velocity and angular velocity of the chassis and the joint velocity of the robot, x init is the first trajectory preliminarily planned. T Ru is the motion smoothing cost term, o(x) is the obstacle avoidance cost term, (x init ) T Q(xx init ) is the trajectory tracking cost term, m(x) is the maneuverability optimization cost term, and st is the constraint condition of the optimal control problem. The purpose of the trajectory optimization problem is to solve u and x.

[0150] It can be understood that when the motion smoothness and maneuverability of the mobile robot are not considered, the above optimal control problem is:

[0151]

[0152] st

[0153] u min <u<u max

[0154] x min <x<x max

[0155] In addition to the above formulas, the optimal control problem can also be constructed by other formulas, which are not limited here.

[0156] The following is a description based on the above formula, where the mathematical expression of operability is:

[0157]

[0158] where J(q) is the Jacobian matrix of the mobile manipulator.

[0159] The obstacle avoidance cost of the optimal control problem is as follows, using a piecewise cubic cost function to penalize the mobile manipulator for approaching obstacles:

[0160]

[0161]

[0162] Where d is the distance to the obstacle, which can be obtained through the ESDF map. i is the center point position of the i-th joint in the mobile robot, and the mobile robot includes M joints. s is the state of the mobile robot, which is used to calculate c.

[0163] The gradient of the joint state can be expressed as (the gradient is used to solve the optimization problem),

[0164]

[0165] Optionally, the optimal control problem is solved by using a differential dynamic programming method to obtain a second trajectory. The solution process is described below.

[0166] Rearrange the problem into the following standard form:

[0167]

[0168] x n ∈X n non-convex constraints

[0169] u n ∈U n non-convex constraints

[0170] Where k is the discretization degree, u is the controller input of the system, and x is the full state parameter of the mobile robot. n is obtained by T and K. For example, if T is 5 seconds and K is 0.1, then n is equal to T / K, which is equal to 50.

[0171] For dynamic programming problems, the cost function (Cost-to-Go) is expanded by the second-order Taylor:

[0172]

[0173]

[0174] Among them, p is the parameter obtained after V is expanded.

[0175] Similarly, the action value function is expanded by Taylor's second order:

[0176]

[0177] Among them, g and G are the parameters obtained after S is expanded.

[0178] According to the Bellman principle of dynamic programming, the recursive expression of the cost function (Cost-to-Go) can be obtained:

[0179]

[0180] Taking the partial derivative of the optimal control rate u in the above formula, we get the optimal feedforward feedback control rate expression (the trajectory after iterative optimization):

[0181]

[0182] Then substitute the obtained optimal control rate back into the cost function (Cost-to-Go) to obtain the recursive result of the cost function:

[0183]

[0184]

[0185]

[0186] By solving the optimal control problem through differential dynamic programming, the high-dimensional optimal control problem (hundreds of thousands of dimensions) can be reduced to a series of low-dimensional optimal control problems (several dimensions and tens of dimensions), thereby increasing the speed of problem solving by an order of magnitude, and then realizing time-domain rolling prediction, and adjusting the mobile trajectory according to the prediction to avoid obstacles in advance, ensuring that the mobile robot can achieve reliable whole-body operation in a complex environment. And the result obtained by solving the differential dynamic programming includes not only an optimized trajectory (equivalent to the feedforward term), but also a feedback control rate, which is used to restrict the behavior of the mobile robot, so there is no need to design an additional controller to track the trajectory of the mobile robot and control the movement of the mobile robot.

[0187] It should be understood that in addition to solving the optimal control problem by differential dynamic programming, a variety of mathematical methods can also be used to solve it. In this embodiment, the method for solving the optimal control problem is not specifically limited.

[0188] In this embodiment, by constructing an ESDF map and performing spherical decomposition processing on the mobile manipulator, environmental information can be quickly acquired and collision perception can be performed. The trajectory of the mobile manipulator is then planned based on the acquired environmental information. This embodiment adopts a topology-guided hierarchical whole-body trajectory search method, that is, the trajectory of the chassis is first planned, and the trajectory of the manipulator is determined based on the trajectory of the chassis, so that the whole-body trajectory can be quickly planned in a high-dimensional space (up to the millisecond level), ensuring the real-time and reliable trajectory planning.

[0189] The map is updated using an incremental update method, that is, the ESDF map is updated after the environmental information is sensed to change. Compared with the traditional method of recalculating the ESDF value from scratch in each frame update, this incremental update method is more efficient and can save a lot of computing workload. It ensures the mapping frequency (above 30 Hz in common platforms) and lays the foundation for the subsequent dexterous operation of the entire mobile robot arm.

[0190] The trajectory of the mobile robotic arm is adjusted according to the updated map information. In this embodiment, an obstacle avoidance strategy based on rolling time domain prediction is adopted, and the feasibility of trajectory execution is continuously predicted based on the system model and planning results, and an obstacle avoidance strategy is adopted in advance. Specifically, the chassis and the robotic arm are uniformly modeled and an optimal control problem is constructed. The feasibility of the trajectory is analyzed based on the constructed optimal control problem. When the trajectory is likely to collide, obstacle avoidance measures are taken in advance, thereby improving the obstacle avoidance capability of the mobile robotic arm. Compared with traditional reactive obstacle avoidance, predictive obstacle avoidance can make an obstacle avoidance response several meters away from the obstacle, ensuring reliable obstacle avoidance under control error.

[0191] Furthermore, the optimal control problem can be constructed by combining multiple constraints. For example, the jerk of the mobile robot arm can be reduced by adding a motion smoothness cost, so that the movement of the mobile robot arm is smoother, for example, the movement trajectory is smoother and the chassis accelerates or decelerates evenly. The maneuverability optimization cost can also be added to improve the maneuverability of the robot arm and prevent the robot arm from falling into a singular state. In addition, kinematic constraints can also be combined, that is, the chassis can be a chassis with non-integrity constraints.

[0192] In summary, the overall operating capability of the mobile robotic arm is improved, thereby improving the obstacle avoidance capability of the mobile robotic arm.

[0193] Combine the following Fig.10 The obstacle avoidance method provided in the embodiments of the present application is summarized and described.

[0194] like Fig.10As shown in the figure, the obstacle avoidance method mainly includes three parts: "body and environment perception", "trajectory search planning" and "trajectory adjustment and whole body dexterous control". The "body and environment perception" part obtains the depth perception data of the mobile manipulator under multiple perspectives through multiple sensors or cameras. In order to avoid slow processing caused by excessive data volume and ensure the real-time perception, the collected data is downsampled and outlier filtered. The mobile manipulator body is modeled by URDF, and then the model is processed by V-HACD. After convex hull decomposition, the mobile manipulator body is filtered by combining the perception data after downsampling and outlier filtering, that is, the point cloud information of the mobile manipulator is filtered out to ensure that the environmental information built in the ESDF map does not include the point cloud of the mobile manipulator body. After filtering the point cloud information of the mobile manipulator, the raycast algorithm is used to generate the underlying occupancy grid map and ESDF map. For the update of the map, an incremental update method is adopted to update the content of the ascending queue or descending queue after the environmental information changes, thereby realizing the update of the ESDF map.

[0195] The "trajectory search planning" section includes the preliminary planning of the mobile robot's trajectory and subsequent optimization and adjustment. The preliminary planning of the trajectory is achieved through a topology-guided heuristic front-end trajectory search method, that is, the chassis's trajectory is first planned, and then the Hybrid Astar algorithm is used to perform a heuristic search of the chassis' trajectory. The chassis's trajectory is used as a heuristic function to form a topological guidance effect on the mobile robot, and the chassis is quickly guided away from obstacles. After determining the chassis's trajectory, the robot's trajectory is regenerated based on the chassis's trajectory. In this way, the hierarchical trajectory search can achieve a millisecond-level collision-free trajectory for the entire body in high-dimensional space. When planning the trajectory, the object's three-dimensional (3D) posture recognition function can also be combined to specifically determine whether the object is an obstacle or a target object that needs to be operated. If it is judged to be an obstacle, the trajectory bypasses the object. If it is judged to be a target object, it is operated on. When the environmental information changes, the mobile trajectory is replanned, the heuristic function is updated, the trajectory search is re-performed, and the trajectory is smoothed.

[0196] The "Trajectory Adjustment and Whole-body Dexterity Control" section uniformly models the chassis and the robot to obtain a whole-body kinematic model. Then, the optimal control problem is constructed in combination with the system constraints, such as the Sequential Linear Quadratic (SLQ) planning problem, and the motion model and dexterity operation scheme of the whole-body control of the mobile robot are constructed into different costs and constraints, such as trajectory tracking cost, obstacle avoidance cost, motion smoothness cost, and operability optimization cost. Among them, the trajectory tracking cost is used to predict whether the mobile robot will collide with obstacles, the obstacle avoidance cost is used to instruct the mobile robot to avoid obstacles when moving, the motion smoothness cost is used to smooth the path, and the operability optimization cost is used to improve the operability of the robot. The SLQ problem is solved in parallel by forward rollout and reverse Ricati equations, so that the feasibility of the trajectory can be predicted and the trajectory can be replanned, and the movement of the mobile robot can be made more dexterous and smooth. In addition, a feedforward feedback control rate can be obtained according to the solution result, and the behavior of the mobile robot arm can be controlled through the feedforward feedback control rate, so there is no need to design an additional controller to track the trajectory of the mobile robot arm.

[0197] It should be understood that the above Fig.10 The summary of the obstacle avoidance method is only an exemplary description. For example, obtaining environmental information by constructing an ESDF map is only one implementation method of obtaining environmental information in the embodiment of the present application. Figure 2 The embodiments shown are for understanding only and are not specifically limited herein.

[0198] The above describes the embodiments of the present application from the perspective of the method. The following describes the relevant devices in the embodiments of the present application from the perspective of specific device implementation.

[0199] See also Fig.11 , an embodiment of the present application provides a schematic diagram of an obstacle avoidance device 1100, wherein the obstacle avoidance device 1100 is applied to a mobile robotic arm, the mobile robotic arm includes a chassis and a robotic arm, and the obstacle avoidance device includes a determination unit 1101, a planning unit 1102, an update unit 1103 and an optimization unit 1104.

[0200] The determining unit 1101 is used to determine first environmental information, where the first environmental information is used to indicate a position of an obstacle.

[0201] The planning unit 1102 is used to plan a first trajectory based on the first environmental information, and the first trajectory is used to instruct the mobile robot arm to avoid obstacles when moving.

[0202] The updating unit 1103 is used to update the first environment information to obtain the second environment information.

[0203] The optimization unit 1104 is used to optimize the first trajectory based on the second environment information, combining the trajectory tracking cost and the obstacle avoidance cost to construct an optimal control problem, wherein the trajectory tracking cost is used to predict whether the mobile robot arm will collide with an obstacle, and the obstacle avoidance cost is used to instruct the mobile robot arm to avoid obstacles while moving.

[0204] Optionally, the optimization unit 1104 is specifically configured to construct an optimal control problem based on the following formula:

[0205]

[0206] st

[0207] u min <u<u max

[0208] x min <x<x max

[0209] The parameters in the above formula are obtained based on the second environment information, where: is the penalty term at the end point of the first trajectory, x is the state parameter of the mobile robot, which includes the moving angle of the chassis and the joint angle of the robot, T is the time, and x T is the state parameter under T, x init is the first trajectory, Q is the penalty weight, o(x) is the obstacle avoidance cost, (x init ) T Q(xx init ) is the trajectory tracking cost, st is the constraint condition of the optimal control problem, and u is the control parameter of the mobile robot, which includes the linear velocity and angular velocity of the chassis and the joint velocity of the robot.

[0210] Optionally, the optimal control problem also includes a motion smoothing cost, and the motion smoothing cost is used to smooth the first trajectory.

[0211] Optionally, the optimal control problem also includes a maneuverability optimization cost, and the maneuverability optimization cost is used to improve the maneuverability of the robot arm.

[0212] Optionally, the obstacle avoidance device 1100 further includes a solving unit 1105 for solving the optimal control problem by differential dynamic programming to obtain a second trajectory.

[0213] Optionally, the updating unit 1103 is specifically configured to update the first environment information when the position of the obstacle changes.

[0214] Optionally, the determination unit 1101 is specifically used to obtain first point cloud information, the first point cloud information including information of the mobile robotic arm and information of the obstacle; filter out the information of the mobile robotic arm in the first point cloud information to obtain second point cloud information; generate a Euclidean signed distance field of the obstacle based on the second point cloud information; and determine the first environment information based on the Euclidean signed distance field.

[0215] Optionally, the first trajectory includes the moving trajectory of the chassis and the moving trajectory of the robotic arm. The planning unit 1102 is specifically used to plan the moving trajectory of the chassis based on the first environmental information, and the moving trajectory of the chassis is used to instruct the chassis to avoid obstacles when moving; the moving trajectory of the robotic arm is planned based on the first environmental information and the moving trajectory of the chassis, and the moving trajectory of the robotic arm is used to instruct the robotic arm to avoid obstacles when moving.

[0216] Optionally, the chassis is a chassis with non-integrity constraints.

[0217] Each unit in the obstacle avoidance device 1100 performs the above Figure 2 , Figure 5 as well as Figures 8 to 10 The operation of the obstacle avoidance device in the illustrated embodiment will not be described in detail here.

[0218] See below Fig.12 , is a possible structural diagram of an obstacle avoidance device 1200 provided in an embodiment of the present application, including a processor 1201, a communication interface 1202, a memory 1203, and a bus 1204. The processor 1201, the communication interface 1202, and the memory 1203 are interconnected via the bus 1204. In an embodiment of the present application, the processor 1201 is used to control and manage the actions of the obstacle avoidance device. For example, the processor 1201 is used to execute Figure 2 The steps performed by the obstacle avoidance device in the method embodiment are shown. The communication interface 1202 is used to support the obstacle avoidance device to communicate. The memory 1203 is used to store program codes and data of the obstacle avoidance device.

[0219] Among them, the processor 1201 can be a central processing unit, a general-purpose processor, a digital signal processor, an application-specific integrated circuit, a field programmable gate array or other programmable logic device, a transistor logic device, a hardware component or any combination thereof. It can implement or execute various exemplary logic blocks, modules and circuits described in conjunction with the contents disclosed in 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, and so on. The bus 1204 can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. The bus can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Fig.12 Only one thick line is used in the diagram, but this does not mean that there is only one bus or only one type of bus.

[0220] The embodiment of the present application also provides a computer-readable storage medium, wherein the computer-readable storage medium includes instructions, and when the instructions are executed on a computer, the computer executes the aforementioned Figure 2 , Figure 5 as well as Figures 8 to 10 The method in the illustrated embodiment.

[0221] The present application also provides a computer program product comprising instructions, which, when executed on a computer, enables the computer to execute the aforementioned Figure 2 , Figure 5 as well as Figures 8 to 10 The method in the illustrated embodiment.

[0222] The embodiment of the present application also provides a chip system, the chip system includes at least one processor and a communication interface, the communication interface and the at least one processor are interconnected through a line, and the at least one processor is used to run a computer program or instruction to execute the aforementioned Figure 2 , Figure 5 as well as Figures 8 to 10 The method in the illustrated embodiment.

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

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

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

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

[0227] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially 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, and the computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, read-only memory), random access memory (RAM, random access memory), disk or optical disk and other media that can store program code.

Claims

1. A mobile robot arm obstacle avoidance method, characterized in that: The mobile mechanical arm comprises a chassis and a mechanical arm, and the method comprises: Determining first environmental information, where the first environmental information is used to indicate a location of an obstacle; Planning a first trajectory based on the first environmental information, the first trajectory being used to instruct the mobile mechanical arm to avoid the obstacle when moving; Updating the first environment information to obtain second environment information; Based on the second environmental information, an optimal control problem is constructed in combination with the trajectory tracking cost and the obstacle avoidance cost to optimize the first trajectory, wherein the trajectory tracking cost is used to predict whether the mobile robotic arm will collide with the obstacle, and the obstacle avoidance cost is used to instruct the mobile robotic arm to avoid the obstacle when moving.

2. The method according to claim 1, characterized in that The optimizing the first trajectory by constructing an optimal control problem based on the second environment information and combining the trajectory tracking cost and the obstacle avoidance cost includes: The optimal control problem is constructed based on the following formula: in min <u<u max x min <x<x max The parameters in the formula are obtained based on the second environment information, where: is the penalty term of the end point of the first trajectory, x is the state parameter of the mobile robot, the state parameter includes the moving angle of the chassis and the joint angle of the robot, T is the time, x T is the state parameter at T, x init is the first trajectory, Q is the penalty weight, o(x) is the obstacle avoidance cost, (x init ) T Q(xx init ) is the trajectory tracking cost, st is the constraint condition of the optimal control problem, u is the control parameter of the mobile robotic arm, and the control parameters include the linear velocity and angular velocity of the chassis and the joint velocity of the robotic arm.

3. The method according to claim 2, characterized in that The optimal control problem also includes a motion smoothing cost, and the motion smoothing cost is used to smooth the first trajectory.

4. The method according to any one of claims 1 to 3, characterized in that: The optimal control problem also includes an operability optimization cost, and the operability optimization cost is used to improve the operability of the robotic arm.

5. The method according to any one of claims 1 to 4, characterized in that: The method further comprises: The optimal control problem is solved by differential dynamic programming to obtain a second trajectory.

6. The method according to any one of claims 1 to 5, characterized in that: The updating of the first environment information includes: When the position of the obstacle changes, the first environment information is updated.

7. The method according to any one of claims 1 to 6, characterized in that: The determining of the first environment information includes: Acquire first point cloud information, where the first point cloud information includes information about the mobile robotic arm and information about the obstacle; Filtering out the information of the mobile mechanical arm in the first point cloud information to obtain second point cloud information; Generate a Euclidean signed distance field of the obstacle based on the second point cloud information; The first environmental information is determined based on the Euclidean signed distance field.

8. The method according to any one of claims 1 to 7, characterized in that: The first trajectory includes a moving trajectory of the chassis and a moving trajectory of the robotic arm, and planning the first trajectory based on the first environmental information includes: Planning a moving trajectory of the chassis based on the first environmental information, wherein the moving trajectory of the chassis is used to instruct the chassis to avoid the obstacle when moving; A moving trajectory of the robotic arm is planned based on the first environmental information and the moving trajectory of the chassis, and the moving trajectory of the robotic arm is used to instruct the robotic arm to avoid the obstacle when moving.

9. The method according to any one of claims 1 to 8, characterized in that: The chassis is a chassis with non-holonomic constraints.

10. An obstacle avoidance device, characterized in that: Applied to a mobile mechanical arm, the mobile mechanical arm comprises a chassis and a mechanical arm, and the obstacle avoidance device comprises: A determination unit, configured to determine first environmental information, wherein the first environmental information is used to indicate a location of an obstacle; a planning unit, configured to plan a first trajectory based on the first environmental information, wherein the first trajectory is used to instruct the mobile mechanical arm to avoid the obstacle when moving; An updating unit, configured to update the first environment information to obtain second environment information; An optimization unit is used to optimize the first trajectory by constructing an optimal control problem based on the second environmental information and combining a trajectory tracking cost and an obstacle avoidance cost, wherein the trajectory tracking cost is used to predict whether the mobile robotic arm will collide with the obstacle, and the obstacle avoidance cost is used to instruct the mobile robotic arm to avoid the obstacle when moving.

11. The device according to claim 10, characterized in that The optimization unit is specifically used for: The optimal control problem is constructed based on the following formula: in min <u<u max x min <x<x max The parameters in the formula are obtained based on the second environment information, where: is the penalty term of the end point of the first trajectory, x is the state parameter of the mobile robot, the state parameter includes the moving angle of the chassis and the joint angle of the robot, T is the time, x T is the state parameter at T, x init is the first trajectory, Q is the penalty weight, o(x) is the obstacle avoidance cost, (x init ) T Q(xx init ) is the trajectory tracking cost, st is the constraint condition of the optimal control problem, u is the control parameter of the mobile robotic arm, and the control parameters include the linear velocity and angular velocity of the chassis and the joint velocity of the robotic arm.

12. The device according to claim 10 or 11, characterized in that The optimal control problem also includes a motion smoothing cost, and the motion smoothing cost is used to smooth the first trajectory.

13. The device according to any one of claims 10 to 12, characterized in that The optimal control problem also includes an operability optimization cost, and the operability optimization cost is used to improve the operability of the robotic arm.

14. The device according to any one of claims 10 to 13, characterized in that The device also includes: The solving unit is used to solve the optimal control problem through differential dynamic programming to obtain a second trajectory.

15. The device according to any one of claims 10 to 14, characterized in that The updating unit is specifically used for: When the position of the obstacle changes, the first environment information is updated.

16. The device according to any one of claims 10 to 15, characterized in that The determining unit is specifically used for: Acquire first point cloud information, where the first point cloud information includes information about the mobile robotic arm and information about the obstacle; Filtering out the information of the mobile mechanical arm in the first point cloud information to obtain second point cloud information; Generate a Euclidean signed distance field of the obstacle based on the second point cloud information; The first environmental information is determined based on the Euclidean signed distance field.

17. The device according to any one of claims 10 to 16, characterized in that The first trajectory includes a moving trajectory of the chassis and a moving trajectory of the robotic arm, and the planning unit is specifically used for: Planning a moving trajectory of the chassis based on the first environmental information, wherein the moving trajectory of the chassis is used to instruct the chassis to avoid the obstacle when moving; A moving trajectory of the robotic arm is planned based on the first environmental information and the moving trajectory of the chassis, and the moving trajectory of the robotic arm is used to instruct the robotic arm to avoid the obstacle when moving.

18. The device according to any one of claims 10 to 17, characterized in that The chassis is a chassis with non-holonomic constraints.

19. An obstacle avoidance 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 9.

20. 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, the method according to any one of claims 1 to 9 is implemented.

21. A computer program product comprising instructions, characterized in that When the computer program product is run on a computer, the computer is caused to execute the method according to any one of claims 1 to 9.

Images