CN115741732A - 一种按摩机器人的交互式路径规划及运动控制方法 - Google Patents

Abstract

本发明涉及一种按摩机器人的交互式路径规划及运动控制方法，包括：采集人体背部的RGB‑D图像，对所述RGB‑D图像进行二维轨迹规划，并映射至三维空间进行在线交互式规划按摩轨迹，得到机器人控制的三维轨迹；在所述机器人控制三维轨迹运动的同时，采集所述人体背部的点云信息，配准所述点云信息，更新所述机器人的按摩轨迹；测量并计算所述机器人的关节扭矩，得到所述机器人末端的接触力；通过自适应控制所述按摩轨迹和调整所述接触力进行力和位置混合控制,控制所述机器人的运动。本发明针对机器人与人触摸擦拭任务，降低了操作者规划路径难度，实现了机器人智能、安全的人机交互作业。

Description

一种按摩机器人的交互式路径规划及运动控制方法

技术领域

本发明涉及机器人交互与运动控制技术领域，特别是涉及一种按摩机器人的交互式路径规划及运动控制方法。

背景技术

机器人与人的物理交互是近年来的一个新兴研究领域。其中机器人擦拭人体背部是典型操作，在传统中医按摩、现代医学超声检测、临床护理等都具有重大研究意义和应用前景。

按摩轨迹规划是机器人进行人体按摩操作的前提。目前，机器人的路径规划方法繁多，例如循迹，遥操作，手动示教等方法。但这些轨迹规划方法难以实现机器人与人的物理交互。近年来将视觉引入机器人控制是主流的发展方向，如超声扫描机器人识别医学记号，传统中医按摩机器人引入人体穴位识别。但要实现这些方法需要较高的学习成本，本发明采用的视觉的交互式路径规划方法对按摩机器人进行操作轨迹规划，以更低成本发挥了机器人视觉感知的优势，降低了对操作者的使用要求，同时在轨迹跟踪的方法上实现了更具挑战的按摩。

揉、擦和拍是目前按摩机器人能够实现的常见三种按摩技术。其中，做拍击的按摩机器人，因为其动作特性，在轨迹追踪上为位置控制，在拍击力道上多为固定的拍击力度。这种位置和力分离控制的结构难以完成更为复杂的擦、揉动作。因此混合的力/位控制在机器人与人接触的环境中衍生，机器人需具备感知环境接触力的能力以提高安全性。本文专利采用机器人各关节力矩传感估算机器人末端接触力，以视觉的方法动态跟踪轨迹，通过内外双循环的闭环控制能够在机器人擦拭按摩中实现混合的力/位控制。这种轨迹追踪的控制方法能够既能实现精确的位置追踪，又能够保持期望的机器人与人的接触力。

发明内容

本发明的目的在于提供一种按摩机器人的交互式路径规划及运动控制方法，该方法可应用于人机物理交互示教，机器人按摩和医学临床护理等场景。

为实现上述目的，本发明提供了如下方案：

一种按摩机器人的交互式路径规划及运动控制方法，包括：

采集人体背部的RGB-D图像，对所述RGB-D图像进行二维轨迹规划，获得二维轨迹，将所述二维轨迹映射至三维空间，并进行在线交互式规划按摩轨迹，得到机器人控制的三维轨迹；

在所述机器人控制三维轨迹运动的同时，采集所述人体背部的点云信息，配准所述点云信息，更新所述机器人的按摩轨迹；

测量并计算所述机器人的关节扭矩，得到所述机器人末端的接触力；

通过自适应控制所述按摩轨迹和调整所述接触力进行力和位置混合控制,控制所述机器人的运动。

优选地，得到所述机器人控制的三维轨迹包括：

在所述二维轨迹中提取所述二维轨迹的像素点，并结合点云数据映射至三维空间进行在线交互式规划按摩轨迹，得到空间三维轨迹；

对所述空间三维轨迹进行拟合，得到机器人控制的三维轨迹。

优选地，映射至三维空间进行在线交互式规划按摩轨迹的方法为：

其中，c_x,c_y分别为深度图像的中心坐标，f为深度相机焦距，u,v为记录的2D轨迹中

的某一像素坐标点，z_c为该像素坐标点的深度信息，x_w,y_w和z_w分别为世界坐标系下的坐标，d_x,d_y分别为实际像素大小。

优选地，更新所述机器人的按摩轨迹包括：

通过配准所述点云数据，得到齐次变化矩阵，计算所述齐次变化矩阵，得到人体背部的平移信息和人体背部的旋转信息，判断所述平移信息和所述旋转信息是否超出预设阈值，若超出预设阈值，则视为所述人体背部发生运动，并基于所述齐次变化矩阵更新所述机器人的按摩轨迹，若未超出预设阈值，则视为所述人体背部未发生运动，所述机器人的按摩轨迹保持不变。

优选地，更新所述机器人的按摩轨迹方法为：

其中，轨迹

为当前机器人理想特征姿态，R为旋转矩阵，

为上一时刻机器人理想特征姿态，t为平移向量。

优选地，对所述关节扭矩进行计算的方法为：

F＝(J^T)^-1Ψ

其中，F为机器人末端估算力，J为雅各比矩阵，Ψ为机器人关节力矩向量。

优选地，自适应追踪所述按摩轨迹包括：

从所述机器人的按摩轨迹中，定义所述机器人接触人体背部的理想末端位置，得到理想特征姿态，所述机器人读取当前时刻的特征姿态，所述理想特征姿态和所述当前时刻的特征姿态相减，得到特征姿态误差，基于所述特征姿态误差，获得所述机器人的期望速度，基于所述期望速度，自适应追踪所述按摩轨迹。

优选地，调整所述接触力包括：

设定期望接触力，将所述接触力与所述期望接触力相减，得到接触力误差，将所述触力误差输入到所述机器人的PID控制器中，输出理想末端法向速度，通过所述理想末端法向速度控制调节所述接触力。

优选地，控制所述机器人的运动包括：

将所述期望速度和所述理想末端法向速度结合，得到理想末端速度；对所述理想末端速度进行计算，得到所述机器人的关节理想速度，基于所述理想速度，进行力和位置混合控制，控制所述机器人的运动。

优选地，对所述理想机器人关节运动速度进行计算为：

q_cmd＝J^-1ν_d

其中，J为雅各比矩阵，ν_d为机器人末端理想速度。

本发明的有益效果为：

本发明的按摩机器人的交互式路径规划及运动控制方法，通过在线二维路径规划可以在三维空间中生成理想机器人运动轨迹，并且在点云中可视化，同时基于点云的最近点迭代算法进行的无标记人体背部动态跟踪，保证了机器人在按摩时人体的潜在移动。此外，依赖基于位置的视觉伺服控制算法和基于力估计的PID力反馈调节控制器组成的外循环和内循环控制系统，在外环控制中，机器人基于视觉伺服算法控制机器人追踪3D按摩轨迹，在内环控制中，采用基于PID的力控制策略，自适应调节调节机器人末端与人体背部接触力到合理范围，能够保证机器人精确追踪轨迹的同时保持理想接触力。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明的一种按摩机器人的交互式路径规划及运动控制方法的结构示意图；

图2为本发明的一种按摩机器人的交互式路径规划及运动控制方法的流程图；

图3为本发明的运动监测器轨迹更新示意图；

图4为本发明的机器人按摩控制方法的示意图；

其中，1-人体上半身模型，2-机器人运动轨迹，3-机器人末端执行器，4-手臂机器人，5-固定的深度相机，6-底座。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。

实施例1

一种按摩机器人的交互式路径规划及运动控制方法，包括如下步骤：

根据医生在RGB图像上进行的二维轨迹规划，通过相机透视原理映射至三维空间进行在线交互式按摩轨迹规划，并通过点云可视化三维轨迹；

由在线轨迹规划输出的3D轨迹，利用视觉动态跟踪方法，监测人体的潜在运动，以更新按摩轨迹；

根据机器人与人体背部接触安全性需求，测量关节力矩估算机器人末端的受力情况；

机器人根据力位混合控制方式进行自适应追踪轨迹并调整人体背部接触力大小。

所述轨迹是医生在RGB图像上选取二维轨迹像素点(u,v)，并结合由深度相机KinectV2获取的深度信息z_c,根据相机透视原理将点(u,v)映射至三维空间(x_w,y_w,z_w)。由像素点[u,v]^T到世界坐标点[x_w,y_w,z_w,]^T的变化公式为：

其中，c_x,c_y分别为深度图像的中心坐标，f为深度相机焦距，u,v为记录的2D轨迹中

的某一像素坐标点，z_c为该像素坐标点的深度信息，x_w,y_w和z_w分别为世界坐标系下的坐标，d_x,d_y分别为实际像素大小。

所述的2D至3D的映射方法为所述空间三维轨迹由离散点组成，由B样条曲线拟合离散轨迹得到可用于机器人控制的连续三维轨迹；

所述世界坐标系下的三维空间轨迹

p_i∈R³以离散点的形式在点云中匹配；

所述的轨迹点在点云匹配的方式，所述点云由Kinect V2深度相机捕捉，通过直通滤波筛选人体背部所在区域，Ransac算法拟合点云平面分割出人体背部点云；所述轨迹在点云中由最近点快速迭代算法匹配点云，并在点云中高亮显现。

所述力估计由机器人力雅各比矩阵建立起所测关节力矩与机器人末端力的关系；

视觉动态跟踪和监测人体潜在运动以更新按摩轨迹的方法，采用无标记点的动态监测,其步骤为：

a.以一定频率将人体背部点云输入监测器；

b.采用迭代最近点(ICP)方法对前后两帧点云进行配准，输出4×4齐次变化矩阵；

c.将输出的变化矩阵输入原轨迹以此更新机器人运动轨迹；

所述的机器人根据力位混合控制方式进行自适应追踪轨迹，所述自适应追踪轨迹，采用无标记点的动态监测方式动态更新机器人运动轨迹.

所述的无标记点的动态监测方式动态更新机器人运动轨迹，所述的4×4齐次变化矩阵包含目标平移信息和旋转信息，通过平移量和旋转量评估目标是否发生变化以此来更新机器人运动轨迹；

所述的机器人根据力位混合控制方式进行自适应追踪轨迹,所述控制方式混合了视觉伺服控制和力控制，前者控制机器人精确追踪位置，后者控制机器人保持接触力在理想大小；

所述的视觉伺服算法速度控制部分，所述采用基于位置的视觉伺服算法，通过三维空间中两点之间的位置和姿态误差，计算机器人末端理想速度。通过机器人雅各比矩阵将末端速度转换到各关节速度，实现机器人速度控制；

所述的机器人根据力位混合控制方式进行自适应追踪轨迹并调整人体背部接触力大小，其特征在于，所述机器人末端接触力通过各个关节力矩估计。估算的末端接触力的法向分量与设定期望法向力输入PID控制器，通过PID控制反馈调节机器人运动速度；

所述的两种控制机器人运动速度所形成的机器人擦拭人体背部控制系统，其特征在于，所述的控制系统输入为两个期望速度，分别为：期望位置追踪速度和期望法向力调节速度。由后者替代前者的Z轴速度分量，将替代后的期望位置追踪速度通过雅各比矩阵转换到机器人各个关节速度。

实施例2

一种按摩机器人的交互式路径规划及运动控制方法，包括：

根据深度相机的图像和点云信息获取场景和患者的状态信息；根据机器人的位姿信息建立机器人与患者之间的位姿转换关系；根据患者图像信息医生手动规划2D按摩轨迹；根据2D按摩轨迹结合投影算法计算真实3D按摩轨迹；根据患者背部点云动态追踪患者运动并更新3D按摩轨迹；根据力位置混合控制方法，外环控制中，机器人基于视觉伺服算法控制机器人追踪3D按摩轨迹，在内环控制中，采用基于PID的力控制策略，自适应调节调节机器人末端与人体背部接触力到合理范围，保证按摩操作过程的安全性。本发明针对机器人与人触摸擦拭任务，降低了操作者规划路径难度，实现了机器人智能、安全的人机交互作业。本发明针对协作机器人对人体背部按摩的任务，所提出方法可提供自然交互方法来规划任意按摩轨迹，在保证了操作的安全性，实现了机器人自主按摩，具体为：

本发明的第一个方面，提供了按摩机器人的在线交互式按摩轨迹规划方法，所述方法包括机器人手眼系统标定，二维轨迹到三维轨迹的映射和视觉的轨迹动态跟踪监视器。

所述的机器人手眼标定系统，包含相机的内参标定和外参标定。采用棋盘标定法对相机内参进行标定，对于相机的外参标定，采用二维码系附机器人末端，记录相机坐标系下二维码的轨迹和机器人坐标系下末端的轨迹，使用奇异值分解的最小二乘拟合方法拟合两条轨迹，其拟合出4×4的齐次变化矩阵为机器人坐标系和相机坐标系的变化矩阵。

所述的二维轨迹到三维轨迹的映射，医生在由相机采集的彩色图像结合深度图像形成的RGB-D图像上规划二维轨迹，基于相机透视原理，将像素坐标下的二维轨迹映射至笛卡尔空间下的世界坐标系，利用B样条曲线对轨迹进行拟合。此外，通过深度相机采集视野内人体背部点云数据，采用快速最近点算法，在点云中可视化规划的三维轨迹。

所述视觉的轨迹动态跟踪监视器，在机器人追踪三维轨迹运动时，实时采集相机前方人体背部点云信息，采用最近点迭代法对前后两帧点云图进行配准输出4×4的齐次变化矩阵，根据变化矩阵算出的目标平移或是旋转信息是否超出阈值，判断目标是否发生了运动。若是发生运动则将变化矩阵对轨迹加以更新，以此方法监测人体的潜在运动。

本发明的第二个方面，提供了机器人与人体背部接触力估计方法。

所述方法依赖机器人关节内安置扭矩传感器，测得各个关节的扭矩为τ，由虚功原理并忽略关节摩擦力计算机器人末端产生力F的公式为：

F＝(J^T)^-1Ψ

其中，F为机器人末端估算力，J为雅各比矩阵，Ψ为机器人关节力矩向量。

因此根据牛顿第三定律，机器人末端与人体背部的接触力大小为F。

本发明的第三个方面，提供了机器人与人体背部接触时的力位混合控制策略。该策略包含位置控制和力控制两个控制框架。

所述的位置控制框架，由基于位置的视觉伺服算法构成。在机器人坐标系下，由交互式轨迹规划输出的3D轨迹定义机器人接触人体背部时的理想末端位置，机器人控制器实现视觉伺服算法的步骤如下：

根据权利要求x拟合的3D轨迹在t时刻给予末端理想特征姿态P_d＝(t_d,θ_du_d),机器人读取t时刻末端执行器特征姿态P_r＝(t_r,θ_ru_r)。其中t_*为位置坐标，θ_*和u_*分别为机器人坐标系下t时刻姿态的旋转角度和旋转矩阵的单位特征值。P_d和P_r的特征姿态误差为e＝P_r-P_d；

根据特征误差e计算计算机器人理想末端速度v_e公式为：

其中λ为速度比例系数，

为交互矩阵；

所述力控制，是基于末端力估计反馈形成的PID控制。估算接触力F和期望接触力F_d之间的误差e＝F-F_d输入PID控制器，PID控制器输出理想末端法向速度v_z，在不断地控制调节末端执行器的速度即可保持理想的表面接触力。

进一步地，所述的力/位混合控制策略，是将位置控制和力控制结合。二者都是通过调节机器人末端速度从而达到理想状态，所以将期望速度v_e和v_z相结合得到理想末端速度v_d，从而经机器人的雅各比矩阵计算机器人各个关节理想速度q_cmd的公式为：

q_cmd＝J^-1ν_d

其中，J为雅各比矩阵，ν_d为机器人末端理想速度。

综上所述，本发明的按摩机器人的交互式路径规划及运动控制方法，通过在线二维路径规划可以在三维空间中生成理想机器人运动轨迹，并且在点云中可视化，同时基于点云的最近点迭代算法进行的无标记人体背部动态跟踪，保证了机器人在按摩时人体的潜在移动。此外，依赖基于位置的视觉伺服控制算法和基于力估计的PID力反馈调节控制器组成的外循环和内循环控制系统，能够保证机器人精确追踪轨迹的同时保持理想接触力。

实施例3

如图1所示，一种按摩机器人的交互式路径规划及运动控制方法，包括在线轨迹规划，实现二维轨迹到三维轨迹的映射；机器人根据关节内置扭矩传感器与机器人力雅各比矩阵估算机器人末端接触力大小；人体运动监测更新轨迹和机器人沿轨迹运动并保持接触力。所述平台基于Baxter双臂机器人，其机械臂由串联弹性关节组成，自带关节力矩传感器；深度相机采用Kinect V2相机，可由480P分辨率的彩色图像和深度图像生成原始点云；末端执行器为与目标接触面附有软胶的3D打印结构。

所述机器人操作对象为人体上半身模型，放置于机器人前方60-70cm的工作区域，深度相机的安置在能够捕捉人体模型的机器人胸前位置。

一种按摩机器人的交互式路径规划及运动控制方法，所述机器人的手眼标定系统，包含相机的内参标定和外参标定。采用棋盘标定法对相机内参进行标定，对于相机的外参标定，将二维码系附机器人末端，记录相机坐标系下二维码的轨迹K＝{k₁,k₂,…,k_n}和机器人坐标系下末端的轨迹G＝{g₁,g₂,…,g_n}，使用奇异值分解的最小二乘拟合方法拟合两条轨迹；

旋转矩阵为：

其中，c为影响因子，U和V为轨迹G和轨迹K之间的协方差矩阵的奇异值分解结果；

平移矩阵为：

其中

和

分别为轨迹G和轨迹K的去中心坐标；

如图2所示，一种按摩机器人的交互式路径规划及运动控制方法，所述控制工作流程，包含如下步骤：

利用深度相机采集人体背部点云数据，在彩色图像结合深度图像的RGB-D图像上规划2D轨迹，记2D轨迹的像素坐标集合为

与之对应的深度集合为

计算像素点[u,v]^T到世界坐标点[x_w,y_w,z_w,]^T的变化公式如下：

其中，c_x,c_y分别为深度图像的中心坐标，f为深度相机焦距，u,v为记录的2D轨迹中

的某一像素坐标点，z_c为该像素坐标点的深度信息，x_w,y_w和z_w分别为世界坐标系下的坐标，d_x,d_y分别为实际像素大小；

世界坐标系下的轨迹集合记为

对人体背部点云数据建立Kdtree，采用领域点搜索方法将

中每一个点映射至点云形成离散轨迹

作为点云可视化输出；

采用B样条曲线插值拟合离散轨迹

形成连续的机器人运动轨迹

本实施例中，基于ICP算法的视觉更新轨迹的方法，所述方法在机器人追踪三维轨迹运动时，相机实时采集人体背部点云信息，采用最近点迭代法对前后两帧点云图进行配准输出4×4的齐次变化矩阵，根据变化矩阵算出的目标平移或是旋转信息是否超出阈值，判断目标是否发生了运动。执行步骤如下：

对于当前点云集记为X_m＝{x₁,x₂,…,x_n}，以1Hz频率更新的点云数据记为P_m＝{p₁,p₂,…,p_n}，ICP算法的核心在于，下述公式值最小：

其中，N为点云中的数据个数，R和t分别为两个点云集之间的旋转矩阵和平移向量；

上述R矩阵的旋转角度θ如下式：

本实施例中，|θ|大于5°或上述t大于1cm的情况下，对轨迹进行更新,更新公式如下：

其中，轨迹

为当前机器人理想特征姿态，R为旋转矩阵，

为上一时刻机器人理想特征姿态，t为平移向量；

如图3所示，一种按摩机器人的交互式路径规划及运动控制方法，轨迹

代表当前机器人理想特征姿态P_d＝(t_d,θ_du_d),同时机器人读取当前末端执行器特征姿态P_r＝(t_r,θ_ru_r)，其中t为位置坐标，θ和u分别为世界坐标系下该点的旋转角度和旋转矩阵的单位特征值，二者特征姿态误差e＝P^d-P^r；

根据特征误差计算计算机器人理想末端速度v_e公式如下：

其中，v_e＝[v_xv_yv_zω_xω_yω_z]，

为交互矩阵。

在机器人控制循环中，为实现接触力自调整的力控制循环步骤如下：

根据内置在机器人手臂关节内的扭矩传感器测量获得各个关节的扭矩τ；

由各关节扭矩估算机器人末端的接触力大小：

F＝(J^T)^-1Ψ

其中，F为机器人末端估算力，J为雅各比矩阵，Ψ为机器人关节力矩向量；

由用户决定(即操作者)设定期望接触力，估算接触力F和期望接触力F_d之间的误差e＝F-F_d输入PID控制器，输出PID期望接触力F_output，公式如下：

根据该反馈控制器，输出的理想末端法向速度

与F_output的关系如下：

如图4所示，所述的力/位混合控制策略，是将位置控制和力控制结合。二者都是通过调节机器人末端速度从而达到理想状态，所以将期望速度v_e和v_z相结合得到理想末端速度v_f，最终机器人末端理想速度v_f为：

末端速度与机器人各个关节速度的关系由机器人雅各比矩阵决定，各个关节理想速度q_cmd公式如下：

q_cmd＝J^-1v_f；

其中，J为雅各比矩阵，ν_d为机器人末端理想速度。

以上所述的实施例仅是对本发明优选方式进行的描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案做出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (10)

1.一种按摩机器人的交互式路径规划及运动控制方法，其特征在于，包括：

采集人体背部的RGB-D图像，对所述RGB-D图像进行二维轨迹规划，获得二维轨迹，将所述二维轨迹映射至三维空间，并进行在线交互式规划按摩轨迹，得到机器人控制的三维轨迹；

在所述机器人控制三维轨迹运动的同时，采集所述人体背部的点云信息，配准所述点云信息，更新所述机器人的按摩轨迹；

测量并计算所述机器人的关节扭矩，得到所述机器人末端的接触力；

通过自适应控制所述按摩轨迹和调整所述接触力进行力和位置混合控制,控制所述机器人的运动。

2.根据权利要求1所述的按摩机器人的交互式路径规划及运动控制方法，其特征在于，得到所述机器人控制的三维轨迹包括：

在所述二维轨迹中提取所述二维轨迹的像素点，并结合点云数据映射至三维空间进行在线交互式规划按摩轨迹，得到空间三维轨迹；

对所述空间三维轨迹进行拟合，得到机器人控制的三维轨迹。

3.根据权利要求2所述的按摩机器人的交互式路径规划及运动控制方法，其特征在于，映射至三维空间进行在线交互式规划按摩轨迹的方法为：

其中，c_x,c_y分别为深度图像的中心坐标，f为深度相机焦距，u,v为记录的2D轨迹中

的某一像素坐标点，z_c为该像素坐标点的深度信息，x_w,y_w和z_w分别为世界坐标系下的坐标，d_x,d_y分别为实际像素大小。

4.根据权利要求1所述的按摩机器人的交互式路径规划及运动控制方法，其特征在于，更新所述机器人的按摩轨迹包括：

通过配准所述点云数据，得到齐次变化矩阵，计算所述齐次变化矩阵，得到人体背部的平移信息和人体背部的旋转信息，判断所述平移信息和所述旋转信息是否超出预设阈值，若超出预设阈值，则视为所述人体背部发生运动，并基于所述齐次变化矩阵更新所述机器人的按摩轨迹，若未超出预设阈值，则视为所述人体背部未发生运动，所述机器人的按摩轨迹保持不变。

5.根据权利要求4所述的按摩机器人的交互式路径规划及运动控制方法，其特征在于，更新所述机器人的按摩轨迹方法为：

其中，轨迹

为当前机器人理想特征姿态，R为旋转矩阵，

为上一时刻机器人理想特征姿态，t为平移向量。

6.根据权利要求1所述的按摩机器人的交互式路径规划及运动控制方法，其特征在于，对所述关节扭矩进行计算的方法为：

F＝(J^T)^-1Ψ

其中，F为机器人末端估算力，J为雅各比矩阵，Ψ为机器人关节力矩向量。

7.根据权利要求1所述的按摩机器人的交互式路径规划及运动控制方法，其特征在于，自适应追踪所述按摩轨迹包括：

从所述机器人的按摩轨迹中，定义所述机器人接触人体背部的理想末端位置，得到理想特征姿态，所述机器人读取当前时刻的特征姿态，所述理想特征姿态和所述当前时刻的特征姿态相减，得到特征姿态误差，基于所述特征姿态误差，获得所述机器人的期望速度，基于所述期望速度，自适应追踪所述按摩轨迹。

8.根据权利要求7所述的按摩机器人的交互式路径规划及运动控制方法，其特征在于，调整所述接触力包括：

设定期望接触力，将所述接触力与所述期望接触力相减，得到接触力误差，将所述触力误差输入到所述机器人的PID控制器中，输出理想末端法向速度，通过所述理想末端法向速度控制调节所述接触力。

9.根据权利要求8所述的按摩机器人的交互式路径规划及运动控制方法，其特征在于，控制所述机器人的运动包括：

将所述期望速度和所述理想末端法向速度结合，得到理想末端速度；对所述理想末端速度进行计算，得到所述机器人的关节理想速度，基于所述理想速度，进行力和位置混合控制，控制所述机器人的运动。

10.根据权利要求9所述的按摩机器人的交互式路径规划及运动控制方法，其特征在于，对所述理想机器人关节运动速度进行计算为：

q_cmd＝J^-1ν_d

其中，J为雅各比矩阵，ν_d为机器人末端理想速度。

Images