CN116774213B - 一种无轨道运动机构导航方法及装置 - Google Patents

Abstract

本发明属于无线电导航技术领域，公开了一种无轨道运动机构导航方法及装置，无轨道运动机构导航装置包括：毫米波雷达，设于无轨道运动机构上；三角锥，与毫米波雷达相对设置，且与无轨道运动机构位于同一直线上；三轴加速度传感器，设于无轨道运动机构上；处理模块，与毫米波雷达和三轴加速度传感器相连，用于根据毫米波雷达检测到的无轨道运动机构相对于三角锥的偏移角度、距离以及三轴加速度传感器检测到的无轨道运动机构的运行速度、无轨道运动机构在x方向上的偏移计算得到调节因子，并根据调节因子调整无轨道运动机构的前进方向。本发明利用毫米波雷达和三轴加速度传感器同时纠偏，精度较高；整体安装无需破坏路面，简单方便。

Description

一种无轨道运动机构导航方法及装置

技术领域

本发明属于无线电导航技术领域，具体涉及一种无轨道运动机构导航方法及装置。

背景技术

在对安全性有一定要求的场合，越来越多的会采用沿轨道行走的运动机构来提高设备移动的可靠性，但轨道一般需要破坏地面进行安装，安装好的轨道需要定期维护，清理杂物。因此对于一些实用场景，只需要运动机构在一条直线上运动，但又未安装轨道时，如何使得运动机构在无轨道的情况下可以保证直线运行，是本发明需要解决的问题。

发明内容

本发明旨在至少在一定程度上解决上述技术问题。为此，本发明目的在于提供一种无轨道运动机构导航方法及装置。

本发明所采用的技术方案为：

一种无轨道运动机构导航装置，包括：

毫米波雷达，设于无轨道运动机构上；

三角锥，与毫米波雷达相对设置，且与无轨道运动机构位于同一直线上；

三轴加速度传感器，设于无轨道运动机构上；

处理模块，与毫米波雷达和三轴加速度传感器相连，用于根据毫米波雷达检测到的无轨道运动机构相对于三角锥的偏移角度、距离以及三轴加速度传感器检测到的无轨道运动机构的运行速度、无轨道运动机构在x方向上的偏移计算得到调节因子，并根据调节因子调整无轨道运动机构的前进方向。

优选地，调节因子为w，

，

其中，d为无轨道运动机构相对于三角锥的距离，d'为上一个时刻无轨道运动机构相对于三角锥的距离，θ为无轨道运动机构相对于三角锥的偏移角度，θ'为上一个时刻无轨道运动机构相对于三角锥的偏移角度，ω为平滑因子，dx为无轨道运动机构在x方向上的偏移，dx'为上一个时刻无轨道运动机构在x方向上的偏移；

无轨道运动机构左轮速度为V _l =V-（w*L/2），无轨道运动机构右轮速度为V_r =V+（w*L/2），其中L为左轮和右轮的距离，V为无轨道运动机构的运行速度。

优选地，处理模块包括学习单元，用于对无轨道运动机构的运行数据进行学习，在毫米波雷达遮挡时，控制无轨道运动机构停止或仅根据三轴加速度传感器数据预测位偏。

优选地，还包括用于调节无轨道运动机构前进方向的角度控制装置，角度控制装置能够分别控制无轨道运动机构左轮速度和无轨道运动机构右轮速度。

优选地，所述处理模块包括滤波单元，滤波单元用于对三轴加速度传感器数据和毫米波雷达数据进行滤波。

一种无轨道运动机构导航方法，包括步骤：

安装在无轨道运动机构上的毫米波雷达实时发送检测波，与无轨道运动机构位于同一直线上的三角锥反射检测波，毫米波雷达接收三角锥反射的回波信号，根据回波信号得到无轨道运动机构相对于三角锥的偏移角度和距离；

安装在无轨道运动机构上的三轴加速度传感器实时检测无轨道运动机构的运行速度和无轨道运动机构在x方向上的偏移；

通过检测到的无轨道运动机构相对于三角锥的偏移角度、距离以及无轨道运动机构的运行速度和无轨道运动机构在x方向上的偏移计算得到调节因子；

根据调节因子调整无轨道运动机构的前进方向。

优选地，调节因子为w，

，

其中，d为无轨道运动机构相对于三角锥的距离，d'为上一个时刻无轨道运动机构相对于三角锥的距离，θ为无轨道运动机构相对于三角锥的偏移角度，θ'为上一个时刻无轨道运动机构相对于三角锥的偏移角度，ω为平滑因子，dx为无轨道运动机构在x方向上的偏移，dx'为上一个时刻无轨道运动机构在x方向上的偏移；

无轨道运动机构左轮速度为V _l =V-（w*L/2），无轨道运动机构右轮速度为V_r =V+（w*L/2），其中L为左轮和右轮的距离，V为无轨道运动机构的运行速度。

优选地，还包括步骤：对无轨道运动机构的运行数据进行学习，在毫米波雷达遮挡时，控制无轨道运动机构停止或仅根据三轴加速度传感器数据预测位偏，完成无轨道运动机构导航。

优选地，根据调节因子控制无轨道运动机构内角度控制装置的运行，角度控制装置调节无轨道运动机构左轮和无轨道运动机构右轮的差速从而调整无轨道运动机构的前进方向。

优选地，对毫米波雷达波数据和三轴加速度传感器数据进行滤波，使用滤波后的毫米波雷达检测到的无轨道运动机构相对于三角锥的偏移角度、距离以及三轴加速度传感器检测到的无轨道运动机构的运行速度和无轨道运动机构在x方向上的偏移来计算调节因子。

本发明的有益效果为：

本发明所提供的无轨道运动机构导航方法及装置，利用毫米波雷达和三轴加速度传感器同时实现无轨道运动机构纠偏，精度较高；且在纠偏的基础上，不增加其他传感器即可实现防撞功能；在使用时若毫米波雷达出现故障时依然不影响无轨道运动机构的导航；整体安装无需破坏路面，简单方便。

附图说明

图1是本发明无轨道运动机构导航装置的原理图。

图2是本发明无轨门机的运行数据示意图。

具体实施方式

下面将结合本发明中的附图，对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

应当理解，还应当注意到实施例中，所出现的功能/动作可能与附图出现的顺序不同。例如，取决于所涉及的功能/动作，实际上可以实质上并发地执行，或者有时可以以相反的顺序来执行连续示出的两个图。

如图1所示，本实施例的无轨道运动机构为无轨门机，无轨门机包括机头，机头具有左轮和右轮，无轨道运动机构导航装置包括毫米波雷达、三角锥、三轴加速度传感器、处理模块和角度控制装置，角度控制装置用于分别控制机头左轮速度和机头右轮速度，调节机头左轮和机头右轮的差速从而调整机头的前进方向；毫米波雷达和三轴加速度传感器均设于无轨门机的机头上；三角锥与毫米波雷达相对设置，且与无轨门机的机头位于同一直线上；处理模块与毫米波雷达和三轴加速度传感器相连，用于通过毫米波雷达波数据和三轴加速度传感器数据来计算调节因子，并根据调节因子调整机头的前进方向实现导航，导航精度高。

具体的，毫米波雷达波数据包括毫米波雷达检测到的机头相对于三角锥的偏移角度和距离，三轴加速度传感器数据包括三轴加速度传感器检测到的机头的运行速度以及机头在x方向上的偏移，处理模块通过根据毫米波雷达检测到的以及三轴加速度传感器检测到的机头的运行速度、机头在x方向上的偏移计算得到调节因子，并根据调节因子调整机头的前进方向，实现无轨门机导航。

其中，调节因子为w，

，

其中，d为机头相对于三角锥的距离，d'为上一个时刻机头相对于三角锥的距离，θ为机头相对于三角锥的偏移角度，θ'为上一个时刻机头相对于三角锥的偏移角度；ω为平滑因子，ω一般设置为0.6-0.8；dx为机头在x方向上的偏移，dx'为上一个时刻机头在x方向上的偏移。在毫米波雷达未被遮挡时，w=d*sin(θ)* ω+(1-ω)*d' sin(θ')，当毫米波雷达被遮挡时，w=ω*dx+(1-ω)* dx'。

机头左轮速度为V _l =V-（w*L/2），机头右轮速度为V_r =V+（w*L/2），其中L为左轮和右轮的距离，V为机头的运行速度。

处理模块包括滤波单元和学习单元，滤波单元用于对三轴加速度传感器数据和毫米波雷达数据进行滤波，处理模块对滤波后的数据进行处理。学习单元用于对无轨门机的运行数据进行学习，在毫米波雷达遮挡时，可以控制机头停止，实现防撞功能，或者仅根据三轴加速度传感器数据预测位偏。具体的，如图2所示，学习单元通过LSTM模型对轨迹进行预测，使用前N个点的轨迹预测后1个点的偏移，依次循环，图2中横坐标的时间的单位为s，纵坐标加速度差值的单位为m/s²，其中较高的曲线是真实值，较低的曲线是预测值。

一种无轨道运动机构导航方法，采用上述的无轨道运动机构导航装置，包括步骤：

安装在无轨门机的机头上的毫米波雷达实时发送检测波，与无轨门机的机头位于同一直线上的三角锥反射检测波，毫米波雷达接收三角锥反射的回波信号，根据回波信号得到机头相对于三角锥的偏移角度和距离；同时，安装在无轨门机的机头上的三轴加速度传感器实时检测机头的运行速度和机头在x方向上的偏移。

对毫米波雷达波数据和三轴加速度传感器数据进行滤波，处理模块使用滤波后的毫米波雷达检测到的机头相对于三角锥的偏移角度、距离以及三轴加速度传感器检测到的机头的运行速度和机头在x方向上的偏移来计算调节因子，最后根据调节因子控制机头内角度控制装置的运行，角度控制装置调节机头左轮和机头右轮的差速从而调整机头的前进方向。

学习单元对无轨门机的运行数据进行学习，在毫米波雷达遮挡时，控制机头停止或仅根据三轴加速度传感器数据预测位偏，完成无轨门机导航。

在其他实施例中，无轨道运动机构可以为AGV小车或轮式移动机器人，当无轨道运动机构为轮式移动机器人时，毫米波雷达和三轴加速度传感器均设于轮式移动机器人的机身上。

本发明不局限于上述可选实施方式，任何人在本发明的启示下都可得出其他各种形式的产品，但不论在其形状或结构上作任何变化，凡是落入本发明权利要求界定范围内的技术方案，均落在本发明的保护范围之内。

Claims (8)

1.一种无轨道运动机构导航装置，其特征在于，包括：

毫米波雷达，设于无轨道运动机构上；

三角锥，与毫米波雷达相对设置，且与无轨道运动机构位于同一直线上；

三轴加速度传感器，设于无轨道运动机构上；

处理模块，与毫米波雷达和三轴加速度传感器相连，用于根据毫米波雷达检测到的无轨道运动机构相对于三角锥的偏移角度、距离以及三轴加速度传感器检测到的无轨道运动机构的运行速度、无轨道运动机构在x方向上的偏移计算得到调节因子，并根据调节因子调整无轨道运动机构的前进方向；

调节因子为w，

，

其中，d为无轨道运动机构相对于三角锥的距离，d'为上一个时刻无轨道运动机构相对于三角锥的距离，θ为无轨道运动机构相对于三角锥的偏移角度，θ'为上一个时刻无轨道运动机构相对于三角锥的偏移角度，ω为平滑因子，dx为无轨道运动机构在x方向上的偏移，dx'为上一个时刻无轨道运动机构在x方向上的偏移；

无轨道运动机构左轮速度为V _l =V-（w*L/2），无轨道运动机构右轮速度为V_r =V+（w*L/2），其中L为左轮和右轮的距离，V为无轨道运动机构的运行速度。

2.根据权利要求1所述的无轨道运动机构导航装置，其特征在于：处理模块包括学习单元，用于对无轨道运动机构的运行数据进行学习，在毫米波雷达遮挡时，控制无轨道运动机构停止或仅根据三轴加速度传感器数据预测位偏。

3.根据权利要求1所述的无轨道运动机构导航装置，其特征在于：还包括用于调节无轨道运动机构前进方向的角度控制装置，角度控制装置能够分别控制无轨道运动机构左轮速度和无轨道运动机构右轮速度。

4.根据权利要求1所述的无轨道运动机构导航装置，其特征在于：所述处理模块包括滤波单元，滤波单元用于对三轴加速度传感器数据和毫米波雷达数据进行滤波。

5.一种无轨道运动机构导航方法，其特征在于，包括步骤：

安装在无轨道运动机构上的毫米波雷达实时发送检测波，与无轨道运动机构位于同一直线上的三角锥反射检测波，毫米波雷达接收三角锥反射的回波信号，根据回波信号得到无轨道运动机构相对于三角锥的偏移角度和距离；

安装在无轨道运动机构上的三轴加速度传感器实时检测无轨道运动机构的运行速度和无轨道运动机构在x方向上的偏移；

通过检测到的无轨道运动机构相对于三角锥的偏移角度、距离以及无轨道运动机构的运行速度和无轨道运动机构在x方向上的偏移计算得到调节因子；

根据调节因子调整无轨道运动机构的前进方向；

调节因子为w，

，

其中，d为无轨道运动机构相对于三角锥的距离，d'为上一个时刻无轨道运动机构相对于三角锥的距离，θ为无轨道运动机构相对于三角锥的偏移角度，θ'为上一个时刻无轨道运动机构相对于三角锥的偏移角度，ω为平滑因子，dx为无轨道运动机构在x方向上的偏移，dx'为上一个时刻无轨道运动机构在x方向上的偏移；

无轨道运动机构左轮速度为V _l =V-（w*L/2），无轨道运动机构右轮速度为V_r =V+（w*L/2），其中L为左轮和右轮的距离，V为无轨道运动机构的运行速度。

6.根据权利要求5所述的无轨道运动机构导航方法，其特征在于，还包括步骤：对无轨道运动机构的运行数据进行学习，在毫米波雷达遮挡时，控制无轨道运动机构停止或仅根据三轴加速度传感器数据预测位偏，完成无轨道运动机构导航。

7.根据权利要求5所述的无轨道运动机构导航方法，其特征在于：根据调节因子控制无轨道运动机构内角度控制装置的运行，角度控制装置调节无轨道运动机构左轮和无轨道运动机构右轮的差速从而调整无轨道运动机构的前进方向。

8.根据权利要求5所述的无轨道运动机构导航方法，其特征在于：对毫米波雷达波数据和三轴加速度传感器数据进行滤波，使用滤波后的毫米波雷达检测到的无轨道运动机构相对于三角锥的偏移角度、距离以及三轴加速度传感器检测到的无轨道运动机构的运行速度和无轨道运动机构在x方向上的偏移来计算调节因子。