基于四轮差速模型的多维度MPC运动规划方法及系统与流程

本发明涉及轨迹规划及运动控制，尤其涉及基于四轮差速模型的多维度mpc运动规划方法及系统。

背景技术：

1、随着科技创新快速发展，自主导航机器人(automated guided vehicle，agv)逐渐在物流、仓储、清洁、安保、医疗、农业、巡检等领域受到广泛的应用，运动规划作为agv导航框架中的重要组件，也一直是学术界和产业界的研究热点。在动态环境中，常用的运动规划算法包括模型预测控制算法(model predictive control，mpc)、动态窗口算法和人工势场法。mpc相对于其它运动规划方法具有对模型精度要求不高，鲁棒性及稳定性良好，可有效处理多变量约束的问题等诸多优点，被越来越多地运用到了运动规划领域。

2、然而，现有的mpc算法应用在运动规划时，很难对速度做实时的调节，往往认为运行过程中速度保持不变，无法达到最佳的控制效果。对此，现有技术中一种解决方式是由上游模块生成详细的参考轨迹，参考轨迹包含位置坐标、速度、加速度、航向角以及时间，计算量大且复杂度高，对上游模块要求较高，在参考线多变和复杂场景下，不利于导航的实时控制，且现有的mpc算法多以机器人速度为控制量，会出现速度变化幅度过大甚至突变导致机器人运行平稳性差的问题。

技术实现思路

1、本发明旨在解决现有机器人m pc算法控制中存在的对上游模块要求较高、导航控制实时性差以及机器人运行平稳性差的技术问题，提供了基于四轮差速模型的多维度m pc运动规划方法及系统。

2、为了实现本发明的上述目的，根据本发明的第一个方面，本发明提供了基于四轮差速模型的多维度m pc运动规划方法，在每个控制周期内，执行：步骤s1，计算参考线上每个路径点的期望线速度，所述路径点包括移动目标的期望位置坐标和期望航向角；步骤s2，结合移动目标当前运动状态和所述参考线，基于预先建立的四轮差速模型的运动学方程预测移动目标未来时间t内的预测轨迹；步骤s3，利用所述预测轨迹求解目标优化函数获得控制序列，选取控制序列的第一个元素作为下一控制周期的控制量，并将所述控制量输入移动目标的运动控制系统；所述控制序列的元素包括线加速度和角速度，所述目标优化函数用于实现未来时间t内的横向误差、航向误差、线速度误差、角速度和线角速度的和最小，所述线速度误差为预测轨迹中预测线速度与期望线速度之差。

3、为了实现本发明的上述目的，根据本发明的第二个方面，本发明提供了一种运动规划系统，用于实现本发明第一方面所述的基于四轮差速模型的多维度m pc运动规划方法，包括期望线速度计算模块、预测轨迹获取模块和控制量获取模块，在每个控制周期内：所述期望线速度计算模块计算参考线上每个路径点的期望线速度，所述路径点包括移动目标的期望位置坐标和期望航向角；所述预测轨迹获取模块结合移动目标当前运动状态和所述参考线基于预先建立的四轮差速模型的运动学方程预测移动目标未来时间t的预测轨迹；所述控制量获取模块利用所述预测轨迹求解目标优化函数获得控制序列，并选取控制序列的第一个元素作为下一控制周期的控制量并输入运动控制系统；所述控制序列的元素包括线加速度和角速度，所述目标优化函数用于实现未来时间t内的横向误差、航向误差、线速度误差、角速度和线角速度的和最小，所述线速度误差为预测轨迹中预测线速度与期望线速度之差。

4、为了实现本发明的上述目的，根据本发明的第三个方面，本发明提供了一种四轮移动机器人，包括处理器和运动控制系统，所述四轮移动机器人为移动目标，所述处理器执行本发明第一方面所述的基于四轮差速模型的多维度mpc运动规划方法的步骤，并将获得的控制量输入运动控制系统。

5、为了实现本发明的上述目的，根据本发明的第四个方面，本发明提供了一种电子设备，所述电子设备包括：至少一个处理器；以及，与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有可被所述至少一个处理器执行的计算机程序，所述计算机程序被所述至少一个处理器执行，以使所述至少一个处理器能够执行如本发明第一方面所述的基于四轮差速模型的多维度mpc运动规划方法。

6、本发明的参考线仅包含路径点的期望位置坐标和期望航向角，降低了对上游模块的要求，降低了计算复杂度，减小运算量，在移动目标运行过程中能根据参考线和环境类型对速度做实时调节；本发明以移动目标的线加速度和角速度为控制量，相较于线速度和角速度，能有效避免线速度变化幅度过大甚至突变，运行过程更平稳；在求解最优线加速度和角速度的过程中，构建了目标优化函数，在目标优化函数中考虑了横向误差和航向误差，相较于目标优化函数仅考虑位置坐标误差，能更直观、方便地进行控制量调节。

技术特征：

1.基于四轮差速模型的多维度mpc运动规划方法，其特征在于，在每个控制周期内，执行：

2.如权利要求1所述的基于四轮差速模型的多维度mpc运动规划方法，其特征在于，所述步骤s1中，计算参考线上每个路径点的期望线速度，包括：

3.如权利要求2所述的基于四轮差速模型的多维度mpc运动规划方法，其特征在于，所述步骤s11中，计算参考线上每个路径点的曲率，包括：

4.如权利要求2所述的基于四轮差速模型的多维度mpc运动规划方法，其特征在于，所述步骤s13包括：

5.如权利要求4所述的基于四轮差速模型的多维度mpc运动规划方法，其特征在于，求取第j个分段上路径点的期望线速度，包括：

6.如权利要求1-5之一所述的基于四轮差速模型的多维度mpc运动规划方法，其特征在于，所述步骤s2中，四轮差速模型的运动学方程为：

7.如权利要求6所述的基于四轮差速模型的多维度mpc运动规划方法，其特征在于，所述步骤s3中，所述目标优化函数为：

8.一种运动规划系统，用于实现权利要求1-7之一所述的基于四轮差速模型的多维度mpc运动规划方法，其特征在于，包括期望线速度计算模块、预测轨迹获取模块和控制量获取模块，在每个控制周期内：

9.一种四轮移动机器人，其特征在于，包括处理器和运动控制系统，所述四轮移动机器人为移动目标，所述处理器执行权利要求1-7之一所述的基于四轮差速模型的多维度mpc运动规划方法的步骤，并将获得的控制量输入运动控制系统。

10.一种电子设备，其特征在于，所述电子设备包括：

技术总结本发明提供了基于四轮差速模型的多维度MPC运动规划方法及系统。在每个控制周期内，执行：S1，计算参考线上每个路径点的期望线速度，参考线包括期望位置坐标和期望航向角；S2，预测未来时间T内的预测轨迹；S3，利用预测轨迹求解目标优化函数获得控制序列，选取控制序列的第一个元素作为下一控制周期的控制量，并将控制量输入移动目标的运动控制系统；控制序列的元素包括线加速度和角速度，目标优化函数用于实现未来时间T内的横向误差、航向误差、线速度误差、角速度和线角速度的和最小。降低了对上游模块的要求，对速度做实时调节，以线加速度和角速度为控制量，能有效避免速度变化幅度过大甚至突变，运行过程更平稳。技术研发人员：朱冬,尹大祥,翟延武,胡小东,唐国梅受保护的技术使用者：七腾机器人有限公司技术研发日：技术公布日：2024/6/26