# uwbworkspace-kaiyuan

**Repository Path**: yongwangzhiqiankai/uwbworkspace-kaiyuan

## Basic Information

- **Project Name**: uwbworkspace-kaiyuan
- **Description**: uwb定位算法
    作者： 苏凯
    微信号：yongwangzhiqiankai 微信昵称：一往无前
    邮箱： 422168787@qq.com
    qq交流群号: 742679602,也欢迎企业hr入群招聘
    项目仅供学习交流，企业使用需联系作者
- **Primary Language**: Unknown
- **License**: Zlib
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 5
- **Forks**: 6
- **Created**: 2025-02-08
- **Last Updated**: 2025-07-05

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

视频演示：
        https://www.bilibili.com/video/BV1Sb41197Sf/?spm_id_from=333.1387.upload.video_card.click&vd_source=a40d5b174b39ff9a4157afe245dd7db1
第一次编译：
	cd uwbworkspace
	source catkit_make_source.sh

 使用：
    启动: roslaunch uwblocation uwblocation.launch
    rostopic echo /uwb/data/all 输出uwb标签在uwb地图下的坐标
    rostopic echo /mapTuwbpose  输出计算好的uwb标签在ros地图下的坐标
    rostopic  echo /initialpose 输出重定位数据
    操作uwb： rosrun rqt_service_caller rqt_service_caller
             service节点 /serviceClient  消息类型contnav_srvs::uwbmain
                                                          request_type：initialpose 通过uwb给小车重新定位
                                                          request_type：demarcateUwbEqBaselink  标定 uwbbaselink 换算成 map下的baselink坐标 与 map下的rosbaselink坐标的误差，得到一个误差的标定数据 demarcateUwbEqBaselink_transforms.txt; 注意： launch 中 参数isdemarcateUwbEqBaselink=false时才能标定.后续代码中使用到该值时时需要再次打开为true;


---
调试：
    rosrun tf tf_echo odom base_link
    roswtf
    roslaunch uwblocation uwblocation.launch
    rosrun rqt_service_caller rqt_service_caller
    rosrun teleop_twist_keyboard teleop_twist_keyboard.py
    rostopic echo /uwb/data/all
    rostopic echo /mapTuwbpose
    rostopic  echo /initialpose


     ros::ServiceClient    uwbLocationClient = nh.serviceClient<contnav_srvs::uwbLocation::Request, contnav_srvs::uwbLocation::Response>("/uwbLocationmain");
                                                          request_type：setUwb    设置uwb基站数据
---


-----

使用前部署配置：

    浩如uwb产品资料：
            source/dos/HR-RTLS1_UserManual.pdf
            source/dos/LD600(-I)_UserManual.pdf
            source/image
        注意：
        拿到标签后，需要拆开后盖，把1号开关拨到0位,才能用于集成ros系统；
        这款产品的串口芯片是ch340EB不支持该串口号。


    uwb基站搭建
    搭建wub基站,基站要标定距离请看供应商提供的视频

                                                  y         2m
                                              a1 ^                       a3
                                                 _
                                                 _
                                                 _
                                            1m   _    uwb                      1m
                                                 _
                                                 _
                                                 _
                                                 ---------------------->x
                                                 a0        2m            a2

    配置基站参数：
           1.分别测量基站之间的距离, a0为uwb坐标的原点（0,0）位，a2是x方向，a1是y方向的值，高度都为2.59m
           2.在 /home/sukai/workspace/uwbworkspace/src/uwblocation/source/param/config.yaml 文件中分别填入测量的距离值单位m；

              例子：
                       a0_x: 0.0
                       a0_y: 0.0
                       a0_z: 2.59

                       a1_x: 0.0
                       a1_y: 1
                       a1_z: 2.59

                       a2_x: 2
                       a2_y: 0.0
                       a2_z: 2.59

                       a3_x: 2
                       a3_y: 1
                       a3_z: 2.59





    小车上安装标签：

           1.将两个标签一前一后安装在车上与车的雷达正前方保持在同一条线上，后面的标签与小车中心位置重合，前面的标签离后面的标签越远越好（取决于uwb的精度）；【如浩uwb产品的标签有标签号，T0标签安装在后面， T1标签安装在前面，保持30cm以上距离；】
              在 launch/uwblocation.launch 的uwbLocation_uwbmain_node节点中配置 uwbId 参数, 配置安装在小车后面与小车中心点重合的那个标签的id号。(标签号在外壳上),
           2.可以绑定串口的化尽量绑定,标签通过串口号来区分前后标签数据
           3.在  launch/uwblocation.launch 中的uwb_imu_node_zero 与 uwb_imu_node_one两个节点中 ，分别配置前后标签的串口号，顺序之分；
             例：
               3.1 配置后面的标签
               <node  pkg="uwblocation" type="uwb_imu_node"  name="uwb_imu_node_zero"  clear_params="true"  output="screen"  >
                    <param name="serialPortName" type="string" value="/dev/ttyUSB0" /> <!-- 串口号,配置后面的T0标签 (后面的标签与小车中心位置重合)-->
               3.2 配置前面的标签
               <node  pkg="uwblocation" type="uwb_imu_node"  name="uwb_imu_node_one"  clear_params="true"  output="screen"  >
                    <param name="serialPortName" type="string" value="/dev/ttyUSB1" /> <!-- 串口号,配置前面的T1标签 -->



    配置uwbmap与map的对应变换换矩阵（uwb与rosmap都是遵循右手坐标系）

        uwb 测量法
        测量uwb坐标系与map坐标系之间的关系
               1.小车建地图时在地上做好小车的起始点位置的标记，此标记就是map地图的的初始点0,0位置，车的朝向是map的x方向。此时车在地图中的坐标是x:0,：y:0,姿态：0；
               2.搭建wub基站,基站要标定距离请看供应商提供的视频


                                                  y
                                              a1 ^
                                                 _
                                                 _
                                                 _
                                                 _    uwb
                                                 _
                                                 _
                                                 _
                                                 ---------------------->x
                                                 a0                    a2


               3.测量uwb的x方向与,ros map中x 的方向的角度值转弧度值填入launch的uwbmaptmap_static_transform_publisher_node 的 tf参数中
               4.测量 map 地图的的初始点 x:0,y:0位置到 uwb a0 位置的差值,把参数填入 launch 的 uwbmaptmap_static_transform_publisher_node 的 tf参数中(以map的初始点 x,y测量 a0 在map初始点的相对位置)
               5.launch 中的参数 isreadmapTuwbmapfromtxt 改 false


                以下是测量法示例子：
                                      |\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
                                      |场地
                                      |   robot(小车坐标系)                                                                                              |
                                      |                                                                                                                 |
                                      |                                                                                                                 |
                                      |                                                          x                                                     |
                                      |                                                      a1   ^                                                     |
                                      |                                                           _                                                     |
                                      |                                                           _                                                     |
                                      |                                                           _                                                     |
                                      |                                                           _    robot(小车坐标系)                                 |
                                      |                                                           _                                                     |
                                      |                                                           _                                                     |
                                      |                                                           _                                                     |
                                      |                                   y <----------------------                                                     |
                                      |                                                         baselink(0,0) 【建图时小车所在的位置】                     |
                                      |   uwb坐标系:                                                                                                     |
                                      |                                                                                                                 |
                                      |                                                                                                                 |
                                      |                       y                                                                                         |
                                      |                   a1 ^                        a3                                                                |
                                      |                      _                                                                                          |
                                      |                      _                                                                                          |
                                      |                      _                                                                                          |
                                      |                      _    uwb坐标系                                                                              |
                                      |                      _                                                                                          |
                                      |                      _                                                                                          |
                                      |                      _                                                                                          |
                                      |                      ---------------------->x                                                                   |
                                      |                      a0 (0,0)                   a2                                                              |
                                      |                                                                                                                 |
                                      |                                                                                                                 |
                                      |\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\
                                      注意： uwb坐标系 与 robot(小车坐标系)   尽量保持统一轴线，平行即可，这样它两的姿态就容易测量，目测就可以解决了。


                1.通过右手坐标系,把 uwb坐标系旋转与 robot(小车坐标系)的x轴方向一致，测量旋转的弧度,已上图为例目测为 正的 90度 既 yaw = 1.57 弧度;
                2.通过右手坐标系,把 uwb坐标系平移与robot(小车坐标系)重合，我们可以用尺测量 uwb坐标系 原点a0与  robot(小车坐标系)   中 baselink(0,0)  的x距离值与y距离值,
                  a0 (0,0)   在 baselink(0,0) 的x的正方向上,测量值为1.2m, 即 x= 1.2
                  a0 (0,0)   在 baselink(0,0) 的y的正方向上,测量值为1.8m, 即 y=-1.8
                3.在  launch/uwblocation.launch 中找到：
                    <node name="uwbmaptmap_static_transform_publisher_node" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0 0 0 0 /map /uwbmap" output="screen"/>
                  修改值：
                    <node name="uwbmaptmap_static_transform_publisher_node" pkg="tf2_ros" type="static_transform_publisher" args="1.2 -1.8 0 1.57 0 0 /map /uwbmap" output="screen"/>
                4.编译后重新启动系统

----------------------
浩如uwb产品数据标定：
     参考资料 source/dos/LD600(-I)_UserManual.pdf
    1.用windos 打开串口进行调试发 $rboot 把系统数据读出来,把16进制的勾去掉
    2.参考这个视频
     【UWB模块启动信息获取-哔哩哔哩】 https://b23.tv/KR4VgX2


----
总结：
    小车姿态是通过2个uwb标签计算的，所以对uwb数据的稳定一致性的要求及其高