WO2023024317A1

WO2023024317A1 - Robot obstacle avoidance method and apparatus, and robot

Info

Publication number: WO2023024317A1
Application number: PCT/CN2021/135758
Authority: WO
Inventors: 刘益彰; 熊友军; 罗璇; 张志豪; 葛利刚; 陈春玉
Original assignee: 深圳市优必选科技股份有限公司
Priority date: 2021-08-24
Filing date: 2021-12-06
Publication date: 2023-03-02
Also published as: CN113618742A; CN113618742B

Abstract

A robot obstacle avoidance method, a robot obstacle avoidance apparatus (100), and a robot. The robot obstacle avoidance method comprises: detecting, according to the position of an obstacle, whether a predicted collision point is present in all linkages of corresponding joint shafts along a robot end direction, and if a predicted collision point is present, calculating a rotating angle between the predicted collision point on the corresponding linkage and the obstacle, wherein the rotating angle is used as an anti-collision angle limit of the joint where the corresponding joint shaft is located (S110); determining an angle constraint of the joint on the basis of a physical angle limit and the anti-collision angle limit of the joint (S120); solving, according to the angle constraint of each joint of the robot and a self-constraint of each joint angular velocity, an obstacle avoidance optimization function with joint angular velocity as an optimization variable and tail end velocity as a control target to obtain an optimal solution of the joint angular velocity (S130); and performing motion control on the robot by using the optimal solution of the joint angular velocity (S140).

Description

A robot obstacle avoidance method, device and robot

Cross References to Related Applications

This application claims the priority of the Chinese patent application with the application number 2021109770276 and titled "A Method, Device and Robot for Robot Obstacle Avoidance" submitted to the China Patent Office on August 24, 2021, the entire contents of which are incorporated herein by reference. Applying.

technical field

The present application relates to the technical field of robot control, in particular to a robot obstacle avoidance method, device and robot.

Background technique

With the expansion of the application scope of the robot, the working environment of the robot is also changeable. Various static or dynamic obstacles often appear within the working space of the robot. Obstacles collide, resulting in damage to the robot body or obstacles. Although the distance between the robot and the environment can be dynamically monitored based on visual equipment, collisions can be predicted to a certain extent, but it is difficult to replan the trajectory in real time according to the dynamic relative pose, and it is difficult for the general obstacle avoidance algorithm to ensure that the obstacle is avoided while ensuring the completion of the task.

application content

The embodiment of the present application provides a robot obstacle avoidance method, device and robot. The robot obstacle avoidance method determines the joint position or angle constraint of the robot by using the joint's own limit and obstacles, and combines the joint angular velocity constraint to jointly control the constructed The obstacle avoidance optimization problem is solved to ensure that the obtained solution can complete the terminal speed task as much as possible while completing the obstacle avoidance.

Embodiments of the present application provide a robot obstacle avoidance method, including:

According to the position of the obstacle, detect whether there is a predicted collision point in all the links corresponding to the joint axis along the direction of the end of the robot, and when there is a predicted collision point, calculate the distance between the predicted collision point on the corresponding link and the obstacle. Rotation angle, the rotation angle is used as the anti-collision angle limit of the joint where the corresponding joint axis is located;

determining an angle constraint of the joint based on the physical angle limit of the joint and the anti-collision angle limit;

According to the angular constraints of each joint of the robot and the self-constraints of each joint angular velocity, the obstacle avoidance optimization function with the joint angular velocity as the optimization variable and the terminal velocity as the control target is solved to obtain the optimal solution of the joint angular velocity;

The motion control of the robot is performed by using the optimal solution of the joint angular velocity.

In some embodiments, when the robot is a planar motion robot, the detection according to the position of the obstacle whether there is a predicted collision point of all the connecting rods corresponding to the joint axis along the direction of the end of the robot includes:

calculating the distance from the corresponding joint axis to the obstacle according to the position of the obstacle;

When locking all the joints of the corresponding joint axis along the direction of the robot base, with the corresponding joint axis as the origin and the distance as the radius, calculate the movement plane of the obstacle along the connecting rod connected to the corresponding joint axis Rotation trajectory of virtual rotation;

When there is an intersection point between the rotation track and any connecting rod, the intersection point is determined to be a predicted collision point.

In some embodiments, when the robot is a space-moving robot, the detection according to the position of the obstacle whether there is a predicted collision point for all the connecting rods corresponding to the joint axis along the direction of the end of the robot includes:

calculating the vertical distance from the corresponding joint axis to the obstacle according to the position of the obstacle;

expanding the obstacle to a preset length in a direction parallel to the corresponding joint axis to obtain an expanded area of the obstacle;

When locking all the joints of the corresponding joint axis along the direction of the robot base, using the corresponding joint axis as the origin and the vertical distance as the radius, calculate the rotation trajectory of the virtual rotation of the expansion area around the corresponding joint axis;

In some embodiments, the calculating the rotation angle between the predicted collision point on the corresponding link and the obstacle includes:

Calculate a first rotation angle at which the obstacle rotates clockwise to the predicted collision point according to the distance from the corresponding joint axis to the obstacle and the position of the predicted collision point, and the first rotation angle is used as Anti-collision angle upper limit;

A second rotation angle at which the obstacle rotates counterclockwise to the predicted collision point is calculated according to the first rotation angle, and the second rotation angle is used as a lower limit of an anti-collision angle.

In some embodiments, the physical angle limit of the joint includes the upper limit of the physical angle and the lower limit of the physical angle of the joint, and the determination based on the physical angle limit of the corresponding joint of the robot and the anti-collision angle limit Angle constraints of the corresponding joints described above, including:

Selecting a minimum value from the upper limit of the physical angle and the sum of the joint angle at the current moment and the upper limit of the anti-collision angle as the upper limit of the joint angle constraint;

A maximum value is selected from the lower limit of the physical angle and the difference between the joint angle at the current moment and the lower limit of the anti-collision angle as the lower limit of the joint angle constraint.

In some embodiments, when there are multiple obstacles, if there are two or more predicted collision points, the rotation angle between each predicted collision point and the obstacle is calculated , each group of rotation angles is used as multiple anti-collision angle limits of the joint where the corresponding joint axis is located; the multiple anti-collision angle limits are used to determine the corresponding joint with the physical angle limit of the corresponding joint angle constraints.

In some embodiments, the multiple anti-collision angle limits and the physical angle limits of the corresponding joints determine the angle constraints of the corresponding joints according to the following rules:

Select an upper minimum value from the upper limits of the multiple anti-collision angles, and select a minimum value from the sum of the joint angle of the corresponding joint at the current moment and the upper minimum value, and the upper limit of the physical angle of the corresponding joint. , as the upper limit of the angle constraint of the corresponding joint;

Select the maximum value of the lower limit from the lower limits of the multiple anti-collision angles, and select the maximum value from the difference between the joint angle of the corresponding joint at the current moment and the maximum value of the lower limit, and the lower limit of the physical angle of the corresponding joint. , as the lower limit of the angle constraints of the corresponding joints.

In some embodiments, the obstacle avoidance optimization function uses the square of the slack variable as the optimization index; the expression of the obstacle avoidance optimization function is as follows:

Wherein, w is the slack variable, J is the speed Jacobian matrix of the robot,

is the joint angular velocity vector of all joints of the robot;

is the joint terminal velocity vector of the robot;

and

are the upper limit and lower limit of the joint angular velocity of the i-th joint, respectively;

and

are the upper limit and lower limit of the angle constraints of the i-th joint, respectively;

is the joint angular velocity of the i-th joint; T is the control command period of the robot;

is the joint angle of the i-th joint at time t.

Embodiments of the present application also provide a robot obstacle avoidance device, including:

The collision detection module is used to detect, according to the position of the obstacle, whether there is a predicted collision point on all the connecting rods corresponding to the joint axis along the direction of the end of the robot, and when there is a predicted collision point, calculate the relationship between the predicted collision point on the corresponding connecting rod and the predicted collision point. The rotation angle between the obstacles, the rotation angle is used as the anti-collision angle limit of the joint where the corresponding joint axis is located;

An angle constraint determination module, configured to determine the angle constraint of the joint based on the physical angle limit of the joint and the anti-collision angle limit;

The optimization solution module is used to solve the obstacle avoidance optimization function with the joint angular velocity as the optimization variable and the terminal velocity as the control target according to the angular constraints of each joint of the robot and the self-constraints of each joint angular velocity, to obtain the joint angular velocity the optimal solution;

The motion control module is used to control the motion of the robot by using the optimal solution of the joint angular velocity.

An embodiment of the present application also provides a robot, the robot includes a processor and a memory, the memory stores a computer program, and the processor is used to execute the computer program to implement the above robot obstacle avoidance method.

The embodiment of the present application also provides a readable storage medium, which stores a computer program, and when the computer program is executed by a processor, implements the above-mentioned robot obstacle avoidance method.

Embodiments of the application have the following beneficial effects:

The obstacle avoidance method of the robot in the embodiment of the present application uses obstacles as the joint position or angle constraints of the robot, and at the same time considers the self-limiting of the robot joints to determine the joint angle limit, and also combines the self-constraints of the joint angular velocity to control the obstacle avoidance. The optimization problem solves the joint angular velocity to ensure that the obtained solution can complete obstacle avoidance; and because the obstacle avoidance optimization problem takes the terminal velocity as the control target, it also completes the task of following the terminal velocity as much as possible while performing obstacle avoidance.

Description of drawings

In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following will briefly introduce the accompanying drawings used in the embodiments. It should be understood that the following drawings only show some embodiments of the present application, so It should be regarded as a limitation on the scope, and those skilled in the art can also obtain other related drawings based on these drawings without creative work.

Fig. 1 shows the schematic flow chart of the robot obstacle avoidance method of the embodiment of the present application;

FIG. 2 shows a schematic flow diagram of a planar robot detecting and predicting a collision point in a robot obstacle avoidance method according to an embodiment of the present application;

Fig. 3 a and Fig. 3 b respectively show the schematic diagram that the planar robot of the robot obstacle avoidance method of the embodiment of the present application collides with an obstacle and a plurality of obstacles;

FIG. 4 shows a schematic flow chart of detecting and predicting a collision point for a space-moving robot in a robot obstacle avoidance method according to an embodiment of the present application;

Fig. 5 shows a schematic diagram of a space robot colliding with an obstacle in the robot obstacle avoidance method of the embodiment of the present application;

Fig. 6 shows a schematic structural diagram of a robot obstacle avoidance device according to an embodiment of the present application.

Detailed ways

The following will clearly and completely describe the technical solutions in the embodiments of the present application with reference to the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are only some of the embodiments of the present application, not all of them.

The components of the embodiments of the application generally described and illustrated in the figures herein may be arranged and designed in a variety of different configurations. Accordingly, the following detailed description of the embodiments of the application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely represents selected embodiments of the application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without making creative efforts belong to the scope of protection of the present application.

The quadratic programming problem is mainly the process of selecting the optimal solution from multiple solutions under the constraints of equality and inequality. Exemplarily, the main form of the quadratic programming problem is as follows:

in,

is an optimization index, H is a Hessian matrix; x is an n-dimensional optimization variable; f is a row vector; A _eq x=b _eq is an equality constraint, A _eq is an mxn (m≤n) dimensional matrix, and b _eq is m Row and column vector; Ax≤b is an inequality constraint, A is a matrix with n columns, and b is a column vector.

For robots, the range of motion and flexibility of the robot are often increased by adding redundant joints. The existing processing of redundant joints is generally very complicated and takes a long time to calculate, and there are certain limitations in practical use. In this regard, considering the characteristics of the robot's own structure, it often has various constraints such as joint angles, joint angular velocities, and joint torques in different application scenarios. Optimal kinematics inverse solution, the embodiment of this application will use the quadratic programming problem to solve the obstacle avoidance optimization.

The robot obstacle avoidance method proposed in the embodiment of this application constructs an obstacle avoidance optimization function with the joint angular velocity as the optimization variable and the terminal velocity as the control target, adding obstacles as the position or angle limit of the robot and considering the physical properties of the robot joints. limit to obtain the angle constraints of the corresponding joints, and then add the joint angle constraints and the joint angular velocity's own constraints as the constraints of the obstacle avoidance optimization function to optimally solve the obstacle avoidance optimization function. The solution obtained in this way makes While avoiding obstacles, the robot also completes the terminal speed following task as much as possible. The following will be described in conjunction with specific embodiments.

Example 1

Referring to FIG. 1 , this embodiment proposes an obstacle avoidance method for a robot, which can be used for obstacle avoidance in different scenarios with one or more obstacles.

Exemplarily, the robot obstacle avoidance method includes:

Step S110, according to the position of the obstacle, detect whether there is a predicted collision point on all the links of the corresponding joint axis along the direction of the end of the robot, and if there is a predicted collision point, calculate the distance between the predicted collision point on the corresponding link and the obstacle Rotation angle. Wherein, the rotation angle is used as the anti-collision angle limit of the joint where the corresponding joint axis is located.

Wherein, the above-mentioned predicted collision point position refers to the predicted position where the robot will come into contact with the obstacle. In this embodiment, the position of the obstacle is transformed into a constraint on the joint angle, and then used as a constraint condition of the obstacle avoidance optimization function, so that the obtained solution can avoid collision with the obstacle.

According to the type of motion of the robot, it is mainly divided into planar motion robots (abbreviated as planar robots) and space motion robots (referred to as space robots). For example, some mechanical arms used for plane grinding are usually planar robots; Workshop processing manipulators, biped robots, etc. that perform relatively complex operations in three-dimensional space are usually space robots, but of course they are not limited thereto.

Taking a planar robot as an example, in one embodiment, as shown in Figure 2, the detection process of whether there is a predicted collision point in the above step S110 includes:

Sub-step S210, calculating the distance from the corresponding joint axis to the obstacle according to the position of the obstacle.

Usually, for fixed obstacles, the robot can obtain them in advance; for dynamic obstacles with indefinite positions, the robot can obtain them through real-time communication with external devices or real-time detection using sensory sensors, etc., which is not limited here. It can be understood that the obstacle avoidance method of this embodiment may be applicable to obstacle avoidance of static obstacles and also applicable to obstacle avoidance of dynamic obstacles, and the principle is the same.

In order to convert the position of the obstacle into the joint constraint of the corresponding joint, this embodiment calculates the distance from the obstacle to each joint axis of the robot, and judges whether the joint where the corresponding joint axis is located needs to add the corresponding obstacle constraint. Exemplarily, a known obstacle can be regarded as a point in the plane, and for a joint axis, it is recorded as the origin, and the coordinates of the two points are used to calculate the distance from the obstacle point to the origin of the current joint axis.

Sub-step S220, when locking all the joints of the corresponding joint axis along the direction of the robot base, take the corresponding joint axis as the origin and the above-mentioned distance as the radius, calculate the virtual rotation of the obstacle along the link motion plane connected by the corresponding joint axis rotation trajectory.

In this embodiment, for the obstacle constraint addition of the joint where a joint axis is located, it is only necessary to detect the collision between the subsequent link of the joint and the obstacle. For example, as shown in Figure 3a, for a joint to be constrained by an obstacle, the corresponding joint axis is marked as the origin O, and P is the position of the obstacle. Exemplarily, when the corresponding joint axis is locked along the direction of the robot base In the case of all joints, that is, when the angles of all joints between the base and the joint are fixed at the current position, with O as the origin and OP as the radius, virtual rotation is performed in the connecting rod motion plane to obtain the corresponding rotation trajectory.

It can be understood that if there is an intersection point between the virtual rotation trajectory and any connecting rod between the current joint and the end of the robot, it means that the joint where the current joint axis is located will collide with an obstacle when rotating. Conversely, if there is no intersection point, it means that the joint will never encounter obstacles when it rotates, and there is no need to add obstacle constraints to the joint at this time.

Sub-step S230, when there is an intersection point between the rotation track and any connecting rod, determine the intersection point as the predicted collision point.

Exemplarily, if there is a predicted collision point, such as the intersection point C shown in Figure 3a, when calculating the rotation angle between the predicted collision point C on the corresponding link and the obstacle P, the joint axis O can be used to The distance of the obstacle P and the position of the predicted collision point C are calculated using the triangle principle to calculate the rotation angle between the obstacle and the predicted collision point. Wherein, the rotation angle will be used as the anti-collision angle limit of the joint where the joint axis O is located.

Wherein, the above-mentioned rotation may include clockwise rotation and counterclockwise rotation. In one embodiment, the obstacle is rotated clockwise to the first rotation angle of the predicted collision point

As the upper limit of the anti-collision angle limit; and, according to the first rotation angle

Calculate the counterclockwise rotation of the obstacle to the second rotation angle of the predicted collision point

Take it as the lower limit of the anti-collision angle limit. Therefore, the anti-collision angle limit of the joint where the joint axis O is located can be expressed as

Optionally, an obstacle may intersect with multiple links at the same time. At this time, the corresponding rotation angle can be calculated for each predicted collision point as the multiple anti-collision angle limits bit. Alternatively, when there are multiple obstacles, if there are two or more predicted collision points, as shown in Figure 3b, the rotation angle between each predicted collision point and the obstacle is calculated, and the calculated Each group of rotation angles will be used as multiple anti-collision angle limits for the joint where the corresponding joint axis is located.

In another embodiment, if the robot is a space-moving robot, as shown in FIG. 4 , the detection process of whether there is a predicted collision point in the above step S110 includes:

Sub-step S310, calculating the vertical distance from the corresponding joint axis to the obstacle according to the position of the obstacle. Exemplarily, reference may be made to the above-mentioned step S210 for details, and the description will not be repeated here.

In sub-step S320, the obstacle is expanded to a preset length in a direction parallel to the corresponding joint axis to obtain an expanded area of the obstacle.

Considering that obstacles usually have a certain shape in three-dimensional space, in order to more accurately reflect the collision between the robot and the obstacle with a certain shape, this embodiment will expand the obstacle. The expansion is performed according to a preset length in a direction parallel to the corresponding joint axis to obtain a corresponding expansion area, as shown in FIG. 5 . Furthermore, the expanded region of the obstacle can be used for collision detection. It can be understood that the preset length can be determined by the actual shape of the obstacle.

Sub-step S330, when locking all the joints with the corresponding joint axis along the direction of the robot base, take the corresponding joint axis as the origin and the above-mentioned vertical distance as the radius, and calculate the virtual rotation trajectory of the expansion area around the corresponding joint axis.

The principle of sub-step S330 is similar to that of the above-mentioned step S120. The only difference between them is that in sub-step S120, an obstacle point rotates in the plane of motion of the connecting rod, while in sub-step S330, an expansion area rotates around a joint axis, as shown in Figure 5 .

Sub-step S340, when there is an intersection point between the rotation trajectory and any connecting rod, then determine the intersection point as the predicted collision point.

Exemplarily, if there is an intersection point between the rotation track and any link between the joint where the current joint axis is located and the end, such as intersection point C shown in FIG. 5 , the intersection point C is the predicted collision point. Furthermore, the corresponding rotation angle can also be calculated by using the triangle principle according to the vertical distance R, the preset length L, etc.

The principle is similar to the calculation method of the rotation angle of the above-mentioned planar motion robot, and will not be described again here.

It can be understood that by using the rotation angle between the obstacle and the predicted collision point as the anti-collision angle limit of the joint where the corresponding joint axis is located, the position of the obstacle can be added as the angle constraint of the robot joint. It can be seen that this method is not only suitable for plane-moving robots, but also suitable for space-moving robots, and has good versatility.

Step S120, determining the angle constraint of the joint based on the physical angle limit and the anti-collision angle limit of the joint.

In this embodiment, the angle constraint of the joint is jointly determined by taking the obstacle as the angle constraint of the robot and considering the physical limit of the robot's joint itself, and adding it to the constraints of the constructed obstacle avoidance optimization function, so that It is guaranteed that the obtained kinematic inverse solution will not collide with obstacles and can satisfy its own structural constraints. It can be understood that if the corresponding joint does not have the above-mentioned anti-collision angle limit constraint, then the angle constraint of the joint only needs to consider the physical angle limit of the joint.

Regarding the joint angle self-constraint, due to the limitation of the joint structure of the robot, some joint positions or angles are unreachable, so there will be joint physical limitations. For the translational joints of the robot, since there is no rotation, the physical limit of the joint mainly refers to the limit of the joint position; for the rotational joint, the physical limit of the joint mainly refers to the limit of the joint angle. It can be understood that the obstacle avoidance method in this embodiment can be applied to a robot of a rotary joint type and a robot of a translational joint type, and the principle is similar.

Generally, the angle physical limit of a joint includes a physical angle upper limit and a physical angle lower limit. Likewise, the angle constraint includes the upper limit of the above-mentioned angle constraint and the lower limit of the angle constraint, and for the above-mentioned step S120, exemplarily, includes:

Select the minimum value from the upper limit of the physical angle, the sum of the joint angle at the current moment and the upper limit of the anti-collision angle as the upper limit of the angle constraint; and, from the lower limit of the physical angle, the joint angle at the current moment and the lower limit of the anti-collision angle The maximum value is selected from the difference as the lower limit of the angle constraint.

For example, if there is a predicted collision point, the angle constraint of joint i corresponding to the current joint axis can be determined as follows: from the upper limit of the physical angle of the joint i

And the joint angle of joint i at time t

Upper limit of anti-collision angle

The sum of these two values, choose the minimum value as the upper limit of the angle constraint of the joint i; and, from the lower limit of the physical angle of the joint i

And the joint angle of joint i at time t

and anti-collision angle lower limit

The difference between these two values, the maximum value is selected as the lower limit of the angle constraint of the joint i. Exemplarily, if expressed in a formula, then:

in,

and

are the upper limit and lower limit of the anti-collision angle corresponding to the predicted collision point;

and

are the upper and lower limits of the physical angle of joint i, respectively;

is the joint angle of joint i at time t;

and

are the upper and lower bounds of the angle constraints of joint i, respectively.

In another embodiment, if there are two or more predicted collision points, the rotation angle between each predicted collision point and the obstacle is calculated, and each set of calculated rotation angles will be used as the joint axis where the joint axis is located. Multiple anti-collision angle limits for joints. Therefore, the multiple anti-collision angle limits and the physical angle limits of the corresponding joints are used to jointly determine the angle constraints of the corresponding joints.

For example, if there are k predicted collision points for a certain joint i, correspondingly, there are k groups of anti-collision angle limits, which are determined by referring to the above-mentioned method of having one predicted collision point. Similarly, for the angle constraint of the i-th joint The upper limit of the upper limit of the k anti-collision angle can be determined first, and then from the minimum value of the upper limit and the sum of the joint angle of the joint at time t, and the upper limit of the physical angle of joint i

The smaller value is selected as the upper limit of the angle constraint. As for the lower limit of the angle constraint of the i-th joint, the maximum value of the lower limits of the k anti-collision angles can be determined first, and then from the difference between the joint angle of the joint at time t and the maximum value of the lower limit, and the physical Angle lower limit

The larger value is selected as the lower limit of the angle constraint. If expressed by the formula, there are:

in,

and

are the upper limit and lower limit of the anti-collision angle corresponding to the jth obstacle; k is the number of predicted collision points;

and

are the upper and lower limits of the physical angle of the i-th joint, respectively;

is the joint angle of the i-th joint at time t;

and

are the upper and lower bounds of the angle constraints of the i-th joint, respectively.

Step S130, according to the angular constraints of each joint of the robot and the self-constraints of each joint angular velocity, solve the obstacle avoidance optimization function with the joint angular velocity as the optimization variable and the terminal velocity as the control target to obtain the optimal solution of the joint angular velocity.

In order to ensure that the end of the robot can preferentially perform tasks in the Cartesian space under multiple constraints, this embodiment will pre-build an obstacle avoidance optimization function with the joint angular velocity as the control variable and the end speed as the control target. Among them, The obstacle avoidance optimization function will add corresponding constraints related to obstacles, of course, other constraints can also be added, for example, it can include but not limited to the robot's own physical constraints including the joint angle or joint position, the joint angular velocity's own physical constraints Constraints, etc. More constraints can be added according to actual needs.

Taking a robot with rotary joints as an example, the following is an illustration of adding the robot's joint angle constraints as the constraints of the obstacle avoidance optimization function. Assume that the upper limit of the angle constraint of the i-th joint is

The lower limit of the angle constraint is

The control command period of the robot is T,

is the joint angle of the i-th joint at time t,

is the joint angular velocity of the i-th joint, then the joint angle at time t+1

The joint angle constraints should be satisfied, as follows:

It is worth noting that here is the joint angle constraint that considers obstacles. If joint i does not have the above-mentioned anti-collision angle limit, that is, it will not collide with obstacles, then in the formula

and

can be replaced by

and

Furthermore, since there is a conversion relationship between the joint angle and the joint angular velocity of the robot, for the convenience of calculation, it is converted into a constraint equation with the joint angular velocity as a variable. Exemplarily, when converting joint angle constraints into joint angular velocity constraints, there are:

In addition, for the self-constraint conditions of each joint angular velocity, for example, if the upper limit of the i-th joint angular velocity is

The lower limit is

Then the i-th joint angular velocity

Should meet:

In this embodiment, since the angular velocity of each joint at the same time needs to satisfy multiple constraints at the same time, in order to facilitate the optimal solution of the obstacle avoidance optimization function, multiple constraint conditions will be synthesized here so as to be used together with other constraint equations as the above-mentioned avoidance Different constraints for barrier optimization functions.

Exemplarily, taking the constraint conditions of joint angle and joint angular velocity added above including obstacles as an example, the following synthetic constraint equation for joint angular velocity can be obtained:

In this embodiment, the above-mentioned obstacle avoidance optimization function will introduce a slack variable, and an index to be optimized is obtained based on the slack variable. For example, the optimization index may be the square of the slack variable, etc., of course, may also take other forms related to the slack variable, which are not limited here.

It can be understood that taking the slack variable as the optimization index can ensure that even if the obstacle avoidance optimization function conflicts with the constraint conditions, it will still solve the approximate solution of the constraint equation, so as to ensure that while giving priority to obstacle avoidance, Enable the terminal to complete the corresponding terminal speed following task.

In one embodiment, the square of the slack variable is used as the optimization index, and the above-mentioned composite constraint of joint angular velocity including obstacles is taken as an example. At this time, the expression of the obstacle avoidance optimization function is as follows:

Exemplarily, some open source solvers can be used to solve the quadratic programming, so as to obtain the optimal joint angular velocity. It can be understood that when ||w|| ² =0, it means

has a solution, that is, there are joint angular velocities under the constraints

The terminal velocity of the robot is

And when ||w|| ² >0,

No solution, the joint angular velocity obtained from the solution

yes

The closest solution of is

solution. By introducing slack, it can be guaranteed that the obstacle avoidance optimization function will not appear to be wrong.

Step S140, using the optimal solution of the joint angular velocity to control the motion of the robot.

Exemplarily, after obtaining an optimal joint angular velocity, the joint angular velocity may be used for integral processing to obtain an optimal joint angle or joint position. Furthermore, the calculated joint angles or joint positions are used as control commands and sent to the corresponding joint motors, so that the robot can realize obstacle avoidance motion. At the same time, the end of the robot can also perform corresponding speed following tasks.

The robot obstacle avoidance method of this embodiment uses obstacles as the joint angle or position constraints of the robot, and combines the joint angle or position, and the joint angular velocity's own constraints to solve the joint angular velocity of the constructed obstacle avoidance optimization problem to ensure that the obtained The solution can complete obstacle avoidance; at the same time, because the obstacle avoidance optimization problem takes the terminal speed as the control target, it can also complete the terminal speed following task as much as possible while avoiding obstacles. The obstacle avoidance method can be applied to various scenarios such as one or more obstacles, dynamic or static obstacles, etc., and has universality. In addition, this method has nothing to do with the joint type and motion type of the robot, and it is universal. Whether it is a robot with rotational joints or translational joints, or a planar motion robot or a space motion robot, this method can be used for joint The angular velocity is optimized to achieve the purpose of obstacle avoidance control.

Example 2

Please refer to FIG. 6. Based on the method of the above-mentioned embodiment 1, this embodiment proposes a robot obstacle avoidance device 100. Exemplarily, the robot obstacle avoidance device 100 includes:

The collision detection module 110 is used to detect, according to the position of the obstacle, whether there is a predicted collision point on all the links of the corresponding joint axis along the direction of the end of the robot, and when there is a predicted collision point, calculate the relationship between the predicted collision point on the corresponding link and The rotation angle between the obstacles, the rotation angle is used as the anti-collision angle limit of the joint where the corresponding joint axis is located.

An angle constraint determining module 120, configured to determine the angle constraint of the corresponding joint based on the anti-collision angle limit of the joint and the physical angle limit of the corresponding joint.

The optimization solving module 130 is used to solve the obstacle avoidance optimization function with the joint angular velocity as the optimization variable and the terminal velocity as the control target according to the angular constraints of each joint of the robot and the self-constraints of each joint angular velocity, to obtain the joint Optimal solution for angular velocity.

The motion control module 140 is configured to use the optimal solution of the joint angular velocity to control the motion of the robot.

It can be understood that the device in this embodiment corresponds to the method in the above-mentioned embodiment 1, and the optional items in the above-mentioned embodiment 1 are also applicable to this embodiment, so the description will not be repeated here.

The present application also provides a robot, for example, the robot may be a robot with multiple degrees of freedom. Exemplarily, the robot includes a processor and a memory, wherein the memory stores a computer program, and the processor executes the computer program to enable the mobile terminal to execute the above robot obstacle avoidance method or the various modules in the above robot obstacle avoidance device function.

The present application also provides a readable storage medium for storing the computer program used in the above robot.

In the several embodiments provided in this application, it should be understood that the disclosed devices and methods may also be implemented in other ways. The device embodiments described above are only illustrative. For example, the flow charts and structural diagrams in the accompanying drawings show the possible implementation architecture and functions of devices, methods and computer program products according to multiple embodiments of the present application. and operation. In this regard, each block in a flowchart or block diagram may represent a module, program segment, or part of code that includes one or more Executable instructions. It should also be noted that, in alternative implementations, the functions noted in the block may occur out of the order noted in the figures. For example, two blocks in succession may, in fact, be executed substantially concurrently, or they may sometimes be executed in the reverse order, depending upon the functionality involved. It is also to be noted that each block of the block diagrams and/or flow diagrams, and combinations of blocks in the block diagrams and/or flow diagrams, can be implemented by a dedicated hardware-based system that performs the specified function or action. may be implemented, or may be implemented by a combination of special purpose hardware and computer instructions.

In addition, each functional module or unit in each embodiment of the present application may be integrated to form an independent part, each module may exist independently, or two or more modules may be integrated to form an independent part.

If the functions described are realized in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or the part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including Several instructions are used to make a computer device (which may be a smart phone, a personal computer, a server, or a network device, etc.) execute all or part of the steps of the methods described in the various embodiments 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), magnetic disk or optical disc, etc., which can store program codes. .

The above is only a specific implementation of the application, but the scope of protection of the application is not limited thereto. Anyone familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed in the application. Should be covered within the protection scope of this application.

Claims

A robot obstacle avoidance method is characterized in that, comprising:

According to the position of the obstacle, detect whether there is a predicted collision point in all the links corresponding to the joint axis along the direction of the end of the robot, and when there is a predicted collision point, calculate the distance between the predicted collision point on the corresponding link and the obstacle. Rotation angle, the rotation angle is used as the anti-collision angle limit of the joint where the corresponding joint axis is located;

determining an angle constraint of the joint based on the physical angle limit of the joint and the anti-collision angle limit;

According to the angular constraints of each joint of the robot and the self-constraints of each joint angular velocity, the obstacle avoidance optimization function with the joint angular velocity as the optimization variable and the terminal velocity as the control target is solved to obtain the optimal solution of the joint angular velocity;

The motion control of the robot is performed by using the optimal solution of the joint angular velocity.
The robot obstacle avoidance method according to claim 1, wherein when the robot is a planar motion robot, detecting whether there is a predicted collision point for all the connecting rods whose corresponding joint axes are along the direction of the end of the robot according to the position of the obstacle ,include:

calculating the distance from the corresponding joint axis to the obstacle according to the position of the obstacle;

When locking all the joints of the corresponding joint axis along the direction of the robot base, with the corresponding joint axis as the origin and the distance as the radius, calculate the movement plane of the obstacle along the connecting rod connected to the corresponding joint axis Rotation trajectory of virtual rotation;

When there is an intersection point between the rotation track and any connecting rod, the intersection point is determined to be a predicted collision point.
The robot obstacle avoidance method according to claim 1, wherein when the robot is a space-moving robot, detecting whether there is a predicted collision point for all the connecting rods corresponding to the joint axis along the direction of the robot end according to the position of the obstacle ,include:

calculating the vertical distance from the corresponding joint axis to the obstacle according to the position of the obstacle;

expanding the obstacle to a preset length in a direction parallel to the corresponding joint axis to obtain an expanded area of the obstacle;

When locking all the joints of the corresponding joint axis along the direction of the robot base, using the corresponding joint axis as the origin and the vertical distance as the radius, calculate the rotation trajectory of the virtual rotation of the expansion area around the corresponding joint axis;

When there is an intersection point between the rotation track and any connecting rod, the intersection point is determined to be a predicted collision point.
The robot obstacle avoidance method according to claim 2 or 3, wherein the calculation of the rotation angle between the predicted collision point on the corresponding link and the obstacle includes:

Calculate a first rotation angle at which the obstacle rotates clockwise to the predicted collision point according to the distance from the corresponding joint axis to the obstacle and the position of the predicted collision point, and the first rotation angle is used as Anti-collision angle upper limit;

A second rotation angle at which the obstacle rotates counterclockwise to the predicted collision point is calculated according to the first rotation angle, and the second rotation angle is used as a lower limit of an anti-collision angle.
The robot obstacle avoidance method according to claim 4, wherein the physical angle limit of the joint includes an upper limit and a lower limit of the physical angle of the joint, and the physical angle limit based on the corresponding joint of the robot Determining the angle constraints of the corresponding joints with the anti-collision angle limit, including:

Selecting a minimum value from the upper limit of the physical angle and the sum of the joint angle at the current moment and the upper limit of the anti-collision angle as the upper limit of the joint angle constraint;

A maximum value is selected from the lower limit of the physical angle and the difference between the joint angle at the current moment and the lower limit of the anti-collision angle as the lower limit of the joint angle constraint.
The robot obstacle avoidance method according to claim 1, wherein when there are multiple obstacles, if there are two or more predicted collision points, then calculate each predicted collision point The rotation angle between the obstacle and each group of rotation angles is used as a plurality of anti-collision angle limits of the joint where the corresponding joint axis is located; the plurality of anti-collision angle limits are used for all joints with the corresponding joint The physical angle limit determines the angle constraint of the corresponding joint.
The robot obstacle avoidance method according to claim 6, wherein the plurality of anti-collision angle limits and the physical angle limits of the corresponding joints determine the angle constraints of the corresponding joints according to the following rules:

Select an upper minimum value from the upper limits of the multiple anti-collision angles, and select a minimum value from the sum of the joint angle of the corresponding joint at the current moment and the upper minimum value, and the upper limit of the physical angle of the corresponding joint. , as the upper limit of the angle constraint of the corresponding joint;

Select the maximum value of the lower limit from the lower limits of the multiple anti-collision angles, and select the maximum value from the difference between the joint angle of the corresponding joint at the current moment and the maximum value of the lower limit, and the lower limit of the physical angle of the corresponding joint. , as the lower limit of the angle constraints of the corresponding joints.
The robot obstacle avoidance method according to claim 1, wherein the obstacle avoidance optimization function takes the square of the slack variable as the optimization index;

The expression of the obstacle avoidance optimization function is as follows:

min ||w|| 2

Wherein, w is the slack variable, J is the speed Jacobian matrix of the robot,
is the joint angular velocity vector of all joints of the robot;
is the joint terminal velocity vector of the robot;
and
are the upper limit and lower limit of the joint angular velocity of the i-th joint, respectively;
and
are the upper limit and lower limit of the angle constraints of the i-th joint, respectively;
is the joint angular velocity of the i-th joint; T is the control command cycle of the robot;
is the joint angle of the i-th joint at time t.
A robot obstacle avoidance device, characterized in that it comprises:

The collision detection module is used to detect, according to the position of the obstacle, whether there is a predicted collision point on all the connecting rods corresponding to the joint axis along the direction of the end of the robot, and when there is a predicted collision point, calculate the relationship between the predicted collision point on the corresponding connecting rod and the predicted collision point. The rotation angle between the obstacles, the rotation angle is used as the anti-collision angle limit of the joint where the corresponding joint axis is located;

An angle constraint determination module, configured to determine the angle constraint of the joint based on the physical angle limit of the joint and the anti-collision angle limit;

The optimization solution module is used to solve the obstacle avoidance optimization function with the joint angular velocity as the optimization variable and the terminal velocity as the control target according to the angular constraints of each joint of the robot and the self-constraints of each joint angular velocity, to obtain the joint angular velocity the optimal solution;

The motion control module is used to control the motion of the robot by using the optimal solution of the joint angular velocity.
A robot, characterized in that the robot comprises a processor and a memory, the memory stores a computer program, and the processor is used to execute the computer program to implement the robot according to any one of claims 1-8 Obstacle avoidance method.
A readable storage medium, characterized in that it stores a computer program, and when the computer program is executed by a processor, implements the robot obstacle avoidance method according to any one of claims 1-8.