Radar holder adaptive superhelix control method based on RMSE speed planning

Abstract

The invention discloses a radar holder adaptive superhelix control method based on RMSE speed planning, and belongs to the technical field of laser radar mapping and motion control. According to the method, the problem that the laser radar dynamic mapping effect is unstable due to the fact that the splicing effect of the point cloud data is not considered and the robustness is poor in an existing radar holder control method is solved. Firstly, feature points in two adjacent frames of point cloud data are roughly matched, then fine matching is completed based on generalized ICP, and RMSE is calculated based on a fine matching result; designing a rotating speed regulation rate according to a fitting equation of the rotating speed of the radar pan-tilt and the RMSE, and observing the disturbance suffered by the radar pan-tilt by using a super-spiral expansion state observer; and finally, designing a self-adaptive super-spiral controller based on a disturbance observation result and a rotating speed regulation rate based on RMSE, driving a radar holder to rotate by using the output of the controller, and continuously scanning to obtain point cloud data of a next frame until the whole scanning process is finished. The method can be applied to a laser radar dynamic mapping task in a complex large scene.

Description

Technical Field

[0001] The invention belongs to the technical field of laser radar mapping and motion control, and in particular relates to an adaptive super-helix control method of a radar pan-tilt based on RMSE velocity planning. Background Art

[0002] With the development of automation and intelligent technology, the application of LiDAR in industrial production and scientific research is becoming more and more extensive. In particular, the dynamic mapping technology based on LiDAR has become a key means of intelligent management and automated measurement of large scenes. In complex and large-scale application scenarios such as open-air coal bunkers, large farmlands and building facilities, if the LiDAR is to be able to comprehensively scan the terrain, materials and other scene information, it is inseparable from the help of a mobile platform. The LiDAR is installed on a mobile platform (such as a turntable, drone, robot, etc.), and the mobile platform can drive the LiDAR to perform dynamic scanning to collect three-dimensional point cloud data of the work site in real time, thereby realizing accurate measurement and three-dimensional reconstruction of complex and large-scale scenes. However, when actually performing dynamic mapping of large-scale scenes, the motion control process of the radar pan-tilt without speed planning does not match the point cloud splicing process, which can easily lead to instability of the dynamic mapping of the LiDAR. In addition, the disturbance and uncertainty factors of the external environment also require the radar pan-tilt control system to have good robustness and stability to ensure that the radar pan-tilt operates safely and stably in a dynamically changing environment. It can be seen that the stitching effect of the laser radar point cloud data directly affects the accuracy and completeness of dynamic mapping. It is necessary to adaptively adjust the control process of the radar gimbal according to the stitching effect of the point cloud data, which puts higher requirements on the motion control strategy of the radar gimbal.

[0003] Therefore, how to design a radar gimbal motion control strategy that takes into account the point cloud data stitching effect and has high robustness has become an urgent problem to be solved in the large-scene lidar dynamic mapping task. Summary of the invention

[0004] The purpose of the present invention is to solve the problem that the existing radar gimbal control method does not consider the splicing effect of point cloud data and has poor robustness, resulting in unstable laser radar dynamic mapping effect, and proposes a radar gimbal adaptive super-helix control method based on RMSE speed planning.

[0005] The technical solution adopted by the present invention to solve the above technical problems is: a radar pan-tilt adaptive super-helix control method based on RMSE velocity planning, the method specifically comprising the following steps:

[0006] Step 1: pre-process the k-th frame point cloud data obtained by laser radar scanning to obtain edge feature points and plane feature points in the k-th frame;

[0007] Step 2: After the radar pan-tilt rotates, the laser radar continues to scan to obtain the k+1th frame of point cloud data, and after preprocessing the k+1th frame of point cloud data, the edge feature points and plane feature points in the k+1th frame are obtained;

[0008] Step 3: perform rough matching on the feature points extracted in step 1 and step 2, then perform fine matching based on generalized ICP, and calculate RMSE based on the fine matching results;

[0009] Step 4: Establish a fitting equation according to the relationship between the radar gimbal rotation speed and RMSE, and design the speed adjustment rate of the radar gimbal based on the fitting equation and the RMSE calculated in step 3;

[0010] Step 5: Use the super-helical expansion state observer to observe the disturbances received during the rotation of the radar pan-tilt platform, and design an adaptive super-helical controller based on the disturbance observation results and the speed regulation rate based on RMSE;

[0011] The output of the adaptive super-helical controller is used to drive the radar pan-tilt platform to rotate, and k=k+1 is set, and the process returns to step 2.

[0012] Furthermore, the specific process of step one is:

[0013] Calculate the smoothness c of point i in the kth frame:

[0014]

[0015] Among them, X (k,i) is the 3D coordinate of point i in the laser radar coordinate system of the kth frame, X (k,j) is the 3D coordinate of point j in the laser radar coordinate system of the kth frame, ||·||2 is the L2 norm, S is the set of neighborhood points of point i, and |S| is the total number of points in set S;

[0016] If the smoothness c is greater than or equal to α1, then point i is an edge feature point;

[0017] If α2<c<α1, then point i is a plane feature point;

[0018] Similarly, the smoothness of each point in the k-th frame is calculated respectively to obtain the edge feature points and plane feature points in the k-th frame.

[0019] Furthermore, the total number of points |S| in the set S is:

[0020]

[0021] Where L is the arc length, h′ is the horizontal scanning resolution, and r is the scanning distance.

[0022] Furthermore, in step three, the feature points extracted in step one and step two are roughly matched, specifically:

[0023] Establish edge feature constraint error function model based on point-edge distance function ε , based on the point-plane distance function, a plane feature constraint error function model f is established h :

[0024]

[0025]

[0026] Among them, d ε is the point-edge distance function, d h is the point-plane distance function, is the pose conversion matrix between the k+1th frame and the kth frame;

[0027] For the mth edge feature point in the k+1th frame, according to Get the edge line corresponding to the mth edge feature point in the kth frame, record the two endpoints of the edge line as point n and point q respectively, and then calculate the distance from the mth edge feature point in the k+1th frame to the edge line in the kth frame, that is, express the point-edge feature function as:

[0028]

[0029] Among them, X (k+1,m) is the 3D coordinate of point m in the laser radar coordinate system of the k+1th frame, X (k,n) is the three-dimensional coordinate of edge feature point n in the laser radar coordinate system of the kth frame, X (k,q) is the three-dimensional coordinate of the edge feature point q in the laser radar coordinate system of the kth frame, |X (k,n) -X (k,q) | indicates X (k,n) With X (k,q) The Euclidean distance of

[0030] For the m′th plane feature point in the k+1th frame, according to Get the feature plane corresponding to the m′th plane feature point in the kth frame, record any three plane feature points on the feature plane as n′, q′ and r′ respectively, and then calculate the distance from the m′th plane feature point in the k+1th frame to the feature plane in the kth frame, that is, express the point-plane feature function as:

[0031]

[0032] Among them, X (k,n′) is the three-dimensional coordinate of the plane feature point n′ in the laser radar coordinate system of the kth frame, X(k,m′) is the three-dimensional coordinate of the plane feature point m′ in the laser radar coordinate system of the kth frame, X (k,q′) is the three-dimensional coordinate of the plane feature point q′ in the laser radar coordinate system of the kth frame, X (k,r′) is the three-dimensional coordinate of the plane feature point r′ in the laser radar coordinate system of the kth frame, X (k+1,m′) is the three-dimensional coordinate of point m′ in the laser radar coordinate system of the k+1th frame;

[0033] In order to obtain a rough matching matrix The optimization calculation is performed by the following formula:

[0034]

[0035] Among them, f ε is the set of all edge feature points in the kth frame, f h is the set of all plane feature points in the kth frame.

[0036] Furthermore, the fine matching is performed based on the rough matching result, specifically:

[0037] The matrix obtained by rough matching As the initial pose conversion matrix of the generalized ICP algorithm, the generalized ICP algorithm is used for precise matching, and the pose conversion matrix obtained by precise matching is calculated:

[0038]

[0039] Among them, p i is the i-th point in the k-th frame, q j is the jth point in the k+1th frame, n j for q j The unit normal vector at ij is a matching point pair (p i ,q j ), and the superscript T stands for transpose.

[0040] Furthermore, the root mean square error is calculated based on the precise matching result, specifically:

[0041]

[0042] Among them, d i is a pair of matching points (p i ,q j ) i and dot q j The distance between them, N is the total number of matching point pairs.

[0043] Furthermore, the specific process of step 4 is as follows:

[0044] Step 41: Establish the fitting equation from radar gimbal speed ω to RMSE:

[0045]

[0046] Among them, R(ω) is the fitting equation from angular velocity ω to RMSE, R t ,ω t , η1 and η2 are the parameters of the fitting equation;

[0047] Step 4.2: Set the fitting equation from speed ω to RMSE as:

[0048] R r (ω)=R rc (ω)+d R (11)

[0049] Among them, R r (ω) is the RMSE after considering the disturbance, and R rc The form of (ω) is the same as R(ω), d R is the disturbance part;

[0050] Step 4.3: Set expected RMSE t With the preset speed ω s , let R t RMSE t ,ω s ω t , substitute the RMSE calculated in step 3 into formula (11), and -1 (ω) Inversely solve the rotation speed

[0051] Step 44: According to Design speed regulation rate ω r :

[0052]

[0053] Among them, f(·) is the adjustment function, ω is the actual speed;

[0054] If the regulation function is taken as a PI controller, the speed regulation rate is:

[0055]

[0056] Among them, k ωp and k ωi are the parameters of the PI controller.

[0057] Furthermore, the specific process of step five is as follows:

[0058] Step 51: The system model of the radar gimbal is:

[0059]

[0060] Among them, θ is the actual rotation angle of the radar pan platform, is the first-order derivative of θ, is the first-order derivative of ω, d is the disturbance to the radar pan-tilt station, h is the rate of change of the disturbance to the radar pan-tilt station, is the first derivative of d, K and T are constants;

[0061] The super-helical expansion state observer of the system model is designed as:

[0062]

[0063] Among them, z1 is the observation value of θ by the superhelical expansion state observer, is the first-order derivative of z1, z2 is the observation value of ω by the superhelical expansion state observer, is the first-order derivative of z2, z3 is the observation value of d by the superhelical expansion state observer, is the first-order derivative of z3, α1, α2, α3, α4 and α5 are the coefficients of the superhelical expansion state observer, e1 = z1-θ m ,θ m is the measured radar pan tilt angle, sgn(·) is the sign function, ω m is the speed measurement value containing noise;

[0064] The observed value z2 is compared with the ω measured by the radar pan-tilt measuring element. m Perform weighted average filtering to obtain a smooth speed ω a :

[0065] ω a =wω m +(1-w)z2 (16)

[0066] Among them, w is the fusion coefficient;

[0067] Step 52: Based on the RMSE speed planning result and the smoothed speed, the speed error e of the radar gimbal is obtained as:

[0068] e=ω a -ω r (17)

[0069] Then, the output of the radar pan-tilt adaptive super-helical controller is designed as:

[0070]

[0071] Where u is the output of the controller, Yesr The first-order derivative of , ε1, ε2, ε3, ε4 and ε5 are the adjustable parameters of the controller, is u a The first derivative of k c is the adaptive parameter;

[0072]

[0073]

[0074] Furthermore, the coefficients α1, α2 and α3 satisfy:

[0075]

[0076] Among them, L h It is the upper bound of the rate of change h of the disturbance to which the radar pan-tilt station is subjected.

[0077] The beneficial effects of the present invention are:

[0078] (1) Establishing the fitting equation of RMSE and gimbal speed ω, the adaptive speed planning of the radar gimbal is realized

[0079] The present invention quantitatively fits the relationship between the actual radar pan-tilt speed ω and the RMSE of mapping evaluation, designs a piecewise, adjustable parameter fitting function, and completes the conversion between the radar pan-tilt speed ω and RMSE. On this basis, a speed planning scheme is designed to achieve the radar pan-tilt speed adjustment based on the specified RMSE index, so that the speed can achieve the purpose of optimizing the quality of point cloud splicing and ensure the effect of mapping.

[0080] (2) Design of a new super-helical expansion state observer to achieve fast and accurate estimation of the rotation speed and disturbance of the radar gimbal

[0081] The present invention designs a novel super-helical expansion state observer, which can overcome the adverse effects of complex dynamic environments on the measurement and control of control systems. The designed observer can quickly and accurately observe the current state quantity of the system. In an environment with complex conditions and high noise, the radar pan-tilt speed measurement data is generally seriously disturbed, and the super-helical expansion state observer can produce a smooth observation value for the system speed, and can perform weighted data fusion on the actual speed observation value of the system to produce a filtering effect, thereby facilitating the use of a more accurate and smooth angular velocity in the controller.

[0082] (3) Design of a new adaptive super-helical controller to achieve smooth and robust motion control of the radar gimbal

[0083] The present invention designs a novel adaptive superhelix controller. Compared with the traditional superhelix controller, the adaptive superhelix controller of the present invention supplements the linear term and improves the convergence speed of the error. The controller has an adaptive coefficient related to the rotational speed error, which can have a faster convergence speed when the error is large, and can also reduce the output jitter of the system when the error is small, so that the controller output is smoother, thereby improving the stitching quality of the lidar point cloud. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] Figure 1 When s =0.01rad / s, RMSE t =5, the speed comparison diagram of the control scheme of the present invention and the traditional control scheme;

[0085] Figure 2 When s =0.01rad / s, RMSE t =5, RMSE comparison diagram of the control scheme of the present invention and the traditional control scheme;

[0086] Figure 3 When s =1rad / s, RMSE t =5, the speed comparison diagram of the control scheme of the present invention and the traditional control scheme;

[0087] Figure 4 When s =1rad / s, RMSE t =5, RMSE comparison diagram of the control scheme of the present invention and the traditional control scheme;

[0088] Figure 5 It is in s =0.01rad / s, RMSE t =5, RMSE result diagram after precise matching of radar point cloud;

[0089] Figure 6 It is in s =0.01rad / s, RMSE t =5, the result diagram of the radar gimbal’s rotation speed;

[0090] Figure 7 It is in s =1rad / s, RMSE t =5, RMSE result diagram after precise matching of radar point cloud;

[0091] Figure 8 It is in s =1rad / s, RMSE t =5, the radar gimbal’s rotation speed result diagram. DETAILED DESCRIPTION

[0092] Specific implementation method 1: The radar gimbal adaptive super-spiral control method based on RMSE velocity planning described in this implementation method specifically includes the following steps:

[0093] Step 1: pre-process the k-th frame point cloud data obtained by laser radar scanning to obtain edge feature points and plane feature points in the k-th frame (i.e., divide the point cloud data into two parts: edge feature points and plane feature points);

[0094] Step 2: After the radar pan-tilt rotates, the laser radar continues to scan to obtain the k+1th frame of point cloud data, and after preprocessing the k+1th frame of point cloud data, the edge feature points and plane feature points in the k+1th frame are obtained;

[0095] Step 3: perform rough matching on the feature points extracted in step 1 and step 2, then perform fine matching based on generalized ICP (generalized iterative closest point), and calculate RMSE (root mean square error) based on the fine matching result;

[0096] Step 4: Establish a fitting equation according to the relationship between the radar gimbal rotation speed and RMSE, and design the speed adjustment rate of the radar gimbal based on the fitting equation and the RMSE calculated in step 3;

[0097] Step 5: Use the super-helical expansion state observer to observe the disturbances received during the rotation of the radar pan-tilt platform, and design an adaptive super-helical controller based on the disturbance observation results and the speed regulation rate based on RMSE;

[0098] The output of the adaptive super-helical controller is used to drive the radar pan-tilt platform to rotate, and k=k+1 is set, and the process returns to step 2.

[0099] After the radar gimbal is controlled by the control force output by the adaptive superhelix controller, the radar gimbal rotates and continues to acquire the next frame of point cloud data, i.e., the k+2th frame of point cloud data. The k+1th frame of point cloud data and the k+2th frame of point cloud data are preprocessed and the RMSE is calculated using the method in step one. The speed regulation rate is then designed based on the RMSE. The output force of the controller is obtained according to the observation results of the superhelix expansion state observer on the disturbance and the designed speed regulation rate. Finally, the radar gimbal is controlled based on the output force until the entire scanning process is completed.

[0100] Specific implementation method 2: This implementation method is different from the specific implementation method 1 in that the specific process of step 1 is as follows:

[0101] Calculate the smoothness c of point i in the kth frame:

[0102]

[0103] Among them, X (k,i) is the 3D coordinate of point i in the laser radar coordinate system of the kth frame, X (k,j) is the 3D coordinate of point j in the laser radar coordinate system of the kth frame, ||·||2 is the L2 norm, S is the set of neighboring points of point i (the number of neighboring points here is |S|, i.e., the |S| points closest to point i), and |S| is the total number of points in set S;

[0104] If the smoothness c is greater than or equal to α1, then point i is an edge feature point;

[0105] If α2<c<α1, then point i is a plane feature point;

[0106] Similarly, the smoothness of each point in the k-th frame is calculated respectively to obtain the edge feature points and plane feature points in the k-th frame.

[0107] The other steps and parameters are the same as those in the first embodiment.

[0108] This embodiment uses the smoothness index to extract edge feature points and plane feature points, which can reduce the amount of point cloud and improve the running speed. Preferably, the value of α1 is 1, and the value of α2 is 0.

[0109] Specific implementation method three: This implementation method is different from specific implementation methods one or two in that the total number of points |S| in the set S is:

[0110]

[0111] Where L is the arc length, h′ is the horizontal scanning resolution, and r is the scanning distance.

[0112] The other steps and parameters are the same as those in the first or second embodiment.

[0113] Specific implementation method 4: This implementation method is different from any one of the specific implementation methods 1 to 3 in that, in step 3, a rough matching is performed on the feature points extracted in step 1 and step 2, specifically:

[0114] Establish edge feature constraint error function model based on point-edge distance function ε , based on the point-plane distance function, a plane feature constraint error function model f is established h :

[0115]

[0116]

[0117] Among them, d ε is the point-edge distance function, d h is the point-plane distance function, is the pose conversion matrix between the k+1th frame and the kth frame;

[0118] For the mth edge feature point in the k+1th frame, according to Get the edge line corresponding to the mth edge feature point in the kth frame, record the two endpoints of the edge line as point n and point q respectively, and then calculate the distance from the mth edge feature point in the k+1th frame to the edge line in the kth frame, that is, express the point-edge feature function as:

[0119]

[0120] Among them, X (k+1,m) is the 3D coordinate of point m in the laser radar coordinate system of the k+1th frame, X (k,n) is the three-dimensional coordinate of edge feature point n in the laser radar coordinate system of the kth frame, X (k,q) is the three-dimensional coordinate of the edge feature point q in the laser radar coordinate system of the kth frame, |X (k,n) -X (k,q) | indicates X (k,n) With X (k,q) The Euclidean distance of

[0121] For the m′th plane feature point in the k+1th frame, according to Get the feature plane corresponding to the m′th plane feature point in the kth frame, record any three plane feature points on the feature plane as n′, q′ and r′ respectively, and then calculate the distance from the m′th plane feature point in the k+1th frame to the feature plane in the kth frame, that is, express the point-plane feature function as:

[0122]

[0123] Among them, X (k,n′) is the three-dimensional coordinate of the plane feature point n′ in the laser radar coordinate system of the kth frame, X (k,m′) is the three-dimensional coordinate of the plane feature point m′ in the laser radar coordinate system of the kth frame, X (k,q′) is the three-dimensional coordinate of the plane feature point q′ in the laser radar coordinate system of the kth frame, X (k,r′) is the three-dimensional coordinate of the plane feature point r′ in the laser radar coordinate system of the kth frame, X (k+1,m′) is the three-dimensional coordinate of point m′ in the laser radar coordinate system of the k+1th frame;

[0124] In order to obtain a rough matching matrix The optimization calculation is performed by the following formula:

[0125]

[0126] Among them, f εis the set of all edge feature points in the kth frame, f h is the set of all plane feature points in the kth frame.

[0127] The other steps and parameters are the same as those in Specific Embodiments 1 to 3.

[0128] It should be noted that when a certain edge feature point has no corresponding edge line in the kth frame (that is, there is no matching point in the kth frame), is 0. Similarly, when a plane feature point has no corresponding feature plane in the kth frame (that is, there is no matching point in the kth frame), is 0.

[0129] Specific implementation method 5: This implementation method is different from the specific implementation methods 1 to 4 in that the precise matching is performed based on the generalized ICP, specifically:

[0130] The matrix obtained by rough matching As the initial pose conversion matrix of the generalized ICP algorithm, the generalized ICP algorithm is used for precise matching, and the pose conversion matrix obtained by precise matching is calculated:

[0131]

[0132] Among them, p i is the i-th point in the k-th frame, q j is the jth point in the k+1th frame, n j for q j The unit normal vector at ij is a matching point pair (p i ,q j ), and the superscript T stands for transpose.

[0133] The other steps and parameters are the same as those in Specific Embodiments 1 to 4.

[0134] Specific implementation method 6: This implementation method is different from any one of the specific implementation methods 1 to 5 in that the RMSE is calculated based on the precise matching result, specifically:

[0135]

[0136] Among them, d i is a pair of matching points (p i ,q j ) i and dot q j The distance between them, N is the total number of matching point pairs.

[0137] The other steps and parameters are the same as those in Specific Implementation Methods 1 to 5.

[0138] Specific implementation method 7: This implementation method is different from the specific implementation methods 1 to 6 in that the specific process of step 4 is as follows:

[0139] Step 41: Establish the fitting equation from radar gimbal speed ω to RMSE:

[0140]

[0141] Among them, R(ω) is the fitting equation from angular velocity ω to RMSE, R t ,ω t , η1 and η2 are adjustable parameters of the fitting equation;

[0142] In actual operation, the fitting equation parameters need to be appropriately adjusted according to the working characteristics of the radar gimbal. Based on the fitting equation, the fitting relationship between the radar gimbal rotation speed and the RMSE index can be obtained, which facilitates the rotation speed adjustment.

[0143] Step 4.2: Set the fitting equation from the speed value ω to RMSE as:

[0144] R r (ω)=R rc (ω)+d R (11)

[0145] Among them, R r (ω) is the RMSE after considering the disturbance, which is the modelable part, and R rc The form of (ω) is the same as R(ω), but the specific parameters are slightly different. R It is the disturbance part and the part that cannot be modeled. The combination of the two produces the actual RMSE index;

[0146] Step 4.3: Set expected RMSE t With the preset speed ω s , let R t RMSE t ,ω s ω t , so as to obtain faster rotation speed and better point cloud stitching quality. Substitute the RMSE calculated in step 3 into formula (11) and -1 (ω) Inversely solve the rotation speed

[0147] Step 44: According to Design speed regulation rate ω r :

[0148]

[0149] Among them, f(·) is the adjustment function, ω is the actual speed;

[0150] If the regulation function is taken as a PI controller, the speed regulation rate is:

[0151]

[0152] Among them, k ωp and k ωi is the adjustable parameter of the PI controller.

[0153] The other steps and parameters are the same as those in Specific Embodiments 1 to 6.

[0154] The present invention uses a super-helical expanded state observer to observe the angle, rotation speed and disturbance of the radar pan-tilt during its rotation, and compensates and eliminates the observed disturbance, which can reduce the negative impact of dust and the like on angle measurement and improve control robustness and tracking performance.

[0155] Specific implementation eight: This implementation differs from specific implementations one to seven in that the specific process of step five is as follows:

[0156] Step 5.1: The system model of the radar gimbal is:

[0157]

[0158] Among them, θ is the actual rotation angle of the radar pan platform, is the first-order derivative of θ, is the first-order derivative of ω, d is the disturbance to the radar pan-tilt station, h is the rate of change of the disturbance to the radar pan-tilt station, is the first derivative of d, K and T are constants;

[0159] Generally, |h|<L h , the super-helical expansion state observer of the system model is designed as:

[0160]

[0161] Among them, z1 is the observation value of θ by the superhelical expansion state observer, is the first-order derivative of z1, z2 is the observation value of ω by the superhelical expansion state observer, is the first-order derivative of z2, z3 is the observation value of d by the superhelical expansion state observer, is the first-order derivative of z3, α1, α2, α3, α4 and α5 are the coefficients of the superhelical expansion state observer, e1 = z1-θ m ,θ m is the measured radar pan tilt angle, sgn(·) is the sign function, ω m is the speed measurement value containing noise;

[0162] The observed value z2 is compared with the ω measured by the radar pan-tilt measuring element. m Perform weighted average filtering to obtain a smooth speed ω a :

[0163] ω a =wω m +(1-w)z2 (16)

[0164] Among them, w is the fusion coefficient;

[0165] Step 52: Based on the RMSE speed planning result and the smoothed speed, the speed error e of the radar gimbal is obtained as:

[0166] e=ω a -ω r (17)

[0167] Then, the output of the radar pan-tilt adaptive super-helical controller is designed as:

[0168]

[0169] Where u is the output of the controller, Yes r The first-order derivative of , ε1, ε2, ε3, ε4 and ε5 are the adjustable parameters of the controller, is u a The first derivative of k c is the adaptive parameter;

[0170]

[0171]

[0172] The other steps and parameters are the same as those in Specific Embodiments 1 to 7.

[0173] Specific implementation method 9: This implementation method is different from any one of specific implementation methods 1 to 8 in that the coefficients α1, α2 and α3 satisfy:

[0174]

[0175] Among them, L h It is the upper bound of the rate of change h of the disturbance to which the radar pan-tilt station is subjected.

[0176] The other steps and parameters are the same as those in Specific Embodiments 1 to 8.

[0177] According to this embodiment, the values ​​of coefficients α1, α2 and α3 are selected to achieve observer convergence.

[0178] Experimental Section

[0179] In order to verify the effectiveness of the radar pan-tilt rotation control scheme based on the evaluation index RMSE of the present invention, a radar pan-tilt control simulation experiment was carried out on a simulation platform with the radar pan-tilt scanning mapping as the background, demonstrating the effectiveness of the control scheme in maintaining good mapping and the robustness of the control performance.

[0180] In this context, the system parameters of the radar pan-tilt are set to K = 3.2, T = 0.53, the initial rotation angle of the radar pan-tilt is set to θ = 0, the initial rotation speed is ω = 0, and the preset rotation speeds are taken as ω s =0.1rad / s and ω s =1rad / s, to represent the preset speed being too fast and too slow respectively. The parameters of the adaptive superhelix controller are ε1=10, ε2=10, ε3=0.5, ε4=1.5, ε5=1. The parameters of the superhelix expansion state observer are α1=8.8, α2=45.2, α3=110, α4=100, α5=5. The parameter of data fusion is w=0.2.

[0181] The parameters of the fitting function are: R t =1,v t =0.05, η1=20, η2=200. The radar speed and point cloud splicing index RMSE will be subject to many external interferences. In actual operation, R(ω) cannot accurately reflect this corresponding relationship. In order to verify the robustness of the radar pan-tilt motion control scheme of the present invention, the corresponding relationship R of actual operation is designed. r (ω)=R rc (ω)+d R , where R rc (ω) parameter is chosen as R t =1,v t =0.1, eta1=10, eta2=100. d R The white noise is superimposed on the sinusoidal signal disturbance, and d R It is proportional to the speed. f(·) selects PI controller, and the parameter is k ωp =0.25, k ωi =1.

[0182] After completing the parameter selection, the preset speed of the radar pan / tilt is taken as ω s =0.01rad / s and ω s =1rad / s for the experiment, and the expected RMSE is set t =5. The control scheme of the present invention adopts an adaptive superhelical control strategy based on a superhelical expansion state observer, and selects a PID control strategy based on a linear expansion state observer as a traditional control scheme. The PID parameter is selected as k p =10, k i= 10, and the disturbance observation bandwidth of the linear extended state observer is set to 100 rad / s. The motion control and speed planning effects of the radar gimbal under different control schemes are shown in the figure. Figures 1 to 8 As shown. Figures 1 to 4 It can be seen that compared with the traditional control scheme, the control scheme of the present invention has a smoother control process and stronger anti-interference ability, and can robustly and smoothly realize the speed control process of the radar pan-tilt platform. Figures 5 to 8 It can be seen that the speed planning scheme of the present invention can be based on the expected RMSE t The rotation speed of the radar gimbal is adjusted to achieve adaptive, high-quality dynamic splicing of the LiDAR point cloud. The specific effect is that when the initial rotation speed of the radar gimbal is low, the solution can adaptively increase the rotation speed and mapping efficiency of the radar gimbal while ensuring the LiDAR mapping effect; and when the initial rotation speed of the radar gimbal is too fast, the solution can adaptively reduce the rotation speed of the radar gimbal to ensure the dynamic splicing quality of the LiDAR point cloud.

[0183] The above calculation examples of the present invention are only used to explain the calculation model and calculation process of the present invention in detail, and are not intended to limit the implementation methods of the present invention. For ordinary technicians in the relevant field, other different forms of changes or modifications can be made based on the above description. It is impossible to list all the implementation methods here. All obvious changes or modifications derived from the technical solution of the present invention are still within the scope of protection of the present invention.

Claims

1. A radar pan-tilt adaptive super-helix control method based on RMSE velocity planning, characterized in that: The method specifically comprises the following steps: Step 1: pre-process the k-th frame point cloud data obtained by laser radar scanning to obtain edge feature points and plane feature points in the k-th frame; Step 2: After the radar platform rotates, the laser radar continues to scan to obtain the k+1th frame of point cloud data, and after preprocessing the k+1th frame of point cloud data, the edge feature points and plane feature points in the k+1th frame are obtained; Step 3: perform rough matching on the feature points extracted in step 1 and step 2, then perform fine matching based on generalized ICP, and calculate RMSE based on the fine matching results; Step 4: Establish a fitting equation according to the relationship between the radar gimbal rotation speed and RMSE, and design the speed adjustment rate of the radar gimbal based on the fitting equation and the RMSE calculated in step 3; Step 5: Use the super-helical expansion state observer to observe the disturbances received during the rotation of the radar pan-tilt platform, and design an adaptive super-helical controller based on the disturbance observation results and the speed regulation rate based on RMSE; The output of the adaptive super-helical controller is used to drive the radar pan-tilt platform to rotate, and k=k+1 is set, and the process returns to step 2.

2. According to claim 1, a radar pan-tilt adaptive super-helix control method based on RMSE velocity planning is characterized in that: The specific process of step one is: Calculate the smoothness c of point i in the kth frame: Among them, X (k,i) is the three-dimensional coordinate of point i in the laser radar coordinate system of the kth frame, X (k,j) is the 3D coordinate of point j in the laser radar coordinate system of the kth frame, ||·||2 is the L2 norm, S is the set of neighborhood points of point i, and |S| is the total number of points in set S; If the smoothness c is greater than or equal to α1, then point i is an edge feature point; If α2<c<α1, then point i is a plane feature point; Similarly, the smoothness of each point in the k-th frame is calculated respectively to obtain the edge feature points and plane feature points in the k-th frame.

3. The radar pan-tilt adaptive super-helix control method based on RMSE velocity planning according to claim 2 is characterized in that: The total number of points |S| in the set S is: Where L is the arc length, h′ is the horizontal scanning resolution, and r is the scanning distance.

4. The radar pan-tilt adaptive super-helix control method based on RMSE velocity planning according to claim 3 is characterized in that: In the step 3, the feature points extracted in the step 1 and the step 2 are roughly matched, specifically: Based on the point-edge distance function, the edge feature constraint error function model f is established ε , based on the point-plane distance function, a plane feature constraint error function model f is established h : Among them, d ε is the point-edge distance function, d h is the point-plane distance function, is the pose conversion matrix between the k+1th frame and the kth frame; For the mth edge feature point in the k+1th frame, according to Get the edge line corresponding to the mth edge feature point in the kth frame, record the two endpoints of the edge line as point n and point q respectively, and then calculate the distance from the mth edge feature point in the k+1th frame to the edge line in the kth frame, that is, express the point-edge feature function as: Among them, X (k+1,m) is the 3D coordinate of point m in the laser radar coordinate system of the k+1th frame, X (k,n) is the three-dimensional coordinate of edge feature point n in the laser radar coordinate system of the kth frame, X (k,q) is the three-dimensional coordinate of the edge feature point q in the laser radar coordinate system of the kth frame, |X (k,n) -X (k,q) | indicates X (k,n) With X (k,q) The Euclidean distance of For the m′th plane feature point in the k+1th frame, according to Get the feature plane corresponding to the m′th plane feature point in the kth frame, record any three plane feature points on the feature plane as n′, q′ and r′ respectively, and then calculate the distance from the m′th plane feature point in the k+1th frame to the feature plane in the kth frame, that is, express the point-plane feature function as: Among them, X (k,n′) is the three-dimensional coordinate of the plane feature point n′ in the laser radar coordinate system of the kth frame, X (k,m′) is the three-dimensional coordinate of the plane feature point m′ in the laser radar coordinate system of the kth frame, X (k,q′) is the three-dimensional coordinate of the plane feature point q′ in the laser radar coordinate system of the kth frame, X (k,r′) is the three-dimensional coordinate of the plane feature point r′ in the laser radar coordinate system of the kth frame, X (k+1,m′) is the three-dimensional coordinate of point m′ in the laser radar coordinate system of the k+1th frame; In order to obtain a rough matching matrix The optimization calculation is performed by the following formula: Among them, f ε is the set of all edge feature points in the kth frame, f h is the set of all plane feature points in the kth frame.

5. The radar pan-tilt adaptive super-helix control method based on RMSE velocity planning according to claim 4 is characterized in that: The precise matching based on the rough matching result is specifically as follows: The matrix obtained by rough matching As the initial pose conversion matrix of the GICP algorithm, the pose conversion matrix obtained by precise matching is calculated based on the GICP algorithm: Among them, p i is the i-th point in the k-th frame, q j is the jth point in the k+1th frame, n j Q j The unit normal vector at ij is a matching point pair (p i ,q j ), and the superscript T stands for transpose.

6. The radar pan-tilt adaptive super-helix control method based on RMSE velocity planning according to claim 5, characterized in that: The root mean square error is calculated based on the precise matching result, specifically: Among them, d i is a pair of matching points (p i ,q j ) i and dot q j The distance between them, N is the total number of matching point pairs.

7. The radar pan-tilt adaptive super-helix control method based on RMSE velocity planning according to claim 6, characterized in that: The specific process of step 4 is as follows: Step 41: Establish the fitting equation from radar gimbal speed ω to RMSE: Among them, R(ω) is the fitting equation from angular velocity ω to RMSE, R t ,ω t , η1 and η2 are the parameters of the fitting equation; Step 4.2: Set the fitting equation from the speed value ω to RMSE as: R r (ω)=R rc (ω)+d R (11) Among them, R r (ω) is the RMSE after considering the disturbance, and R rc The form of (ω) is the same as R(ω), d R is the disturbance part; Step 4.3: Set expected RMSE t With the preset speed ω s , let R t RMSE t ,ω s ω t , substitute the RMSE calculated in step 3 into formula (11), and -1 (ω) Inversely solve the rotation speed value Step 44: According to Design speed regulation rate ω r : Among them, f(·) is the adjustment function, ω is the actual speed; If the regulation function is taken as a PI controller, the speed regulation rate is: Among them, k ωp and k ωi are the parameters of the PI controller.

8. The radar pan-tilt adaptive super-helix control method based on RMSE velocity planning according to claim 7 is characterized in that: The specific process of step five is as follows: Step 51: The system model of the radar platform is: Among them, θ is the actual rotation angle of the radar platform, is the first-order derivative of θ, is the first-order derivative of ω, d is the disturbance to the radar pan-tilt station, h is the rate of change of the disturbance to the radar pan-tilt station, is the first derivative of d, K and T are constants; The super-helical expansion state observer of the system model is designed as: Among them, z1 is the observation value of θ by the superhelical expansion state observer, is the first-order derivative of z1, z2 is the observation value of ω by the superhelical expansion state observer, is the first-order derivative of z2, z3 is the observation value of d by the superhelical expansion state observer, is the first-order derivative of z3, α1, α2, α3, α4 and α5 are the coefficients of the superhelical expansion state observer, e1 = z1-θ m ,θ m is the measured radar pan tilt angle, sgn(·) is the sign function, ω m is the speed measurement value including noise; The observed value z2 is compared with the ω measured by the radar pan-tilt measuring element. m Perform weighted average filtering to obtain a smooth speed ω a : oh a =wω m +(1-w)z2 (16) Among them, w is the fusion coefficient; Step 52: Based on the RMSE speed planning result and the smoothed speed, the speed error e of the radar gimbal is obtained as: e=ω a -oh r (17) Then, the output of the radar pan-tilt adaptive super-helical controller is designed as: Where u is the output of the controller, Yes r The first-order derivative of , ε1, ε2, ε3, ε4 and ε5 are the adjustable parameters of the controller, is u a The first derivative of k c is the adaptive parameter; 9. The radar pan-tilt adaptive super-helix control method based on RMSE velocity planning according to claim 8, characterized in that: The coefficients α1, α2 and α3 satisfy: Among them, L h It is the upper bound of the rate of change h of the disturbance to which the radar pan-tilt station is subjected.

Images