AMCL定位融合UWB

#AMCL定位融合UWB| 来源: 网络整理| 查看: 265

2021/12/26，实验效果不好，发现多传感器融合好像不像我想得那么简单，还不太懂…

2021/12/27，一个大问题ekf并没有融合odom，odom_combined发布的数据和uwb(gps)基本是重合的，就算把数据改得很夸张，也和uwb(gps)重合，而ACML订阅的是什么呢？因为还能导航，ACML输出的位置数据和odom是差不多的。看了一些博客发现ekf有两种融合模式，一种是增加/gps话题，uwb以/gps话题发布，一种是uwb以/vo话题发布，两种形式的融合算法是不同的（具体我也没看），前者以/gps为主导，后者根据协方差加权？？？

AMCL定位融合UWB ros串口通信

1.安装串口助手

sudo apt-get install cutecom sudo cutecom

2.查看电脑链接的串口信息（名称）：

dmesg | grep ttyS*

在这里插入图片描述然后在串口助手里打开串口没输出，而且dmesg | grep ttyS*报错了请添加图片描述 3.设置串口权限（为了解决2的问题）

创建文件/etc/udev/rules.d/70-ttyusb.rules

sudo gedit /etc/udev/rules.d/70-ttyusb.rules

在文件内增加一行

KERNEL=="ttyUSB[0-9]*", MODE="0666"

增加访问权限

sudo chmod 666 /dev/ttyUSB0

一顿操作下来，依然报2错，而且串口工具收不到数据

4.USB转串口驱动（PL2303）

参考ubuntu装pl2303USB转串口驱动依然报错

5.可以用cutecom收发数据了请添加图片描述虽然能收数据，但是输入：dmesg | grep ttyS*依然报pl2303 converter now disconnected from ttyUSB0，也就是第2步的错，难道是传感器传输消息不稳定？我甚至错都没找到，诶，难道我只能当个复制粘贴怪？

6.下载ROS对应版本的工具包

sudo apt-get install ros-melodic-serial

进入下载的软件包的位置

roscd serial

若是安装成功会看到

$:/opt/ros/melodic/share/serial

7.将uwb数据融合进robot_pose_ekf定位的一点点理论

Adding a GPS sensor to the Robot Pose EKF Filter：官网通过robot_pose_ekf三个基本输入中odom,imu,vo中的三维视觉里程计VO来实现融合的，就是把GPS数据经过UTM转换再输入VO中 ROS中加入GPS的robot_pose_ekf定位：robot_pose_ekf可以接收4路输入分别是odom/imu/vo/gps 官网中gps的数据格式为：

msg.header.stamp = gps_time // time of gps measurement msg.header.frame_id = base_footprint // the tracked robot frame msg.pose.pose.position.x = gps_x // x measurement GPS. msg.pose.pose.position.y = gps_y // y measurement GPS. msg.pose.pose.position.z = gps_z // z measurement GPS. msg.pose.pose.orientation.x = 1 // identity quaternion msg.pose.pose.orientation.y = 0 // identity quaternion msg.pose.pose.orientation.z = 0 // identity quaternion msg.pose.pose.orientation.w = 0 // identity quaternion msg.pose.covariance = {cox_x, 0, 0, 0, 0, 0, // covariance on gps_x 0, cov_y, 0, 0, 0, 0, // covariance on gps_y 0, 0, cov_z, 0, 0, 0, // covariance on gps_z 0, 0, 0, 99999, 0, 0, // large covariance on rot x 0, 0, 0, 0, 99999, 0, // large covariance on rot y 0, 0, 0, 0, 0, 99999} // large covariance on rot z

对角线就是测量数据的方差，这里请教了朋友，协方差矩阵根据经验给定，可靠的数据就把协方差设置小点比如10的负六次方，不可靠的数据协方差设置大点比如10的六次方（毕竟方差越大的数据越不可信），也可以自己写算法计算可以动态调整的方差。

8:创建一个工作空间uwb_ws,并建立相应的pkg：uwbwork 参考：ROS下搭建 UWB 下行数据解析驱动

#include #include //ROS已经内置了的串口包 #include #include #include #include #include #include int main (int argc,char** argv) { float uwb_x; float uwb_y; char data_string[2][7];//字符串形式存串口读取的x,y 在用函数转为浮点型数据 serial::Serial ser; //声明串口对象 //初始化节点 ros::init(argc, argv, "uwbsub"); //声明节点句柄 ros::NodeHandle nh; //发布UWB话题 ros::Publisher uwb_publisher = nh.advertise("gps", 100); try { auto port_name =ros::param::param("~port_name", "/dev/ttyUSB0"); auto baud_rate = ros::param::param("~baud_rate", 115200); auto timeout = serial::Timeout::simpleTimeout(1000); //设置串口属性，并打开串口 ser.setPort(port_name); ser.setBaudrate(baud_rate); ser.setTimeout(timeout); ser.open(); } catch (serial::IOException& e) { ROS_ERROR_STREAM("Unable to open port "); return -1; } //检测串口是否已经打开，并给出提示信息 if(ser.isOpen()) { ROS_INFO_STREAM("Serial Port initialized"); } else { return -1; } //指定循环的频率 ros::Rate loop_rate(200); //处理从串口来的uwb数据 while(ros::ok()) { //返回串口缓冲区中当前剩余的字符个数。一般用这个函数来判断串口的缓冲区有无数据 //当Serial.available()>0时，说明串口接收到了数据，可以读取 //串口缓存字符数 size_t n = ser.available(); //ROS_INFO_STREAM(n); int32_t ex; int32_t ey; if(n>=22) { uint8_t tmpdata[5000]; ser.read(tmpdata,n);//tmpdata 参数缓存 data_size 数据长度 if(tmpdata[0]==0X55)//效验是否正确 { int32_t ax=tmpdata[8]; int32_t bx=tmpdata[9]; int32_t cx=tmpdata[10]; int32_t dx=tmpdata[11]; ex=ax+bx*16*16+cx*16*16*16*16+dx*16*16*16*16*16*16; int32_t ay=tmpdata[12]; int32_t by=tmpdata[13]; int32_t cy=tmpdata[14]; int32_t dy=tmpdata[15]; ey=ay+by*16*16+cy*16*16*16*16+dy*16*16*16*16*16*16; nav_msgs::Odometry uwb_data; float uwb_x=ex/100.0; float uwb_y=ey/100.0; ROS_INFO_STREAM("***************(x,y)***************"); //ROS_INFO_STREAM(ex); //ROS_INFO_STREAM(ey); ROS_INFO_STREAM(uwb_x); ROS_INFO_STREAM(uwb_y); ROS_INFO_STREAM("***************(x,y)***************"); uwb_data.pose.pose.position.x = uwb_x; uwb_data.pose.pose.position.y = uwb_y; uwb_data.pose.pose.position.z = 0; //返回当前时刻的Time对象 uwb_data.header.stamp=ros::Time::now(); uwb_data.header.frame_id="uwb"; uwb_data.child_frame_id="base_footprint"; uwb_data.pose.pose.orientation.x=1; uwb_data.pose.pose.orientation.y=0; uwb_data.pose.pose.orientation.z=0; uwb_data.pose.pose.orientation.w=0; uwb_data.pose.covariance={0.1, 0, 0, 0, 0, 0, // covariance on gps_x 0, 0.1, 0, 0, 0, 0, // covariance on gps_y 0, 0, 999999, 0, 0, 0, // covariance on gps_z 0, 0, 0, 999999, 0, 0, // large covariance on rot x 0, 0, 0, 0, 999999, 0, // large covariance on rot y 0, 0, 0, 0, 0, 999999} ; uwb_publisher.publish(uwb_data); //处理ROS的信息，比如订阅消息,并调用回调函数 } } ros::spinOnce(); loop_rate.sleep(); } //关闭串口 ser.close(); return 0; }

8.录制bag文件参考ROS下bag包的录制与回放数据

mkdir ~/savebag //创建新的文件夹 cd ~/savebag //到这个文件夹下 rosbag record -a //录制 -a表示录制了所有节点发布的话题 rosbag record -O setbagname 话题1 话题2 //录制部分话题

8.在rviz中显示轨迹

遇到的问题

串口工具cutecom可以打印信息，但是程序报错Unable to open port 串口添加权限，*是要打开的串口

sudo chmod 777 /dev/ttyUSB*

如何查看串口信息查看所有串口信息（有时候是/drivers）：

cat /proc/tty/driver/serial

catkin_make不能生成可执行文件指定功能包编译

catkin_make -DCATKIN_WHITELIST_PACKAGES="package_name"

运行串口读取，并做成topic发布程序时，报错Segmentation fault (core dumped) Segmentation fault (core dumped)一般是指内存越界，将程序一点点注释排查后，应该是serial.read()引起的报错Segmentation fault (core dumped) 原程序为：

size_t n = ser.available(); uint8_t tmpdata[22];//串口返回的数据为22个字节 ser.read(tmpdata,n);

将22改为n出现过的最大数

【本文地址】

AMCL定位融合UWB

AMCL定位融合UWB

今日新闻

推荐新闻