PX4 imu传感器配置

PX4 传感器配置

##坐标系定义

旋转变化

reference

在机器人中各个坐标系之间的变换是非常常见的，而在ROS tf 工具中，以及Eigen 中 我们常常会见到RPY的旋转表示和四元数的旋转表示。当然学航空出生的我们可能更常见的是Euler描述。那么这三者的关系我们就必须说清楚的。

首先，三种表达方式显然都是描述同一个旋转。在不加说明的情况下，我们所描述的都是从机体坐标系( b 系 )到参考坐标系( i 系 )的旋转,即: $X_i=R{X}_b$

Euler的描述顺序： 依次绕机体系的 Z–>Y–>X的旋转，即动系旋转, 应该采用右乘法：
$$R=R_Z{R}_Y{R}_X$$
![在这里插入图片描述](https://img-blog.csdnimg.cn/eca0ee5d6baf420d94e51dd30e6b4bba.png
注：图片是B是动系，A是参考系

RPY的描述顺序： 依次绕着参考系（定系）的 X–>Y—>Z ,即定系旋转，应采用左乘法：
$$R=R_Z{R}_Y{R}_X$$
![在这里插入图片描述](https://img-blog.csdnimg.cn/903e42152a8f44778424e23bd5118f5e.png
由以上公式可见，两个描述下的$ \alpha , \beta ,\gamma$ 的值，一模一样

所以RPY [90 , 45, 10] 也是欧拉角描述的：滚转角90度，俯仰角45度，偏航角10度。

imu配置

px4 对 IMU，compass传感器的配置都是一致的。其启动配置在/boards/.../init/rc.board_sensors中。 以FMU_V5为例，具体配置如下：

#!/bin/sh
#
# PX4 FMUv5 specific board sensors init
#------------------------------------------------------------------------------

board_adc start

# Internal SPI bus ICM-20602
icm20602 -s -R 2 -q start

# Internal SPI bus ICM-20689
icm20689 -s -R 2 start

# Internal SPI bus BMI055 accel/gyro
bmi055 -A -R 2 -s start
bmi055 -G -R 2 -s start

# Baro on internal SPI
ms5611 -s start

# internal compass
ist8310 -I -R 10 start

# External compass on GPS1/I2C1 (the 3rd external bus): standard Holybro Pixhawk 4 or CUAV V5 GPS/compass puck (with lights, safety button, and buzzer)
ist8310 -X -b 1 -R 10 start

在配置中其他参数的具体作用，可以查看PX4开发者手册，也可以在nsh中输入icm20649(具体的驱动模块名) help:

icm20649 <command> [arguments...]
 Commands:
   start
     [-s]        Internal SPI bus(es)
     [-S]        External SPI bus(es)
     [-b <val>]  board-specific bus (default=all) (external SPI: n-th bus
                 (default=1))
     [-c <val>]  chip-select index (for external SPI)
                 default: 1
     [-m <val>]  SPI mode
     [-f <val>]  bus frequency in kHz
     [-q]        quiet startup (no message if no device found)
     [-R <val>]  Rotation
                 default: 0

   stop

   status        print status info

这里，主要再说明一下R参数的作用。正如help功能所输出的结果，R表示的是传感器到机体坐标系(NED)的一个旋转关系，具体参数在rotation.h中定义：

这里采用了RPY的描述，它的具体定义和飞机常用欧拉角的关系，我在上一章旋转变化作了说明

enum Rotation : uint8_t {
	ROTATION_NONE                = 0,
	ROTATION_YAW_45              = 1,
	ROTATION_YAW_90              = 2,
	ROTATION_YAW_135             = 3,
	ROTATION_YAW_180             = 4,
	ROTATION_YAW_225             = 5,
	ROTATION_YAW_270             = 6,
	ROTATION_YAW_315             = 7,
	ROTATION_ROLL_180            = 8,
	ROTATION_ROLL_180_YAW_45     = 9,
	ROTATION_ROLL_180_YAW_90     = 10,
	ROTATION_ROLL_180_YAW_135    = 11,
	ROTATION_PITCH_180           = 12,
	ROTATION_ROLL_180_YAW_225    = 13,
	ROTATION_ROLL_180_YAW_270    = 14,
	ROTATION_ROLL_180_YAW_315    = 15,
	ROTATION_ROLL_90             = 16,
	ROTATION_ROLL_90_YAW_45      = 17,
	ROTATION_ROLL_90_YAW_90      = 18,
	ROTATION_ROLL_90_YAW_135     = 19,
	ROTATION_ROLL_270            = 20,
	ROTATION_ROLL_270_YAW_45     = 21,
	ROTATION_ROLL_270_YAW_90     = 22,
	ROTATION_ROLL_270_YAW_135    = 23,
	ROTATION_PITCH_90            = 24,
	ROTATION_PITCH_270           = 25,
	ROTATION_PITCH_180_YAW_90    = 26,
	ROTATION_PITCH_180_YAW_270   = 27,
	ROTATION_ROLL_90_PITCH_90    = 28,
	ROTATION_ROLL_180_PITCH_90   = 29,
	ROTATION_ROLL_270_PITCH_90   = 30,
	ROTATION_ROLL_90_PITCH_180   = 31,
	ROTATION_ROLL_270_PITCH_180  = 32,
	ROTATION_ROLL_90_PITCH_270   = 33,
	ROTATION_ROLL_180_PITCH_270  = 34,
	ROTATION_ROLL_270_PITCH_270  = 35,
	ROTATION_ROLL_90_PITCH_180_YAW_90 = 36,
	ROTATION_ROLL_90_YAW_270          = 37,
	ROTATION_ROLL_90_PITCH_68_YAW_293 = 38,
	ROTATION_PITCH_315                = 39,
	ROTATION_ROLL_90_PITCH_315        = 40,

	ROTATION_MAX
};

不过需要注意的一点是，这里的旋转关系有一点复杂。之前已经说了，这个旋转代表的是传感器坐标系到机体坐标系的旋转。那么机体坐标NED已经确定了，那么传感器坐标系呢？这里分为两种情况：

• 传感器出厂坐标系为右手系的

大部分imu和mag本身都是右手系的，这里我收集了一些常用传感器的datasheet定义图：

这一类传感器在硬件设计时，期望将他们厂家定义的坐标系与机体的ENU相重合。这样的话，在传感器启动参数中 R 就为默认值0，也就是ROTATION_NONE 。这里，你肯定会疑惑道？难道不该是和之前说的机体坐标系NED重合么？你说的很对，确实应该如此。但是PX4在相应驱动中做了如下处理:

		// sensor's frame is +x forward, +y left, +z up
		//  flip y & z to publish right handed with z down (x forward, y right, z down)
		gyro.x[i] = gyro_x;
		gyro.y[i] = (gyro_y == INT16_MIN) ? INT16_MAX : -gyro_y;
		gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;

经过这样对 y, z轴的反转，最终传感器的测量值和机体坐标系NED一致了。这样做其实是很合理的，因为大多数imu和mag是布置在飞控的top，而不会把它们翻过来焊接在bottom。

那么当imu或者mag由于布线需要不是那样焊接的呢？那么这时请按上面的处理，把imu的出厂坐标系通过 y, z 翻转变为相似的NED坐标系。这个翻转后的传感器坐标系到机体坐标系的旋转就是我们要输入R。

• 传感器出厂坐标系为左手系的

这听起来很奇怪，居然有传感器坐标系是左手的。但是我们常用的ist8310还真就是这样的：

首先，PX4在驱动中进行了如下处理：

					// sensor's frame is +x forward, +y right, +z up
					z = (z == INT16_MIN) ? INT16_MAX : -z; // flip z

好了，看到这里结合前面的讲解，我想你肯定懂了？没错，就是把它出厂坐标系的z轴进行翻转，然后就得到了一个NED的坐标系。这个坐标系到机体坐标系的旋转就是我们要输入的R。

总结： 基于上述传感器配置的描述，我们就可以自己随意安排传感器在飞控板上的布置了！