一种机器人的助力导航方法与流程

1.本发明涉及机器人技术领域，尤其涉及一种机器人的助力导航方法。


背景技术：

2.近年来，随着技术愈加成熟，工业机器人应用领域也不断增加，需求持续攀升，机器人可代替或协助人类完成各种工作，医疗机器人也是如此。
3.腔镜手术机器人作为一种高级机器人平台越来越受到医生青睐，其设计理念是通过使用微创的方法实施复杂的外科手术。腔镜手术机器人由外科医生控制台、患者手术台和成像系统组成。其中患者手术台作为移动操作平台，需要与助手医生进行长时间的协作交互，其灵活方便的移动能力可有效降低助手医生的额外工作负担。因此，构建相应的助力导航系统对于提升腔镜手术机器人的使用体验具有十分重要意义。


技术实现要素：

4.发明目的：本发明针对上述不足，提出一种机器人的助力导航方法，可以与助手医生进行高效协作交互，有效降低医生的额外工作负担。
5.技术方案：
6.一种机器人的助力导航方法，包括步骤：
7.获取机器人移动的速度信息以及助力信息，构建其助力运动模型；
8.进行机器人的路径规划，在执行路径过程中采集机器人的航向角和速度信息，构建其主动牵引运动模型，并结合是否有障碍物及前述助力运动模型控制机器人运动。
9.所述速度信息包括机器人移动的线加速度和旋转角速度，由传感器测量获得。
10.对机器人移动的线加速度进行积分得到机器人的移动速度。
11.根据以下步骤构建助力运动模型：
12.获取机器人的在机器人的直行方向和与其垂直方向上的推力f
x
和fy；
13.根据推力与速度之间的线性关系，得到在手推力助力作用下机器人的命令速度vf和命令角速度ωf：
[0014][0015][0016]
其中，v为机器人的移动速度，通过机器人移动的线加速度进行积分计算得到；ω为机器人移动时的旋转角速度；m为机器人的质量；j为机器人的转动惯量；l为手推力施力点与机器人前轮之间在机器人直行方向上的距离；k1与k2为调整助力效果的系数。
[0017]
根据以下步骤构建主动牵引运动模型：
[0018]
获取机器人的实时航向偏转角θ：
[0019]
[0020]
其中，为机器人的实时航向角与参考航向角之间的角度差，参考航向角为规划路径的切线朝向；k为比例系数；e为机器人当前位置与规划路径之间的在垂直于机器人的直行方向上的距离；v为机器人的移动速度；
[0021]
计算得到在当前位置下机器人的牵引角速度：
[0022][0023]
采用pi控制器生成机器人的牵引线速度：
[0024]
vn＝k
p
*(δd) ki*∫δd*dt
[0025]
其中，δd为当前位置与规划路径中下一目标点位置的距离差，k
p
为距离误差比例系数，ki为距离误差的积分系数。
[0026]
当机器人的当前位置与终点的距离在预设范围内时，采用助力运动模型控制机器人运动。
[0027]
所述进行机器人的路径规划具体如下：
[0028]
根据机器人的起点以及终点设定指定的特征过路点，根据特征过路点生成贝塞尔曲线并对所有特征过路点进行贝塞尔曲线平滑从而得到所有路径插值点，即得到规划路径。
[0029]
相邻两个特征过路点之间的路径插值计算如下：
[0030]
p
ε
＝ps*(1-ε)3 3p
c1
*(1-ε)2*ε 3p
c2
*(1-ε)*ε2 pe*ε3[0031]
其中，ps和p
ε
分别为相邻两个特征过路点坐标；p
c1
和p
c2
分别为该条贝塞尔曲线的中间控制点坐标，对其位置进行随机拖动以控制贝塞尔曲线的生成；ε为插值点的稀疏控制参数，取值范围为(0,1)。
[0032]
对ε以固定间隔进行递增，且在每次插值后得到的路径插值点从起点开始计算所有相邻两点之间的距离，判断其是否大于设定距离阈值，若小于则剔除相邻两点中后一点，其中，若后一点为终点，则剔除前一点；若大于，则以该次插值后得到的路径插值点生成路径。
[0033]
在执行路径过程中采用uwb基站进行测距定位或采用rfid标签进行定位。
[0034]
在机器人前端布置有若干个超声波避障传感器用于感知障碍物。
[0035]
若所述机器人前端有障碍物，则机器人通过助力实现运动；若所述机器人前端无障碍物，则机器人通过主动牵引实现运动。
[0036]
通过安装在所述机器人的手推机构上的力传感器获取机器人的助力信息。
[0037]
所述力传感器数量为2，对称安装于所述机器人的手推机构上。
[0038]
有益效果：本发明构建了机器人的助力运动模型和主动牵引运动模型，并根据实际情况进行选择控制，灵活地将机器人的助力运动和主动牵引运动结合起来，操作简便快捷，反应迅速，可以与助手医生进行高效协作交互，有效降低医生的额外工作负担。
附图说明
[0039]
图1为手术机器人的结构示意简图；
[0040]
图2为手术机器人底盘机构简图；
[0041]
图3为手术机器人底盘助力控制示意图；
[0042]
图4为手术机器人助力导航方案示意图；
[0043]
图5为对规划路径进行跟踪控制的流程图。
[0044]
其中：
[0045]
11为底盘，12为立柱，13为预摆位结构，14为机械臂，15为手推机构；
[0046]
21为imu传感器，22为力传感器，23为超声波避障传感器。
具体实施方式
[0047]
下面结合附图和具体实施例，进一步阐明本发明。
[0048]
如图1所示，手术机器人(也即患者手术台)包括用于行走及支撑的底盘11、安装于底盘上的立柱12、安装在立柱12顶部的预摆位结构13以及若干机械臂14，立柱12上安装有用于助力操作的手推机构15。
[0049]
如图1、2所示，在手术机器人底盘11上安装imu传感器21，用于测量底盘移动时的线加速度、旋转角速度以及底盘的航向角；在手术机器人的手推机构上对称安装有两个力传感器22，用于测量助力操作时手推力；在手术机器人底盘前端布置有4个超声波避障传感器23用于感知局部障碍，图2中箭头起始所在位置表示各传感器的安装位置，以进行简单示意。
[0050]
那么可以根据imu传感器和力传感器采集得到的数据计算得到当前底盘的实际速度和手推力；
[0051]
通过对imu传感器测量得到的底盘移动时的线加速度进行积分计算得到当前底盘的实际速度v：
[0052][0053]
其中，v0为底盘在上一时刻的实际速度；a
x
、ay分别为底盘移动时线加速度在手术机器人坐标系x轴和y轴上的线加速度，由imu传感器直接测量获得；
[0054]
手推力为：
[0055]fx
＝0.5*(f
lx
f
rx
)
[0056]fy
＝0.5*(f
ly
f
ry
)
[0057]
其中，f
x
、fy为手推力在手术机器人坐标系中x、y轴上的推力；f
lx
、f
ly
和f
rx
、f
ry
分别为两个力传感器测量得到的力在x轴和y轴上的分量。
[0058]
其中，手术机器人坐标系以底盘直行方向为x轴，以水平方向上与底盘直行方向垂直的方向为y轴，如图3所示。
[0059]
本发明的手术机器人的助力导航方法包括如下步骤：
[0060]
s1：构建手术机器人的助力运动模型；
[0061]
根据力学可得手推力如下：
[0062]fx
＝ma
[0063]fy
＝jω
[0064]
其中，m为手术机器人的质量，j为手术机器人的转动惯量；
[0065]
由于手术机器人重量较大，不可能完全通过手推力实现手术机器人运动，可以通过手术机器人内的动力机构根据手推力助力控制运动，以实现对应的移动线加速度和旋转
角速度，其控制系数可以设置为k1和k2，则实际手推力如下：
[0066]fx
＝k1ma
[0067]fy
＝k2jω
[0068]
则可得在手推力助力作用下底盘的命令速度vf和命令角速度ωf：
[0069][0070][0071]
其中，ω为底盘移动时的旋转角速度，由imu传感器直接测量获得；l为手推机构(即手推力施力点)与底盘前轮之间在底盘直行方向上的距离；
[0072]
通过调节k1与k2的大小即可调整手推力助力的效果；
[0073]
s2：构建手术机器人的主动牵引运动模型；包括步骤：
[0074]
采用路径牵引进行路径规划，并控制手术机器人执行规划路径，并在执行过程中采用stanley横向控制方法对规划路径进行跟踪控制，如图5所示，具体如下：
[0075]
获取手术机器人底盘的实时航向偏转角θ：
[0076][0077]
其中，为手术机器人底盘的实时航向角与参考航向角之间的角度差，手术机器人底盘的实时航向角通过imu传感器测量得到，参考航向角为规划路径的切线朝向；k为比例系数；e为手术机器人当前位置与规划路径之间的在手术机器人坐标系中y轴方向上的距离；v为手术机器人底盘的实际速度，由s1中获得；
[0078]
则可计算得到在当前位置下手术机器人的牵引角速度为：
[0079][0080]
采用pi控制器生成手术机器人的牵引线速度：
[0081]
vn＝k
p
*(δd) ki*∫δd*dt
[0082]
其中，δd为当前位置与规划路径中下一目标点位置的距离差，k
p
为距离误差比例系数，ki为距离误差的积分系数；
[0083]
当手术机器人的当前位置与终点的距离在预设范围内时，即手术机器人运动到终点附近时，停止计算牵引线速度与牵引角速度，采用助力运动模型控制手术机器人运动；
[0084]
其中，采用路径牵引进行路径规划如图4所示，其中，本实施例中手术机器人的定位方式采用uwb基站进行测距定位，则路径规划具体为：
[0085]
根据手术机器人的起点以及终点设定指定的特征过路点，根据特征过路点生成贝塞尔曲线对路径进行平滑从而得到路径插值点，即可得到规划路径；
[0086]
其中，相邻两个特征过路点之间路径插值计算如下：
[0087]
p
ε
＝ps*(1-ε)3 3p
c1
*(1-ε)2*ε 3p
c2
*(1-ε)*ε2 pe*ε3[0088]
其中，ps和p
ε
分别为相邻两个特征过路点坐标，p
c1
和p
c2
分别为该条贝塞尔曲线的中间控制点坐标，其位置可以进行随机拖动用于控制贝塞尔曲线的生成，ε为插值点的稀疏控制参数，取值范围为(0,1)；
[0089]
当进行贝塞尔曲线插值时，对ε以固定间隔进行递增，且在每次插值后得到的路径
插值点从起点开始计算所有相邻两点之间的距离，判断其是否大于设定距离阈值，若小于则剔除相邻两点中后一点，其中，若后一点为终点，则剔除前一点；若大于，则以该次插值后得到的路径插值点生成路径；
[0090]
依次对所有特征过路点进行贝塞尔曲线平滑，生成全局路径序列，完成路径规划。
[0091]
本发明中，在路径规划和控制手术机器人执行的过程中需要对手术机器人进行定位，本发明采用常规的室内定位方法确定手术机器人的位置，更具体地，可以采用uwb基站进行测距定位，也可以采用rfid标签进行定位。
[0092]
s3：构建速度混合器，并据此控制手术机器人；
[0093]
通过底盘前端的超声波避障传感器检测是否出现障碍物，若是，则手术机器人底盘的最终命令线速度与角速度分别为vf和ωf，即手术机器人需要通过操作人员的手推力实现运动；若否，则手术机器人底盘的最终命令线速度与角速度分别为vn和ωn，此时无需通过操作人员的手推力，通过主动牵引即可实现运动；
[0094]
那么根据最终手术机器人的线速度和角速度结合差速底盘模型，计算出底盘左轮速度v
l
与右轮速度vr，发送至底盘驱动器执行相关的命令。
[0095]
在本发明中，是以手术机器人为例，但是其实本发明适用于所有行走机器人的助力导航。
[0096]
本发明通过安装imu传感器采集机器人底盘的线加速度、旋转角速度以及底盘的航向角，同时通过力传感器采集手推力大小，从而可以计算得到在手推力助力作用下机器人底盘的命令速度和角速度，从而构建了机器人的助力运动模型；然后通过路径牵引进行路径规划并执行，并由此计算得到机器人底盘的主动牵引速度和角速度，从而建立机器人的主动牵引运动模型，然后根据机器人底盘前端安装的超声波避障传感器检测是否出现障碍物来具体选择通过手推力控制机器人运动还是通过主动牵引控制机器人运动，操作简便快捷，反应迅速，可以与助手医生进行高效协作交互，有效降低医生的额外工作负担。
[0097]
以上详细描述了本发明的优选实施方式，但是本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换(如数量、形状、位置等)，这些等同变换均属于本发明的保护范围。