接前一篇文章:ICM20948 DMP代码详解(11)
上一回开始解析icm20948_sensor_setup函数的第2段代码也即inv_icm20948_init_matrix函数:
/* Setup accel and gyro mounting matrix and associated angle for current board */
inv_icm20948_init_matrix(&icm_device);
讲到了inv_icm20948_init_matrix中最后调用的函数inv_icm20948_set_chip_to_body_axis_quaternion。
void inv_icm20948_init_matrix(struct inv_icm20948 *s)
{
// initialize chip to body
s->s_quat_chip_to_body[0] = (1L<<30);
s->s_quat_chip_to_body[1] = 0;
s->s_quat_chip_to_body[2] = 0;
s->s_quat_chip_to_body[3] = 0;
//initialize mounting matrix
memset(s->mounting_matrix, 0, sizeof(s->mounting_matrix));
s->mounting_matrix[0] = 1;
s->mounting_matrix[4] = 1;
s->mounting_matrix[8] = 1;
//initialize soft iron matrix
s->soft_iron_matrix[0] = (1L<<30);
s->soft_iron_matrix[4] = (1L<<30);
s->soft_iron_matrix[8] = (1L<<30);
inv_icm20948_set_chip_to_body_axis_quaternion(s, s->mounting_matrix, 0.0);
}
inv_icm20948_set_chip_to_body_axis_quaternion函数在EMD-Core\sources\Invn\Devices\Drivers\ICM20948\Icm20948DataConverter.c中,代码如下:
void inv_icm20948_set_chip_to_body_axis_quaternion(struct inv_icm20948 *s, signed char *accel_gyro_matrix, float angle)
{
int i;
float rot[9];
long qcb[4];
long q_all[4];
long q_adjust[4];
for (i=0; i<9; i++)
rot[i] = (float)accel_gyro_matrix[i];
//convert Chip to Body transformation matrix to quaternion
//inv_icm20948_convert_matrix_to_quat_fxp(rot, qcb);
inv_rotation_to_quaternion(rot, qcb);
//The quaterion generated is the inverse, take the inverse again.
qcb[1] = -qcb[1];
qcb[2] = -qcb[2];
qcb[3] = -qcb[3];
//now rotate by angle, negate angle to rotate other way
q_adjust[0] = (long)((1L<<30) * cosf(-angle*(float)M_PI/180.f/2.f));
q_adjust[1] = 0;
q_adjust[2] = (long)((1L<<30) * sinf(-angle*(float)M_PI/180.f/2.f));
q_adjust[3] = 0;
invn_convert_quat_mult_fxp(q_adjust, qcb, q_all);
inv_icm20948_set_chip_to_body(s, q_all);
}
仍旧一段一段来看。先来看第1段代码。
for (i=0; i<9; i++)
rot[i] = (float)accel_gyro_matrix[i];
这段很好理解,就是依次将accel_gyro_matrix数组的每一个值赋给rot数组对应的项。
rot数组就是函数中定义的局部变量
float rot[9];
accel_gyro_matrix是函数的第2个参数signed char *accel_gyro_matrix,对应的实参为inv_icm20948_init_matrix函数中的s->mounting_matrix([9])。
接下来是第2段代码:
//convert Chip to Body transformation matrix to quaternion
//inv_icm20948_convert_matrix_to_quat_fxp(rot, qcb);
inv_rotation_to_quaternion(rot, qcb);
根据注释,这段代码的意思是将Chip to Body变换矩阵转换为四元数。
inv_rotation_to_quaternion函数在同文件(EMD-Core\sources\Invn\Devices\Drivers\ICM20948\Icm20948DataConverter.c)中,代码如下:
static void inv_rotation_to_quaternion(float Rcb[9], long Qcb_fp[4])
{
float q[4];
inv_icm20948_convert_matrix_to_quat_flt(Rcb, q);
INVN_CONVERT_FLT_TO_FXP(q, Qcb_fp, 4, 30);
}
inv_rotation_to_quaternion函数的两个参数的实参分别对应于inv_icm20948_set_chip_to_body_axis_quaternion函数中的float rot[9]和long qcb[4]。
inv_icm20948_convert_matrix_to_quat_flt函数也在EMD-Core\sources\Invn\Devices\Drivers\ICM20948\Icm20948DataConverter.c中,代码如下:
void inv_icm20948_convert_matrix_to_quat_flt(float *R, float *q)
{
float r11,r12,r13, r21,r22,r23, r31,r32,r33;
r11 = R[0]; //assume matrix is stored row wise first, that is rot[1] is row 1, col 2
r12 = R[1];
r13 = R[2];
r21 = R[3];
r22 = R[4];
r23 = R[5];
r31 = R[6];
r32 = R[7];
r33 = R[8];
q[0] = (1.f + r11 + r22 + r33) / 4.f;
q[1] = (1.f + r11 - r22 - r33) / 4.f;
q[2] = (1.f - r11 + r22 - r33) / 4.f;
q[3] = (1.f - r11 - r22 + r33) / 4.f;
if(q[0] < 0.0f) q[0] = 0.0f;
if(q[1] < 0.0f) q[1] = 0.0f;
if(q[2] < 0.0f) q[2] = 0.0f;
if(q[3] < 0.0f) q[3] = 0.0f;
q[0] = sqrtf(q[0]);
q[1] = sqrtf(q[1]);
q[2] = sqrtf(q[2]);
q[3] = sqrtf(q[3]);
/* Above paragraph could be reduced in :
q[0] =(q[0] < 0.0f) ? q[0] = 0.0f : sqrtf(q[0]);
q[1] =(q[1] < 0.0f) ? q[1] = 0.0f : sqrtf(q[1]);
q[2] =(q[2] < 0.0f) ? q[2] = 0.0f : sqrtf(q[2]);
q[3] =(q[3] < 0.0f) ? q[3] = 0.0f : sqrtf(q[3]);
*/
if(q[0] >= q[1] && q[0] >= q[2] && q[0] >= q[3]) //q[0] is max
{
q[1] = (r23 - r32)/(4.f*q[0]);
q[2] = (r31 - r13)/(4.f*q[0]);
q[3] = (r12 - r21)/(4.f*q[0]);
}
else if(q[1] >= q[0] && q[1] >= q[2] && q[1] >= q[3]) //q[1] is max
{
q[0] = (r23 - r32)/(4.f*q[1]);
q[2] = (r12 + r21)/(4.f*q[1]);
q[3] = (r31 + r13)/(4.f*q[1]);
}
else if(q[2] >= q[0] && q[2] >= q[1] && q[2] >= q[3]) //q[2] is max
{
q[0] = (r31 - r13)/(4.f*q[2]);
q[1] = (r12 + r21)/(4.f*q[2]);
q[3] = (r23 + r32)/(4.f*q[2]);
}
else if(q[3] >= q[0] && q[3] >= q[1] && q[3] >= q[2]) //q[3] is max
{
q[0] = (r12 - r21)/(4.f*q[3]);
q[1] = (r31 + r13)/(4.f*q[3]);
q[2] = (r23 + r32)/(4.f*q[3]);
}
}
看似代码较长较复杂,其实并不难理解。
1)先把R[0]~R[8](即rot[0]~rot[8])由行向量转换为矩阵(r11、r12、……,r33 )。代码片段如下:
float r11,r12,r13, r21,r22,r23, r31,r32,r33;
r11 = R[0]; //assume matrix is stored row wise first, that is rot[1] is row 1, col 2
r12 = R[1];
r13 = R[2];
r21 = R[3];
r22 = R[4];
r23 = R[5];
r31 = R[6];
r32 = R[7];
r33 = R[8];
2)然后依次计算得到q[0]~q[3]的值,并作一定修正。代码片段如下:
q[0] = (1.f + r11 + r22 + r33) / 4.f;
q[1] = (1.f + r11 - r22 - r33) / 4.f;
q[2] = (1.f - r11 + r22 - r33) / 4.f;
q[3] = (1.f - r11 - r22 + r33) / 4.f;
if(q[0] < 0.0f) q[0] = 0.0f;
if(q[1] < 0.0f) q[1] = 0.0f;
if(q[2] < 0.0f) q[2] = 0.0f;
if(q[3] < 0.0f) q[3] = 0.0f;
3)依次对于q[0]~q[3]做开方运算。代码片段如下:
q[0] = sqrtf(q[0]);
q[1] = sqrtf(q[1]);
q[2] = sqrtf(q[2]);
q[3] = sqrtf(q[3]);
4)根据最大值具体出自于q[0]/q[1]/q[2]/q[3],依次做不同运算。代码片段如下:
if(q[0] >= q[1] && q[0] >= q[2] && q[0] >= q[3]) //q[0] is max
{
q[1] = (r23 - r32)/(4.f*q[0]);
q[2] = (r31 - r13)/(4.f*q[0]);
q[3] = (r12 - r21)/(4.f*q[0]);
}
else if(q[1] >= q[0] && q[1] >= q[2] && q[1] >= q[3]) //q[1] is max
{
q[0] = (r23 - r32)/(4.f*q[1]);
q[2] = (r12 + r21)/(4.f*q[1]);
q[3] = (r31 + r13)/(4.f*q[1]);
}
else if(q[2] >= q[0] && q[2] >= q[1] && q[2] >= q[3]) //q[2] is max
{
q[0] = (r31 - r13)/(4.f*q[2]);
q[1] = (r12 + r21)/(4.f*q[2]);
q[3] = (r23 + r32)/(4.f*q[2]);
}
else if(q[3] >= q[0] && q[3] >= q[1] && q[3] >= q[2]) //q[3] is max
{
q[0] = (r12 - r21)/(4.f*q[3]);
q[1] = (r31 + r13)/(4.f*q[3]);
q[2] = (r23 + r32)/(4.f*q[3]);
}
inv_icm20948_convert_matrix_to_quat_flt函数代码本身很好理解,但是为什么这样计算?公式从何而来?参考以下文章:
四元数的基本定义和性质12
四元数是一种扩展的复数,由一个实数部分和三个虚数部分组成。每个四元数可以表示为a + bi + cj + dk,其中a、b、c、d是实数,i、j、k是虚数单位,满足i² = j² = k² = -1。
四元数与旋转矩阵的关系
旋转矩阵和四元数之间存在着一种对应关系,可以通过这种关系将旋转矩阵转换成四元数。具体来说,一个旋转矩阵可以表示为一个3x3的矩阵,而一个四元数可以表示为一个包含四个分量的向量。
具体的转换公式和方法
从旋转矩阵转换到四元数的公式如下:
- 首先计算w的值:
w = sqrt(1 + r00 + r11 + r22) / 2
其中r00、r11、r22是旋转矩阵的对角线元素。- 然后计算x、y、z的值:
x = (r21 - r12) / (4w)
y = (r02 - r20) / (4w)
z = (r10 - r01) / (4w)
需要注意的是,由于四元数具有多个等价的表示方式,上述公式中的分母部分使用了4w来确保单位化。实际应用和注意事项
在实际应用中,旋转矩阵转四元数的转换在计算机图形学、机器人学等领域有着广泛的应用。例如,在计算机图形学中,四元数常用于表示三维空间的旋转,以避免万向锁问题。需要注意的是,由于四元数有多种表示方式,转换过程中需要注意选择合适的表示方式,并确保转换后的四元数是单位化的。
回到inv_rotation_to_quaternion函数中。在调用inv_icm20948_convert_matrix_to_quat_flt函数之后,接下来调用INVN_CONVERT_FLT_TO_FXP函数,代码片段如下:
static void inv_rotation_to_quaternion(float Rcb[9], long Qcb_fp[4])
{
float q[4];
inv_icm20948_convert_matrix_to_quat_flt(Rcb, q);
INVN_CONVERT_FLT_TO_FXP(q, Qcb_fp, 4, 30);
}
INVN_CONVERT_FLT_TO_FXP实际上是一个宏,在EMD-Core\sources\Invn\Devices\Drivers\ICM20948\Icm20948DataConverter.h中,定义如下:
//! \def INVN_FLT_TO_FXP
//! Convert the \a value from float to QN value. \ingroup invn_macro
#define INVN_FLT_TO_FXP(value, shift) ( (int32_t) ((float)(value)*(1ULL << (shift)) + ( (value>=0)-0.5f )) )
//! Macro to convert float values from an address into QN values, and copy them to another address. \ingroup invn_macro
#define INVN_CONVERT_FLT_TO_FXP(fltptr, fixptr, length, shift) { int i; for(i=0; i<(length); ++i) (fixptr)[i] = INVN_FLT_TO_FXP((fltptr)[i], shift); }
将以上代码转换成容易理解的函数方式,如下:
//! \def INVN_FLT_TO_FXP
//! Convert the \a value from float to QN value. \ingroup invn_macro
#define INVN_FLT_TO_FXP(value, shift)
{
(int32_t) ((float)(value)*(1ULL << (shift)) + ( (value>=0)-0.5f ))
}
//! Macro to convert float values from an address into QN values, and copy them to another address. \ingroup invn_macro
#define INVN_CONVERT_FLT_TO_FXP(fltptr, fixptr, length, shift)
{
int i;
for (i=0; i<(length); ++i)
(fixptr)[i] = INVN_FLT_TO_FXP((fltptr)[i], shift);
}
这一部分代码应该是浮点数转定点数,参考:定点转浮点的Qn定义及计算公式方法。
参考:https://www.zhihu.com/question/50534472/answer/2144622129。
这里,shift就是公式中的n。
最终,将计算得到的Qcb_fp形参的值返给inv_icm20948_set_chip_to_body_axis_quaternion函数的调用处,也就是生成了qcb的值。
long qcb[4];
//convert Chip to Body transformation matrix to quaternion
//inv_icm20948_convert_matrix_to_quat_fxp(rot, qcb);
inv_rotation_to_quaternion(rot, qcb);
综合以上内容,inv_rotation_to_quaternion函数的功能是:
1)先将旋转矩阵(数组rot[9])转换成浮点数形式的四元数;
2)再将浮点数形式的四元数转换为整数形式的四元数。
static void inv_rotation_to_quaternion(float Rcb[9], long Qcb_fp[4])
{
float q[4];
inv_icm20948_convert_matrix_to_quat_flt(Rcb, q);
INVN_CONVERT_FLT_TO_FXP(q, Qcb_fp, 4, 30);
}
回到inv_icm20948_set_chip_to_body_axis_quaternion函数中。对于该函数接下来代码的解析,请看下回。