voidbodyzToAttitude(Vector3f body_z,constfloat yaw_sp, vehicle_attitude_setpoint_s &att_sp){// zero vector, no direction, set safe level valueif(body_z.norm_squared()< FLT_EPSILON){body_z(2)=1.f;}
body_z.normalize();// vector of desired yaw direction in XY plane, rotated by PI/2
Vector3f y_C(-sinf(yaw_sp),cosf(yaw_sp),0.0f);// desired body_x axis, orthogonal to body_z
Vector3f body_x = y_C % body_z;// keep nose to front while inverted upside downif(body_z(2)<0.0f){
body_x =-body_x;}if(fabsf(body_z(2))<0.000001f){// desired thrust is in XY plane, set X downside to construct correct matrix,// but yaw component will not be used actually
body_x.zero();body_x(2)=1.0f;}
body_x.normalize();// desired body_y axis
Vector3f body_y = body_z % body_x;
Dcmf R_sp;// fill rotation matrixfor(int i =0; i <3; i++){R_sp(i,0)=body_x(i);R_sp(i,1)=body_y(i);R_sp(i,2)=body_z(i);}// copy quaternion setpoint to attitude setpoint topic
Quatf q_sp = R_sp;
q_sp.copyTo(att_sp.q_d);// calculate euler angles, for logging only, must not be used for control
Eulerf euler = R_sp;
att_sp.roll_body =euler(0);
att_sp.pitch_body =euler(1);
att_sp.yaw_body =euler(2);}
1.1. 求期望体轴X轴向量
// vector of desired yaw direction in XY plane, rotated by PI/2
Vector3f y_C(-sinf(yaw_sp),cosf(yaw_sp),0.0f);// desired body_x axis, orthogonal to body_z
Vector3f body_x = y_C % body_z;
Dcmf R_sp;// fill rotation matrixfor(int i =0; i <3; i++){R_sp(i,0)=body_x(i);R_sp(i,1)=body_y(i);R_sp(i,2)=body_z(i);}
期望姿态矩阵根据求得的期望体轴向量借助旋转矩阵作用原理求得。
R
e
b
⋅
v
e
⃗
=
v
b
⃗
R_e^b \cdot \vec{v_e} = \vec{v_b}
Reb⋅ve=vb
其中
R
e
b
R_e^b
Reb为NED到体轴的旋转矩阵，
v
e
⃗
\vec{v_e}
ve为NED坐标系下的向量，
v
b
⃗
\vec{v_b}
vb为经过旋转后的体轴坐标系下的向量。
R
e
b
R_e^b
Reb向量如下:
R
e
b
=
[
a
00
a
01
a
02
a
10
a
11
a
12
a
20
a
21
a
22
]
R_e^b = \left[ \begin{matrix} a_{00}&a_{01}&a_{02}\\ a_{10}&a_{11}&a_{12}\\ a_{20}&a_{21}&a_{22}\\ \end{matrix} \right]
Reb=⎣⎡a00a10a20a01a11a21a02a12a22⎦⎤
取
v
e
⃗
\vec{v_e}
ve为NED坐标系下
x
x
x轴的单位向量
x
e
i
⃗
=
[
1
0
0
]
′
\vec{x_{ei}} = \left[\begin{matrix}1&0&0\end{matrix}\right]'
xei=[100]′，经过旋转后成为体轴坐标系下的向量
x
b
i
⃗
\vec{x_{bi}}
xbi，即body_x，即:
R
e
b
⋅
x
e
i
⃗
=
[
a
00
a
01
a
02
a
10
a
11
a
12
a
20
a
21
a
22
]
⋅
[
1
0
0
]
=
[
a
00
a
10
a
20
]
=
[
b
o
d
y
_
x
(
0
)
b
o
d
y
_
x
(
1
)
b
o
d
y
_
x
(
2
)
]
R_e^b \cdot \vec{x_{ei}} = \left[ \begin{matrix} a_{00}&a_{01}&a_{02}\\ a_{10}&a_{11}&a_{12}\\ a_{20}&a_{21}&a_{22}\\ \end{matrix} \right] \cdot \left[\begin{matrix}1\\0\\0\end{matrix}\right]=\left[\begin{matrix}a_{00}\\a_{10}\\a_{20}\end{matrix}\right]=\left[\begin{matrix}body\_x(0)\\body\_x(1)\\body\_x(2)\end{matrix}\right]
Reb⋅xei=⎣⎡a00a10a20a01a11a21a02a12a22⎦⎤⋅⎣⎡100⎦⎤=⎣⎡a00a10a20⎦⎤=⎣⎡body_x(0)body_x(1)body_x(2)⎦⎤ 据此可根据body_x求得
R
e
b
R_e^b
Reb中对应位置变量，同理可采用同样方法通过body_y和body_z求得
R
e
b
R_e^b
Reb其他位置变量，据此可得出期望姿态旋转矩阵
R
e
b
R_e^b
Reb。
1.4. 求期望姿态角
// calculate euler angles, for logging only, must not be used for control
Eulerf euler = R_sp;
att_sp.roll_body =euler(0);
att_sp.pitch_body =euler(1);
att_sp.yaw_body =euler(2);
