位姿矩阵确定方法、定位方法、处理器和移动机器人与流程

1.本技术涉及计算机领域，特别是涉及一种位姿矩阵确定方法、定位方法、处理器和移动机器人。


背景技术：

2.基于激光雷达的slam定位算法被广泛应用于移动机器人领域，移动机器人在运动过程中需要对激光雷达每帧点云数据进行拟合，以实现地图匹配和精确定位。在地图匹配的点云拟合过程中，需要对位姿矩阵进行累计计算以获得地图配准的最小误差。
3.现有技术在位姿矩阵的获得过程中，在执行jacobi旋转时，尽管也是并行处理，但每次只操作一行元素，多行元素仍然是顺序执行，最后按照round-robin调度规则通过写bram的方式，来覆盖更新数据。因此，现有技术中旋转更新的方式效率低，从而无法实现快速的定位。


技术实现要素：

4.基于此，有必要针对上述技术问题，提供一种位姿矩阵确定方法、定位方法、处理器和移动机器人。
5.第一方面，本发明实施例提出一种位姿矩阵确定方法，包括：
6.根据所声明的矩阵a的维度，生成矩阵乘法单元m以及调度矩阵r；其中，所述矩阵a的元素a
ij
由待分解的hessian矩阵的元素h
ij
所确定；
7.基于所述矩阵a以及所述矩阵乘法单元m，生成三个角度的正旋值、余旋值所组成的旋转矩阵j；
8.基于所述矩阵a、所述旋转矩阵j、所述调度矩阵r以及所述矩阵乘法单元m，对所述矩阵a进行更新；
9.根据所声明的矩阵j1、所述旋转矩阵j、所述调度矩阵r以及所述矩阵乘法单元m，对所述矩阵j1进行更新；
10.在对所述矩阵a和所述矩阵j1更新设定次数后，得到矩阵a
′
和矩阵j1
′
，若所述矩阵a
′
满足设定条件，则基于所述矩阵a
′
、所述矩阵j1
′
、所述矩阵乘法单元m以及所获取的梯度矩阵g，得到位姿矩阵p
′
。
11.在一实施例中，所述基于所述矩阵a以及所述矩阵乘法单元m，生成三个角度的正旋值、余旋值所组成的旋转矩阵j包括：
12.声明矩阵h
′
，其元素为h
′
ij
，将元素h
′
ij
、元素h
ij
输入到所述乘法单元m，得到第一结果其中，h
′
ij
＝h
ji
；
13.将所述第一结果的值赋予矩阵a的元素a
ij
，并声明αi为矩阵a第i奇数列对应范数的平方，声明βi为矩阵a第i偶数列对应范数的平方，声明γi为矩阵a第i奇数与第i偶数列的乘积；
14.将元素a
ij
、所声明的元素b
ij
输入到所述乘法单元m，得到第二结果并对αi、βi、γi进行更新得到α
′i、β
′i、γ
′i，其中b
ij
＝a
ji
；
15.基于α
′i、β
′i、γ
′i生成三个角度的正旋值、余旋值所组成的旋转矩阵j，旋转矩阵j的元素为j
ij
。
16.在一实施例中，所述基于所述矩阵a、所述旋转矩阵j、所述调度矩阵r以及所述矩阵乘法单元m，对所述矩阵a进行更新包括：
17.将矩阵a的元素a
ij
、旋转矩阵j的元素j
ij
输入到所述矩阵乘法单元m中，得到第三结果将第三结果调度矩阵r的元素r
ij
输入到所述矩阵乘法单元m中得到第四结果将第四结果赋予矩阵a的元素a
ij
来对矩阵a进行旋转更新。
18.在一实施例中，所述根据所声明的矩阵j1、所述旋转矩阵j、所述调度矩阵r以及所述矩阵乘法单元m，对所述矩阵j1进行更新包括：
19.将矩阵j1的元素j1
ij
、旋转矩阵j的元素j
ij
输入到所述乘法单元m中，得到第五结果
20.将第五结果调度矩阵r的元素r
ij
输入到所述乘法单元m中，得到第六结果将第六结果赋予矩阵j1的元素j1
ij
来对矩阵j1进行更新。
21.在一实施例中，所述基于所述矩阵a
′
、所述矩阵j1
′
、所述矩阵乘法单元m以及所获取的梯度矩阵g，确定位姿矩阵p
′
包括：
22.确定矩阵a
′
是否为奇异矩阵，并在矩阵a
′
为奇异矩阵的情况下对所述矩阵a
′
进行调整；
23.将矩阵a
′
的各列元素均除以对应范数的平方，得到矩阵a
″
，矩阵a
″
的元素为a
″
ij
；
24.声明矩阵l，其元素为l
ij
，将元素a
″
ij
的转置(a
″
ij
)
t
、矩阵j1
′
的元素j1
′
ij
输入到所述矩阵乘法单元m，得到第七结果并将第七结果赋予矩阵l的元素l
ij
来对矩阵l进行更新；
25.声明矩阵k，其元素为k
ij
，将元素l
ij
、h
′
ij
输入到矩阵乘法单元m，得到第八结果并将第八结果赋予矩阵k的元素k
ij
来对矩阵k进行更新；
26.声明矩阵p，其元素为p
ij
，将矩阵k的元素k
ij
、梯度矩阵g的元素g
i1
输入到矩阵乘法单元m，得到第九结果并将第九结果赋予矩阵p的元素p
ij
来对矩阵p进行更新，得到位姿矩阵p
′
。
27.第二方面，本发明实施例提出一种基于位姿矩阵的定位方法，包括：
28.基于获取得到的地图数据和雷达数据，生成hessian矩阵和梯度矩阵；
29.基于第一方面所述的方法，确定位姿矩阵p
′
；
30.基于所述位姿矩阵p
′
进行定位。
31.在一实施例中，所述基于获取得到的地图数据和雷达数据，生成hessian矩阵和梯度矩阵包括：
32.并行遍历接收到的地图点云数据，对地图进行栅格化处理，每一个栅格拥有一个栅格id；
33.以栅格id作为地址，将每一个栅格内的质心坐标向量q以及协方差矩阵向量∑存入到对应的bram中；
34.将接收到的雷达点云数据转换到栅格中，以栅格id为地址，生成hessian矩阵和梯度矩阵。
35.在一实施例中，所述基于所述位姿矩阵p
′
进行定位包括：
36.对所述位姿矩阵p
′
进行归一化处理，得到位姿方向和步长；
37.若步长满足设定条件，则位姿矩阵所对应的坐标为定位坐标；反之，则在位姿方向上，对步长进行迭代优化，直到步长满足设定条件。
38.第三方面，本发明实施例提出一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行第一方面或第二方面所述的步骤。
39.第四方面，本发明实施例提出一种计算机可读存储介质，其上存储有计算机程序，所述处理器执行所述计算机程序时实现第一方面或第二方面所述的步骤。
40.上述方法、计算机设备和存储介质，通过矩阵乘法单元m实现矩阵a的更新以及矩阵j1的更新，有效简化了数据的复杂操作，从而提高了得到位姿矩阵的整体效率。多次调用的矩阵乘法单元m，可以通过时分复用技术使得电路满负荷运行，从而有效减少了计算资源的使用。
附图说明
41.图1为一个实施例位姿矩阵确定方法的应用环境示意图；
42.图2为一个实施例中位姿矩阵确定方法的流程示意图；
43.图3为一个实施例中基于位姿矩阵的定位方法的流程示意图；
44.图4为一个实施例中栅格化处理方法的示意图；
45.图5为一个实施例中移动机器人的结构示意图。
具体实施方式
46.为了使本技术的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本技术进行进一步详细说明。应当理解，此处描述的具体实施例仅用以解释本技术，并不用于限定本技术。
47.本技术提供的位姿矩阵确定方法，可以应用于如图1所示的应用环境中，其中，终端102与移动机器人104通信。终端102根据所声明的矩阵a的维度，生成矩阵乘法单元m以及调度矩阵r；其中，所述矩阵a的元素a
ij
由待分解的hessian矩阵的元素h
ij
所确定；基于所述矩阵a以及所述矩阵乘法单元m，生成三个角度的正旋值、余旋值所组成的旋转矩阵j；基于所述矩阵a、所述旋转矩阵j、所述调度矩阵r以及所述矩阵乘法单元m，对所述矩阵a进行更新；根据所声明的矩阵j1、所述旋转矩阵j、所述调度矩阵r以及所述矩阵乘法单元m，对所述矩阵j1进行更新；在对所述矩阵a和所述矩阵j1更新设定次数后，得到矩阵a
′
和矩阵j1
′
，若所述矩阵a
′
满足设定条件，则基于所述矩阵a
′
、所述矩阵j1
′
、所述矩阵乘法单元m以及所获取的梯度矩阵g，得到位姿矩阵p
′
。终端102再将位姿矩阵p
′
发送给移动机器人104，移动机
器人104基于位姿矩阵p
′
进行定位。
48.其中，终端102可以包括一个或以上处理器(例如，单芯片处理器或多芯片处理器)。仅作为示例，终端102可以包括中央处理单元(cpu)、专用集成电路(asic)、专用指令集处理器(asip)、图像处理单元(gpu)、物理运算处理单元(ppu)、数字信号处理器(dsp)、现场可编程门阵列(fpga)、可编程逻辑器件(pld)、控制器、微控制器单元、精简指令集计算机(risc)、微处理器等或其任意组合。
49.通信可以是或包括公共网络(例如，因特网)、专用网络(例如，局部区域网络(lan))、有线网络、无线网络(例如，802.11网络、wi-fi网络)、帧中继网络、虚拟专用网(vpn)、卫星网络、电话网络、路由器、集线器、交换机、服务器计算机和/或其任何组合。例如，网络可以包括电缆网络、有线网络、光纤网络、电信网络、内联网、无线局部区域网络(wlan)、城域网(man)、公共电话交换网络(pstn)、蓝牙网络、zigbee网络、近场通信(nfc)网络等，或其任意组合。
50.在一实施例中，如图2所示，提供了一种位姿矩阵确定方法，以该方法应用于图1中的应用环境为例进行说明，包括以下步骤：
51.s201：根据所声明的矩阵a的维度，生成矩阵乘法单元m以及调度矩阵r；其中，所述矩阵a的元素a
ij
由待分解的hessian矩阵的元素h
ij
所确定；
52.s202：基于所述矩阵a以及所述矩阵乘法单元m，生成三个角度的正旋值、余旋值所组成的旋转矩阵j；
53.s203：基于所述矩阵a、所述旋转矩阵j、所述调度矩阵r以及所述矩阵乘法单元m，对所述矩阵a进行更新；
54.s204：根据所声明的矩阵j1、所述旋转矩阵j、所述调度矩阵r以及所述矩阵乘法单元m，对所述矩阵j1进行更新；
55.s205：在对所述矩阵a和所述矩阵j1更新设定次数后，得到矩阵a
′
和矩阵j1
′
，若所述矩阵a
′
满足设定条件，则基于所述矩阵a
′
、所述矩阵j1
′
、所述矩阵乘法单元m以及所获取的梯度矩阵g，得到位姿矩阵p
′
。
56.在本实施例中，将现有技术中所采用的雅可比旋转、round-robin循环迭代等复杂操作简化为矩阵乘法，即通过矩阵乘法单元m实现矩阵a的更新以及矩阵j1的更新，有效简化了数据的复杂操作，从而提高了得到位姿矩阵的整体效率。而且整个方法不需要接受额外的控制命令即可独立完成计算，从而有效降低了系统的复杂性。
57.在本实施例中，多次调用的矩阵乘法单元m，可以通过时分复用技术使得电路满负荷运行，从而有效减少了计算资源的使用。
58.在本实施例中，整个确定方法没有中间数据需要缓存，因此不需要消耗额外的内存资源，同时也没有频繁读写内存带来的时间开销。
59.在步骤s201中，根据所声明的矩阵a的维度，生成矩阵乘法单元m以及调度矩阵r具体为：
60.根据矩阵a的维度，生成一个矩阵乘法单元，并声明一组信号m
ij
做为矩阵乘法单元m的运算结果。
61.根据矩阵a的维度，生成round-robin调度矩阵r，其各元素为r
ij
。例如：矩阵a是的维度为6x6，则：
[0062][0063]
在步骤s202中，基于所述矩阵a以及所述矩阵乘法单元m，生成三个角度的正旋值、余旋值所组成的旋转矩阵j包括以下步骤：
[0064]
s2021：声明矩阵h
′
，其元素为h
′
ij
，将元素h
′
ij
、元素h
ij
输入到所述乘法单元m，得到第一结果其中，h
′
ij
＝h
jj
；
[0065]
s2022：将所述第一结果的值赋予矩阵a的元素a
ij
，并声明αi为矩阵a第i奇数列对应范数的平方，声明βi为矩阵a第i偶数列对应范数的平方，声明γi为矩阵a第i奇数与第i偶数列的乘积；
[0066]
在一示例实施例中，待分解的hessian矩阵为6x6的矩阵，则声明α1、α2、α3分别为矩阵a第1、3、5列范数的平方，声明β1、β2、β3分别为矩阵a第2、4、6列范数的平方，声明γ1、γ2、γ3分别为矩阵a第1列和第2列、第3列和第4列、第5列和第6列的乘积。
[0067]
s2023：将元素a
ij
、所声明的元素b
ij
输入到所述乘法单元m，得到第二结果并对αi、βi、γi进行更新得到α
′i、β
′i、γ
′i，其中b
ij
＝a
ji
；
[0068]
在一示例实施例中，α
′i、β
′i、γ
′i的更新结果为：
[0069][0070][0071][0072]
s2024：基于αi、βi、γi生成三个角度的正旋值、余旋值所组成的旋转矩阵j，旋转矩阵j的元素为j
ij
。
[0073]
将上述数据α
′i、β
′i、γ
′i代入以下公式，得到三个角度的正旋、余旋值，组成旋转矩阵j，其中的角度可以理解为旋转角度。
[0074]
[0075][0076]
在本实施例中，通过三个角度的正旋、余旋值组成旋转矩阵j，利用旋转矩阵j来实现矩阵a的旋转更新。
[0077]
在步骤s203中，基于所述矩阵a、所述旋转矩阵j、所述调度矩阵r以及所述矩阵乘法单元m，对所述矩阵a进行更新具体为：
[0078]
将矩阵a的元素a
ij
、旋转矩阵j的元素j
ij
输入到所述矩阵乘法单元m中，得到第三结果将第三结果调度矩阵r的元素r
ij
输入到所述矩阵乘法单元m中得到第四结果将第四结果赋予矩阵a的元素a
ij
来对矩阵a进行旋转更新。
[0079]
在本实施例中，基于矩阵a、旋转矩阵j、调度矩阵r以及矩阵乘法单元m，实现矩阵a各列元素的全部更新，相比于现有技术中每次旋转只能更新一次，提高了矩阵a的更新效率。
[0080]
在步骤s204中，根据所声明的矩阵j1、所述旋转矩阵j、所述调度矩阵r以及所述矩阵乘法单元m，对所述矩阵j1进行更新具体为：
[0081]
将矩阵j1的元素j1
ij
、旋转矩阵j的元素j
ij
输入到所述乘法单元m中，得到第五结果
[0082]
将第五结果调度矩阵r的元素r
ij
输入到所述乘法单元m中，得到第六结果将第六结果赋予矩阵j1的元素j1
ij
来对矩阵j1进行更新。
[0083]
在步骤s205中，在得到位姿矩阵p
′
之前，还需要重复步骤s202～s204，在对所述矩阵a和所述矩阵j1更新设定次数后，得到矩阵a
′
和矩阵j1
′
。其中，设定次数是基于待分解的hessian矩阵的维度所确定的。
[0084]
如果此时cosθ1＝cosθ2＝cosθ3＝1或者γ1＝γ2＝γ3≤μ，其中μ为预设值。则进行奇异矩阵的判断，否则返回步骤s202～s204，再次执行设定次数后，再进行判断，直到上述条件满足。
[0085]
在步骤s205中，基于所述矩阵a
′
、所述矩阵j1
′
、所述矩阵乘法单元m以及所获取的梯度矩阵g，得到位姿矩阵p
′
具体为：
[0086]
s2051：确定矩阵a
′
是否为奇异矩阵，并在矩阵a
′
为奇异矩阵的情况下对所述矩阵a
′
进行调整；
[0087]
以上述示例为例，对α
′1、α
′2、α
′3、β
′1、β
′2、β
′3使用计数排序法进行排序。假设排序后从大到小为α
′1、α
′2、α
′3、β
′1、β
′2、β
′3。那么有g1＝α
′1÷
α
′2,g2＝α
′2÷
α
′3,g3＝α
′3÷
β
′1,g4＝β
′1÷
β
′2，g5＝β
′2÷
β
′3。假设g1、g2、g3、g4、g5都大于预设值δ，则直接位姿矩阵的计算。假设g5小于预设值δ，那么说明此时矩阵a
′
是奇异矩阵，需要将矩阵a
′
第6列元素写为0，然后
再执行位姿矩阵的计算。
[0088]
在本实施例中，通过对奇异矩阵的判断，以提高位姿矩阵计算的准确度。
[0089]
s2052：将矩阵a
′
的各列元素均除以对应范数的平方，得到矩阵a
″
，矩阵a
″
的元素为a
″
ij
；
[0090]
s2053：声明矩阵l，其元素为l
ij
，将元素a
″
ij
的转置(a
″
ij
)
t
、矩阵j1
′
的元素j1
′
ij
输入到所述矩阵乘法单元m，得到第七结果并将第七结果赋予矩阵l的元素l
ij
来对矩阵l进行更新；
[0091]
s2054：声明矩阵k，其元素为k
ij
，将元素l
ij
、h
′
ij
输入到矩阵乘法单元m，得到第八结果并将第八结果赋予矩阵k的元素k
ij
来对矩阵k进行更新；
[0092]
s2055：声明矩阵p，其元素为p
ij
，将矩阵k的元素k
ij
、梯度矩阵g的元素g
i1
输入到矩阵乘法单元m，得到第九结果并将第九结果赋予矩阵p的元素p
ij
来对矩阵p进行更新，得到位姿矩阵p
′
。
[0093]
在一实施例中，提出一种基于位姿矩阵的定位方法，该方法可以应用于实现定位的智能终端、移动机器人、智能汽车等，如图3所示，包括以下步骤：
[0094]
s301：基于获取得到的地图数据和雷达数据，生成hessian矩阵和梯度矩阵；
[0095]
s302：基于上述实施例中的位姿矩阵确定方法，确定位姿矩阵p
′
；
[0096]
s303：基于所述位姿矩阵p
′
进行定位。
[0097]
在本实施例中，利用上述实施例中的位姿矩阵确定方法提高了得到位姿矩阵的整体效率，从而提高了定位效率。
[0098]
在步骤s301中，基于获取得到的地图数据和雷达数据，生成hessian矩阵和梯度矩阵包括以下步骤：
[0099]
s3011：并行遍历接收到的地图点云数据，对地图进行栅格化处理，每一个栅格拥有一个栅格id。
[0100]
在传统栅格化处理时，是串行遍历地图点云数据，效率较低。在本实施例中，如图4所示，通过多路并行遍历地图点云数据，效率较高。
[0101]
s3012：以栅格id作为地址，将每一个栅格内的质心坐标向量q以及协方差矩阵向量∑存入到对应的bram中。
[0102]
s3013：将接收到的雷达点云数据转换到栅格中，以栅格id为地址，生成hessian矩阵和梯度矩阵。
[0103]
假设初始的变换参数p＝(t
x
,ty,tz,θ
x
,θy,θz)
t
，并由(θ
x
,θy,θz)生成转换矩阵t。如果初始变换参数无法使得点云拟合结束，则由fpga更新这个参数。
[0104]
已知欧拉角(θ
x
,θy,θz)，可以通过以下等式生成欧拉转换矩阵t。为了方便将c
x
代替cosθ
x
,s
x
代替sinθ
x
，依次类推，得到：
[0105][0106]
将雷达点云数据通过转换到地图点云栅格中。然后再遍历每一个栅格，计算转换
后的点云数据和栅格之间的距离。剔除距离较远的栅格，得到符合距离要求的栅格id。
[0107]
假设雷达点云数据坐标为xi＝(x,y,z)，转换之后的点云坐标为x
′i＝(x
′
,y
′
,z
′
)，那么有:
[0108][0109]
根据获取的正太分布参数以及从bram中获取的质心坐标向量q以及协方差矩阵向量∑，得到此时雷达点云数据落在相应栅格中的概率函数score(p)，其中score(p)由以下公式确定：
[0110][0111]
根据链式求导法则以及向量、矩阵求导公式对score(p)求导，得到梯度矩阵，其各元素值由以下计算公式得到。
[0112][0113]
根据上述梯度矩阵的计算结果，继续求score(p)的二阶导数，得到hessian矩阵，其各元素值由以下计算公式得到：
[0114][0115]
在步骤s303中，基于所述位姿矩阵p
′
进行定位包括以下步骤：
[0116]
s3031：对所述位姿矩阵p
′
进行归一化处理，得到位姿方向和步长；
[0117]
s3032：若步长满足设定条件，则位姿矩阵所对应的坐标为定位坐标；反之，则在位姿方向上，对步长进行迭代优化，直到步长满足设定条件。
[0118]
如果步长等于0或者小于预设值β，表示点云拟合结束。此时，位姿矩阵的坐标是移动机器人在地图中准确的位置。反之，则在位姿方向上，对步长进行迭代优化，直到步长等于0或者小于预设值。
[0119]
迭代优化的过程具体为：
[0120]
首先需要对步长进行优化。假设步长为step_cur，每次优化最大步长为max_step，最小步长为min_step，优化后的步长为step_nex。那么有：
[0121]
step_cur
′
＝(max_step》step_cur)？step_cur:max_step
[0122]
step_nex＝(min_step》step_cur
′
)？min_step:step_cur
′
[0123]
按照如下公式对变换参数p进行更新，然后重复上述步骤，直到点云拟合结束。
[0124]
p
′
＝p+位姿方向矩阵*step_nex。
[0125]
在一个实施例中，提供了一种处理器，其上存储有计算机程序，所述计算机程序被处理器执行时，实现上述方法的步骤。仅作为示例，处理器可以包括中央处理单元(cpu)、专用集成电路(asic)、专用指令集处理器(asip)、图像处理单元(gpu)、物理运算处理单元(ppu)、数字信号处理器(dsp)、现场可编程门阵列(fpga)、可编程逻辑器件(pld)、控制器、微控制器单元、精简指令集计算机(risc)、微处理器等或其任意组合。
[0126]
在一个实施例中，提供了一种移动机器人，如图5所示，包括上述实施例中的处理器501，以及与所述处理器501连接的基于处理器的处理结果进行移动的移动系统502。
[0127]
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本技术所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和易失性存储器中的至少一种。非易失性存储器可包括只读存储器(read-only memory，rom)、磁带、软盘、闪存或光存储器等。易失性存储器可包括随机存取存储器(random access memory，ram)或外部高速缓冲存储器。作为说明而非局限，ram可以是多种形式，比如静态随机存取存储器(static random access memory，sram)或动态随机存取存储器(dynamic random access memory，dram)等。
[0128]
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
[0129]
以上所述实施例仅表达了本技术的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本技术构思的前提下，还可以做出若干变形和改进，这些都属于本技术的保护范围。因此，本技术专利的保护范围应以所附权利要求为准。

技术特征：
1.一种位姿矩阵确定方法，其特征在于，包括：根据所声明的矩阵a的维度，生成矩阵乘法单元m以及调度矩阵r；其中，所述矩阵a的元素a
ij
由待分解的hessian矩阵的元素h
ij
所确定；基于所述矩阵a以及所述矩阵乘法单元m，生成三个角度的正旋值、余旋值所组成的旋转矩阵j；基于所述矩阵a、所述旋转矩阵j、所述调度矩阵r以及所述矩阵乘法单元m，对所述矩阵a进行更新；根据所声明的矩阵j1、所述旋转矩阵j、所述调度矩阵r以及所述矩阵乘法单元m，对所述矩阵j1进行更新；在对所述矩阵a和所述矩阵j1更新设定次数后，得到矩阵a
′
和矩阵j1
′
，若所述矩阵a
′
满足设定条件，则基于所述矩阵a
′
、所述矩阵j1
′
、所述矩阵乘法单元m以及所获取的梯度矩阵g，得到位姿矩阵p
′
。2.根据权利要求1所述的方法，其特征在于，所述基于所述矩阵a以及所述矩阵乘法单元m，生成三个角度的正旋值、余旋值所组成的旋转矩阵j包括：声明矩阵h
′
，其元素为h
′
ij
，将元素h
′
ij
、元素h
ij
输入到所述乘法单元m，得到第一结果其中，h
′
ij
＝h
ji
；将所述第一结果的值赋予矩阵a的元素a
ij
，并声明α
i
为矩阵a第i奇数列对应范数的平方，声明β
i
为矩阵a第i偶数列对应范数的平方，声明γ
i
为矩阵a第i奇数与第i偶数列的乘积；将元素a
ij
、所声明的元素b
ij
输入到所述乘法单元m，得到第二结果并对α
i
、β
i
、γ
i
进行更新得到α
′
i
、β
′
i
、γ
′
i
，其中b
ij
＝a
ji
；基于α
′
i
、β
′
i
、γ
′
i
生成三个角度的正旋值、余旋值所组成的旋转矩阵j，旋转矩阵j的元素为j
ij
。3.根据权利要求1所述的方法，其特征在于，所述基于所述矩阵a、所述旋转矩阵j、所述调度矩阵r以及所述矩阵乘法单元m，对所述矩阵a进行更新包括：将矩阵a的元素a
ij
、旋转矩阵j的元素j
ij
输入到所述矩阵乘法单元m中，得到第三结果将第三结果调度矩阵r的元素r
ij
输入到所述矩阵乘法单元m中得到第四结果将第四结果赋予矩阵a的元素a
ij
来对矩阵a进行旋转更新。4.根据权利要求1所述的方法，其特征在于，所述根据所声明的矩阵j1、所述旋转矩阵j、所述调度矩阵r以及所述矩阵乘法单元m，对所述矩阵j1进行更新包括：将矩阵j1的元素j1
ij
、旋转矩阵j的元素j
ij
输入到所述乘法单元m中，得到第五结果将第五结果调度矩阵r的元素r
ij
输入到所述乘法单元m中，得到第六结果将第六结果赋予矩阵j1的元素j1
ij
来对矩阵j1进行更新。
5.根据权利要求1所述的方法，其特征在于，所述基于所述矩阵a
′
、所述矩阵j1
′
、所述矩阵乘法单元m以及所获取的梯度矩阵g，确定位姿矩阵p
′
包括：确定矩阵a
′
是否为奇异矩阵，并在矩阵a
′
为奇异矩阵的情况下对所述矩阵a
′
进行调整；将矩阵a
′
的各列元素均除以对应范数的平方，得到矩阵a
″
，矩阵a
″
的元素为a
″
ij
；声明矩阵l，其元素为l
ij
，将元素a
″
ij
的转置(a
″
ij
)
t
、矩阵j1
′
的元素j1
′
ij
输入到所述矩阵乘法单元m，得到第七结果并将第七结果赋予矩阵l的元素l
ij
来对矩阵l进行更新；声明矩阵k，其元素为k
ij
，将元素l
ij
、h
′
ij
输入到矩阵乘法单元m，得到第八结果并将第八结果赋予矩阵k的元素k
ij
来对矩阵k进行更新；声明矩阵p，其元素为p
ij
，将矩阵k的元素k
ij
、梯度矩阵g的元素g
i1
输入到矩阵乘法单元m，得到第九结果并将第九结果赋予矩阵p的元素p
ij
来对矩阵p进行更新，得到位姿矩阵p
′
。6.一种基于位姿矩阵的定位方法，其特征在于，包括：基于获取得到的地图数据和雷达数据，生成hessian矩阵和梯度矩阵；基于权利要求1～5任一项所述的方法，确定位姿矩阵p
′
；基于所述位姿矩阵p
′
进行定位。7.根据权利要求6所述的方法，其特征在于，所述基于获取得到的地图数据和雷达数据，生成hessian矩阵和梯度矩阵包括：并行遍历接收到的地图点云数据，对地图进行栅格化处理，每一个栅格拥有一个栅格id；以栅格id作为地址，将每一个栅格内的质心坐标向量q以及协方差矩阵向量∑存入到对应的bram中；将接收到的雷达点云数据转换到栅格中，以栅格id为地址，生成hessian矩阵和梯度矩阵。8.根据权利要求6所述的方法，其特征在于，所述基于所述位姿矩阵p
′
进行定位包括：对所述位姿矩阵p
′
进行归一化处理，得到位姿方向和步长；若步长满足设定条件，则位姿矩阵所对应的坐标为定位坐标；反之，则在位姿方向上，对步长进行迭代优化，直到步长满足设定条件。9.一种处理器，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时，实现权利要求1至5中任一项，或权利要求6至8中任一项所述的方法的步骤。10.一种移动机器人，其特征在于，包括如权利要求9所述的处理器，以及与所述处理器连接的基于处理器的处理结果进行移动的移动系统。

技术总结
本申请涉及计算机领域，特别是涉及一种位姿矩阵确定方法、定位方法、处理器和移动机器人，该方法包括：根据所声明的矩阵A的维度，生成矩阵乘法单元M以及调度矩阵R；基于所述矩阵A以及所述矩阵乘法单元M，生成三个角度的正旋值、余旋值所组成的旋转矩阵J；对所述矩阵A进行更新；对所述矩阵J1进行更新；在对所述矩阵A和所述矩阵J1更新设定次数后，得到矩阵A


技术研发人员：何江 马亚飞 陈仕韬
受保护的技术使用者：舜宇光学（浙江）研究院有限公司
技术研发日：2022.01.26
技术公布日：2023/8/5