uwb|实现基于双uwb的无人机位置控制

背景

考虑到无人机偏航角为磁力计所测得的前右下机体坐标系与北东地惯性系按321旋转时绕z轴的旋转角度，该偏航角在uwb坐标系与北东地坐标系无法完全对齐时不能使用。此时计算期望机体速度时所用的偏航角应为无人机相对uwb坐标系的偏航角

解决办法

在基于uwb的多无人机飞行中，可以采用将多架无人机同时朝uwb基站的同一个方向摆放，通过对各无人机的偏航角融合得到uwb坐标系该方向的地磁朝向，在位置控制时将该角度误差进行补偿
单无人机飞行时，可以在无人机上部署两个已知位置uwb标签，在初始化时通过计算两个标签过后成的向量在uwb坐标系中的方向，融合磁力计所测得的地磁角度，即可以得到uwb坐标系相对北东地坐标系的偏角，并在进行位置控制时予以补偿

执行步骤

编写获取双uwb位置坐标的节点

本文所用uwb为nooploop，运行以下launch文件实现两个uwb标签数据的同时获取

<launch>
    <node pkg="nlink_parser" type="linktrack" name="linktrack1" output="screen">
        <param name="port_name" value="/dev/tag0" />
        <param name="baud_rate" value="921600" />
    </node>

    <node pkg="nlink_parser" type="linktrack" name="linktrack2" output="screen">
        <param name="port_name" value="/dev/tag1" />
        <param name="baud_rate" value="921600" />
    </node>
</launch>

运行uwb标签数据解析节点sub_2uwb，该节点接收nooploop官方数据包后解析为两个标签的位置及速度并发布。
运行uwb2yawpos节点，接收标签位置，并做滑动窗口平均处理以去抖，最后利用相角求解两个uwb标签构成向量的方向，得到抖动在2度以内的uwb坐标系下的偏航角信息

roslaunch nlink_parser linktrack1.launch
rosrun yjh_msg sub_2uwb
rosrun yjh_msg uwb2yawpos

执行trans_odom节点，得到uwb坐标系下的位姿，并发布代替原有的odom