CN110764506A - 移动机器人的航向角融合方法、装置和移动机器人 - Google Patents

移动机器人的航向角融合方法、装置和移动机器人 Download PDF

Info

Publication number
CN110764506A
CN110764506A CN201911072969.9A CN201911072969A CN110764506A CN 110764506 A CN110764506 A CN 110764506A CN 201911072969 A CN201911072969 A CN 201911072969A CN 110764506 A CN110764506 A CN 110764506A
Authority
CN
China
Prior art keywords
angle
imu
course
uwb
angular velocity
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201911072969.9A
Other languages
English (en)
Other versions
CN110764506B (zh
Inventor
黄云龙
陈雪峰
韩定
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangdong Bozhilin Robot Co Ltd
Original Assignee
Guangdong Bozhilin Robot Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guangdong Bozhilin Robot Co Ltd filed Critical Guangdong Bozhilin Robot Co Ltd
Priority to CN201911072969.9A priority Critical patent/CN110764506B/zh
Publication of CN110764506A publication Critical patent/CN110764506A/zh
Application granted granted Critical
Publication of CN110764506B publication Critical patent/CN110764506B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S3/00—Direction-finders for determining the direction from which infrasonic, sonic, ultrasonic, or electromagnetic waves, or particle emission, not having a directional significance, are being received
    • G01S3/02—Direction-finders for determining the direction from which infrasonic, sonic, ultrasonic, or electromagnetic waves, or particle emission, not having a directional significance, are being received using radio waves
    • G01S3/14—Systems for determining direction or deviation from predetermined direction
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS

Abstract

本申请涉及移动载体位姿估计技术领域，尤其涉及一种移动机器人的航向角融合方法、装置和移动机器人，包括获取移动机器人的IMU测量系统检测的IMU初始航向角和IMU行进航向角，以及UWB测量系统检测的UWB行进航向角；根据所述IMU初始航向角和所述UWB行进航向角之间的差值确定初始修正偏置角，并将所述IMU行进航向角的天向角速度与预设角速度阈值进行比对；若所述天向角速度小于所述预设角速度阈值，则获取在小于所述预设角速度阈值的持续时间段内的动态修正偏置角，并根据所述初始修正偏置角和所述动态修正偏置角确定实时修正偏置角；本方案的融合过程较为简便，计算结果较为稳定可靠，能够保证移动机器人在室内封闭环境下获取更为可靠、准确的航向信息。

Description

移动机器人的航向角融合方法、装置和移动机器人
技术领域
本申请涉及移动载体位姿估计技术领域，尤其涉及一种移动机器人的航向角融合方法、装置和移动机器人。
背景技术
移动机器人在室内进行移动操作时，均依靠其搭载的移动载体位姿估计技术实现，而航向估计技术，作为体位姿估计技术中极为重要的一环，它提供了必要的航向反馈，实现了移动机器人的精准控制。
目前，室内航向定位多采用UWB+IMU(超宽带+惯性测量单元)融合定位技术，二者融合使用能够提供理想的航向角，但是由于IMU短期使用精度较高，IMU长期使用精度较高，融合方式的不同会直接影响航向结果，现有的融合处理方式较为复杂、融合不稳定，且融合效果较为不理想。
发明内容
本申请的目的旨在至少能解决上述的技术缺陷之一，特别是现有技术中室内航向融合处理方式操作复杂、融合不稳定，且融合效果不佳的技术缺陷。
本申请提供了一种移动机器人的航向角融合方法，其包括如下步骤：
获取移动机器人的IMU测量系统检测的IMU初始航向角和IMU行进航向角，以及UWB测量系统检测的UWB行进航向角；
根据所述IMU初始航向角和所述UWB行进航向角之间的差值确定初始修正偏置角，并将所述IMU行进航向角的天向角速度与预设角速度阈值进行比对；
若所述天向角速度小于所述预设角速度阈值，则获取在小于所述预设角速度阈值的持续时间段内的动态修正偏置角，并根据所述初始修正偏置角和所述动态修正偏置角确定实时修正偏置角；
根据所述实时修正偏置角和所述IMU行进航向角生成基准航向角。
在一个实施例中，本申请的移动机器人的航向角融合方法还包括：
将所述实时修正偏置角作为下一持续时间段的初始修正偏置角。
在一个实施例中，本申请的移动机器人的航向角融合方法还包括：
构建卡尔曼滤波模型，并根据所述移动机器人当前的位置、速度和航向确定估计值；
将所述基准航向角作为所述卡尔曼滤波模型的观测值，并根据所述估计值与所述观测值之间的差值，得到测量误差；
根据所述测量误差和所述基准航向角计算得到所述移动机器人的最优航向角。
在一个实施例中，所述获取移动机器人的IMU测量系统检测的IMU初始航向角的步骤，包括：
初始化IMU测量系统，获取初始时刻所述IMU测量系统检测的移动机器人在载体坐标系下的角速度信息；
对所述载体坐标系下的角速度信息进行姿态转换，得到大地坐标系下的角速度信息；
根据所述大地坐标系下的角速度信息计算得到IMU初始航向角。
在一个实施例中，所述获取移动机器人的IMU测量系统检测的IMU行进航向角的步骤，包括：
获取初始时刻至当前时间段内所述IMU测量系统检测的移动机器人在载体坐标系下的角速度信息；
对所述载体坐标系下的角速度信息进行姿态转换，得到大地坐标系下的角速度信息；
根据所述大地坐标系下的角速度信息以及所述IMU初始航向角计算得到IMU行进航向角。
在一个实施例中，所述获取UWB测量系统检测的UWB行进航向角的步骤，包括：
获取安装在所述移动机器人表面且与所述移动机器人航向一致的相邻两组UWB标签的位置信息；
根据所述位置信息确定相邻两组所述UWB标签的航向矢量，并根据所述航向矢量计算得到UWB行进航向角。
在一个实施例中，所述获取在小于所述预设角速度阈值的持续时间段内的动态修正偏置角的步骤，包括：
获取小于所述预设角速度阈值的持续时间段内的IMU航向均值和UWB航向均值，根据所述IMU航向均值和所述UWB航向均值确定动态修正偏置角。
在一个实施例中，所述获取小于所述预设角速度阈值的持续时间段内的IMU航向均值和UWB航向均值的步骤，包括：
获取小于所述预设角速度阈值的持续时间段内的时间帧信息；其中，所述时间帧信息包括所述持续时间段内的帧数、每一帧对应的实时IMU行进航向角和实时UWB行进航向角；
根据所述帧数和所述实时IMU行进航向角计算得到初始IMU航向均值，根据所述帧数和所述实时UWB行进航向角计算得到初始UWB航向均值。
在一个实施例中，根据所述帧数和所述实时UWB行进航向角计算得到初始UWB航向均值的步骤之后，还包括：
更新所述帧数，并将更新后的帧数与预设帧数阈值进行比对；
当所述更新后的帧数等于所述预设帧数阈值时，则根据所述更新后的帧数重新计算所述初始IMU航向均值和所述初始UWB航向均值，得到更新后的IMU航向均值和UWB航向均值。
在一个实施例中，根据所述初始修正偏置角和所述动态修正偏置角确定实时修正偏置角的步骤，包括：
根据所述更新后的UWB航向均值与所述更新后的IMU航向均值之间的差值确定动态修正偏置角；
获取输入的权值参数，根据所述权值参数、所述初始修正偏置角和所述动态修正偏置角计算得到实时修正偏置角。
在一个实施例中，将所述IMU行进航向角的天向角速度与预设角速度阈值进行比对的步骤之后，还包括：
若所述天向角速度不小于所述预设角速度阈值，则保持所述初始修正偏置角的数值不变，并将上一时间段内测得的帧数、IMU航向均值和UWB航向均值重新置零。
本申请还提供了一种移动机器人的航向角融合装置，其包括：
信息获取模块：用于获取移动机器人的IMU测量系统检测的IMU初始航向角和IMU行进航向角，以及UWB测量系统检测的UWB行进航向角；
信息处理模块：用于根据所述IMU初始航向角和所述UWB行进航向角之间的差值确定初始修正偏置角，并将所述IMU行进航向角的天向角速度与预设角速度阈值进行比对；
角度修正模块：用于若所述天向角速度小于所述预设角速度阈值，则获取在小于所述预设角速度阈值的持续时间段内的动态修正偏置角，并根据所述初始修正偏置角和所述动态修正偏置角确定实时修正偏置角；
融合模块：用于根据所述实时修正偏置角和所述IMU行进航向角生成基准航向角。
本申请还提供了一种移动机器人，该移动机器人在航向估计过程中，采用如上述实施例中任一项所述的移动机器人的航向角融合方法的步骤。
上述移动机器人的航向角融合方法、装置和移动机器人，获取移动机器人的IMU测量系统检测的IMU初始航向角和IMU行进航向角，以及UWB测量系统检测的UWB行进航向角；根据所述IMU初始航向角和所述UWB行进航向角之间的差值确定初始修正偏置角，并将所述IMU行进航向角的天向角速度与预设角速度阈值进行比对；若所述天向角速度小于所述预设角速度阈值，则获取在小于所述预设角速度阈值的持续时间段内的动态修正偏置角，并根据所述初始修正偏置角和所述动态修正偏置角确定实时修正偏置角；根据所述实时修正偏置角和所述IMU行进航向角生成基准航向角。
本申请中，分别利用移动机器人中的IMU测量系统和UWB测量系统检测当前状态下的IMU行进航向角和UWB行进航向角，并将IMU行进航向角中的天向角速度与预设角速度阈值进行比对，由于IMU测量系统适合短期作业，且在短期作业时间内精度较高，而UWB测量系统适合长期作业，其长期作业精度较高，因而采用UWB+IMU互补融合方案的过程中，可将天向角速度与预设角速度阈值之间的比对结果作为判断IMU行进航向角是否需要UWB行进航向角进行修正的依据，并在需要修正的情况下采用互补滤波的算法对IMU行进航向角进行修正，最终根据IMU行进航向角和实时修正偏置角确定基准航向角，该融合过程较为简便，计算结果较为稳定可靠，能够保证移动机器人在室内封闭环境下获取更为可靠、准确、实时的航向信息。
另外，本申请通过互补滤波算法确定移动机器人的基准航向角后，可进一步构建卡尔曼滤波模型，并将基准航向角作为卡尔曼滤波模型的观测值，将移动机器人的位置、速度及姿态信息输入卡尔曼滤波模型后，即可根据卡尔曼滤波模型中的估计值与观测值进行比较，确定实时航向角的最优结果，该过程通过互补滤波的方法得到基准航向角的同时，将该基准航向角送入卡尔曼滤波模型中，使得最终得到的实时航向角更为精确，更有利于移动机器人的精准控制，解决了室内航向估计复杂且精度不高的技术问题。
本申请附加的方面和优点将在下面的描述中部分给出，这些将从下面的描述中变得明显，或通过本申请的实践了解到。
附图说明
本申请上述的和/或附加的方面和优点从下面结合附图对实施例的描述中将变得明显和容易理解，其中：
图1为一个实施例的移动机器人的航向角融合方法流程图；
图2为一个实施例的获取最优航向角结构示意图；
图3为一个实施例的确定是否更新初始修正偏置角流程图；
图4为一个实施例的移动机器人的航向角融合装置结构图。
具体实施方式
下面详细描述本申请的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本申请，而不能解释为对本申请的限制。
本技术领域技术人员可以理解，除非特意声明，这里使用的单数形式“一”、“一个”、“所述”和“该”也可包括复数形式。应该进一步理解的是，本申请的说明书中使用的措辞“包括”是指存在所述特征、整数、步骤、操作、元件和/或组件，但是并不排除存在或添加一个或多个其他特征、整数、步骤、操作、元件、组件和/或它们的组合。
本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)，具有与本申请所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语，应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像本申请实施例中一样被特定定义，否则不会用理想化或过于正式的含义来解释。
目前，移动机器人在室内进行移动操作时，均依靠其搭载的移动载体位姿估计技术实现，而航向估计技术，作为体位姿估计技术中极为重要的一环，它提供了必要的航向反馈，实现了移动机器人的精准控制。
传统的航向估计技术大多采用磁力计或IMU等实现航向定位，然而磁力计的航向定位精度较低且抗干扰能力弱，基于IMU技术得到的航向会随时间逐渐发散，并且卫星定位系统只有在室外空旷环境下才能够提供较为精准的航向信息，室内航向难以准确获取。
而室内航向定位多采用室内地磁铺贴技术、激光SLAM技术或视觉SLAM技术，且均能够得到较为准确的航向信息，但其对环境改造的要求较高，计算量过大，实时性较差，使用环境受限制。
现提出利用UWB+IMU(超宽带+惯性测量单元)的融合定位技术，二者融合使用能够提供理想的航向角，但是由于IMU短期使用精度较高，IMU长期使用精度较高，融合方式的不同会直接影响航向结果，现有的融合处理方式较为复杂、融合不稳定，且融合效果较为不理想。
因而，为了解决现有技术中室内航向融合处理方式操作复杂、融合不稳定，且融合效果不佳的技术缺陷，本申请通过下述实施例对上文中提及的内容进行相应的改进，具体实施方式如下：
在一个实施例中，如图1所示，图1为一个实施例的移动机器人的航向角融合方法流程图，本实施例中提供了一种移动机器人的航向角融合方法，其包括如下步骤：
S110：获取移动机器人的IMU测量系统检测的IMU初始航向角和IMU行进航向角，以及UWB测量系统检测的UWB行进航向角。
本申请利用UWB+IMU(超宽带+惯性测量单元)融合定位技术实现移动机器人的精准控制。
其中，IMU测量系统指的是惯性测量单元，是一种测量物体三轴姿态角(或角速率)以及加速度的装置；一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，利用速度计和陀螺测量出物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。
UWB测量系统指的是使用超宽带技术，即无线载波通信技术对移动载体进行定位的测量系统，该系统具有复杂度低，发射信号功率谱密度低，对信道衰落不敏感，截获能力低，定位精度高等优点，尤其适用于室内等密集多径场所的高速无线接入。
因此，本步骤中，需要通过移动机器人的IMU测量系统获取其检测的IMU初始航向角和IMU行进航向角，以及通过UWB测量系统获取其检测的UWB行进航向角。
可以理解的是，这里的IMU初始航向角指的是初始化IMU测量系统后得到的初始时刻三周陀螺仪测得的载体坐标系下的角速度信息，并利用一定的计算方式得到的初始航向角信息，该初始航向角信息即表示通过IMU测量系统检测的IMU初始航向角。
这里的IMU行进航向角指的是利用IMU测量系统中三轴陀螺仪检测的运行状态下的载体坐标系下的角速度信息，并将该角速度信息进行一定运算后得到的航向角信息，该航向角信息即为IMU测量系统检测的IMU行进航向角。
这里的UWB行进航向角指的是利用UWB测量系统检测的室内环境下移动载体，如移动机器人运行状态下的航向矢量，并将该航向矢量与正北方向的夹角进行计算后，得到相应的航向角信息，航向角信息即为UWB测量系统检测的UWB行进航向角。
S120：根据所述IMU初始航向角和所述UWB行进航向角之间的差值确定初始修正偏置角，并将所述IMU行进航向角的天向角速度与预设角速度阈值进行比对。
本步骤中，通过步骤S110获取到IMU初始航向角和UWB行进航向角后，可利用两者之间的差值得到初始修正偏置角，该初始修正偏置角表示的是利用IMU测量系统和UWB测量系统检测得到的航向角之间的误差，利用该误差即可修正运行状态下检测的IMU行进航向角，得到最终精确度较高的实时航向角。
另外，利用步骤S110得到的IMU行进航向角，可通过该IMU行进航向角中的天向角速度与预设角速度阈值进行比对，即可确定移动机器人目前的运行状态。
例如，当移动机器人进行直线运动时，其天向角速度值趋近于0，因此，可将该预设角速度阈值设置为0，将IMU测量系统检测的实时状态下的IMU行进航向角中的天向角速度与该预设角速度阈值进行比对，即可确定当前移动机器人是否处于直线运动状态下。
可以理解的是，这里的天向角速度指的是将IMU测量系统中通过三轴陀螺仪检测的载体坐标系下的角速度进行姿态转换，得到大地坐标系下的角速度，该角速度即为天向角速度。
S130：若所述天向角速度小于所述预设角速度阈值，则获取在小于所述预设角速度阈值的持续时间段内的动态修正偏置角，并根据所述初始修正偏置角和所述动态修正偏置角确定实时修正偏置角。
本步骤中，通过步骤S120获取到初始修正偏置角，以及将IMU行进航向角中的天向角速度与该预设角速度阈值进行比对后，可得到比对结果，该比对结果分为两种，一种是天向角速度小于所述预设角速度阈值，一种是天向角速度不小于所述预设角速度阈值。
当天向角速度小于所述预设角速度阈值时，可以获取通过IMU测量系统检测的IMU行进航向角对应的时间段内的动态修正偏置角，并根据该动态修正偏置角和初始修正偏置角得到实时修正偏置角。
需要说明的是，这里的动态修正偏置角指的是通过对上述IMU测量系统以及UWB测量系统定时采样得到的航向角信息取平均值，并对两者的平均值进行做差得到的实时误差，该实时误差即为动态修正偏置角。
通过计算该动态修正偏置角，可以对IMU测量系统在长期检测过程中形成的误差进行一定程度的校正，提高UWB测量系统检测的航向角的比重，以减小IMU测量系统的测量误差对定位结果造成的影响。
另外，在计算实时修正偏置角时，采用互补滤波算法中的角度融合算法，并将初始修正偏置角与动态修正偏置角的值带入其中，以计算得到实时修正偏置角。
利用上述方法得到的该实时修正偏置角，不仅包含了IMU初始航向角与UWB初始航向角之间的差值信息，还包含了移动机器人行进过程检测的IMU行进航向角与UWB行进航向角之间的动态差值，并利用互补滤波中的角度融合的算法得到实时修正偏置角，该过程计算较为简便，容易实现，并且利用计算得到的实时修正偏置角对IMU行进航向角进行修正，可进一步提高航向定位的精度。
当天向角速度不小于预设角速度阈值时，表明目前移动机器人不处于预设角速度阈值下的运行状态，那么此时可保持该初始修正偏置角不变，直接利用该初始动态偏置角对IMU行进航向角进行修正，当条件成立时，才进行上述计算动态修正偏置角以及实时修正偏置角的操作。
S140：根据所述实时修正偏置角和所述IMU行进航向角生成基准航向角。
本步骤中，当通过步骤S130得到实时修正偏置角以及通过步骤S110得到IMU行进航向角后，即可根据IMU行进航向角与实时修正偏置角之间的差值得到基准航向角，该基准航向角中利用UWB检测系统检测的UWB行进航向角对IMU行进航向角进行一定程度的修正，使得移动机器人获取的室内航向信息较为精确，从而更有利于移动机器人的精准位移。
上述移动机器人的航向角融合方法，获取移动机器人的IMU测量系统检测的IMU初始航向角和IMU行进航向角，以及UWB测量系统检测的UWB行进航向角；根据所述IMU初始航向角和所述UWB行进航向角之间的差值确定初始修正偏置角，并将所述IMU行进航向角的天向角速度与预设角速度阈值进行比对；若所述天向角速度小于所述预设角速度阈值，则获取在小于所述预设角速度阈值的持续时间段内的动态修正偏置角，并根据所述初始修正偏置角和所述动态修正偏置角确定实时修正偏置角；根据所述实时修正偏置角和所述IMU行进航向角生成基准航向角。
本申请中，分别利用移动机器人中的IMU测量系统和UWB测量系统检测当前状态下的IMU行进航向角和UWB行进航向角，并将IMU行进航向角中的天向角速度与预设角速度阈值进行比对，由于IMU测量系统适合短期作业，且在短期作业时间内精度较高，而UWB测量系统适合长期作业，其长期作业精度较高，因而采用UWB+IMU互补融合方案的过程中，可将天向角速度与预设角速度阈值之间的比对结果作为判断IMU行进航向角是否需要UWB行进航向角进行修正的依据，并在需要修正的情况下采用互补滤波的算法对IMU行进航向角进行修正，最终根据IMU行进航向角和实时修正偏置角确定基准航向角，该融合过程较为简便，计算结果较为稳定可靠，能够保证移动机器人在室内封闭环境下获取更为可靠、准确、实时的航向信息。
在一个实施例中，本申请的移动机器人的航向角融合方法还可以包括：将所述实时修正偏置角作为下一持续时间段的初始修正偏置角。
本实施例中，当通过初始修正偏置角和动态修正偏置角得到当前状态下的实时修正偏置角后，可将该实时修正偏置角作为下一持续时间段内的初始修正偏置角。
可以理解的是，这里的初始修正偏置角是通过IMU初始航向角与UWB初始航线角计算得到的，因此，当需要对下一持续时间段内实时修正偏置角进行计算时，可根据上一持续时间段内计算得到的实时修正偏置角以及动态修正偏置角进行获取，进而，需要将上一持续时间段内计算得到的实时修正偏置角作为下一持续时间段内的初始修正偏置角。
在一个实施例中，如图2所示，图2为一个实施例的获取最优航向角结构示意图；本申请的移动机器人的航向角融合方法还可以包括：
S150：构建卡尔曼滤波模型，并根据所述移动机器人当前的位置、速度和航向确定估计值；
S160：将所述基准航向角作为所述卡尔曼滤波模型的观测值，并根据所述估计值与所述观测值之间的差值，得到测量误差；
S170：根据所述测量误差和所述基准航向角计算得到所述移动机器人的最优航向角。
本实施例中，通过构建卡尔曼滤波模型，来对得到的基准航向角进行滤波，以得到最优航向角。
可以理解的是，卡尔曼滤波模型的主要作用是根据当前的仪器的“测量值”、上一刻的“预测量”以及“误差”，计算得到当前的最优量，再预测下一刻的量。
因此，本实施例中，将当前移动机器人的位置、速度以及姿态中的航向信息，送入卡尔曼滤波模型中，通过卡尔曼滤波模型中的卡尔曼滤波方程，计算得到当前移动机器人的估计值，将基准航向角作为卡尔曼滤波模型的观测值，计算估计值与观测值之间的差值，可得到测量误差，然后利用估计值、观测值和测量误差进行计算后，即可得到最优航向角。
本实施例中，通过互补滤波算法确定移动机器人的基准航向角后，可进一步构建卡尔曼滤波模型，并将基准航向角作为卡尔曼滤波模型的观测值，将移动机器人的位置、速度及姿态信息输入卡尔曼滤波模型后，即可根据卡尔曼滤波模型中的估计值与观测值进行比较，确定实时航向角的最优结果，该过程通过互补滤波的方法得到基准航向角的同时，将该基准航向角送入卡尔曼滤波模型中，使得最终得到的实时航向角更为精确，更有利于移动机器人的精准控制，解决了室内航向估计复杂且精度不高的技术问题。
在一个实施例中，步骤S110中获取移动机器人的IMU测量系统检测的IMU初始航向角的步骤，可以包括：
S111：初始化IMU测量系统，获取初始时刻所述IMU测量系统检测的移动机器人在载体坐标系下的角速度信息；
S112：对所述载体坐标系下的角速度信息进行姿态转换，得到大地坐标系下的角速度信息；
S113：根据所述大地坐标系下的角速度信息计算得到IMU初始航向角。
本实施例中，由于IMU测量系统在检测前需要进行初始化操作，即对IMU测量系统中的测量数据等进行初始化，防止该测量系统中缓存的数据对后续检测产生影响。
在对IMU测量系统进行初始化操作后，可获取初始时刻IMU测量系统检测的移动机器人在载体坐标系下的角速度信息，一般而言，IMU测量系统需要安装在被测物体的重心上。
由于UWB测量系统本身精度不高，并且容易上下抖动，因而在得到载体坐标系下的角速度信息后，可通过姿态转换为大地坐标系下的角速度信息，该角速度信息中的角速度即为天向角速度，通过对特定时间段内获取的天向角速度进行积分，即可得到IMU初始航向角。
一般情况下，由于初始时刻并未产生时间差，因而IMU初始航向角为0，即移动机器人处于静止状态下。
在一个实施例中，步骤S110中所述获取移动机器人的IMU测量系统检测的IMU行进航向角的步骤，可以包括：
S114：获取初始时刻至当前时间段内所述IMU测量系统检测的移动机器人在载体坐标系下的角速度信息；
S115：对所述载体坐标系下的角速度信息进行姿态转换，得到大地坐标系下的角速度信息；
S116：根据所述大地坐标系下的角速度信息以及所述IMU初始航向角计算得到IMU行进航向角。
本实施例中，由于UWB测量系统本身精度不高，并且容易上下抖动，因而在得到载体坐标系下的角速度信息后，可通过姿态转换为大地坐标系下的角速度信息，该角速度信息中的角速度即为天向角速度，通过对特定时间段内获取的天向角速度进行积分，即可得到IMU行进航向角。
该特定时间段即为初始时刻至当前时刻的时间段，对该时间段内获取的天向角速度进行积分，即可得到IMU行进航向角，然后将该IMU行进航向角与IMU初始航向角进行做差，即可得到当前时刻的IMU行进航向角。
由于IMU初始航向角为0，因而，当前时刻的IMU行进航向角即为对初始时刻至当前时间段内获取的天向角速度进行积分得到的IMU行进航向角。
在一个实施例中，步骤S110中所述获取UWB测量系统检测的UWB行进航向角的步骤，可以包括：
S117：获取安装在所述移动机器人表面且与所述移动机器人航向一致的相邻两组UWB标签的位置信息；
S118：根据所述位置信息确定相邻两组所述UWB标签的航向矢量，并根据所述航向矢量计算得到UWB行进航向角。
本实施例提供了一种获取UWB行进航向角的方法，当通过UWB测量系统对移动机器人进行检测前，可以在移动机器人的表面安装多组UWB标签，如安装两组UWB标签时，需要保证两组UWB标签构成的直线与移动机器人的航向一致，且两组UWB标签之间的距离尽可能得大，这样得到的航向角更为准确。
当安装好两组UWB标签后，可获取两组UWB标签的位置信息，该位置信息包括位置坐标，根据两者位置坐标之间的差值可得到航向矢量，然后计算该航向矢量与正北方向之间的夹角的反正切函数，进而得到UWB行进航向角。
例如：安装两个标签时，两标签分别为，通过位置坐标相减得到航向矢量，通过航向矢量确定航向角，即
Yawu＝arctan(Ty,Tx)，
其中，Tx为两标签航向矢量中x轴的矢量坐标，Ty为两标签航向矢量中y轴的矢量坐标，Yawu为UWB行进航向角。通过该方法得到的UWB行进航向角不但准确，而且计算简单。
在一个实施例中，如图3所示，图3为一个实施例的确定是否更新初始修正偏置角流程图；图3中，步骤S130中所述获取在小于所述预设角速度阈值的持续时间段内的动态修正偏置角的步骤，可以包括：
S131：判断天向角速度是否小于预设角速度阈值；若是，则执行步骤S132；
S132：获取小于所述预设角速度阈值的持续时间段内的IMU航向均值和UWB航向均值；
S133：根据所述IMU航向均值和所述UWB航向均值确定动态修正偏置角。
本实施例中，当IMU行进航向角的天向角速度小于预设角速度阈值时，即表示当前移动机器人的运行状态在预设范围内，因而，可对当前时间段内检测的IMU行进航向角和UWB行进航向角进行计算，确定IMU航向均值和UWB航向均值，通过两个平均值之间差值对IMU行进航向角进行校正，以加大UWB行进航向角的比重，减小IMU测量系统中的测量误差的影响。
其中，通过IMU航向均值和UWB航向均值之间的差值得到的动态修正偏置角，表示的是通过对上述IMU测量系统以及UWB测量系统定时采样得到的航向角信息取平均值，并对两者的平均值进行做差得到的实时误差。
在一个实施例中，步骤S132中获取小于所述预设角速度阈值的持续时间段内的IMU航向均值和UWB航向均值的步骤，可以包括：
S310：获取小于所述预设角速度阈值的持续时间段内的时间帧信息；其中，所述时间帧信息包括所述持续时间段内的帧数、每一帧对应的实时IMU行进航向角和实时UWB行进航向角；
S311：根据所述帧数和所述实时IMU行进航向角计算得到初始IMU航向均值，根据所述帧数和所述实时UWB行进航向角计算得到初始UWB航向均值。
本实施例中，获取小于预设角速度阈值的持续时间段内的时间帧信息，该持续时间段指的是IMU测量系统检测的角速度信息中的特定时间段，该特定时间段内的时间帧信息包括但不限定于该特定时间段内的帧数、每一帧对应的实时IMU行进航向角和实时UWB行进航向角。
需要说明的是，由于检测时间段有长有短，当检测时间段过长或过短时，检测精度都会下降，因而可对检测时间设置时间帧，如设置60帧为一秒，当确定时间段后，即可确定该时间段对应的帧数。
可以理解的是，IMU行进航向角随着时间的变化而变化，因而每帧对应有一个实时IMU行进航向角，以及实时UWB行进航向角。
当得到各个帧对应的实时IMU行进航向角和实时UWB行进航向角，以及该持续时间段内的帧数后，即可确定初始IMU航向均值和初始UWB航向均值，由此即可校正由IMU测量系统检测的IMU行进航向角。
在一个实施例中，步骤S311中根据所述帧数和所述实时UWB行进航向角计算得到初始UWB航向均值的步骤之后，还可以包括：
S312：更新所述帧数，并将更新后的帧数与预设帧数阈值进行比对；
S313：当所述更新后的帧数等于所述预设帧数阈值时，则根据所述更新后的帧数重新计算所述初始IMU航向均值和所述初始UWB航向均值，得到更新后的IMU航向均值和UWB航向均值。
本实施例中，当得到初始IMU航向均值和初始UWB航向均值后，为了得到更为精准的动态修正偏置角，可对得到的帧数进行更新，如对当前帧数加1，以得到更新后的帧数，将该更新后的帧数与预设帧数阈值进行比对，以便确定是否需要更新初始修正偏置角。该预设帧数阈值即为帧数值上限，当更新后的帧数达到帧数值上限时，即可更新初始修正偏置角。
再根据该帧数和初始IMU航向均值、初始UWB航向均值进行计算，即可得到更新后的IMU航向均值和UWB航向均值。
该计算过程可以包括：
Figure BDA0002261540120000151
其中，
Figure BDA0002261540120000162
表示的是更新后的IMU航向均值，
Figure BDA0002261540120000163
表示的是初始IMU航向均值，Yawu1表示的是实时IMU航向角，N表示的是更新后的帧数。
表示的是更新后的IMU航向均值，
Figure BDA0002261540120000165
表示的是初始IMU航向均值，Yawi1表示的是实时IMU航向角。
另外，在更新初始修正偏置角后，还需将上一时间段内测得的帧数、IMU航向均值和UWB航向均值重新置零，以便下一次计算。
在一个实施例中，步骤S130中根据所述初始修正偏置角和所述动态修正偏置角确定实时修正偏置角的步骤，可以包括：
S133：根据所述更新后的UWB航向均值与所述更新后的IMU航向均值之间的差值确定动态修正偏置角；
S134：获取输入的权值参数，根据所述权值参数、所述初始修正偏置角和所述动态修正偏置角计算得到实时修正偏置角。
本实施例中，可根据上一实施例中计算得到的更新后的UWB航向均值与更新后的IMU航向均值之间的差值确定动态修正偏置角，当动态修正偏置角确定后，可根据互补滤波中的角度融合公式计算得到实时修正偏置角。
该公式可以是：
b1＝p×bv+(1-p)×b
其中，p∈(0,1]，p为权值参数，bv为动态修正偏置角，b为初始修正偏置角，b1为实时修正偏置角。
需要说明的是，这里的权值参数依据上一次检测的基准航向角的曲线进行设置。
在一个实施例中，步骤S120中将所述IMU行进航向角的天向角速度与预设角速度阈值进行比对的步骤之后，还可以包括：
S131：判断天向角速度是否小于预设角速度阈值；若否，则执行步骤S121；
S121：保持所述初始修正偏置角的数值不变；
S122：将上一时间段内测得的帧数、IMU航向均值和UWB航向均值重新置零。
本实施例中，当天向角速度不小于所述预设角速度阈值，则表示当前移动机器人的运行状态不符合预设运行状态，此时需要保持初始修正偏置角的数值不变。
另外，还需将上一时间段内测得的帧数、IMU航向均值和UWB航向均值重新置零，以便下一次计算。
在一个实施例中，如图4所示，图4为一个实施例的移动机器人的航向角融合装置结构图，本实施例中提供了一种移动机器人的航向角融合装置，包括信息获取模块210、信息处理模块220、角度修正模块230、融合模块240，包括如下步骤：
信息获取模块210：用于获取移动机器人的IMU测量系统检测的IMU初始航向角和IMU行进航向角，以及UWB测量系统检测的UWB行进航向角。
本申请利用UWB+IMU(超宽带+惯性测量单元)融合定位技术实现移动机器人的精准控制。
其中，IMU测量系统指的是惯性测量单元，是一种测量物体三轴姿态角(或角速率)以及加速度的装置；一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，利用速度计和陀螺测量出物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。
UWB测量系统指的是使用超宽带技术，即无线载波通信技术对移动载体进行定位的测量系统，该系统具有复杂度低，发射信号功率谱密度低，对信道衰落不敏感，截获能力低，定位精度高等优点，尤其适用于室内等密集多径场所的高速无线接入。
因此，本步骤中，需要通过移动机器人的IMU测量系统获取其检测的IMU初始航向角和IMU行进航向角，以及通过UWB测量系统获取其检测的UWB行进航向角。
可以理解的是，这里的IMU初始航向角指的是初始化IMU测量系统后得到的初始时刻三周陀螺仪测得的载体坐标系下的角速度信息，并利用一定的计算方式得到的初始航向角信息，该初始航向角信息即表示通过IMU测量系统检测的IMU初始航向角。
这里的IMU行进航向角指的是利用IMU测量系统中三轴陀螺仪检测的运行状态下的载体坐标系下的角速度信息，并将该角速度信息进行一定运算后得到的航向角信息，该航向角信息即为IMU测量系统检测的IMU行进航向角。
这里的UWB行进航向角指的是利用UWB测量系统检测的室内环境下移动载体，如移动机器人运行状态下的航向矢量，并将该航向矢量与正北方向的夹角进行计算后，得到相应的航向角信息，航向角信息即为UWB测量系统检测的UWB行进航向角。
信息处理模块220：用于根据所述IMU初始航向角和所述UWB行进航向角之间的差值确定初始修正偏置角，并将所述IMU行进航向角的天向角速度与预设角速度阈值进行比对。
本步骤中，通过信息获取模块210获取到IMU初始航向角和UWB行进航向角后，可利用两者之间的差值得到初始修正偏置角，该初始修正偏置角表示的是利用IMU测量系统和UWB测量系统检测得到的航向角之间的误差，利用该误差即可修正运行状态下检测的IMU行进航向角，得到最终精确度较高的实时航向角。
另外，利用信息获取模块210得到的IMU行进航向角，可通过该IMU行进航向角中的天向角速度与预设角速度阈值进行比对，即可确定移动机器人目前的运行状态。
例如，当移动机器人进行直线运动时，其天向角速度值趋近于0，因此，可将该预设角速度阈值设置为0，将IMU测量系统检测的实时状态下的IMU行进航向角中的天向角速度与该预设角速度阈值进行比对，即可确定当前移动机器人是否处于直线运动状态下。
可以理解的是，这里的天向角速度指的是将IMU测量系统中通过三轴陀螺仪检测的载体坐标系下的角速度进行姿态转换，得到大地坐标系下的角速度，该角速度即为天向角速度。
角度修正模块230：用于若所述天向角速度小于所述预设角速度阈值，则获取在小于所述预设角速度阈值的持续时间段内的动态修正偏置角，并根据所述初始修正偏置角和所述动态修正偏置角确定实时修正偏置角。
本步骤中，通过信息处理模块220获取到初始修正偏置角，以及将IMU行进航向角中的天向角速度与该预设角速度阈值进行比对后，可得到比对结果，该比对结果分为两种，一种是天向角速度小于所述预设角速度阈值，一种是天向角速度不小于所述预设角速度阈值。
当天向角速度小于所述预设角速度阈值时，可以获取通过IMU测量系统检测的IMU行进航向角对应的时间段内的动态修正偏置角，并根据该动态修正偏置角和初始修正偏置角得到实时修正偏置角。
需要说明的是，这里的动态修正偏置角指的是通过对上述IMU测量系统以及UWB测量系统定时采样得到的航向角信息取平均值，并对两者的平均值进行做差得到的实时误差，该实时误差即为动态修正偏置角。
通过计算该动态修正偏置角，可以对IMU测量系统在长期检测过程中形成的误差进行一定程度的校正，提高UWB测量系统检测的航向角的比重，以减小IMU测量系统的测量误差对定位结果造成的影响。
另外，在计算实时修正偏置角时，采用互补滤波算法中的角度融合算法，并将初始修正偏置角与动态修正偏置角的值带入其中，以计算得到实时修正偏置角。
利用上述方法得到的该实时修正偏置角，不仅包含了IMU初始航向角与UWB初始航向角之间的差值信息，还包含了移动机器人行进过程检测的IMU行进航向角与UWB行进航向角之间的动态差值，并利用互补滤波中的角度融合的算法得到实时修正偏置角，该过程计算较为简便，容易实现，并且利用计算得到的实时修正偏置角对IMU行进航向角进行修正，可进一步提高航向定位的精度。
当天向角速度不小于预设角速度阈值时，表明目前移动机器人不处于预设角速度阈值下的运行状态，那么此时可保持该初始修正偏置角不变，直接利用该初始动态偏置角对IMU行进航向角进行修正，当条件成立时，才进行上述计算动态修正偏置角以及实时修正偏置角的操作。
融合模块240：用于根据所述实时修正偏置角和所述IMU行进航向角生成基准航向角。
本模块中，当通过角度修正模块230得到实时修正偏置角以及通过信息获取模块210得到IMU行进航向角后，即可根据IMU行进航向角与实时修正偏置角之间的差值得到基准航向角，该基准航向角中利用UWB检测系统检测的UWB行进航向角对IMU行进航向角进行一定程度的修正，使得移动机器人获取的室内航向信息较为精确，从而更有利于移动机器人的精准位移。
上述移动机器人的航向角融合装置，获取移动机器人的IMU测量系统检测的IMU初始航向角和IMU行进航向角，以及UWB测量系统检测的UWB行进航向角；根据所述IMU初始航向角和所述UWB行进航向角之间的差值确定初始修正偏置角，并将所述IMU行进航向角的天向角速度与预设角速度阈值进行比对；若所述天向角速度小于所述预设角速度阈值，则获取在小于所述预设角速度阈值的持续时间段内的动态修正偏置角，并根据所述初始修正偏置角和所述动态修正偏置角确定实时修正偏置角；根据所述实时修正偏置角和所述IMU行进航向角生成基准航向角。
本申请中，分别利用移动机器人中的IMU测量系统和UWB测量系统检测当前状态下的IMU行进航向角和UWB行进航向角，并将IMU行进航向角中的天向角速度与预设角速度阈值进行比对，由于IMU测量系统适合短期作业，且在短期作业时间内精度较高，而UWB测量系统适合长期作业，其长期作业精度较高，因而采用UWB+IMU互补融合方案的过程中，可将天向角速度与预设角速度阈值之间的比对结果作为判断IMU行进航向角是否需要UWB行进航向角进行修正的依据，并在需要修正的情况下采用互补滤波的算法对IMU行进航向角进行修正，最终根据IMU行进航向角和实时修正偏置角确定基准航向角，该融合过程较为简便，计算结果较为稳定可靠，能够保证移动机器人在室内封闭环境下获取更为可靠、准确、实时的航向信息。
关于移动机器人的航向角融合装置的具体限定可以参见上文中对于移动机器人的航向角融合方法的限定，在此不再赘述。上述移动机器人的航向角融合装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于终端设备中的处理器中，也可以以软件形式存储于终端设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中，提出了一种移动机器人，该移动机器人在航向估计过程中，采用如上述实施例中任一项所述的移动机器人的航向角融合方法的步骤。
应该理解的是，虽然附图的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，其可以以其他的顺序执行。而且，附图的流程图中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，其执行顺序也不必然是依次进行，而是可以与其他步骤或者其他步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
以上所述仅是本申请的部分实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本申请原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本申请的保护范围。

Claims (13)

1.一种移动机器人的航向角融合方法，其特征在于，包括如下步骤：
获取移动机器人的IMU测量系统检测的IMU初始航向角和IMU行进航向角，以及UWB测量系统检测的UWB行进航向角；
根据所述IMU初始航向角和所述UWB行进航向角之间的差值确定初始修正偏置角，并将所述IMU行进航向角的天向角速度与预设角速度阈值进行比对；
若所述天向角速度小于所述预设角速度阈值，则获取在小于所述预设角速度阈值的持续时间段内的动态修正偏置角，并根据所述初始修正偏置角和所述动态修正偏置角确定实时修正偏置角；
根据所述实时修正偏置角和所述IMU行进航向角生成基准航向角。
2.根据权利要求1所述的移动机器人的航向角融合方法，其特征在于，还包括：
将所述实时修正偏置角作为下一持续时间段的初始修正偏置角。
3.根据权利要求2所述的移动机器人的航向角融合方法，其特征在于，还包括：
构建卡尔曼滤波模型，并根据所述移动机器人当前的位置、速度和航向确定估计值；
将所述基准航向角作为所述卡尔曼滤波模型的观测值，并根据所述估计值与所述观测值之间的差值，得到测量误差；
根据所述测量误差和所述基准航向角计算得到所述移动机器人的最优航向角。
4.根据权利要求1所述的移动机器人的航向角融合方法，其特征在于，所述获取移动机器人的IMU测量系统检测的IMU初始航向角的步骤，包括：
初始化IMU测量系统，获取初始时刻所述IMU测量系统检测的移动机器人在载体坐标系下的角速度信息；
对所述载体坐标系下的角速度信息进行姿态转换，得到大地坐标系下的角速度信息；
根据所述大地坐标系下的角速度信息计算得到IMU初始航向角。
5.根据权利要求4所述的移动机器人的航向角融合方法，其特征在于，所述获取移动机器人的IMU测量系统检测的IMU行进航向角的步骤，包括：
获取初始时刻至当前时间段内所述IMU测量系统检测的移动机器人在载体坐标系下的角速度信息；
对所述载体坐标系下的角速度信息进行姿态转换，得到大地坐标系下的角速度信息；
根据所述大地坐标系下的角速度信息以及所述IMU初始航向角计算得到IMU行进航向角。
6.根据权利要求1所述的移动机器人的航向角融合方法，其特征在于，所述获取UWB测量系统检测的UWB行进航向角的步骤，包括：
获取安装在所述移动机器人表面且与所述移动机器人航向一致的相邻两组UWB标签的位置信息；
根据所述位置信息确定相邻两组所述UWB标签的航向矢量，并根据所述航向矢量计算得到UWB行进航向角。
7.根据权利要求1所述的移动机器人的航向角融合方法，其特征在于，所述获取在小于所述预设角速度阈值的持续时间段内的动态修正偏置角的步骤，包括：
获取小于所述预设角速度阈值的持续时间段内的IMU航向均值和UWB航向均值，根据所述IMU航向均值和所述UWB航向均值确定动态修正偏置角。
8.根据权利要求7所述的移动机器人的航向角融合方法，其特征在于，所述获取小于所述预设角速度阈值的持续时间段内的IMU航向均值和UWB航向均值的步骤，包括：
获取小于所述预设角速度阈值的持续时间段内的时间帧信息；其中，所述时间帧信息包括所述持续时间段内的帧数、每一帧对应的实时IMU行进航向角和实时UWB行进航向角；
根据所述帧数和所述实时IMU行进航向角计算得到初始IMU航向均值，根据所述帧数和所述实时UWB行进航向角计算得到初始UWB航向均值。
9.根据权利要求8所述的移动机器人的航向角融合方法，其特征在于，根据所述帧数和所述实时UWB行进航向角计算得到初始UWB航向均值的步骤之后，还包括：
更新所述帧数，并将更新后的帧数与预设帧数阈值进行比对；
当所述更新后的帧数等于所述预设帧数阈值时，则根据所述更新后的帧数重新计算所述初始IMU航向均值和所述初始UWB航向均值，得到更新后的IMU航向均值和UWB航向均值。
10.根据权利要求9所述的移动机器人的航向角融合方法，其特征在于，根据所述初始修正偏置角和所述动态修正偏置角确定实时修正偏置角的步骤，包括：
根据所述更新后的UWB航向均值与所述更新后的IMU航向均值之间的差值确定动态修正偏置角；
获取输入的权值参数，根据所述权值参数、所述初始修正偏置角和所述动态修正偏置角计算得到实时修正偏置角。
11.根据权利要求8所述的移动机器人的航向角融合方法，其特征在于，将所述IMU行进航向角的天向角速度与预设角速度阈值进行比对的步骤之后，还包括：
若所述天向角速度不小于所述预设角速度阈值，则保持所述初始修正偏置角的数值不变，并将上一时间段内测得的帧数、IMU航向均值和UWB航向均值重新置零。
12.一种移动机器人的航向角融合装置，其特征在于，包括：
信息获取模块：用于获取移动机器人的IMU测量系统检测的IMU初始航向角和IMU行进航向角，以及UWB测量系统检测的UWB行进航向角；
信息处理模块：用于根据所述IMU初始航向角和所述UWB行进航向角之间的差值确定初始修正偏置角，并将所述IMU行进航向角的天向角速度与预设角速度阈值进行比对；
角度修正模块：用于若所述天向角速度小于所述预设角速度阈值，则获取在小于所述预设角速度阈值的持续时间段内的动态修正偏置角，并根据所述初始修正偏置角和所述动态修正偏置角确定实时修正偏置角；
融合模块：用于根据所述实时修正偏置角和所述IMU行进航向角生成基准航向角。
13.一种移动机器人，其特征在于：该移动机器人在航向估计过程中，采用如权利要求1至11中任一项所述的移动机器人的航向角融合方法的步骤。
CN201911072969.9A 2019-11-05 2019-11-05 移动机器人的航向角融合方法、装置和移动机器人 Active CN110764506B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911072969.9A CN110764506B (zh) 2019-11-05 2019-11-05 移动机器人的航向角融合方法、装置和移动机器人

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911072969.9A CN110764506B (zh) 2019-11-05 2019-11-05 移动机器人的航向角融合方法、装置和移动机器人

Publications (2)

Publication Number Publication Date
CN110764506A true CN110764506A (zh) 2020-02-07
CN110764506B CN110764506B (zh) 2022-10-11

Family

ID=69336317

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911072969.9A Active CN110764506B (zh) 2019-11-05 2019-11-05 移动机器人的航向角融合方法、装置和移动机器人

Country Status (1)

Country Link
CN (1) CN110764506B (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111897322A (zh) * 2020-06-23 2020-11-06 北京旷视机器人技术有限公司 运输车的方位调整方法和产品
CN111998850A (zh) * 2020-09-01 2020-11-27 广东博智林机器人有限公司 移动机构的定位方法及装置
CN112034479A (zh) * 2020-06-15 2020-12-04 煤炭科学技术研究院有限公司 一种应用于煤矿井下智能巡检无人机的定位方法及系统
CN112504296A (zh) * 2020-10-26 2021-03-16 南京苏美达智能技术有限公司 一种自行走设备的初始粗对准方法和自行走设备
CN113494910A (zh) * 2020-04-02 2021-10-12 广州汽车集团股份有限公司 一种基于uwb定位的车辆定位方法、装置及存储介质
WO2021227334A1 (zh) * 2020-05-09 2021-11-18 苏州科瓴精密机械科技有限公司 航向校正方法、系统、自移动装置、可读存储介质
CN114019954A (zh) * 2021-10-12 2022-02-08 深圳元戎启行科技有限公司 航向安装角标定方法、装置、计算机设备和存储介质

Citations (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110025562A1 (en) * 2009-08-03 2011-02-03 Xsens Technologies, B.V. Tightly Coupled UWB/IMU Pose Estimation System and Method
CN103410512A (zh) * 2013-08-15 2013-11-27 中国矿业大学 融合地质环境信息的采煤机绝对定位装置与方法
US20160193729A1 (en) * 2015-01-06 2016-07-07 Discovery Robotics Method and system for determining precise robotic position and orientation using near-simultaneous radio frequency measurements
CN106052684A (zh) * 2016-06-16 2016-10-26 济南大学 采用多模式描述的移动机器人imu/uwb/码盘松组合导航系统及方法
WO2017215024A1 (zh) * 2016-06-16 2017-12-21 东南大学 一种基于新型多传感器融合技术的行人导航装置和方法
US20180025498A1 (en) * 2016-07-21 2018-01-25 Gopro, Inc. Subject Tracking Systems for a Movable Imaging System
CN107655476A (zh) * 2017-08-21 2018-02-02 南京航空航天大学 基于多信息融合补偿的行人高精度足部导航算法
US20180181136A1 (en) * 2016-12-23 2018-06-28 Gecko Robotics, Inc. Inspection robot
CN108279687A (zh) * 2018-03-28 2018-07-13 西北农林科技大学 一种郁闭果园履带拖拉机组合导航系统
CN108426574A (zh) * 2018-02-02 2018-08-21 哈尔滨工程大学 一种基于zihr的航向角修正算法的mems行人导航方法
CN207908980U (zh) * 2018-03-28 2018-09-25 西北农林科技大学 一种郁闭果园履带拖拉机组合导航系统
US20190037348A1 (en) * 2017-07-28 2019-01-31 Electronics And Telecommunications Research Institute Method of measuring inter-device relative coordinates and device using the same
CN109855621A (zh) * 2018-12-27 2019-06-07 国网江苏省电力有限公司检修分公司 一种基于uwb与sins的组合室内行人导航系统及方法
CN110081882A (zh) * 2019-04-24 2019-08-02 中南大学 四旋翼无人机航向测量器及控制方法
CN110308729A (zh) * 2019-07-18 2019-10-08 石家庄辰宙智能装备有限公司 基于视觉与imu或里程计的agv组合导航定位方法
CN110375730A (zh) * 2019-06-12 2019-10-25 深圳大学 基于imu和uwb融合的室内定位导航系统

Patent Citations (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20110025562A1 (en) * 2009-08-03 2011-02-03 Xsens Technologies, B.V. Tightly Coupled UWB/IMU Pose Estimation System and Method
CN103410512A (zh) * 2013-08-15 2013-11-27 中国矿业大学 融合地质环境信息的采煤机绝对定位装置与方法
US20160193729A1 (en) * 2015-01-06 2016-07-07 Discovery Robotics Method and system for determining precise robotic position and orientation using near-simultaneous radio frequency measurements
CN106052684A (zh) * 2016-06-16 2016-10-26 济南大学 采用多模式描述的移动机器人imu/uwb/码盘松组合导航系统及方法
WO2017215024A1 (zh) * 2016-06-16 2017-12-21 东南大学 一种基于新型多传感器融合技术的行人导航装置和方法
US20180025498A1 (en) * 2016-07-21 2018-01-25 Gopro, Inc. Subject Tracking Systems for a Movable Imaging System
US20180181136A1 (en) * 2016-12-23 2018-06-28 Gecko Robotics, Inc. Inspection robot
US20190037348A1 (en) * 2017-07-28 2019-01-31 Electronics And Telecommunications Research Institute Method of measuring inter-device relative coordinates and device using the same
CN107655476A (zh) * 2017-08-21 2018-02-02 南京航空航天大学 基于多信息融合补偿的行人高精度足部导航算法
CN108426574A (zh) * 2018-02-02 2018-08-21 哈尔滨工程大学 一种基于zihr的航向角修正算法的mems行人导航方法
CN108279687A (zh) * 2018-03-28 2018-07-13 西北农林科技大学 一种郁闭果园履带拖拉机组合导航系统
CN207908980U (zh) * 2018-03-28 2018-09-25 西北农林科技大学 一种郁闭果园履带拖拉机组合导航系统
CN109855621A (zh) * 2018-12-27 2019-06-07 国网江苏省电力有限公司检修分公司 一种基于uwb与sins的组合室内行人导航系统及方法
CN110081882A (zh) * 2019-04-24 2019-08-02 中南大学 四旋翼无人机航向测量器及控制方法
CN110375730A (zh) * 2019-06-12 2019-10-25 深圳大学 基于imu和uwb融合的室内定位导航系统
CN110308729A (zh) * 2019-07-18 2019-10-08 石家庄辰宙智能装备有限公司 基于视觉与imu或里程计的agv组合导航定位方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
段世红 等: "传感网中UWB和IMU融合定位的性能评估", 《计算机研究与发展》 *
赵辉 等: "基于主方向的行人自主定位航向修正算法", 《电子技术应用》 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113494910A (zh) * 2020-04-02 2021-10-12 广州汽车集团股份有限公司 一种基于uwb定位的车辆定位方法、装置及存储介质
WO2021227334A1 (zh) * 2020-05-09 2021-11-18 苏州科瓴精密机械科技有限公司 航向校正方法、系统、自移动装置、可读存储介质
CN112034479A (zh) * 2020-06-15 2020-12-04 煤炭科学技术研究院有限公司 一种应用于煤矿井下智能巡检无人机的定位方法及系统
CN111897322A (zh) * 2020-06-23 2020-11-06 北京旷视机器人技术有限公司 运输车的方位调整方法和产品
CN111998850A (zh) * 2020-09-01 2020-11-27 广东博智林机器人有限公司 移动机构的定位方法及装置
CN112504296A (zh) * 2020-10-26 2021-03-16 南京苏美达智能技术有限公司 一种自行走设备的初始粗对准方法和自行走设备
CN114019954A (zh) * 2021-10-12 2022-02-08 深圳元戎启行科技有限公司 航向安装角标定方法、装置、计算机设备和存储介质
CN114019954B (zh) * 2021-10-12 2024-02-09 深圳元戎启行科技有限公司 航向安装角标定方法、装置、计算机设备和存储介质

Also Published As

Publication number Publication date
CN110764506B (zh) 2022-10-11

Similar Documents

Publication Publication Date Title
CN110764506B (zh) 移动机器人的航向角融合方法、装置和移动机器人
CN109931926B (zh) 一种基于站心坐标系的小型无人机无缝自主式导航方法
Fan et al. Data fusion for indoor mobile robot positioning based on tightly coupled INS/UWB
US20140172361A1 (en) Multi-posture stride length calibration system and method for indoor positioning
CN109855621B (zh) 一种基于uwb与sins的组合室内行人导航系统及方法
CN110243358A (zh) 多源融合的无人车室内外定位方法及系统
EP3047304B1 (en) Method and apparatus for determination of misalignment between device and vessel using acceleration/deceleration
US20150247729A1 (en) System and method for device bearing estimation
CN105652306A (zh) 基于航迹推算的低成本北斗与mems紧耦合定位系统及方法
US10054442B2 (en) Method and apparatus for handling vertical orientations of devices for constraint free portable navigation
US11441906B2 (en) Iterative estimation of non-holonomic constraints in an inertial navigation system
CN110398245A (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN111426320B (zh) 一种基于图像匹配/惯导/里程计的车辆自主导航方法
CN113124856A (zh) 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN111025366B (zh) 基于ins及gnss的网格slam的导航系统及方法
CN110986941B (zh) 一种手机安装角的估计方法
CN104535065A (zh) 无人机飞行器在复杂磁场环境下的航向生成方法
CN109459028A (zh) 一种基于梯度下降的自适应步长估计方法
CN106370182A (zh) 一种个人组合导航方法
CN108592907A (zh) 一种基于双向滤波平滑技术的准实时步进式行人导航方法
CN109297484A (zh) 一种有步态约束的行人自主定位误差修正方法
CN113324544B (zh) 一种基于图优化的uwb/imu的室内移动机器人协同定位方法
WO2020030966A1 (en) Methods and systems for estimating the orientation of an object
US20220306089A1 (en) Relative Position Tracking Using Motion Sensor With Drift Correction
CN110849364B (zh) 基于动中通的自适应卡尔曼姿态估计方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant