Pose matrix determination method, positioning method, processor and mobile robot

Abstract

The present application relates to the field of computers, and in particular to a method for determining a pose matrix, a positioning method, a processor, and a mobile robot. The method comprises: generating a matrix multiplication unit M and a scheduling matrix R based on the dimensions of a declared matrix A; generating a rotation matrix J composed of the positive rotation values ​​and the cosine values ​​of three angles based on the matrix A and the matrix multiplication unit M; updating the matrix A; updating the matrix J1; after updating the matrix A and the matrix J1 a set number of times, obtaining matrices A′ and J1′; if the matrix A′ meets the set conditions, obtaining a pose matrix P′ based on the matrix A′, the matrix J1′, the matrix multiplication unit M, and the obtained gradient matrix G. The present invention implements the updating of the matrix A and the matrix J1 through the matrix multiplication unit M, effectively simplifying the complex operations of the data, thereby improving the overall efficiency of obtaining the pose matrix.

Description

Technical Field

[0001] The present application relates to the field of computers, and in particular to a pose matrix determination method, a positioning method, a processor, and a mobile robot. Background Art

[0002] LiDAR-based SLAM positioning algorithms are widely used in the field of mobile robotics. During motion, mobile robots need to fit each frame of LiDAR point cloud data to achieve map matching and precise positioning. During the point cloud fitting process of map matching, the pose matrix needs to be accumulated to obtain the minimum error for map registration.

[0003] In the existing technology, while performing Jacobi rotations during pose matrix acquisition, although this is also done in parallel, only one row of elements is operated on at a time. Multiple rows are processed sequentially, and the updated data is then overwritten by writing to the BRAM using a round-robin schedule. Therefore, this rotation update method in the existing technology is inefficient and cannot achieve fast positioning. Summary of the Invention

[0004] Based on this, it is necessary to provide a pose matrix determination method, positioning method, processor and mobile robot to address the above technical problems.

[0005] In a first aspect, an embodiment of the present invention provides a method for determining a pose matrix, comprising:

[0006] Generate a matrix multiplication unit M and a scheduling matrix R according to the dimensions of the declared matrix A; wherein the element a of the matrix A ij The element h of the Hessian matrix to be decomposed ij determined;

[0007] Based on the matrix A and the matrix multiplication unit M, a rotation matrix J consisting of the positive rotation values ​​and the cosine values ​​of the three angles is generated;

[0008] Update the matrix A based on the matrix A, the rotation matrix J, the scheduling matrix R, and the matrix multiplication unit M;

[0009] Update the matrix J1 according to the declared matrix J1, the rotation matrix J, the scheduling matrix R and the matrix multiplication unit M;

[0010] After updating the matrix A and the matrix J1 a set number of times, the matrix A′ and the matrix J1′ are obtained. If the matrix A′ meets the set conditions, the pose matrix P′ is obtained based on the matrix A′, the matrix J1′, the matrix multiplication unit M and the obtained gradient matrix G.

[0011] In one embodiment, generating a rotation matrix J composed of three angles' direct rotation values ​​and cosine values ​​based on the matrix A and the matrix multiplication unit M includes:

[0012] Declare the matrix h′, whose elements are h′ ij , the element h′ ij Element h ij Input to the multiplication unit M to obtain the first result Where h′ ij =h ji ;

[0013] The first result The value of is assigned to the element a of matrix A ij , and declare α i Declare β as the square of the norm corresponding to the ith odd column of matrix A. i is the square of the norm of the i-th even column of matrix A, and declares γ i is the product of the ith odd and ith even columns of matrix A;

[0014] The element a ij , the declared element b ij Input to the multiplication unit M to obtain the second result And for α i , β i , γ i Update to get α′ i , β′ i ,γ′ i , where b ij =a ji ;

[0015] Based on α′ i , β′ i ,γ′ i Generate the rotation matrix J composed of the positive rotation value and the cosine value of the three angles. The elements of the rotation matrix J are J ij .

[0016] In one embodiment, updating the matrix A based on the matrix A, the rotation matrix J, the scheduling matrix R, and the matrix multiplication unit M includes:

[0017] The element a of matrix A ij , the element J of the rotation matrix J ij Input into the matrix multiplication unit M to obtain the third result The third result The element R of the scheduling matrix R ij Input into the matrix multiplication unit M to obtain the fourth result The fourth result Assign element a to matrix A ij To update the matrix A.

[0018] In one embodiment, updating the matrix J1 according to the declared matrix J1, the rotation matrix J, the scheduling matrix R, and the matrix multiplication unit M includes:

[0019] The element J1 of matrix J1 ij , the element J of the rotation matrix J ij Input into the multiplication unit M to obtain the fifth result

[0020] The fifth result The element R of the scheduling matrix R ij Input into the multiplication unit M to obtain the sixth result The sixth result Assign element J1 of matrix J1 ij To update the matrix J1.

[0021] In one embodiment, determining the pose matrix P′ based on the matrix A′, the matrix J1′, the matrix multiplication unit M, and the obtained gradient matrix G includes:

[0022] Determining whether the matrix A′ is a singular matrix and adjusting the matrix A′ if the matrix A′ is a singular matrix;

[0023] Divide each column element of matrix A' by the square of the corresponding norm to obtain matrix A", the elements of matrix A" are A" ij ;

[0024] Declare a matrix L whose elements are L ij , the element A″ ij Transpose(A″ ij ) T , the element J1′ of the matrix J1′ ij Input to the matrix multiplication unit M to obtain the seventh result And the seventh result Assign the element L of matrix L ij To update the matrix L;

[0025] Declare matrix K, whose elements are K ij , the element L ij , h′ ij Input to the matrix multiplication unit M to get the eighth result And the eighth result Assign element K of matrix K ij To update the matrix K;

[0026] Declare a matrix P whose elements are P ij , the element K of matrix K ij , the element G of the gradient matrix G i1 Input to the matrix multiplication unit M to get the ninth result And the ninth result Assign element P of matrix P ij To update the matrix P, we get the pose matrix P′.

[0027] In a second aspect, an embodiment of the present invention proposes a positioning method based on a pose matrix, comprising:

[0028] Generate Hessian matrix and gradient matrix based on the acquired map data and radar data;

[0029] Determine the pose matrix P′ based on the method described in the first aspect;

[0030] Positioning is performed based on the pose matrix P′.

[0031] In one embodiment, generating a Hessian matrix and a gradient matrix based on the acquired map data and radar data includes:

[0032] Traverse the received map point cloud data in parallel and rasterize the map. Each grid has a grid ID.

[0033] Using the grid id as the address, store the centroid coordinate vector q and covariance matrix vector ∑ in each grid into the corresponding BRAM;

[0034] Convert the received radar point cloud data into a grid, use the grid ID as the address, and generate the Hessian matrix and gradient matrix.

[0035] In one embodiment, the positioning based on the pose matrix P′ includes:

[0036] Normalizing the pose matrix P′ to obtain the pose direction and step length;

[0037] If the step length meets the set conditions, the coordinates corresponding to the pose matrix are the positioning coordinates; otherwise, the step length is iteratively optimized in the pose direction until the step length meets the set conditions.

[0038] In a third aspect, an embodiment of the present invention provides a computer device, comprising a memory and a processor, wherein the memory stores a computer program, and the processor executes the steps described in the first aspect or the second aspect.

[0039] In a fourth aspect, an embodiment of the present invention provides a computer-readable storage medium having a computer program stored thereon, and when the processor executes the computer program, the steps described in the first aspect or the second aspect are implemented.

[0040] The above-described method, computer device, and storage medium implement updates to both matrix A and matrix J1 via matrix multiplication unit M, effectively simplifying complex data manipulation and thereby improving the overall efficiency of obtaining pose matrices. Multiple calls to matrix multiplication unit M allow circuits to operate at full capacity through time-division multiplexing, effectively reducing computing resource usage. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 A schematic diagram of an application environment of a method for determining a pose matrix according to an embodiment;

[0042] Figure 2 1 is a flow chart of a method for determining a pose matrix in one embodiment;

[0043] Figure 3 1 is a flow chart of a positioning method based on a pose matrix in one embodiment;

[0044] Figure 4 is a schematic diagram of a rasterization processing method in one embodiment;

[0045] Figure 5 Schematic diagram of the structure of a mobile robot in one embodiment. DETAILED DESCRIPTION

[0046] In order to make the purpose, technical solutions and advantages of this application more clear, the following further describes this application in detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain this application and are not intended to limit this application.

[0047] The pose matrix determination method provided in this application can be applied to Figure 1 In the application environment shown, the terminal 102 communicates with the mobile robot 104. The terminal 102 generates a matrix multiplication unit M and a scheduling matrix R according to the dimensions of the declared matrix A; wherein the element a of the matrix A is ij The element h of the Hessian matrix to be decomposed ijDetermined; Based on the matrix A and the matrix multiplication unit M, a rotation matrix J consisting of the positive rotation value and the corotation value of the three angles is generated; Based on the matrix A, the rotation matrix J, the scheduling matrix R, and the matrix multiplication unit M, the matrix A is updated; Based on the declared matrix J1, the rotation matrix J, the scheduling matrix R, and the matrix multiplication unit M, the matrix J1 is updated; After updating the matrix A and the matrix J1 a set number of times, the matrix A' and the matrix J1' are obtained. If the matrix A' meets the set conditions, the pose matrix P' is obtained based on the matrix A', the matrix J1', the matrix multiplication unit M, and the obtained gradient matrix G. The terminal 102 then sends the pose matrix P' to the mobile robot 104, and the mobile robot 104 performs positioning based on the pose matrix P'.

[0048] The terminal 102 may include one or more processors (e.g., a single-chip processor or a multi-chip processor). By way of example only, the terminal 102 may include a central processing unit (CPU), an application-specific integrated circuit (ASIC), an application-specific instruction set processor (ASIP), a graphics processing unit (GPU), a physical processing unit (PPU), a digital signal processor (DSP), a field programmable gate array (FPGA), a programmable logic device (PLD), a controller, a microcontroller unit, a reduced instruction set computer (RISC), a microprocessor, or any combination thereof.

[0049] The communication may be or include a public network (e.g., the Internet), a private network (e.g., a local area network (LAN)), a wired network, a wireless network (e.g., an 802.11 network, a Wi-Fi network), a frame relay network, a virtual private network (VPN), a satellite network, a telephone network, a router, a hub, a switch, a server computer, and / or any combination thereof. For example, the network may include a cable network, a wired network, a fiber optic network, a telecommunications network, an intranet, a wireless local area network (WLAN), a metropolitan area network (MAN), a public switched telephone network (PSTN), a Bluetooth network, a ZigBee network, a near field communication (NFC) network, etc., or any combination thereof.

[0050] In one embodiment, if Figure 2 As shown, a method for determining a pose matrix is ​​provided, which is applied to Figure 1 The application environment in the example is used to illustrate the following steps:

[0051] S201: Generate a matrix multiplication unit M and a scheduling matrix R according to the dimensions of the declared matrix A; wherein the element a of the matrix A ij The element h of the Hessian matrix to be decomposed ij determined;

[0052] S202: Based on the matrix A and the matrix multiplication unit M, generate a rotation matrix J composed of the positive rotation values ​​and the corotation values ​​of three angles;

[0053] S203: Based on the matrix A, the rotation matrix J, the scheduling matrix R, and the matrix multiplication unit M, update the matrix A;

[0054] S204: updating the matrix J1 according to the declared matrix J1, the rotation matrix J, the scheduling matrix R, and the matrix multiplication unit M;

[0055] S205: After updating the matrix A and the matrix J1 a set number of times, the matrix A′ and the matrix J1′ are obtained. If the matrix A′ meets the set conditions, the pose matrix P′ is obtained based on the matrix A′, the matrix J1′, the matrix multiplication unit M and the obtained gradient matrix G.

[0056] In this embodiment, complex operations such as Jacobi rotations and round-robin iterations used in the prior art are simplified into matrix multiplications. This means that updates to both matrix A and matrix J1 are implemented via a matrix multiplication unit M. This effectively simplifies complex data manipulation and improves the overall efficiency of obtaining the pose matrix. Furthermore, the entire method can independently complete calculations without requiring additional control commands, effectively reducing system complexity.

[0057] In this embodiment, the matrix multiplication unit M that is called multiple times can enable the circuit to run at full load through time division multiplexing technology, thereby effectively reducing the use of computing resources.

[0058] In this embodiment, the entire determination method does not require caching of intermediate data, so no additional memory resources need to be consumed, and there is no time overhead caused by frequent reading and writing of memory.

[0059] In step S201, according to the declared dimension of matrix A, the matrix multiplication unit M and the scheduling matrix R are generated as follows:

[0060] According to the dimension of matrix A, a matrix multiplication unit is generated and a set of signals m is declared. ij As the calculation result of the matrix multiplication unit M.

[0061] According to the dimension of matrix A, a round-robin scheduling matrix R is generated, whose elements are R ij For example, if the matrix A is of dimension 6x6, then:

[0062]

[0063] In step S202, generating a rotation matrix J composed of the positive rotation values ​​and the corotation values ​​of three angles based on the matrix A and the matrix multiplication unit M includes the following steps:

[0064] S2021: Declare matrix h′, whose elements are h′ ij , the element h′ ij Element h ij Input to the multiplication unit M to obtain the first result Where h′ ij =h jj ;

[0065] S2022: The first result The value of is assigned to the element a of matrix A ij , and declare α i Declare β as the square of the norm corresponding to the ith odd column of matrix A. i is the square of the norm of the i-th even column of matrix A, and declares γ i is the product of the ith odd and ith even columns of matrix A;

[0066] In an exemplary embodiment, the Hessian matrix to be decomposed is a 6x6 matrix, then α1, α2, and α3 are declared to be the squares of the norms of the 1st, 3rd, and 5th columns of matrix A, respectively; β1, β2, and β3 are declared to be the squares of the norms of the 2nd, 4th, and 6th columns of matrix A, respectively; and γ1, γ2, and γ3 are declared to be the products of the 1st and 2nd columns, the 3rd and 4th columns, and the 5th and 6th columns of matrix A, respectively.

[0067] S2023: Element a ij , the declared element b ij Input to the multiplication unit M to obtain the second result And for α i , β i , γ i Update to get α′ i , β′ i ,γ′ i , where b ij =a ji ;

[0068] In an example embodiment, α′ i , β′ i ,γ′ i The updated result is:

[0069]

[0070]

[0071]

[0072] S2024: Based on alpha i , β i , γ i Generate the rotation matrix J composed of the positive rotation value and the cosine value of the three angles. The elements of the rotation matrix J are J ij .

[0073] The above data α′ i , β′ i ,γ′ i Substituting the following formula, we can get the positive rotation and cosine values ​​of the three angles to form the rotation matrix J, where the angles can be understood as the rotation angles.

[0074]

[0075]

[0076] In this embodiment, the rotation matrix J is composed of the positive rotation and cosine values ​​of the three angles, and the rotation matrix J is used to implement the rotation update of the matrix A.

[0077] In step S203, based on the matrix A, the rotation matrix J, the scheduling matrix R, and the matrix multiplication unit M, the matrix A is updated as follows:

[0078] The element a of matrix A ij , the element J of the rotation matrix J ij Input into the matrix multiplication unit M to obtain the third result The third result The element R of the scheduling matrix R ij Input into the matrix multiplication unit M to obtain the fourth result The fourth result Assign element a to matrix A ij To update the matrix A.

[0079] In this embodiment, based on matrix A, rotation matrix J, scheduling matrix R and matrix multiplication unit M, all column elements of matrix A are updated. Compared with the existing technology that can only update once per rotation, the update efficiency of matrix A is improved.

[0080] In step S204, the matrix J1 is updated according to the declared matrix J1, the rotation matrix J, the scheduling matrix R, and the matrix multiplication unit M as follows:

[0081] The element J1 of matrix J1 ij , the element J of the rotation matrix J ij Input into the multiplication unit M to obtain the fifth result

[0082] The fifth result The element R of the scheduling matrix R ij Input into the multiplication unit M to obtain the sixth result The sixth result Assign element J1 of matrix J1 ij To update the matrix J1.

[0083] In step S205, before obtaining the pose matrix P′, steps S202 to S204 need to be repeated to obtain the matrix A′ and the matrix J1′ after updating the matrix A and the matrix J1 a set number of times. The set number of times is determined based on the dimension of the Hessian matrix to be decomposed.

[0084] If cosθ1=cosθ2=cosθ3=1 or γ1=γ2=γ3≤μ, where μ is a preset value, the singular matrix is ​​judged. Otherwise, the process returns to steps S202 to S204 and is repeated for the set number of times before judging again until the above conditions are met.

[0085] In step S205, based on the matrix A′, the matrix J1′, the matrix multiplication unit M and the obtained gradient matrix G, the pose matrix P′ is obtained as follows:

[0086] S2051: Determine whether the matrix A′ is a singular matrix, and adjust the matrix A′ if the matrix A′ is a singular matrix;

[0087] Using the above example, we sort α′1, α′2, α′3, β′1, β′2, and β′3 using counting sort. Assume that after sorting, from largest to smallest, they are α′1, α′2, α′3, β′1, β′2, and β′3. Then, we have g1 = α′1 ÷ α′2, g2 = α′2 ÷ α′3, g3 = α′3 ÷ β′1, g4 = β′1 ÷ β′2, and g5 = β′2 ÷ β′3. Assuming that g1, g2, g3, g4, and g5 are all greater than the preset value δ, we proceed directly to pose matrix calculation. Assuming that g5 is less than the preset value δ, matrix A′ is singular and we need to set the sixth column of matrix A′ to 0 before continuing with pose matrix calculation.

[0088] In this embodiment, the accuracy of pose matrix calculation is improved by determining the singular matrix.

[0089] S2052: Divide each column element of the matrix A′ by the square of the corresponding norm to obtain the matrix A″. The elements of the matrix A″ are A″ ij ;

[0090] S2053: Declare matrix L, whose elements are L ij , the element A″ ij Transpose(A″ ij ) T , the element J1′ of the matrix J1′ ij Input to the matrix multiplication unit M to obtain the seventh result And the seventh result Assign the element L of matrix L ij To update the matrix L;

[0091] S2054: Declare matrix K, whose elements are K ij , the element L ij , h′ ij Input to the matrix multiplication unit M to get the eighth result And the eighth result Assign element K of matrix K ij To update the matrix K;

[0092] S2055: Declare matrix P, whose elements are P ij , the element K of matrix K ij , the element G of the gradient matrix G i1 Input to the matrix multiplication unit M to get the ninth result And the ninth result Assign element P of matrix P ij To update the matrix P, get the pose matrix P ′ .

[0093] In one embodiment, a positioning method based on a pose matrix is ​​proposed, which can be applied to smart terminals, mobile robots, smart cars, etc. for achieving positioning. Figure 3 As shown, the following steps are included:

[0094] S301: Generate a Hessian matrix and a gradient matrix based on the acquired map data and radar data;

[0095] S302: Determine the pose matrix P′ based on the pose matrix determination method in the above embodiment;

[0096] S303: Perform positioning based on the posture matrix P′.

[0097] In this embodiment, the posture matrix determination method in the above embodiment is used to improve the overall efficiency of obtaining the posture matrix, thereby improving the positioning efficiency.

[0098] In step S301, generating a Hessian matrix and a gradient matrix based on the acquired map data and radar data includes the following steps:

[0099] S3011: Traverse the received map point cloud data in parallel and perform rasterization on the map. Each grid has a grid ID.

[0100] In traditional rasterization processing, the map point cloud data is traversed serially, which is inefficient. Figure 4 As shown in FIG, by traversing the map point cloud data in parallel in multiple ways, the efficiency is higher.

[0101] S3012: Using the grid id as the address, store the centroid coordinate vector q and the covariance matrix vector ∑ in each grid into the corresponding BRAM.

[0102] S3013: Convert the received radar point cloud data into a grid, use the grid ID as the address, and generate the Hessian matrix and gradient matrix.

[0103] Assume that the initial transformation parameter p = (t x ,t y ,t z ,θ x ,θ y ,θ z ) T , and by (θ x ,θ y ,θ z ) Generate the transformation matrix T. If the initial transformation parameters cannot complete the point cloud fitting, the FPGA will update this parameter.

[0104] Known Euler angles (θ x ,θ y ,θ z ), the Euler transformation matrix T can be generated by the following equation. For the convenience of C x Instead of cosθ x ,S x Instead of sinθ x , and so on, we get:

[0105]

[0106] The radar point cloud data is converted to a map point cloud grid. Each grid is then traversed and the distance between the converted point cloud data and the grid is calculated. Grids that are too far away are eliminated to obtain grid IDs that meet the distance requirements.

[0107] Assume that the coordinates of the radar point cloud data are x i =(x,y,z), the point cloud coordinate after conversion is x′ i =(x′,y′,z′), then:

[0108]

[0109] According to the obtained normal distribution parameters and the centroid coordinate vector q and covariance matrix vector ∑ obtained from BRAM, the probability function score(p) of the radar point cloud data falling in the corresponding grid is obtained, where score(p) is determined by the following formula:

[0110]

[0111] According to the chain rule and the vector and matrix derivative formulas, score(p) is derived to obtain the gradient matrix, whose element values ​​are obtained by the following calculation formula.

[0112]

[0113] According to the calculation results of the above gradient matrix, we continue to calculate the second-order derivative of score(p) to obtain the Hessian matrix, whose element values ​​are obtained by the following calculation formula:

[0114]

[0115] In step S303, positioning based on the pose matrix P′ includes the following steps:

[0116] S3031: Normalize the pose matrix P′ to obtain a pose direction and a step length;

[0117] S3032: If the step length meets the set conditions, the coordinates corresponding to the pose matrix are the positioning coordinates; otherwise, the step length is iteratively optimized in the pose direction until the step length meets the set conditions.

[0118] If the step size is equal to 0 or less than the preset value β, the point cloud fitting is complete. At this point, the coordinates of the pose matrix are the accurate position of the mobile robot in the map. Otherwise, the step size is iteratively optimized in the pose direction until the step size is equal to 0 or less than the preset value.

[0119] The specific process of iterative optimization is as follows:

[0120] First, we need to optimize the step size. Assume the step size is step_cur, the maximum step size of each optimization is max_step, the minimum step size is min_step, and the optimized step size is step_nex. Then we have:

[0121] step_cur′=(max_step>step_cur)? step_cur:max_step

[0122] step_nex=(min_step>step_cur′)? min_step:step_cur′

[0123] Update the transformation parameter p according to the following formula, and then repeat the above steps until the point cloud fitting is completed.

[0124] p′=p+pose direction matrix*step_nex.

[0125] In one embodiment, a processor is provided having a computer program stored thereon, wherein when the computer program is executed by the processor, the steps of the above method are implemented. By way of example only, the processor may include a central processing unit (CPU), an application-specific integrated circuit (ASIC), an application-specific instruction set processor (ASIP), a graphics processing unit (GPU), a physical processing unit (PPU), a digital signal processor (DSP), a field programmable gate array (FPGA), a programmable logic device (PLD), a controller, a microcontroller unit, a reduced instruction set computer (RISC), a microprocessor, or the like, or any combination thereof.

[0126] In one embodiment, a mobile robot is provided. Figure 5 As shown, it includes the processor 501 in the above embodiment, and a mobile system 502 connected to the processor 501 and moving based on the processing result of the processor.

[0127] Those skilled in the art will appreciate that all or part of the processes in the above-mentioned embodiment methods can be implemented by instructing the relevant hardware through a computer program, and the computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the above-mentioned methods. Among them, any reference to memory, storage, database or other media used in the embodiments provided in this application may include at least one of non-volatile and volatile memory. Non-volatile memory may include read-only memory (ROM), magnetic tape, floppy disk, flash memory or optical memory, etc. Volatile memory may include random access memory (RAM) or external cache memory. As an illustration and not limitation, RAM can be in various forms, such as static random access memory (SRAM) or dynamic random access memory (DRAM).

[0128] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0129] The above-described embodiments merely represent several implementation methods of the present application. While the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that a person skilled in the art could make various modifications and improvements without departing from the spirit of the present application, all of which fall within the scope of protection of the present application. Therefore, the scope of protection of the present patent application shall be determined by the appended claims.

Claims

1. A positioning method based on a pose matrix, characterized in that: include: Generate Hessian matrix and gradient matrix based on the acquired map data and radar data; Generate a matrix multiplication unit M and a scheduling matrix R according to the dimensions of the declared matrix A; wherein the elements of the matrix A are The elements of the Hessian matrix to be decomposed determined; Based on the matrix A and the matrix multiplication unit M, a rotation matrix J consisting of the positive rotation values ​​and the cosine values ​​of the three angles is generated; Update the matrix A based on the matrix A, the rotation matrix J, the scheduling matrix R, and the matrix multiplication unit M; Update the matrix J1 according to the declared matrix J1, the rotation matrix J, the scheduling matrix R and the matrix multiplication unit M; After updating the matrix A and the matrix J1 for a set number of times, the matrix and matrix , if the matrix If the set conditions are met, then based on the matrix , the matrix , the matrix multiplication unit M and the obtained gradient matrix G, to obtain the pose matrix ; Based on the pose matrix Perform positioning.

2. The method according to claim 1, characterized in that The method of generating a rotation matrix J composed of the positive rotation values ​​and the corotation values ​​of three angles based on the matrix A and the matrix multiplication unit M includes: Declaration Matrix , whose elements are , the element ,element Input to the multiplication unit M to obtain the first result ,in, ; The first result The value of is assigned to the elements of matrix A , and declare is the square of the norm corresponding to the ith odd column of matrix A, and declare is the square of the norm of the i-th even column of matrix A, and declare is the product of the ith odd and ith even columns of matrix A; The elements , the declared element Input to the multiplication unit M to obtain the second result , and 、 、 Update to get 、 、 ,in ; based on 、 、 Generate the rotation matrix J composed of the positive rotation value and the cosine value of the three angles. The elements of the rotation matrix J are .

3. The method according to claim 1, characterized in that The updating of the matrix A based on the matrix A, the rotation matrix J, the scheduling matrix R, and the matrix multiplication unit M includes: The elements of matrix A , the elements of the rotation matrix J Input into the matrix multiplication unit M to obtain the third result , the third result , elements of the scheduling matrix R Input into the matrix multiplication unit M to obtain the fourth result , the fourth result Assign the elements of matrix A To update the matrix A.

4. The method according to claim 1, wherein The updating of the matrix J1 according to the declared matrix J1, the rotation matrix J, the scheduling matrix R, and the matrix multiplication unit M includes: The elements of matrix J1 , the elements of the rotation matrix J Input into the multiplication unit M to obtain the fifth result ; The fifth result , elements of the scheduling matrix R Input into the multiplication unit M to obtain the sixth result , the sixth result Assign the elements of matrix J1 To update the matrix J1.

5. The method according to claim 1, wherein The matrix based on , the matrix , the matrix multiplication unit M and the obtained gradient matrix G, determine the pose matrix include: Determine the matrix Is it a singular matrix and in the matrix For the case of a singular matrix, the matrix Make adjustments; The matrix Each column element of is divided by the square of the corresponding norm, and the matrix is ​​obtained. ,matrix The elements of ; Declare a matrix L whose elements are , the element Transpose Input to the matrix multiplication unit M to obtain the seventh result and the seventh result Assign the elements of matrix L To update the matrix L; Declare a matrix K whose elements are , the element Input to the matrix multiplication unit M to get the eighth result and the eighth result Assign elements of matrix K To update the matrix K; Declare a matrix P whose elements are , the elements of matrix K Elements of the gradient matrix G Input to the matrix multiplication unit M to get the ninth result and the ninth result Assign the elements of matrix P To update the matrix P, get the pose matrix .

6. The method according to claim 1, wherein The generating of the Hessian matrix and the gradient matrix based on the acquired map data and radar data includes: Traverse the received map point cloud data in parallel and rasterize the map. Each grid has a grid ID. Using the grid id as the address, the centroid coordinate vector of each grid and the covariance matrix vector Store it in the corresponding BRAM; Convert the received radar point cloud data into a grid, use the grid ID as the address, and generate the Hessian matrix and gradient matrix.

7. The method according to claim 1, characterized in that The pose matrix based on Positioning includes: For the pose matrix Perform normalization to obtain the pose direction and step length; If the step length meets the set conditions, the coordinates corresponding to the pose matrix are the positioning coordinates; otherwise, the step length is iteratively optimized in the pose direction until the step length meets the set conditions.

8. A processor having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

9. A mobile robot, characterized in that: The device comprises the processor according to claim 8, and a movement system connected to the processor and moving based on a processing result of the processor.

Images