[ROB_2] webots+ros 激光雷达的定位与控制

[ROB_2] webots + ros 激光雷达的定位与控制

本次实验是对四轮驱动的搭载一个SickLms291雷达的机器人的控制代码的编写，机器人在webot中仿真，通过ros的控制节点进行控制，算法通过解析激光雷达的点云信息获取世界坐标系到机器人坐标系变换信息从而实现机器人的定位，通过键盘控制机器人的运动。题目源码 https://gitee.com/buaarobot_admin/Online-test-of-algorithm-group-in-2021

一. webot与ros的连接

webot本身自带控制器，可以直接在其中编写控制脚本，当然，它也可以通过ros进行控制。需要手动设置robot节点的controller为ros。可以选择性地在设置控制器时加入参数--name=NAME改写当前webot仿真机器人在ros中的名称。

二. webot_lidar的激活并与ros通信

此处需要安装webots_ros包(一个webots与ros建立联系的接口)，当webots选择控制器为ros后，webots将仿真机器人的传感器信息、控制接口以ros_srv或ros_topic的形式在roscore中注册。程序员可以在自建的节点中直接订阅topic或访问服务，从而获取webots中机器人的信息并进行控制。

通过rosservice list可以看到所有的srv信息。其中包含激光雷达的一些服务器

其中enable是激光雷达开启的开关，默认是关闭的，需要手动打开，这里选择通过命令行的方式开启，返回success

此后rostopic中将增加lidar相关的topic，需要记录下来名字，从而在控制脚本中订阅

在控制脚本中订阅该topic即可获得信息，至此即完成了webots与ros的激光雷达的通讯链接。

class Controller:

    def __init__(self):
        self.node = rospy.init_node("lidar_ctrl", anonymous=True)
        self.lidar_subscriber = rospy.Subscriber("/mybot/lidar/laser_scan/layer0", LaserScan,
                                                 callback=self.lidarCallback)
        self.lidarFlag = True
        self.lidarInfo = None
        self.laserProj = LaserProjection()
        self.pcPub = rospy.Publisher("/laserPointCloud", pc, queue_size=1)

三. 激光雷达数据的处理

首先需要将雷达的原始数据转化为点云数据：

cloud_out = self.laserProj.projectLaser(self.lidarInfo)
data = []
for item in point_cloud2.read_points(cloud_out, field_names=("x", "y"), skip_nans=True):
    data.append([item[0], item[1]])

由于此处使用的是二维雷达，仅需要获得x y信息。以(x, y)的形式保存在data中。

以灰度图的形式存储数据，并通过opencv进行进一步处理，得到原始的点云信息如下：

array = np.full((1000, 1000), 0, np.uint8)
for item in data:
    if int(item[0] * 100) + 500 >= 1000 or int(item[1] * 100) + 500 >= 1000:
         continue
    array[int(item[0] * 100) + 500][int(item[1] * 100) + 500] = 255
img = array
cv2.imshow("origin", img)

由于墙壁的存在对圆柱的检测会产生障碍，因此先对点云进行霍夫线变换，在调整合理参数后检测到线，并剔除距离这些线较近的点

lines = cv2.HoughLinesP(img, 1, np.pi / 180, threshold=35, minLineLength=0, maxLineGap=2)
# 剔除属于墙壁的点
for i in range(len(img)):
    for j in range(len(img[0])):
        if img[i][j] == 255 and onLine(i, j, linesP, threshold=10):
             img[i][j] = 0

剔除结果如下(绿色表示检测到的霍夫线，红色表示被剔除的点)

剔除墙壁的干扰后，进行霍夫圆变换，调整参数到合适：

circles = cv2.HoughCircles(img, cv2.HOUGH_GRADIENT, dp=2, minDist=60, param1=20, param2=5, minRadius=10, maxRadius=15)

将检测到的圆心坐标记录下来，至此完成点云的处理

四. 机器人坐标变换的求解

通过激光雷达的点云信息，我们获得了机器人坐标系下四个圆柱的圆心位置(没有确定顺序)，在正式计算之前，需要将两个坐标系下圆心坐标一一配对，即该四边形的特征识别从而确定对应关系。

这里直接利用当前四边形的特殊性，首先找到直角顶点和钝角的顶点，其次确定与钝角无关的一个直角边点，最后确定剩下的一个点。

这样即完成了四个点的对应。

for center in centers:
    for d1 in centers:
        if d1 != center:
            for d2 in centers:
                if d2 != center and d2 != d1:
                    v1 = [center[0] - d1[0], center[1] - d1[1]]
                    v2 = [center[0] - d2[0], center[1] - d2[1]]
                    if abs(v1[0] * v2[0] + v1[1] * v2[1]) < 0.5:
                        centerIndex['2'] = center
                    if v1[0] * v2[0] + v1[1] * v2[1] < -2:
                        centerIndex['3'] = center
                        a = centers.copy()
                        a.remove(center)
                        a.remove(d1)
                        a.remove(d2)
                        centerIndex['1'] = a[0]
a = centers.copy()
a.remove(centerIndex['1'])
a.remove(centerIndex['2'])
a.remove(centerIndex['3'])
centerIndex['4'] = a[0]

在世界坐标系下的圆心位置已经给出。即已知变换前后点的坐标，现在需要逆求机器人坐标系的坐标。相关思想如下：

原始坐标变换：

\[\left[ \begin{array}{1} cos\theta & -sin\theta \\ sin\theta & cos\theta \end{array} \right] \left[ \begin{array}{1} x \\ y \end{array} \right] - \left[ \begin{array}{1} x_0 \\ y_0 \end{array} \right] = \left[ \begin{array}{1} x' \\ y' \end{array} \right] \]

已知四个点变换后的结果，因此可以建立以cos\theta 、sin\theta 、x0、y0为未知数的方程组。反过来进行求解：

\[\begin{array}{1} x_1cos\theta -y_1sin\theta - x_0 = x_1'\\ x_2cos\theta -y_2sin\theta - x_0 = x_2'\\ x_3cos\theta -y_3sin\theta - x_0 = x_3'\\ x_4cos\theta -y_4sin\theta - x_0 = x_4' \end{array} \]

此处有四个点贡献信息的点，未知数的个数小于方程个数，可以考虑最小二乘法，记

\[A = \left[ \begin{array}{1} x_1 & -y_1 & -1\\ x_2 & -y_2 & -1\\ x_3 & -y_3 & -1\\ x_4 & -y_4 & -1\\ \end{array} \right] \]

\[c = \left[ \begin{array}{1} x1'\\ x2'\\ x3'\\ x4'\\ \end{array} \right] \]

\[X = \left[ \begin{array}{1} cos\theta\\ sin\theta\\ x_0\\ \end{array} \right] \]

原方程组改写为:

\[AX = c \]

通过最小二乘法

\[A^TAX = A^Tc\\ X = (A^TA)^{-1}A^Tc \]

即可解得坐标变化接情况，Y方向同理。由此即确定机器人坐标系相对世界坐标系的坐标变换, 计算的部分代码如下:

A = np.array([[std_Index['1'][0], -std_Index['1'][1], -1],
              [std_Index['2'][0], -std_Index['2'][1], -1],
              [std_Index['3'][0], -std_Index['3'][1], -1],
              [std_Index['4'][0], -std_Index['4'][1], -1]])
c = np.array([centerIndex['1'][0], centerIndex['2'][0], centerIndex['3'][0], centerIndex['4'][0]])
c = np.matmul(A.T, c)
A = np.matmul(A.T, A)
X = np.linalg.inv(A).dot(c)

正余弦值可以通过X, Y的均值并单位化获得。

对上述示例的点云数据进行解算，可以得到如下结果

cos:-0.9851773049645389, sin:0.007801418439715713, x:3.9861702127659555, y:4.48744680851064

对应世界坐标系下的结果为， 与原始点云图像对比可以发现基本吻合:

在通过点云信息确定robot的坐标后，就可以通过一个节点定时解析robot的坐标系的变换信息。

rate = rospy.Rate(200)
while not rospy.is_shutdown():
      self.dataProcess()
rate.sleep()

其中dataProcess方法即完成上述全过程并打印结果。

五. ros键盘控制webots

键盘控制即通过import tty, termios包获取键盘的输入。这里的重点在于连接webots为我们创建好的四个轮子的服务器，调用服务器修改轮子转速的请求从而完成对机器人的控制。服务器信息可以通过rosservice获取，而服务器的请求类别为webots_ros.set_float，因此核心的订阅函数如下(共四个电机)

from webots_ros import set_float

def serveWheel(i: int, v):
    rospy.wait_for_service(f'/mybot/wheel{i+1}/set_velocity')
    try:
        setVelocity = rospy.ServiceProxy(f'/mybot/wheel{i+1}/set_velocity', set_float)
        response = setVelocity(v[i])
        return response.name
    except:
        print("Service Call failed")

注意四个电机需要设置初始位置为inf

def setPosition(i: int):
    rospy.wait_for_service(f'/mybot/wheel{i + 1}/set_position')
    try:
        setPosition = rospy.ServiceProxy(f'/mybot/wheel{i + 1}/set_position', set_float)
        response = setPosition(float('inf'))
        return response
    except:
        print("Service Call failed: %s" % e)

至此即完成键盘控制节点。

六. 启动最后的工程

进入工作空间目录下

catkin_make
./run.sh
另一个终端下：
rosservice call /mybot/lidar/enable "value: 1"
rosrun lidar_ctrl lidarctrl.py
另一个终端下：
rosrun lidar_ctrl lidarctrl.py
(此终端中通过wsad控制，x停止，q退出)

最终的实现效果如图所示：

七. 不足与改进方向

• 本次实验中的雷达只能扫描180度，需要增加雷达扫描的层数，扩大扫描范围，最好达到360度才能获得地图全貌，从而更好地定位。
• 若出现圆柱的遮挡其后圆柱的现象(实例图中圆柱挡住了墙壁)，则无法确定所有圆柱的位置，因而无法确定机器人的位置。
• 本次处理中直接将墙壁作为霍夫圆识别的干扰去除了，其实墙壁也可以成为识别机器人位置的重要信息。
• 点云处理中直线和圆的识别直接调用OpenCV的API，执行效率较低，解算的算法耗时过长，导致无法实时反馈位置信息，需要改进点云处理的算法。