阿木实验室PX4开发课程整理

1.1：alt+ctrl+t 打开终端

cd Desktop/ 进入到桌面目录 cd - 返回上次访问目录 cd .. 返回上一目录

gedit circular.cpp 进入某文件 roscd px4_control 进入文件夹px4_control

rostopic echo /mavros/local_position/pose查看某主题信息

ls显示当下所有目录

touch my.txt 创建文件

cp my.txt Desktop/ 复制文件至桌面

mkdir 创建目录

rm -r 强制删除

cat 显示文件内容

sudo update-alternatives --config python 查看多版本Python并切换

apt-cache search package 搜索包

sudo apt-get install package 安装包

sudo apt-get remove package --purge 删除包，删除配置文件

sudo apt-get update 更新

sudo apt-get upgrade 更新已安装包

dpkg-l | grep gazebo 查看安装包信息

git clone 网址 下载github上相关文件至本地

1.2：下载编译工具链 ubuntu.sh requirements.txt 运行 source ubuntu.sh

wget https://raw.githubusercontent.com/PX4/Firmware/master/Tools/setup/ubuntu.sh

wget https://raw.githubusercontent.com/PX4/Firmware/master/Tools/setup/requirements.txt

source ubuntu.sh

下载px4源码 git clone https://github.com/px4/firmware或git clone https://github.com/bingobinlw/firmware

cd firmware 更新子模块 git submodule update --init --recursive

切换至稳定版本 git tag -l 切换git checkout v1.13.2 有警告 git status 查看版本 make distclean code

编译加下载：make px4_fmu_v5_default upload

仿真：make px4_sitl_default gazebo

解锁：commander arm

起飞：commander takeoff

3.1 仿真遥控控制，航迹仿真

状态信息查看：MAVLINK inspector

位置信息：local_position_NED 期望位置信息：position_target_local_NED

补充：打开无人机仿真与QGC可以下载飞行日志使用flightplot查看波形分析

3.2 姿态环实机调试 绑绳调节PID 调节外环使得角度响应值稍微超调期望值

再调节内环p及d抑制震荡

4.1位置环调试

MAVLINK inspector position_target_local_NED z and vz

先调节z方向的内环数据vz pid再调节外环的p

调节水平的xy方向，调节内环及外环 rviz

QGC参数 aid——EKF2_AID_MASK vsion position vision yaw fusion

hg EKF2_HGT_Mode vision

调节参数：Multicopter position control 内环p MPC_XY_VEL_P

震荡分析：确保加速度在0.5g以内 flightplot openlog volans_log

sensor_combined_0.accelerometer_m_s2[0] x

添加噪声：project volans firmware tools sitl_gazebo models iris.sdf

噪声密度：accelerometernoisedensity

5.1：利用PSP工具箱部署控制算法

matlab PSP工具箱 add—ones PSP src firmware git checkout v1.8.0 git status

MATLAB软件加速 ：opengl('save','software')

仿真：打开px4demo_uorbraed simulink model settings hardware board:px4 host target

build options posix_sitl_default 启动

在~/PSP/px4_src/Firmware下输入： git status git diff make posix_sitl_default gazebo

在MATLAB启动按钮下 点击connect 再点击启动建立源码与simulink的连接

实物部署：model settings hardware board:px4 pixhawk4 读取传感器与cmake连接

build options pixhawk 4 nuttx_px4mu-v5_default cmake

下载到实际飞控中，把自动生成的代码px4_simulink_app模块加入到编译路径中，并启动连接此模块与飞控

在~/PSP/px4_src/Firmware下输入： git status

cmake/configs/nuttx_px4fu-v5_default.cmake

终端输入vim ROMFS/px4fmu_common/init.d/rc.mc_apps

在mc_pos_control start后面一行加入 px4_simulink_app start

wq

make px4fmu-v5_default upload编译 完成后返回simulink来connect

5.2 MATLAB中PSP/demo/px4examples/positioncontrol with ratecontrol 控制算法仿真

model settings hardware board:px4 host target build options posix_sitl_default 启动

在~/PSP/px4_src/Firmware下输入： git status git diff make posix_sitl_default gazebo

在MATLAB启动按钮下 点击connect 再点击启动建立源码与simulink的连接

commander arm

部署自己的控制算法：attitude system 编译生成px4 simulink app模块

终端输入vim ROMFS/px4fmu_common/init.d/rc.mc_apps

在mc_pos_control start后面一行加入 px4_simulink_app start

wq

6.1：介绍如何配置相关环境如px4下ros，mavros等的仿真平台。主要介绍的板块为project

下的volans，其中README.md文件下有相关说明。其中px4为硬件方面，而板载，ros等为感知方面

可在板载计算机开发上级应用。（工具链安装：/project/volans/firmware/tools/setup source ubuntu.sh 若仅仿真则INSTALL_NUTTX='false'）

查看固件版本 git status 切换分支 git checkout v1.10.1

验证mavros是否安装成功：/project/volans/firmware/launch roslaunch mavros_posix_sitl.launch

编译工程：cd volans 根据需要选择编译的包 catkin build simulation px4_control ros_slam octomap

ros_vision path_planning explore_lite

编译成功后运行source_environment.sh 添加firmware环境变量，volans gazebo模型路径，gazebo_modles模型路径。

1.在/project/volans下运行roslaunch simulation circular_px4.launch

可实现键盘控制仿真无人机运动。（通过调节QGC中的参数实现）

2.offboards下slam自主探索。roslaunch simulation ros_Auto2Dnav_demo_px4.launch

6.2 offboard走预定航点

加载带有激光雷达的场景（加载模型）/volans/src/simulation/launch/px4 roslaunch 2Dlidar_px4.launch （其中urdf_px4.launch模型可显示在rviz上，打开rviz—Add—RobotState

打开激光雷达消息：rviz—fixed frame 2Dlidar link Add—by topic laserscan

打开摄像头信息：rqt_image_view 小车模型打开后，可在gazebo中控制

/volans/src/simulation/launch/Demo/px4_quadrotor 2D定位，地图建立，3D定位，走圆，kcf跟踪，moveit（路径规划） obstacle（避障联调）

/volans/src/simulation/msg 消息 /scripts/keyboard_control_px4.py 键盘遥控控制

改变仿真环境：gazebo建立保存至volans/src/simulation/worlds 复制empty_room.world

<physics 至<physics>部分替换刚刚保存的文件内部分。随后可通过launch/Demo/px4_quadrotor/circular_px4.lauch加载出来（打开circular_px4.lauch修改下述加载无人机地图）

offboard模式下走圆在/project/volans下运行roslaunch simulation circular_px4.launch

================================================================================

<launch>

<!-- offboard 模式下飞圆形轨迹-->

<!-- 圆心在：切offbord时飞机所在位置

desire_z：期望圆半径

desire_Radius：期望高度-->

#加载无人机世界地图

<arg name="world_path" default="$(find simulation)/worlds/classroom.world" />

#加载环境，激光雷达，仿真模型

<include file="$(find simulation)/launch/px4/iris_px4.launch">

<arg name="world" value="$(arg world_path)" />

</include>

#主要的控制脚本（可以在终端roscd px4_control 进入文件circular.launch）（volans/src/modules/px4_control/launch）

<include file="$(find px4_control)/launch/circular.launch" >

<arg name="desire_z" value="1" />

<arg name="desire_Radius" value="1" />

</include>

<node pkg="simulation" type="keyboard_control_px4.py" name="keyboard_control_px4" output="screen" launch-prefix="gnome-terminal --tab -e">

</node>

</launch>

============================================================================

文件circular.launch

============================================================================

<launch>

<!-- 圆心在：切offbord时飞机所在位置

desire_z：期望圆半径

desire_Radius：期望高度-->

<arg name="desire_z" default="1" />

<arg name="desire_Radius" default="1" />

#加载ros的节点 通过文件circular.cpp

<node pkg="px4_control" type="circular_node" name="circular_node" output="screen">

<param name="desire_z" value = "$(arg desire_z)"/>

<param name="desire_Radius" value = "$(arg desire_Radius)"/>

</node>

</launch>

============================================================================

文件circular.cpp（/project/volans/src/modules/px4_control/src）

============================================================================

/

#include <ros/ros.h>

#include <math.h>

#include <string>

#include <vector>

#include <iostream>

#include <stdio.h>

#include <mavros_msgs/PositionTarget.h>

#include <mavros_msgs/State.h>

#include <mavros_msgs/SetMode.h>

#include <Eigen/Eigen>

#include <geometry_msgs/PoseStamped.h>

using namespace std;

Eigen::Vector3d pos_target;//offboard模式下，发送给飞控的期望值

float desire_z = 1; //期望高度

float desire_Radius = 1;//期望圆轨迹半径

float MoveTimeCnt = 0;

float priod = 1000.0; //减小数值可增大飞圆形的速度

Eigen::Vector3d temp_pos_drone;

Eigen::Vector3d temp_pos_target;

mavros_msgs::SetMode mode_cmd;

ros::Publisher setpoint_raw_local_pub;

ros::ServiceClient set_mode_client;

enum

{

WAITING,//等待offboard模式

CHECKING,//检查飞机状态

PREPARE,//起飞到第一个点

REST,//休息一下

FLY,//飞圆形路经

FLYOVER,//结束

}FlyState = WAITING;//初始状态WAITING

//接收来自飞控的当前飞机位置

Eigen::Vector3d pos_drone;

void pos_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)

{

// Read the Drone Position from the Mavros Package [Frame: ENU]

Eigen::Vector3d pos_drone_fcu_enu(msg->pose.position.x,msg->pose.position.y,msg->pose.position.z);

pos_drone = pos_drone_fcu_enu;

}

//接收来自飞控的当前飞机状态

mavros_msgs::State current_state;

void state_cb(const mavros_msgs::State::ConstPtr& msg) {

current_state = *msg;

}

//发送位置期望值至飞控（输入：期望xyz,期望yaw）

void send_pos_setpoint(const Eigen::Vector3d& pos_sp, float yaw_sp)

{

mavros_msgs::PositionTarget pos_setpoint;

//Bitmask toindicate which dimensions should be ignored (1 means ignore,0 means not ignore; Bit 10 must set to 0)

//Bit 1:x, bit 2:y, bit 3:z, bit 4:vx, bit 5:vy, bit 6:vz, bit 7:ax, bit 8:ay, bit 9:az, bit 10:is_force_sp, bit 11:yaw, bit 12:yaw_rate

//Bit 10 should set to 0, means is not force sp

pos_setpoint.type_mask = 0b100111111000; // 100 111 111 000 xyz + yaw

pos_setpoint.coordinate_frame = 1;

pos_setpoint.position.x = pos_sp[0];

pos_setpoint.position.y = pos_sp[1];

pos_setpoint.position.z = pos_sp[2];

pos_setpoint.yaw = yaw_sp;

setpoint_raw_local_pub.publish(pos_setpoint);

}

//状态机更新

void FlyState_update(void)

{

switch(FlyState)

{

case WAITING:

if(current_state.mode != "OFFBOARD")//等待offboard模式

{

pos_target[0] = pos_drone[0];

pos_target[1] = pos_drone[1];

pos_target[2] = pos_drone[2];

temp_pos_drone[0] = pos_drone[0];

temp_pos_drone[1] = pos_drone[1];

temp_pos_drone[2] = pos_drone[2];

send_pos_setpoint(pos_target, 0);

}

else

{

pos_target[0] = temp_pos_drone[0];

pos_target[1] = temp_pos_drone[1];

pos_target[2] = temp_pos_drone[2];

send_pos_setpoint(pos_target, 0);

FlyState = CHECKING;

}

//cout << "WAITING" <<endl;

break;

case CHECKING:

if(pos_drone[0] == 0 && pos_drone[1] == 0) //没有位置信息则执行降落模式

{

cout << "Check error, make sure have local location" <<endl;

mode_cmd.request.custom_mode = "AUTO.LAND";

set_mode_client.call(mode_cmd);

FlyState = WAITING;

}

else

{

FlyState = PREPARE;

MoveTimeCnt = 0;

}

//cout << "CHECKING" <<endl;

break;

case PREPARE://起飞到圆轨迹的第一个点,起点在X负半轴

temp_pos_target[0] = temp_pos_drone[0] - desire_Radius;

temp_pos_target[1] = temp_pos_drone[1];

temp_pos_target[2] = desire_z;

MoveTimeCnt +=2;

if(MoveTimeCnt >=500)

{

FlyState = REST;

MoveTimeCnt = 0;

}

pos_target[0]=temp_pos_drone[0]+(temp_pos_target[0]-temp_pos_drone[0])*(MoveTimeCnt/500);

pos_target[1]=temp_pos_drone[1]+(temp_pos_target[1]-temp_pos_drone[1])*(MoveTimeCnt/500);

pos_target[2]=temp_pos_drone[2]+(temp_pos_target[2]-temp_pos_drone[2])*(MoveTimeCnt/500);

send_pos_setpoint(pos_target, 0);

if(current_state.mode != "OFFBOARD")//如果在准备中途中切换到onboard，则跳到WAITING

{

FlyState = WAITING;

}

//cout << "PREPARE" <<endl;

break;

case REST:

pos_target[0] = temp_pos_drone[0] - desire_Radius;

pos_target[1] = temp_pos_drone[1] ;

pos_target[2] = desire_z;

send_pos_setpoint(pos_target, 0);

MoveTimeCnt +=1;

if(MoveTimeCnt >= 100)

{

MoveTimeCnt = 0;

FlyState = FLY;

}

if(current_state.mode != "OFFBOARD")//如果在REST途中切换到onboard，则跳到WAITING

{

FlyState = WAITING;

}

break;

case FLY:

{

float phase = 3.1415926;//起点在X负半轴

float Omega = 2.0*3.14159*MoveTimeCnt / priod;//0~2pi

MoveTimeCnt += 3;//调此数值可改变飞圆形的速度

if(MoveTimeCnt >=priod)//走一个圆形周期

{

FlyState = FLYOVER;

}

pos_target[0] = temp_pos_drone[0]+desire_Radius*cos(Omega+phase);

pos_target[1] = temp_pos_drone[1]+desire_Radius*sin(Omega+phase);

pos_target[2] = desire_z;

send_pos_setpoint(pos_target, 0);

if(current_state.mode != "OFFBOARD")//如果在飞圆形中途中切换到onboard，则跳到WAITING

{

FlyState = WAITING;

}

}

//cout << "FLY" <<endl;

break;

case FLYOVER:

{

mode_cmd.request.custom_mode = "AUTO.LAND";

set_mode_client.call(mode_cmd);

FlyState = WAITING;

}

//cout << "FLYOVER" <<endl;

break;

default:

cout << "error" <<endl;

}

}

int main(int argc, char **argv)

{

ros::init(argc, argv, "circular_offboard");

ros::NodeHandle nh("~");

// 频率 [20Hz]

ros::Rate rate(20.0);

#ros节点 接收无人机位置信息（终端：rostopic echo /mavros/local_position/pose ）

ros::Subscriber position_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/local_position/pose", 100, pos_cb);

ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state", 10, state_cb);

#发布无人机期望的位置信息

setpoint_raw_local_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 10);

// 【服务】修改系统模式

set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");

nh.param<float>("desire_z", desire_z, 1.0);

nh.param<float>("desire_Radius", desire_Radius, 1.0);

while(ros::ok())

{

FlyState_update(); #发布信息

ros::spinOnce(); #不断接收位置与状态

rate.sleep();

}

return 0;

}

=================================================================================

7.1 ssh volans@10.42.0.1 连接电脑

roslaunch volans t265_position_to_mavros.launch 连接飞机

gedit ~/.bashrc 配置本地电脑ip export ROS_MASTER_URI=http://10.42.0.0:11311

export ROS_IP=10.42.0.1

打开QGC comm links add Name:unname Type: TCP 设置地址：10.42.0.1 connect

传感器磁罗盘校准 rostopic echo /mavros/state 查看连接状态

板载计算机中： roslaunch px4_control circular.launch 先飞定点模式 解锁切换offboard

7.2 simulink VFH 自动生成代码（ros包）复制到Ubuntu编译部署到实际硬件中。

cd project/volans git status git pull catkin build px4_control catkin build simulation 编译环境

cd project/volans/firmware git status make px4_sitl_default gazebo

roslaunch simulation simulinkVFH_px4.launch

~/project/volans/src/modules/matlab

打开MATLAB

7.3

1.更新代码 cd project/volans git status git pull

2.打开MATLAB simulinkAstar 使用Cartograph生成全局地图 roslaunch simulation cartographer

(使用前安装功能包 sudo apt-get install ros-melodic-cartographer*）

雷达建图roslaunch simulation cartographer2Dlidar_mapping_demo_px4.launch

保存地图rosrun map_server map_saver 得到：map.pgm

roslaunch simulation simulinkAstar_px4.launch

打开MATLAB运行pgmtomap.m加载全局地图bwimage

打开simulink仿真

3.配对ros节点 终端 ifconfig 找到ip 在MATLAB中rosinit 192.168.0.106

ctrl+D更新simulink模块 ctrl+T运行simulink

创建脚本planStar.m：

image = imread('simulation_gmapping_map.pgm'); #打开地图

imageCropped = image(1:184,7:290); #转为图像形式

bwimage = imageCropped < 100; #二值化处理

imshow(bwimage)

map = binaryOccupancyMap(bwimage,20); #像素分辨率转化

show(map)

validator = validatorOccupancyMap;

validator.Map = map;

planner = plannerHybridAStar(validator,'MinTurningRadius',0.8,'MotionPrimitiveLength',1.2); #转弯半径，运动基元长度

startPose = [7.5 7.5 0]; #起始位置

goalPose= [7.5 1.5 0]; #终点位置

refpath = plan(planner,startPose,goalPose);

show(planner);

Astart ~/project/volans/src/modules/matlab/2D_Astar