CN111428334B - Robot station planning method in laser radar measurement - Google Patents

Abstract

The invention discloses a robot station position planning method in laser radar measurement, which is used for solving the technical problem that the measurement station position is more in the existing measurement viewpoint planning method in laser radar measurement. The technical scheme includes that firstly, a CAD simulation model is built, a coordinate system is built, then a view-point reachable cone model is built, discrete processing is conducted on the view-point reachable cone model according to the measurement precision requirement, a small ball set is screened by utilizing laser radar measurement constraint and industrial robot arm reachable space range constraint, small balls meeting constraint conditions are reserved, an intersection area containing the most kinds of small balls is taken, and the core of the intersection area is used as a laser radar measurement station site. The invention uses the discrete ball to carry out measurement reachable domain calculation, and determines the radius of the off-ball according to the measurement precision, so that the measurement speed is adaptive to the measurement precision. Aiming at different measurement accuracies, the algorithm keeps higher calculation efficiency, and the total number of the laser radar measurement station sites is reduced by 20-30%.

Description

Robot station planning method in laser radar measurement

Technical Field

The invention relates to a method for planning a measuring viewpoint in laser radar measurement, in particular to a method for planning a robot station in laser radar measurement.

Background

The automatic three-dimensional measurement of the laser radar is realized by adjusting the pose of the laser radar by a multi-degree-of-freedom robot and measuring parts from a plurality of measurement viewpoints so as to obtain accurate measurement data. The generation process of the measurement viewpoint affects the overall efficiency and accuracy of the detection.

The document "CN 109163674A a method for planning viewpoint of sensor measurement in three-dimensional automated measurement of surface structured light" proposes a method for planning viewpoint of measurement based on surface structured light measurement. By blocking the complex part, a large number of detected points are assigned to a single volume. A plurality of similar detected points are detected by using one measuring viewpoint, thereby improving the detection efficiency. However, for a complex assembly body, the spatial structure of the assembly body is more complex, and more measurement viewpoints are needed. The detection task is also several times that of complex parts. The method cannot meet the detection requirement of a complex assembly body.

There have been many studies on the automatic planning technology of the measuring viewpoint at home and abroad. The more advanced method is to analyze the interrelation among various constraints in the detection task and generate a measurement viewpoint which meets the limiting condition based on the interrelation. For the detection of complex assemblies, this method is computationally intensive and time consuming. A process for clustering optimization of a large number of measured viewpoints is lacking.

In summary, in the measurement process of the existing complex assembly body, the problems of low algorithm efficiency, redundant measurement points and the like exist.

Disclosure of Invention

The invention provides a robot station position planning method in laser radar measurement, aiming at overcoming the defect that the measurement station position is more in the existing measurement viewpoint planning method in laser radar measurement. The method comprises the steps of firstly constructing a CAD simulation model and a coordinate system, then constructing a view-point reachable cone model, performing discrete processing on the view-point reachable cone model according to the measurement precision requirement, screening a small ball set by utilizing laser radar measurement constraint and industrial robot arm reachable space range constraint, reserving small balls meeting the constraint condition, taking an intersecting area containing the most kinds of small balls, and taking the core of the intersecting area as a laser radar measuring station site. And removing the discrete balls corresponding to the measuring points which can be measured from the measuring station positions from the discrete ball set corresponding to all the measuring points. The above process continues for the remaining measurement points until lidar measurement station sites corresponding to all measurement points are generated. The invention uses the discrete ball to carry out measurement reachable domain calculation, and determines the radius of the off-ball according to the measurement precision, so that the measurement speed is adaptive to the measurement precision. The algorithm can keep higher calculation efficiency aiming at different measurement accuracies. The flexibility of the method of the invention is higher than that of the background art method. And performing geometric intersection calculation on the measurement reachable domain, so that the detection task is completed by using the laser radar measuring station sites with the least quantity, and the total number of the laser radar measuring station sites is reduced by 20-30%.

The technical scheme adopted by the invention for solving the technical problems is as follows: a robot station planning method in laser radar measurement is characterized by comprising the following steps:

(a) and (4) constructing a CAD simulation model and establishing a coordinate system. Adopting three-dimensional modeling software to assemble the known laser radar, robot arm model and part model on the detection platformAnd (4) placing on a table. Establishing a detection world coordinate system S_wOptionally, a point on the detection platform is taken as S_wUsing three orthogonal moving directions of the three-dimensional moving platform as S_wThe directions of the X, Y, Z axes of (1). Using the central point O of the robot arm base_bEstablishing a motion coordinate system S for a base point_bDirection of three coordinate axes and S_wThe three coordinate axes are in the same direction. The coordinates and surface unit normal vectors for all measurement points are noted.

(b) Constructing a view-point reachable cone model; extracting the coordinates and surface unit normal vector of each measuring point from the measured model; according to each measuring point P_w,iCoordinate (x) of_w,i,y_w,i,z_w,i) And surface unit normal vector

i 1,2, m, m is the total number of measurement points. With P_w,i(x_w,i,y_w,i,z_w,i) The vertex of the cone can be reached for the viewpoint,

making an apex angle theta for the axis_kThe viewpoint of can be reached as a cone. The view point can be taken to reach the zeta of the upper bus of the cone_i,0，ζ_i,0Has a direction vector of

Will ζ_i,0Rotated through an angle about an axis

Another bus zeta of the obtained station position on the accessible cone_i,l，

l＝0,1,...L-1，ζ_i,lThe direction vector of (a) is expressed as:

where I is a 3 x 3 identity matrix,

and representing the viewpoint reachable cone model by a plurality of discrete buses.

(c) And carrying out discrete processing on the view point reachable cone model according to the measurement precision requirement. Radius of sphere

C represents the accuracy requirement of the measured feature. The opposite station can be separated by layers of cones, and the height h of each layer is 2 multiplied by r_qThe composite material is divided into J layers in total,

radius of circle of j-th layer

Then the circles are scattered into circular rings, and the distance d between adjacent circular rings is 2 xr_qThe composite material is divided into K layers in total,

the ring of the kth layer of the jth layer of the circular table is recorded as ring_j,kThe radius of the circle is expressed as

Calculating circumference of a ring

By means of annular rings_j,kA circumference C of_c,j,kDivided by the diameter d of the pellet_q＝2·r_qThe result is rounded down to obtain the ring_j,kThe number of discrete beads L. The discrete globules are denoted q_j,k,l. Calculating to obtain a small ball q_j,k,lCenter coordinates (x) of_j,k,l,y_j,k,l,z_j,k,l)。

From the measuring point P_w,iEstablishingThe view reachable cone of (c) is represented by a collection of discrete spheres, denoted as:

S_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N⁺} (4)

(d) lidar measurement constraint definition. According to each measuring point P_w,iCharacteristic type and measurement accuracy requirement, laser radar station point M_w,iAnd the measuring point P_w,iThe distance constraint, the angle constraint and the interference constraint are satisfied.

Distance constraint laser radar station M_w,iAnd the measuring point P_w,iDistance L between_iMeets the requirement of effective range, i.e. L_min<L_i<L_max. Wherein L is_min，L_maxRespectively the minimum and maximum distances allowed, while meeting the requirements of measurement accuracy.

Angle constraint: from the measuring point P_w,iPointing laser radar station M_w,iVector of (2)

And the measuring point P_w,iIs the normal vector of

Angle theta_iMeet the requirement of effective range, i.e. theta_min<θ_i<θ_max. Wherein theta is_min，θ_maxAre the minimum and maximum angles allowed to meet the measurement accuracy requirements. Determined by the feature type of the detected object.

The measurement constraint boundaries are represented by discrete generatrix vectors, where the cone vertex angle θ_k＝2θ_i. Constraining L according to distance_min，L_maxAt bus ζ_i,0Upper cut line segment mu_i,0Line segment mu_i,0End point of is

And

P_A,w,i P_B,w,iwith surface sheetVector of normal position

The measurement constraint boundary of the lidar is obtained for a 360 ° rotation of the shaft.

(e) And the reachable space range of the arm of the industrial robot is restricted and defined. Establishing a connecting rod coordinate system by a classical D-H method, and establishing a connecting rod coordinate system by a connecting rod coordinate system R_iRelative to the link coordinate system R_i-1Coordinate transformation matrix of

Obtaining an equation W of a working space formed by the first three joints of the industrial robot_i(P_i ^b){W₀(P_i ^b)、W₁(P_i ^b)、W₂(P_i ^b) Therein of

c_θi＝cos(θ_i)，s_θi＝sin(θ_i)，c_αi＝cos(α_i)，

s_αi＝sin(α_i)

The working reachable area of the robot arm is determined by the working areas of the first three joints. According to structural parameters theta of industrial robot_iSatisfies theta_i ^min<θ_i<θ_i ^maxFor joint variable θ₂、θ₃By using the principle of limit combination, the product theta can be obtained₁0 hour industrial robot wrist joint end point P_i ^bWorking space boundary in robot coordinate system, and working space W is obtained according to working space boundary₀(P_i ^b) Z coordinates of key points, these key points being θ₁When the Z coordinate of the point with the maximum and minimum Z coordinates of the inner and outer boundaries of the working space and the point where the boundary expression changes is 0, the Z coordinate is recorded as Z₁,Z₂......,Z₇. Then, the end point P of the wrist joint is obtained_i ^bDistance D to the z-axis of the robot coordinate system_iAnd in correspondence with P_i ^wZ coordinate of (3) a workspace W₀(P_w) Distance from the inner and outer boundaries to the z-axis of the robot coordinate system

And

if there is

If true, then P is indicated_i ^bIn the working space W₀(P_i ^b) Inside. W₀(P_i ^b)，W₁(P_i ^b)W₂(P_i ^b) The parameter equations are respectively as follows:

in the formula (I), the compound is shown in the specification,

c₁＝cos(θ₁),c₂＝cos(θ₂),c₃＝cos(θ₃)；

s₁＝sin(θ₁),s₂＝sin(θ₂),s₃＝sin(θ₃)；

s₂₃＝sin(θ₂+θ₃)；

c₂₃＝cos(θ₂+θ₃)；

d₄is the joint offset distance of the industrial robot connecting rod 4; theta₁Is the joint corner of the industrial robot connecting rod 1; theta₂Is the joint corner of the industrial robot connecting rod 2;

θ₃is the joint corner of the industrial robot connecting rod 3; a is₁Is the length of the industrial robot link 1; a is₂Is a connecting rod of an industrial robot2;

a₃is the length of the industrial robot link 3; equation W for robot arm motion space boundary_i(P_i ^b){W₀(P_i ^b)、W₁(P_i ^b)、W₂(P_i ^b) Represents it.

(f) Collecting small balls S by using laser radar measurement constraint and industrial robot arm reachable space range constraint in (d) and (e)_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N⁺And (4) screening. And keeping the small balls meeting the constraint condition. To S_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N⁺Get the measurement reachable domain S by screening_i'。

(h) Measuring reachable domain S of each measuring point_i' intersection. Taking the intersection region T containing the most balls_iBy intersecting the region T_iThe core is used as a laser radar measuring station point Q_w,i. Will be measured from station site Q_w,iMeasurable measuring point P_w,iAnd removing the corresponding discrete balls from the discrete ball set corresponding to all the measuring points. The above process continues for the remaining measurement points until all measurement points P are generated_w,iCorresponding laser radar survey station point Q_w,i。

The invention has the beneficial effects that: the method comprises the steps of firstly constructing a CAD simulation model and a coordinate system, then constructing a view-point reachable cone model, performing discrete processing on the view-point reachable cone model according to the measurement precision requirement, screening a small ball set by utilizing laser radar measurement constraint and industrial robot arm reachable space range constraint, reserving small balls meeting the constraint condition, taking an intersecting area containing the most kinds of small balls, and taking the core of the intersecting area as a laser radar measuring station site. And removing the discrete balls corresponding to the measuring points which can be measured from the measuring station positions from the discrete ball set corresponding to all the measuring points. The above process continues for the remaining measurement points until lidar measurement station sites corresponding to all measurement points are generated. The invention uses the discrete ball to carry out measurement reachable domain calculation, and determines the radius of the off-ball according to the measurement precision, so that the measurement speed is adaptive to the measurement precision. The algorithm can keep higher calculation efficiency aiming at different measurement accuracies. The algorithm of the invention has higher flexibility than the algorithm used at present. And performing geometric intersection calculation on the measurement reachable domain, so that the detection task is completed by using the laser radar measuring station sites with the least quantity, and the total number of the laser radar measuring station sites is reduced by 20-30%.

The present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

Drawings

FIG. 1 is a flow chart of a robot station planning method in laser radar measurement according to the present invention.

Fig. 2 is a lidar measurement constraint definition in the method of the present invention.

FIG. 3 is a discretization model of the measured accessible cone in the method of the invention.

FIG. 4 illustrates the range of motion constraints for the robot arm in the method of the present invention.

FIG. 5 is a schematic diagram of the measurement reachable domain intersection in the method of the present invention.

Detailed Description

Reference is made to fig. 1-5. The robot station planning method in the laser radar measurement comprises the following specific steps:

step 1, establishing a CAD simulation model and establishing a coordinate system.

Known lidar, robotic arm models and part models were assembled on an inspection platform using UG software. Establishing a detection world coordinate system S_wOptionally, a point on the detection platform is taken as S_wUsing three orthogonal moving directions of the three-dimensional moving platform as S_wThe directions of the X, Y, Z axes of (1). Using the central point O of the robot arm base_bEstablishing a motion coordinate system S for a base point_bDirection of three coordinate axes and S_wThe three coordinate axes are in the same direction. The coordinates and surface normal of all measurement points are noted.

And 2, constructing a view point reachable cone model.

According to each measuring point P_w,iCoordinate (x) of_w,i,y_w,i,z_w,i) And surface unit normal vector

i 1,2, m, m is the total number of measurement points. With P_w,i(x_w,i,y_w,i,z_w,i) The vertex of the cone can be reached for the viewpoint,

making an apex angle theta for the axis_kThe viewpoint of can be reached as a cone. The view point can be taken to reach the zeta of the upper bus of the cone_i,0，ζ_i,0Has a direction vector of

Wherein

Will ζ_i,0Rotated through an angle about an axis

Another bus zeta of the obtained station position on the accessible cone_i,l，

l＝0,1,...L-1，ζ_i,lCan be expressed as

Where I is a 3 x 3 identity matrix,

and representing the viewpoint reachable cone model by a plurality of discrete buses.

And 3, discrete processing of the view point reachable cone model.

And carrying out discrete processing on the view point reachable cone model according to the measurement precision requirement. The view reachable cone is represented by a discrete sphere. Radius of sphere

C represents the accuracy requirement of the measured feature. The opposite station can be separated by layers of cones, and the height h of each layer is 2 multiplied by r_qThe composite material is divided into J layers in total,

radius of circle of j-th layer

Then the circles are scattered into circular rings, and the distance d between adjacent circular rings is 2 xr_qThe composite material is divided into K layers in total,

the ring of the kth layer of the jth layer of the circular table is recorded as ring_j,kThe radius of the circle is expressed as

Calculating circumference of a ring

By means of annular rings_j,kA circumference C of_c,j,kDivided by the diameter d of the pellet_q＝2·r_qThe result is rounded down to obtain the ring_j,kThe number of discrete beads L. The discrete globules are denoted as q_j,k,l. Calculating to obtain a small ball q_j,k,lCenter coordinates (x) of_j,k,l,y_j,k,l,z_j,k,l)。

From the measuring point P_w,iThe established view point reachable cone is represented by a set of discrete small balls, which is marked as S_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N⁺}。

And 4, defining the measurement constraint of the laser radar.

According to each measuring point P_w,iCharacteristic type and measurement accuracy requirement, laser radar station point M_w,iAnd the measuring point P_w,iAnd the distance constraint, the angle constraint and the interference constraint are met.

Distance constraint laser radar station M_w,iAnd the measuring point P_w,iDistance L between_iMeets the requirement of effective range, i.e. L_min<L_i<L_max. Wherein L is_min，L_maxRespectively the minimum and maximum distances allowed, while meeting the requirements of measurement accuracy.

Angle constraint: from the measuring point P_w,iPointing laser radar station M_w,iVector of (2)

And the measuring point P_w,iIs the normal vector of

Included angle theta_iMeet the requirement of effective range, i.e. theta_min<θ_i<θ_max. Wherein theta is_min，θ_maxAre the minimum and maximum angles allowed to meet the measurement accuracy requirements. Determined by the feature type of the detected object.

The measurement constraint boundaries are represented by discrete generatrix vectors, where the cone vertex angle θ_k＝2θ_i. Constraining L according to distance_min，L_maxAt bus ζ_i,0Upper cut line segment mu_i,0Line segment mu_i,0End point of is

And

P_A,w,i P_B,w,inormal vector of surface unit

The measurement constraint boundary of the lidar is obtained for a 360 ° rotation of the shaft.

And 5, defining the reachable space range constraint of the arm of the industrial robot.

Establishing a connecting rod coordinate system by a classical D-H method and using a connecting rod coordinate system R_iRelative to the link coordinate system R_i-1Coordinate transformation matrix of

Obtaining an equation W of a working space formed by the first three joints of the industrial robot_i(P_i ^b){W₀(P_i ^b)、W₁(P_i ^b)、W₂(P_i ^b) Therein of

c_θi＝cos(θ_i)，s_θi＝sin(θ_i)，c_αi＝cos(α_i)，

s_αi＝sin(α_i)

The working reachable area of the robot arm is determined by the working areas of the first three joints. According to structural parameters theta of industrial robot_iSatisfies theta_i ^min<θ_i<θ_i ^maxFor joint variable θ₂、θ₃By using the principle of limit combination, the product theta can be obtained₁0 hour industrial robot wrist joint end point P_i ^bWorking space boundary in robot coordinate system, and working space W is obtained according to working space boundary₀(P_i ^b) Z coordinates of key points, these key points being θ₁When the Z coordinate of the point with the maximum and minimum Z coordinates of the inner and outer boundaries of the working space and the point where the boundary expression changes is 0, the Z coordinate is recorded as Z₁,Z₂......,Z₇. Then, the end point P of the wrist joint is obtained_i ^bDistance D to the z-axis of the robot coordinate system_iAnd in correspondence with P_i ^wZ coordinate of (2) working space W₀(P_w) Distance between the inner and outer boundaries and the z-axis of the robot coordinate system

And

if there is

If true, then P is indicated_i ^bIn the working space W₀(P_i ^b) Inside. W₀(P_i ^b)，W₁(P_i ^b)W₂(P_i ^b) The parameter equations are respectively as follows:

in the formula:

c₁＝cos(θ₁),c₂＝cos(θ₂),c₃＝cos(θ₃)；

s₁＝sin(θ₁),s₂＝sin(θ₂),s₃＝sin(θ₃)；

s₂₃＝sin(θ₂+θ₃)；

c₂₃＝cos(θ₂+θ₃)；

d₄is the joint offset distance of the industrial robot connecting rod 4; theta₁Is the joint corner of the industrial robot connecting rod 1; theta₂Is the joint corner of the industrial robot connecting rod 2; theta₃Is the joint corner of the industrial robot connecting rod 3; a is₁Is the length of the industrial robot link 1; a is₂Is the length of the industrial robot link 2; a is₃Is the length of the industrial robot link 3;

equation W for robot arm motion space boundary_i(P_i ^b){W₀(P_i ^b)、W₁(P_i ^b)、W₂(P_i ^b) } tableShown in the figure.

And 6, generating a measurement reachable domain according to the small balls meeting the constraint screening conditions.

Collecting the small balls S by using the laser radar measurement constraint and the industrial robot arm reachable space range constraint in the steps 4 and 5_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N⁺And (4) screening. The pellets that meet the constraints are retained. To S_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N⁺Get the measurement reachable domain S by screening_i'。

And 7, intersecting the measurement reachable domains and calculating to obtain the measurement station site.

Measuring reachable domain S of each measuring point_i' intersection. Taking the intersection region T containing the most balls_iBy intersecting the region T_iThe core is used as a laser radar measuring station point Q_w,i. Will be measured from station site Q_w,iMeasurable measuring point P_w,iAnd removing the corresponding discrete balls from the discrete ball set corresponding to all the measuring points. The above process continues for the remaining measurement points until all measurement points P are generated_w,iCorresponding laser radar survey station point Q_w,i。

Claims (1)

1. A robot station planning method in laser radar measurement is characterized by comprising the following steps:

(a) establishing a CAD simulation model and establishing a coordinate system; assembling a known laser radar, a robot arm model and a part model on a detection platform by adopting three-dimensional modeling software; establishing a detection world coordinate system S_wOptionally, a point on the detection platform is taken as S_wUsing three orthogonal motion directions of the three-dimensional moving platform as S_wThe directions of the X, Y, Z axes of (a); using the central point O of the robot arm base_bEstablishing a motion coordinate system S for a base point_bDirection of three coordinate axes and S_wThe directions of the three coordinate axes are the same; marking out coordinates and surface unit normal vectors of all measurement points;

(b) constructing a view-point reachable cone model; slave quiltExtracting the coordinates and surface unit normal vector of each measuring point from the measuring model; according to each measuring point P_w,iCoordinate (x) of_w,i,y_w,i,z_w,i) And surface unit normal vector

m is the total number of the measuring points; with P_w,i(x_w,i,y_w,i,z_w,i) The vertex of the cone can be reached for the viewpoint,

making an apex angle theta for the axis_kThe viewpoint of can reach a cone; the view point can be taken to reach the zeta of the upper bus of the cone_i,0，ζ_i,0Has a direction vector of

Will ζ_i,0Rotated through an angle about an axis

Another bus zeta of which the station position can reach on the cone is obtained_i,l，

ζ_i,lThe direction vector of (a) is expressed as:

where I is a 3 x 3 identity matrix,

representing a viewpoint reachable cone model by a plurality of discrete buses;

(c) performing discrete processing on the view point reachable cone model according to the measurement precision requirement; radius of sphere

C represents the accuracy requirement of the measured characteristic; the opposite station can be separated by layers of cones, and the height h of each layer is 2 multiplied by r_qThe composite material is divided into J layers in total,

radius of circle of j-th layer

Then the circles are scattered into circular rings, and the distance d between adjacent circular rings is 2 xr_qThe composite film is divided into K layers in total,

the ring of the kth layer of the jth layer of the circular table is recorded as ring_j,kThe radius of the circle is expressed as

Calculating circumference of a ring

By means of annular rings_j,kA circumference C of_c,j,kDivided by the diameter d of the pellet_q＝2·r_qThe result is rounded down to obtain the ring_j,kThe number L of upper discrete pellets; the discrete globules are denoted as q_j,k,l(ii) a Calculating to obtain a small ball q_j,k,lCenter coordinates (x) of_j,k,l,y_j,k,l,z_j,k,l)；

From the measuring point P_w,iThe established view reachable cone is represented by a set of discrete spheres, and is recorded as:

S_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N⁺} (4)

(d) laserOptical radar measurement constraint definition; according to each measuring point P_w,iCharacteristic type and measurement accuracy requirement, laser radar station point M_w,iAnd the measuring point P_w,iSatisfying distance constraints, angle constraints and interference constraints;

distance constraint laser radar station M_w,iAnd the measuring point P_w,iA distance L therebetween_iMeets the requirement of effective range, i.e. L_min<L_i<L_max(ii) a Wherein L is_min，L_maxThe minimum and maximum distances allowed under the requirement of meeting the measurement precision;

angle constraint: from the measuring point P_w,iPointing laser radar station M_w,iVector of (2)

And the measuring point P_w,iIs the normal vector of

Included angle theta_iMeet the requirement of effective range, i.e. theta_min<θ_i<θ_max(ii) a Wherein theta is_min，θ_maxIs the minimum and maximum angle allowed to meet the measurement accuracy requirement; determining by the characteristic type of the detection object;

the measurement constraint boundaries are represented by discrete generatrix vectors, where the cone vertex angle θ_k＝2θ_i(ii) a Constraining L according to distance_min，L_maxAt bus ζ_i,0Upper cut line segment mu_i,0Line segment mu_i,0End point of is

And

P_A,w,iP_B,w,inormal vector of surface unit

Is rotated by 360 degrees to obtain the excitationA measurement constraint boundary for the optical radar;

(e) the reachable space range of the arm of the industrial robot is restricted and defined; establishing a connecting rod coordinate system by a D-H method and using a connecting rod coordinate system R_iRelative to the link coordinate system R_i-1Coordinate transformation matrix of

Obtaining an equation W of a working space formed by the first three joints of the industrial robot_i(P_i ^b){W₀(P_i ^b)、W₁(P_i ^b)、W₂(P_i ^b) Therein of

c_θi＝cos(θ_i)，s_θi＝sin(θ_i)，c_αi＝cos(α_i)，

s_αi＝sin(α_i)

The working reachable area of the robot arm is determined by the working areas of the first three joints; according to structural parameters theta of industrial robot_iSatisfy the requirement of

For joint variable theta₂、θ₃By using the principle of limit combination, the product theta can be obtained₁0 hour industrial robot wrist joint end point P_i ^bWorking space boundary in robot coordinate system, and working space W is obtained according to working space boundary₀(P_i ^b) Z coordinates of key points, these key points being θ₁When the Z coordinate of the point with the maximum and minimum Z coordinates of the inner and outer boundaries of the working space and the point where the boundary expression changes is 0, the Z coordinate is recorded as Z₁,Z₂......,Z₇(ii) a Then, the end point P of the wrist joint is obtained_i ^bDistance D to the z-axis of the robot coordinate system_iAnd in correspondence with P_i ^wZ coordinate of (3) a workspace W₀(P_w) Distance between the inner and outer boundaries and the z-axis of the robot coordinate system

And

if there is

If true, then P is indicated_i ^bIn the working space W₀(P_i ^b) An inner portion; w₀(P_i ^b)，W₁(P_i ^b)W₂(P_i ^b) The parameter equations are respectively as follows:

in the formula (I), the compound is shown in the specification,

c₁＝cos(θ₁),c₂＝cos(θ₂),c₃＝cos(θ₃)；

s₁＝sin(θ₁),s₂＝sin(θ₂),s₃＝sin(θ₃)；

s₂₃＝sin(θ₂+θ₃)；

c₂₃＝cos(θ₂+θ₃)；

d₄is the joint offset distance of the industrial robot connecting rod 4; theta₁Is the joint corner of the industrial robot connecting rod 1; theta₂Is the joint corner of the industrial robot connecting rod 2;

θ₃is the joint corner of the industrial robot connecting rod 3; a is₁Is the length of the industrial robot link 1; a is₂Is the length of the industrial robot link 2;

a₃is the length of the industrial robot link 3; equation W for robot arm motion space boundary_i(P_i ^b){W₀(P_i ^b)、W₁(P_i ^b)、W₂(P_i ^b) Represents;

(f) collecting small balls S by using laser radar measurement constraint and industrial robot arm reachable space range constraint in (d) and (e)_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N⁺Screening; reserving the small balls meeting the constraint condition; to S_i＝{q_j,k,l|j∈[1,J],k∈[1,K],l∈[1,L],N⁺Screening to obtain a measured reachable domain S'_i；

(h) Measuring reachable domain S 'of each measuring point'_iIntersection is carried out; taking the intersection region T containing the most balls_iBy intersecting the region T_iThe core is used as a laser radar measuring station point Q_w,i(ii) a Will be from the survey station site Q_w,iMeasurable measuring point P_w,iRemoving the corresponding discrete small balls from the discrete small ball set corresponding to all the measuring points; the above process continues for the remaining measurement points until all measurement points P are generated_w,iCorresponding laser radar survey station point Q_w,i。

Images