使用手册 - 同时使用激光、地纹、二维码、轮编里程计和IMU进行鲁棒定位
1.为什么要使用如此多的里程计
由于detour中tc(紧耦合)，参与的里程计越多，小车的定位就会越稳定、越平滑，所以希望能够有足够多的里程计参与其中。
激光的优点在于，视野大，并且可以从可视化中轻易看出小车目前所在位置，但是缺点是在高动态环境或无明显几何特征的环境中，定位会变得不稳定。
地纹的有点在于精度非常高，但是范围小，且脱离图层范围之后，难以快速重新重定位。
二维码的优点在于，可以快速恢复定位，但是若大规模使用，施工量大，且柔性不足。
轮里程计的有点在于，稳定，不受环境影响，缺点是打滑会产生影响，且无法回环消除误差。
所以将以上所有里程计同时使用，进行鲁棒定位
2.部署多个定位源
激光和地纹部署，在wiki的其它文章中已有说明，此处不做赘述，二维码部署也有详细说明，此处重点讲如何部署轮里程计
轮里程计需要通过车轮编码器、imu数据进行积分，得出一个完整的位置，然后post给detour使用。
首先在medulla中创建用于上传数据的类
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;
}
}
由于上传的数据要求无重复，且轨迹平滑，所以直接在motorRoutine中进行处理，保证每次读到驱动器数据后进行一次上传。
以差速轮底盘为例，进行轨迹计算。首先计算车轮在此次循环中走过的路程，单位使用毫米。由于是差速轮模型，所以车体此时的里程变换应该是左右轮变化量和的一半。
然后使用此次循环imu数据和上次循环做差，得到此时间段车辆角度变换。
然后在之前位置的基础上，加上此次位置变换，即不断对位置变化量做积分得到目前的位置，得到最新的位置，然后post给detour使用。
ExternalFeed中数据含义如下表
| 名称 | h含义 |
|---|---|
| name | 名称 |
| x | 定位的x坐标 |
| y | 定位的y坐标 |
| th | 定位的th |
| hasTranslation | 定位信息中是否包含x、y（如纯imu就只有角度） |
| hasRotation | 定位信息中是否包含th（如纯gps只有坐标） |
| is2D | 是否是2D位姿 |
| startingTick | startingTick不变时，认为是一个连续的轨迹 |
| counter | 计数，每个循环+1 |
| integrated | 是否通过积分得到的位姿（轮里程计通过积分获得，gps是绝对位置） |
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");
此时就完成了轮里程计数据的上传，然后需要在detour中添加数据源来使用此数据。
首先在detour车体布局中，添加外部定位源，并将name字段改为"cart_odometry"，将该定位源的x、y、th都改为0，然后点击动作中的捕捉，此时可以在状态中看到数据已经在跳动了。此时代表轮里程计已经参与到detour的tc计算中。
3.耦合效果判定
可以在detour-里程计-里程计状态中进行查看。
需要在稳定的环境中进行测试，即激光、地纹、轮里程计都无外部影响。此时的现象应该是，多个里程计交替成为最好传感器，且没有传感器被频繁丢弃。
4.Q&A
Q：若轮里程计始终是最好传感器或轮里程计频繁被丢弃，会是什么原因
A：detour判断最好传感器时，是根据此里程计提供的轨迹进行判断，轨迹越平滑、越合理，则会成为最好传感器。所以若轮里程计始终是最好传感器，需要查看轮里程计上传周期是否过于频繁，导致上传的轨迹”异常“的平滑和合理。一般将motorRoutine的扫描周期设为50ms，此时轮里程计也是50ms上传一次。若轮里程计被频繁丢弃，则需从两个方面检查，首先查看上传频率是否过低，导致位置虽然正确，但是不平滑，从而被detour丢弃。然后查看数据源是否正确，可以暂停其它里程计，仅使用轮里程计，通过Simple下发任务，看小车位置是否和激光提供的定位误差有多大。具体操作如下，在定位稳定的环境下建好激光地图，然后在一个点找到此时的激光定位（锁定到正确的关键帧），然后暂停激光里程计，通过simple下发任务，让小车进行行走、自旋、转弯等操作，然后记录此时位置，然后恢复激光里程计，再次找到此时的激光定位，记录此时位置，比较两次记录的位置是否有较大差异。
Q：陀螺仪选型标准是什么
A：笔者使用瑞芬tl740d进行测试，可以满足紧耦合要求，测试结果如下图，可作为参考，打码的为另一品牌陀螺仪
