A head-mounted lidar point cloud matching method and device

Abstract

The present application relates to mobile mapping and wearable computing technology field, disclose a kind of head-mounted laser radar point cloud matching method, device, including receiving original laser point cloud data stream and inertial measurement data;Read human cervical kinematic chain parameter;Continuous time trajectory model is established using cumulative B spline function;According to current trajectory state, human cervical joint configuration vector is solved inversely;Calculate analytical geometry jacobian matrix;Analytical geometry jacobian matrix is used to construct manifold tangent space projection operator;Perform joint constraint optimization based on tangent space projection, construct joint constraint jacobian matrix using manifold tangent space projection operator and solve incremental equation, and B spline control point is iteratively updated.The present application is strictly limited in human cervical kinematic manifold by using manifold tangent space projection operator to update trajectory, effectively filter the motion noise that violates physiological structure, improve the physical consistency and precision of pose solution in complex motion scene.

Description

Technical Field

[0001] This invention relates to the fields of mobile surveying and wearable computing technology, specifically to a head-mounted lidar point cloud matching method and device. Background Technology

[0002] Head-mounted LiDAR mobile measurement systems are widely used in 3D spatial data acquisition and augmented reality positioning and navigation tasks in complex environments. Traditional point cloud matching and simultaneous localization and mapping algorithms typically assume that the sensor platform is a free rigid body with full six degrees of freedom. This assumption ignores the inherent kinematic constraints imposed on head movements by the wearer's cervical spine biomechanical structure. In scenarios involving rapid turns, vigorous running, and complex tactical maneuvers, trajectory estimation lacking physiological kinematic constraints is prone to converge to erroneous states that violate human anatomical limits, leading to ghosting, layering, and non-physical distortions in the point cloud map.

[0003] Existing continuous-time trajectory parameterization methods generally employ a fixed-time B-spline control point distribution strategy when modeling lidar motion. However, human head motion exhibits strong non-stationarity and nonlinear characteristics. Especially when the cervical spine joint approaches its physiological activity limit angle, the micro-amplitude high-frequency tremors caused by muscle antagonism and abrupt changes in motion stiffness can disrupt motion smoothness. A fixed-density control point setup cannot achieve a balance between low-frequency smooth motion and high-frequency extreme dynamics, leading to fitting distortion and accuracy loss in the trajectory model during moments of intense motion.

[0004] Furthermore, in scenarios such as long, straight corridors, tunnels, and open plazas, the sparse effective geometric features of the external environment often lead to degradation in the LiDAR point cloud registration process. Existing matching algorithms rely excessively on the extraction and correlation of external environmental features, failing to effectively utilize the occlusion information formed by the wearer's own torso on the LiDAR scanning data. In the absence of external feature textures, ignoring the wearer's own occlusion data causes the system to lose a stable and dynamic near-field geometric constraint reference, easily resulting in difficult-to-correct cumulative drift and attitude divergence of the positioning trajectory over long-term operation. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a head-mounted lidar point cloud matching method and device, which solves the problems of existing technologies ignoring the biomechanical constraints of the human cervical spine, resulting in trajectory estimation violating physiological limits; the inability of fixed control point density to adapt to the nonlinear high-frequency motion characteristics of the human body; and the failure to utilize body occlusion information when external environmental features are sparse, leading to positioning drift and divergence.

[0006] To achieve the above objectives, the present invention provides the following technical solution: a head-mounted lidar point cloud matching method and apparatus.

[0007] The first aspect of this invention provides a point cloud matching method for a head-mounted lidar system, the method comprising:

[0008] The system receives the raw laser point cloud data stream generated by the lidar acquisition module and the inertial measurement data output by the inertial measurement module, and reads the preset human cervical spine kinematic chain parameters from the memory; it establishes a continuous-time trajectory model using the cumulative B-spline function, parameterizing the lidar's motion trajectory into a series of B-spline control points; it inversely solves the human cervical spine joint configuration vector based on the current trajectory state, and calculates the analytic geometric Jacobian matrix describing the mapping relationship between the cervical spine joint angular velocity space and the radar end Cartesian velocity space; and it performs joint constraint optimization based on tangent space projection.

[0009] The joint constraint optimization specifically includes: constructing a manifold tangent space projection operator using an analytic geometric Jacobian matrix, configured to project the tangent vector of the Lie group space onto the tangent space of the cervical spine kinematic manifold; constructing a joint constraint Jacobian matrix containing physiological motion constraints using the manifold tangent space projection operator; constructing and solving incremental equations based on the joint constraint Jacobian matrix, iteratively updating the B-spline control points until the geometric residuals of the point cloud matching converge; and outputting the optimized continuous-time trajectory and the distortion-free point cloud data.

[0010] In a preferred embodiment, the process of establishing a continuous time trajectory model using cumulative B-spline functions includes: discretizing the continuous motion trajectory of the lidar into a series of time nodes, defining a B-spline control point in the Lie group space at each time node; mapping the global timestamp to normalized time parameters; determining a subset of control points participating in the pose calculation at the current moment based on the lidar's sampling frequency; performing nonlinear interpolation on the selected subset of control points using cumulative B-spline basis functions, and mapping the interpolation results back to the Lie group space through cascaded exponential mappings to obtain the lidar pose in the world coordinate system at any moment.

[0011] In a preferred embodiment, the inverse solution of the human cervical spine joint configuration vector is based on the human cervical spine kinematic chain parameters, which define the series rigid body transformation relationship between the human torso coordinate system and the radar body coordinate system. This series rigid body transformation relationship simplifies the cervical spine into a mechanical structure composed of multiple rigid links connected by rotational joints. The human cervical spine joint configuration vector includes cervical flexion-extension angle components, cervical axial rotation angle components, and cervical lateral flexion angle components. The mapping relationship from the joint configuration space to the Cartesian pose space is described by forward kinematic equations, and a linear differential relationship between the joint angular velocity vector and the radar terminal rotation spinor is established by differentiating the forward kinematic equations with respect to time. Each column vector of the analytic geometric Jacobian matrix is ​​calculated, and each column vector represents the spatial projection of the corresponding joint axis under the current configuration.

[0012] In a preferred embodiment, the joint constraint optimization based on tangent space projection further includes distortion correction and residual construction steps during the iterative process: calculating the radar pose corresponding to each laser point in the original laser point cloud data stream using the current B-spline control points, transforming the laser points to the world coordinate system to complete distortion correction; searching for the nearest neighbor set of the distorted laser points in the reference map and fitting a local plane; calculating the distance from the distorted laser points to the local plane to obtain the geometric residual; calculating the measurement Jacobian matrix of the geometric residual relative to the Lie algebraic perturbation of the radar pose, and calculating the spline Jacobian matrix of the Lie algebraic perturbation of the radar pose relative to the perturbation of the B-spline control points.

[0013] In a preferred embodiment, the process of constructing a manifold tangent space projection operator using an analytic geometric Jacobian matrix includes: calculating the product of the transpose and the analytic geometric Jacobian matrix to obtain an intermediate matrix; calculating the inverse of the intermediate matrix; and sequentially calculating the product of the analytic geometric Jacobian matrix, its inverse, and its transpose to obtain the manifold tangent space projection operator. This operator is used to filter out motion components that do not conform to the physiological structure of the human cervical spine.

[0014] In a preferred embodiment, the process of constructing and solving the incremental equation based on the joint constraint Jacobian matrix includes: embedding the manifold tangent space projection operator into the gradient transfer path; sequentially multiplying the measurement Jacobian matrix, the manifold tangent space projection operator, and the spline Jacobian matrix to obtain the joint constraint Jacobian matrix; accumulating the joint constraint Jacobian matrix and geometric residuals of all effective laser points to construct a linear incremental equation system; and solving the linear incremental equation system to obtain the update increment of the B-spline control points.

[0015] In a preferred embodiment, the method further includes an adaptive node allocation step based on physiological limits: reading the zero-position angle and maximum range of motion parameters of each joint of the cervical spine; calculating the physiological proximity index of the human cervical spine joint configuration vector at the current moment relative to the physiological limit boundary; dynamically adjusting the time interval of B-spline control points in the continuous time trajectory model according to the physiological proximity index; reducing the time interval to increase the control point density when the physiological proximity index approaches the physiological limit boundary, thereby enabling more accurate capture of high-frequency tremors or stiff movements.

[0016] In a preferred embodiment, the method further includes a body occlusion auxiliary constraint step: reading preset virtual torso geometric model parameters; calculating the relative pose of the lidar relative to the human torso based on the current trajectory state; filtering out the body occlusion point set from the original laser point cloud data stream that falls into the projection area of ​​the virtual torso geometric model and whose depth value meets the preset range; calculating the distance deviation between each laser point in the body occlusion point set and the virtual torso geometric model after transformation to the human torso coordinate system; constructing the body geometric constraint residual and adding it to the objective function of joint constraint optimization, using the wearer's own geometric characteristics to limit trajectory drift.

[0017] A second aspect of the present invention provides a head-mounted lidar point cloud matching device, the device comprising:

[0018] The head-mounted device has a rigid frame structure that conforms to the contours of the human head.

[0019] The lidar acquisition module is rigidly fixed to the head-mounted carrier and configured to generate raw lidar point cloud data streams.

[0020] An inertial measurement module is rigidly fixed to a head-mounted device and configured to output inertial measurement data.

[0021] The memory is configured to store human cervical spine kinematic chain parameters; the data processing unit is electrically connected to the lidar acquisition module, the inertial measurement module and the memory.

[0022] The data processing unit is configured to receive raw laser point cloud data streams and inertial measurement data, and read human cervical spine kinematic chain parameters from the memory;

[0023] The data processing unit is configured to use the cumulative B-spline function to establish a continuous time trajectory model, and parameterize the motion trajectory of the lidar acquisition module into multiple B-spline control points.

[0024] The data processing unit is configured to inversely solve the human cervical spine joint configuration vector based on the trajectory state at the current moment, and calculate the analytic geometric Jacobian matrix describing the mapping relationship between the cervical spine joint angular velocity space and the radar terminal Cartesian velocity space.

[0025] The data processing unit is configured to perform joint constraint optimization operations based on tangent space projection;

[0026] In the joint constraint optimization operation, the manifold tangent space projection operator is constructed using the analytical geometric Jacobian matrix. The joint constraint Jacobian matrix containing physiological motion constraints is constructed using the manifold tangent space projection operator. The incremental equation is constructed and solved based on the joint constraint Jacobian matrix. The B-spline control points are iteratively updated until the geometric residuals converge.

[0027] The data processing unit is configured to output optimized continuous time trajectories and distortion-free point cloud data.

[0028] This invention provides a method and apparatus for point cloud matching using a head-mounted lidar system. It offers the following advantages:

[0029] 1. This invention constructs a manifold tangent space projection operator using the analytical geometric Jacobian matrix and embeds it into the joint constraint optimization process based on cumulative B-splines. This forces the trajectory update increment to be strictly located within the tangent space of the cervical spine kinematic manifold. By utilizing the inherent physiological kinematic structure of the human body to restrict and filter out noise interference and erroneous estimations that do not conform to biomechanical characteristics, the physical consistency and robustness of pose calculation of head-mounted devices in fast motion and feature degradation scenarios are improved.

[0030] 2. This invention calculates the physiological proximity index of the human cervical spine joint configuration vector relative to the physiological limit boundary, and adaptively adjusts the time interval of B-spline control points according to the physiological proximity index. When approaching the physiological activity limit, the control point density is automatically increased, thereby accurately fitting the high-frequency micro-motion and nonlinear stiff motion generated by the human body under extreme postures. While ensuring the accuracy of trajectory fitting, it optimizes the allocation of computing resources.

[0031] 3. This invention filters the set of body occlusion points in the original laser point cloud data stream that are occluded by the wearer's own torso, establishes a direct geometric relationship between the body occlusion point set and a preset virtual torso geometric model, constructs a body geometric constraint residual independent of external environmental features and participates in the optimization solution, thereby providing a stable self-reference constraint when the external scene texture is sparse and the geometric features are insufficient, effectively suppressing the pose drift accumulated by the lidar odometer during long-term operation. Attached Figure Description

[0032] Figure 1 This is a schematic diagram of the head-mounted lidar point cloud matching device of the present invention;

[0033] Figure 2 This is a block diagram of the logical function modules of the head-mounted lidar point cloud matching device of the present invention;

[0034] Figure 3 This is a schematic diagram of the head-mounted lidar point cloud matching method of the present invention;

[0035] Figure 4 This is a schematic diagram of the joint constraint optimization process for tangent space projection according to the present invention.

[0036] Among them, 100 is the head-mounted device; 110 is the lidar acquisition module; 120 is the inertial measurement module; 200 is the data processing unit; 200a is the data preprocessing module; 200b is the kinematic manifold construction module; 200c is the trajectory parameterization module; 200d is the constraint optimization solution module; 200f is the body occlusion auxiliary constraint module; 201 is the trajectory parameterization module; 202 is the kinematic constraint calculation module; 203 is the optimization solution module; and 210 is the memory. Detailed Implementation

[0037] The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0038] See attached document Figure 1 , Figure 1 This is a schematic diagram of a head-mounted lidar point cloud matching device according to an embodiment of the present invention. The head-mounted lidar point cloud matching device provided by the present invention may include: a head-mounted carrier 100, a lidar acquisition module 110, an inertial measurement module 120, a data processing unit 200, and a memory 210.

[0039] The head-mounted device 100 has a rigid frame structure. This rigid frame structure is configured to fit the contours of the human head and be securely worn. The rigid frame structure has a first mounting position and a second mounting position. The first mounting position is used to rigidly fix the lidar acquisition module 110. The second mounting position is used to rigidly fix the inertial measurement module 120. The head-mounted device 100 ensures a fixed relative positional relationship between the lidar acquisition module 110 and the inertial measurement module 120.

[0040] The lidar acquisition module 110 is configured to emit a laser beam into the surrounding environment and receive echo signals. The lidar acquisition module 110 generates a raw laser point cloud data stream containing three-dimensional coordinate information and reflection intensity information. The lidar acquisition module 110 establishes the radar's body coordinate system. The lidar acquisition module 110 integrates a time synchronization circuit. This circuit assigns a timestamp with microsecond-level precision to each generated laser point data point. .

[0041] The inertial measurement module 120 is configured to measure the triaxial angular velocity and triaxial acceleration of the head-mounted carrier 100. The inertial measurement module 120 establishes an inertial coordinate system. The inertial measurement module 120 and the lidar acquisition module 110 have a pre-calibrated rigid extrinsic parameter transformation matrix. The inertial measurement module 120 outputs high-frequency inertial measurement data.

[0042] The data processing unit 200 is electrically connected to the lidar acquisition module 110 and the inertial measurement module 120 via a data bus. The data processing unit 200 is configured to receive raw lidar point cloud data streams and inertial measurement data. The data processing unit 200 performs point cloud distortion correction and pose calculation tasks. The memory 210 is bidirectionally connected to the data processing unit 200. The memory 210 is configured to store preset human cervical spine kinematic chain parameters and B-spline basis function parameters.

[0043] The data processing unit 200 includes a trajectory parameterization module 201. The trajectory parameterization module 201 is configured to model the motion trajectory of the lidar acquisition module 110 in the continuous time domain using a cumulative B-spline function. The trajectory parameterization module 201 represents the continuous time trajectory as a sequence of control points. The trajectory parameterization module 201 calculates the trajectory at any given time using the following formula. radar pose :

[0044] ;

[0045] In the formula, For a moment Radar pose matrix in world coordinate system; For the first A B-spline control point in the Lie group space The pose value in the middle; Let B be the order of the spline; This is the normalized time parameter; for The cumulative B-spline basis function matrix values; For the exponential mapping from Lie algebras to Lie groups; Let be the logarithmic mapping from Lie groups to Lie algebras.

[0046] The data processing unit 200 includes a kinematic constraint calculation module 202. The kinematic constraint calculation module 202 is configured to read human cervical spine kinematic chain parameters from the memory 210. The human cervical spine kinematic chain parameters are defined from the human torso coordinate system. To the radar body coordinate system The series rigid body transformation relationship. Kinematic constraint calculation module 202 calculates the kinematic constraint based on the current joint configuration vector. Calculate the analytic geometric Jacobian matrix Analytic geometry Jacobian matrix Describe the linear mapping relationship between the angular velocity space of the cervical spine joints and the velocity space of the radar terminal.

[0047] The data processing unit 200 includes an optimization solution module 203. The optimization solution module 203 is configured to construct and solve a nonlinear least squares objective function. The optimization solution module 203 utilizes the analytic geometric Jacobian matrix. Constructing the tangent space projection operator Tangent space projection operator The configuration projects the original gradient vector generated by point cloud matching into the tangent space of the cervical spine kinematic manifold. The optimization solution module 203 calculates the tangent space projection operator using the following formula. :

[0048] ;

[0049] In the formula, The projection matrix of the tangent space of the manifold; The analytic geometric Jacobian matrix for the cervical spine kinematic model; This is the current cervical spine joint configuration vector; This is the matrix transpose symbol; This represents the matrix inversion operation.

[0050] The optimization solution module 203 is configured to utilize the tangent space projection operator. B-spline control points The update increment is constrained. The optimization solution module 203 calculates the control point update amount using the following linear incremental equation. :

[0051] ;

[0052] In the formula, Let be the increment vector of the B-spline control points to be solved; The residual vector for point cloud matching; Let be the constrained Jacobian matrix after tangent space projection mapping; satisfy Relationship; Let be the unconstrained original residual Jacobian matrix.

[0053] Data processing unit 200 is configured to update the amount based on the calculated control points. The B-spline control points are iteratively updated. The data processing unit 200 outputs the updated continuous-time trajectory and the distortion-free point cloud data.

[0054] See attached document Figure 2 , Figure 2This is a block diagram of the logical functional modules of a head-mounted LiDAR point cloud matching device according to an embodiment of the present invention. The data processing unit 200 of the head-mounted LiDAR point cloud matching device provided by the present invention may logically include: a data preprocessing module 200a, a kinematic manifold construction module 200b, a trajectory parameterization module 200c, and a constraint optimization solution module 200d.

[0055] The data preprocessing module 200a is configured to perform time alignment operations on multi-sensor data. The data preprocessing module 200a receives raw laser point cloud data streams from the lidar acquisition module 110 and inertial measurement data from the inertial measurement module 120. The data preprocessing module 200a reads each laser point from the raw laser point cloud data stream. Collection timestamp The data preprocessing module 200a processes data based on the collected timestamps. Interpolation is performed on the time series of inertial measurement data to obtain estimated values ​​of angular velocity and acceleration at corresponding moments. The data preprocessing module 200a outputs preprocessed point cloud data with time synchronization information and initial inertial value information.

[0056] The kinematic manifold construction module 200b is configured to establish and maintain a biomechanical constraint model of the human cervical spine. The kinematic manifold construction module 200b reads preset human cervical spine kinematic chain parameters from the memory 210. These parameters include the link lengths, rotation axis directions, and joint limit angles of each joint. Based on the current trajectory state, the kinematic manifold construction module 200b inversely solves for the cervical spine joint configuration vectors. The kinematic manifold construction module 200b utilizes the inverse kinematics solution of the cervical joint configuration vectors. Calculate the analytic geometric Jacobian matrix used to describe the physiological limitations of human movement. The kinematic manifold construction module 200b calculates the analytic geometric Jacobian matrix using the following formula. Defined motion constraint relationships:

[0057] ;

[0058] In the formula, For radar terminals in Lie algebra space The kinetic spinor in the equation includes three-dimensional linear velocity components and three-dimensional angular velocity components. The analytic geometric Jacobian matrix for the cervical spine kinematic model; This is the cervical joint configuration vector; This is the angular velocity vector of the cervical spine joint.

[0059] The trajectory parameterization module 200c is configured to manage the control point states of a continuous-time B-spline trajectory. The trajectory parameterization module 200c maintains a set of B-spline control point sequences in memory. The trajectory parameterization module 200c receives query requests and, based on any input timestamp... The radar pose is calculated using the cumulative B-spline basis function. The trajectory parameterization module 200c is configured to calculate the pose relative to the B-spline control points. The perturbation derivative, i.e., the spline Jacobian matrix. .

[0060] The constrained optimization solver module 200d is configured to perform nonlinear optimization iterations based on tangent space projection. The constrained optimization solver module 200d receives preprocessed point cloud data. It transforms the preprocessed point cloud data to the world coordinate system and searches for the nearest neighbor corresponding plane in the reference map. The constrained optimization solver module 200d constructs the residual equations from the points to the planes. The constrained optimization solver module 200d calculates the... Geometric residual of each laser point :

[0061] ;

[0062] In the formula, For the first Geometric residual scalar of each laser point; It is the normal vector of the plane corresponding to the nearest neighbor; Based on the current set of control points Calculated time The radar pose transformation matrix; For the first The coordinate vector of an original laser point in the radar body coordinate system; The coordinate vector of the reference point on the nearest neighbor plane; This is the vector transpose symbol.

[0063] The constraint optimization solution module 200d is configured to construct the manifold tangent space projection operator. The constraint optimization solution module 200d receives the analytic geometric Jacobian matrix from the kinematic manifold construction module 200b. The constraint optimization solution module 200d utilizes the analytic geometry Jacobian matrix. A projection matrix capable of filtering non-physiological motion components is constructed. The constraint optimization solution module 200d calculates the manifold tangent space projection operator using the following formula. :

[0064] ;

[0065] In the formula, The projection matrix of the tangent space of the manifold; For analytic geometric Jacobian matrices; This represents the matrix inversion operation.

[0066] The constraint optimization solution module 200d is configured to calculate the effective gradient after tangent space projection constraints. The constraint optimization solution module 200d first calculates the original measurement Jacobian. and sample Jacobi The constraint optimization solution module 200d will use the tangent space projection operator. Embedded in the gradient propagation path. The constraint optimization solution module 200d calculates the joint constraint Jacobian matrix using the following formula. :

[0067] ;

[0068] In the formula, For the first The final Jacobian matrix of each point after physiological constraints; The Jacobian is the measure of the residual relative to the Lie algebra perturbation; The projection matrix of the tangent space; Let be the spline Jacobian of the Lie algebraic perturbation relative to the control point perturbation.

[0069] The constraint optimization solution module 200d is configured to utilize the joint constraint Jacobian matrix. Construct and solve the incremental equations to update the B-spline control point sequence. until geometric residuals Convergence. The constraint optimization solution module 200d outputs the optimized B-spline trajectory and the corrected point cloud data.

[0070] See attached document Figure 3 , Figure 3 This is a schematic flowchart of a head-mounted LiDAR point cloud matching method according to an embodiment of the present invention. The present invention provides a head-mounted LiDAR point cloud matching method that may include the following steps: steps S100, S200, S300, S400, and S500.

[0071] Step S100 is configured to perform data acquisition and parameter initialization. Step S100 receives the raw laser point cloud data stream generated by the lidar acquisition module 110 and the inertial measurement data output by the inertial measurement module 120. Step S100 reads each raw laser point... Precise sampling timestamp Step S100 loads preset human cervical spine kinematic chain parameters from memory 210. The human cervical spine kinematic chain parameters include parameters describing the human torso coordinate system. To radar body coordinate system Link length data and joint degree-of-freedom constraint data for rigid body transformation relationships.

[0072] Step S200 is configured to construct a continuous-time trajectory model. Step S200 uses a cumulative B-spline function to parametrically model the motion trajectory of the lidar in the time domain. Step S200 sets a series of B-spline control points on the time axis. Step S200 calculates arbitrary sampling times based on the control point sequence. radar pose Step S200 performs pose calculation using the following formula:

[0073] ;

[0074] In the formula, For a moment Radar pose matrix in world coordinate system; For the first A B-spline control point in the Lie group space The pose value in the middle; Let B be the order of the spline; This is the normalized time parameter; for The cumulative B-spline basis function matrix values; For the exponential mapping from Lie algebras to Lie groups; Let be the logarithmic mapping from Lie groups to Lie algebras.

[0075] Step S300 is configured to construct the kinematic manifold constraints of the cervical spine. Step S300 decomposes the human cervical spine joint configuration vector based on the trajectory state estimate at the current moment. Step S300 calculates the analytic geometric Jacobian matrix describing the mapping relationship between the cervical spine joint motion space and the Cartesian motion space of the radar terminal. Step S300 defines a linear mapping relationship between the radar terminal velocity and the joint angular velocity using the following formula:

[0076] ;

[0077] In the formula, For radar terminals in Lie algebra space The kinetic spinor in the equation includes three-dimensional linear velocity components and three-dimensional angular velocity components. The analytic geometric Jacobian matrix for the cervical spine kinematic model; This is the cervical joint configuration vector; This is the angular velocity vector of the cervical spine joint.

[0078] Step S400 is configured to perform joint constraint optimization based on tangent space projection. Step S400 includes an iterative loop process until the objective function converges. In each iteration, step S400 utilizes the current B-spline trajectory to optimize the original laser point. Perform distortion correction to obtain points in the world coordinate system. Step S400: Locate the reference map... The corresponding nearest neighbor plane is used to construct the geometric residual. Step S400 calculates the geometric residual using the following formula:

[0079] ;

[0080] In the formula, For the first Geometric residual scalar of each laser point; It is the normal vector of the plane corresponding to the nearest neighbor; Based on the current set of control points Calculated time The radar pose transformation matrix; These are the original laser point coordinates; is the coordinate vector of the reference point on the nearest neighbor plane.

[0081] Step S400 utilizes the analytic geometric Jacobian matrix Constructing the projection operator of the tangent space of a manifold Tangent space projection operator Used to An arbitrary tangent vector in space is projected onto the tangent space of the cervical spine kinematic manifold. Step S400 calculates the projection operator using the following formula:

[0082] ;

[0083] In the formula, The projection matrix of the tangent space of the manifold; For analytic geometric Jacobian matrices; This represents the matrix inversion operation.

[0084] Step S400: Calculate the effective Jacobian matrix after projection constraints. And construct linear incremental equations to update the B-spline control points. Step S400 performs control point increments using the following formula. Solving for:

[0085] ;

[0086] In the formula, This is the update increment for the B-spline control points; The residual vector for all laser points; Let be the constrained Jacobian matrix after tangent space projection mapping; From the original measurement Jacobi Tangent space projection operator and sample Jacobi The result of multiplying together is... .

[0087] Step S500 is configured to output results and update the map. After determining that the optimization has converged, step S500 outputs the corrected precise pose and the dedistorted point cloud for the current frame. Step S500 registers the dedistorted point cloud to the global point cloud map as reference data for subsequent frame matching.

[0088] The present invention provides a method for matching point clouds of a head-mounted lidar, in which step S200 may specifically include: establishing a time-domain parameterized model, setting a control point sequence, and defining a trajectory generation equation.

[0089] Step S200 is configured to establish a time-domain parametric model. Step S200 uses a Lie group. The cumulative B-spline function on the time axis is used as the mathematical expression for the LiDAR motion trajectory. Step S200 discretizes the continuous motion trajectory of the LiDAR into a series of uniformly or non-uniformly distributed time nodes. Step S200 defines the interval of each time node as... Step S200 will change the continuous time variable. Mapping to normalized time parameters Step S200 calculates the normalized time parameter using the following formula. :

[0090] ;

[0091] In the formula, This is the normalized time parameter, and its value range is... ; This is the global timestamp for the current moment; This represents the start time of the time interval to which the current moment belongs; This represents the time interval between control points.

[0092] Step S200 configures the setting of control point sequence. Step S200 defines a control point at each time node. Step S200 denotes the set consisting of all control points as follows: Each control point This includes the pose information of the lidar at the corresponding moment. Step S200 will control the points. Represented as Transformation matrix in space. Step S200: Based on the sampling frequency of the lidar and the preset B-spline order. Determine the impact on the current moment. A subset of control points for pose. For B-order spline, step S200 selects the index from arrive The control points are involved in the trajectory calculation.

[0093] Step S200 is configured to define the trajectory generation equation. Step S200 uses cumulative B-spline basis functions to perform nonlinear interpolation on a selected subset of control points. Step S200 uses cascaded exponential mappings to transform the Lie algebra space. The interpolation results are mapped back to the Lie group space. Step S200 calculates any time using the following formula. Radar pose in world coordinate system :

[0094] ;

[0095] In the formula, For a moment Radar pose matrix in world coordinate system; For the first A B-spline control point in the Lie group space The pose value in the middle; Let B be the order of the spline; This is the normalized time parameter; for The cumulative B-spline basis function matrix values; For the exponential mapping from Lie algebras to Lie groups; For the logarithmic mapping from Lie groups to Lie algebras; For the first The inverse matrix of each control point; This is a loop variable that is multiplied by itself.

[0096] Step S200 is configured to calculate the cumulative B-spline basis function values. Step S200 pre-stores... Basis function matrix of B-order splines Step S200 is based on the normalized time parameter. Construct the time vector UU. Step S200 obtains the time vector UU through matrix operations. The value of . Step S200 ensures that the basis functions satisfy the unit partition property.

[0097] Step S200 is configured to calculate the motion derivative based on a continuous-time trajectory model. Step S200 uses the trajectory generation equation to calculate the time derivative. The first derivative analytical expression is used to calculate the angular velocity and linear velocity of the lidar at any given time. Step S200 uses the trajectory generation equation to calculate the time... The second derivative analytical expression is used to calculate the angular acceleration and linear acceleration of the lidar at any given time. Step S200 uses the calculated motion derivative for distortion correction of point cloud data and pre-integration constraints of inertial measurement data.

[0098] The present invention provides a method for matching point clouds of a head-mounted lidar, in which step S300 may specifically include: defining a human cervical spine kinematic chain model, determining the joint configuration space, and deriving the analytical geometric Jacobian matrix.

[0099] Step S300 configures the definition of a human cervical spine kinematic chain model. Step S300 sets the wearer's torso as the base coordinate system. Step S300 sets the head-mounted lidar as the coordinate system of the end effector. Step S300 in the base coordinate system coordinate system with end effector A series rigid body kinematic chain is established between them. Step S300 simplifies the cervical spine into a mechanical structure composed of multiple rigid links connected by rotational joints. Step S300 defines the cervical spine kinematic chain as including... Each has an independent physiological degree of freedom. Step S300 defines the cervical joint configuration vector. Cervical joint configuration vector It includes cervical spine flexion-extension angle components, cervical spine axial rotation angle components, and cervical spine lateral flexion angle components.

[0100] Step S300 is configured to determine the joint configuration space. Step S300 uses forward kinematics equations to describe the mapping relationship from the joint configuration space to the Cartesian pose space. Step S300 calculates the radar pose matrix relative to the torso using the exponential product formula. Step S300 defines the positive kinematic mapping using the following formula:

[0101] ;

[0102] In the formula, This is the transformation matrix between the radar coordinate system and the torso coordinate system; This refers to the number of degrees of freedom of the cervical vertebrae. For joint index variables; For the first The scalar of the rotation angle of a joint; For the first Unit rotation of motion of each joint in the base coordinate system; This is the initial pose matrix in the zero-position state; For Lie algebra To Liqun Exponential mapping; This refers to matrix chain multiplication.

[0103] Step S300 is configured to derive the analytic geometric Jacobian matrix. Step S300 calculates the analytic geometric Jacobian matrix describing the linear mapping relationship between the joint velocity space and the end effector Cartesian velocity space. Step S300 corrects the kinematic equations with respect to time. Differentiate and establish the joint angular velocity vector. With radar terminal motion spinor The differential relationship between them. Step S300 calculates the instantaneous motion spinor of the radar terminal using the following formula:

[0104] ;

[0105] In the formula, For radar terminals in Lie algebra space The kinetic spinor in the equation includes three-dimensional linear velocity components and three-dimensional angular velocity components. The analytic geometric Jacobian matrix of the cervical spine kinematic model has a dimension of 6. ; This is the current cervical spine joint configuration vector; This is the angular velocity vector of the cervical spine joint.

[0106] Step S300 is configured to calculate the specific value of the analytic geometric Jacobian matrix. Step S300 is based on the joint configuration vector obtained by inverse solving the B-spline trajectory at the current time. Calculate the matrix The value of each column vector in the array. Step S300 will... The column vectors Represented as the first The spatial projection of the joint axis in the current configuration. Step S300 calculates the first joint axis using the following formula. column vectors :

[0107] ;

[0108] In the formula, For analytic geometric Jacobian matrix The List; The accompanying transformation operator is used to transform Lie algebra elements from one coordinate system to another. From the base coordinate system to the first Transformation matrix of each joint coordinate system; For the first Unit rotation of motion of each joint.

[0109] Step S300 is configured to define the cervical spine motion manifold using an analytic geometric Jacobian matrix. Step S300 will be based on all joint configurations. The pose set generated by positive kinematics mapping is defined as embedded in... Cervical spine motion manifold in space Step S300 utilizes the analytic geometric Jacobian matrix. The column space spans the tangent space of the manifold at the current configuration. Step S300 uses this tangent space as a constraint boundary to restrict the direction of point cloud matching optimization in subsequent steps.

[0110] The present invention provides a method for matching point clouds of a head-mounted lidar, in which step S400 may specifically include: performing motion compensation to correct distortion, searching for the nearest neighbor set, and constructing point-to-point correspondences.

[0111] Step S400 is configured to perform motion compensation distortion correction. Step S400 receives the first... laser points and its corresponding timestamp Step S400 utilizes the set of B-spline control points from the current iteration step. And trajectory generation equation, calculate timestamp Corresponding radar pose in world coordinate system Step S400 will change the original laser point Transform the radar body coordinate system to the world coordinate system to obtain the distortion-free world coordinate points. Step S400 performs distortion correction calculations using the following formula:

[0112] ;

[0113] In the formula, The world coordinate system laser point coordinate vector after distortion correction; Based on the current set of control points Calculated time The radar pose transformation matrix; This is the coordinate vector of the original laser point in the radar body coordinate system.

[0114] Step S400 is configured to search for the nearest neighbor set. Step S400 searches for points in a pre-built global point cloud map or a local sliding window map that are... closest in space A reference point. Step S400 uses a KD-tree or octree data structure to accelerate the search process. Step S400 sets... The value is 5. Step S400: Obtain the nearest neighbor set. .

[0115] Step S400 is configured to construct point-to-face correspondences. Step S400 utilizes the nearest neighbor set. Fit the local plane. Step S400: Calculate the nearest neighbor set. geometric center and covariance matrix Step S400 calculates the covariance matrix using the following formula. :

[0116] ;

[0117] In the formula, Let be the covariance matrix of the nearest neighbor set; This represents the number of nearest neighbors. For nearest neighbor index; For the nearest neighbor set, the first The coordinate vector of each point; is the geometric center coordinate vector of the nearest neighbor set; This is the vector transpose symbol.

[0118] Step S400 is configured to solve for the normal vector of the corresponding plane. Step S400 involves calculating the covariance matrix. Perform eigenvalue decomposition. Step S400: Obtain the covariance matrix. The three eigenvalues And satisfy Step S400: Select the smallest eigenvalue. The corresponding eigenvectors serve as the normal vectors of the corresponding planes. Step S400: Select the geometric center. As a reference point on the corresponding plane .

[0119] Step S400 is configured to verify the validity of the correspondence. Step S400 calculates the plane curvature. Step S400 is performed using the formula. Calculate the plane curvature. Step S400: Determine the plane curvature. Is it less than a preset flatness threshold? If the plane curvature is... If the flatness is less than the preset flatness threshold, step S400 confirms that the point-to-face correspondence is valid and... and Used for subsequent residual equation construction. If the plane curvature... If the flatness threshold is greater than or equal to the preset flatness threshold, step S400 will remove the corresponding relationship and it will not participate in subsequent optimization calculations.

[0120] See attached document Figure 4 , Figure 4This is a schematic diagram of the joint constraint optimization process for tangent space projection according to an embodiment of the present invention. Step S400 in the head-mounted lidar point cloud matching method provided by the present invention may specifically include: constructing a nonlinear least squares objective function, deriving the original gradient propagation chain, constructing a tangent space projection operator, calculating the joint constraint Jacobian matrix, and solving the incremental equation.

[0121] Step S400 is configured to construct a nonlinear least squares objective function. Based on the effective point-to-plane correspondence determined in step S300, step S400 calculates the geometric residuals of all effective laser points. The sum of squares is defined as the optimization objective. Step S400 introduces the Cauchy kernel function or the Huber kernel function as a robust kernel function. To suppress the influence of outliers, step S400 defines the objective function using the following formula. :

[0122] ;

[0123] In the formula, Let be the value of the objective function to be minimized; This represents the total number of valid laser points within the current frame. Index for laser points; For the first Geometric residuals of the laser point surface; Let be the set of B-spline control points to be optimized; This is a robust kernel function.

[0124] Step S400 is configured to derive the original gradient propagation chain. Step S400 calculates the geometric residuals. Lie algebraic perturbation relative to radar pose Measurement Jacobian matrix Step S400 derives the model using the Lie algebra left perturbation. Step S400 calculates the measurement Jacobian matrix using the following formula. :

[0125] ;

[0126] In the formula, For the first The measurement Jacobian matrix for each point has dimensions of . ; This is the normal vector of the corresponding plane; For a moment The rotation matrix part of the radar pose; These are the original laser point coordinates; An antisymmetric matrix operator used to map a three-dimensional vector to... Antisymmetric matrix; This is the matrix transpose symbol.

[0127] Step S400 is configured to calculate the B-spline Jacobian matrix. The calculation time for step S400 is... Radar attitude disturbance relative to B-spline control point disturbance spline Jacobian matrix Step S400 determines the value of the matrix based on the derivative properties of the basis functions of the cumulative B-spline.

[0128] Step S400 is configured to construct the tangent space projection operator. Step S400 utilizes the analytic geometric Jacobian matrix obtained in step S300. Construct a projection matrix capable of filtering non-physiological motion components. Step S400 calculates the tangent space projection operator using the following formula. :

[0129] ;

[0130] In the formula, The projection matrix of the tangent space of the manifold; For analytic geometric Jacobian matrices; This represents the matrix inversion operation.

[0131] Step S400 is configured to compute the joint constraint Jacobian matrix. Step S400 will use the tangent space projection operator. Embedded into the gradient propagation path from the spline control points to the geometric residuals. Step S400 mandates that the gradient update must lie within the tangent space of the cervical kinematic manifold. Step S400 calculates the joint constraint Jacobian matrix using the following formula. :

[0132] ;

[0133] In the formula, For the first The final Jacobian matrix of each point after physiological constraints; To measure the Jacobian matrix; The projection matrix of the tangent space; It is a spline Jacobian matrix.

[0134] Step S400 is configured to solve the incremental equations. Step S400 uses the Gauss-Newton method or the Levenberg-Marquardt method to construct a system of linear incremental equations. Step S400 then accumulates the constraint Jacobian matrix and residual vectors of all laser points to construct the Hessian matrix. and gradient vector Step S400 calculates the control point increment using the following formula. :

[0135] ;

[0136] In the formula, This is the update increment vector for the B-spline control points; For the first The joint constraint Jacobian matrix of points; This refers to the damping factor in the Levenberg-Marquardt algorithm; It is the identity matrix; For the first The geometric residual values ​​of each point.

[0137] Step S400 is configured to update the control point state. Step S400 utilizes the calculated increment. For the current B-spline control points Perform the update. Step S400: Determine the control point increment. Is the norm less than a preset convergence threshold? If it is less than the preset convergence threshold, step S400 determines that the optimization has converged and ends this step. If it is greater than or equal to the preset convergence threshold, step S400 returns to execute the motion compensation and distortion correction step and enters the next iteration loop.

[0138] The present invention provides a head-mounted lidar point cloud matching method, which further includes: constructing a physiological limit distance field, calculating a physiological proximity index, and dynamically adjusting the B-spline node density.

[0139] The present invention provides a data processing unit 200 for a head-mounted lidar point cloud matching device, which logically further includes an adaptive node allocation module 200e. The adaptive node allocation module 200e is configured to allocate nodes based on cervical spine joint configuration vectors. The time node interval of the continuous-time B-spline trajectory is dynamically adjusted relative to the physiological limit boundary. The adaptive node allocation module 200e reads preset joint range of motion parameters from the memory 210. The joint range of motion parameters define the degree of freedom of each cervical vertebra joint. zero angle and the maximum range of motion on one side .

[0140] The adaptive node allocation module 200e is configured to calculate the physiological proximity index. The adaptive node allocation module 200e evaluates the cervical joint configuration vector at the current moment. The degree of approximation to the physiological limit boundary in the joint configuration space. The adaptive node allocation module 200e calculates the normalized physiological proximity index using the following formula. :

[0141] ;

[0142] In the formula, This is the physiological proximity index, with a value range of [value range missing]. ; Index of joint degrees of freedom; This represents the total number of joint degrees of freedom. For the current moment, the first The angle values ​​of each joint; For the first The anatomical zero angle of each joint; For the first The maximum allowable deflection angle amplitude for each joint; This is the non-linear sensitivity coefficient, configured as a positive integer.

[0143] The adaptive node allocation module 200e is configured to calculate the adaptive time node interval based on the physiological proximity index. The adaptive node allocation module 200e establishes a mapping relationship from the physiological proximity index to the time density of B-spline control points. The adaptive node allocation module 200e calculates the time node interval at the current moment using the following formula. :

[0144] ;

[0145] In the formula, This refers to the time span between adjacent control points of the B-spline, calculated adaptively. The preset minimum time interval corresponds to the high-frequency sampling requirements in the physiological limit region; The preset maximum time interval corresponds to the smooth trajectory requirement in the zero-position comfort zone; This is the calculated physiological proximity index.

[0146] The adaptive node allocation module 200e is configured to perform node reallocation operations. The adaptive node allocation module 200e determines the current physiological proximity index. Has a change occurred? When detected... When the value approaches 1, the adaptive node allocation module 200e determines that the cervical spine is in a state of high stiffness and potential tremor. The adaptive node allocation module 200e uses the calculated... The time sequence of the B-spline trajectory is redefined. The adaptive node allocation module 200e inserts additional control points in the physiological limit region to increase the local support density of the trajectory. The adaptive node allocation module 200e transmits the updated node sequence to the trajectory parameterization module 200c for subsequent trajectory generation and optimization calculations.

[0147] The present invention provides a head-mounted lidar point cloud matching method, which further includes: constructing a virtual torso geometric model, extracting the ontology occlusion point set, and constructing ontology geometric constraint residuals.

[0148] The present invention provides a data processing unit 200 for a head-mounted lidar point cloud matching device, which logically further includes a body occlusion auxiliary constraint module 200f. The body occlusion auxiliary constraint module 200f is configured to establish a direct geometric constraint between the lidar and the torso base by utilizing lidar point data obscured by the wearer's shoulder or torso in the lidar's scanning field of view. The body occlusion auxiliary constraint module 200f reads preset virtual torso geometric model parameters from a memory 210. The virtual torso geometric model parameters describe the human shoulder and upper chest in the torso coordinate system. The spatial geometry below. The body occlusion auxiliary constraint module 200f uses parametric planes or parametric surfaces to describe the virtual torso geometry model.

[0149] The body occlusion auxiliary constraint module 200f is configured to extract the body occlusion point set. The body occlusion auxiliary constraint module 200f uses the cervical joint configuration vector obtained from the inverse solution at the current time step. Calculate the pose transformation matrix of the lidar relative to the torso. The 200f module for body occlusion auxiliary constraint calculates the virtual torso geometry model in the radar body coordinate system. The projection area is shown below. The body occlusion auxiliary constraint module 200f sets the filtering conditions for the region of interest (ROI). The body occlusion auxiliary constraint module 200f traverses the raw laser point cloud data of the current frame. The body occlusion auxiliary constraint module 200f selects laser points that fall into the ROI and whose depth values ​​meet the preset shoulder distance range, forming a body occlusion point set. .

[0150] The ontology occlusion auxiliary constraint module 200f is configured to construct ontology geometric constraint residuals. The ontology occlusion auxiliary constraint module 200f handles the ontology occlusion point set. Each laser point in Calculate its transformation to the torso coordinate system The distance deviation between the virtual torso geometric model and the actual body is calculated. The body occlusion auxiliary constraint module 200f calculates the body geometric constraint residual using the following formula. :

[0151] ;

[0152] In the formula, For the first Geometric constraint residuals of each occlusion point; This is the normal vector of the surface of the virtual torso geometric model in the torso coordinate system; The symbol for vector transpose; To be based on the current joint configuration The calculated transformation matrix from torso to radar; This is a matrix inversion operation used to calculate the radar-to-toe transformation; For the first The coordinate vectors of each point of obstruction on the radar body coordinate system; is the coordinate vector of the reference point on the surface of the virtual torso geometric model.

[0153] The body occlusion auxiliary constraint module 200f is configured to incorporate the body geometric constraint residuals into the overall optimization objective function. The body occlusion auxiliary constraint module 200f assigns weight coefficients to the body geometric constraint residuals. The 200f module for assisting in occlusion utilizes the residuals of ontological geometric constraints on the cervical joint configuration vector. Perform reverse correction constraints. Because... With B-spline control points Through kinematic model coupling, this constraint indirectly corrects the drift of the B-spline trajectory. The body occlusion auxiliary constraint module 200f outputs an extended objective function containing body constraint terms to the constraint optimization solution module 200d for unified solution.

Claims

1. A point cloud matching method for a head-mounted lidar system, characterized in that, Includes the following steps: Receive the raw laser point cloud data stream generated by the lidar acquisition module and the inertial measurement data output by the inertial measurement module, and read the preset human cervical spine kinematic chain parameters; A continuous-time trajectory model is established using the cumulative B-spline function, and the motion trajectory of the lidar acquisition module is parameterized into a series of B-spline control points; Based on the trajectory state at the current moment, the human cervical spine joint configuration vector is deduced, and the analytic geometric Jacobian matrix describing the mapping relationship between the cervical spine joint angular velocity space and the radar terminal Cartesian velocity space is calculated. Perform joint constraint optimization based on tangent space projection. The joint constraint optimization includes: A manifold tangent space projection operator is constructed using the Jacobian matrix of analytic geometry. The manifold tangent space projection operator is configured to project the tangent vector of the Lie group space onto the tangent space of the cervical kinematic manifold. A joint constraint Jacobian matrix containing physiological motion constraints is constructed using the manifold tangent space projection operator; Based on the joint constraint Jacobian matrix, an incremental equation is constructed and solved to iteratively update the B-spline control points until the geometric residual of the point cloud matching converges. Output corrected continuous time trajectory and distortion-free point cloud data to complete point cloud matching for head-mounted LiDAR; The method further includes a body occlusion auxiliary constraint step: Read the preset virtual torso geometric model parameters; Calculate the relative pose of the lidar acquisition module with respect to the torso based on the current trajectory status; Filter out the set of occlusion points in the original laser point cloud data stream that fall into the projection area of ​​the virtual torso geometric model and whose depth values ​​meet the preset range; Calculate the distance deviation between each laser point in the body occlusion point set after transformation to the torso coordinate system and the virtual torso geometric model, construct the body geometric constraint residual and add it to the objective function of joint constraint optimization.

2. The head-mounted lidar point cloud matching method according to claim 1, characterized in that, A continuous-time trajectory model is established using the cumulative B-spline function, including: The continuous motion trajectory of the lidar acquisition module is discretized into a series of time nodes, and a B-spline control point in the Lie group space is defined at each time node. Map the global timestamp to a normalized time parameter; The subset of control points participating in the pose calculation at the current moment is determined based on the sampling frequency of the lidar acquisition module. The selected subset of control points is nonlinearly interpolated using the cumulative B-spline function, and the interpolation results are mapped back to the Lie group space through cascaded exponential mapping to obtain the radar pose in the world coordinate system at any time.

3. The head-mounted lidar point cloud matching method according to claim 1, characterized in that, The inverse solution of the human cervical spine joint configuration vector is based on the human cervical spine kinematic chain parameters, which define the series rigid body transformation relationship between the human torso coordinate system and the radar body coordinate system. The series rigid body transformation relationship simplifies the cervical spine into a mechanical structure composed of multiple rigid links connected by rotational joints. The human cervical spine joint configuration vector includes cervical flexion-extension angle components, cervical axial rotation angle components, and cervical lateral flexion angle components.

4. The head-mounted lidar point cloud matching method according to claim 3, characterized in that, Calculating the analytic geometric Jacobian matrix includes: The mapping relationship from joint configuration space to Cartesian pose space is described using positive kinematic equations; By differentiating the kinematic equations with respect to time, a linear differential relationship is established between the joint angular velocity vector and the radar terminal spinor. Calculate each column vector of the analytic geometry Jacobian matrix, where each column vector represents the spatial projection of the corresponding joint axis in the current configuration.

5. The head-mounted lidar point cloud matching method according to claim 1, characterized in that, The iterative process of performing joint constraint optimization based on tangent space projection also includes: Calculate the radar pose corresponding to each original laser point using the current B-spline control points, transform the original laser points in the original laser point cloud data stream to the world coordinate system, and complete the distortion removal process. Search for the nearest neighbor set of the distorted laser points in the reference map and fit the local plane; Calculate the distance from the distorted laser point to the local plane to obtain the geometric residual; Calculate the measurement Jacobian matrix of the geometric residual relative to the Lie algebraic perturbation of the radar pose, and calculate the spline Jacobian matrix of the radar pose perturbation relative to the B-spline control point perturbation.

6. The head-mounted lidar point cloud matching method according to claim 5, characterized in that, The manifold tangent space projection operator is constructed using the Jacobian matrix of analytic geometry, specifically as follows: The intermediate square matrix is ​​obtained by multiplying the transpose of the analytic geometric Jacobian matrix with the analytic geometric Jacobian matrix. Calculate the inverse matrix of the intermediate square matrix; By performing matrix multiplication sequentially on the analytic geometric Jacobian matrix, the inverse matrix of the intermediate square matrix, and the transpose of the analytic geometric Jacobian matrix, the manifold tangent space projection operator is obtained.

7. The head-mounted lidar point cloud matching method according to claim 6, characterized in that, The steps for constructing and solving the incremental equations based on the joint constraint Jacobian matrix include: The manifold tangent space projection operator is embedded in the gradient transfer path, and the measurement Jacobian matrix, the manifold tangent space projection operator and the spline Jacobian matrix are multiplied sequentially to obtain the joint constraint Jacobian matrix. By summing the joint constraint Jacobian matrix and geometric residuals of all valid laser points, a system of linear incremental equations is constructed. Solve the linear incremental equation system to obtain the update increment of the B-spline control points.

8. The head-mounted lidar point cloud matching method according to claim 1, characterized in that, The method also includes an adaptive node allocation step based on physiological limits: Read the zero-position angle and maximum range of motion parameters of each joint in the cervical spine; Calculate the physiological proximity index of the current human cervical spine joint configuration vector relative to the physiological limit boundary; The time interval of B-spline control points in the continuous-time trajectory model is dynamically adjusted based on the physiological proximity index, and the time interval is reduced when the physiological proximity index approaches the physiological limit boundary.

9. A head-mounted lidar point cloud matching device, characterized in that, The head-mounted lidar point cloud matching method according to any one of claims 1-8 includes: The head-mounted device has a rigid frame structure that conforms to the contours of the human head. The lidar acquisition module is rigidly fixed to the head-mounted carrier and configured to generate raw lidar point cloud data streams. An inertial measurement module is rigidly fixed to a head-mounted device and configured to output inertial measurement data. The memory is configured to store parameters of the human cervical spine kinematic chain. The data processing unit is electrically connected to the lidar acquisition module, the inertial measurement module, and the memory. The data processing unit is configured to receive raw laser point cloud data streams and inertial measurement data, and read human cervical spine kinematic chain parameters from the memory; The data processing unit is configured to use the cumulative B-spline function to establish a continuous time trajectory model, and parameterize the motion trajectory of the lidar acquisition module into multiple B-spline control points. The data processing unit is configured to inversely solve the human cervical spine joint configuration vector based on the trajectory state at the current moment, and calculate the analytic geometric Jacobian matrix describing the mapping relationship between the cervical spine joint angular velocity space and the radar terminal Cartesian velocity space. The data processing unit is configured to perform joint constraint optimization operation based on tangent space projection. In the joint constraint optimization operation, the manifold tangent space projection operator is constructed using the analytical geometric Jacobian matrix. The joint constraint Jacobian matrix containing physiological motion constraints is constructed using the manifold tangent space projection operator. The incremental equation is constructed and solved based on the joint constraint Jacobian matrix. The B-spline control points are iteratively updated until the geometric residuals converge. The data processing unit is configured to output optimized continuous time trajectories and distortion-free point cloud data.

Images