CN113184077A - 基于slam与机器视觉的灵巧任务型双足机器人及其控制方法 - Google Patents

Abstract

本申请涉及一种基于SLAM与机器视觉的灵巧任务型双足机器人及其控制方法，该机器人包括舵机、车轮、连接舵机与车轮的调节机构及与车轮连接的驱动件，所述调节机构至少有两组且对称分布于舵机的两侧，所述调节机构包括与车轮转动连接的支架、支臂、第一关节驱动器及第二关节驱动器，所述支臂一端通过第一关节驱动器与支架转动连接，其另一端通过第二关节驱动器与舵机转动连接，当第一关节驱动器与第二关节驱动器同向转动时舵机与车轮相互靠近或远离。本申请具有方便在复杂地形中移动，并提升移动速度的效果。

Description

基于SLAM与机器视觉的灵巧任务型双足机器人及其控制方法

技术领域

本申请涉及机器人的领域，尤其是涉及一种基于SLAM与机器视觉的灵巧任务型双足机器人及其控制方法。

背景技术

SLAM是同步定位与地图构建(Simultaneous Localization And Mapping)的缩写，主要用于解决移动机器人在未知环境中运行时定位导航与地图构建的问题。

近年来，市场上出现了大量的双足机器人及平衡车机器人，广泛运用在交通，检测，服务业中。其中双足机器人的优点在于对于地形的适应性强，可以在阶梯等复杂的地形上移动，其缺点在于移动速度较慢，且在复杂地形移动时容易因自身重心偏移而发生失衡现象。平衡车机器人在其底部配置有车轮，因此其速度相较于双足机器人更快，稳定性更强，但是缺点在于只能在平地或者坡度较低的坡地上移动。

针对上述中的相关技术，发明人认为存在有目前的双足机器人移动速度慢，而平衡车机器人则受限于地形限制，因此缺乏一种可以在复杂地形中移动且移动速度快的机器人。

发明内容

第一方面，为了方便在复杂地形中移动，并提升移动速度，本申请提供一种基于SLAM与机器视觉的灵巧任务型双足机器人。

本申请提供的一种基于SLAM与机器视觉的灵巧任务型双足机器人，采用如下的技术方案：

一种基于SLAM与机器视觉的灵巧任务型双足机器人，包括舵机、车轮、连接舵机与车轮的调节机构及与车轮连接的驱动件，所述调节机构至少有两组且对称分布于舵机的两侧，所述调节机构包括与车轮转动连接的支架、支臂、第一关节驱动器及第二关节驱动器，所述支臂一端通过第一关节驱动器与支架转动连接，其另一端通过第二关节驱动器与舵机转动连接，当第一关节驱动器与第二关节驱动器同向转动时舵机与车轮相互靠近或远离。

通过采用上述技术方案，驱动件用于带动车轮转动，以此使得机器人可以在路面上行驶，提升移动速度；而支架、支臂、第一关节驱动器及第二关节驱动器可以使得某一车轮抬起，另一车轮保持着地，通过控制第一关节驱动器与第二关节驱动器的运转实现支臂、车架的联动，实现车轮的抬升与降落，从而完成跨越障碍物等动作，实现在复杂地形上的移动，且舵机与第二关节驱动器转动连接，通过调整舵机的角度，可以保持机器人的平衡，避免摔倒现象。

优选的，所述支架包括与车轮转动连接的第一构件及与支臂连接的第二构件，所述第一构件一端与第二构件一端铰接，且所述第一构件另一端与第二构件另一端之间连接有避震器。

通过采用上述技术方案，通过第一构件与第二构件的转动结构，实现支臂与车轮之间的相对活动，而避震器可以缓解从车轮传递而来的震感，减少支臂、舵机的振动，从而提升机器人整体的稳定性。

优选的，所述避震器位于第一构件朝向舵机的一侧且位于机器人的前进侧，所述避震器的两端分别与第一构件、第二构件转动连接。

通过采用上述技术方案，车轮在前进时移速一般较高，当机器人刹车时惯性力较大，此时由于避震器位于第一构件朝向舵机的一侧，即位于机器人的前进侧，因此可以最大程度削减第一构件与第二构件之间的冲击力，减少震感，减少对第一关节驱动器、第二关节驱动器的力矩负荷，从而提升机器人的稳定性及延长使用寿命。

优选的，所述驱动件包括与支架连接的驱动电机，所述驱动电机位于支架远离舵机的一侧，且通过同步带、传动带或链条带动车轮转动。

通过采用上述技术方案，驱动电机位于支架远离舵机的一侧，舵机位于机器人的前进侧，驱动电机位于机器人的后侧，以此平衡机器人的重心，方便通过调整舵机守住平衡，且驱动电机通过同步带、传动带或链条带动车轮转动，以此减少车轮对驱动电机的震动影响，减少对驱动电机输出轴的损伤。

优选的，所述第二构件上设置有垫块，所述垫块用于与支臂抵接。

通过采用上述技术方案，当第一关节驱动器与第二关节驱动器同向转动使得支臂下移时，支臂抵住垫块，减少第一关节驱动器与第二关节驱动器的受力，使两者可长时间高负载的运行，减少损伤，从而可以使关节驱动器的性能要求更低，节约了机器人成本。

优选的，还包括卧姿调整模块，所述卧姿调整模块与第一关节驱动器、第二关节驱动器连接，用于检测机器人当前所处的移动模式，移动模式包括平整地形模式及复杂地形模式；

当处于平整地形模式时，卧姿调整模块输出用于控制第一关节驱动器及第二关节驱动器转动至支臂与垫块抵接的状态的控制指令。

通过采用上述技术方案，通过机器视觉等监测前方地面是否平整，当地面平整时，无需调整第一关节驱动器及第二关节驱动器，因此可以将机器人调整至卧姿模式，使得第一关节驱动器及第二关节驱动器转动至支臂与垫块抵接的状态，此时舵机位置下沉，机器人整体重心降低，减少了刹车前倾的趋势，避免机器人摔倒，提升稳定性，且同时减少了第一关节驱动器及第二关节驱动器的力矩负荷。

优选的，所述支架上设置有陀螺仪，所述陀螺仪用于监测舵机沿车轮轴向的偏移角度，且其与第一关节驱动器、第二关节驱动器连接，用于依据机器人的偏移角度控制第一关节驱动器、第二关节驱动器动作。

通过采用上述技术方案，陀螺仪自动监测机器人的左右方向的偏转角度，以此预测机器人的重心变化情况，当机器人重心在左、右车轮之间的正上方时表示机器人处于平衡状态，当机器人重心在左、右车轮的外侧时表示机器人处于失衡状态，会发生倾倒现象，通过该判断结果对第一关节驱动器、第二关节驱动器进行控制，将机器人的重心始终保持在左、右车轮之间的正上方时，以此避免机器人摔倒。

第二方面，为了方便控制机器人在复杂地形中移动，并提升移动速度，本申请提供一种控制方法，采用如下的技术方案：

一种控制方法，应用上述基于SLAM与机器视觉的灵巧任务型双足机器人，包括，

获取机器人当前的姿态信息；

执行姿态解算处理；

判断机器人是否摔倒；

若是则启动恢复平衡例程；

否则判断机器人的当前姿态是否符合正运动学；

若当前姿态符合正运动学，则执行姿态融合处理，并输出用于控制机器人姿态的控制信号；

若当前姿态不符合正运动学，则执行错误分析。

通过采用上述技术方案，通过捕捉机器人的姿态信息可以检测机器人是否摔倒，若出现摔倒，可通过启动恢复平衡例程进行恢复；若未摔倒则判断其当前姿态是否符合正运动学，进行姿态融合处理，通过第一关节驱动器及第二关节驱动器的配合实现对机器人的控制，从而提升控制的精度。

优选的，在所述执行姿态融合处理的步骤前，执行MATLAB正逆运动学解算处理。

通过采用上述技术方案，MATLAB正逆运动学解算处理可以模拟及预测在当前控制信号下机器人的运行结果，以此提前排除机器人运行的安全隐患，同时避免各关节变量输出后产生姿态失调、失衡的现象，从而提升机器人姿态控制的准确度。

优选的，在所述执行错误分析的步骤后，判断当前姿态错误问题是否为可处理错误；

若为可处理错误，则执行MATLAB正逆运动学解算处理；

若非可处理错误，则输出待机指令，并输出用于控制第一关节驱动器及第二关节驱动器卸力的操作指令。

通过采用上述技术方案，将姿态错误分为可以自动解决的错误及无法自动解决的错误，对于可以自动解决的错误，则按照MATLAB正逆运动学解算处理的方式解决，模拟姿态恢复的情形并进行应用；若无法自动解决，则减少关节的力矩，方便操作人员进行人工调整。

综上所述，本申请包括以下至少一种有益技术效果：

1.驱动件使得机器人可以在路面上行驶，提升移动速度；而支架、支臂、第一关节驱动器及第二关节驱动器可以使得某一车轮抬起，另一车轮保持着地，通过控制第一关节驱动器与第二关节驱动器的运转实现支臂、车架的联动，实现车轮的抬升与降落，从而实现在复杂地形上的移动，且舵机与第二关节驱动器转动连接，通过调整舵机的角度，可以保持机器人的平衡，避免摔倒现象；

2.通过第一构件与第二构件的转动结构，实现支臂与车轮之间的相对活动，而避震器可以缓解从车轮传递而来的震感，减少支臂、舵机的振动，从而提升机器人整体的稳定性；

3.当第一关节驱动器与第二关节驱动器同向转动使得支臂下移时，支臂抵住垫块，减少第一关节驱动器与第二关节驱动器的受力，使两者可长时间高负载的运行，减少损伤，从而可以使关节驱动器的性能要求更低，节约了机器人成本。

附图说明

图1是本申请实施例的双足机器人的整体结构示意图。

图2是本申请实施例的双足机器人的侧面结构示意图。

图3是本申请实施例的双足机器人的侧面结构示意图。

图4是本申请实施例的控制方法的方法流程图。

附图标记说明：1、舵机；11、车轮；2、驱动件；21、驱动电机；22、同步带；3、调节机构；31、支架；311、第一构件；312、第二构件；32、垫块；33、避震器；34、支臂；35、第一关节驱动器；36、第二关节驱动器。

具体实施方式

以下结合全部附图对本申请作进一步详细说明。

机器人的正运动学表示给定机器人各关节变量，计算机器人末端的位置姿态，机器人的逆运动学表示已知机器人末端的位置姿态，计算机器人对应位置的全部关节变量。

本申请实施例公开一种基于SLAM与机器视觉的灵巧任务型双足机器人。参照图1，基于SLAM与机器视觉的灵巧任务型双足机器人包括舵机1、车轮11、调节机构3及驱动件2。结合图2所示，调节机构3有两组且对称分布于舵机1的两侧，其包括与车轮11转动连接的支架31、支臂34、第一关节驱动器35及第二关节驱动器36，支臂34两端分别连接第一关节驱动器35及第二关节驱动器36，第一关节驱动器35连接支架31，第二关节驱动器36连接舵机1。驱动件2带动车轮11转动以此使得机器人可在平整路面上快速移动，第一关节驱动器35及第二关节驱动器36配合可以控制车轮11抬升或着地，从而完成跨越障碍物等动作，实现在复杂地形上的移动。

车轮11上套设有橡胶胎，橡胶胎的胎面平整，可以与地面贴合，宽度在5厘米以上，以此提升车轮11的稳定性，方便在单个车轮11抬起时使得另一车轮11可以更好地保持直立状态，减少歪斜现象。支架31包括第一构件311及第二构件312，而驱动件2包括与第一构件311通过螺钉固定的驱动电机21，驱动电机21可采用伺服电机，其位于支架31远离舵机1的一侧，即位于机器人的背侧。车轮11上及驱动电机21输出轴上均同轴固定有带轮，两个带轮上通过同步带22进行传动，以此带动车轮11转动，在其他实施例中，同步带22也可替换为传动带或链条。

第一构件311与第二构件312均采用碳板、亚克力等低密度高强度的材料，以此减少驱动电机21的负荷，且在机器人移动时，第一构件311向机器人背侧倾斜，以此减少车轮11移动时第一构件311与第二构件312与障碍物的擦碰，提高灵活性。

第二构件312呈月牙形，其中部与第一构件311铰接，其靠近车轮11的一端通过避震器33与第一构件311铰接，其另一端与第一关节驱动器35铰接。避震器33采用FST型号，通过阻尼加弹簧的形式减弱第一构件311与第二构件312之间的震动传递，以此减少震动对第一关节驱动器35、第二关节驱动器36的损伤，同时减少对机器人平衡的影响。

参照图2、图3，第一关节驱动器35及第二关节驱动器36均可采用RG20机器人关节模组，内置增量编码器、减速电机、低压直流驱动器、制动器、力矩传感器等，支臂34呈圆柱状，且通过螺钉分别与第一关节驱动器35及第二关节驱动器36固定。第二构件312上安装有垫块32，垫块32可采用橡胶、塑胶、塑料等弹性材料制成，当舵机1两侧的第一关节驱动器35及第二关节驱动器36同向转动，使得两侧车轮11与舵机1相互靠近时，支臂34与垫块32抵接，此时机器人处于卧姿状态，重心降低，不易倾斜，稳定性提升。且垫块32对支臂34进行支撑，分担了舵机1的重量，分担了第一关节驱动器35及第二关节驱动器36上的力矩，降低了第一关节驱动器35及第二关节驱动器36的负荷。

舵机1上还设有视觉模块及卧姿调整模块，视觉模块包括双目摄像头及单片机，双目摄像头一侧设置有照明用的LED探照灯，双目摄像头用于捕捉机器人前方的路面图像，路面图像通过单片机进行处理。单片机依据路面图像判断前方路面上有无明显障碍物，若无明显障碍物则输出用于控制机器人切换移动模式至平整地形模式的第一触发信号，若存在明显障碍物则输出用于控制机器人切换移动模式至复杂地形模式的第二触发信号。

卧姿调整模块在检测到移动模式切换至平整地形模式时，即接收到单片机的第一触发信号时，输出用于控制第一关节驱动器35及第二关节驱动器36转动至支臂34与垫块32抵接的状态的控制指令，此时垫块32分担了舵机1的重量，分担了第一关节驱动器35及第二关节驱动器36上的力矩，从而减少机械损伤。

由于第二关节驱动器36与舵机1铰接，因此通过第二关节驱动器36可以控制舵机1进行转动，舵机1在转动时可以调整重心位置，使得机器人的重心与两侧车轮11的中间处保持在同一竖直线上，从而避免机器人摔倒。且支架31上设置有陀螺仪，陀螺仪采用MPU6050六轴陀螺仪及GY99十轴陀螺仪，且采用分布式测量，按照熵值法估算权重，冗余输出系统的欧拉角、角加速度等参数作为PID算法输入参数，以此对测量数据进行优化。

且为了减小位置式PID控制算法的积分饱和以及增量式PID控制算法的积分阶段效应带来的误差影响，本实施例还采用了双环串级PID控制算法，用内环来控制加速度，用外环输出目标速度，从而来控制位置，内环将外环的目标速度作为目标值，根据当前的速度控制加速度从而向目标加速度靠近，以达到快速、灵敏、精确的效果。

陀螺仪用于监测舵机1沿车轮11轴向的偏移角度，偏移角度即欧拉角，且其与第一关节驱动器35、第二关节驱动器36连接，单片机依据机器人的偏移角度控制第一关节驱动器35、第二关节驱动器36动作。当机器人向其前进方向的左侧或右侧倾斜时，使得机器人的重心位置与车轮11触地处的连线趋向于与水平面垂直时，输出对应信号，控制远离偏转侧的第一关节驱动器35及第二关节驱动器36动作，以此使远离偏转侧的车轮11与舵机1的间距缩短，或者使得靠偏转侧的车轮11与舵机1的间距伸长，以此使重心回归，避免机器人摔倒。

支架31上还设置有SLAM模块，可采用ORB-SLAM2的系统框架，采用TOF激光雷达传感器模块及Jetson Xavier NX开发板作为硬件支撑，TOF激光雷达传感器模块用于对机器人周围的环境模型进行稀疏重建，并将重建结果传输至Jetson Xavier NX和地面站，使用最大似然数据关联将扩展卡尔曼滤波应用于SLAM模块，以此获得更加精确的机器人位置、周围环境建模及行动路线，为机器人提供避障或其他运动的基础数据。机器人通过上述SLAM模块可以得到空间内某一高度的二维数据，结合双目摄像头捕捉的路面图像，可以判断地面及障碍物的三维形态，从而实现障碍物的识别，进而方便机器人进行避障操作。

本实施例还公开了一种控制方法，参照图4，应用上述基于SLAM与机器视觉的灵巧任务型双足机器人，包括如下步骤：

S100、获取机器人当前的姿态信息。

具体的，姿态信息包括机器人的移动速度、加速度，还包括支架31、支臂34、舵机1的欧拉角等，可采用STM32控制板进行采集。

S200、执行姿态解算处理。

具体的，姿态解算可以采用正运动学或逆运动学进行解算，通过接收如遥控终端等上位机的控制信号，通过MATLAB导出机器人逆运动学算法将动作姿态解算并发送至第一关节驱动器35或第二关节驱动器36，通过三环，即角度环、速度环及转向环PID控制机器人保持在稳定的姿态，实现机器人的平衡。

S200、判断机器人是否摔倒；

若是则启动恢复平衡例程；

否则判断机器人的当前姿态是否符合正运动学。

具体的，判断机器人是否摔倒，可采用前述的陀螺仪检测欧拉角的方式实现。恢复平衡例程，指在机器人即将失衡前，通过逆运动学结合MATLAB仿真预测机器人姿态并控制机器人回正。具体可通过正运动学，通过对给定机器人各关节变量的读取，计算机器人末端的位置姿态，判断第一关节驱动器35与第二关节驱动器36运作后的机器人姿态是否正常，以此使得机器人保持平衡状态。

S300、若当前姿态符合正运动学，则执行MATLAB正逆运动学解算处理；

具体的，MATLAB正逆运动学解算处理可以预测第一关节驱动器35与第二关节驱动器36在协调动作后对机器人姿态的影响，以此减少机器人摔倒现象同时避免产生其他异常问题。

S310、执行姿态融合处理，并输出用于控制机器人姿态的控制信号；

若当前姿态不符合正运动学，则执行错误分析。

具体的，姿态融合是指两组第一关节驱动器35、两组第二关节驱动器36在受到控制后进行调整使得机器人整体的姿态产生变化，其姿态变化取决于多个关节的控制变量，因此需要进行姿态融合。

S400、错误分析包括：判断当前姿态错误问题是否为可处理错误；

若为可处理错误，则执行MATLAB正逆运动学解算处理；

若非可处理错误，则输出待机指令，并输出用于控制第一关节驱动器35及第二关节驱动器36卸力的操作指令。

具体的，可处理错误包括机器人倾斜、与障碍物擦碰等；不可处理错误包括硬件丢失、损坏等，还包括未录入的机器人姿态，可处理错误可以采用控制第一关节驱动器35与第二关节驱动器36的方式解决，例如转向、角度偏转等动作，而硬件丢失、损伤，或者遇到没有录入过的姿态，则需要人工进行辅助维护纠正。

本申请实施例一种基于SLAM与机器视觉的灵巧任务型双足机器人的实施原理为：当行驶在平整路面上时，驱动电机21带动车轮11转动，以此推动机器人进行快速移动，且第二关节驱动器36控制舵机1进行转动，调整机器人整体的重心，保持机器人平衡。且卧姿调整模块输出用于控制第一关节驱动器35及第二关节驱动器36转动至支臂34与垫块32抵接的状态的控制指令，降低机器人的重心，降低第一关节驱动器35及第二关节驱动器36的负荷。

当行驶在复杂地形上时，通过双目摄像头及激光雷达监测障碍物，控制第一关节驱动器35与第二关节驱动器36动作以抬升或降低车轮11，以此跨越或躲避障碍物，提升机器人的稳定性，实现在复杂地形上的移动。

以上均为本申请的较佳实施例，并非依此限制本申请的保护范围，故：凡依本申请的结构、形状、原理所做的等效变化，均应涵盖于本申请的保护范围之内。

Claims (10)

1.一种基于SLAM与机器视觉的灵巧任务型双足机器人，其特征在于：包括舵机（1）、车轮（11）、连接舵机（1）与车轮（11）的调节机构（3）及与车轮（11）连接的驱动件（2），所述调节机构（3）至少有两组且对称分布于舵机（1）的两侧，所述调节机构（3）包括与车轮（11）转动连接的支架（31）、支臂（34）、第一关节驱动器（35）及第二关节驱动器（36），所述支臂（34）一端通过第一关节驱动器（35）与支架（31）转动连接，其另一端通过第二关节驱动器（36）与舵机（1）转动连接，当第一关节驱动器（35）与第二关节驱动器（36）同向转动时舵机（1）与车轮（11）相互靠近或远离。

2.根据权利要求1所述的基于SLAM与机器视觉的灵巧任务型双足机器人，其特征在于：所述支架（31）包括与车轮（11）转动连接的第一构件（311）及与支臂（34）连接的第二构件（312），所述第一构件（311）一端与第二构件（312）一端铰接，且所述第一构件（311）另一端与第二构件（312）另一端之间连接有避震器（33）。

3.根据权利要求2所述的基于SLAM与机器视觉的灵巧任务型双足机器人，其特征在于：所述避震器（33）位于第一构件（311）朝向舵机（1）的一侧且位于机器人的前进侧，所述避震器（33）的两端分别与第一构件（311）、第二构件（312）转动连接。

4.根据权利要求1所述的基于SLAM与机器视觉的灵巧任务型双足机器人，其特征在于：所述驱动件（2）包括与支架（31）连接的驱动电机（21），所述驱动电机（21）位于支架（31）远离舵机（1）的一侧，且通过同步带（22）、传动带或链条带动车轮（11）转动。

5.根据权利要求2所述的基于SLAM与机器视觉的灵巧任务型双足机器人，其特征在于：所述第二构件（312）上设置有垫块（32），所述垫块（32）用于与支臂（34）抵接。

6.根据权利要求1所述的基于SLAM与机器视觉的灵巧任务型双足机器人，其特征在于：还包括卧姿调整模块，所述卧姿调整模块与第一关节驱动器（35）、第二关节驱动器（36）连接，用于检测机器人当前所处的移动模式，移动模式包括平整地形模式及复杂地形模式；

当处于平整地形模式时，卧姿调整模块输出用于控制第一关节驱动器（35）及第二关节驱动器（36）转动至支臂（34）与垫块（32）抵接的状态的控制指令。

7.根据权利要求1所述的基于SLAM与机器视觉的灵巧任务型双足机器人，其特征在于：所述支架（31）上设置有陀螺仪，所述陀螺仪用于监测舵机（1）沿车轮（11）轴向的偏移角度，且其与第一关节驱动器（35）、第二关节驱动器（36）连接，用于依据机器人的偏移角度控制第一关节驱动器（35）、第二关节驱动器（36）动作。

8.一种控制方法，应用权利要求1-7中任一项所述的基于SLAM与机器视觉的灵巧任务型双足机器人实现，其特征在于，包括，

获取机器人当前的姿态信息；

执行姿态解算处理；

判断机器人是否摔倒；

若是则启动恢复平衡例程；

否则判断机器人的当前姿态是否符合正运动学；

若当前姿态符合正运动学，则执行姿态融合处理，并输出用于控制机器人姿态的控制信号；

若当前姿态不符合正运动学，则执行错误分析。

9.根据权利要求8所述的控制方法，其特征在于：在所述执行姿态融合处理的步骤前，执行MATLAB正逆运动学解算处理。

10.根据权利要求8所述的控制方法，其特征在于：在所述执行错误分析的步骤后，判断当前姿态错误问题是否为可处理错误；

若为可处理错误，则执行MATLAB正逆运动学解算处理；

若非可处理错误，则输出待机指令，并输出用于控制第一关节驱动器（35）及第二关节驱动器（36）卸力的操作指令。

Images