CN104097205B - The robot real time kinematics self collision in task based access control space avoids control method - Google Patents

Abstract

The robot real time kinematics self collision that the present invention relates to a kind of task based access control space avoids control method, the method comprises follows the tracks of the end orbit of robot, specifically comprise the following steps: 1) utilize enclosure body to carry out modeling according to the three-dimensional geometric information of robot, by the region unit that the body segmentation of robot is different, the region formed every two region units is to carrying out self-collision detection; 2) to can there is self collision if detect in region, then in task space, increase one and avoid task for the self collision adjusting movement locus; 3) complete self collision simultaneously and avoid task and end orbit tracing task.Compared with prior art, the present invention in the process of robot real-time motion planning, can avoid the generation of self collision, facilitates the scene application of robot.

Description

The robot real time kinematics self collision in task based access control space avoids control method

Technical field

The present invention relates to a kind of robot control method, the robot real time kinematics self collision especially relating to a kind of task based access control space avoids control method.

Background technology

On-the-spot and the service field such as family, hospital, the Aged Care center, military affairs, tourism, transport, exploration, rescue and relief work is extended to along with the development of Robotics, robot application field also start to manufacture field from traditional factory.In the evolution that human society is very long, the environment of mankind's daily life is gradually by the applicable human sensory that transforms and behavioral trait, and this just requires to coexist with the mankind and the intelligent robot of serving the mankind can imitate outward appearance and the behavior processing transactions of 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 attempt producing similar bipod walking robot of more walking with the mankind constantly 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 the oneself of robot may be caused to collide, namely two positions of health collide.Such as, in the process of walking, the arm of swing may collide with leg, and these collisions may cause the object of planning not complete, and even make robot disequilibrium or control, produces damage robot and working environment.In the last few years, a lot of scholar 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 expanded random tree (RRTs), probability map (PRMs) etc. fast, achieve the method avoided by the self collision of off-line learning, training.These class methods can learn the target trajectory of planning, make robot while carrying out trajectory planning, avoid self collision.

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

Summary of the invention

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

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

The robot real time kinematics self collision in task based access control space avoids a control method, and the method comprises follows the tracks of the end orbit of robot, specifically comprises the following steps:

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

2) to can there is self collision if detect in region, then in task space, increase one and avoid task for the self collision adjusting movement locus;

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

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

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

12) utilize optimal method off-line learning to produce the candidate region pair of self collision, reduce and when detecting in real time, need the region detected to quantity,

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

Compared with prior art, the present invention represents by setting up three-dimensional space model, the region unit that self collision may occur is carried out detection and estimated, introduce collision candidate region pair by off-line learning simultaneously, and by set up dynamic task space distribute realize simultaneously end orbit follow the tracks of and self collision avoid, make robot in the process of planning in real time, avoid the generation of self collision, facilitate the scene application of robot.

Accompanying drawing explanation

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

The set figure of region unit of Fig. 2 for setting up according to robot three-dimensional geological information;

Fig. 3 is that region is internally apart from the schematic diagram calculated.

Detailed description of the invention

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

Embodiment

The robot real time kinematics self collision in task based access control space avoids a control method, the method containing following the tracks of the end orbit of robot, concrete step as shown in Figure 1:

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

Wherein, self-collision detection includes following step:

11) utilizing enclosure body to carry out modeling according to the three-dimensional geometric information of robot, is N number of region unit by the body segmentation of robot as shown in Figure 2.Then from N number of region unit, two region units will be got arbitrarily be defined as region pair such measuring robots whether there occurs self collision can be defined as detect each region between distance whether be less than threshold values, to each need detect region pair utilization declutch shaft (SAT) algorithm can 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 vector in a coordinate system, solving of it can be by try to achieve, can find out that the distance of in fact each region unit is the function about joint configuration space C-Space.

12) in actual motion design, due to the range of articulation of robot, constraint of velocity, the mechanical constraints such as rigid body length, there is an accessible maximum region in robot end, same each region retrains due to these, 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, all can not collide, like this when carrying out collision detection in real time, in fact do not need all regions detecting.

Therefore, utilize optimal method (SQP) off-line learning to produce the candidate region pair of self collision, reduce and when detecting in real time, need the region detected 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 needs to try 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 Jacobian matrix of the right position of nearest neighbor point about joint angles, its region pair that may collide obtained, then for needing the candidate region pair of carrying out self-collision detection.

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

2) if there is the situation that distance is less than threshold values, namely illustrate that this region there occurs collision to being about to, in order to avoid the generation of this collision, then in task space, increase one and avoid task for the self collision adjusting movement locus, carry out avoiding of self collision.The task description of this task is for making become large, to task space, nonlinear equation t=h (q) is mapped as the joint space of robot, carries out against motion calculation to this equation, this equation may have countless solution, so utilization Solutions of Ordinary Differential Equations, namely the speed of joint space replaces joint space itself to calculate such self collision task can be described as 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 declined, if descent direction is consistent with end orbit direction, in order to avoid 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 region is adjusted the distance nearer, need a larger speed to make two regions to avoiding, have employed 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.$

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

3) now, self collision avoids task to introduce task space as a task, 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 (1)

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

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

2) to can there is self collision if detect in region, then in task space, increase one and avoid task for the self collision adjusting movement locus;

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

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

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

12) utilize optimal method off-line learning to produce the candidate region pair of self collision, reduce and when detecting in real time, need the region detected to quantity;

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