查看“使用手册 - 同时使用激光、地纹、二维码、轮编里程计和IMU进行鲁棒定位”的源代码

=== 1.为什么要使用如此多的里程计 ===
由于detour中tc(紧耦合)，参与的里程计越多，小车的定位就会越稳定、越平滑，所以希望能够有足够多的里程计参与其中。

激光的优点在于，视野大，并且可以从可视化中轻易看出小车目前所在位置，但是缺点是在高动态环境或无明显几何特征的环境中，定位会变得不稳定。

地纹的有点在于精度非常高，但是范围小，且脱离图层范围之后，难以快速重新重定位。

二维码的优点在于，可以快速恢复定位，但是若大规模使用，施工量大，且柔性不足。

轮里程计的有点在于，稳定，不受环境影响，缺点是打滑会产生影响，且无法回环消除误差。

所以将以上所有里程计同时使用，进行鲁棒定位

=== 2.部署多个定位源 ===
激光和地纹部署，在wiki的[[使用手册 - 激光＋纹理导航|其它文章]]中已有说明，此处不做赘述，二维码部署也有[[使用手册 - 在地纹导航中使用二维码快速找回定位|详细说明]]，此处重点讲如何部署轮里程计

轮里程计需要通过车轮编码器、imu数据进行积分，得出一个完整的位置，然后post给detour使用。

首先在medulla中创建用于上传数据的类<syntaxhighlight lang="c#" line="1">
 public partial class TightCoupler
 {
     // Should put into medulla.
     public static SharedObject externalFeed;
     public static void PostExternalFeed(ExternalFeed obj, string name)
     {
         externalFeed ??= new SharedObject(name);
         byte[] buf = new byte[1024];
         using Stream stream = new MemoryStream(buf);
         using BinaryWriter bw = new BinaryWriter(stream);
         bw.Write(obj.name);
         bw.Write(obj.counter);
         bw.Write(obj.hasTranslation);
         bw.Write(obj.hasRotation);
         bw.Write(obj.is3D);
         bw.Write(obj.is2D);
         bw.Write(obj.integrated);
         bw.Write(obj.toRemap);
         bw.Write(obj.startingTick);
         bw.Write(obj.x);
         bw.Write(obj.y);
         bw.Write(obj.z);
         bw.Write(obj.th);
         bw.Write(obj.pitch);
         bw.Write(obj.roll);
         externalFeed.Post(buf.Take((int)stream.Position).ToArray());
     }


     public class ExternalFeed
     {
         // always aggregates.
         public string name;
         public long counter; // increment one per frame.
         public bool hasTranslation, hasRotation, is3D, is2D, integrated, toRemap;
         public long startingTick;
         public float x, y, z, th, pitch, roll;
     }

 }
</syntaxhighlight>由于上传的数据要求无重复，且轨迹平滑，所以直接在motorRoutine中进行处理，保证每次读到驱动器数据后进行一次上传。

以差速轮底盘为例，进行轨迹计算。首先计算车轮在此次循环中走过的路程，单位使用毫米。由于是差速轮模型，所以车体此时的里程变换应该是左右轮变化量和的一半。

然后使用此次循环imu数据和上次循环做差，得到此时间段车辆角度变换。

然后在之前位置的基础上，加上此次位置变换，即不断对位置变化量做积分得到目前的位置，得到最新的位置，然后post给detour使用。

ExternalFeed中数据含义如下表
{| class="wikitable"
|+
!名称
!h含义
|-
|name
|名称
|-
|x
|定位的x坐标
|-
|y
|定位的y坐标
|-
|th
|定位的th
|-
|hasTranslation
|定位信息中是否包含x、y（如纯imu就只有角度）
|-
|hasRotation
|定位信息中是否包含th（如纯gps只有坐标）
|-
|is2D
|是否是2D位姿
|-
|startingTick
|startingTick不变时，认为是一个连续的轨迹
|-
|counter
|计数，每个循环+1
|-
|integrated
|是否通过积分得到的位姿（轮里程计通过积分获得，gps是绝对位置）
|}
<syntaxhighlight lang="c#" line="1">
var leftDelta = cart.ActualLeftWheelPos - lastLeftWheelEncoder;
var rightDelta = cart.ActualRightWheelPos - lastRightWheelEncoder;
var distanceDelta = (leftDelta + rightDelta) / 2f;
var thDelta = MathTools.thDiff((float)cart.GyrosTh, (float)lastGyro);
var moveTup = Tuple.Create(distanceDelta, 0f, thDelta);
var curMode = cart.NavigationMode;
var moved = MathTools.Transform2D(
    Tuple.Create(cart.InertialNavigationX, cart.InertialNavigationY,
        cart.InertialNavigationTh), moveTup);
cart.InertialNavigationX = moved.Item1;
cart.InertialNavigationY = moved.Item2;
cart.InertialNavigationTh = moved.Item3;
lastLeftWheelEncoder = cart.ActualLeftWheelPos;
lastRightWheelEncoder = cart.ActualRightWheelPos;
lastGyro = cart.GyrosTh;

TightCoupler.PostExternalFeed(new TightCoupler.ExternalFeed()
    {
        name = "wheel",
        x = cart.InertialNavigationX,
        y = cart.InertialNavigationY,
        th = cart.InertialNavigationTh,
        hasTranslation = true,
        hasRotation = true,
        is2D = true,
        startingTick = startTime,
        counter = counter++,
        integrated = true,
    }, "cart_odometry");
</syntaxhighlight>此时就完成了轮里程计数据的上传，然后需要在detour中添加数据源来使用此数据。

首先在detour车体布局中，添加外部定位源，并将name字段改为"cart_odometry"，将该定位源的x、y、th都改为0，然后点击动作中的捕捉，此时可以在状态中看到数据已经在跳动了。此时代表轮里程计已经参与到detour的tc计算中。
[[文件:C4e06e908e2e3f4c9223045ee89fd5d.png|居中|缩略图|500x500像素]]

=== 3.耦合效果判定 ===
可以在detour-里程计-里程计状态中进行查看。
[[文件:954a68c5edbe408b7ef7293a7206706.png|居中|缩略图|500x500像素]]
需要在稳定的环境中进行测试，即激光、地纹、轮里程计都无外部影响。此时的现象应该是，多个里程计交替成为最好传感器，且没有传感器被频繁丢弃。

=== 4.Q&A ===
Q：若轮里程计始终是最好传感器或轮里程计频繁被丢弃，会是什么原因

A：detour判断最好传感器时，是根据此里程计提供的轨迹进行判断，轨迹越平滑、越合理，则会成为最好传感器。所以若轮里程计始终是最好传感器，需要查看轮里程计上传周期是否过于频繁，导致上传的轨迹”异常“的平滑和合理。一般将motorRoutine的扫描周期设为50ms，此时轮里程计也是50ms上传一次。若轮里程计被频繁丢弃，则需从两个方面检查，首先查看上传频率是否过低，导致位置虽然正确，但是不平滑，从而被detour丢弃。然后查看数据源是否正确，可以暂停其它里程计，仅使用轮里程计，通过Simple下发任务，看小车位置是否和激光提供的定位误差有多大。具体操作如下，在定位稳定的环境下建好激光地图，然后在一个点找到此时的激光定位（锁定到正确的关键帧），然后暂停激光里程计，通过simple下发任务，让小车进行行走、自旋、转弯等操作，然后记录此时位置，然后恢复激光里程计，再次找到此时的激光定位，记录此时位置，比较两次记录的位置是否有较大差异。

Q：陀螺仪选型标准是什么

A：笔者使用瑞芬tl740d进行测试，可以满足紧耦合要求，测试结果如下图，可作为参考，打码的为另一品牌陀螺仪
[[文件:618e4f05b59db604d4e312d6f0ef72b.png|居中|缩略图|600x600像素]]