CN114473998A - 一种自动开门的智能服务机器人*** - Google Patents

Abstract

一种自动开门的智能服务机器人***，包括医护机器人硬件平台以及开门软件平台；医护机器人硬件平台包括机器人智能移动平台、机械臂开门装置以及计算机识别定位装置；机器人智能移动平台包括AGV移动底盘、供电***、工控机、路由器以及运动控制装置，开门软件平台安装在医护机器人的所述工控机上，包括物体识别检测模块、坐标系转换模块、机械臂运动模块、物体抓取模块、运动模块、机械臂柔顺控制模块；将基于速度的以及基于位置的控制器相结合共同作用于机械臂移动过程，最终完成开门任务。本发明能帮助行动不便的患者完成开门操作。

Description

一种自动开门的智能服务机器人***

技术领域

本发明涉及智能机器人领域，具体涉及一种自动开门的智能服务机器人***。

背景技术

随着当今医疗水平的不断提高，我国人口的平均寿命也在不断提升，而当今的人口出生率不断地降低，因此，人口老龄化问题愈发严重。随之而来的，照顾老年人的医护成本不断提升，且一些术后无法自由活动的患者以及一些体弱的老年人在病房中想要出门做检查或者散心，却无法自己打开门，需要护士帮忙的需求增大，相比之下我国医护资源紧缺，具有护理专业知识的医护人员缺口巨大，因此护士的工作压力也逐渐增高。

为了解决上述问题，雷浩提出了一种机器人开门控制方法、装置和电子设备(雷浩.一种机器人开门控制方法、装置和电子设备[P].上海市：CN113386138A,2021-09-14.)，该装置通过按压门把手的方式来进行开门，然而按压过程中无法灵活的控制按压力大小，容易造成机械臂损伤或者门把手被压坏。

发明内容

本发明要克服现有技术的上述问题，提出一种自动开门的智能服务机器人***。

本发明的一种自动开门的智能服务机器人***，其特征在于：包括医护机器人硬件平台以及开门软件平台。

所述的医护机器人硬件平台包括机器人智能移动平台、机械臂开门装置以及计算机识别定位装置。

所述机器人智能移动平台包括AGV移动底盘、供电***、工控机、嵌入式控制器、路由器以及运动控制装置，所述AGV移动底盘包括驱动轮、麦克纳姆轮、超声波传感器和激光雷达，所述工控机安装于所述移动底盘上方，所述工控机拥有室内导航模块，通过由所述路由器提供的ETH网络连接的激光雷达传递而来的数据对室内环境进行建图与导航，通过连接所述路由器提供的ETH网络运动控制装置接收同一局域网下所述工控机传递而来的指令处理所述超声波传感器所获得的数据，来对室内环境中的障碍物进行检测；所述工控机通过局域网将控制指令传递给运动控制模块，所述运动控制模块通过CAN总线将控制指令发送给所述嵌入式控制器，同时所述嵌入式控制器也将返回反馈数据给所述运动控制装置，所述嵌入式控制器将PWM信号传递给2路H桥进行电机驱动控制，所述2路电桥同时通过电流采样IC来将电流信号反馈给所述嵌入式控制器，以及将电机电压信号传递给两个电机进行电机运转，电机通过光电编码器将转速信号反馈给所述嵌入式控制器，同时电机获得驱动转动信号用于驱动所述驱动轮转动，且驱动轮带动所述麦克纳姆轮进行机器人整体的运动；所述供电***包括电源管理器、变压器以及锂电池，所述运动控制装置通过485总线连接供电***，所述电源管理器用于防止所述电源过载，所述变压器用于对所述锂电池电压进行升降压处理来连接机器人内部各个元器件；

所述机械臂开门装置安装于所述智能移动平台上方，包括机械臂、末端执行器和躯干部分，所述机械臂置于所述医护机器人左侧，所述机械臂末端安装所述末端执行器，所述躯干部分安装在所述医护机器人右侧部分，包括交互屏、载物台以及抬升杆，所述交互屏通过USB总线用于显示所述工控机的控制界面，所述载物台用于搭载所述机械臂，所述抬升杆与通过局域网接收所述工控机控制指令的运动控制装置通过CAN总线相连接，用于控制躯干部分的整体高度。

所述计算机识别装置包括双目RGBD相机以及四自由度云台，所述双目RGBD相机置于所述四自由度云台之上，所述工控机通过USB总线与所述双目RGBD相机连接，将所述RGBD相机获取的环境信息进行处理，通过深度信息以及RGB图像利用目标检测算法，用以完成待抓取物体的识别与定位，所述4自由度云台与运动控制装置通过485总线相连接，用以改变所述RGBD相机的角度。

所述的开门软件平台安装在医护机器人的所述工控机上，包括物体识别检测模块、坐标系转换模块、机械臂运动模块、物体抓取模块、运动模块、机械臂柔顺控制模块，以及安装在嵌入式控制器上的驱动轮控制模块、升降杆控制模块、四自由度云台控制模块。

各模块的具体构成是：驱动轮控制模块，控制驱动轮转动；从运动模块输入速度信息，并通过PID控制器调整驱动轮内部电机转速，控制驱动轮的转动。

升降杆控制模块，控制升降杆运动的模块；从机械臂运动模块输入速度信息，并通过PID控制器调整升降杆内部电机转速，控制升降杆运动。

四自由度云台控制模块，控制四自由度云台运动；从机械臂运动模块输入速度信息，并通过PID控制器调整四自由度云台内部的电机转速，控制四自由度云台的转动。

物体识别检测模块，识别检测物体；通过目标检测算法以及RGBD相机传入的实时数据，识别检测出周围环境中门与门把手的信息，并得到门与门把手位于相机像素坐标系中的坐标，再将门的位置信息输出给运动模块，将门把手像素坐标输出给坐标系转换模块。

坐标系转换模块，转换物体坐标；接收物体识别模块的门位置信息，并将其转换成为位于机械臂基坐标系下的坐标，再输出给机械臂运动模块。

机械臂运动模块，控制机械臂运动到目标位置；接收坐标系转换模块传入的机械臂基坐标系下门把手坐标信息，设为机械臂末端所需要到达的位置，通过逆运动学求解获得机械臂各个关节所需要旋转的角度，并通过机械臂的轨迹规划获取执行最优路径，使得机械臂运动至待抓取物体处，同时，机械臂运动模块将速度信息输出给升降杆控制模块以及四自由度云台控制模块。

物体抓取模块，用于控制机械臂末端执行器；接收工控机的控制指令来控制机械臂末端夹爪闭合，完成物体抓取与放置操作。

运动模块，控制机器人智能移动平台进行轨迹规划，动态避障；接收超声波传感器以及激光雷达获得的周围环境的数据，并与物体检测模块结合，通过接收物体检测模块传入的门的位置信息定位出门位于整体环境的位置并通过路径规划算法使得工控机通过局域网将运动信息传输给运动控制装置，再通过CAN总线将控制指令发送给嵌入式控制器，同时嵌入式控制器也将返回反馈数据给运动控制装置，嵌入式控制器将PWM信号传递给2路H桥进行电机驱动控制，2路电桥同时通过电流采样IC来将电流信号反馈给嵌入式控制器，以及将电机电压信号传递给两个电机进行电机运转，电机通过光电编码器将转速信号反馈给嵌入式控制器，同时电机获得驱动转动信号用于驱动驱动轮转动，实现机器人可移动平台的运动。

机械臂柔顺控制模块，是一个用于机械臂运动的控制器，通过机械臂末端实时力反馈修正控制机械臂的各个关节的旋转角度；

机械臂柔顺控制模块与环境的模型为：

m(a_d-a)+b(v_d-v)+k(x_d-x)＝F-f (1)

其中m、b、k分别表示质量、空气阻尼以及弹性系数。a_d,v_d,x_d分别为期望的加速度、速度和位置，a,v,x分别为实际的加速度、速度和位置。F,f分别表示该模型中的期望交互力以及实际交互力。在实际开门任务中，由于开门力矩实时变化，因此加速度以及速度变为0，因此将上式可变为：

ma_d+bv_d＝F-f (2)

将上式离散化，T为离散时间周期，将上式变形为：

其中，v_s+1分别表示s+1时刻的末端速度，整理可得基于速度的柔顺控制器，

然而，在实际使用中，由于该控制器没有引入位置参数，因此无法对位置偏差进行校正。在考虑位姿信息后可得机械臂与环境的新模型为：

其中Δx表示期望位姿的偏差，将Δx离散后可得：

其中Δx₀表示机械臂刚开始运动时，末端与期望位置的差值。该柔顺控制器最终可变为：

同时，本发明在所提出的上述基于位置的控制器的基础上又提出一种基于速度的机械臂运动控制器，用来抑制机械臂运动过程中的抖动。

引入自适应变量ρ：

其中ρ_k，Δρ_k分别代表k时刻时自适应参数以及期望力与实际力的误差；ρ表示接触力的精度，将自适应变量ρ引入导纳控制器,可得：

M'_k＝M₀kρ_k+m (8)

N'_k＝N₀kρ_k+n (9)

其中M'_k，N'_k分别表示时刻k的质量参数和阻尼参数，M₀,N₀为理想条件下的阻抗参数。m,n分别为自适应的权重参数值。

因此，本发明将上述基于速度的以及基于位置的控制器相结合共同作用于机械臂移动过程，最终完成开门任务。

开门软件平台在静态环境下自动开门时执行以下步骤：

S1：根据上述给出的医护机器人平台，对所述的KINOVA机械臂101以及所述RGBD相机9进行手眼标定，用以将物体在相机坐标系下的坐标转换为物体在机械臂基坐标系下的坐标，由于硬件的设计要求，本发明所进行的标定是眼在手外的标定方式；

S2：通过所述物体检测模块识别出门把手的位置；

S3：将步骤S2中的所检测到的门把手的位置通过所述坐标系转换模块将门把手在相机像素坐标系下的坐标转换为在机械臂基坐标系下的坐标；

S4：通过所述机械臂运动模块使得机械臂移动至S3所提供的位置处，并通过GPD物***姿检测算法求得抓取门把手时最合适的抓握姿势，同时所述NODE卡将速度信息传输给所述升降杆以及所述四自由度云台，采用PID控制来使得所述升降杆调整高度使得所述机械臂移动至合适的位置，同时所述四自由度云台调整角度使得相机更好的进行环境检测；

S5：通过所述物体抓取模块使得所述机械臂夹爪夹住门把手；

S6：给出门被打开时门把手的旋转角度，并将其作为机械臂末端的目标位置，通过机械臂运动模块使得所述机械臂运动至门把手旋开状态，在机械臂开门过程中采用所述机械臂柔顺控制模块通过引入位姿反馈使得所述机械臂能够根据反馈值实时调整各关节角度值，实现开门动作；

开门软件平台在动态环境下开门时，执行以下步骤：

T1：根据上述给出的医护机器人平台，对所述的KINOVA机械臂以及kinect相机进行手眼标定，用以将物体在相机坐标系下的坐标转换为物体在机械臂基坐标系下的坐标，由于硬件的设计要求，所进行的标定是眼在手外的标定方式；

T2：利用SLAM中的Cartographer算法根据移动机器人的里程计数据和激光雷达数据(通过kinect相机所获取的深度消息转换为雷达消息)来绘制二维的栅格地图，实现通过Cartographer算法来构建当前病房环境中的地图；

T3：通过ROS***中所提供的map_server功能包中提供的map_saver以及map_server节点来完成栅格地图数据的转存，map_server节点将栅格地图保存到本地，map_server节点通过service方式将栅格地图发布；

T4：通过ROS***中的amcl功能包，实现导航中的机器人定位用以确定机器人的位置，并以此作为机器人移动过程中的起始位置点；

T5：患者给机器人下达开门指令，所述医护机器人接收到指令后，通过所述物体检测模块识别检测出门这一物体；

T6：在步骤T5的基础上将门的信息通过局域网传递给所述运动模块，并以此作为目标位置，此时利用ROS中的navigation导航功能包提供的move_base功能包实现移动底盘的路径规划，move_base能够根据给定的目标点来进行导航，同时所述NODE卡将速度信息传递给所述驱动轮，通过PID控制完成移动底盘的运动，最终控制所述医护机器人移动到门口；

T7：通过所述RGBD相机传入的实时环境数据，所述物体检测模块获取到门把手的位置信息；

T8：在步骤T7的基础上将所检测到的门把手的位置通过所述坐标系转换模块将门把手在相机像素坐标系下的坐标转换为在机械臂基坐标系下的坐标；

T9：将步骤T8中获取的门把手信息设为机械臂末端目标位置信息并通过所述机械臂运动模块使得机械臂末端运动至门把手处，通过GPD物***姿检测算法求得抓取门把手时最合适的抓握姿势，同时所述NODE卡将速度信息传输给所述升降杆以及所述四自由度云台，采用PID控制来使得所述升降杆调整高度使得所述机械臂移动至合适的位置，同时所述四自由度云台调整角度使得相机更好的进行环境检测；

T10：通过所述物体抓取模块使得所述机械臂夹爪夹住门把手；

T11：给出门被打开时门把手的旋转角度，并将其作为机械臂末端的目标位置，通过机械臂运动模块使得所述机械臂运动至门把手旋开状态，在机械臂开门过程中采用所述机械臂柔顺控制模块通过引入位姿反馈使得所述机械臂能够根据反馈值实时调整各关节角度值，实现开门动作。

进一步，所述机械臂包括从下而上的基座、大臂、肩关节、腰关节、肘关节、小臂以及腕关节，其中所述腕关节为所述机械臂末端关节，其接口处通过485总线与所述末端执行器相连接，所述机械臂基座安装于所述载物台上。

进一步，所述机械臂装置与所述交互屏下方装有所述抬升杆，通过所述抬升杆实现所述机器人整体高度的变化。

本发明的有益效果表现在：本发明可以应用于医护环境下，用于帮助缓解医护人员不足的情况。在医院病房中，用于帮助行动不便的患者完成开门操作，能够有效的减轻医护人员的工作压力。

附图说明

图1位本发明的医护机器人硬件结构示意图；

图2位本发明的医护机器人的硬件框架图；

图3为本发明的静态环境下开门流程框图；

图4为本发明的动态环境下开门流程框图；

图5为移动底盘的连接图；

图6为工控机与嵌入式控制器软件的联系图。

其中：1为变压器，2为电源，3为麦克纳姆轮，4为工控机，5为路由器，6为激光雷达，7为屏幕，8为铁箱，9为相机，10为用于开门的机械手，101为机械臂，102为机械臂末端夹爪，11为铝型材搭建的底盘，12为驱动轮，13为超声波传感器，14为4自由度云台，15为载物台，16为抬升杆，17为NODE卡，18为电源管理器，19为嵌入式控制器。

具体实施方式

以下结合附图和具体实施例对本发明作详细描述。注意，以下结合附图和具体实施例描述的诸方面仅是示例性的，而不应被理解为对本发明的保护范围进行任何限制。

本发明的一种自动开门的智能服务机器人***，其特征在于：包括医护机器人硬件平台以及开门软件平台。

所述的医护机器人硬件平台包括AGV移动底盘、供电***、工控机4、嵌入式控制器19、路由器5以及运动控制装置NODE卡17，其中所述工控机4为NUC控制器，所述AGV移动底盘包括驱动轮12、麦克纳姆轮3，所述超声波传感器13和所述激光雷达6安装于所述AGV移动底盘的外侧，所述工控机4安装于所述移动底盘11上，所述工控机4拥有室内导航模块，通过由所述路由器5提供的ETH网络连接的所述激光雷达6传递而来的数据对室内环境进行建图与导航，通过连接所述路由器5提供的ETH网络所述运动控制装置17接收同一局域网下所述工控机4传递而来的指令处理所述超声波传感器13所获得的数据，来对室内环境中的障碍物进行检测；结合图5，所述工控机4通过局域网将控制指令传递给所述运动控制装置17，所述运动控制装置17通过CAN总线来将控制指令发送给所述嵌入式控制器19，所述嵌入式控制器19采用STM32F103单片机，同时所述STM32F103单片机也返回反馈数据给所述运动控制装置，所述STM32F103单片机将PWM信号传递给2路H桥进行电机驱动控制，所述2路电桥同时通过电流采样IC来将电流信号反馈给STM32F103单片机，以及将电机电压信号传递给两个电机进行电机运转，电机通过光电编码器将转速信号反馈给STM32F103单片机，同时电机获得驱动转动信号用于驱动所述驱动轮12转动，且所述驱动轮12带动所述麦克纳姆轮3进行机器人整体的运动；所述供电***包括电源管理器、变压器以及锂电池，所述运动控制装置17通过485总线连接供电***，所述电源管理器18用于防止所述电源2过载，所述变压器1用于对所述锂电池2电压进行升降压处理来连接机器人内部各个元器件，其中所述电池2为48V20Ah的锂电池，所述变压器1包括12V、24V、36V三类变压器，通过12V变压器给所述路由器5、所述运动控制装置17、所述工控机4、所述驱动轮12、所述超声波传感器13、所述激光雷达6、所述抬升杆16、所述RGBD相机9、所述4自由度云台14以及所述嵌入式控制器19供电，通过24V变压器给所述机械臂101供电，通过36V变压器给所述交互屏7供电。

所述机械臂开门装置安装于所述智能移动平台上方，包括机械臂、末端执行器和躯干部分，所述机械臂101为kinova7自由度机械臂，所述机械臂101末端安装所述末端执行器102，所述末端执行器102为二指夹爪，用时夹取物体，所述躯干部分安装在所述医护机器人右侧，包括交互屏、载物台以及抬升杆，所述交互屏7通过USB总线用于显示所述工控机4的控制界面，所述载物台15用于搭载所述机械臂101，其中所述载物台15与所述机械臂101基座通过法兰相连接，所述抬升杆16与通过局域网接收所述工控机4控制指令的运动控制装置17通过CAN总线相连接，用于控制躯干部分的整体高度，所述抬升杆16所能抬升的范围为0至30cm。

所述计算机识别装置包括双目RGBD相机以及四自由度云台，所述双目RGBD相机9置于所述四自由度云台14之上，所述工控机4通过USB总线与所述双目RGBD相机9连接，将所述RGBD相机9获取的环境信息进行处理，通过深度信息以及RGB图像利用目标检测算法，用以完成待抓取物体的识别与定位，所述4自由度云台14与所述运动控制装置17通过485总线相连接，用以改变所述RGBD相机的角度。

进一步，所述机械臂101为kinova7自由度机械臂，包括从下而上的基座、大臂、肩关节、腰关节、肘关节、小臂以及腕关节，其中所述腕关节为所述机械臂101末端关节，其接口处通过485总线与所述末端执行器102相连接，所述机械臂基座安装于所述载物台15上。

本发明的所述的开门软件平台安装在医护机器人的所述工控机上，软件模块包括药液有无检测模块、物体识别检测模块、坐标系转换模块、机械臂运动模块、物体抓取模块、运动模块、机械臂柔顺控制模块，以及安装在嵌入式控制器上的驱动轮控制模块、升降杆控制模块、四自由度云台控制模块。

各模块的具体构成为：

驱动轮控制模块，控制驱动轮转动；从运动模块输入速度信息，并通过PID控制器调整驱动轮内部电机转速，控制驱动轮的转动。

升降杆控制模块，控制升降杆运动的模块；从机械臂运动模块输入速度信息，并通过PID控制器调整升降杆内部电机转速，控制升降杆运动。

四自由度云台控制模块，控制四自由度云台运动；从机械臂运动模块输入速度信息，并通过PID控制器调整四自由度云台内部的电机转速，控制四自由度云台的转动。物体识别检测模块，识别检测物体；通过目标检测算法以及RGBD相机识别出待检测物体的位置坐标。

进一步的，目标检测算法采用Yolov4算法，通过训练门与门把手数据的数据集，实时处理RGBD相机9所获取的机器人的周围环境信息，检测出待识别的门与门把手信息，且获得门与门把手在相机像素坐标系下的位置坐标，再将门的位置信息输出给所述运动模块，将门把手像素坐标输出给坐标系转换模块。

坐标系转换模块，转换物体坐标；在完成机械臂眼在手上的手眼标定后，接收物体识别模块的门位置信息，通过ROS***中的TF变换工具将门位于相机像素坐标系下的坐标转换为门位于机械臂基坐标系下的坐标，再将其输出给机械臂运动模块。

机械臂运动模块，控制所述机械臂运动到目标位置；接收坐标系转换模块传入的机械臂基坐标系下门把手坐标信息使得机械臂101能将其设为末端目标点后进行逆运动学求解，进一步获取机械臂101各个关节的运动角度，通过RRT算法进行机械臂运动的轨迹规划，使得机械臂101能够运动至待检测物体处，同时，机械臂运动模块将速度信息输出给升降杆控制模块以及四自由度云台控制模块。

物体抓取模块，控制所述机械臂末端夹爪102的开启与闭合，完成物体抓取与放置操作。

运动模块，控制机器人智能移动平台进行轨迹规划，动态避障，接收物体识别检测模块输入的门的位置信息，将其设为目标位置并使机器人智能移动平台运动到目标位置；运动模块可根据超声波传感器13以及激光雷达6获得的周围环境的数据，并与物体检测模块结合，可以根据物体检测模块中所识别物体的位置信息定位出该物***于整体环境的位置并通过A*路径规划算法使得工控机通过局域网将运动信息传输给运动控制装置，再通过CAN总线将控制指令发送给嵌入式控制器19，同时嵌入式控制器19返回反馈数据给运动控制装置NODE卡17，嵌入式控制器将PWM信号传递给2路H桥进行电机驱动控制，2路电桥同时通过电流采样IC将电流信号反馈给嵌入式控制器19，以及将电机电压信号传递给两个电机进行电机运转，电机通过光电编码器将转速信号反馈给嵌入式控制器17，同时电机获得驱动转动信号用于驱动驱动轮转，实现机器人可移动平台的运动。

机械臂柔顺控制模块，是一个用于机械臂运动的控制器，通过机械臂末端实时力反馈修正控制机械臂的各个关节的旋转角度；

机械臂柔顺控制模块与环境的模型为：

m(a_d-a)+b(v_d-v)+k(x_d-x)＝F-f (1)

其中m、b、k分别表示质量、空气阻尼以及弹性系数。a_d,v_d,x_d分别为期望的加速度、速度和位置，a,v,x分别为实际的加速度、速度和位置。F,f分别表示该模型中的期望交互力以及实际交互力。在实际开门任务中，由于开门力矩实时变化，因此加速度以及速度变为0，因此将上式可变为：

ma_d+bv_d＝F-f (2)

将上式离散化，T为离散时间周期，将上式变形为：

其中，v_s+1分别表示s+1时刻的末端速度，整理可得基于速度的柔顺控制器，

然而，在实际使用中，由于该控制器没有引入位置参数，因此无法对位置偏差进行校正。在考虑位姿信息后可得机械臂与环境的新模型为：

其中Δx表示期望位姿的偏差，将Δx离散后可得：

其中Δx₀表示机械臂刚开始运动时，末端与期望位置的差值。该柔顺控制器最终可变为：

同时，本发明在所提出的上述基于位置的控制器的基础上又提出一种基于速度的机械臂运动控制器，用来抑制机械臂运动过程中的抖动。

引入自适应变量ρ：

其中ρ_k，Δρ_k分别代表k时刻时自适应参数以及期望力与实际力的误差；ρ表示接触力的精度，将自适应变量ρ引入导纳控制器,可得：

M'_k＝M₀kρ_k+m (8)

N'_k＝N₀kρ_k+n (9)

其中M'_k，N'_k分别表示时刻k的质量参数和阻尼参数，M₀,N₀为理想条件下的阻抗参数。m,n分别为自适应的权重参数值。

因此，本发明将上述基于速度的以及基于位置的控制器相结合共同作用于机械臂移动过程，最终完成开门任务。

开门软件平台在静态环境下自动开门时，执行以下步骤：

S1：根据上述给出的医护机器人平台，对所述的KINOVA机械臂101以及所述RGBD相机9进行手眼标定，用以将物体在相机坐标系下的坐标转换为物体在机械臂基坐标系下的坐标，由于硬件的设计要求，本发明所进行的标定是眼在手外的标定方式；

S2：通过所述物体检测模块识别出门把手的位置；

S3：将步骤S2中的所检测到的门把手的位置通过所述坐标系转换模块将门把手在相机像素坐标系下的坐标转换为在机械臂基坐标系下的坐标；

S4：通过所述机械臂运动模块使得机械臂移动至步骤S3所提供的位置处，并通过GPD物***姿检测算法求得抓取门把手时最合适的抓握姿势，同时所述NODE卡将速度信息传输给所述升降杆以及所述四自由度云台，采用PID控制来使得所述升降杆调整高度使得所述机械臂移动至合适的位置，同时所述四自由度云台调整角度使得相机更好的进行环境检测；

S5：通过所述物体抓取模块使得所述机械臂夹爪夹住门把手；

S6：给出门被打开时门把手的旋转角度，并将其作为机械臂末端的目标位置，通过机械臂运动模块使得所述机械臂运动至门把手旋开状态，在机械臂开门过程中采用所述机械臂柔顺控制模块通过引入位姿反馈使得所述机械臂能够根据反馈值实时调整各关节角度值，实现开门动作。

开门软件平台在动态环境下开门时，执行以下步骤：

T1：根据上述给出的医护机器人平台，对所述的KINOVA机械臂以及kinect相机进行手眼标定，用以将物体在相机坐标系下的坐标转换为物体在机械臂基坐标系下的坐标，由于硬件的设计要求，本发明所进行的标定是眼在手外的标定方式；

T2：利用SLAM中的Cartographer算法根据移动机器人的里程计数据和激光雷达数据(通过kinect相机所获取的深度消息转换为雷达消息)来绘制二维的栅格地图，实现通过Cartographer算法来构建当前病房环境中的地图；

T3：通过ROS***中所提供的map_server功能包中提供的map_saver以及map_server节点来完成栅格地图数据的转存，map_server节点将栅格地图保存到本地，map_server节点通过service方式将栅格地图发布；

T4：通过ROS***中的amcl功能包，实现导航中的机器人定位用以确定机器人的位置，并以此作为机器人移动过程中的起始位置点；

T5：患者给机器人下达开门指令，所述医护机器人接收到指令后，通过所述物体检测模块识别检测出门这一物体；

T6：在步骤T5的基础上将门的信息通过局域网传递给所述运动模块，并以此作为目标位置，此时利用ROS中的navigation导航功能包提供的move_base功能包实现移动底盘的路径规划，move_base能够根据给定的目标点来进行导航，同时所述NODE卡将速度信息传递给所述驱动轮，通过PID控制完成移动底盘的运动，最终控制所述医护机器人移动到门口；

T7：通过所述RGBD相机传入的实时环境数据，所述物体检测模块获取到门把手的位置信息；

T8：在步骤T7的基础上将所检测到的门把手的位置通过所述坐标系转换模块将门把手在相机像素坐标系下的坐标转换为在机械臂基坐标系下的坐标；

T9：将步骤T8中获取的门把手信息设为机械臂末端目标位置信息并通过所述机械臂运动模块使得机械臂末端运动至门把手处，通过GPD物***姿检测算法求得抓取门把手时最合适的抓握姿势，同时所述NODE卡将速度信息传输给所述升降杆以及所述四自由度云台，采用PID控制来使得所述升降杆调整高度使得所述机械臂移动至合适的位置，同时所述四自由度云台调整角度使得相机更好的进行环境检测；

T10：通过所述物体抓取模块使得所述机械臂夹爪夹住门把手；

T11：给出门被打开时门把手的旋转角度，并将其作为机械臂末端的目标位置，通过机械臂运动模块使得所述机械臂运动至门把手旋开状态，在机械臂开门过程中采用所述机械臂柔顺控制模块通过引入位姿反馈使得所述机械臂能够根据反馈值实时调整各关节角度值，实现开门动作。

本说明书实施例所述的内容仅仅是对发明构思的实现形式的列举，本发明的保护范围不应当被视为仅限于实施例所陈述的具体形式，本发明的保护范围也包括本领域技术人员根据本发明构思所能够想到的等同技术手段。

Claims (3)

1.一种自动开门的智能服务机器人***，其特征在于：包括医护机器人硬件平台以及开门软件平台；

所述的医护机器人硬件平台包括机器人智能移动平台、机械臂开门装置以及计算机识别定位装置；

所述机器人智能移动平台包括AGV移动底盘、供电***、工控机、嵌入式控制器、路由器以及运动控制装置，所述AGV移动底盘包括驱动轮、麦克纳姆轮、超声波传感器和激光雷达，所述工控机安装于所述移动底盘上方，所述工控机拥有室内导航模块，通过由所述路由器提供的ETH网络连接的激光雷达传递而来的数据对室内环境进行建图与导航，通过连接所述路由器提供的ETH网络运动控制装置接收同一局域网下所述工控机传递而来的指令处理所述超声波传感器所获得的数据，来对室内环境中的障碍物进行检测；所述工控机通过局域网将控制指令传递给运动控制模块，所述运动控制模块通过CAN总线将控制指令发送给所述嵌入式控制器，同时所述嵌入式控制器也将返回反馈数据给所述运动控制装置，所述嵌入式控制器将PWM信号传递给2路H桥进行电机驱动控制，所述2路电桥同时通过电流采样IC来将电流信号反馈给所述嵌入式控制器，以及将电机电压信号传递给两个电机进行电机运转，电机通过光电编码器将转速信号反馈给所述嵌入式控制器，同时电机获得驱动转动信号用于驱动所述驱动轮转动，且驱动轮带动所述麦克纳姆轮进行机器人整体的运动；所述供电***包括电源管理器、变压器以及锂电池，所述运动控制装置通过485总线连接供电***，所述电源管理器用于防止所述电源过载，所述变压器用于对所述锂电池电压进行升降压处理来连接机器人内部各个元器件；

所述机械臂开门装置安装于所述智能移动平台上方，包括机械臂、末端执行器和躯干部分，所述机械臂置于所述医护机器人左侧，所述机械臂末端安装所述末端执行器，所述躯干部分安装在所述医护机器人右侧部分，包括交互屏、载物台以及抬升杆，所述交互屏通过USB总线用于显示所述工控机的控制界面，所述载物台用于搭载所述机械臂，所述抬升杆与通过局域网接收所述工控机控制指令的运动控制装置通过CAN总线相连接，用于控制躯干部分的整体高度；

所述计算机识别装置包括双目RGBD相机以及四自由度云台，所述双目RGBD相机置于所述四自由度云台之上，所述工控机通过USB总线与所述双目RGBD相机连接，将所述RGBD相机获取的环境信息进行处理，通过深度信息以及RGB图像利用目标检测算法，用以完成待抓取物体的识别与定位，所述4自由度云台与运动控制装置通过485总线相连接，用以改变所述RGBD相机的角度；

所述的开门软件平台包括安装在医护机器人的所述工控机上的物体识别检测模块、坐标系转换模块、机械臂运动模块、物体抓取模块、运动模块、机械臂柔顺控制模块，以及安装在嵌入式控制器上的驱动轮控制模块、升降杆控制模块、四自由度云台控制模块；

各模块的具体构成是：

驱动轮控制模块，控制驱动轮转动；从运动模块输入速度信息，并通过PID控制器调整驱动轮内部电机转速，控制驱动轮的转动。

升降杆控制模块，控制升降杆运动的模块；从机械臂运动模块输入速度信息，并通过PID控制器调整升降杆内部电机转速，控制升降杆运动。

四自由度云台控制模块，控制四自由度云台运动；从机械臂运动模块输入速度信息，并通过PID控制器调整四自由度云台内部的电机转速，控制四自由度云台的转动。

物体识别检测模块，识别检测物体；通过目标检测算法以及RGBD相机传入的实时数据，识别检测出周围环境中门与门把手的信息，并得到门与门把手位于相机像素坐标系中的坐标，再将门的位置信息输出给运动模块，将门把手像素坐标输出给坐标系转换模块。

坐标系转换模块，转换物体坐标；接收物体识别模块的门位置信息，并将其转换成为位于机械臂基坐标系下的坐标，再输出给机械臂运动模块。

机械臂运动模块，控制机械臂运动到目标位置；接收坐标系转换模块的机械臂基坐标系下门把手坐标信息，设为机械臂末端所需要到达的位置，通过逆运动学求解获得机械臂各个关节所需要旋转的角度，并通过机械臂的轨迹规划获取执行最优路径，使得机械臂运动至待抓取物体处，同时，机械臂运动模块将速度信息输出给升降杆控制模块以及四自由度云台控制模块。

物体抓取模块，控制机械臂末端执行器；接收工控机的控制指令来控制机械臂末端夹爪闭合，完成物体抓取与放置操作；

运动模块，控制机器人智能移动平台进行轨迹规划，动态避障；接收超声波传感器以及激光雷达获得的周围环境的数据，并与物体检测模块结合，通过接收物体检测模块传入的门的位置信息定位出门位于整体环境的位置并通过路径规划算法使得工控机通过局域网将运动信息传输给运动控制装置，再通过CAN总线将控制指令发送给嵌入式控制器，同时嵌入式控制器也将返回反馈数据给运动控制装置，嵌入式控制器将PWM信号传递给2路H桥进行电机驱动控制，2路电桥同时通过电流采样IC来将电流信号反馈给嵌入式控制器，以及将电机电压信号传递给两个电机进行电机运转，电机通过光电编码器将转速信号反馈给嵌入式控制器，同时电机获得驱动转动信号用于驱动驱动轮转动，实现机器人可移动平台的运动；

机械臂柔顺控制模块，是一个用于机械臂运动的控制器，通过机械臂末端实时力反馈修正控制机械臂的各个关节的旋转角度；

机械臂柔顺控制模块与环境的模型为：

m(a_d-a)+b(v_d-v)+k(x_d-x)＝F-f (1)

其中m、b、k分别表示质量、空气阻尼以及弹性系数；a_d,v_d,x_d分别为期望的加速度、速度和位置，a,v,x分别为实际的加速度、速度和位置；F,f分别表示该模型中的期望交互力以及实际交互力；在实际开门任务中，由于开门力矩实时变化，因此加速度以及速度变为0，因此将上式可变为：

ma_d+bv_d＝F-f (2)

将上式离散化，T为离散时间周期，将上式变形为：

其中，v_s+1分别表示s+1时刻的末端速度，整理可得基于速度的柔顺控制器，

然而，在实际使用中，由于该控制器没有引入位置参数，因此无法对位置偏差进行校正；在考虑位姿信息后可得机械臂与环境的新模型为：

其中Δx表示期望位姿的偏差，将Δx离散后可得：

其中Δx₀表示机械臂刚开始运动时，末端与期望位置的差值；该柔顺控制器最终可变为：

同时，本发明在所提出的上述基于位置的控制器的基础上又提出一种基于速度的机械臂运动控制器，用来抑制机械臂运动过程中的抖动；

引入自适应变量ρ：

其中ρ_k，Δρ_k分别代表k时刻时自适应参数以及期望力与实际力的误差；ρ表示接触力的精度，将自适应变量ρ引入导纳控制器,可得：

M'_k＝M₀kρ_k+m (8)

N'_k＝N₀kρ_k+n (9)

其中M'_k，N'_k分别表示时刻k的质量参数和阻尼参数，M₀,N₀为理想条件下的阻抗参数；m,n分别为自适应的权重参数值；

将上述基于速度的以及基于位置的控制器相结合共同作用于机械臂移动过程，最终完成开门任务；

开门软件平台在静态环境下自动开门时执行以下步骤：

S1：根据上述给出的医护机器人平台，对所述的KINOVA机械臂101以及所述RGBD相机9进行手眼标定，用以将物体在相机坐标系下的坐标转换为物体在机械臂基坐标系下的坐标，由于硬件的设计要求，本发明所进行的标定是眼在手外的标定方式；

S2：通过所述物体检测模块识别出门把手的位置；

S3：将步骤S2中的所检测到的门把手的位置通过所述坐标系转换模块将门把手在相机像素坐标系下的坐标转换为在机械臂基坐标系下的坐标；

S4：通过所述机械臂运动模块使得机械臂移动至S3所提供的位置处，并通过GPD物***姿检测算法求得抓取门把手时最合适的抓握姿势，同时所述NODE卡将速度信息传输给所述升降杆以及所述四自由度云台，采用PID控制来使得所述升降杆调整高度使得所述机械臂移动至合适的位置，同时所述四自由度云台调整角度使得相机更好的进行环境检测；

S5：通过所述物体抓取模块使得所述机械臂夹爪夹住门把手；

S6：给出门被打开时门把手的旋转角度，并将其作为机械臂末端的目标位置，通过机械臂运动模块使得所述机械臂运动至门把手旋开状态，在机械臂开门过程中采用所述机械臂柔顺控制模块通过引入位姿反馈使得所述机械臂能够根据反馈值实时调整各关节角度值，实现开门动作；

开门软件平台在动态环境下开门时，执行以下步骤：

T1：根据上述给出的医护机器人平台，对所述的KINOVA机械臂以及kinect相机进行手眼标定，用以将物体在相机坐标系下的坐标转换为物体在机械臂基坐标系下的坐标，由于硬件的设计要求，所进行的标定是眼在手外的标定方式；

T2：利用SLAM中的Cartographer算法根据移动机器人的里程计数据和激光雷达数据(通过kinect相机所获取的深度消息转换为雷达消息)来绘制二维的栅格地图，实现通过Cartographer算法来构建当前病房环境中的地图；

T3：通过ROS***中所提供的map_server功能包中提供的map_saver以及map_server节点来完成栅格地图数据的转存，map_server节点将栅格地图保存到本地，map_server节点通过service方式将栅格地图发布；

T4：通过ROS***中的amcl功能包，实现导航中的机器人定位用以确定机器人的位置，并以此作为机器人移动过程中的起始位置点；

T5：患者给机器人下达开门指令，所述医护机器人接收到指令后，通过所述物体检测模块识别检测出门这一物体；

T6：在步骤T5的基础上将门的信息通过局域网传递给所述运动模块，并以此作为目标位置，此时利用ROS中的navigation导航功能包提供的move_base功能包实现移动底盘的路径规划，move_base能够根据给定的目标点来进行导航，同时所述NODE卡将速度信息传递给所述驱动轮，通过PID控制完成移动底盘的运动，最终控制所述医护机器人移动到门口；

T7：通过所述RGBD相机传入的实时环境数据，所述物体检测模块获取到门把手的位置信息；

T8：在步骤T7的基础上将所检测到的门把手的位置通过所述坐标系转换模块将门把手在相机像素坐标系下的坐标转换为在机械臂基坐标系下的坐标；

T9：将步骤T8中获取的门把手信息设为机械臂末端目标位置信息并通过所述机械臂运动模块使得机械臂末端运动至门把手处，通过GPD物***姿检测算法求得抓取门把手时最合适的抓握姿势，同时所述NODE卡将速度信息传输给所述升降杆以及所述四自由度云台，采用PID控制来使得所述升降杆调整高度使得所述机械臂移动至合适的位置，同时所述四自由度云台调整角度使得相机更好的进行环境检测；

T10：通过所述物体抓取模块使得所述机械臂夹爪夹住门把手；

T11：给出门被打开时门把手的旋转角度，并将其作为机械臂末端的目标位置，通过机械臂运动模块使得所述机械臂运动至门把手旋开状态，在机械臂开门过程中采用所述机械臂柔顺控制模块通过引入位姿反馈使得所述机械臂能够根据反馈值实时调整各关节角度值，实现开门动作。

2.如权利要求1所述的一种自动开门的智能服务机器人***，其特征在于：所述机械臂包括从下而上的基座、大臂、肩关节、腰关节、肘关节、小臂以及腕关节，其中所述腕关节为所述机械臂末端关节，其接口处通过485总线与所述末端执行器相连接，所述机械臂基座安装于所述载物台上。

3.如权利要求1所述的一种自动开门的智能服务机器人***，其特征在于：所述机械臂装置与所述交互屏下方装有所述抬升杆，通过所述抬升杆实现所述机器人整体高度的变化。

Images