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