|
|
发表于 2020-7-14 10:26:14
4318 浏览 2 回复
求助帖：加速度->推力->姿态角转化
求助：有三个问题期望大佬们的回答～
1.根据github中的讨论和px4_command中的代码，~setpoint_raw/local (mavros_msgs/PositionTarget)---Local position, velocity and acceleration setpoint. 接收的是否为归一化的推力？
2.在px4_command中，accelToThrust是根据加速度直接乘质量得到X，Y，Z轴方向的推力，thrustToThrottle是如何根据电机模型得到归一化的推力呢？如何得到电机的推力-油门模型？
3.在px4_command中，ThrottleToAttitude功能是如何实现的？能否给出数学证明公式。
附上ThrottleToAttitude代码：
| //Thrust to Attitude | [tr] [/tr]
| //Input: desired thrust (desired throttle [0,1]) and yaw_sp(rad) | [tr] [/tr]
| //Output: desired attitude (quaternion) | [tr] [/tr]
| prometheus_msgs::AttitudeReference ThrottleToAttitude(const Eigen::Vector3d& thr_sp, float yaw_sp) | [tr] [/tr]
| prometheus_msgs::AttitudeReference _AttitudeReference; | [tr] [/tr]
| Eigen::Vector3d att_sp; | [tr] [/tr]
| att_sp[2] = yaw_sp; | [tr] [/tr]
| // desired body_z axis = -normalize(thrust_vector) | [tr] [/tr]
| Eigen::Vector3d body_x, body_y, body_z; | [tr] [/tr]
| double thr_sp_length = thr_sp.norm(); | [tr] [/tr]
| //cout << "thr_sp_length : "<< thr_sp_length << endl; | [tr] [/tr]
| if (thr_sp_length > 0.00001f) { | [tr] [/tr]
| body_z = thr_sp.normalized(); | [tr] [/tr]
| // no thrust, set Z axis to safe value | [tr] [/tr]
| body_z = Eigen::Vector3d(0.0f, 0.0f, 1.0f); | [tr] [/tr]
| // vector of desired yaw direction in XY plane, rotated by PI/2 | [tr] [/tr]
| Eigen::Vector3d y_C = Eigen::Vector3d(-sinf(yaw_sp),cosf(yaw_sp),0.0); | [tr] [/tr]
| if (fabsf(body_z(2)) > 0.000001f) { | [tr] [/tr]
| // desired body_x axis, orthogonal to body_z | [tr] [/tr]
| body_x = y_C.cross(body_z); | [tr] [/tr]
| // keep nose to front while inverted upside down | [tr] [/tr]
| if (body_z(2) < 0.0f) { | [tr] [/tr]
| body_x = -body_x; | [tr] [/tr]
| body_x.normalize(); | [tr] [/tr]
| // desired thrust is in XY plane, set X downside to construct correct matrix, | [tr] [/tr]
| // but yaw component will not be used actually | [tr] [/tr]
| body_x = Eigen::Vector3d(0.0f, 0.0f, 0.0f); | [tr] [/tr]
| body_x(2) = 1.0f; | [tr] [/tr]
| // desired body_y axis | [tr] [/tr]
| body_y = body_z.cross(body_x); | [tr] [/tr]
| Eigen::Matrix3d R_sp; | [tr] [/tr]
| // fill rotation matrix | [tr] [/tr]
| for (int i = 0; i < 3; i++) { | [tr] [/tr]
| R_sp(i, 0) = body_x(i); | [tr] [/tr]
| R_sp(i, 1) = body_y(i); | [tr] [/tr]
| R_sp(i, 2) = body_z(i); | [tr] [/tr]
| Eigen::Quaterniond q_sp(R_sp); | [tr] [/tr]
| rotation_to_euler(R_sp, att_sp); | [tr] [/tr]
| //cout << "Desired euler [R P Y]: "<< att_sp[0]* 180/M_PI <<" [deg] " << att_sp[1]* 180/M_PI <<" [deg] "<< att_sp[2]* 180/M_PI <<" [deg] "<< endl; | [tr] [/tr]
| //cout << "Desired Thrust: "<< thr_sp_length<< endl; | [tr] [/tr]
| // cout << "q_sp [x y z w]: "<< q_sp.x() <<" [ ] " << q_sp.y() <<" [ ] "<<q_sp.z() <<" [ ] "<<q_sp.w() <<" [ ] "<<endl; | [tr] [/tr]
| // cout << "R_sp : "<< R_sp(0, 0) <<" " << R_sp(0, 1) <<" "<< R_sp(0, 2) << endl; | [tr] [/tr]
| // cout << " : "<< R_sp(1, 0) <<" " << R_sp(1, 1) <<" "<< R_sp(1, 2) << endl; | [tr] [/tr]
| // cout << " : "<< R_sp(2, 0) <<" " << R_sp(2, 1) <<" "<< R_sp(2, 2) << endl; | [tr] [/tr]
| _AttitudeReference.throttle_sp[0] = thr_sp[0]; | [tr] [/tr]
| _AttitudeReference.throttle_sp[1] = thr_sp[1]; | [tr] [/tr]
| _AttitudeReference.throttle_sp[2] = thr_sp[2]; | [tr] [/tr]
| _AttitudeReference.desired_throttle = thr_sp_length; | [tr] [/tr]
| _AttitudeReference.desired_att_q.w = q_sp.w(); | [tr] [/tr]
| _AttitudeReference.desired_att_q.x = q_sp.x(); | [tr] [/tr]
| _AttitudeReference.desired_att_q.y = q_sp.y(); | [tr] [/tr]
| _AttitudeReference.desired_att_q.z = q_sp.z(); | [tr] [/tr]
| _AttitudeReference.desired_attitude[0] = att_sp[0]; | [tr] [/tr]
| _AttitudeReference.desired_attitude[1] = att_sp[1]; | [tr] [/tr]
| _AttitudeReference.desired_attitude[2] = att_sp[2]; | [tr] [/tr]
| return _AttitudeReference; | [tr] [/tr]
| //Throttle to Attitude | [tr] [/tr]
xlxzzzrr已获得悬赏 2 阿木币最佳答案
代码复制格式有些问题,可以查看：https://github.com/amov-lab/Prometheus/blob/master/Modules/control/include/prometheus_control_utils.h
|
扫一扫浏览分享
|
|
|
|
|
|
|
|
|
|
|
楼主|
发表于 2020-7-14 10:27:44
|
|
|
|
|
|
|
|
|
|
|
|
最近我们要开prometheus及PX4_command的课程，有直播，欢迎来找老师提问哈~~~ |
|
|
|
|
|
|
|
|
|
|
