Yuhang：创建页面，内容为“=== 1.为什么要使用如此多的里程计 === 由于detour中tc(紧耦合)，参与的里程计越多，小车的定位就会越稳定、越平滑，所以希望能够有足够多的里程计参与其中。激光的优点在于，视野大，并且可以从可视化中轻易看出小车目前所在位置，但是缺点是在高动态环境或无明显几何特征的环境中，定位会变得不稳定。地纹的有点在于精度非常高，但是范…”

2024-06-17T07:18:35Z

创建页面，内容为“=== 1.为什么要使用如此多的里程计 === 由于detour中tc(紧耦合)，参与的里程计越多，小车的定位就会越稳定、越平滑，所以希望能够有足够多的里程计参与其中。激光的优点在于，视野大，并且可以从可视化中轻易看出小车目前所在位置，但是缺点是在高动态环境或无明显几何特征的环境中，定位会变得不稳定。地纹的有点在于精度非常高，但是范…”

新页面

=== 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像素]]

使用手册 - 同时使用激光、地纹、二维码、轮编里程计和IMU进行鲁棒定位 - 版本历史