2019-12-05

Posted 2023-04-16

参考技术A 简介

随着科技的进步，导航的方案也层出不穷，尤其是SLAM技术极大的促进了导航方案在机器人、无人驾驶等领域的发展，绝大多数导航方案都会使用惯性测量单元（IMU）来融合其他传感器来实现更加精确的导航。然而在体积、成本、性能、功耗等诸多因素的限制下，如何选择一款适合客户应用的姿态传感器便成了客户最大的难点。

HI2XX系列姿态模块是综合了体积、成本、性能、功耗等诸多因素后推出的工业级姿态传感器，这篇文章旨在帮助设计人员理解IMU的基础知识及相关应用，并为用户提供选择方案。

术语

IMU惯性测量单元(Inertial Measurement Unit) 是测量物体三轴角速度和加速度的设备。一个IMU内可能会装有三轴陀螺仪和三轴加速度计，来测量物体在三维空间中的角速度和加速度。严格意义上的IMU只为用户提供三轴角速度以及三轴加速度数据。

VRU 垂直参考单元(Vertical Reference Unit)是在IMU的基础上，以重力向量作为参考，用卡尔曼或者互补滤波等算法为用户提供有重力向量参考的俯仰角、横滚角以及无参考标准的航向角。通常所说的6轴姿态模块就属于这类系统。航向角没有参考，不管模块朝向哪里，启动后航向角都为0°(或一个设定的常数)。随着模块工作时间增加，航向角会缓慢累计误差。俯仰角，横滚角由于有重力向量参考，低机动运动情况下，长时间不会有累积误差。

AHRS 航姿参考系统(Attitude and Heading Reference System)AHRS系统是在VRU的基础上增加了磁力计或光流传感器，用卡尔曼或者互补滤波等算法为用户提供拥有绝对参考的俯仰角、横滚角以及航向角的设备，这类系统用来为飞行器提供准确可靠的姿态与航行信息。我们通常所说的9轴姿态传感器就属于这类系统，因为航向角有地磁场的参考，所以不会漂移。但地磁场很微弱，经常受到周围带磁物体的干扰，所以如何在高机动情况下抵抗各种磁干扰成为AHRS研究的热门。

GNSS/INS 组合导航系统，顾名思义这种系统是利用全球卫星导航系统(Global Navigation Satellite System 简称GNSS，它是GPS，北斗，GLONASS、GALILEO等系统的统称) 与惯性导航(Inertial Navigation System)各自的优势进行算法融合，为用户提供更加精准的姿态及位置信息。

下图是组合导航系统的一个基本的框图，它以加速度计、陀螺仪、磁力计、气压计、GNSS等作为基本输入，利用融合算法输出用户所需要的姿态信息、位置信息以及速度信息。

HI2XX系列传感器包含HI226和HI229，其中HI226是VRU，HI229是AHRS。它们都是工业级姿态传感器。

关于几轴

我们生活在三维世界，人们理所当然的认为只有三个轴。那么为什么会出来6轴，9轴以及10轴？在这里我们不能简单地把6轴，9轴向物理世界对应: 它实际的意思是表示N种测量值。一个典型的姿态测量系统可以测得加速度和角速度，总共是6维向量，这是我们通常所说的6轴IMU，除此之外系统还可能包括三维的地磁场以及一维的气压计，就成了我们通常称的9轴和10轴系统。

自由度(Degree of Freedom, DoF) : 6DoF，9DoF也是经常听到的说法。空间中的刚体只有平移和旋转。其中平移三个自由度，旋转三个自由度，因此用9Dof与10Dof来描述姿态系统实际上是不恰当的，但是目前国内有一些厂商依然会用9DoF，10DoF来描述系统，我们理解就可以，不必过于深究。

HI226与HI229分别属于6轴姿态传感器与9轴姿态传感器。

下表为HI2XX系列的产品特性总结：

IMU性能指标及误差源

量程(Range) 它指的是IMU可以测量的加速度角速度的范围，意味着只要传感器运动范围如果不超过IMU的量程，那么便可以提供准确的数据，通常角速度的单位是 °/s，加速度单位是g。正常情况下, 机器人和无人驾驶角速度一般不会超过200 °/s，加速度不会超过4g，但是大量程可以在产品使用的过程中经受偶发性冲击，鲁棒性好。

随机游走(Random Walk) IMU系统通过对角速度与角加速度积分来获得姿态角与速度，但是原始测量值中含有噪声，这些噪声被积分后便形成了随机游走，并随着时间的平方根变化而变化。角度随机游走（ARW）的单位是°/sqrt(hr),速度随机游走噪声（VRW）的单位是m/s/ sqrt(hr)。

随机游走(Random Walk) 它体现了在输出带宽内，频率对噪声的影响。角速度的噪声频谱密度单位是°/s/√Hz，加速度的噪声频谱密度是g/√Hz。

零偏(Bias) 当IMU保持静止时, 它依然会有一个很小输出，这个输出的数值就是零偏。它会受到IMU的上电状态、温度、内部结构等因素影响，比如陀螺仪理论上静止时应该是0°/s，实际上陀螺仪静止的输出是一个均值不为零的噪声。

零偏稳定性(Bias Stability/In-run bias ) 这是评价低成本IMU非常重要的性能指标，它可以被定义为IMU相对于其输出速率平均值的偏差或漂移量。陀螺仪零偏稳定性的单位是°/h，加速度计的零偏稳定性单位是g/h。

比例因子(Scale Factor) 比例因子描述了输入与输出的相关性，比如载体实际旋转的角速度是100 °/s，但是陀螺仪输出的确是98°/s，真实值和测量值之间受到了比例的影响，可以被描述为下公式：

y=SF(x)+b+v

其中y为传输器输出，x为真实值，SF为比例因子，b为零篇，v为传感器噪声。

非线性度(Nonlinearity) 也叫做比例因子的高次性。比例因子实际上不是常量。它本身还会随着加速度或角速度的变化而变化(一般变化程度很小)，非线性度越小越好。

非正交性(Misalignment) 在理想的情况下，坐标系的轴与轴之间是绝对正交的，但是现实情况下IMU的坐标轴之间却不是完全正交的。这个指标会对剧烈无规则高机动运动产生很大的影响。

加计敏感度(G- Sensitivity) 陀螺仪是感应角速度的器件，但也会受到加速度的影响。这是微机电陀螺仪最常见的现象。

并不是上述所有误差源都会对系统造成很严重的影响，通常每种应用对应着不同的典型工作环境。但是了解哪些误差源对系统的影响大才有可能在系统设计阶段尽量减小误差。一般来讲，零偏，比例因子，非正交性可以通过出厂前校准得到改善。而零偏稳定性，噪声和非线性度对校准后的IMU使用影响比较大。除此之外，温度对系统影响也很大，但是鉴于高性价比的一般中低端IMU受温度影响的特性非常复杂，批量级温度补偿对于中低端工业级IMU来说还是一个挑战。影响IMU性能的主要因素如下图所示：

针对上述误差，我们专门为HI2XX系列定制了校准设备，出厂之前每一颗都经过了严格的校准，校准之后各方面指标如下。

精度指标如下表：

物理尺寸以及电气特性如下表所示：

应用场景

导航

很多应用场景都需要监测位置以及方向，比如无人驾驶汽车、移动机器人、无人船等，HI226最典型的应用就是移动机器人，比如扫地机器人，送餐机器人，大型商用机器人。性能优异的IMU有助于机器人用户降低激光雷达成本，缩短开发时间，下图是某机器人公司利用HI226以及低成本激光雷达做的导航方案效果图。

控制

很多应用场景会用到IMU的原始加速度与角速度数据，缓慢的航向漂移对这种应用不重要，可以接受一些噪声以及误差比如摄像机稳台、云台、关节的动作捕捉、体育训练等，这些领域对于IMU的要求必须是低成本的。HI226低延时与高速率特性可以很好的与这些场合相结合。

总结

模板与泛型编程

16.1.1函数模板

//template parameter list
template int compare(const T1& v1, const T2&v2) {     if (v1 < v2) return -1;     if (v2 < v1) return 1;     return 0; }

When we call a function template, the compiler (ordinarily) uses the arguments of the call to deduce the template parameter(s) for us.

The compiler uses the deduced template parameter(s) to instantiatea specific version of the function for us.

模板类型参数

函数模板的参数列表必须明确标明typename或者class，两个是同义的（typename在模板引入C++之后才出现，很多早先的程序使用class）

非类型模板参数

// 整数
template int compare(const char(&p1)[N], const char(&p2)[M]) {     strcpy(p1,p2); } // 指针 template<const char* C> void func1(const char* str) {     cout << C << " " << str << endl; }  // 引用 template<char(&R)[9]> void func2(const char* str) {     cout << R << " " << str << endl; }  // 函数指针 template<void(*f)(const char*)> void func3(const char* c) {     f(c); } void print(const char* c) { cout << c << endl; } char arr[9] = "template";   // 全局变量，具有静态生存期 int main() {     func1("pointer");     func2("reference");     func3("template function pointer");     return 0; }

绑定到非类型整型的实参必须是一个常量表达式，绑定到指针或引用的非类型参数的实参必须有静态的生存期（比如全局变量）

inline和constexpr的函数模板

template inline int compare(const T1& v1, const T2&v2); template constexpr int compare(const T1& v1, const T2&v2);

 

对于函数模板，编译器可以通过参数推断，得出模板中的类型信息，因而在使用函数模板时可以省略。

16.1.2类模板

对于函数模板，编译器可以通过参数推断，得出模板中的类型信息，因而在使用函数模板时可以省略。对于类模板，必须在模板后的尖括号中提供信息，代替模板参数实参列表。

1. 类模板中，模板参数被被当做stand-ins（替身），使用时被替换
2. 类模板中的成员函数只有在使用时才会被实例化，这使的某种类型即使不完全符合模板要求，仍然可以使用该类型的模板。
3. 实例化后的类型为ClassName类型。
4. 类包含友元时，模板类包含非模板友元，则友元可以访问模板的所有类型实例；友元是模板，则所有的友元实例都可以访问模板类的所有实例。
5. 对于static成员，每一个不同类型实例，都有不同的static成员，相同类型的实例实例化出来的对象共享static成员。

#include #include  using namespace std;  template  class BlobPtr;// forward declarations needed for friend declarations in Blob template  class Blob; // needed for parameters in operator==  template  bool operator==(const Blob&, const Blob&); template class Blob {     // each instantiation of Blob grants access to the version of     // BlobPtr and the equality operator instantiated with the same type     friend class BlobPtr;     friend bool operator== (const Blob&, const Blob&); public:     using size_type = vector::size_type;     //construct function     Blob():data(make_shared

类模板示例

16.1.3模板参数

模板参数的作用域在声明之后定义结束之前。

模板参数可以覆盖外部作用域的类型参数。

模板参数可以有默认值

使用类的类型成员时需要使用typename关键字，如typename T::value_type()

//默认模板参数
templateint> //使用参数类型的类型成员作为返回值的类型 typename vector::size_type differ(vector& lh, vector& rh) {     return lh.size() - rh.size(); }

16.1.4类的成员也可以是模板

普通类中的成员可以为模板

模板类中的成员可以为模板

//为了在unique_ptr析构对象的时候打印出来信息
class DebugDelete { public:     DebugDelete(std::ostream&s = std::cerr) :os(s) {}     template  void operator()(T*p)const     {         os << "deleting unique_ptr" << std::endl;         delete p;     } private:     std::ostream &os; }; int main() {     //会生成一个int版本的DebugDelete实例     unique_ptr<int, DebugDelete> p(new int, DebugDelete());     return 0; }

16.1.5控制实例化

模板被使用的时候才会实例化，其实例可能出现在多个对象文件中，当独立编译的原文件使用相同的模板并提供了相同的模板参数，每个文件都会有一个实例。为了解决这个开销，可以使用显式实例化。

extern template declaration; //实例化的声明

template declaration;//实例化的定义

编译器遇到external声明，将会在其他位置寻找定义，但在其他位置必须有一个定义。

实例化定义会实例化所有成员，所以类型必须能够引用所有成员函数

16.2模板实参推断

1. 在调用模板的时候，一般不会发生参数类型的转换。会发生的转换有两种，非const转换成const，数组或函数名转换成指针。
2. 如果函数参数不是模板参数类型，这个参数是可以进行类型转换的
3. 使用相同模板参数类型的函数形参类型必须相同，不会进行参数转换
4. 当模板实参类型无法推断，例如返回值类型时，必须显式指定。（通过fun
5. 显式指定了模板实参类型的，也可以进行类型转换（fun(15)，15转换为double）。

16.2.3尾置返回类型与类型转换

有时候返回值的类型与参数的类型有关，这时候可以用尾置返回类型

template  auto fcn(It beg, It end) -> decltype(*beg) {     return *beg; }

由于解引用运算返回的是引用类型，所以返回值是引用类型。

如果要返回的是值，则使用remove_reference（是一个转换模板，定义在type_traits头文件中）。

template   //第二个typename表示type成员为类型 auto fcn(It beg, It end) -> typename remove_reference

其他的类型转换模板请参见这里。

16.2.6理解std::move

Std::move将一个对象转变为右值引用对象。

template<class _Ty> inline constexpr typename remove_reference<_Ty>::type&& move(_Ty&& _Arg) noexcept {    // forward _Arg as movable
    return (static_cast::type&&>(_Arg)); }

在调用时

std::string s1("hi!"), s2; s2 = std::move(std::string("bye!")); s2 = std::move(s1);

使用调用

s2 = std::move(std::string("bye!"));

1. 推断出模板类型_Ty为string
2. remove_reference<_Ty>::type为string
3. move返回类型为string&& 

使用调用

s2 = std::move(s1);

1. 推断出模板类型_Ty为string&
2. remove_reference<_Ty>::type为string
3. move返回类型为string& &&，折叠后为string&
4. move函数参数t实例化为string& &&，会折叠为string&

16.2.7转发

某些函数需要将其一个或多个实参和类型不变地转发给其他函数，包括是否是const以及左值还是右值。

这个模板将一个函数的参数转发到里边的函数。

template void flip1(F f, T1 t1, T2, t2) {     f(t2, t1); }

这个模板在值传递的实例中是正确的，但是在如下调用：

function<void(int, int&)> fun = [](int a, int& b) {cout << a << ++b << endl; }; int a = 1, b = 2; flip1(fun, a, b); cout << a << b << endl;

你会发现，函数并没有改变第二个参数的值，没有将引用这个参数类型传入。

 

修改模板为

template void flip1(F f, T1&& t1, T2&& t2) {     f(t1, t2); }

这也会存在一些情况下的错误

function<void(int&&, int&)> fun = [](int &&a, int& b) {cout << a << ++b << endl; }; int a = 1; flip1(fun, 1, a);

1为右值传递到模板flip1中后，作为一个变量的形参，会成为一个左值（变量是左值），左值绑定到fun的第一个右值参数时，会出现错误。

 

再次修改为

template void flip1(F f, T1&& t1, T2&& t2) {     f(std::forward(t1), std::forward(t2)); }

使用forward，将返回类型为T&&的结果，如果出入为值，则返回为右值，传入为左值引用，则引用折叠后返回左值引用。

16.3重载与模板

如下两个模板

template std::string debug_rep(const T& t) {     std::ostringstream ret;     ret << t;     return ret.str(); } template std::string debug_rep(T* p) {     std::ostringstream ret;     ret << "pointer: " << p << " " << (p ? debug_rep(*p) : "null pointer");     return ret.str(); }

如果有调用

int a = 1; std::cout << debug_rep(a) << std::endl;

只有第一个模板时可行的，则选择第一个实例化

 

如果有调用

int a = 1; std::cout << debug_rep(&a) << std::endl;

则有

debug_rep(const int*&) debug_rep(int*)

这两个版本的实例化，由于第一个需要有非const转换到const，所以选择了第二个版本。

 

如果有调用

const int* a = &b; std::cout << debug_rep(a) << std::endl;

则上述两个版本都是精确的，然而debug_rep(T* p)对于这个调用来讲，是更特例的版本，只针对于指针，所以选择最特例话的版本。（从另一个角度讲，(const T& t)适用任何类型，这样debug_rep(T* p)永远不会被使用，这条规则才能使其被使用）

 

另外，非模板函数会优先匹配

16.4可变参数模板

一个可变参数模板（variadic template）中可变数目的参数被称为参数包（parameter packet），分为两种参数包：模板参数包（template parameter packet）、函数参数包（function parameter）。

// Args is a template parameter pack; rest is a function parameter pack // Args represents zero or more template type parameters // rest represents zero or more function parameters
template  void foo(const T &t, const Args& ... rest) {     cout << sizeof...(Args) << endl;  // number of type parameters     cout << sizeof...(args) << endl;  // number of function parameters }

用typename…表示零或多个类型的列表，一个类型名后边跟省略号，表示非类型参数的列表。如果一个函数参数的类型是模板参数包，则此参数是一个函数参数包，可以用sizeof…求出参数的个数。

16.4.1例子，打印所有参数

第一个模板在第二个模板递归调用的最后一次中，时最为匹配的，调用时终止了第二个模板的递归。

// function to end the recursion and print the last element // this function must be declared before the variadic version of print is defined
template ostream &print(ostream &os, const T &t) {     return os << t; // no separator after the last element in the pack } // this version of print will be called for all but the last element in the pack template  ostream &print(ostream &os, const T &t, const Args&... rest) {     os << t << ", ";  // print the first argument     return print(os, rest...); // recursive call; print the other arguments }

16.4.2包扩展

template  ostream &print(ostream &os, const T &t, const Args&... rest) //扩展Args，生成参数列表 {     os << t << ", ";       return print(os, rest...); //扩展实参，形成实参列表 }

在扩展实参的时候，还可以这样用

print(os, debug_rep(rest)...);

可以将扩展出来的每一个参数，执行函数调用

return print(os, debug_rep(p1), debug_rep(p2), debug_rep(p3));

16.4.3转发参数包

// fun haszero or more parameters each of which is // an rvalue reference to a template parameter type
template

16.5模板特例化

// firstversion; can compare any two types
template  int compare(const T&, const T&); // second version to handle string literals template int compare(const char(&)[N], const char(&)[M]);

定义的这两个模板，可以比较任意对象类型。但是，如果传入的是字符串类型的指针（char*）时，回到第一个模板中调用，然而比较了参数指针的大小。

为了能够处理字符指针的类型，可以定义一个特列

// specialversion of compare to handle pointers to character arrays
template <>
int compare(const char* const &p1, const char* const &p2) {     return strcmp(p1, p2); }

当提供一个特例化，其类型必须与之前的模板参数中对应的类型匹配。如上是为了特例化第一个模板template int compare(const T&, const T&);了，其参数是顶层const的，而对于指针类型char*，其参数是底层const，所以在特例化的时候为const char* const &p1。

 

需要注意的是，特例化本质是实例化一个模板，而非重载。因此特例化不影响函数匹配。这与将其定义为非模板函数是有区别的。

加入定义了非模板函数，这样在传入字符常量的时候，例如

compare("hi", "mom");

会匹配到我们定义的非模板函数，而这个实际上我们希望匹配到int compare(const char(&)[N], const char(&)[M]); 

注意，特例化必须声明在作用域中，故声明在一个头文件中。

 

类模板的特例化

如果自定义类需要使用无序容器，必须定义自己的hasher版本，或者特例化hash版本，一个特例化的hash类必须定义：

1. 一个重载调用运算符，接收一个容器关键字类型的对象，返回一个size_t
2. 两个类型成员，result_type和argument_type，分别为调用运算符的返回类型和参数类型
3. 默认构造函数和拷贝赋值运算符（可以隐式定义）。

注意，特例化需要与被特例化的模板在同一个作用域，所以使用namespace参数。

示例：对Sales_data

// openthe std namespace so we can specialize std::hash
namespace std  {     template <>  // we‘re defining a specialization with
    struct hash // the template parameter of Sales_data     {         // the type used to hash an unordered container must define these types         typedef size_t result_type;         typedef Sales_data argument_type; // by default, this type needs ==         size_t operator()(const Sales_data& s) const;         // our class uses synthesized copy control and default constructor     };     size_t hash::operator()(const Sales_data& s) const     {         //下边是hash函数的算法，使用了string、uint、double的hash算法         return hash<string>()(s.bookNo) ^             hash()(s.units_sold) ^             hash<double>()(s.revenue);     } }

注意，有时候会使用私有成员进行hash，所以声明为友元

// needed for the friend declaration
template  class std::hash;   class Sales_data {     friend class std::hash;     // other members as before };

 

类模板的部分特例化

我们可以部分特例化类模板，不能部分特例化函数模板

template <class T> struct remove_reference {     typedef T type; }; // partialspecializations that will be used for lvalue and rvalue references
template <class T> struct remove_reference

特例化模板的成员

template  struct Foo {     Foo(constT &t = T()) : mem(t) { }     void Bar() {  }     T mem;     // other members of Foo }; template<>  // we‘re specializing a template void Foo<int>::Bar() // we‘re specializing the Bar member of Foo {     // do whatever specialized processing that applies to ints }

当使用int类型的模板时，会使用这个特例化的Bar()函数成员。