[发明专利]障碍物运动轨迹预测方法、系统、设备及计算机可读存储介质

说明书

本发明实施例公开了一种障碍物运动轨迹预测方法、系统、设备及计算机可读存储介质，涉及导航技术领域。其中方法包括：获取目标障碍物在连续多个时刻所对应的跟踪点位置数据，并对跟踪点位置数据进行过滤；选择函数模型，采用随机采样一致性算法根据过滤后的跟踪点位置数据求解函数模型中参数的初始值；根据函数模型构建最小二乘问题，并采用梯度下降算法结合参数的初始值求解最小二乘问题，以获取优化后的模型参数；根据优化后的函数计算目标障碍物下一时刻的预测位置。本发明实施例可以提高障碍物运动轨迹预测结果的准确性，降低算法的计算量，提高实时性；此外，还可以支持同时对多个障碍物运动轨迹进行预测。

技术领域

本发明实施例涉及导航技术领域，特别涉及一种障碍物运动轨迹预测方法、系统、设备及计算机可读存储介质。

背景技术

导航避障能力是机器人的基本功能之一，为了实现这个功能，机器人需要对障碍物的运动轨迹进行预判，以避免在导航行进过程中与障碍物发生碰撞。目前，机器人一般采用卡尔曼滤波算法对障碍物的运动轨迹进行预测，然而这种障碍物的运动轨迹预测方法需要对障碍物的运动状态(例如：运动速度及运动加速度等)有准确的了解，而且要求系统噪声和测量噪声为高斯噪声且相互独立，而这两点在实时跟踪过程中很难满足，导致预测结果的准确性低，并且这种算法的计算量大，导致实时性较差。

发明内容

有鉴于此，本发明实施例的目的在于提供一种障碍物运动轨迹预测方法、系统、设备及计算机可读存储介质，以解决上述现有的障碍物运动轨迹预测方法存在的预测结果的准备性低以及计算量大，导致实时性较差的问题。

本发明实施例解决上述技术问题所采用的技术方案如下：

根据本发明实施例的第一个方面，提供一种障碍物运动轨迹预测方法，该方法包括：

获取目标障碍物在连续多个时刻所对应的跟踪点位置数据，并对所述跟踪点位置数据进行过滤；

选择函数模型，采用随机采样一致性算法根据过滤后的跟踪点位置数据求解所述函数模型中参数的初始值；

根据所述函数模型构建最小二乘问题，并采用梯度下降算法结合所述参数的初始值求解所述最小二乘问题，以获取优化后的模型参数；

将优化后的模型参数代入所述函数模型得到优化后的函数，并根据所述优化后的函数计算所述目标障碍物下一时刻的预测位置。

其中，所述采用随机采样一致性算法根据过滤后的跟踪点位置数据求解所述函数模型中参数的初始值包括：

从过滤后的跟踪点位置数据中随机选择一组局内跟踪点位置数据，根据所述局内跟踪点位置数据求解所述函数模型对应的函数；

用求解出的函数去测试其他跟踪点位置数据，获取所述求解出的函数的假设局内点；其中，若某个跟踪点位置适用所述函数模型，则该跟踪点位置为所述函数的假设局内点；

判断所述求解出的函数的假设局内点的个数是否达到第一预设阈值；

若所述求解出的函数的假设局内点的个数达到第一预设阈值，则用所述求解出的函数的所有假设的局内点重新求解所述函数模型对应的函数；

返回到用求解出的函数去测试其他跟踪点位置数据，获取所述求解出的函数的假设局内点的步骤重复执行上述流程，直至求解出的函数的假设局内点的个数达到第二预设阈值。

其中，所述判断所述求解出的函数的假设内点的个数是否达到第一预设阈值之后还包括：

若所述求解出的函数的假设局内点的个数未达到所述第一预设阈值，则放弃所述函数模型，并返回到所述从过滤后的跟踪点位置数据中随机选择一组局内跟踪点位置数据，根据所述局内跟踪点位置数据求解所述函数模型的步骤。

其中，所述采用梯度下降算法结合所述参数的初始值求解所述最小二乘问题，以获取优化后的模型参数包括：