PX4学习笔记3: 速度控制（转载）

摘自：https://blog.csdn.net/wbzhang233/article/details/106727276/

 

PX4学习笔记3: 速度控制

 

wbzhang233 2020-06-13 07:58:28 251 收藏 2

分类专栏： 自主避障

版权

记录一下PX4在offboard板外模式下用速度控制四旋翼的过程.
参考资料如下:

1. PX4板外模式教程
2. youtube速度控制小实验
3. PX4飞行模式
4. offboard模式
5. offboard模式控制例子
   注意:必须2Hz以上的频率发布控制消息,否则PX4则自动由offboard切会返航return模式(一直网往上飞至一定高度后返航,可由以下参数进行调整)
   

1.mavros话题

详见: mavros话题
四旋翼的控制量主要为[x,y,z,roll,pitch,yaw,vx,vy,vz,wx,wy,wz]；也就是位置和姿态以及两者的变化率共12个变量.
其中位置在mavros中包括global和local两种,global指的是经纬高LLA,通常会转化成东北天ENU,local指得是在Gazebo坐标系下的坐标和姿态(xyzw四元数表示). 通常可以使用发布local坐标的位置进行控制, 也可以使用速度控制. 话题如下:
以下为local坐标控制:

以下为速度控制:

2.youtube速度控制小实验

详见本文摘要参考链接2.
主要分为一下几步:

• 启动PX4在Gazebo中的SITL
• 发布位置控制消息,同时打开QGC地面站,切换到Offboard模式
• 发布速度控制消息,停止发布位置控制消息
• PX4飞机将以发布的速度消息飞行.

# 参考: https://www.youtube.com/watch?v=2jksI-S3ojY

## 1.发送位置
rostopic pub -r 20 /mavros/setpoint_position/local geometry_msgs/PoseStamped "header:
+ 'Tab键'+光标输入改变数字  "
## 2.切换到Offboard模式
rosrun mavros mavsys mode -c OFFBOARD
# 解锁
rosrun mavros mavsafety arm

## 3.发送速度
rostopic pub -r 20 /mavros/setpoint_velocity/cmd_vel_unstamped geometry_msgs/Twist "linear:
+ 'Tab键'+光标输入改变数字  "
### 关闭发布位置的终端,飞机将以速度消息控制飞行.

3.body frame下的速度控制

以上速度和位置控制都是在NED坐标系下的，在gazebo的SITL仿真中即为设置在gazebo坐标系的xyz方向的分量。要使用机体系下的速度控制，先给出参考：
机体系速度控制1
机体系速度控制2
body_frame_vel
机体系速度控制3
offboard_avoidance

setpoint_raw/local消息如下：

其中各个变量含义如下：

坐标系选择

uint8 coordinate_frame表示坐标系；
、
1和7分别表示LOCAL的北东地坐标系和offset，8和9分别表示机体坐标系和offset。

控制变量选择

uint16 type_mask 表示要控制变量的忽略码字。

共12位，如果要忽略多位，只需要将对应的码相加即可。
如速度控制的码字应该是111,111,000,111等于2*2048-1-8-16-32=4039。
速度控制加上偏航角控制应该是：101,111,000,111等于3015
位置控制的码字应该是111,111,111,000，等于4088.
或者参考以下问题中的写法：https://github.com/mavlink/mavros/issues/909
like this:

   while(ros::ok() && !this->manual){
        mavros_msgs::PositionTarget velocity_msg;
        velocity_msg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED;
        velocity_msg.header.frame_id = "drone";
        velocity_msg.type_mask = mavros_msgs::PositionTarget::IGNORE_PX |
                                 mavros_msgs::PositionTarget::IGNORE_PY |
                                 mavros_msgs::PositionTarget::IGNORE_PZ |
                                 mavros_msgs::PositionTarget::IGNORE_AFX |
                                 mavros_msgs::PositionTarget::IGNORE_AFY |
                                 mavros_msgs::PositionTarget::IGNORE_AFZ |
                                 mavros_msgs::PositionTarget::FORCE |
                                 mavros_msgs::PositionTarget::IGNORE_YAW |
                                 mavros_msgs::PositionTarget::IGNORE_YAW_RATE ;
        velocity_msg.header.stamp = ros::Time::now();

        velocity_msg.velocity.x = this->xs;
        velocity_msg.velocity.y = this->ys;
        velocity_msg.velocity.z = this->zs;

        if(this->testen){
            velocity_msg.velocity.x = 0.0;
            velocity_msg.velocity.y = -0.4;
            velocity_msg.velocity.z = 0.0;
        }

大佬还是很多。参考：
控制节点的学习案例
机体系速度的学习案例2