一种导航定位方法及相关装置与流程

本技术涉及机器人定位，尤其涉及一种导航定位方法及相关装置。

背景技术：

1、随着科技的进步，机器人已经能够在物流和仓储环境中执行复杂的搬运任务，这大大提高了工厂的工作效率。在这其中，卡尔曼滤波器在机器人和航天器的导航算法中得到了广泛应用。但在导航定位过程中，经典的状态变量(如当前坐标和角度位姿)在距离原点较远时，其泰勒方程的二阶项数值可能会变得较大。如果忽略了这些高阶项，可能会导致定位精度不足，而导航定位精度过低时，机器人无法精准地完成搬运任务，避免发生诸如取不到货物、货物放置位置不满足生产需求等生产事故，以及防止机器人撞坏工厂设备等造成的损失，无法满足实际生产需求。

2、为了解决这一问题，现有技术将经典卡尔曼滤波中的每个状态变量的误差作为新的状态变量。这样做的目的是确保状态变量始终保持在零点附近，从而使得泰勒方程的高阶项变得较小，可以被忽略；同时通过后端优化，对传感器获取的位姿信息进行处理，能够提供相较于直接输出传感器位姿信息更加平滑、精度更高的位姿信息。然而，目前的后端优化技术主要应用于gins组合导航、激光惯性里程计和视觉惯性里程计等。这些方法都是基于卡尔曼滤波的后端优化都是通过惯性测量单元(imu)进行预测的，必须将速度作为输入进行预测，计算量大，传感器的适用范围小。

3、本技术提出了一种新的解决方案，它不限制预测所使用的传感器，只需提供预测的位姿信息即可。与现有的基于imu预测的方法相比，本技术在状态量中剔除了速度、imu零偏噪声和重力等因素，并重新定义了状态量的运动方程，不仅减少了计算量，而且能够适用于更多的传感器和输入量，如轮式里程计、激光里程计、视觉里程计等，同时提高定位精度。

技术实现思路

1、本技术的目的在于一种导航定位方法及相关装置，用以适用于更多传感器而不仅仅是基于惯性测量单元(imu)进行位置预测，同时减少计算量，提高定位精度。

2、本技术的目的采用以下技术方案实现：

3、第一方面，本技术提供了一种导航定位方法，所述方法包括：

4、通过传感器获取定位装置当前位姿数据，所述当前位姿数据包括定位装置的三维位置坐标和角度朝向；

5、基于后端优化模型，对所述当前位姿数据进行处理获得处理结果；

6、根据所述处理结果获得定位装置的位置。

7、优选地，一种导航定位方法，所述后端优化模型为基于误差状态的卡尔曼滤波，所述后端优化模型包括预测部分和更新部分，通过所述预测部分和更新部分对当前位姿进行优化；所述预测部分通过当前位置对下一时刻位姿进行预测获得预测结果，所述更新部分根据新的位姿输入对预测结果进行修正获得修正后结果。

8、优选地，一种导航定位方法，所述预测部分的实现方法包括：

9、根据tk-1时刻的名义协方差，以及tk时刻传感器的输入位姿；获得tk时刻预测的名义状态量；所述预测的名义状态量为预测部分的输出位姿；

10、根据传感器数据和误差状态运动方程获得tk时刻的线性雅可比矩阵；根据线性雅可比矩阵、传感器坐标测量误差和角度测量误差、tk时刻的误差状态量以及tk-1时刻的名义协方差，获得预测tk时刻的名义协方差；

11、返回预测tk时刻名义状态量和名义协方差。

12、优选地，一种导航定位方法，所述根据tk-1时刻的名义状态量，以及tk时刻传感器的输入位姿；通过迭代计算，获得tk时刻预测的名义状态量；包括：

13、所述tk时刻名义状态量为xk：

14、xk＝[pk,rk]

15、

16、

17、其中，为tk时刻输入的传感器测量坐标；为tk时刻输入的传感器测量的角速度；[pk-1,rk-1]为tk-1时刻输出的名义状态量；为tk-1时刻输入的传感器测量坐标；exp运算是指李代数到李群的指数映射对应。

18、优选地，一种导航定位方法，所述根据tk时刻的名义状态量，获得tk时刻的线性雅可比矩阵；根据线性雅可比矩阵，获得tk时刻的误差状态向量；包括：

19、根据tk时刻的名义状态向量，获得tk时刻的线性雅可比矩阵；

20、

21、其中fk为tk时刻的线性雅可比矩阵，r为旋转状态量；exp运算是指李代数到李群的指数映射对应；

22、根据线性雅可比矩阵，传感器坐标测量误差和角度测量误差、tk时刻的误差状态向量以及tk-1时刻的名义协方差，获得预测tk时刻的名义协方差pk；

23、pk＝fkpk-1fkt+q

24、

25、其中，vpx、vpy、vpz为传感器x轴、y轴、z轴方向坐标测量噪声的方差；vwx、vwy、vwz为传感器绕x轴、y轴、z轴方向角速度测量噪声的方差；预测的传感器的固有坐标测量噪声在x轴、y轴、z轴相互不相关；nw为传感器角速度误差；np为传感器坐标测量误差；fkt为fk的转置矩阵；pk-1为tk-1时刻的名义协方差。

26、优选地，一种导航定位方法，所述更新部分的实现方法包括：

27、获取预测完成后当前时刻的名义状态量和名义协方差；

28、获取用于更新的传感器当前时刻的观测量，所述观测量包括当前观测坐标和观测角度朝向；

29、根据预测完成后当前时刻的名义状态量、名义协方差和用于更新的传感器的观测量误差获取当前卡尔曼增益矩阵；

30、通过当前卡尔曼增益矩阵获得误差状态量和更新后的名义协方差；

31、根据所述误差状态量获得更新后的名义状态量即机器人当前坐标和角度位姿。

32、优选地，一种导航定位方法，所述根据当前时刻的名义状态量、名义协方差和用于更新的传感器的观测量误差获取当前卡尔曼增益矩阵；包括：

33、k＝ppred(ppred+v)-1

34、

35、其中，v为观测误差矩阵，vopx、vopy、vopz为传感器x轴、y轴、z轴方向坐标观测量的误差；voθx、voθy、voθz为传感器x轴、y轴、z轴方向角度误差；ppred为预测完成后当前时刻的名义协方差。

36、优选地，一种导航定位方法，所述通过当前卡尔曼增益矩阵获得误差状态量和更新后的名义协方差probot；包括：

37、probot＝ppred-ppred(ppred+v)-1ppred

38、优选地，一种导航定位方法，所述根据所述误差状态量获得更新后的名义状态量即机器人当前坐标和角度位姿，包括：

39、xrobot＝[probot，rrobot]

40、probot＝ppred+δp

41、rrobot＝rpredexp(δθ)

42、[δp，δθ]＝k[pob-ppred，θob-θpred]

43、其中，xrobot为更新后的名义状态量；pob为当前观测坐标；ppred预测完成后当前时刻的坐标；θob为当前观测角度位姿旋转矩阵对应的李代数；θpred为预测完成后当前时刻角度位姿旋转矩阵对应的李代数；δp为坐标误差状态；δθ为旋转误差状态李代数。

44、另一方面，本技术提供一种定位装置，所述定位装置定位时执行权利上述任一导航定位方法。

45、本发明的有益效果至少包括如下几个方面：

46、传感器获取的原始位姿数据可能存在抖动或不稳定的情况，导致运动轨迹不平滑。通过后端优化模型的处理，可以对位姿数据进行平滑处理，使得运动轨迹更加平滑，减少了机器人在搬运任务中的震荡或突变现象，提高了运动的稳定性和准确性。该方法不限制预测所使用的传感器，可以适用于多种传感器，包括轮式里程计、激光里程计、视觉里程计等。这样可以根据具体应用场景选择合适的传感器，灵活应对不同的导航定位需求。相比于基于imu预测的方法，本技术只用输入坐标和角度朝向，剔除了速度、imu零偏噪声和重力等因素，并重新定义了状态量的运动方程。大大减少了计算量，提高了计算效率，提高定位的实时性和响应性；同时保证了计算精度，而且可以适用于多种传感器，并不限于惯性测量单元(imu)。

47、总而言之，该导航定位方法通过后端优化模型对传感器获取的位姿数据进行处理，能够提高定位精度、平滑运动轨迹，并适用于多种传感器，同时减少计算量。这样可以确保机器人在搬运任务中能够精准地完成工作，避免生产事故和设备损坏，满足实际生产需求。