[发明专利]穿刺速度规划方法、装置、穿刺设备以及计算机存储介质

权利要求书

1.一种穿刺速度规划方法，其特征在于，包括：

获取穿刺对象的质心运动模型以及目标穿刺位置；

检测穿刺装置末端的初始位姿以及穿刺对象质心的初始质心位置；

基于所述质心运动模型，在满足穿刺初始时间对应的质心位置为所述初始质心位置、穿刺终止时间对应的质心位置为所述目标穿刺位置的条件下，根据所述初始位姿、所述初始质心位置以及所述目标穿刺位置确定穿刺规划速度；

所述根据所述初始位姿、所述初始质心位置以及所述目标穿刺位置确定穿刺规划速度的步骤包括：

在预设的速度参数区间内进行离散取值，分别根据得到的各速度参数离散值与所述初始位姿、所述初始质心位置以及所述目标穿刺位置计算出相应的穿刺终止时间；

查找出以所述穿刺终止时间作为变量、所述目标穿刺位置作为目标值时，满足所述质心运动模型对应的速度离散值；

根据查找到的所述速度参数离散值与所述穿刺终止时间计算穿刺规划速度。

2.根据权利要求1所述的方法，其特征在于，所述速度参数包括穿刺加速度和/或穿刺速度。

3.根据权利要求1所述的方法，其特征在于，所述根据得到的各速度参数离散值与所述初始位姿、所述初始质心位置以及所述目标穿刺位置计算出相应的穿刺终止时间的步骤包括：

根据所述初始质心位置与所述质心运动模型确定穿刺初始时间；

根据所述初始位姿、所述目标穿刺位置以及速度参数离散值计算用于穿刺方向对准的第一穿刺时间以及用于穿刺直线运动的第二穿刺时间；

根据所述初始时间、所述第一穿刺时间以及所述第二穿刺时间确定所述穿刺终止时间。

4.根据权利要求3所述的方法，其特征在于，所述速度参数离散值包括第一穿刺时间内的角加速度离散值以及所述第二穿刺时间内的线加速度离散值，所述根据所述初始位姿、所述目标穿刺位置以及速度参数离散值计算用于穿刺方向对准的第一穿刺时间以及用于穿刺直线运动的第二穿刺时间的步骤包括：

根据所述初始位姿和所述目标穿刺位置确定调整位姿；

根据所述角加速度离散值、所述初始位姿以及所述调整位姿计算第一穿刺时间；

根据所述线加速度离散值、所述调整位姿以及所述目标穿刺位置计算第二穿刺时间。

5.根据权利要求1所述的方法，其特征在于，在获取穿刺对象的质心运动模型以及目标穿刺位置的步骤前，所述方法还包括：

获取预设时间范围内各时刻穿刺对象的质心坐标；

拟合所述质心坐标随时间变化的曲线，以得到所述质心运动模型。

6.根据权利要求5所述的方法，其特征在于，所述拟合所述质心坐标随时间变化的曲线，以得到所述质心运动模型的步骤包括：

采用最小二乘法对各坐标维度的质心坐标进行拟合，得到各坐标维度的时间变化曲线；

基于各坐标维度的时间变化曲线确定所述质心运动模型。

7.一种穿刺速度规划装置，其特征在于，包括：

数据获取模块，用于获取穿刺对象的质心运动模型以及目标穿刺位置；

位姿检测模块，用于检测穿刺装置末端的初始位姿以及穿刺对象质心的初始质心位置；

速度规划模块，用于基于所述质心运动模型，在满足穿刺初始时间对应的质心位置为所述初始质心位置、穿刺终止时间对应的质心位置为所述目标穿刺位置的条件下，根据所述初始位姿、所述初始质心位置以及所述目标穿刺位置确定穿刺规划速度；

所述根据所述初始位姿、所述初始质心位置以及所述目标穿刺位置确定穿刺规划速度的步骤包括：

在预设的速度参数区间内进行离散取值，分别根据得到的各速度参数离散值与所述初始位姿、所述初始质心位置以及所述目标穿刺位置计算出相应的穿刺终止时间；

查找出以所述穿刺终止时间作为变量、所述目标穿刺位置作为目标值时，满足所述质心运动模型对应的速度离散值；

根据查找到的所述速度参数离散值与所述穿刺终止时间计算穿刺规划速度。