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

Abstract

The invention discloses a robot station planning method in laser radar measurement, which is used for solving the technical problem of many measurement stations in the existing measurement viewpoint planning method in laser radar measurement. The technical solution is to first build a CAD simulation model and establish a coordinate system, and then build a viewpoint reachable cone model. According to the measurement accuracy requirements, the viewpoint reachable cone model is discretely processed, and the measurement constraints of the lidar and the reachable space range of the industrial robot arm are used to measure the constraints. The collection of small balls is screened, the balls that meet the constraints are retained, the intersection area with the most types of balls is selected, and the center of the intersection area is used as the lidar measurement station. The invention uses discrete small balls to perform measurement reachable domain calculation, determines the distance from the small ball radius according to the measurement accuracy, and adapts the measurement rate to the measurement accuracy. For different measurement precisions, the algorithms maintain high computational efficiency, and the total number of lidar measurement stations is reduced by 20-30%.

Description

Robot station planning method in lidar survey

technical field

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

Background technique

Lidar automated 3D measurement is a multi-degree-of-freedom robot that adjusts the lidar pose and measures parts from multiple measurement viewpoints to obtain accurate measurement data. The generation process of measurement viewpoints affects the overall efficiency and accuracy of detection.

The document "CN109163674A A method for planning a measurement viewpoint of a sensor in automatic three-dimensional measurement of surface structured light" proposes a measurement viewpoint planning method based on surface structured light measurement. Distribute a large number of inspected points into a single volume by partitioning complex parts. Use one measurement viewpoint to detect multiple similar detected points, thereby improving detection efficiency. However, for complex assemblies, the spatial structure of the assembly is more complex, and more measurement viewpoints are required. The inspection task is also several times that of complex parts. This method cannot meet the inspection needs of complex assemblies.

There have been many researches on automatic planning technology of measurement viewpoints at home and abroad. One of the more advanced methods is to analyze the relationship between various constraints in the detection task, and generate a measurement viewpoint that meets the constraints based on these relationships. For the detection of complex assemblies, this method is computationally intensive and time-consuming. There is a lack of an optimized process for clustering a large number of measurement viewpoints.

To sum up, in the current complex assembly measurement process, there are problems such as low algorithm efficiency and redundant measurement points.

SUMMARY OF THE INVENTION

In order to overcome the shortage of many measuring stations in the existing measurement viewpoint planning method in the laser radar measurement, the present invention provides a robot station planning method in the laser radar measurement. This method first builds a CAD simulation model and establishes a coordinate system, and then builds a viewpoint reachable cone model. According to the measurement accuracy requirements, the viewpoint reachable cone model is discretized. The measurement constraints of lidar and the constraints of the reachable space range of industrial robot arms are used to adjust the small The ball set is screened, the balls that meet the constraints are retained, the intersection area with the most types of balls is selected, and the center of the intersection area is used as the lidar measurement station. The discrete spheres corresponding to the measurement points that can be measured from the measuring station are removed from the set of discrete spheres corresponding to all the measurement points. Continue the above process for the remaining measurement points until the lidar measurement station points corresponding to all the measurement points are generated. The invention uses discrete small balls to perform measurement reachable domain calculation, determines the distance from the small ball radius according to the measurement accuracy, and adapts the measurement rate to the measurement accuracy. For different measurement accuracy, the algorithm can maintain high computational efficiency. The flexibility of the method of the present invention is higher than that of the background art method. The geometric intersection operation is performed on the measurement reachable area, so that the detection task can be completed with the least number of lidar measurement stations, and the total number of lidar measurement stations can be reduced by 20-30%.

The technical solution adopted by the present invention to solve the technical problem: a robot station planning method in laser radar measurement, which is characterized by comprising the following steps:

(a) Build a CAD simulation model and establish a coordinate system. Using 3D modeling software, the known lidar, robot arm model and part model are assembled on the inspection platform. Establish the detection world coordinate system S _w , choose a point on the detection platform as the origin of S _w , and use the three orthogonal motion directions of the three-dimensional moving platform as the directions of the X, Y, and Z axes of S _w respectively. The motion coordinate system S _b is established with the center point _Ob of the base of the robot arm as the base point, and the directions of the three coordinate axes are the same as those of the three coordinate axes of S _w . Label the coordinates and surface unit normals of all measurement points.

i＝1,2,...,m，m是测量点总个数。以P_w,i(x_w,i,y_w,i,z_w,i)为视点可达圆锥顶点，

为轴线作一个顶角为θ_k的视点可达圆锥。取视点可达圆锥上一条母线ζ_i,0，ζ_i,0的方向矢量为(b) Construct the view point reachable cone model; extract the coordinates of each measurement point and the surface unit normal vector from the measured model; according to the coordinates of each measurement point _Pw,i ( _xw,i , _yw,i ,z _w,i ) and the surface unit normal

i=1,2,...,m, where m is the total number of measurement points. Taking P _w,i (x _w,i ,y _w,i ,z _w,i ) as the viewpoint, the vertex of the cone can be reached,

Make a point of view accessibility cone with an apex angle θ _k for the axis. Taking a generatrix ζ _i,0 on the viewpoint reachable cone, the direction vector of ζ _i,0 is

后得到站位可达圆锥上的另一条母线ζ_i,l，

l＝0,1,...L-1，ζ_i,l的方向矢量表示为:Rotate ζ _i,0 around the axis by an angle

Then get another bus ζ _i,l on the station reachable cone,

l=0,1,...L-1, the direction vector of ζ _i,l is expressed as:

where I is a 3 × 3 identity matrix,

The viewpoint-reachable cone model is represented by multiple discrete bus bars.

C表示测量特征的精度要求。对站位可达圆锥分层离散，每层高度h＝2×r_q，共分为J层，

第j层的圆半径

再将圆离散成圆环，相邻圆环间距d＝2×r_q，共分为K层，

第j层圆台的第k层圆环记为ring_j,k，圆环半径表示为

计算圆环的周长

用圆环ring_j,k的周长C_c,j,k除以小球直径d_q＝2·r_q，结果向下取整，得到圆环ring_j,k上离散小球的数量L。离散小球记作q_j,k,l。计算得出小球q_j,k,l的圆心坐标(x_j,k,l,y_j,k,l,z_j,k,l)。(c) Discrete the viewpoint-reachable cone model according to the measurement accuracy requirements. Ball radius

C represents the accuracy requirement of the measurement feature. The station reachable cone is discrete in layers, the height of each layer is h=2×r _q , and it is divided into J layers.

The circle radius of the jth layer

The circle is then discretized into rings, and the distance between adjacent rings is d=2×r _q , which are divided into K layers.

The k-th ring of the j-th truncated cone is denoted as ring _j,k , and the radius of the ring is expressed as

Calculate the circumference of a ring

Divide the perimeter C _c,j,k of the ring _j, k by the diameter of the sphere d _q =2·r _q , and round down the result to obtain the number L of discrete spheres on the ring _j,k . The discrete ball is denoted q _j,k,l . Calculate the coordinates of the center of the ball q _j,k,l (x _j,k,l ,y _j,k,l ,z _j,k,l ).

The viewpoint reachable cone established by the measurement point _Pw,i is represented by a set of discrete spheres, denoted as:

S _i ={q _j,k,l |j∈[1,J],k∈[1,K],l∈[1,L],N ⁺ } (4)

(d) Definition of lidar measurement constraints. According to the characteristic type and measurement accuracy requirements of each measurement point _Pw,i , the lidar station _Mw,i and the measurement point _Pw,i satisfy the distance constraint, the angle approximation and the interference constraint.

Distance constraint: the distance Li between the lidar station M _w,i and the measurement point P _{w, i} satisfies the effective range requirement, that is, L _min <L _i <L _max . Among them, L _min and L _max are the minimum and maximum distances allowed under the requirement of measurement accuracy, respectively.

与测量点P_w,i的法矢

夹角θ_i满足有效范围要求，即θ_min<θ_i<θ_max。其中θ_min，θ_max是满足测量精度要求所允许的最小和最大角度。由检测对象的特征类型决定。Angle constraint: the vector from the measurement point _Pw,i to the lidar station _Mw,i

The normal vector to the measurement point P _w,i

The included angle θ _i meets the effective range requirement, that is, θ _min <θ _i <θ _max . where θ _min , θ _max are the minimum and maximum angles allowed to meet the measurement accuracy requirements. It is determined by the feature type of the detected object.

和

P_A,w,i P_B,w,i以表面单位法矢

为轴旋转360°得到激光雷达的测量约束边界。The measurement constraint boundaries are represented by discrete generatrix vectors, where the cone apex angle θ _k = 2θ _i . According to the distance constraint L _min , L _max intercepts the line segment μ _{i, 0 on the busbar ζ i,0} , and the end point of the line segment μ _i,0 is

and

P _A,w,i P _B,w,i in surface units normal

Rotate 360° for the axis to get the lidar's measurement constraint bounds.

得到工业机器人前三个关节形成的工作空间的方程W_i(P_i ^b){W₀(P_i ^b)、W₁(P_i ^b)、W₂(P_i ^b)}，其中(e) The constraint definition of the reachable space range of the industrial robot arm. The connecting rod coordinate system is established by the classical DH method, and the coordinate transformation matrix of the connecting rod coordinate system R _i relative to the connecting rod coordinate system R _i-1 is used

Obtain the equation Wi (P _i ^b ){W ₀ (P _i ^b ), W ₁ (P _i ^b ), W ₂ (P _i ^b )} of the workspace formed by the first three joints of the industrial robot _, where

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

c _θi =cos(θ _i ), s _θi =sin(θ _i ), c _αi =cos(α _i ),

s _αi =sin(α _i )

和

如果有

成立，则说明P_i ^b在工作空间W₀(P_i ^b)内部。W₀(P_i ^b)，W₁(P_i ^b)W₂(P_i ^b)的参数方程分别为：The working reachable area of the robot arm is determined by the working area of the first three joints. According to the structural parameters of the industrial robot, the joint variable θ _i satisfies θ _i ^min <θ _i <θ _i ^max , and the limit combination principle is adopted for the joint variables θ ₂ and θ ₃ , and the endpoint P _i of the wrist joint of the industrial robot can be obtained when θ ₁ =0 ^b is the workspace boundary in the robot coordinate system, and then according to the workspace boundary, the z-coordinates of the key points of the workspace W ₀ (P _i ^b ) are obtained. These key points are the maximum sum of the z-coordinates of the inner and outer boundaries of the workspace when θ ₁ =0 The smallest point and the z-coordinate of the point where the boundary expression changes, denoted Z ₁ , Z ₂ ......, Z ₇ . Then find the distance D _i from the wrist joint endpoint P _i ^b to the z-axis of the robot coordinate system, and the distance from the inner and outer boundaries of the workspace W ₀ (P _w ) at the z-coordinate of the corresponding P _i ^w to the z-axis of the robot coordinate system

and

If there is

If established, it means that P _i ^b is inside the workspace W ₀ (P _i ^b ). The parametric equations of W ₀ (P _i ^b ), W ₁ (P _i ^b ) and W ₂ (P _i ^b ) are:

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 link 4; θ ₁ is the joint rotation angle of the industrial robot link 1; θ ₂ is the joint rotation angle of the industrial robot link 2;

θ ₃ is the joint rotation angle of the industrial robot link 3; a ₁ is the length of the industrial robot link 1; a ₂ is the length of the industrial robot link 2;

a ₃ is the length of the industrial robot link 3; the robot arm motion space boundary uses the equation Wi (P _i ^b ){W ₀ (P _i ^b ), W ₁ (P _i ^b ), W ₂ ( _{P i b} ⁾ } express.

(f) Using the lidar measurement constraints in (d) and (e) and the reachable space range constraints of the industrial robot arm for the set of small balls S _i ={q _j,k,l |j∈[1,J],k ∈[1,K],l∈[1,L],N ⁺ } to filter. Keep the balls that meet the constraints. S _i = {q _j,k,l |j∈[1,J],k∈[1,K],l∈[1,L],N ⁺ _} to obtain the measurement reachable domain Si '.

(h) Intersect the measurement reachable domains S _i ' of each measurement point. Take the intersection area Ti that contains the most types of spheres, and take the center of the intersection area Ti as the lidar measurement station Q _{w ,i .} The discrete spheres corresponding to the measurement points P _w,i that can be measured from the measuring station site Q _w,i are removed from the set of discrete spheres corresponding to all the measurement points. The above process is continued for the remaining measurement points until the lidar measurement station points Q _{w, i corresponding to all the measurement points P w,i} are generated.

The beneficial effects of the invention are as follows: the method first constructs a CAD simulation model and establishes a coordinate system, and then constructs a viewpoint reachable cone model, performs discrete processing on the viewpoint reachable cone model according to the measurement accuracy requirements, and uses laser radar to measure constraints and industrial robot arms. The reachable space constraints are used to screen the set of small balls, and the small balls that meet the constraints are retained. The intersection area with the most types of small balls is selected, and the center of the intersection area is used as the lidar measurement station. The discrete spheres corresponding to the measurement points that can be measured from the measuring station are removed from the set of discrete spheres corresponding to all the measurement points. Continue the above process for the remaining measurement points until the lidar measurement station points corresponding to all the measurement points are generated. The invention uses discrete small balls to perform measurement reachable domain calculation, determines the distance from the small ball radius according to the measurement accuracy, and adapts the measurement rate to the measurement accuracy. For different measurement accuracy, the algorithm can maintain high computational efficiency. The algorithm of the present invention is more flexible than currently used algorithms. The geometric intersection operation is performed on the measurement reachable area, so that the detection task can be completed with the least number of lidar measurement stations, and the total number of lidar measurement stations can be reduced by 20-30%.

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

Description of drawings

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

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

Fig. 3 is the discretization model of the measurement reachable cone in the method of the present invention.

FIG. 4 is the restriction of the movement range of 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 ways

Refer to Figures 1-5. The specific steps of the robot station planning method in the laser radar measurement of the present invention are as follows:

Step 1. Build a CAD simulation model and establish a coordinate system.

Using UG software, the known lidar, robot arm model and part model are assembled on the inspection platform. Establish the detection world coordinate system S _w , choose a point on the detection platform as the origin of S _w , and use the three orthogonal motion directions of the three-dimensional moving platform as the directions of the X, Y, and Z axes of S _w respectively. The motion coordinate system S _b is established with the center point _Ob of the base of the robot arm as the base point, and the directions of the three coordinate axes are the same as those of the three coordinate axes of S _w . The coordinates and surface formulas of all measurement points are marked.

Step 2. Construct the viewpoint reachable cone model.

i＝1,2,...,m，m是测量点总个数。以P_w,i(x_w,i,y_w,i,z_w,i)为视点可达圆锥顶点，

为轴线作一个顶角为θ_k的视点可达圆锥。取视点可达圆锥上一条母线ζ_i,0，ζ_i,0的方向矢量为

其中

According to the coordinates ( _xw,i , _yw,i , _zw,i ) of each measurement point _Pw ,i and the surface unit normal vector

i=1,2,...,m, where m is the total number of measurement points. Taking P _w,i (x _w,i ,y _w,i ,z _w,i ) as the viewpoint, the vertex of the cone can be reached,

Make a point of view accessibility cone with an apex angle θ _k for the axis. Taking a generatrix ζ _i,0 on the viewpoint reachable cone, the direction vector of ζ _i,0 is

in

后得到站位可达圆锥上的另一条母线ζ_i,l，

l＝0,1,...L-1，ζ_i,l的方向矢量可表示为

其中I为3×3单位矩阵，

Rotate ζ _i,0 around the axis by an angle

Then get another bus ζ _i,l on the station reachable cone,

l=0,1,...L-1, the direction vector of ζ _i,l can be expressed as

where I is a 3 × 3 identity matrix,

The viewpoint-reachable cone model is represented by multiple discrete bus bars.

Step 3: Discrete processing of the viewpoint reachable cone model.

C表示测量特征的精度要求。对站位可达圆锥分层离散，每层高度h＝2×r_q，共分为J层，

第j层的圆半径

再将圆离散成圆环，相邻圆环间距d＝2×r_q，共分为K层，

第j层圆台的第k层圆环记为ring_j,k，圆环半径表示为

计算圆环的周长

用圆环ring_j,k的周长C_c,j,k除以小球直径d_q＝2·r_q，结果向下取整，得到圆环ring_j,k上离散小球的数量L。离散小球记作q_j,k,l。计算得出小球q_j,k,l的圆心坐标(x_j,k,l,y_j,k,l,z_j,k,l)。According to the requirements of measurement accuracy, the viewpoint-reachable cone model is discretized. The viewpoint accessibility cone is represented by discrete spheres. Ball radius

C represents the accuracy requirement of the measurement feature. The station reachable cone is discrete in layers, the height of each layer is h=2×r _q , and it is divided into J layers.

The circle radius of the jth layer

The circle is then discretized into rings, and the distance between adjacent rings is d=2×r _q , which are divided into K layers.

The k-th ring of the j-th truncated cone is denoted as ring _j,k , and the radius of the ring is expressed as

Calculate the circumference of a ring

Divide the perimeter C _c,j,k of the ring _j, k by the diameter of the sphere d _q =2·r _q , and round down the result to obtain the number L of discrete spheres on the ring _j,k . The discrete ball is denoted q _j,k,l . Calculate the coordinates of the center of the ball q _j,k,l (x _j,k,l ,y _j,k,l ,z _j,k,l ).

The viewpoint reachable cone established by the measurement point P _w,i is represented by a set of discrete spheres, denoted as S _i ={q _j,k,l |j∈[1,J],k∈[1,K], l∈[1,L],N ⁺ }.

Step 4. Definition of lidar measurement constraints.

According to the characteristic type and measurement accuracy requirements of each measurement point _Pw,i , the lidar station _Mw,i and the measurement point _Pw,i satisfy distance constraints, angle constraints, and interference constraints.

Distance constraint: the distance Li between the lidar station M _w,i and the measurement point P _{w, i} satisfies the effective range requirement, that is, L _min <L _i <L _max . Among them, L _min and L _max are the minimum and maximum distances allowed under the requirement of measurement accuracy, respectively.

与测量点P_w,i的法矢

夹角θ_i满足有效范围要求，即θ_min<θ_i<θ_max。其中θ_min，θ_max是满足测量精度要求所允许的最小和最大角度。由检测对象的特征类型决定。Angle constraint: the vector from the measurement point _Pw,i to the lidar station _Mw,i

The normal vector to the measurement point P _w,i

The included angle θ _i meets the effective range requirement, that is, θ _min <θ _i <θ _max . where θ _min , θ _max are the minimum and maximum angles allowed to meet the measurement accuracy requirements. It is determined by the feature type of the detected object.

和

P_A,w,i P_B,w,i以表面单位法矢

为轴旋转360°得到激光雷达的测量约束边界。The measurement constraint boundaries are represented by discrete generatrix vectors, where the cone apex angle θ _k = 2θ _i . According to the distance constraint L _min , L _max intercepts the line segment μ _{i, 0 on the busbar ζ i,0} , and the end point of the line segment μ _i,0 is

and

P _A,w,i P _B,w,i in surface units normal

Rotate 360° for the axis to get the lidar's measurement constraint bounds.

Step 5. Define the reachable space range constraint of the industrial robot arm.

得到工业机器人前三个关节形成的工作空间的方程W_i(P_i ^b){W₀(P_i ^b)、W₁(P_i ^b)、W₂(P_i ^b)}，其中The connecting rod coordinate system is established by the classical DH method, and the coordinate transformation matrix of the connecting rod coordinate system R _i relative to the connecting rod coordinate system R _i-1 is used

Obtain the equation Wi (P _i ^b ){W ₀ (P _i ^b ), W ₁ (P _i ^b ), W ₂ (P _i ^b )} of the workspace formed by the first three joints of the industrial robot _, where

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

c _θi =cos(θ _i ), s _θi =sin(θ _i ), c _αi =cos(α _i ),

s _αi =sin(α _i )

和

如果有

成立，则说明P_i ^b在工作空间W₀(P_i ^b)内部。W₀(P_i ^b)，W₁(P_i ^b)W₂(P_i ^b)的参数方程分别为：The working reachable area of the robot arm is determined by the working area of the first three joints. According to the structural parameters of the industrial robot, the joint variable θ _i satisfies θ _i ^min <θ _i <θ _i ^max , and the limit combination principle is adopted for the joint variables θ ₂ and θ ₃ , and the endpoint P _i of the wrist joint of the industrial robot can be obtained when θ ₁ =0 ^b is the workspace boundary in the robot coordinate system, and then according to the workspace boundary, the z-coordinates of the key points of the workspace W ₀ (P _i ^b ) are obtained. These key points are the maximum sum of the z-coordinates of the inner and outer boundaries of the workspace when θ ₁ =0 The smallest point and the z-coordinate of the point where the boundary expression changes, denoted Z ₁ , Z ₂ ......, Z ₇ . Then find the distance D _i from the wrist joint endpoint P _i ^b to the z-axis of the robot coordinate system, and the distance from the inner and outer boundaries of the workspace W ₀ (P _w ) at the z-coordinate of the corresponding P _i ^w to the z-axis of the robot coordinate system

and

If there is

If established, it means that P _i ^b is inside the workspace W ₀ (P _i ^b ). The parametric equations of W ₀ (P _i ^b ), W ₁ (P _i ^b ) and W ₂ (P _i ^b ) are:

where:

c ₁ =cos(θ ₁ ), c ₂ =cos(θ ₂ ), c ₃ =cos(θ ₃ );

s ₁ =sin(θ ₁ ), s ₂ =sin(θ ₂ ), s ₃ =sin(θ ₃ );

s ₂₃ =sin(θ ₂ +θ ₃ );

c ₂₃ =cos(θ ₂ +θ ₃ );

_d4 is the joint offset distance of the industrial robot link 4; _θ1 is the joint rotation angle of the industrial robot link 1; θ2 is the joint rotation angle of the industrial robot link ₂ ; θ3 is the joint rotation angle of the industrial robot link ₃ ; a1 is the length of the industrial robot connecting rod ₁ ; a2 is the length of the industrial robot connecting rod ₂ ; a3 is the length of the industrial robot connecting rod ₃ ;

The motion space boundary of the robot arm is represented by equations W _i (P _i ^b ){W ₀ (P _i ^b ), W ₁ (P _i ^b ), W ₂ (P _i ^b )}.

Step 6: Screen the balls that meet the conditions according to the constraints to generate a measurement reachable domain.

Using the lidar measurement constraints and the reachable space constraints of the industrial robot arm in steps 4 and 5, the set of small balls S _i ={q _j,k,l |j∈[1,J],k∈[1,K] , l∈[1,L],N ⁺ } to filter. Keep the balls that meet the constraints. S _i = {q _j,k,l |j∈[1,J],k∈[1,K],l∈[1,L],N ⁺ _} to obtain the measurement reachable domain Si '.

Step 7: Find the intersection of the measurement reachable domain and calculate the measurement station location.

The measurement reachable domains S _i ' of each measurement point are intersected. Take the intersection area Ti that contains the most types of spheres, and take the center of the intersection area Ti as the lidar measurement station Q _{w ,i .} The discrete spheres corresponding to the measurement points P _w,i that can be measured from the measuring station site Q _w,i are removed from the set of discrete spheres corresponding to all the measurement points. The above process is continued for the remaining measurement points until the lidar measurement station points Q _{w, i corresponding to all the measurement points P w,i} are generated.

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