海南省住房和城乡建设部网站,高端室内设计公司起名,平原网站建设价格,网站建设 实训一、引言
1、AGV需要同时具备定位、避障与导航的功能#xff0c;其中避障对于雷达本身的分辨率、精度要求并不是很高#xff0c;只需要能够根据预设定的雷达扫描范围准确避开障碍物即可#xff0c;故本文以TIM240#xff08;SICK激光类雷达#xff09;为例介绍实现多雷达…一、引言
1、AGV需要同时具备定位、避障与导航的功能其中避障对于雷达本身的分辨率、精度要求并不是很高只需要能够根据预设定的雷达扫描范围准确避开障碍物即可故本文以TIM240SICK激光类雷达为例介绍实现多雷达时空标定的问题。 2、多个避障雷达可能会被安装在车体各个位置并且不一定有重叠区域所以通过提取特征点再进行ICP或NDT配准的方法获取相对位姿变换关系的方式不可行由于机械结构本身已经按照设计图纸预先设置好雷达的安装位置所以雷达之间的相对位置关系相对准确故可以直接利用已有参数可以进行雷达空间标定将两个雷达数据统一到同一个坐标系下进行需要用到PCL库。 3、坐标变换实际使用过程当中确实可以用tf进行静态坐标变换但是相比较直接手动写代码进行转换延迟较高故采用手动根据相对位置关系进行坐标变换的方式。 4、进行时空标定步骤主要分为1驱动安装及雷达参数配置2雷达时间同步3雷达空间同步RpT这里的旋转矩阵和平移矩阵的定义后文会给出
二、整体思路与流程
1、SICK激光雷达驱动安装 新买的SICK雷达不仅需要安装驱动还需要进配置文件修改参数才能正常工作 1安装驱动
mkdir -p ./sick_scan_ws
cd ./sick_scan_wsmkdir ./src
pushd ./src
git clone https://ghproxy.com/https://github.com/SICKAG/libsick_ldmrs.git
git clone https://ghproxy.com/https://github.com/SICKAG/msgpack11.git
git clone https://ghproxy.com/https://github.com/SICKAG/sick_scan_xd.git
popdmkdir -p ./build/msgpack11
pushd ./build/msgpack11
cmake -G Unix Makefiles -D CMAKE_CXX_FLAGS-fPIC -D CMAKE_BUILD_TYPERelease -D MSGPACK11_BUILD_TESTS0 ../../src/msgpack11
make
sudo make install
popdsource /opt/ros/melodic/setup.bash # replace noetic by your ros distro
catkin_make_isolated --install --cmake-args -DROS_VERSION1 -Wno-dev
source ./devel_isolated/setup.bash2雷达配置文件修改 1首先需要设置PC机的以太网ip端口号如192.168.0.2 2SICK官网下载sopas软件在Windows端使用用Win10系统Win11有问题 3进入软件修改登录权限为授权的用户密码是client 4修改雷达ip与PC机同一网段即可192.168.0.1以及Cola dialect改为cola binary格式。此处修改雷达ip是为了保证两个雷达能同时使用 3硬件组装 需要交换机、电源模块、电源线普通的家庭供电线就可以火线为棕灰色、地线为黄绿双色、零线为蓝色分别对应接到电源模块的L端、FG端、N端、供电线、网线如下图连接示意图 4修改雷达启动文件launch需要创建两个启动文件分别对应不同ip的雷达我这里设置一个ip为192.168.0.1一个为192.168.0.3因为PC的ip为192.168.0.2
?xml version1.0?
!--**********************************************Launch File for TIM 240 scanner**********************************************Start and stop angle is given in [rad]Check IP-address, if you scanner is not found after roslaunch.
--!-- You can launch a TIM_240-scanner on a specific ip address (e.g. 192.68.0.71) using the following example call:roslaunch sick_scan sick_tim_240.launch hostname:192.168.0.71--
!-- Using node option requiredtrue will close roslaunch after node exits --launcharg namehostname default192.168.0.1/arg nameport default2111/arg namecloud_topic defaultcloud1/arg nameframe_id defaultcloud1/arg namesw_pll_only_publish defaulttrue/arg namenodename defaultsick_tim_240_1/arg nameadd_transform_xyz_rpy default0,0,0,0,0,0/arg nameadd_transform_check_dynamic_updates defaultfalse/ !-- Note: dynamical updates of parameter add_transform_xyz_rpy can decrease the performance and is therefor deactivated by default --node name$(arg nodename) pkgsick_scan typesick_generic_caller respawnfalse outputscreen requiredtrueparam namescanner_type typestring valuesick_tim_240/!-- datagram shows values from -90 ... 120 (211 values) --!-- -120 deg --param namemin_ang typedouble value-2.094395102/!-- 120 deg --param namemax_ang typedouble value2.094395102/!-- corresponds to 1.0/(14.4 * 360) --param nametime_increment typedouble value0.00019290123/param nameuse_binary_protocol typebool valueTrue/param nameintensity typebool valuetrue/param nameintensity_resolution_16bit typebool valuetrue/param namehostname typestring value$(arg hostname)/param namecloud_topic typestring value$(arg cloud_topic)/param nameframe_id typestr value$(arg frame_id)/param nameport typestring value$(arg port)/param nametimelimit typeint value5/param namesw_pll_only_publish typebool value$(arg sw_pll_only_publish)/param nameuse_generation_timestamp typebool valuetrue/ !-- Use the lidar generation timestamp (true, default) or send timestamp (false) for the software pll converted message timestamp --param namemin_intensity typedouble value0.0/ !-- Set range of LaserScan messages to infinity, if intensity min_intensity (default: 0) --!-- Optional range filter configuration: If the range of a scan point is less than range_min or greater than range_max, the point can be filtered. --!-- Depending on parameter range_filter_handling, the following filter can be applied for points with a range not within [range_min, range_max], --!-- see enumeration RangeFilterResultHandling in range_filter.h: --!-- 0: RANGE_FILTER_DEACTIVATED, do not apply range filter (default) --!-- 1: RANGE_FILTER_DROP, drop point, if range is not within [range_min, range_max] --!-- 2: RANGE_FILTER_TO_ZERO, set range to 0, if range is not within [range_min, range_max] --!-- 3: RANGE_FILTER_TO_RANGE_MAX, set range to range_max, if range is not within [range_min, range_max] --!-- 4: RANGE_FILTER_TO_FLT_MAX, set range to FLT_MAX, if range is not within [range_min, range_max] --!-- 5: RANGE_FILTER_TO_NAN set range to NAN, if range is not within [range_min, range_max] --!-- Note: Range filter applies only to Pointcloud messages, not to LaserScan messages. --!-- Using range_filter_handling 4 or 5 requires handling of FLT_MAX and NAN values in an application. --param namerange_min typedouble value0.0/param namerange_max typedouble value100.0/param namerange_filter_handling typeint value0/!-- Apply an additional transform to the cartesian pointcloud, default: 0,0,0,0,0,0 (i.e. no transform) --!-- Note: add_transform_xyz_rpy is specified by 6D pose x, y, z, roll, pitch, yaw in [m] resp. [rad] --!-- It transforms a 3D point in cloud coordinates to 3D point in user defined world coordinates: -- !-- add_transform_xyz_rpy : T[world,cloud] with parent world and child cloud, i.e. P_world T[world,cloud] * P_cloud --!-- The additional transform applies to cartesian lidar pointclouds and visualization marker (fields) --!-- It is NOT applied to polar pointclouds, radarscans, ldmrs objects or other messages --param nameadd_transform_xyz_rpy typestring value$(arg add_transform_xyz_rpy) / param nameadd_transform_check_dynamic_updates typebool value$(arg add_transform_check_dynamic_updates) /param namestart_services typebool valueTrue / !-- Start ros service for cola commands, default: true --param namemessage_monitoring_enabled typebool valueTrue / !-- Enable message monitoring with reconnectreinit in case of timeouts, default: true --param nameread_timeout_millisec_default typeint value5000/ !-- 5 sec read timeout in operational mode (measurement mode), default: 5000 milliseconds --param nameread_timeout_millisec_startup typeint value120000/ !-- 120 sec read timeout during startup (sensor may be starting up, which can take up to 120 sec.), default: 120000 milliseconds --param nameread_timeout_millisec_kill_node typeint value150000/ !-- 150 sec pointcloud timeout, ros node will be killed if no point cloud published within the last 150 sec., default: 150000 milliseconds --param nameclient_authorization_pw typestring valueF4724744/ !-- Default password for client authorization --!-- Configuration of ROS quality of service: --!-- On ROS-1, parameter ros_qos sets the queue_size of ros publisher --!-- On ROS-2, parameter ros_qos sets the QoS of ros publisher to one of the following predefined values: --!-- 0: rclcpp::SystemDefaultsQoS(), 1: rclcpp::ParameterEventsQoS(), 2: rclcpp::ServicesQoS(), 3: rclcpp::ParametersQoS(), 4: rclcpp::SensorDataQoS() --!-- See e.g. https://docs.ros2.org/dashing/api/rclcpp/classrclcpp_1_1QoS.html#ad7e932d8e2f636c80eff674546ec3963 for further details about ROS2 QoS --!-- Default value is -1, i.e. queue_size10 on ROS-1 resp. qosrclcpp::SystemDefaultsQoS() on ROS-2 is used.--param nameros_qos typeint value-1/ !-- Default QoS-1, i.e. do not overwrite, use queue_size10 on ROS-1 resp. qosrclcpp::SystemDefaultsQoS() on ROS-2 --/node
/launch
2、SICK激光雷达时间同步 主要使用ros官方提供的软同步方式message_filters 使用方式如下
ros::Publisher pointcloud_pub;
typedef message_filters::sync_policies::ApproximateTimesensor_msgs::PointCloud2, sensor_msgs::PointCloud2 testSyncPolicy;void callback(const sensor_msgs::PointCloud2ConstPtr point1, const sensor_msgs::PointCloud2ConstPtr point2) //回调中包含多个消息
{//需要处理的内容//
}int main (int argc, char **argv)
{ros::init (argc, argv, lidar2base);ros::NodeHandle n;pointcloud_pub n.advertisesensor_msgs::LaserScan(/scan, 1000);//发布的话题message_filters::Subscribersensor_msgs::PointCloud2 pointcloud1_sub(n, /cloud1, 1);// 需要同步的topic1message_filters::Subscribersensor_msgs::PointCloud2 pointcloud2_sub(n, /cloud2, 1);// 需要同步的topic2 message_filters::SynchronizertestSyncPolicy sync(testSyncPolicy(10), pointcloud1_sub, pointcloud2_sub);// 同步sync.registerCallback(boost::bind(callback, _1, _2));ros::spin();ros::shutdown();return 0;
}
3、SICK激光雷达空间同步 主要是通过雷达之间的旋转和平移参数将两个雷达数据统一到一个坐标系下再使用PCL库对雷达数据进行融合这里编写了相关函数
//坐标变换将激光雷达坐标系下的点转换到小车坐标系下
void transPoint(pcl::PointCloudpcl::PointXYZ::Ptr lidarCloud,pcl::PointCloudpcl::PointXYZ::Ptr carCloud,double yaw,double pitch,double roll,double x,double y,double z)
{Eigen::Matrix4f transformEigen::Matrix4f::Identity();Eigen::Matrix4f transformYaw;Eigen::Matrix4f transformPitch;Eigen::Matrix4f transformRoll;//航向角transformYawcos(yaw),-sin(yaw),0,0,\sin(yaw),cos(yaw),0,0,\0,0,1,0,\0,0,0,1;//俯仰角transformPitchcos(pitch),0,sin(pitch),0,\0,1,0,0,\-sin(pitch),0,cos(pitch),0,\0,0,0,1;//横滚角transformRoll1,0,0,0,\0,cos(roll),-sin(roll),0,\0,sin(roll),cos(roll),0,\0,0,0,1;//旋转矩阵transformtransformRoll*transformPitch*transformYaw;//平移矩阵transform(0,3)x;transform(1,3)y;transform(2,3)z;//坐标转换pcl::transformPointCloud(*lidarCloud,*carCloud,transform);
}如下图 需求是通过上述函数将P点从激光雷达坐标系变换到车体坐标系函数的输入分别为偏航角yaw, pitch, rollxyz 偏航角yaw的获取方式是以车坐标系为右手基准坐标系大拇指指向为z轴正方向激光雷达系为目标坐标系顺时针旋转z轴的角度分量为yaw的值图中x1Oy1为车体坐标系。 同理俯仰角pictch为绕y轴旋转角度分量横滚角roll为绕x轴的分量。 平移分量x, y, z是激光雷达相对于车体坐标原点的xyz的坐标。 4、雷达数据融合 需要将ros的PointCloud2类型数据转换为PCL数据类型而后将PCL数据类型转为ros的LaserScan数据类型。 1数据融合主要在SICK激光雷达时间同步这一部分的回调函数中写 pcl::PointCloudpcl::PointXYZ::Ptr colorcloud1(new pcl::PointCloudpcl::PointXYZ);pcl::fromROSMsg(*point1, *colorcloud1);pcl::PointCloudpcl::PointXYZ::Ptr colorcloud2(new pcl::PointCloudpcl::PointXYZ);pcl::fromROSMsg(*point2, *colorcloud2);pcl::PointCloudpcl::PointXYZ::Ptr output_points1(new pcl::PointCloudpcl::PointXYZ);pcl::PointCloudpcl::PointXYZ::Ptr output_points2(new pcl::PointCloudpcl::PointXYZ);transPoint(colorcloud1, output_points1, yaw1, pitch1, roll1, x1, y11, z1);transPoint(colorcloud2, output_points2, yaw2, pitch2, roll2, x2, y2, z2);pcl::PointCloudpcl::PointXYZ output *output_points1 *output_points2;//pcl::toROSMsg((*output_points1 *output_points2), output);//以实时发布的方式发布sensor_msgs::LaserScan output2 PointCloudToLaserscan(output);output2.header.frame_id base_link;pointcloud_pub.publish(output2);
2PCL转LaserScan单独写了一个函数
sensor_msgs::LaserScan PointCloudToLaserscan(pcl::PointCloudpcl::PointXYZ _pointcloud)
{float angle_min, angle_max, range_min, range_max, angle_increment;//需要自行调整的参数angle_min -2.094395102;angle_max 2.094395102;range_min 0.05;range_max 10;//角度分辨率分辨率越小转换后的误差越小angle_increment 0.0174444444;//计算扫描点个数unsigned int beam_size ceil((angle_max - angle_min) / angle_increment);sensor_msgs::LaserScan output;output.header.stamp ros::Time::now();output.header.frame_id base_link;output.angle_min angle_min;output.angle_max angle_max;output.range_min range_min;output.range_max range_max;output.angle_increment angle_increment;output.time_increment 0.0;output.scan_time 0.0;//先将所有数据用nan填充output.ranges.assign(beam_size, std::numeric_limitsfloat::quiet_NaN());//output.intensities.assign(beam_size, std::numeric_limitsfloat::quiet_NaN());for (auto point : _pointcloud.points){float range hypot(point.x, point.y);float angle atan2(point.y, point.x);int index (int)((angle - output.angle_min) / output.angle_increment);if (index 0 index beam_size){//如果当前内容为nan则直接赋值if (isnan(output.ranges[index])){output.ranges[index] range;}//否则只有距离小于当前值时才可以重新赋值else{if (range output.ranges[index]){output.ranges[index] range;}}//output.intensities[index] point.intensity;}}return output;
}5、创建启动脚本 首先启动两个雷达其次启动坐标转换的脚本即可实现两个雷达的时空同步
#!/bin/bash
gnome-terminal --tab -- bash -c roslaunch sick_scan sick_tim_240_1.launch
sleep 1s
gnome-terminal --tab -- bash -c roslaunch sick_scan sick_tim_240_2.launch
sleep 2s
gnome-terminal --tab -- bash -c source /home/lixushi/catkin_ws/devel/setup.bash; roslaunch lidar2base lidar2base.launch如下图为雷达实际摆放位置
转换前的点云图 转换后的点云图
三、参考文献
[1] https://blog.csdn.net/m0_68312479/article/details/128266483 [2] https://blog.csdn.net/hltt3838/article/details/123067620 [3] https://blog.csdn.net/m0_68312479/article/details/128266483 [4] https://adamshan.blog.csdn.net/article/details/105930565?spm1001.2014.3001.5502 [5] https://mp.weixin.qq.com/s?__bizMzU1NjEwMTY0Mwmid2247556040idx1snfa1f7cc63f7aeeaed69242add86efd75chksmfbc862acccbfebba12fd29cef32b7a34ac296f54e8ef6f19485aff81ebddd794a3364419c90bscene27
