工作笔记05：imu、底盘车轮、单激光雷达外参校准（基于hdl_graph_slam建图和定位）

最新推荐文章于 2024-04-25 18:56:34 发布

坤虫H.M

最新推荐文章于 2024-04-25 18:56:34 发布

阅读量3k

点赞数

分类专栏：机器人应用调试

本文链接：https://blog.csdn.net/weixin_42214178/article/details/108847283

版权

机器人应用调试专栏收录该内容

5 篇文章 1 订阅

订阅专栏

1. imu 校准

1.1 IMU型号

阅读IMU产品说明书，获得如下信息：
Yesense YIS300 系列惯性传感系统
YIS300 系列包含：

YIS300-U 惯性测量单元（IMU）：输出经过校准的加速度和角速度值，同时也能够输出磁场强度信息。
YIS300-V 垂直参考系统（VRU）：除了具备 YIS300-U 特性的同时，还能够输出无漂移的横滚角、俯仰角以及无参考的航向角。
YIS300-A 航姿参考系统（AHRS）：除了具备YIS300-U 特性的同时，还能够输出无漂移的横滚角、俯仰角以及参考地球磁北方向的航向角信息。同时，YIS300-A 经过配置可以作为一个垂直参考单元（VRU, Vertical Reference Unit），输出的航向角为无参考方向，不受磁场环境影响。

YIS300 的动态姿态检测精度为 0.3°，同时提供稳定、低漂移的相对航向输出以及无漂移的绝对航向输出。
在这里插入图片描述
图4-1 产品系列简图

1.2 坐标定义

1.2.1 大地坐标系(WGS84)

现在，最普遍的方式是用大地坐标系来表示某点相对于地球的位置。这种坐标系用一个椭球来代表整个地球的形状。

目前，WGS84 椭球系统是应用最普遍的一个大地坐标系，因为它被用作 GPS 的标准。因此，在 YIS300 的中采用的大地坐标系为 WGS84 参考坐标系。

1.2.2 当地地理坐标系（ENU）

YIS300 中采用的当地地理坐标系为 ENU（东北天坐标系）。

ENU（East North Up）坐标系，即东北天坐标系，简称为 n 坐标系，也叫做导航坐标系，是在导航时根据导航系统工作的需要而选取的用于导航结算的参考坐标系。

ENU 坐标系各轴的定义：

E——东轴指向地球东；
N——北轴指向地球北；
U——地轴垂直于地球表面并指向上。

在这里插入图片描述
图4-2 当地地理坐标

1.2.3 传感器坐标轴定义

在这里插入图片描述
图4-3 YIS300 系列坐标轴定义

注意：imu参数标定最重要的工作就是令IMU的坐标轴方向和小车车体中心的坐标轴方向平行，即：x、y、z都平行。

1.3 传感器内参校准

每一个 YIS300 系列姿态传感器都单独进行过全测量范围内的校准和测试。陀螺仪、加速度计简化的统一模型表示如下：
$\cdot y + b$
其中：

$u$ 表示陀螺仪、加速度计校准后的测量值(单位 $° / s$ ， $m/s^2$ ）
$y$ 表示陀螺仪、加速度计的原始输出值
$H$ 表示传感器的复合误差校准矩阵
$b$ 表示传感器的偏置

YIS300 的校准过程包括传感器的标度因数、传感器轴间非正交、以及传感器与 PCB 板间的不对准等误差校准，所有的校准参数都预先完成并写入到板基内嵌算法中。

1.4 校准imu和车底坐标系

根据IMU产品说明书中的坐标系说明，如图4-3 YIS300 系列坐标轴定义，预先将imu位置摆正，使得imu的坐标系方向和车底坐标系方向一致，并且令imu位于x轴正方向上，可以使得在后面的xyz三轴的位置校准上，可以不需要计算y轴方向上的数据，直接令y=0。

在hdl的建图启动文件~/sx_ws/src/hdl_graph_slam/launch/hdl_graph_slam_sx.launch 新增imu相对与底盘的tf变换关系：

<node pkg="tf" type="static_transform_publisher" name="imu2base_publisher" args="1.86 0 -0.35 0 0 0 base_link imu 10" />

args参数顺序分别为：

前边的x y z分别代表着相应轴的平移，单位是米。
yaw pitch roll 分别代表着绕三个轴的转动，单位是弧度
前文介绍过了，可能有人感觉这东西不好记，很容易忘掉哪个对应哪个坐标轴，其实你可以发现，yaw pitch roll 分别对应着 Z，Y，X轴的旋转，也就是把我们总说的XYZ的反过来，只要记住顺序还是不容易弄错的。
再之后的frame_id为坐标系变换中的父坐标系， child_frame_id为坐标系变换中的子坐标系。
最后一个参数为发布频率，单位为毫秒。通常取100。

一毫秒为一秒的千分之一，100毫秒即为0.1秒，也就是10Hz。

注意：欧拉角定义如下：

Yaw（偏航）：欧拉角向量的y轴
Pitch（俯仰）：欧拉角向量的x轴
Roll（翻滚）：欧拉角向量的z轴

在tf中填写imu 相对于车底中心的距离，以xyz标准。由于已经人为使得imu和车体坐标系方向一致，不用添加 rpy的参数。否则，以逆时针旋转为正、顺时针旋转为负，添加rpy的参数。

其中，x=1.86 z=-0.35 单位是米，均是实际测量值。

同理，在hdl的定位启动文件~/sx_ws/src/hdl_localization/launch/hdl_localization.launch添加相同的tf变换关系：

<node pkg="tf" type="static_transform_publisher" name="imu2base_publisher" args="1.86 0 -0.35 0 0 0 i_base_link i_imu 10" />

2. 底盘车辆参数配置

修改底盘参数配置文件：~/sx_ws/src/iexpress_omni_4_driver/config/vehicle_params_sx.yaml:

#agv 车体参数 
# 端口名称
port_name : "/dev/usbDIPAN" 

header_frame_id_odom : "i_odom"
child_frame_id_odom :  "i_base_link"

# 串口的波特率
baud_rate : 115200

control_rate : 50
battery_rate : 1

# 编码率=电机转一圈的脉冲数*减速比 800*25
encoder_resolution : 20000

# 对角线/2
diagonal : 1.677

Battery_min : 21.5
Battery_max : 29.4

# 标定里程计: 目前置1 ；走10米，实际走的路/里程计数据
# y轴   8/8.555
calibrate_y : 0.9351
# x轴  8/8,467
calibrate_x : 1

# w轴，车旋转180度进行测量
calibrate_w : 1

max_radio : 0.8
max_angular : 15

# 轮直径
wheel_diameter_rf : 0.412
wheel_diameter_lf : 0.412
wheel_diameter_lr : 0.412
wheel_diameter_rr : 0.412

angle_rf : 2.3562
angle_lf : 3.9270
angle_lr : 5.4978
angle_rr : 0.78

其中，重点修改以下参数：（以实际测量为准）

port_name：端口名称，在usb转串口中设置
header_frame_id_odom : “i_odom” 轮式里程计坐标系
child_frame_id_odom : "i_base_link"车底坐标系
encoder_resolution：计算公式为编码率=电机转一圈的脉冲数*减速比
wheel_diameter_rf、lf、lr、rr：右前轮、左前轮、左后轮、右后轮的轮胎直径
calibrate_x、y、w：车体坐标x,y,绕z轴旋转的里程计标定参数。
计算方法如下：初始设1，然后进行标定。
标定参数 = 小车实际走的长度、角度/odom里程计数据
预计设置标定参数后的效果：（小车实际走的长度、转的角度）约等于（odom里程计显示的数据）

注意1：直接测量的数据要精准，否则会影响后续标定参数的设置，导致其参数不准。

注意2：参考里程计数据的方法：

启动底盘驱动：

roslaunch iexpress_omni_4_driver iexpress_omni_4_driver.launch

找到底盘驱动的topic：显示内容包含 wheel_odom

rostopic list

在终端输出里程计数据：

rostopic echo 里程计节点名称

3. 单激光雷达校准

3.1 产品说明摘要

在这里插入图片描述
图4-4 雷达示例图

RS-LiDAR-16，固态混合激光雷达，有以下特点：

测量范围高达150米
测量误差在2厘米内
320000点/秒的数据速率
360度水平视野
30度垂直视野

RS-LiDAR-16紧凑外壳安装有16对激光/探测器对，快速旋转并发出高频激光束，连续扫描周围环境。先进的数字信号处理和测距算法计算点云数据和物体反射率，使机器能够看到世界，并为定位、导航和避障提供可靠的数据。
在这里插入图片描述
图4-4 RS-LiDAR成像系统

3.2 雷达出厂设置

要使得电脑能连接上雷达，需要设置电脑的以太网IP，具体参数如下图所示：
在这里插入图片描述
图4-5 出厂设置的IP地址和端口号

每个RS-LiDAR-16的默认MAC地址都在出厂前设置。MAC地址可以根据需要改变。
为了在传感器和计算机之间建立通信，需要将计算机的IP地址设置在传感器的同一网络段。默认:192.168.1.102，子网掩码:255.255.255.0。如果对传感器的互联网设置不确定，请将计算机子网掩码设置为0.0.0.0，将传感器连接到计算机，通过Wireshark解析包获取IP和端口。

RS-LiDAR-16采用3种通信协议与计算机建立通信：

MSOP(主数据流输出协议)。将传感器采集到的距离、方位角和反射率数据打包输出到计算机。
DIFOP（设备信息输出协议）。监控传感器的当前配置信息。
UCWP（用户配置写协议）。用户可以根据需要修改传感器的一些参数。

3.3 使用RSView自定义雷达IP和端口号

具体参见工作笔记02.编译功能包并配置传感器
“3.2 配置雷达参数”。

由于我们使用hdl进行建图和定位工作，而hdl只支持使用一颗激活雷达进行建图和定位工作，所以将原先使用两颗雷达的启动文件修改为启动一颗雷达的启动文件（同时获取双雷达的数据，但是只使用一颗，貌似对定位的精度有影响）。~/sx_ws/src/iexpress_ladar_driver/iexpress_rs_driver_new/rslidar_pointcloud_new/launch/one_lidar.launch内容如下：

<!--注释掉雷达1，只使用雷达2-->

<launch>
<arg name="iexpress" default="iexpress"/>
<!--
<arg name="iexpress_rs_driver1" default="iexpress_rs_driver1"/>
-->
<arg name="iexpress_rs_driver2" default="iexpress_rs_driver2"/>
<group ns="$(arg iexpress)">

<!--
<group ns="$(arg iexpress_rs_driver1)">
  <node  name="rslidar_node" pkg="rslidar_driver_new" type="rslidar_node" output="screen" >
    <param name="model" value="RS16"/>
     <param name="cut_angle" value="240"/>
 
    <param name="device_ip" value="192.168.1.200" />
    <param name="msop_port" value="6699"/>
    <param name="difop_port" value="7788"/>
  </node>

  <node  name="cloud_node" pkg="rslidar_pointcloud_new" type="cloud_node" output="screen" >
    <param name="model" value="RS16"/>
    <param name="curves_path" value="$(find rslidar_pointcloud_new)/data/lidar1/curves.csv" />
    <param name="angle_path" value="$(find rslidar_pointcloud_new)/data/lidar1/angle.csv" />
    <param name="channel_path" value="$(find rslidar_pointcloud_new)/data/lidar1/ChannelNum.csv" />
    <param name="topic_name" value="rslidar_points1" />
    <param name="frame_id" value="i_scan1" />
  </node>
</group>
-->

 <group ns="$(arg iexpress_rs_driver2)">
  <node  name="rslidar_node" pkg="rslidar_driver_new" type="rslidar_node" output="screen" >
    <param name="model" value="RS16"/>
      <param name="cut_angle" value="240"/>
    <!--param name="pcap" value="/home/tony-sti/lidar_data/lm75-170616-roadtest.pcap"/ -->
    <param name="device_ip" value="192.168.1.201"/>
    <param name="msop_port" value="9966"/>
    <param name="difop_port" value="8877"/>
  </node>
	
  <node  name="cloud_node" pkg="rslidar_pointcloud_new" type="cloud_node" output="screen" >
    <param name="model" value="RS16"/>
    <param name="curves_path" value="$(find rslidar_pointcloud_new)/data/lidar2/curves.csv" />
    <param name="angle_path" value="$(find rslidar_pointcloud_new)/data/lidar2/angle.csv" />
    <param name="channel_path" value="$(find rslidar_pointcloud_new)/data/lidar2/ChannelNum.csv" />
    <param name="topic_name" value="rslidar_points2" />
    <param name="frame_id" value="i_scan2" />
  </node>
</group>
</group>
</launch>

和imu一样，在hdl的建图和定位中添加雷达相对于小车中心的TF变换关系：

<node pkg="tf" type="static_transform_publisher" name="lidar2base_publisher" args="-1.925 1.1 0.0 2.25  0 0 base_link i_scan2 10" />

上述语句的作用的直观表现是：在令小车和仓库墙面保持平行的前提下，雷达发射的激光射线和墙面反射的射线平行。代表雷达的校准成功。

参考文献

[1] LATEX：玩转数学公式

[2] tf之static_transform_publisher

坤虫H.M

关注

0
点赞
踩
26

收藏

觉得还不错? 一键收藏
1
评论
工作笔记05：imu、底盘车轮、单激光雷达外参校准（基于hdl_graph_slam建图和定位）

目录1. imu 校准1.1 IMU型号1.2 坐标定义1.2.1 大地坐标系(WGS84)1.2.2 当地地理坐标系（ENU）1.2.3 传感器坐标轴定义1.3 传感器校准2. 底盘车辆参数配置3. 单激光雷达校准参考文献1. imu 校准1.1 IMU型号阅读IMU产品说明书，获得如下信息：Yesense YIS300 系列惯性传感系统YIS300 系列包含：YIS300-U 惯性测量单元（IMU）：输出经过校准的加速度和角速度值，同时也能够输出磁场强度信息。YIS300-V 垂直参考系
复制链接

扫一扫