【PX4二次开发】PX4 offboard 模式发布 推力+姿态

写在前面

由于项目需要，最近想发布推力+姿态指令给PX4控制器，然后由PX4跟踪给定的控制指令。我注意到，所需的推力是一个归一化的值，所以我想当然的将所需推力与最大推力的比值作为归一化推力发布出去。不过，在测试时候发现，发布的这个归一化推力根本无法让无人机起飞，于是我就这一问题展开了资料的查询和分析……

PX4 offboard 模式发布推力+姿态

我们先进行一个简单的小测试让无人机实现悬停，此时推力为无人机的重力，姿态指令为0。
参考官方mavros写的python源码如下：

px4_node_test.py

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy
from geometry_msgs.msg import PoseStamped, TwistStamped, Twist, Pose, Quaternion, Point
from mavros_msgs.msg import State, AttitudeTarget
from sensor_msgs.msg import Imu
from std_msgs.msg import Bool
from mavros_msgs.srv import CommandBool, CommandBoolRequest, SetMode, SetModeRequest
import tf
import math
import numpy as np

PI = 3.14159
THRUST_FULL = 31

# sdf参数：
m = 1.5
thrust = m * 9.8
motor_constant = 5.84e-06
min_motor_vel = 0
max_motor_vel = 1100
THRUST_FULL = 4 * motor_constant * max_motor_vel * max_motor_vel

current_state = State()

# Callback functions
def state_cb(msg):
    global current_state
    current_state = msg

# Function to convert Euler angles to quaternion
def euler_to_quat(roll, pitch, yaw):
    q = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
    quat = Quaternion()
    quat.x = q[0]
    quat.y = q[1]
    quat.z = q[2]
    quat.w = q[3]
    return quat

# Function to scale thrust
def thrust_scale(thrust):
    print("THRUST_FULL:",THRUST_FULL)
    if thrust >= THRUST_FULL:
        thrust = THRUST_FULL

    # 每个电机推力的归一化
    # THR_MDL_FAC = 1 时使用！
    thrust_normalized = thrust/THRUST_FULL
    print("thrust_normalized:",thrust_normalized)

    # 每个电机转速的归一化
    # THR_MDL_FAC = 0 时使用！
    # thrust_normalized = (np.sqrt(thrust /4 /motor_constant)-min_motor_vel)/(max_motor_vel-min_motor_vel)
    # print("thrust_normalized:",thrust_normalized)

    return thrust_normalized


def main():
    rospy.init_node("offb_node_py")

    state_sub = rospy.Subscriber("mavros/state", State, callback=state_cb)
    imu_sub = rospy.Subscriber("mavros/imu/data", Imu)
    pos_sub = rospy.Subscriber("mavros/local_position/pose", PoseStamped)
    vel_sub = rospy.Subscriber("mavros/global_position/raw/gps_vel", TwistStamped)
    local_pos_pub = rospy.Publisher("mavros/setpoint_position/local", PoseStamped, queue_size=10)
    setvel_pub = rospy.Publisher("mavros/setpoint_velocity/cmd_vel_unstamped", Twist, queue_size=10)
    start_pub = rospy.Publisher("start_time", Bool, queue_size=10)
    refv_sub = rospy.Subscriber("reference", Twist)
    refp_sub = rospy.Subscriber("reference2", Pose)

    rospy.wait_for_service("/mavros/cmd/arming")
    arming_client = rospy.ServiceProxy("mavros/cmd/arming", CommandBool)
    rospy.wait_for_service("/mavros/set_mode")
    set_mode_client = rospy.ServiceProxy("mavros/set_mode", SetMode)

    raw_pub_att = rospy.Publisher('mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=10)

    rate = rospy.Rate(20.0)
    while not rospy.is_shutdown() and not current_state.connected:
        rospy.loginfo("Waiting for FCU connection...")
        rate.sleep()

    offb_set_mode = SetModeRequest()
    offb_set_mode.custom_mode = "OFFBOARD"

    arm_cmd = CommandBoolRequest()
    arm_cmd.value = True

    raw_att = AttitudeTarget()
    raw_att.type_mask = 7  # Ignore body rate
    raw_att.thrust = thrust_scale(thrust)  # Initial thrust
    raw_att.orientation = euler_to_quat(0, 0, 0)  # Level orientation

    last_request = rospy.Time.now()

    while not rospy.is_shutdown():
        if current_state.mode != "OFFBOARD" and (rospy.Time.now() - last_request > rospy.Duration(5.0)):
            set_mode_resp = set_mode_client(offb_set_mode)
            if set_mode_resp.mode_sent:
                rospy.loginfo("Offboard enabled")
            last_request = rospy.Time.now()
        else:
            if not current_state.armed and (rospy.Time.now() - last_request > rospy.Duration(5.0)):
                arm_resp = arming_client(arm_cmd)
                if arm_resp.success:
                    rospy.loginfo("Vehicle armed")
                last_request = rospy.Time.now()

        ned_euler = Point()
        ned_euler.x = 0
        ned_euler.y = 0 # 10 degrees
        ned_euler.z = 0       # 30 degrees
        enu_euler = ned_euler  # Assuming ENU is same as NED for simplicity
        q = euler_to_quat(enu_euler.x, enu_euler.y, enu_euler.z)
        thr = thrust_scale(thrust)

        raw_att.header.stamp = rospy.Time.now()
        raw_att.thrust = thr
        raw_att.orientation = q

        raw_pub_att.publish(raw_att)

        rate.sleep()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

代码中的推力归一化是根据无人机的重量与无人机最大推力的比值得到的，无人机重量$m=1.5kg$，最大推力 THRUST_FULL= 4 * motor_constant * max_motor_vel * max_motor_vel，其中$moto{r}_{c}onstant=5.84e-06$，$ma{x}_{m}oto{r}_{v}el=1100rad/s$。这些参数参见下一部分的SDF参数解释。

仿真过程：

make px4_sitl gazebo_iris

roslaunch mavros px4.launch fcu_url:="udp://:14540@127.0.0.1:14557"

python3 px4_node_test.py

推力归一化

运行程序之后，丸辣，无人机无法起飞！！！
这是为什么呢？带着疑惑，我们来分析一下，PX4的基本工作原理。首先外环的位置控制器根据期望轨迹和实际状态通过串级PID控制器生成推力+姿态的控制指令，该指令作为姿态内环的期望值被执行，生成推力+扭矩的控制量。推力+扭矩控制量经过控制量分配得到每个电机的转速，在理想情况下，PWM与转速呈线性对应关系，可以将转速归一化值约等于PWM归一化值，再将PWM信号发布给电机执行。因此，我们给定的推力这个归一化的指令其实最后对应的是PWM的归一化值。

PWM与推力的关系并不是线性的且受到电压等影响，经过静态推力测试可以得到 F-PWM 是一个二次的对应关系，这个关系在PX4中可以通过调节THR_MDL_FAC 进行修改（参见第三部分THR_MDL_FAC 参数的设置与解释） 。

此时如果你打开QGC地面站查看 THR_MDL_FAC 参数，应该默认是0。
将 THR_MDL_FAC 设置为1，则无人机可以起飞（此时无人机不能够悬停，因为位置没有闭环）。

SDF 文件参数解释

在PX4的仿真环境中，SDF（Simulation Description Format）文件的作用是定义无人机模型及其各个组件的详细参数和属性。这些文件通常用于Gazebo仿真环境中，以便准确地模拟无人机的物理行为和动力学特性。文件位置如下：

PX4-Autopilot/Tools/sitl_gazebo/models/iris/iris.sdf

在文件中我们可以发现一些有用的参数：

<maxRotVelocity>1100</maxRotVelocity>
<motorConstant>5.84e-06</motorConstant>
<momentConstant>0.06</momentConstant>

这些参数的意义如下：

maxRotVelocity：电机的最大转速，以每秒的弧度数（rad/s）表示。这个参数限制了电机的最大转速。

motorConstant：电机常数，表示电机产生的力与转速的比例关系。这个值通常通过实验测定，对于每种电机和螺旋桨组合都是特定的。单位是牛顿·米·秒²（N·m·s²）。

momentConstant：转矩常数，表示电机产生的力矩与转速的比例关系。这个值也通常通过实验测定。单位是牛顿·米·秒²（N·m·s²）。

根据推力与转速的关系$F=motorConstant\ast maxRotVelocity\ast maxRotVelocity$，可以得到电机能产生的最大推力。

THR_MDL_FAC 参数的设置与解释

THR_MDL_FAC (FLOAT)	

Thrust to motor control signal model parameter

Comment: Parameter used to model the nonlinear relationship between motor control signal (e.g. PWM) and static thrust. The model is: rel_thrust = factor * rel_signal^2 + (1-factor) * rel_signal, where rel_thrust is the normalized thrust between 0 and 1, and rel_signal is the relative motor control signal between 0 and 1.

这个参数定义了PWM信号与电机推力的模型rel_thrust = factor * rel_signal^2 + (1-factor) * rel_signal，是用作PX4固件中计算PWM信号与电机推力的对应关系的。

当factor =0时，rel_thrust = rel_signal；

当factor =1时，rel_thrust = rel_signal^2；

这就可以解释当THR_MDL_FAC =0时，我们使用推力归一化值无法起飞的原因：

举个栗子，我们想要发布一个推力，大小为总推力的0.5倍，于是我们发布的推力归一化系数为0.5。

此时，THR_MDL_FAC =0时，rel_thrust = rel_signal，rel_signal为PWM占空比归一化值等于转速归一化值，在这个条件下PX4经过推力分配之后产生的转速归一值为0.5（推力与转速、PWM不再是平方关系，而是定义的线性关系）。而gazebo仿真环境中，无人机的推力和转速模型仍然是平方关系，即F=k*w^2，所以，根据转速产生的推力与最大推力的比值为$(0.5{)}^{1/2}$，也就是我们发布的推力归一化系数的开方。同理，我们想发布一个悬停推力时候，最后得到的是推力开方的值，因此不能起飞。

而当 THR_MDL_FAC =1时，rel_thrust = rel_signal^ 2，在这个条件下PX4经过推力分配之后产生的转速归一值为0.25（推力与转速、PWM是平方关系）。gazebo仿真环境中，无人机的推力和转速模型是平方关系，即F=k*w^2。所以，根据转速产生的推力与最大推力的比值为$(0.25{)}^{1/2}=0.5$，也就是我们发布的推力归一化系数，因此可以起飞。

写在最后

其实说到底，就是仿真器和控制器二者关于PWM和推力对应关系的不同导致的，这在实物实验中也会遇到，我们可以通过实验测量二者的关系，并在固件参数中进行修改，使得控制器中的PWM和推力关系贴近现实。

这是我自己总结的一些观点，请大家批评指正！

参考

https://docs.px4.io/v1.12/en/advanced_config/parameter_reference.html#THR_MDL_FAC

https://discuss.px4.io/t/about-normalized-torque-and-thrust-in-offboard-control-mode-ros2-sitl/36265

https://discuss.px4.io/t/what-is-the-maximum-thrust-in-newton-of-the-iris-model/15839/2

多旋翼PID调参（手动/高级） | PX4 Guide (main)