位姿调整方法、位姿调整装置、电子设备及可读存储介质制造方法及图纸

本申请提供一种位姿调整方法、位姿调整装置、电子设备及可读存储介质。该方法包括：获取移动装置在上一时刻的坐标及噪声参数，上一时刻的坐标及噪声参数通过卡尔曼滤波模型预测得到，上一时刻为与当前时刻相邻的上一时刻；通过若干测量模块获取移动装置在当前时刻的若干测量坐标；基于若干测量坐标、上一时刻的坐标及上一时刻的噪声参数，通过卡尔曼滤波模型预测移动装置在当前时刻的坐标；根据当前时刻的坐标与移动装置的预设轨迹，调整移动装置在当前时刻的位姿。本申请提供的位姿调整方法可有效提高定位精度。可有效提高定位精度。可有效提高定位精度。

【技术实现步骤摘要】
位姿调整方法、位姿调整装置、电子设备及可读存储介质


[0001]本专利技术涉及定位
，具体涉及一种位姿调整方法、位姿调整装置、电子设备及计算机可读存储介质。

技术介绍

[0002]机器人实时定位技术是机器人实现自主导航的关键，其使得机器人应用于各种自动化场景，例如农业自动化场景、物流自动化场景。然而，在机器人执行作业的过程中，由于各种使用场景中的环境因素，比如天气影响，路上的障碍物，地面颠簸等，导致实时定位有误差。如此，将极大影响机器人定位的精度。

技术实现思路

[0003]鉴于此，有必要提供一种位姿调整方法、电子装置、移动装置及计算机可读存储介质，以提定位精度。
[0004]本申请第一方面提供一种位姿调整方法，应用于移动装置。该位姿调整方法包括：获取移动装置在上一时刻的坐标及噪声参数；上一时刻的坐标及噪声参数通过卡尔曼滤波模型预测得到；上一时刻为与当前时刻相邻的上一时刻。通过若干测量模块获取移动装置在当前时刻的若干测量坐标；基于若干测量坐标、上一时刻的坐标及上一时刻的噪声参数，通过卡尔曼滤波模型预测移动装置在当前时刻的坐标及噪声参数；根据当前时刻的坐标与移动装置的预设轨迹，调整移动装置在当前时刻的位姿。上述方案通过将基于卡尔曼滤波模型得到的上一时刻的坐标及噪声参数，结合当前时刻的若干测量坐标，以通过卡尔曼滤波模型预测移动装置的当前时刻的坐标，以较大程度地降低外界环境干扰、传感器干扰，从而获取较准确的移动装置的当前时刻的坐标。
[0005]根据本专利技术的一个实施例，卡尔曼滤波模型包括坐标更新模型及噪声更新模型，基于若干测量坐标、上一时刻的坐标及上一时刻的噪声参数，通过卡尔曼滤波模型预测移动装置在当前时刻的坐标包括：基于若干测量坐标、上一时刻的坐标及上一时刻的噪声参数，以分别根据坐标更新模型及噪声更新模型输出对应的计算参数；基于若干测量坐标、上一时刻的坐标、上一时刻的噪声参数及基于噪声更新模型输出的计算参数，根据坐标更新模型得到当前时刻的坐标；基于若干测量坐标、上一时刻的坐标、上一时刻的噪声参数及基于坐标更新模型输出的计算参数，根据噪声更新模型得到当前时刻的噪声参数。上述方案通过在卡尔曼滤波模型中引入坐标更新模型及噪声更新模型，并使噪声参数实时动态地参与进对当前时刻的坐标的预测运算中，以进一步提高基于卡尔曼滤波模型预测得到的当前时刻的坐标的准确度；
[0006]且执行根据当前时刻的坐标与移动装置的预设轨迹，调整移动装置在当前时刻的位姿之后，位姿调整方法还包括：
[0007]将当前时刻的坐标和噪声参数更新为上一时刻的坐标及噪声参数。
[0008]根据本专利技术的一个实施例，噪声参数包括观测噪声参数及状态噪声参数，观测噪
声参数表征移动装置的外界工作环境引起的噪声，状态噪声参数表征测量模块的误差引起的噪声。上述方案通过引入观测噪声参数及状态噪声参数，以通过卡尔曼滤波模型实现对观测噪声参数及状态噪声参数的实时动态预测，以将预测得到的当前时刻的观测噪声参数及状态噪声参数参与对当前时刻的坐标的预测，以解决多传感器融合定位方法中状态估计受外界不确定因素的影响而导致传统卡尔曼滤波发散的问题，同时还可以解决各传感器无法及时消除累积误差从而无法实现连续无缝导航的问题。
[0009]根据本专利技术的一个实施例，状态噪声参数包括状态噪声均值及状态噪声方差，观测噪声参数包括观测噪声均值及观测噪声方差；噪声更新模型包括如下方程：
[0010]q
k
＝(1
‑
d
k
)q
k
‑1+d
k
(X
k
‑
AX
k
‑1)；
[0011][0012]r
k
＝(1
‑
d
k
)r
k
‑1+d
k
(Z
k
‑
HX
k∨k
‑1)；
[0013][0014]其中，q
k
为移动装置在当前时刻的状态噪声的均值；d
k
＝(1
‑
b)/(1
‑
b
k+1
)，其中b为遗忘因子，其范围为0＜b＜1；q
k
‑1为移动装置在上一时刻的状态噪声的均值；X
k
为移动装置在当前时刻的坐标；A为状态转移矩阵；X
k
‑1为上一时刻的坐标；Q
k
为当前时刻的状态噪声的方差；Q
k
‑1为上一时刻的状态噪声的方差；K
k
为卡尔曼增益；e
k
为当前时刻的坐标X
k
与当前时刻的具有误差的坐标X
k∨k
‑1之间的误差；P
k
为当前时刻的协方差；P
k
‑1为上一时刻的协方差；r
k
为当前时刻的观测噪声的均值；r
k
‑1为上一时刻的观测噪声的均值；Z
k
为移动装置的观测数据；H为观测矩阵；X
k∨k
‑1为移动装置在当前时刻未根据当前时刻的状态噪声的方差进行更新的具有误差的坐标；R
k
为当前时刻的观测噪声的方差；R
k
‑1为上一时刻的观测噪声的方差；P
k∨k
‑1为移动装置在当前时刻未根据观测矩阵进行更新的具有误差的协方差。
[0015]根据本专利技术的一个实施例，若干测量模块包括UWB模块、IMU模块及里程计模块，若干测量坐标包括第一测量坐标、第二测量坐标及第三测量坐标。如此，位姿调整方法还包括：通过UWB模块获取移动装置在当前时刻的第一测量坐标；通过IMU模块获取移动装置在当前时刻的第二测量坐标；通过里程计模块获取移动装置在当前时刻的第三测量坐标。上述方案通过UWB模块、IMU模块及里程计模块获取移动装置在当前时刻的若干测量坐标，以解决位姿调整方法中由于单一传感器所带来的鲁棒性差的问题。
[0016]根据本专利技术的一个实施例，通过IMU模块获取移动装置在当前时刻的第二测量坐标包括：通过IMU模块获取移动装置的位姿数据；基于滑动平均滤波算法对位姿数据进行滤波；基于航迹推算算法对滤波后的位姿数据进行处理，以得到第二测量坐标。上述方案先通过滑动平均滤波算法对IMU模块获取的位姿数据滤波，以降低干扰；再通过航迹推算算法对位姿数据进行处理，以获得准确度较高的第二测量坐标。
[0017]根据本专利技术的一个实施例，通过UWB模块获取移动装置在当前时刻的第一测量坐标包括：通过UWB模块与若干基站通信，以请求测距信息，且当与UWB模块通信的基站的数量大于等于预设数量时，UWB模块与距离超过预设阈值的基站断开通信；根据若干基站返回的测距信息确定第一测量坐标。上述方案通过UWB模块与距离超过预设阈值的基站断开通信，从而扩大移动装置的移动范围。
[0018]本申请第二方面提供一种位姿本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种位姿调整方法，应用于移动装置，其特征在于，所述位姿调整方法包括：获取所述移动装置在上一时刻的坐标及噪声参数，所述上一时刻的坐标及噪声参数通过卡尔曼滤波模型预测得到，所述上一时刻为与当前时刻相邻的上一时刻；通过若干测量模块获取所述移动装置在所述当前时刻的若干测量坐标；基于若干所述测量坐标、所述上一时刻的坐标及所述上一时刻的噪声参数，通过所述卡尔曼滤波模型预测所述移动装置在所述当前时刻的坐标；根据所述当前时刻的坐标与所述移动装置的预设轨迹，调整所述移动装置在所述当前时刻的位姿。2.如权利要求1所述的位姿调整方法，其特征在于，所述卡尔曼滤波模型包括坐标更新模型及噪声更新模型，所述基于若干所述测量坐标、所述上一时刻的坐标及所述上一时刻的噪声参数，通过所述卡尔曼滤波模型预测所述移动装置在所述当前时刻的坐标包括：基于若干所述测量坐标、所述上一时刻的坐标及所述上一时刻的噪声参数，以分别根据所述坐标更新模型及所述噪声更新模型输出对应的计算参数；基于若干所述测量坐标、所述上一时刻的坐标、所述上一时刻的噪声参数及基于所述噪声更新模型输出的计算参数，根据所述坐标更新模型得到所述当前时刻的坐标；基于若干所述测量坐标、所述上一时刻的坐标、所述上一时刻的噪声参数及基于所述坐标更新模型输出的计算参数，根据所述噪声更新模型得到所述当前时刻的噪声参数；且所述根据所述当前时刻的坐标与所述移动装置的预设轨迹，调整所述移动装置在所述当前时刻的位姿之后，所述位姿调整方法还包括：将所述当前时刻的坐标和噪声参数更新为所述上一时刻的坐标及噪声参数。3.如权利要求1所述的位姿调整方法，其特征在于：所述噪声参数包括观测噪声参数及状态噪声参数，所述观测噪声参数表征所述移动装置的外界工作环境引起的噪声，所述状态噪声参数表征所述测量模块的误差引起的噪声。4.如权利要求3所述的位姿调整方法，其特征在于，所述状态噪声参数包括状态噪声均值及状态噪声方差，所述观测噪声参数包括观测噪声均值及观测噪声方差；所述噪声更新模型包括如下方程：q
k
＝(1
‑
d
k
)q
k
‑1+d
k
(X
k
‑
AX
k
‑1)；r
k
＝(1
‑
d
k
)r
k
‑1+d
k
(Z
k
‑
HX
k∨k
‑1)；其中，q
k
为所述移动装置在当前时刻的状态噪声的均值；d
k
＝(1
‑
b)/(1
‑
b
k+1
)，其中b为遗忘因子，其范围为0<b<1；q
k
‑1为所述移动装置在上一时刻的状态噪声的均值；X
k
为所述移动装置在当前时刻的坐标；A为状态转移矩阵；X
k
‑1为所述上一时刻的坐标；Q
k
为所述当前时刻的状态噪声的方差；Q
k

【专利技术属性】
技术研发人员：吴迪，赵文泉，张琪昌，罗旭彬，
申请(专利权)人：丰疆智能深圳有限公司，
类型：发明
国别省市：