结合大象机器人六轴协作机械臂myCobot 280 ，解决特定的自动化任务和挑战！（下）-电子发烧友网

LimoPro小车建图导航

引言

前景提要：我们在上文介绍了使用LIMOcobot实现一个能够执行复杂任务的复合机器人系统的应用场景的项目，从以下三个方面：概念设计、系统架构以及关键组件。

本文主要深入项目内核的主要部分，同样也主要分为三个部分：机械臂的视觉抓取，LIMOPro在ROS中的功能，建图导航避障等，以及两个系统的集成。

设备准备

myCobot280M5stack

myCobotAdaptiveGripper

myCobotCameraFlange2.0

LIMOPRO

机械臂视觉抓取

这是机械臂安装AdaptiveGripper，和cameraflange2.0之后的样子。

我们是用的cameraflange2.0是一款2D的相机，他并不能够依靠他相机本身来获取到一个物体的三维（长宽高），但我们可以使用标记物来获得到目标物体的都长宽高。常见的有ArUco，STag，AR码，AprilTags。今天我们用STag算法来做视觉识别。

STag标记系统

STag是一个为了高稳定性和精确的三位定位而设计的标记系统。它特别适用于环境中有遮挡和光照变化的情况。

下面有一个视频展示了STag标记码，ARToolKit+，ArUco，RUNE-Tag码在同一个环境下的识别效果。

https://www.youtube.com/watch?v=vnHI3GzLVrY

可以从视频中看出来STag对环境变化的强大适应性和在复杂场景下的高可靠性，使其成为在要求高精度跟踪和定位的应用中的首选。还有一篇论文专门讲解STag稳定的基准标记系统，感兴趣的可以自己点击链接去了解一下。

https://arxiv.org/abs/1707.06292

STag系统可以是适配与ROS，有ROS软件包，用的是c++编写的，也能够支持python进行使用。

C++/ROS:GitHub - bbenligiray/stag: STag: A Stable Fiducial Marker System

Python：GitHub - ManfredStoiber/stag-python: Python Package for STag - A Stable, Occlusion-Resistant Fiducial Marker System

用python简单写一个例子

importcv2
importstag
importnumpyasnp

#加载相机参数
camera_params=np.load("camera_params.npz")
mtx,dist=camera_params["mtx"],camera_params["dist"]

#初始化STag检测器
stag_detector=stag.detectMarkers(mtx,dist)

#初始化视频捕获
cap=cv2.VideoCapture(0)
whilecap.isOpened():
ret,frame=cap.read()
    if notret:
        break
    
    #应用相机校正（可选）
frame_undistorted=cv2.undistort(frame,mtx,dist, None,mtx)
    #检测STag标记
    (corners,ids,rejected_corners) =stag.detectMarkers(frame_undistorted, 21)
    #绘制检测到的标记及其ID
stag.drawDetectedMarkers(frame_undistorted,corners,ids)
    #绘制被拒绝的候选区域，颜色设为红色
stag.drawDetectedMarkers(frame_undistorted,rejected_corners,border_color=(255, 0, 0))

    #显示结果
cv2.imshow("STagDetection",frame_undistorted)

    ifcv2.waitKey(1) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()

标记了STag码之后可以获得标记码四个角的参数，

(array([[[257., 368.],
        [185., 226.],
        [345., 162.],
        [420., 302.]]],dtype=float32),)

给定坐标过后，可以用opencv的‘cv2.solvePnP'函数来计算标记相对于相机的旋转和偏移量。这个函数需要标记的3D坐标（在物理世界中的位置）和相应的2D图像坐标（即检测到的角点），以及相机的内参和畸变系数。solvePnP会返回旋转向量（rvec）和平移向量（tvec），它们描述了从标记坐标系到相机坐标系的转换。这样，就可以根据这些参数计算出标记的位置和朝向。

以下是伪代码方便理解。

defestimate_pose(corners):
#DosomecalculationswithPnP,camerarotationandoffset
returnrvec，tvec
    
defconvert_pose_to_arm_coordinates(rvec,tvec):
#将旋转向量和平移向量转换为机械臂坐标系统中的x,y,z,rx,ry,rz
returnx,y,z,rx,ry,rz
    
defconvert_grab(object_coord_list):
#Dosomecalculationstoconvertthecoordinatesintograspingcoordinatesfortheroboticarm
returngrab_position
    
    
cap=cv2.VideoCapture(0)
whilecap.isOpened():
ret,frame=cap.read()
ifnotret:
break
        
    
maker=(corners,ids,rejected_corners)=stag.detectMarkers(image,21)
    
rvec,tvec=stag.estimate_pose(marker)
    
object_coord_list=convert_pose_to_arm_coordinates(rvec,tvec)
grab_position=convert_grab(object_coord_list)
mycobot.move_to_position(grab_position)
机械臂控制

以上的代码就是大概的抓取的流程，比较复杂的部分解决了，接下来我们处理机械臂的运动控制,用到的是pymycobot库

frompymycobotimportMyCobot

#Createaninstanceandlinktheroboticarm
mc=MyCobot('com3',115200)

#Controltheroboticarmwithangle
mc.send_angles(angles_list,speed)

#Controltheroboticarmusingcoordinates
mc.send_coords(coords_list,speed,mode)

#Controlgripper,value-0~100
mc.set_gripper_value(value,speed)

因为机械臂的开放接口比较多，我们只需要使用坐标控制，夹爪控制就好了。

LimoPro建图导航

完成了机械臂抓取部分的功能，接下来我们要实现小车的建图导航模块了。

首先我们要见图，有了地图之后才能够在地图上进行导航，定点巡航等一些的功能，目前有多种建图的算法，因为我们搭建的场景并不是很大，环境相对于静态我们选择使用gmapping算法来实现。

建图

Gmapping是基于滤波SLAM框架的常用开源SLAM算法。Gmapping有效利用了车轮里程计信息，对激光雷达的频率要求不高，在构建小场景地图时，所需的计算量较小且精度较高。这里通过使用ROS封装了的GMapping功能包来实现limo的建图。

注：以下的功能都是封装好的可以直接使用

首先需要启动雷达，打开一个新终端，在终端中输入命令：

roslaunchlimo_bringuplimo_start.launchpub_odom_tf:=false

然后启动gmapping建图算法，打开另一个新终端，在终端中输入命令：

roslaunchlimo_bringuplimo_gmapping.launch

成功启动之后会打开rviz可视化工具，这时候看到的界面如图

这时候就可以把手柄调为遥控模式，控制limo建图了。

构建完地图之后，需要运行以下命令，把地图保存到指定目录：

1、切换到需要保存地图的目录下，这里把地图保存到~/agilex_ws/src/limo_ros/limo_bringup/maps/，在终端中输入命令：

cd~/agilex_ws/src/limo_ros/limo_bringup/maps/

2、切换到/agilex_ws/limo_bringup/maps之后，继续在终端中输入命令：

rosrunmap_servermap_saver-fmap1

map1为保存地图的名称，保存地图时应避免地图的名称重复

机器人只需要发布必要的传感器信息和导航的目标位置,ROS即可完成导航功能。在该框架中,move_base功能包提供导航的主要运行、交互接口。为了保障导航路径的准确性,机器人还要对自己所处的位置进行精确定位,这部分功能由amcl功能包实现。

在导航的过程中，运用了两种算法DWA和TEB算法，这两种算法分别处理全局路径和局部路径规划，来保证小车能够安全的前进到目的地，避免与障碍物发生碰撞。

（1）首先启动雷达，在终端中输入命令：

roslaunchlimo_bringuplimo_start.launchpub_odom_tf:=false

（2）启动导航功能，在终端中输入命令：

roslaunchlimo_bringuplimo_navigation_diff.launch

启动成功之后会打开rviz界面，如图

编辑我们需要把刚才建的地图给导进去，请打开limo_navigation_diff.launch文件修改参数,文件所在目录为：~/agilex_ws/src/limo_ros/limo_bringup/launch。把map02修改为需要更换的地图名称。

开启导航之后，会发现激光扫描出来的形状和地图没有重合，需要我们手动校正，在rviz中显示的地图上矫正底盘在场景中实际的位置，通过rviz中的工具，发布一个大概的位置，给limo一个大致的位置，然后通过手柄遥控limo旋转，让其自动校正，当激光形状和地图中的场景形状重叠的时候，校正完成。操作步骤如图：

校正完成后

通过2DNavGoal设置导航目标点

路径巡检

如果要在一条路上来回运动的话，我们要启用路径巡检功能，后续会使用上这个功能。

（1）首先启动雷达，开启一个新的终端，在终端中输入命令：

roslaunchlimo_bringuplimo_start.launchpub_odom_tf:=false

（2）启动导航功能，开启一个新的终端，在终端中输入命令：

roslaunchlimo_bringuplimo_navigation_diff.launch

注：如果是阿克曼运动模式，请运行

roslaunchlimo_bringuplimo_navigation_ackerman.launch

（3）启动路径记录功能，开启一个新的终端，在终端中输入命令：

roslaunchagilex_pure_pursuitrecord_path.launch

路径记录结束之后终止路径记录程序，在终端中输入命令为：Ctrl+c

（4）启动路径巡检功能，开启一个新的终端，在终端中输入命令：

注：把手柄调至指令模式

roslaunchagilex_pure_pursuitpure_pursuit.launch

两个系统的集成

上面分布完成了myCobot机械臂视觉的抓取，LIMO的建图导航，路径巡检功能，现在我们需要把它们集成在ROS系统上。我们预设的场景是，LIMO进行定点的巡检，当遇到了标志物的时候停止运动，等待机械臂执行抓取物体，完成之后LIMO移动到下一个点位。

功能节点分布

在ROS（RobotOperatingSystem）中实现一个功能的流程涉及到多个步骤和组件，包括节点（nodes）、话题（topics）、服务（services）、参数服务器（parameterserver）和动作（actions）。根据ROS的功能节点架构，我们确定了节点的分布和它们交互的方式：