机械臂物辅助进食的轨迹规划方法及系统

本发明涉及轨迹规划，尤其涉及机械臂物辅助进食的轨迹规划方法及系统。

背景技术：

1、传统助餐机器人的轨迹规划方式是基于传统的运动学和动力学方法实现的，该实现方法非常机械，助餐机器人缺乏人性化体验和不够成熟完善。

2、传统机器人通常缺乏自主性，需要严格的预定轨迹和操作环境；它们通常无法应对不同的情境或未知的障碍物，这限制了它们在复杂和多变的环境中的应用。

3、传统机器人通常不能适应不同的任务约束或需求，例如不同的运动速度、不同的机械臂类型或不同的运动角度；这限制了它们在多样化任务中的应用；

4、传统机器人应用到食物辅助进食时存在食物定位不准确、抓取失败率高、运输过程中易掉落等问题。

技术实现思路

1、针对现有方法的不足，本发明将快速路径规划和梯度规划相结合，实现从起始位置到终点位置的高效食物抓取，确保合理的轨迹规划以及保持食物的平稳。

2、本发明所采用的技术方案是：机械臂物辅助进食的轨迹规划方法及系统包括以下步骤：

3、步骤一、对机械臂拾取食物的路径进行规划，得到盛取食物和喂食的规划路径；

4、进一步的，利用rrtconnect算法得到从机械臂起始位置到餐盘位置再到目标位置的路径，从而得到辅助进食的规划路径。

5、利用rrtconnect算法，通过不断扩展树结构，找到起始位置和拾取目标位置之间的快速路径；实现餐盘位置不固定场景下的定位和食物的抓取。

6、步骤二、利用minimum snap对得到的规划路径进行优化，确保最小加速度,以时间最优为目标构建minimum snap梯度模型；

7、进一步的，minimum snap梯度模型包括：

8、步骤21、通过rrtconnect算法得到包含m段轨迹的路径p(t)；

9、步骤22、以n次多项式拟合m段轨迹，得到多项式系数pm,n；

10、步骤23、构建约束方程，分别对路径p(t)进行一至四阶求导，分别得到速度函数、加速度函数、jerk函数和snap函数；

11、步骤24、构建目标函数；

12、进一步的，目标函数是最小化jerk平方的积分以及最小化snap平方的积分。

13、步骤25、根据目标函数计算代价函数之和；

14、步骤26、通过最小化代价函数之和对路径的中间点的速度、加速度jerk和snap进行约束，从而构建带约束的qp问题。

15、步骤三、为了使得末端执行器的稳定，强制末端执行器保持水平，添加姿态约束，使得末端执行器保持水平运动。

16、进一步的，姿态约束使用四元数法来确保末端执行器在路径中保持水平运动。

17、进一步的，具体包括：

18、定义一个水平平面，使用单位向量来表示水平平面的方向；使用四元数表示末端执行器的期望姿态，末端执行器的期望姿态为qtarget＝[0,0,sin(θ/2),cos(θ/2)]，θ是末端执行器需要旋转的角度。

19、进一步的，机械臂物辅助进食的轨迹规划系统，其特征在于，包括：存储器，用于存储可由处理器执行的指令；处理器，用于执行指令以实现机械臂物辅助进食的轨迹规划方法及系统。

20、进一步的，存储有计算机程序代码的计算机可读介质，计算机程序代码在由处理器执行时实现机械臂物辅助进食的轨迹规划方法及系统。

21、本发明的有益效果：

22、1、利用本发明方法使助餐机器人的自主性和灵活性增强：能够独立执行任务，而不需要实时人工干预；它具有灵活性，可以适应不同的助餐需求；

23、2、使用rrt-connect轨迹规划算法可以快速规划机械臂的运动轨迹，从而减少了助餐的等待时间，从而提高助餐效率：

24、3、由于算法的高效性，机械臂的运动更加精确，减少了食物的浪费和错误，提高了助餐的质量，提高精度和准确性；

25、4、适应不同的机械臂和任务约束，包括速度、加速度和食物提供的角度，从而确保任务在不同情境下都能成功完成；

26、5、快速、准确和自主的助餐机器人可以提高用户的整体体验，减少等待时间和错误，使助餐过程更加愉快和舒适。

技术特征：

1.机械臂物辅助进食的轨迹规划方法，其特征在于，包括以下步骤：

2.根据权利要求1所述的机械臂物辅助进食的轨迹规划方法，其特征在于，利用rrtconnect算法得到从机械臂起始位置到餐盘位置再到目标位置的路径，从而得到辅助进食的规划路径。

3.根据权利要求1所述的机械臂物辅助进食的轨迹规划方法，其特征在于，minimumsnap梯度模型包括：

4.根据权利要求3所述的机械臂物辅助进食的轨迹规划方法，其特征在于，目标函数是最小化jerk平方的积分以及最小化snap平方的积分。

5.根据权利要求1所述的机械臂物辅助进食的轨迹规划方法，其特征在于，姿态约束使用四元数法来确保末端执行器在路径中保持水平运动。

6.根据权利要求5所述的机械臂物辅助进食的轨迹规划方法，其特征在于，姿态约束具体包括：

7.机械臂物辅助进食的轨迹规划系统，其特征在于，包括：存储器，用于存储可由处理器执行的指令；处理器，用于执行指令以实现如权利要求1-6任一项所述的机械臂物辅助进食的轨迹规划方法。

8.存储有计算机程序代码的计算机可读介质，其特征在于，计算机程序代码在由处理器执行时实现如权利要求1-6任一项所述的机械臂物辅助进食的轨迹规划方法。

技术总结
本发明涉及轨迹规划技术领域，尤其涉及机械臂物辅助进食的轨迹规划方法及系统，包括对机械臂拾取食物路径进行规划，得到盛取食物和喂食的规划路径；利用Minimum Snap对得到的规划路径进行优化，确保最小加速度,以时间最优为目标构建Minimum Snap梯度模型；为了使得末端执行器的稳定，强制末端执行器保持水平，添加姿态约束，使得末端执行器保持水平运动。本发明将快速路径规划和梯度规划相结合，实现从起始位置到终点位置的高效食物抓取，确保合理的轨迹规划以及保持运送食物的平稳。

技术研发人员：邹凌,朱堃,吕继东,周天彤,戎海龙
受保护的技术使用者：常州大学
技术研发日：
技术公布日：2024/1/22