CN105425828A - Robot anti-impact double-arm coordination control system based on sensor fusion technology - Google Patents

Abstract

The present invention discloses a robot anti-impact double-arm coordination control system based on sensor fusion technology, belonging to the field of robot control. The system comprises a binocular camera, an ultrasonic sensor, a sensor information fusion module, a path planning module, a motion controller, and first and second six-degree-of-freedom manipulators. The binocular camera and the ultrasonic sensor are used for detecting the obstacle information in an operation environment, after the obtained environment is subjected to fusion processing by the sensor information fusion module, accurate obstacle position information is obtained and is transmitted to the path planning module to carry out anti-impact path planning, then the path planning module sends planned path information to the motion controller, and the motion controls the motion of the first and second six-degree-of-freedom manipulators. According to the system, the information from different sensors can be fused effectively, a reasonable path is planned through the path planning module, and thus the control of a robot is more precise.

Description

Robot anticollision based on sensor fusion techniques impacts Dual-Arm Coordination control system

Technical field

The present invention relates to robot controlling field, refer to that a kind of robot anticollision based on sensor fusion techniques impacts Dual-Arm Coordination control system especially.

Background technology

Along with improving constantly of industrial automation level, the darker wider future development of application forward of robot, the intellectuality of robot seems particularly important in the industrial production.In commercial production, tow-armed robot is compared one armed robot and can be completed more complicated operation task, but robot wants to understand environment, to conform and barrier collisionless in working environment is impacted carries out the co-ordination that operation then needs between high-performance sensors and multiple sensor.

Increasing and improve sensor performance in intelligent system is the important means strengthening system intelligence, obvious defect is there is in traditional single-sensor in the anticollision impact system of mechanical arm, be difficult to obtain accurate environmental information, this is because each sensor all exists certain defect, only single-sensor is relied on to be difficult to meet control overflow.Therefore, how fusion treatment is carried out to the information from different sensors, thus more effective being applied in a flexible way makes the control of robot more accurate, is an emphasis of research at present.

Summary of the invention

The technical problem to be solved in the present invention is to provide a kind of robot anticollision based on sensor fusion techniques and impacts Dual-Arm Coordination control system, and it can carry out effective integration to the information from different sensors, to make the control of robot more accurate.

For solving the problems of the technologies described above, the invention provides technical scheme as follows:

A kind of robot anticollision based on sensor fusion techniques impacts Dual-Arm Coordination control system, comprise binocular camera, ultrasonic sensor, sensor data fusion module, path planning module, motion controller, the first and second sixdegree-of-freedom simulation, wherein:

Described binocular camera is two and is installed on the end of described first and second sixdegree-of-freedom simulation respectively, described ultrasonic sensor is also two and is installed on the end of described first and second sixdegree-of-freedom simulation respectively, and described binocular camera is connected the input end of described sensor data fusion module with the output terminal of ultrasonic sensor;

The output terminal of described sensor data fusion module connects the input end of described path planning module;

The output terminal of described path planning module connects the input end of described motion controller;

The output terminal of described motion controller connects described first and second sixdegree-of-freedom simulation;

Described binocular camera and ultrasonic sensor are for detecting the obstacle information in operating environment, by the obstacle information of acquisition after described sensor data fusion module fusion treatment, the comparatively accurate Obstacle Position information obtained also passes to the path planning that described path planning module carries out anticollision impact, the routing information of planning is issued described motion controller by described path planning module afterwards, is controlled the motion of described first and second sixdegree-of-freedom simulation by described motion controller.

Further, described binocular camera is further used for demarcating barrier, and gathers two dimensional image in different angles to the unique point on barrier;

Described ultrasonic sensor is further used for real-time detection surrounding environment, and after constantly detecting ultrasound wave and launching, run into the echo that barrier reflects, measure from being transmitted into the time T receiving echo, calculate relative distance S=CT/2, in formula, C is ultrasonic velocity.

Further, the unique point that described sensor data fusion module is further used for binocular camera is caught carries out fusion calculation, obtain the track of unique point and the spatial attitude parameter of unique point place plane, after corresponding algorithm process, obtain the coordinate P (X of unique point in world coordinate system _w, Y _w, Z _w), then by the three-dimensional coordinate P (X of neural network algorithm fusion feature point _w, Y _w, Z _w) and the range information R that catches of ultrasonic sensor, obtain the three-dimensional coordinate P (X, Y, Z) that barrier is more accurate.

Further, described path planning module is further used for the Obstacle Position information obtained according to described sensor data fusion module, the inverse of inspection machine mechanical arm is separated, for Robot Design one is without the movement locus of barrier, original path can be got back to simultaneously after requiring avoiding obstacles and continued corresponding operation.

Further, described neural network algorithm adopts three layers of BP neural network, and input layer is P _in=[X _w, Y _w, Z _w, R], be respectively the object dimensional volume coordinate (X that stereoscopic vision merges rear gained _w, Y _w, Z _w) and the range information R that obtains of ultrasonic sensor, hidden layer has 9 nodes, output layer T=[X, Y, Z] three dimensional space coordinate P (X, the Y of object for obtaining after fusion treatment, Z), in learning process, adopt the vibration in Error function and self-adaptative adjustment learning rate algorithm suppression learning process, improve speed of convergence, improve the learning efficiency of neural network.

Further, the formula of described Error function and self-adaptative adjustment learning rate algorithm is as follows:

ω _ij(k+1)＝ω _ij(k)+Δω _ij+α(ω _ij(k)-ω _ij(k-1))

ω _j2(k+1)＝ω _j2(k)+Δω _j2+α(ω _j2(k)-ω _j2(k-1))(1)

$\mathrm{\&eta;}(k+1)=\left\{\begin{array}{cc}0.7\mathrm{\&eta;}\left(k\right)& E(k+1)>E\left(k\right)\\ 1.05\mathrm{\&eta;}\left(k\right)& E(k+1)<E\left(k\right)\end{array}\right.---\left(2\right)$

Formula (1) is Error function, wherein ω _ij, ω _j2for link weight coefficients, α is factor of momentum α ∈ (0,1);

Formula (2) is self-adaptative adjustment learning rate algorithm, and wherein η is self study speed, and E (k) is error sum of squares.

The present invention has following beneficial effect:

Information from different sensors (i.e. binocular camera and ultrasonic sensor) can be carried out effective integration by the present invention, and cooks up rational path accordingly by path planning module, thus makes the control of robot more accurate.

Accompanying drawing explanation

Fig. 1 is the structural representation that the robot anticollision based on sensor fusion techniques of the present invention impacts Dual-Arm Coordination control system;

Fig. 2 is barrier unique point image procossing coordinate conversion schematic diagram of the present invention;

Fig. 3 is control principle schematic diagram of the present invention.

Embodiment

For making the technical problem to be solved in the present invention, technical scheme and advantage clearly, be described in detail below in conjunction with the accompanying drawings and the specific embodiments.

The invention provides a kind of robot anticollision based on sensor fusion techniques and impact Dual-Arm Coordination control system, as shown in Figure 1, comprise binocular camera 1, ultrasonic sensor 2, sensor data fusion module 3, path planning module 4, motion controller 5, first sixdegree-of-freedom simulation 6 and the second sixdegree-of-freedom simulation 7, wherein:

Binocular camera 1 is two and is installed on the end of the first sixdegree-of-freedom simulation 6 and the second sixdegree-of-freedom simulation 7 respectively, ultrasonic sensor 2 is also two and is installed on the end of the first sixdegree-of-freedom simulation 6 and the second sixdegree-of-freedom simulation 7 respectively, the input end of the output terminal connecting sensor information fusion module 3 of binocular camera 1 and ultrasonic sensor 2;

The input end of the output terminal access path planning module 4 of sensor data fusion module 3;

The output terminal of path planning module 4 connects the input end of motion controller 5;

The output terminal of motion controller 5 connects the first sixdegree-of-freedom simulation 6 and the second sixdegree-of-freedom simulation 7;

Binocular camera 1 and ultrasonic sensor 2 are for detecting the obstacle information in operating environment, by the obstacle information of acquisition after sensor data fusion module 3 fusion treatment, the comparatively accurate Obstacle Position information obtained also passes to the path planning that path planning module 4 carries out anticollision impact, the routing information of planning is issued motion controller 5 by path planning module 4 afterwards, is controlled the motion of the first sixdegree-of-freedom simulation 6 and the second sixdegree-of-freedom simulation 7 by motion controller 5.

Information from different sensors (binocular camera and ultrasonic sensor) can be carried out effective integration by the present invention, and cooks up rational path accordingly by path planning module, thus makes the control of robot more accurate.

As to a modification of the present invention, binocular camera 1 is further used for demarcating barrier, and gathers two dimensional image in different angles to the unique point on barrier; Concrete, establish world coordinate system, camera coordinate system and imaging plane coordinate system as shown in Figure 2 when asking for barrier characteristic point position information by the two dimensional image gathered, the step being asked for barrier characteristic point position coordinate information by the two dimensional image gathered is as follows:

(1) binocular camera gathers two dimensional image in different angles to the unique point on barrier, obtains the coordinate (x, y) of unique point in imaging plane coordinate system;

(2) coordinate (X of unique point in imaging plane coordinate system can be tried to achieve by (x, y) by following formula again _c, Y _c, Z _c);

$\begin{array}{c}(x-u_0)(1+k_1{r}^2+k_2{r}^4)=-f\frac{X_c}{Z_c}\\ (y-v_0)(1+k_1{r}^2+k_2{r}^2)=-f\frac{Y_c}{Z_c}\end{array}---\left(1\right)$

In formula: (x, y) is the coordinate of unique point in imaging plane;

K ₁, k ₂for binocular camera camera lens radial distortion penalty coefficient;

(u ₀, v ₀) be the intersection point of camera optics main shaft and imaging plane;

r ²＝(x-u ₀) ²+(y-v ₀) ²。

Ask for (X _c, Y _c, Z _c) time application epipolar line restriction and algorithm for stereo matching can find out two imaging point (x about binocular camera ₁, y ₁) and (x ₂, y ₂), these two imaging points substitution formula (1) can be obtained four equatioies and solve remaining three variable (X _c, Y _c, Z _c), thus try to achieve the coordinate (X of unique point in imaging plane coordinate system _c, Y _c, Z _c).

(3) more just can according to (X by following formula _c, Y _c, Z _c) try to achieve the coordinate P (X of barrier unique point in world coordinate system _w, Y _w, Z _w).

[X _W,Y _W,Z _W]＝([X _C,Y _C,Z _C]+t)·R ^-1(2)

In formula: $t=\left[\begin{array}{c}{X}_0\\ Y_0\\ Z_0\end{array}\right];$

$R=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\&alpha;}& -sin\mathrm{\&alpha;}\\ 0& sin\mathrm{\&alpha;}& \cos \mathrm{\&alpha;}\end{array}\right]\left[\begin{array}{ccc}cos\mathrm{\&beta;}& 0& sin\mathrm{\&beta;}\\ 0& 1& 0\\ -sin\mathrm{\&beta;}& 0& cos\mathrm{\&beta;}\end{array}\right]\left[\begin{array}{ccc}cos\mathrm{\&gamma;}& -sin\mathrm{\&gamma;}& 0\\ sin\mathrm{\&gamma;}& cos\mathrm{\&gamma;}& 0\\ 0& 0& 1\end{array}\right];---\left(3\right)$

Wherein (X ₀, Y ₀, Z ₀) be the coordinate of unique point in camera coordinate system;

α, β, λ are the anglec of rotation of camera coordinate system relative to world coordinate system.

Ultrasonic sensor 2 is further used for real-time detection surrounding environment, and after constantly detecting ultrasound wave and launching, run into the echo that barrier reflects, measure from being transmitted into the time T receiving echo, calculate relative distance S=CT/2, in formula, C is ultrasonic velocity.Like this, by the acting in conjunction of two kinds of sensors (binocular camera and ultrasonic sensor), more accurate environmental information can be obtained.

Improve as to another kind of the present invention, path planning module 4 is further used for the Obstacle Position information obtained according to sensor data fusion module 3, the inverse of inspection machine mechanical arm is separated, for Robot Design one is without the movement locus of barrier, original path can be got back to simultaneously after requiring avoiding obstacles and continued corresponding operation.

As to further improvement of the present invention, the unique point that sensor data fusion module 3 is further used for binocular camera 1 is caught carries out fusion calculation, obtain the track of unique point and the spatial attitude parameter of unique point place plane, after corresponding algorithm process, obtain the coordinate P (X of unique point in world coordinate system _w, Y _w, Z _w), then by the three-dimensional coordinate P (X of neural network algorithm fusion feature point _w, Y _w, Z _w) and the range information R that catches of ultrasonic sensor 2, obtain the three-dimensional coordinate P (X, Y, Z) that barrier is more accurate.

Preferably, neural network algorithm can adopt three layers of BP neural network, and input layer is P _in=[X _w, Y _w, Z _w, R], be respectively the object dimensional volume coordinate P (X that stereoscopic vision merges rear gained _w, Y _w, Z _w) and the range information R that obtains of ultrasonic sensor, hidden layer has 9 nodes, the three dimensional space coordinate P (X, Y, Z) of object of output layer T=[X, Y, Z] for obtaining after fusion treatment.

Described BP neural network algorithm, first training sample is utilized to train neural network, neural network can according to current system accept the similarity determination criteria for classification of sample, the learning ability of neural network is utilized to obtain knowledge, probabilistic inference mechanism is obtained, the positional information P (X that can obtain video sensor according to the inference mechanism after study by the study of some steps _w, Y _w, Z _w) and the range information R that obtains of ultrasonic sensor merge, obtain comparatively accurate Obstacle Position information P (X, Y, Z).

The learning process of described BP neural network algorithm is made up of forward-propagating and backpropagation, first forward-propagating is carried out in learning process, if if output layer can not obtain the output expected in forward-propagating process, then turn to backpropagation through the study of multistep deconditioning study until error meets the demands.

Forward-propagating in described BP neural network algorithm learning process be input layer by the mapping of hidden layer to output layer, after forward-propagating, output layer obtains neuronic output y _n(k), if desirable network exports as y (k), then can obtain the error that network exports and ideal exports is:

e(k)＝y(k)-y _n(k)(4)

Getting error performance target function is:

$E=\frac{1}{2}e{\left(k\right)}^2---\left(5\right)$

Backpropagation in described BP neural network algorithm learning process is by error signal by connecting path backwards calculation, adjusts the neuronic weights of each layer, error signal is reduced by gradient descent method, the connection weights ω of output layer and hidden layer _j2learning algorithm be

${\mathrm{\&Delta;\&omega;}}_{j2}=-\mathrm{\&eta;}\frac{\&part;E}{\&part;{\mathrm{\&omega;}}_{j2}}=\mathrm{\&eta;}\&CenterDot;e\left(k\right)\&CenterDot;{x^{\&prime;}}_j---\left(6\right)$

In formula: η is learning rate, η ∈ [0,1];

X' _jfor the output of hidden layer neuron;

The network weight in k+1 moment is:

ω _j2(k+1)＝ω _j2(k)+Δω _j2(7)

Hidden layer and input layer connect weights ω _ijlearning algorithm is:

${\mathrm{\&Delta;\&omega;}}_{ij}=-\mathrm{\&eta;}\frac{\&part;E}{\&part;{\mathrm{\&omega;}}_{ij}}=\mathrm{\&eta;}\&CenterDot;e\left(k\right)\&CenterDot;\frac{\&part;y_n}{\&part;{\mathrm{\&omega;}}_{ij}}---\left(8\right)$

The network weight in k+1 moment is:

ω _ij(k+1)＝ω _ij(k)+Δω _ij(9)

For suppressing the vibration of BP neural network in learning process, improving its speed of convergence and learning efficiency, in learning process, adopting the method for Error function and self-adaptative adjustment learning rate.

Error function in described neural network learning process adds factor of momentum in weights learning process, and concrete algorithm is preferably:

ω _j2(k+1)＝ω _j2(k)+Δω _j2+α(ω _j2(k)-ω _j2(k-1))(10)

ω _ij(k+1)＝ω _ij(k)+Δω _ij+α(ω _ij(k)-ω _ij(k-1))(11)

In formula: α is factor of momentum, α ∈ (0,1).

Further, the self-adaptative adjustment learning rate control strategy in described neural network learning process, its method can with reference to as follows:

(1) suppose that initial learning rate is that η trains, obtain error performance index E (k);

(2) continuation training obtains error performance index is E (k+1);

(3) if E (k+1) > E (k), then make η (k+1)=0.7 η (k), E (k)=E (k+1), goes to (2);

(4) if E (k+1) < E (k), then η (k+1)=1.05 η (k) is made, E (k)=E (k+1);

(5) if E (k) meets the demands, deconditioning, otherwise go to (2).

The above is the preferred embodiment of the present invention; it should be pointed out that for those skilled in the art, under the prerequisite not departing from principle of the present invention; can also make some improvements and modifications, these improvements and modifications also should be considered as protection scope of the present invention.

Claims (6)

1. one kind is impacted Dual-Arm Coordination control system based on the robot anticollision of sensor fusion techniques, it is characterized in that, comprise binocular camera, ultrasonic sensor, sensor data fusion module, path planning module, motion controller, the first and second sixdegree-of-freedom simulation, wherein:

Described binocular camera is two and is installed on the end of described first and second sixdegree-of-freedom simulation respectively, described ultrasonic sensor is also two and is installed on the end of described first and second sixdegree-of-freedom simulation respectively, and described binocular camera is connected the input end of described sensor data fusion module with the output terminal of ultrasonic sensor;

The output terminal of described sensor data fusion module connects the input end of described path planning module;

The output terminal of described path planning module connects the input end of described motion controller;

The output terminal of described motion controller connects described first and second sixdegree-of-freedom simulation;

Described binocular camera and ultrasonic sensor are for detecting the obstacle information in operating environment, by the obstacle information of acquisition after described sensor data fusion module fusion treatment, the comparatively accurate Obstacle Position information obtained also passes to the path planning that described path planning module carries out anticollision impact, the routing information of planning is issued described motion controller by described path planning module afterwards, is controlled the motion of described first and second sixdegree-of-freedom simulation by described motion controller.

2. the robot anticollision based on sensor fusion techniques according to claim 1 impacts Dual-Arm Coordination control system, it is characterized in that, described binocular camera is further used for demarcating barrier, and gathers two dimensional image in different angles to the unique point on barrier;

Described ultrasonic sensor is further used for real-time detection surrounding environment, and after constantly detecting ultrasound wave and launching, run into the echo that barrier reflects, measure from being transmitted into the time T receiving echo, calculate relative distance S=CT/2, in formula, C is ultrasonic velocity.

3. the robot anticollision based on sensor fusion techniques according to claim 2 impacts Dual-Arm Coordination control system, it is characterized in that, the unique point that described sensor data fusion module is further used for binocular camera is caught carries out fusion calculation, obtain the track of unique point and the spatial attitude parameter of unique point place plane, after corresponding algorithm process, obtain the coordinate P (X of unique point in world coordinate system _w, Y _w, Z _w), then by the three-dimensional coordinate P (X of neural network algorithm fusion feature point _w, Y _w, Z _w) and the range information R that catches of ultrasonic sensor, obtain the three-dimensional coordinate P (X, Y, Z) that barrier is more accurate.

4. the robot anticollision based on sensor fusion techniques according to claim 3 impacts Dual-Arm Coordination control system, it is characterized in that, described path planning module is further used for the Obstacle Position information obtained according to described sensor data fusion module, the inverse of inspection machine mechanical arm is separated, for Robot Design one is without the movement locus of barrier, original path can be got back to simultaneously after requiring avoiding obstacles and continued corresponding operation.

5. the robot anticollision based on sensor fusion techniques according to claim 3 impacts Dual-Arm Coordination control system, and it is characterized in that, described neural network algorithm adopts three layers of BP neural network, and input layer is P _in=[X _w, Y _w, Z _w, R], be respectively the object dimensional volume coordinate (X that stereoscopic vision merges rear gained _w, Y _w, Z _w) and the range information R that obtains of ultrasonic sensor, hidden layer has 9 nodes, output layer T=[X, Y, Z] three dimensional space coordinate P (X, the Y of object for obtaining after fusion treatment, Z), in learning process, adopt the vibration in Error function and self-adaptative adjustment learning rate algorithm suppression learning process, improve speed of convergence, improve the learning efficiency of neural network.

6. the robot anticollision based on sensor fusion techniques according to claim 5 impacts Dual-Arm Coordination control system, and it is characterized in that, the formula of described Error function and self-adaptative adjustment learning rate algorithm is as follows:

ω _ij(k+1)＝ω _ij(k)+Δω _ij+α(ω _ij(k)-ω _ij(k-1))

ω _j2(k+1)＝ω _j2(k)+Δω _j2+α(ω _j2(k)-ω _j2(k-1))(1)

$\mathrm{\&eta;}(k+1)=\left\{\begin{array}{cc}0.7\mathrm{\&eta;}\left(k\right)& E(k+1)>E\left(k\right)\\ 1.05\mathrm{\&eta;}\left(k\right)& E(k+1)<E\left(k\right)\end{array}\right.---\left(2\right)$

Formula (1) is Error function, wherein ω _ij, ω _j2for link weight coefficients, α is factor of momentum α ∈ (0,1);

Formula (2) is self-adaptative adjustment learning rate algorithm, and wherein η is self study speed, and E (k) is error sum of squares.