【技术实现步骤摘要】
本申请涉及机器人定位，尤其涉及一种导航定位方法及相关装置。
技术介绍
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为当前观测角度本文档来自技高网...
【技术保护点】
1.一种导航定位方法，其特征在于，所述方法包括：
2.根据权利要求1所述的导航定位方法，其特征在于，所述后端优化模型为基于误差状态的卡尔曼滤波，所述后端优化模型包括预测部分和更新部分，通过所述预测部分和更新部分对当前位姿进行优化；所述预测部分通过当前位置对下一时刻位姿进行预测获得预测结果，所述更新部分根据新的位姿输入对预测结果进行修正获得修正后结果。
3.根据权利要求2所述的导航定位方法，其特征在于，所述预测部分的实现方法包括：
4.根据权利要求3所述的导航定位方法，其特征在于，所述根据tk-1时刻的名义状态量，以及tk时刻传感器的输入位姿；通过迭代计算，获得tk时刻预测的名义状态量；包括：
5.根据权利要求3所述的导航定位方法，其特征在于，所述根据tk时刻的名义状态量，获得tk时刻的线性雅可比矩阵；根据线性雅可比矩阵，获得tk时刻的误差状态向量；包括：
6.根据权利要求2所述的导航定位方法，其特征在于，所述更新部分的实现方法包括：
7.根据权利要求6所述的导航定位方法，其特征在于，所述根据当前时刻的名义状态
8.根据权利要求7所述的导航定位方法，其特征在于，所述通过当前卡尔曼增益矩阵获得误差状态量和更新后的名义协方差Probot；包括：
9.根据权利要求7所述的导航定位方法，其特征在于，所述根据所述误差状态量获得更新后的名义状态量即机器人当前坐标和角度位姿，包括：
10.一种定位装置，其特征在于，所述定位装置定位时执行权利要求1～9所述的导航定位方法。
...【技术特征摘要】
1.一种导航定位方法，其特征在于，所述方法包括：
2.根据权利要求1所述的导航定位方法，其特征在于，所述后端优化模型为基于误差状态的卡尔曼滤波，所述后端优化模型包括预测部分和更新部分，通过所述预测部分和更新部分对当前位姿进行优化；所述预测部分通过当前位置对下一时刻位姿进行预测获得预测结果，所述更新部分根据新的位姿输入对预测结果进行修正获得修正后结果。
3.根据权利要求2所述的导航定位方法，其特征在于，所述预测部分的实现方法包括：
4.根据权利要求3所述的导航定位方法，其特征在于，所述根据tk-1时刻的名义状态量，以及tk时刻传感器的输入位姿；通过迭代计算，获得tk时刻预测的名义状态量；包括：
5.根据权利要求3所述的导航定位方法，其特征在于，所述根据tk时刻的名义状态量，...
【专利技术属性】
技术研发人员：刘旺，庄文密，
申请(专利权)人：苏州艾吉威机器人有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。