PX4水下机器人参数配置

wacwl

于 2024-07-02 02:55:34 发布

阅读量21

点赞数

文章标签：机器人

一、Px4版本1.14.1机型文件
PX4Autopilotmain\ROMFS\px4fmu_common\init.d\airframes路径下

这个脚本设置了BlueROV2（重型配置）的各种参数和初始化步骤，包括电池设置、通信设置、机架和旋翼配置以及PWM输出功能的映射。通过这些设置，可以确保机器人在启动时加载正确的配置，并按照预期的方式运行

加载默认的UUV配置

. ${R}etc/init.d/rc.uuv_defaults

设置参数

param

set-default

CBRK_IO_SAFETY

22027

MAV_1_CONFIG

102

BAT1_A_PER_V

37.8798

BAT1_CAPACITY

18000

BAT1_V_DIV

BAT1_N_CELLS

BAT_V_OFFS_CURR

0.33

机架和旋翼配置

param

set-default

CA_AIRFRAME

CA_ROTOR_COUNT

CA_R_REV

255

旋翼0参数

param

set-default

CA_ROTOR0_AX

CA_ROTOR0_AY

-1

CA_ROTOR0_AZ

CA_ROTOR0_KM

CA_ROTOR0_PX

0.5

CA_ROTOR0_PY

0.3

CA_ROTOR0_PZ

0.2

旋翼1参数

param

set-default

CA_ROTOR1_AX

CA_ROTOR1_AY

CA_ROTOR1_AZ

CA_ROTOR1_KM

CA_ROTOR1_PX

0.5

CA_ROTOR1_PY

-0.3

CA_ROTOR1_PZ

0.2

旋翼2参数

param

set-default

CA_ROTOR2_AX

CA_ROTOR2_AY

CA_ROTOR2_AZ

CA_ROTOR2_KM

CA_ROTOR2_PX

-0.5

CA_ROTOR2_PY

0.3

CA_ROTOR2_PZ

0.2

旋翼3参数

param

set-default

CA_ROTOR3_AX

CA_ROTOR3_AY

-1

CA_ROTOR3_AZ

CA_ROTOR3_KM

CA_ROTOR3_PX

-0.5

CA_ROTOR3_PY

-0.3

CA_ROTOR3_PZ

0.2

旋翼4参数

param

set-default

CA_ROTOR4_AZ

-1

CA_ROTOR4_KM

CA_ROTOR4_PX

0.5

CA_ROTOR4_PY

0.5

旋翼5参数

param

set-default

CA_ROTOR5_AZ

CA_ROTOR5_KM

CA_ROTOR5_PX

0.5

CA_ROTOR5_PY

-0.5

旋翼6参数

param

set-default

CA_ROTOR6_AZ

CA_ROTOR6_KM

CA_ROTOR6_PX

-0.5

CA_ROTOR6_PY

0.5

旋翼7参数

param

set-default

CA_ROTOR7_KM

CA_ROTOR7_PX

-0.5

CA_ROTOR7_PY

-0.5

PWM功能映射

param

set-default

PWM_MAIN_FUNC1

101

PWM_MAIN_FUNC2

102

PWM_MAIN_FUNC3

103

PWM_MAIN_FUNC4

104

PWM_MAIN_FUNC5

105

PWM_MAIN_FUNC6

106

PWM_MAIN_FUNC7

107

PWM_MAIN_FUNC8

108

二、Rcs启动脚本
启动脚本是一个神奇的东西，它能够识别出你对应的飞机类型，加载对应的混控器，选择对应的姿态、位置估计程序以及控制程序，初始化你需要的驱动程序。
启动脚本实现了挂载SD卡，设置存储好的飞行控制初始参数（可以通过地面站修改），启动所有外设传感器，启动与地面站通行的Mavlink服务，以及机型的选择后对应启动的控制服务（以uuv为例，启动了ekf2，uuv_att_control，uuv_pos_control等服务），打开navigator服务。
图片
启动机架代码

Configure vehicle type specific parameters.

# Note: rc.vehicle_setup is the entry point for all vehicle type specific setup.

    . ${R}etc/init.d/rc.vehicle_setup

进入rc.vehicle_setup 后，可以看到有一个机型判断

UUV setup

if [ $VEHICLE_TYPE = uuv ]
then
# Start standard vtol apps.

    . ${R}etc/init.d/rc.uuv_appsfi

然后进入rc.uuv.app

Start Control Allocator

Start UUV Attitude Controller.

Start UUV Land Detector.

land_detector start rover
可以看到启动控制分配器、UUV姿态控制器和UUV着陆检测器的命令。
三、然后我们进入到uuv_att_control.h

先来看它的头文件，uuv_att_control.h定义了一个名为 UUVAttitudeControl的类，它继承了ModuleBase、ModuleParams 和 px4::WorkItem。
公有成员函数
构造函数和析构函数
UUVAttitudeControl(); ~UUVAttitudeControl(); );
任务启动

static int task_spawn(int argc, char *argv[]);
定义命令

static int custom_command(int argc, char *argv[]);
打印用法

static int print_usage(const char *reason = nullptr);
初始化函数

bool init();
私有成员函数
发布扭矩设定点

发布推理设定点

运行函数（主要控制逻辑）

参数更新

姿态控制

void control_attitude_geo(const vehicle_attitude_s &attitude, const vehicle_attitude_setpoint_s &attitude_setpoint, const vehicle_angular_velocity_s &angular_velocity, const vehicle_rates_setpoint_

四、然后我们进入到uuv_att_control.cpp
1、函数constrain_actuator_commands
用于约束并设置姿态控制和推力控制的命令，以确保这些命令在合理的范围内。具体地说，它处理滚转（roll）、俯仰（pitch）、偏航（yaw）以及在 x、y、z 方向上的推力（thrust）的控制命令。以其中一个举例：

检查滚转命令（roll_u）

if (PX4_ISFINITE(roll_u)) {
 roll_u = math::constrain(roll_u, -1.0f, 1.0f);4. _vehicle_torque_setpoint.xyz[0] = roll_u;} else {
            _vehicle_torque_setpoint.xyz[0] = 0.0f;
    }

首先检查roll_u是否为有限值（不是无穷大或NaN）
如果是有限值，则将roll_u约束在 [-1.0, 1.0] 范围内，并设置为车辆的滚转扭矩设定点。
如果不是有限值，则将滚转扭矩设定点设置为 0。
2、control_attitude_geo函数，使用比例和微分控制器调节水下机器人的姿态。
通过计算姿态误差和角速度误差，生成控制力矩，并结合推力设定值，生成最终的控制命令。通过 constrain_actuator_commands 函数，将这些命令约束在合理范围内，以确保机器人的稳定性和安全性。

1）从四元数提取姿态和设定值
Eulerf euler_angles(matrix::Quatf(attitude.q));

float roll_body = attitude_setpoint.roll_body;
    float pitch_body = attitude_setpoint.pitch_body;
    float yaw_body = attitude_setpoint.yaw_body;

    float roll_rate_desired = rates_setpoint.roll;
    float pitch_rate_desired = rates_setpoint.pitch;
    float yaw_rate_desired = rates_setpoint.yaw;

将姿态四元数转换为欧拉角。
提取设定的姿态（滚转、俯仰、偏航）和角速度。
2）计算旋转矩阵

/* get attitude setpoint rotational matrix */
 Dcmf rot_des = Eulerf(roll_body, pitch_body, yaw_body);/* get current rotation matrix from control state quaternions */
    Quatf q_att(attitude.q);
    Matrix3f rot_att =  matrix::Dcm<float>(q_att);;

将设定的欧拉角转换成旋转矩阵rot_des。
将当前姿态四元数转换为旋转矩阵rot_att。
3）计算姿态误差

/* Compute matrix: attitude error */
 Matrix3f e_R = (rot_des.transpose() * rot_att - rot_att.transpose() * rot_des) * 0.5;/* vee-map the error to get a vector instead of matrix e_R */
    e_R_vec(0) = e_R(2, 1);  /**< Roll  */
    e_R_vec(1) = e_R(0, 2);  /**< Pitch */
    e_R_vec(2) = e_R(1, 0);  /**< Yaw   */

e_R(2, 1)表示矩阵e_R的第3行第2列元素。
计算姿态误差矩阵e_R该矩阵表示当前姿态与期望姿态之间的误差。
使用vee-map将误差矩阵转换为误差向量e_R_vec分别对应滚转、俯仰和偏航的误差。
4）计算角速度误差并应用PD控制器
/< P-Control */
torques(0) = - e_R_vec(0) * _param_roll_p.get(); /< Roll */
torques(1) = - e_R_vec(1) * _param_pitch_p.get(); /< Pitch */
torques(2) = - e_R_vec(2) * _param_yaw_p.get(); /< Yaw */

/**< PD-Control */
    torques(0) = torques(0) - omega(0) * _param_roll_d.get();  /**< Roll  */
    torques(1) = torques(1) - omega(1) * _param_pitch_d.get(); /**< Pitch */
    torques(2) = torques(2) - omega(2) * _param_yaw_d.get();   /**< Yaw   */

    float roll_u = torques(0);
    float pitch_u = torques(1);
    float yaw_u = torques(2);

    // take thrust as
    float thrust_x = attitude_setpoint.thrust_body[0];
    float thrust_y = attitude_setpoint.thrust_body[1];
    float thrust_z = attitude_setpoint.thrust_body[2];

计算当前角度与设定角度之间的误差omega。

通过P控制器计算初步的控制力矩。

通过PD控制器（比例微分控制器）调整控制力矩。

五、提取推力设定值并约束控制命令

constrain_actuator_commands(roll_u, pitch_u, yaw_u, thrust_x, thrust_y, thrust_z);
 /* Geometric Controller END*/

原创作者: u_15352549 转载于: https://blog.51cto.com/u_15352549/11315232

wacwl

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
PX4水下机器人参数配置

一、Px4版本1.14.1机型文件 PX4Autopilotmain\ROMFS\px4fmu_common\init.d\airframes路径下这个脚本设置了BlueROV2（重型配置）的各种参数和初始化步骤，包括电池设置、通信设置、机架和旋翼配置以及PWM输出功能的映射。通过这些设置，可以确保机器人在启动时加载正确的...
复制链接

扫一扫