CN104097205A - Task space based self-collision avoidance control method for real-time movements of robot - Google Patents

Abstract

The invention relates to a task space based self-collision avoidance control method for real-time movements of a robot. The method comprises the step of tracking an end trajectory of the robot and specifically comprises the following steps: (1) modeling by using a bounding volume according to three-dimensional geometric information of the robot, dividing a body of the robot into different region blocks, and carrying out self-collision detection on region pairs which are formed by every two region blocks; (2) if the condition that the region pairs can be subjected to self-collision is detected, creating a self-collision avoiding task, which is used for adjusting a movement trajectory, in a task space; (3) simultaneously completing the self-collision avoiding task and an end trajectory tracking task. Compared with the prior art, the method has the advantages that the occurrence of self-collision can be avoided during the real-time movement planning of the robot, and the field application of the robot is promoted.

Description

Robot real time kinematics self collision based on task space is avoided control method

Technical field

The present invention relates to a kind of robot control method, especially relate to a kind of robot real time kinematics self collision based on task space and avoid control method.

Background technology

Along with development, the robot application field of Robotics also starts to be extended to scene and the service fields such as family, hospital, the Aged Care center, military affairs, tourism, transportation, exploration, rescue and relief work from traditional factory's manufacture field.In the very long evolution of human society, applicable mankind's sense organ and behavioral trait that the environment of mankind's daily life is transformed gradually, this just requires to coexist with the mankind and serves outward appearance and the behavior processing transactions that the mankind's intelligent robot can imitate the mankind.From the beginning of the seventies in last century, since First bipod walking robot WAP-1 is born, bipod walking robot technology is one of hot research problem always, and people are constantly attempting producing more the similar bipod walking robot of walking with the mankind always.

In the motion planning process of robot, the object of planning mostly is the end orbit of robot, but owing to not considering the three-dimensional geometric information of robot, so may cause oneself's collision of robot, two of health positions bump.For example, in the process of walking, the arm of swing may bump with shank, and these collisions may cause the object of planning not complete, and even make robot disequilibrium or control, and robot and working environment are produced to damage.In the last few years, a lot of scholars and mechanism consider to have studied avoiding of self collision, by setting up the control algolithm of Obstacle avoidance model, as Artificial Potential Field Method (APF) and the method based on sampling, as quick expansion random tree (RRTs), probability map (PRMs) etc., realized the method for avoiding by the self collision of off-line learning, training.These class methods can be learnt the target trajectory of planning, make robot when carrying out trajectory planning, have avoided self collision.

But the research for self collision concentrates on the method that adopts study mostly at present, the high-dimensional configuration space (C-Space) that these class methods produce because joint multiple degrees of freedom is high for humanoid robot need to spend a large amount of computing times, is not suitable for real-time motion planning.

Summary of the invention

Object of the present invention is exactly to provide a kind of robot real time kinematics self collision based on task space to avoid control method in order to overcome the defect of above-mentioned prior art existence, the method can be in the process of robot real-time motion planning, avoid the generation of self collision, promoted the scene application of robot.

Object of the present invention can be achieved through the following technical solutions:

Robot real time kinematics self collision based on task space is avoided a control method, and the method comprises follows the tracks of the end orbit of robot, specifically comprises the following steps:

1) according to the three-dimensional geometric information of robot, utilizing enclosure body to carry out modeling, is different region units by the body segmentation of robot, and the region that every two region units are formed is to carrying out self-collision detection;

2) if region detected, be there is to self collision in meeting, in task space, increase by one and avoid task for adjusting the self collision of movement locus;

3) complete self collision simultaneously avoid task and end orbit tracing task.

Step 1) in, the detailed process of self-collision detection is:

11) according to the three-dimensional geometric information of robot, utilizing enclosure body to carry out modeling, is different region units by the body segmentation of robot, and every two region units are divided into one group of region pair;

12) candidate region pair that utilizes optimal method off-line learning to produce self collision, reduces the region that need to detect while detecting in real time to quantity,

13) adopt declutch shaft method to candidate region to carrying out collision status detection.

Compared with prior art, the present invention represents by setting up three-dimensional space model, to there is the region unit of self collision, detect and estimate, by off-line learning, introduce collision candidate region pair simultaneously, and distribute and realize that end orbit is followed the tracks of and self collision is avoided simultaneously by setting up dynamic task space, make robot in the process of planning in real time, avoided the generation of self collision, promoted the scene application of robot.

Accompanying drawing explanation

Fig. 1 is the main flow chart that self collision of the present invention is avoided;

The set figure that Fig. 2 is the region unit set up according to robot three-dimensional geological information;

Fig. 3 is the region schematic diagram that internally distance is calculated.

The specific embodiment

Below in conjunction with the drawings and specific embodiments, the present invention is described in detail.

Embodiment

Robot real time kinematics self collision based on task space is avoided a control method, and the end orbit of the method Han Dui robot is followed the tracks of, concrete step as shown in Figure 1:

1) according to the three-dimensional geometric information of robot, utilizing enclosure body to carry out modeling, is different region units by the body segmentation of robot, and the region that every two region units are formed is to carrying out self-collision detection,

Wherein, self-collision detection has comprised following step:

11) according to the three-dimensional geometric information of robot, utilizing enclosure body to carry out modeling, is N region unit by the body segmentation of robot as shown in Figure 2.Then will from N region unit, get arbitrarily two region units and be defined as region pair detect like this robot whether occurred self collision can be defined as detect each region between distance whether be less than threshold values, the region pair that need to detect each utilization declutch shaft (SAT) algorithm can be in the hope of this region to two of upper arest neighbors some D _iand D _j, the distance between 2 can be expressed as wherein a D _ilocational space in coordinate system vector, solving of it can be by try to achieve, can find out that in fact the distance of each region unit is the function about joint configuration space C-Space.

12) in actual motion design, range of articulation due to robot, constraint of velocity, the mechanical constraints such as rigid body length, there is an accessible maximum region in robot end, same each region is to due to these constraints, there is a minimum range that may reach, if this minimum range is greater than certain threshold values, namely this region is in fact in motion planning, and gets what value, can not bump, when carrying out collision detection in real time, in fact do not need all regions detecting like this.

Therefore, the candidate region pair that utilizes optimal method (SQP) off-line learning to produce self collision, the region that need to detect while reducing detection is in real time to quantity.In the present embodiment, following constraints is considered: 1, the extreme value of joint space 2, the region unit on same articulated chain can not produce collision.3, need concrete accounting equation is as follows:

$\mathrm{st}:\min f\left(q\right)=d^2=(\stackrel{\‾}{P_i^D}-\stackrel{\‾}{P_j^D})$

condition：

$\left\{\begin{array}{c}{q}_x^{\min }\≤q_x\≤q_x^{\max }\\ d_{\mathrm{mn}}\≥d_b\end{array}\right.$

In order to solve minimum of a value, this equation need to be tried to achieve for the partial differential equation in joint:

$\frac{\∂f}{\∂q}=\frac{\∂\left(d^2\right)}{\∂q}=2*(\stackrel{\‾}{P_i^D}-\stackrel{\‾}{P_j^D})*(J_i^D-J_j^D)*\mathrm{\Δq}$

Wherein, wherein with be the right position of nearest neighbor point about the Jacobian matrix of joint angles, the region pair that may bump that it obtains, for carrying out the candidate region pair of self-collision detection.

13), for the internal region unit in these candidate regions, adopt step 1) in declutch shaft (SAT) algorithm to candidate region to carrying out collision status detection;

2) if exist distance to be less than the situation of threshold values, namely illustrate that this region is to being about to occur collision, for fear of the generation of this collision, in task space, increase by one and avoid task for adjusting the self collision of movement locus, carry out avoiding of self collision.The task description of this task is for making become large, for the joint space of robot to task space be mapped as nonlinear equation t=h (q), this equation is carried out against motion calculation, this equation may have countless solutions, so utilization Solutions of Ordinary Differential Equations, the speed of joint space replaces joint space itself to calculate self collision task can be described as like this defining this task is wherein h (d) is command range change direction, v (d) is command range rate of change, wherein, it is along gradient method that command range change direction is similar to Artificial Potential Field Method (APF), make the fastest direction declining, if descent direction is consistent with end orbit direction, for fear of local optimum, increase a random direction vector to avoid, its equation is:

$h\left(d\right)=\left\{\begin{array}{c}\stackrel{\·}{d}\\ \stackrel{\·}{d}+r\left(\mathrm{\θ}\right)\end{array}\right.$

And for command range rate of change, when adjusting the distance when nearer in region, need a larger speed make two regions to avoiding, adopted a kind of three decline curves:

$v\left(d_k\right)=v_0*a_h=\left\{\begin{array}{cc}\frac{v_0}{{(d_m-d_k)}^3}& ,d_k<d_m\\ 0& ,\mathrm{else}\end{array}\right.$

V wherein ₀be a constant variable, d _mfor threshold values, this function can meet the characteristic of successively decreasing.

3) now, self collision avoids task as a task, to introduce task space, when this task and end orbit tracing task need to complete simultaneously, introduces task kernel N=I-J ^#* J, and complete two tasks simultaneously.

Claims (2)

1. the robot real time kinematics self collision based on task space is avoided a control method, and the method comprises follows the tracks of the end orbit of robot, specifically comprises the following steps:

1) according to the three-dimensional geometric information of robot, utilizing enclosure body to carry out modeling, is different region units by the body segmentation of robot, and the region that every two region units are formed is to carrying out self-collision detection;

2) if region detected, be there is to self collision in meeting, in task space, increase by one and avoid task for adjusting the self collision of movement locus;

3) complete self collision simultaneously avoid task and end orbit tracing task.

2. a kind of robot real time kinematics self collision based on task space according to claim 1 is avoided control method, it is characterized in that step 1) in the detailed process of self-collision detection be:

11) according to the three-dimensional geometric information of robot, utilizing enclosure body to carry out modeling, is different region units by the body segmentation of robot, and every two region units are divided into one group of region pair;

12) candidate region pair that utilizes optimal method off-line learning to produce self collision, the region that need to detect while reducing detection is in real time to quantity;

13) adopt declutch shaft method to candidate region to carrying out collision status detection.