CN103722565B - Anthropomorphic robot self collision monitoring system and method for supervising - Google Patents

Abstract

Anthropomorphic robot self collision monitoring system and method for supervising, relate to robot controlling field.The present invention is the safety of precision in order to improve robot collision detection and real-time protection anthropomorphic robot.The present invention uses ball to scan convex body and builds collision model, discrete adaptive window angular speed calculation method and GJK algorithm, develop the quick high accuracy collision detection monitoring system of machine-independent people's mission planning and control, this systemic circulation is run, using joint of robot angle as input, judging whether robot can collide, sending stop command when colliding to robot controller.When robot security runs, this system does not affect robot operation.The present invention is applicable to robot controlling.

Description

Anthropomorphic robot self collision monitoring system and method for supervising

Technical field

The present invention relates to robot controlling field.

Background technology

The self collision of imitative robot comprises the body and hands arm of robot and the collision etc. of both arms/both legs.The generation of self collision can damage robot, even hurts the staff of surrounding.Along with the increase of the robot free degree, the possibility of robot generation self collision also will increase, and it is vital for therefore addressing this problem for having multivariant anthropomorphic robot, the basis being also anthropomorphic robot security control and executing the task.

Robot collision detecting system needs the potential collision of real-time measuring robots self, provides collision information, calculates the beeline of separating member, contributes to motion planning and the security control of robot.Wherein the information of beeline can make robot and barrier maintain a certain distance, and avoids obstacle.When comprising the object that a large amount of needs detect, hierarchical wrap box technology can accelerate the test process of object.Utilize volume bigger and the geometric object of the bounding box of simple shape parcel complexity, first carry out the simple intersection test of bounding box, get rid of a large amount of disjoint geometric object rapidly, more accurate test for intersection is carried out to residue geometric object.Hierarchical wrap box technology can select different bounding boxs to carry out the computational efficiency of balanced algorithm, complexity, and the relation of the levels of precision three of parcel geometric object.Conventional bounding box comprises: axis aligned bounding box (AABB bounding box), oriented bounding box (OBB bounding box), encircle sphere, ball scans body and convex body.Different hierarchical wrap box technology has respective pluses and minuses.Such as: the method based on AABB bounding box is faster than the method based on OBB bounding box, but OBB bounding box precision is higher.These two kinds of methods are all judge whether to intersect by the test for intersection of bounding box, do not provide range information.Encircle sphere is not subject to the impact of rotation transformation, the effective collision detection of energy and the beeline calculated between many objects, but the precision of encircle sphere model is lower.It is with corresponding pel Minkowski with formed by spheroid that ball scans body.Difference according to corresponding pel can be divided into different types, such as: ball scans straight line (capsule body), ball scans rectangle (rhombogen) and ball scans convex body.The test for intersection that two balls scan body is relatively realized by their the radius sum of Distance geometry calculated between two inner pels.The test that ball scans body consumes the calculating consumption depending on respective distances completely.Ball scans the self-collision detection that straight line is applied to dobby robot widely, but ball scans straight line is only applicable to articulationes cylindroideus, cannot describe the joint of complex contour, limit the scope of application of the method.Convex body can describe the joint of complex contour, and have good space structure characteristic, can optimize the efficiency of collision detection, therefore convex body is also widely used in collision detection.Ball scans convex body and has convex body advantage, simpler than convex body data structure, means that ball scans convex body under the prerequisite ensureing collision detection model accuracy, can improve the real-time of collision detection.In order to the precision and real-time that improve robot collision detection further protect the safety of anthropomorphic robot; the collision model that the present invention uses ball to scan convex body develops the quick high accuracy collision detection monitoring system of machine-independent people's mission planning and control; this systemic circulation is run; using joint of robot angle as input, judge whether robot can collide.

Summary of the invention

The present invention is the safety of precision in order to improve robot collision detection and real-time protection anthropomorphic robot, thus provides a kind of anthropomorphic robot self collision monitoring system and method for supervising.

Anthropomorphic robot self collision monitoring system, it comprises based on the discrete collision detection module 7 of ball sweeping convex body, robot motion's state prediction module 6, joint angles speed calculation module 5 and communication module 4;

Communication module 4 for sending the joint angles information of the joint angle velocity sensor collection on anthropomorphic robot to joint angle speed calculation module 5, also for sending to robot controller 2 when collision being detected of the discrete collision detection module 7 based on ball sweeping convex body anxious is stopped control instruction;

Joint angles speed calculation module 5 calculates the angular speed in each joint of anthropomorphic robot for sampling period of the joint angles information that gathers according to robot joints position sensor and robot joints position sensor; And the angular speed in each joint of anthropomorphic robot of the joint angles of robot joints position sensor collection and acquisition is sent to robot motion's state prediction module 6;

The motion state of the latency prediction anthropomorphic robot that the joint angles that robot motion's state prediction module 6 gathers for the robot joints position sensor sent according to joint angles speed calculation module 5, the angular speed in each joint of anthropomorphic robot and self collision monitoring system are introduced; And the discrete collision detection module 7 of ball sweeping convex body is sent to by predicting the outcome;

Discrete collision detection module 7 based on ball sweeping convex body scans convex body collision detection model for what build anthropomorphic robot based on ball, and according to robot motion's state real-time update collision detection model that robot motion's state prediction module 6 is predicted, optimize collision right, and detect each collision to whether colliding, and send when colliding and anxious stop control instruction.

Anthropomorphic robot self collision method for supervising, the step that it comprises the step of the discrete collision detection based on ball sweeping convex body, the step of robot motion's status predication, joint angles speed calculate and communicating step;

Step based on the discrete collision detection of ball sweeping convex body: send the joint angles information of the joint angle velocity sensor collection on anthropomorphic robot to joint angle speed calculation module 5, and the discrete collision detection module 7 based on ball sweeping convex body sent anxious step of stopping control instruction when collision being detected to robot controller 2;

Joint angles speed calculation step: the angular speed in the sampling period calculating each joint of anthropomorphic robot of the joint angles information gathered according to robot joints position sensor and robot joints position sensor; And the angular speed in each joint of anthropomorphic robot of the joint angles of robot joints position sensor collection and acquisition is sent to the step of robot motion's state prediction module 6;

Robot motion's status predication step: according to the motion state of joint angles, the angular speed in each joint of anthropomorphic robot and the latency prediction anthropomorphic robot of self collision monitoring system introducing that the robot joints position sensor of joint angles speed calculation module 5 transmission gathers; And the step of the discrete collision detection module 7 of ball sweeping convex body is sent to by predicting the outcome;

Discrete collision detection step based on ball sweeping convex body: what build anthropomorphic robot scans convex body collision detection model based on ball, and according to robot motion's state real-time update collision detection model that robot motion's state prediction module 6 is predicted, optimize collision right, and detect each collision to whether colliding, and send anxious step of stopping control instruction when colliding.

In joint angles speed calculation step, the method that the angular speed that the joint angles information gathered according to robot joints position sensor and the sampling period of robot joints position sensor calculate each joint of anthropomorphic robot adopts discrete adaptive window speed to calculate realizes.

In joint angles speed calculation step, the method that the angular speed that the joint angles information gathered according to robot joints position sensor and the sampling period of robot joints position sensor calculate each joint of anthropomorphic robot adopts discrete adaptive window speed to calculate realizes, and is specially:

Selection window size, described window size n=max{1,2,3...}, namely meet the maximum of variable i in following formula;

$|{\mathrm{\α}}_i(t-i)-{\widehat{\mathrm{\α}}}_i(t-i)|\≤{\mathrm{\ϵ}}_i,\∀i\∈\{\mathrm{1,2},...,n\}$

In formula: wherein α _i(t-i) be the joint angles that t-i moment joint sensors obtains, the angle in the t-i moment joint obtained by self-adapting window speed calculation method, ε _iworst error allowed between the two, ε _isize depend on the maximum angular acceleration in each joint of anthropomorphic robot;

α _i(t-i) by following formula:

${\widehat{\mathrm{\α}}}_i(t-i)={\mathrm{\α}}_i\left(t\right)-\frac{i({\mathrm{\α}}_i\left(t\right)-{\mathrm{\α}}_i(t-n))}{n}$

Obtain;

Wherein: α (t) and α (t-n) is respectively the joint angles of t and the acquisition of t-n moment sensor;

Formula is passed through after window size is determined:

${\stackrel{.}{\mathrm{\α}}}_i\left(t\right)=\frac{{\mathrm{\α}}_i\left(t\right)-{\mathrm{\α}}_i(t-n)}{\mathrm{n\Δt}}$

Calculate the angular speed in each joint of anthropomorphic robot, wherein: △ t is the sampling period of robot joints position sensor.

In discrete collision detection based on ball sweeping convex body, build scanning convex body collision detection model based on ball and being specially of anthropomorphic robot:

Use ball to scan convex body data structure to build robot collision detection model, the data structure definition that described ball scans convex body is:

V(r;P)=convP+{b∈R ³||b|≥r}

Wherein: convP is point set the convex body formed, ball scan convex body be by the Minkowski of radius r to be spheroid with corresponding point set P formed convex body with.

In discrete collision detection based on ball sweeping convex body, detecting each collision to the method whether collided is:

Use Gilbert-Johnson-Keerthi algorithm to calculate beeline between each collision pair and closest approach, when the distance that two balls scan convex body is less than or equal to zero, then two articles collides.

Concrete grammar based on the discrete collision detection of ball sweeping convex body is:

Step one, the robot motion's state input after prediction is scanned the discrete collision detection module of convex body based on ball, first this module will carry out pretreatment to the robot model of monitoring, be that ball scans convex body model by the pre model conversation of each for robot component, for non-convex body component, be broken down into the combination of multiple convex body, thus construct static robot collision model;

Step 2, use robot positive kinematics to calculate transformation matrix by the robot joint angles of prediction, thus calculate robot component at the stylish coordinate position of motion, the collision model of robot component is moved thereupon;

Step 3, according to each component range of movement, determine each sense cycle of anthropomorphic robot need detect collision right;

Step 4, use GJK algorithm calculate each and collide right beeline and closest approach position, and Three-dimensional Display robot collision model and the possible point of impingement; When collision being detected, other are stopped to collide right detection immediately; Communication module is used to send stop command to robot controller.

The present invention ensure that the precision of anthropomorphic robot collision detection, and can the safety of real-time protection anthropomorphic robot, makes the average calculation times of collision detection be 0.7ms.

Accompanying drawing explanation

Fig. 1 is the structural representation of present system; Mark 1 is robot task planning end, and mark 2 is robot controllers, and mark 3 is anthropomorphic robots.

Fig. 2 is the workflow schematic diagram of the discrete collision detection module of scanning convex body based on ball;

Detailed description of the invention

Detailed description of the invention one, composition graphs 1 illustrate this detailed description of the invention, anthropomorphic robot self collision monitoring system, it comprises based on the discrete collision detection module 7 of ball sweeping convex body, robot motion's state prediction module 6, joint angles speed calculation module 5 and communication module 4;

Communication module 4 for sending the joint angles information of the joint angle velocity sensor collection on anthropomorphic robot to joint angle speed calculation module 5, also for sending to robot controller 2 when collision being detected of the discrete collision detection module 7 based on ball sweeping convex body anxious is stopped control instruction;

Joint angles speed calculation module 5 calculates the angular speed in each joint of anthropomorphic robot for sampling period of the joint angles information that gathers according to robot joints position sensor and robot joints position sensor; And the angular speed in each joint of anthropomorphic robot of the joint angles of robot joints position sensor collection and acquisition is sent to robot motion's state prediction module 6;

The motion state of the latency prediction anthropomorphic robot that the joint angles that robot motion's state prediction module 6 gathers for the robot joints position sensor sent according to joint angles speed calculation module 5, the angular speed in each joint of anthropomorphic robot and self collision monitoring system are introduced; And the discrete collision detection module 7 of ball sweeping convex body is sent to by predicting the outcome;

Discrete collision detection module 7 based on ball sweeping convex body scans convex body collision detection model for what build anthropomorphic robot based on ball, and according to robot motion's state real-time update collision detection model that robot motion's state prediction module 6 is predicted, optimize collision right, and detect each collision to whether colliding, and send when colliding and anxious stop control instruction.

Detailed description of the invention two, composition graphs 2 illustrate this detailed description of the invention, anthropomorphic robot self collision method for supervising, the step that it comprises the step of the discrete collision detection based on ball sweeping convex body, the step of robot motion's status predication, joint angles speed calculate and communicating step;

Communicating step: send the joint angles information of the joint angle velocity sensor collection on anthropomorphic robot to joint angle speed calculation module 5, and the discrete collision detection module 7 based on ball sweeping convex body sent anxious step of stopping control instruction when collision being detected to robot controller 2;

Joint angles speed calculation step: the angular speed in the sampling period calculating each joint of anthropomorphic robot of the joint angles information gathered according to robot joints position sensor and robot joints position sensor; And the angular speed in each joint of anthropomorphic robot of the joint angles of robot joints position sensor collection and acquisition is sent to the step of robot motion's state prediction module 6;

Robot motion's status predication step: according to the motion state of joint angles, the angular speed in each joint of anthropomorphic robot and the latency prediction anthropomorphic robot of self collision monitoring system introducing that the robot joints position sensor of joint angles speed calculation module 5 transmission gathers; And the step of the discrete collision detection module 7 of ball sweeping convex body is sent to by predicting the outcome;

Discrete collision detection step based on ball sweeping convex body: what build anthropomorphic robot scans convex body collision detection model based on ball, and according to robot motion's state real-time update collision detection model that robot motion's state prediction module 6 is predicted, optimize collision right, and detect each collision to whether colliding, and send anxious step of stopping control instruction when colliding.

Principle: the object of the invention is for the deficiencies in the prior art, ball is used to scan convex body member impacts model, discrete adaptive window angular speed calculation method and GJK algorithm, develop the quick high accuracy collision detection monitoring system of machine-independent people's mission planning and control, this systemic circulation is run, using joint of robot angle as input, judging whether robot can collide, sending stop command when colliding to robot controller.

When robot security runs, this system does not affect robot operation, improves the precision of robot collision detection and the safety of real-time protection anthropomorphic robot further.

Anthropomorphic robot self collision monitoring system comprises communication module, joint angle speed calculation module, robot motion's prediction module and scan the discrete collision detection module of convex body based on ball.

The joint angles information of each joint position sensor of communication module periodic samples robot and send stop command to robot controller when self collision monitoring system detects collision.

Joint angle speed calculation module is according to the angular speed in each joint of sampling period calculating robot of the joint angles of robot joint position sensor and communication module.Although the joint angles of the double sampling only poor joint angle speed divided by sampling period acquisition can be used, but in actual applications due to meeting measurement by magnification of little sampling period error, therefore joint angle speed measurement module employs discrete adaptive window speed calculation method.The method can according to the size of speed automatic selection window size to ensure the accurate of result of calculation.When speed is fast, window size will diminish, and when speed is slow, the size of window will become large on the contrary.

Window size n=max{1,2,3...}, namely meet the maximum of equation (1) variable i.

$|{\mathrm{\α}}_i(t-i)-{\widehat{\mathrm{\α}}}_i(t-i)|\≤{\mathrm{\ϵ}}_i,\∀i\∈\{\mathrm{1,2},...,n\}$

Wherein α _i(t-i) be the joint angles that t-i moment sensor obtains, the angle in the t-i moment joint obtained by self-adapting window speed calculation method, ε _iworst error allowed between the two, ε _isize depend on the maximum angular acceleration in each joint of robot. obtained by following formula:

${\widehat{\mathrm{\α}}}_i(t-i)={\mathrm{\α}}_i\left(t\right)-\frac{i({\mathrm{\α}}_i\left(t\right)-{\mathrm{\α}}_i(t-n))}{n}$

Wherein α (t) and α (t-n) is respectively the joint angles of t and the acquisition of t-n moment sensor.

Calculate the angular speed in corresponding joint by following formula after window size is determined.

${\stackrel{.}{\mathrm{\α}}}_i\left(t\right)=\frac{{\mathrm{\α}}_i\left(t\right)-{\mathrm{\α}}_i(t-n)}{\mathrm{n\Δt}}$

Wherein △ t is the sampling period that communication module obtains robot joint position sensor.

The information of direct use joint position sensor is carried out self-collision detection and is existed certain delayed, the safety of robot can not be ensured, therefore robot motion's prediction module hypothesis can be ignored in the velocity variations in the very short each joint of sense cycle inner machine people, according to joint angles, the motion state of time delay to current robot that the joint angle speed calculated and self-collision detection system are introduced is predicted, the time delay that self collision monitoring system is introduced comprises communication cycle, the time lag that self-collision detection cycle and robot controlling cycle may cause.

The discrete collision detection module of scanning convex body based on ball carries out collision detection to robot motion's state that module 3 is predicted.Because self-collision detection module needs to calculate in each sense cycle the coordinate that in collision model, each point is new.Therefore use ball to scan convex body and describe each component of robot, reduce the data volume that collision model needs, increase the real-time of collision detection.

Ball scans convex body and is defined as:

V(r;P)=convP+{b∈R ³||b|≥r} (1)

Wherein, convP is point set the convex body formed, ball scan convex body be by the Minkowski of radius r to be spheroid with corresponding point set P formed convex body with.

Each sense cycle calculates the transformation matrix of each component according to joint angles and robot positive kinematics for last sense cycle component i coordinate system is relative to the transformation matrix of world coordinates, for current detection cycle component i coordinate system is relative to the transformation matrix of world coordinates.And the coordinate that in collision model, each point is new is ^wp ' _i=T ^wp _i, ^wp _ifor the coordinate of the relative world coordinate system of point of i component convex body point set in previous sense cycle, ^wp ' _ifor the point of current detection cycle i component convex body point set is relative to the new coordinate of world coordinate system.

After having upgraded collision model, collision detection pair is set, it is right that anthropomorphic robot self collision monitoring system needs in each sense cycle, detect a large amount of collisions, therefore real-time and efficiency that the right logarithm of collision can improve robot astronaut self-collision detection is optimized, by considering that each component range of movement reduces collision logarithm further.

The discrete collision detection module of scanning convex body based on ball uses GJK iterative algorithm to the collision set to judging.GJK algorithm is a kind of descent algorithm based on monomorphous, and the distance between two convex bodys calculates and is converted into the distance solved in single convex body collection by it, will search for Minkowski difference object, and obtain a monomorphous subspace body in each loop iteration.After successive ignition, algorithmic statement is in a certain distance values, i.e. the beeline of two convex bodys, calculates the closest approach of two convex bodys.The vertex set of any two convex bodys of this algorithm input, exports the Euclidean distance between convex body and nearest point.The element figure scanning convex body due to ball is convex body, possesses the characteristic of convex body, is therefore easy to be applied to ball for the GJK algorithm of convex body and scans on convex body.The distance that two balls scan convex body be corresponding element figure two convex body beeline deduct the radius that two scan ball.When the distance that two balls scan convex body is less than or equal to zero, think that two articles collides.Three-dimensional Display robot collision detection model and the possible point of impingement, give a warning, send stop command by communication module to robot controller.

Below in conjunction with accompanying drawing and specific embodiment, the present invention is described in further detail.：

Use C Plus Plus programming enforcement anthropomorphic robot self collision monitoring system of the present invention to have the anthropomorphic robot of 19 components for one, its concrete steps are as follows:

(1) use the Socket programming based on UDP to build communication module, periodically accepted the joint angles information of each joint position sensor of robot that robot controller sends by LAN.

(2) joint angle speed calculation module accepts the joint angles of robot joint position sensor and the sampling period of communication module, uses discrete adaptive window speed calculation method, calculates window size, thus calculate the angular speed in each joint.

(3) robot motion's prediction module hypothesis can be ignored in the velocity variations in the very short each joint of sense cycle inner machine people, according to the joint angles of position sensor, the time delay that the joint angle speed calculated and self-collision detection system are introduced, predict the motion state of current robot, the motion state of current machine person joint is the angle that the joint angles of sensor adds upper joint and keeps present speed time delay to turn over.

(4) the robot motion's state input after prediction is scanned the discrete collision detection module of convex body based on ball.First this module will carry out pretreatment to the robot model of monitoring, is that ball scans convex body model, for non-convex body component, is broken down into the combination of multiple convex body by the pre model conversation of each for robot component.Thus construct static robot collision model.

(5) use robot positive kinematics to calculate transformation matrix by the robot joint angles of prediction, thus calculate robot component at the stylish coordinate position that moves, the collision model of robot component also will be moved thereupon.

(6) by considering each component range of movement, the component as anthropomorphic robot adjacent segment does not collide; Homonymy arm/leg does not collide; Head can not to collide etc. with pin and reduce collision logarithm.In this example, an each sense cycle of anthropomorphic robot having 19 components needs detection 101 right to collision.

(7) use GJK algorithm to calculate each and collide right beeline and closest approach position, and Three-dimensional Display robot collision model and the possible point of impingement.When collision being detected, system stops other colliding right detection immediately.Communication module is used to send stop command to robot controller.When collisionless occurs, this system can not interfere the normal operation of robot.In this example, communication cycle is 50ms, and the sense cycle of scanning the discrete collision detection module of convex body based on ball is 50ms, judge 101 to collision right average calculation times be 0.7ms.

Claims (5)

1. anthropomorphic robot self collision method for supervising, the step that it comprises the step of the discrete collision detection based on ball sweeping convex body, the step of robot motion's status predication, joint angles speed calculate and communicating step;

Communicating step: send the joint angles information of the joint angle velocity sensor collection on anthropomorphic robot to joint angle speed calculation module (5), and by the step of suddenly stopping control instruction when collision being detected to robot controller (2) transmission of the discrete collision detection module (7) based on ball sweeping convex body;

Joint angles speed calculation step: the angular speed in the sampling period calculating each joint of anthropomorphic robot of the joint angles information gathered according to robot joints position sensor and robot joints position sensor; And the angular speed in each joint of anthropomorphic robot of the joint angles of robot joints position sensor collection and acquisition is sent to the step of robot motion's state prediction module (6);

Robot motion's status predication step: the motion state of the latency prediction anthropomorphic robot that the joint angles that the robot joints position sensor sent according to joint angles speed calculation module (5) gathers, the angular speed in each joint of anthropomorphic robot and self collision monitoring system are introduced; And the step of the discrete collision detection module (7) of ball sweeping convex body is sent to by predicting the outcome;

Discrete collision detection step based on ball sweeping convex body: what build anthropomorphic robot scans convex body collision detection model based on ball, and according to robot motion's state real-time update collision detection model that robot motion's state prediction module (6) is predicted, optimize collision right, and detect each collision to whether colliding, and send anxious step of stopping control instruction when colliding;

It is characterized in that: in joint angles speed calculation step, the method that the angular speed that the joint angles information gathered according to robot joints position sensor and the sampling period of robot joints position sensor calculate each joint of anthropomorphic robot adopts discrete adaptive window speed to calculate realizes, and is specially:

Selection window size, described window size n=max{1,2,3...}, namely meet the maximum of variable i in following formula;

$|{\mathrm{\α}}_i(t-i)-{\widehat{\mathrm{\α}}}_i(t-i)|\≤{\mathrm{\ϵ}}_i,\∀i\∈\{\mathrm{1,2},...,n\}$

In formula: wherein α _i(t-i) be the joint angles that t-i moment joint sensors obtains, the angle in the t-i moment joint obtained by self-adapting window speed calculation method, ε _iworst error allowed between the two, ε _isize depend on the maximum angular acceleration in each joint of anthropomorphic robot;

α _i(t-i) by following formula:

${\widehat{\mathrm{\α}}}_i=(t-i)={\mathrm{\α}}_i\left(t\right)-\frac{i({\mathrm{\α}}_i\left(t\right)-{\mathrm{\α}}_i(t-n))}{n}$

Obtain;

Wherein: α (t) and α (t-n) is respectively the joint angles of t and the acquisition of t-n moment sensor;

Formula is passed through after window size is determined:

${\stackrel{.}{\mathrm{\α}}}_i\left(t\right)=\frac{{\mathrm{\α}}_i\left(t\right)-{\mathrm{\α}}_i(t-n)}{\mathrm{n\Δt}}$

Calculate the angular speed in each joint of anthropomorphic robot, wherein: △ t is the sampling period of robot joints position sensor.

2. anthropomorphic robot self collision method for supervising according to claim 1, it is characterized in that in joint angles speed calculation step, the method that the angular speed that the joint angles information gathered according to robot joints position sensor and the sampling period of robot joints position sensor calculate each joint of anthropomorphic robot adopts discrete adaptive window speed to calculate realizes.

3. anthropomorphic robot self collision method for supervising according to claim 1, is characterized in that in the discrete collision detection step based on ball sweeping convex body, builds scanning convex body collision detection model based on ball and being specially of anthropomorphic robot:

Use ball to scan convex body data structure to build robot collision detection model, the data structure definition that described ball scans convex body is:

V(r；P)＝convP+{b∈R ³||b|≥r}

Wherein: convP is point set the convex body formed, ball scan convex body be by the Minkowski of radius r to be spheroid with corresponding point set P formed convex body with.

4. anthropomorphic robot self collision method for supervising according to claim 1, is characterized in that in the discrete collision detection step based on ball sweeping convex body, detects each collision to the method whether collided to be:

Use Gilbert-Johnson-Keerthi algorithm to calculate beeline between each collision pair and closest approach, when the distance that two balls scan convex body is less than or equal to zero, then two articles collides.

5. anthropomorphic robot self collision method for supervising according to claim 1, is characterized in that the concrete grammar of the discrete collision detection based on ball sweeping convex body is:

Step one, the robot motion's state input after prediction is scanned the discrete collision detection module of convex body based on ball, first this module will carry out pretreatment to the robot model of monitoring, be that ball scans convex body model by the pre model conversation of each for robot component, for non-convex body component, be broken down into the combination of multiple convex body, thus construct static robot collision model;

Step 2, use robot positive kinematics to calculate transformation matrix by the robot joint angles of prediction, thus calculate robot component at the stylish coordinate position of motion, the collision model of robot component is moved thereupon;

Step 3, according to each component range of movement, determine each sense cycle of anthropomorphic robot need detect collision right;

Step 4, use GJK algorithm calculate each and collide right beeline and closest approach position, and Three-dimensional Display robot collision model and the possible point of impingement; When collision being detected, other are stopped to collide right detection immediately; Communication module is used to send stop command to robot controller.