ROS2机器人自动充电之雷达点云法

前言

大家好，我是小鱼。最近我在折腾一个户外机器人项目，由于这个项目需要365*24小时不间断运行，偶尔会出现系统死机的情况。为了应对这个问题，我在研究看门狗，但这次我想分享另一个在项目中用到的有趣方法：如何通过雷达点云和特定形状（90度）的反光板识别充电桩的坐标，实现机器人自动充电。

硬件和最终目标

我们的目标是利用雷达来识别安装在充电桩上的90度反光板，并确定其中心点坐标和朝向。以下是具体的实现方法：

1. 订阅雷达数据并转换成点云滤波

首先，我们需要订阅雷达数据，并筛选出反光强度较大的点，将其转换为点云并保存。

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan, PointCloud2, PointField
import numpy as np
import open3d as o3d

def points_to_pointcloud2(points, frame_id):
    msg = PointCloud2()
    msg.header.frame_id = frame_id
    msg.height = 1
    msg.width = len(points)
    msg.fields.append(PointField(name="x", offset=0, datatype=PointField.FLOAT32, count=1))
    msg.fields.append(PointField(name="y", offset=4, datatype=PointField.FLOAT32, count=1))
    msg.fields.append(PointField(name="z", offset=8, datatype=PointField.FLOAT32, count=1))
    msg.is_bigendian = False
    msg.point_step = 12
    msg.row_step = msg.point_step * len(points)
    msg.is_dense = False

    msg_data = []
    for p in points:
        msg_data.append(p[0])  # x
        msg_data.append(p[1])  # y
        msg_data.append(p[2])  # z

    msg.data = np.array(msg_data, dtype=np.float32).tostring()
    return msg

class ScanSubscriberPublisher(Node):
    def __init__(self):
        super().__init__('scan_subscriber_publisher')
        self.subscription = self.create_subscription(
            LaserScan,
            'scan',
            self.scan_callback,
            10)
        self.publisher_ = self.create_publisher(PointCloud2, 'scan_match', 10)
        self.threshold = 250  # 设置强度阈值

    def scan_callback(self, msg):
        points = []
        intensities = np.array(msg.intensities)
        ranges = np.array(msg.ranges)
        # 检查强度大于阈值的点
        indices = np.where(intensities > self.threshold)[0]
        # 将符合条件的点添加到列表中
        for i in indices:
            range_val = ranges[i]
            if range_val < msg.range_max and range_val > msg.range_min:
                angle = msg.angle_min + i * msg.angle_increment
                x = range_val * np.cos(angle)
                y = range_val * np.sin(angle)
                points.append([x, y, 0.0])  # 假设所有点都在平面上，z轴设为0

        # 发布点云消息
        cloud_msg = points_to_pointcloud2(points, msg.header.frame_id)
        self.publisher_.publish(cloud_msg)

        pcd = o3d.geometry.PointCloud()
        pcd.points = o3d.utility.Vector3dVector(points)
        o3d.io.write_point_cloud("point_cloud.pcd", pcd)

        self.destroy_subscription(self.subscription)
        rclpy.shutdown()
        rclpy.get_global_executor().shutdown()

def main(args=None):
    rclpy.init(args=args)
    node = ScanSubscriberPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

在上面的代码中，我们订阅了雷达数据并筛选出反光强度较大的点，然后将这些点转换为点云并发布。同时，保存点云数据为文件，供后续处理使用。

2. 下采样和滤波

接下来，我们对点云数据进行下采样和滤波，以去除噪声点并减少计算量。

import numpy as np
import open3d as o3d
import time

# Load point cloud data
pcd = o3d.io.read_point_cloud('point_cloud.pcd', remove_nan_points=True, remove_infinite_points=True)

start_time = time.time()
# Radius filtering
cl, index = pcd.remove_radius_outlier(nb_points=10, radius=0.2)
new_pcd = pcd.select_by_index(index)

# Downsampling
voxel_size = 0.025
downsampled_pcd = new_pcd.voxel_down_sample(voxel_size=voxel_size)

在这段代码中，我们首先加载点云数据，并通过半径滤波去除孤立点。接着，对点云进行下采样，以减少计算量。

3. 提取点云趋势

我们需要将点云数据转换为极坐标系，以便更容易分析趋势。

# Convert point cloud to polar coordinates
points = np.asarray(downsampled_pcd.points)
r = np.sqrt(points[:, 0]**2 + points[:, 1]**2)
theta = np.arctan2(points[:, 1], points[:, 0])
z = points[:, 2]

# Construct polar point cloud and sort by angle
polar_points = np.vstack((r, theta, z)).T
sorted_indices = np.argsort(polar_points[:, 1])
sorted_polar_points = polar_points[sorted_indices]

# Convert back to Cartesian coordinates
cart_points = np.column_stack((r * np.cos(theta), r * np.sin(theta)))

# Track distance changes
distance_changes = np.diff(sorted_polar_points[:, 0])
direction_changes = np.sign(distance_changes)

# Prepare data
positive_points = []
negative_points = []
for i in range(len(direction_changes)):
    if direction_changes[i] > 0:
        positive_points.append(cart_points[sorted_indices[i], :2])
    else:
        negative_points.append(cart_points[sorted_indices[i], :2])

# Convert lists to numpy arrays
positive_points = np.array(positive_points)
negative_points = np.array(negative_points)

在这段代码中，我们将点云转换为极坐标系，并按照角度进行排序。接着，我们跟踪距离变化，并根据变化方向将点分为两类：正变化点和负变化点。

4. 拟合直线

使用RANSAC算法拟合直线，以找到反光板的边缘。

from sklearn.linear_model import RANSACRegressor

def fit_line(points):
    if len(points) < 2:
        return None, None
    reg = RANSACRegressor().fit(points[:, 0].reshape(-1, 1), points[:, 1])
    return reg, reg.estimator_.coef_[0], reg.estimator_.intercept_

pos_line, pos_slope, pos_intercept = fit_line(positive_points)
neg_line, neg_slope, neg_intercept = fit_line(negative_points)

在这段代码中，我们使用RANSAC算法拟合两条直线，分别对应正变化点和负变化点。

5. 计算斜率交点和夹角

通过计算两条直线的交点和夹角，确定反光板的中心点坐标和朝向。

import math

# 计算交点
def find_intersection(k1, b1, k2, b2):
    if k1 == k2:
        return None  # 平行或重合，无交点
    x = (b2 - b1) / (k1 - k2)
    y = k1 * x + b1
    return (x, y)

# 计算夹角
def find_angle(k1, k2):
    tan_theta = abs((k1 - k2) / (1 + k1 * k2))
    theta_radians = math.atan(tan_theta)
    theta_degrees = math.degrees(theta_radians)
    return theta_degrees

intersection = find_intersection(pos_slope, pos_intercept, neg_slope, neg_intercept)
angle = 180 - find_angle(pos_slope, neg_slope)

# 计算与x轴的夹角
def angle_with_x_axis(k):
    theta_radians = math.atan(k)
    theta_degrees = math.degrees(theta_radians)
    return theta_degrees

angle1 = angle_with_x_axis(pos_slope)
angle2 = angle_with_x_axis(neg_slope)

# 输出结果
station_angle = (angle1 + angle2) / 2 + 180
print(f"交点: {intersection}, 夹角: {angle}")
print(f"充电桩坐标: {intersection}, 坐标夹角: {station_angle}")

在这段代码中，我们计算了两条直线的交点、夹角以及相对于x轴的角度，得出了充电桩的坐标和朝向。

示例运行结果：

0.00625920295715332
交点: (0.4631433880355934, 0.004179420328549566), 夹角: 119.32425765226662
充电桩坐标: (0.4631433880355934, 0.004179420328549566), 坐标夹角: 175.06814132186594

6. 结果展示

最后，我们绘

制点云和拟合的直线，以可视化结果。

import matplotlib.pyplot as plt
import numpy as np

# Plot the points and the fitted lines
plt.figure(figsize=(10, 8))
plt.scatter(positive_points[:, 0], positive_points[:, 1], color='blue', label='Positive change points')
plt.scatter(negative_points[:, 0], negative_points[:, 1], color='red', label='Negative change points')

# Lines
if pos_line:
    x_values = np.array([min(positive_points[:, 0]), max(positive_points[:, 0])])
    y_values = pos_slope * x_values + pos_intercept
    plt.plot(x_values, y_values, color='cyan', label='Fitted line for positive points')

if neg_line:
    x_values = np.array([min(negative_points[:, 0]), max(negative_points[:, 0])])
    y_values = neg_slope * x_values + neg_intercept
    plt.plot(x_values, y_values, color='magenta', label='Fitted line for negative points')

plt.xlabel('X Coordinate')
plt.ylabel('Y Coordinate')
plt.xlim(-1.0,1.0)
plt.ylim(-1.0,1.0)
plt.title('Point Cloud and Fitted Lines')
plt.legend()
plt.grid(True)
plt.show()

通过上述步骤，我们成功地通过雷达点云识别充电桩，并计算出了充电桩的坐标和朝向。这种方法在实际应用中能够有效地帮助机器人实现自动充电功能。

希望这篇文章对大家有所帮助，欢迎在下次有空的时候一起讨论看门狗的实现。谢谢大家！