PX4 SITL MAVROS速度控制-用机身坐标系发布速度
不改变PX4控制双闭环源码，仅依靠mavros现有的速度控制和位置控制话题，来实现旋翼圆形轨迹。
1. 位置控制：/mavros/setpoint_position/local
优点：位置准确；
缺点：需给出多个waypoint坐标，计算较麻烦；waypoint到waypoint之间有加减速，飞机晃动较大。
2. 速度控制：/mavros/setpoint_velocity/vel_cmd
观察了下飞手圆形控制时，仅控制yaw, roll, pitch为恒定即可实现。
ROS海龟实例也是给了一个线速度x，和z角速度（yaw),即可。
但是该话题默认使用LOCAL_NED坐标系。
mavros/src/plugins/setpoint_velocity.cpp中添加了可选项，通过ros::service来改变坐标系，选择FRAME_BODY_NED即可。
优点：平稳；
缺点：位置会飘。
没有更多推荐了，求问pixhawk 和树莓派如何通信连接，使得用树莓派给pixhawk发送飞行相关的指令来控制飞机？ - 知乎有问题，上知乎。知乎作为中文互联网最大的知识分享平台，以「知识连接一切」为愿景，致力于构建一个人人都可以便捷接入的知识分享网络，让人们便捷地与世界分享知识、经验和见解，发现更大的世界。80被浏览<strong class="NumberBoard-itemValue" title="分享邀请回答3 条评论分享收藏感谢收起mavros - ROS Wiki
&&Show EOL distros:&Package Links (19) (2)Package Summary Released
Continuous Integration
Documentedpixhawk 四旋翼笔记1
开始入坑pixhawk。讲讲遇到的坑。（见解可能有误，欢迎拍砖）
1.一开始并不清楚pixhawk飞控是什么，后来发现是一个开源硬件平台，而且有两大开源飞控代码可以在其上运行。一个出自APM开源社区，一个出自苏黎世理工。
APM的代码已有多年的积累，其稳定性较好，但由于历史原因，导致其原有的硬件平台性能已经被榨干，于是APM社区人员将其移植到pixhawk的硬件平台上继续发展。也正是因为代码多年的累积，导致其结构逻辑较为晦涩，要想再做改动，非专业人士很难入门和下手。
PIXHAWK的软件是重构了APM的架构，将模块间的关系重新整理，但在算法上并非完全复制。同时又有苏黎世理工的大神加盟，代码性能和APM的相比，据说有所提高。不过最重要的是代码架构的清晰让新鲜血液更快的融入，大大降低了开发者的工作量，果断选择这个了。（dota和lol的区别吧，嘿嘿）
2.关开发环境配置。最要吐槽的是官方要求使用git clone 。而下载的速度惨目忍睹。而且无论是win还是ubuntu，clone工程以后，第一次编译你会发现非常非常慢。仔细观察会发现，他还在clone其他的依赖文件，而这个过程也是非常非常慢。。运气不好，clone失败。总之要有耐心，跟着官网的教程来。我试过从别人那儿拷贝整个工程过来，发现编译会失败，原因莫名其妙，总之还是乖乖地clone吧。
3.遇到的第一个问题，nsh启动乱码。按照官网的要求，一步步配置，到了插上usb下载固件后连接串口，发现无论如何，一打开串口就是乱码。后来发现sd卡不插时就能正常启动nsh服务。一开始没搞懂，后来查阅了\Firmware\ROMFS\px4fmu_common\init.d\rcS文件。这个是启动脚本，在最后找到了问题所在。
# Start USB shell if no microSD present, MAVLink else
if [ $LOG_FILE == /dev/null ]
# Try to get an USB console
nshterm /dev/ttyACM0 &
mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
官网上的说明没有更新（好像页面上有说教程过时了，会有所不同）。在这里启动程序检测了sd卡的存在，如果存在就开启mavlink传输协议，否则开启nsh。
4.未解决的问题1。将错就错，我把代码修改了一下，去除mavlink，保留nsh，但是在插着sd卡的时候，还是无法打开nsh。实在不明白，求高人指点。
if [ $LOG_FILE == /dev/null ]
# Try to get an USB console
nshterm /dev/ttyACM0 &
nshterm /dev/ttyACM0 &
#mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
5.未解决的问题2。在跑了官网的教程代码后想试试程序自动启动。px4_daemon_app 在正常模式下运行 px4_daemon_app start 会隔10秒输出信息，px4_daemon_app stop会终止程序，px4_daemon_app status会输出当前程序是否运行。我稍稍修改了启动文件。在不插sd卡的情况下进行测试。
if [ $LOG_FILE == /dev/null ]
# Try to get an USB console
px4_daemon_app start
nshterm /dev/ttyACM0 &
mavlink start -r 800000 -d /dev/ttyACM0 -m config -x
理论上，px4_daemon_app 随着系统启动而启动，会定时输出信息，但实际上没有任何信息输出到串口上。但是奇怪的是，我直接输入px4_daemon_app status命令（注意之前没有输入px4_daemon_app start ），串口输出px4_daemon_app:
running。换言之，px4_daemon_app 程序是在运行的！只是warnx("Hello daemon!\n");没有将数据输出到串口。之后输入px4_daemon_app stop 再px4_daemon_app
start ，数据正常输出。
之后我在想是否是由于在启动文件中先调用了px4_daemon_app start 再调用nshterm /dev/ttyACM0 &导致的问题。于是将其位置互换，得到的结果还是一样的。没有解决。
贴一下px4_daemon_app 的代码。
#include &stdio.h&
#include &stdlib.h&
#include &string.h&
#include &unistd.h&
#include &px4_config.h&
#include &nuttx/sched.h&
#include &systemlib/systemlib.h&
#include &systemlib/err.h&
static bool thread_should_exit =
/**& daemon exit flag */
static bool thread_running =
/**& daemon status flag */
static int daemon_
/**& Handle of daemon task / thread */
* daemon management function.
__EXPORT int px4_daemon_app_main(int argc, char *argv[]);
* Mainloop of daemon.
int px4_daemon_thread_main(int argc, char *argv[]);
* Print the correct usage.
static void usage(const char *reason);
static void
usage(const char *reason)
if (reason) {
warnx("%s\n", reason);
warnx("usage: daemon {start|stop|status} [-p &additional params&]\n\n");
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
* Makefile does only apply to this management task.
* The actual stack size should be set in the call
* to task_create().
int px4_daemon_app_main(int argc, char *argv[])
if (argc & 2) {
usage("missing command");
if (!strcmp(argv[1], "start")) {
if (thread_running) {
warnx("daemon already running\n");
/* this is not an error */
thread_should_exit =
daemon_task = px4_task_spawn_cmd("daemon",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT,
px4_daemon_thread_main,
(argv) ? (char *const *)&argv[2] : (char *const *)NULL);
if (!strcmp(argv[1], "stop")) {
thread_should_exit =
if (!strcmp(argv[1], "status")) {
if (thread_running) {
warnx("\trunning\n");
warnx("\tnot started\n");
usage("unrecognized command");
int px4_daemon_thread_main(int argc, char *argv[])
warnx("[daemon] starting\n");
thread_running =
while (!thread_should_exit) {
warnx("Hello daemon!\n");
sleep(10);
warnx("[daemon] exiting.\n");
thread_running =
没有更多推荐了，zhengyuxin0507的博客
https://blog.csdn.net/
https://static-blog.csdn.net/images/logo.gif
https://blog.csdn.net/zhengyuxin0507
https://blog.csdn.net/
https://blog.csdn.net/zhengyuxin0507/article/details/
https://blog.csdn.net/zhengyuxin0507/article/details/
zhengyuxin0507
之前，我们已经能够成功将地标识别出来，那么如何引导无人机向地标飞行呢？我们可以分为两部分：一、计算无人机与地标的相对位置；二、根据相对位置控制无人机飞行。这部分和之前几章不太一样，理论推导较多，请大家耐心看一看。若有不对的地方，也请指正~1 相对位置估计
这里要提一下，目前很主流的一种估计方法使PnP，也是我最开始尝试的一种方法。但是，PnP是基于点的估计方法，我们的地标中特征点（角点）不多，而且用来估计的点噪声比较大，所以效果不太好。后来，我决定用面积来估计相对位置，以提高估计的鲁棒性和精度。整个相对位置估计可以分为相机坐标系下的估计和绝对坐标系下的估计，我们先来做相机坐标系下的估计。1.1 相机坐标系下的估计
首先，我们来估计Zc，估计的思路是，图像中地标面积（看成矩形）与地标实际面积的比值与高度成正比例关系，如果我们能够知道这个比例，那么就能求得无人机高度。在理论推导这个比例之前，我们先要讲一讲摄像头的内参，直接上公式吧。其中，K即为摄像头的内参（张友正标定法可得），第一个式子表示了图像平面坐标与相机平面坐标之间的关系，是我们后面进行推导的重要依据。如果有博友的不了解的话，建议先看一下高博的《视觉SLAM十四讲》。言归正传，为了后面的推导，我们先做三个假设。
（1）地标实际面积已知（的确，我们量一下就知道了）。
（2）相机内参中fx与fy相等（绝大多数实际情况下他们的确是十分接近的）。
（3）相机正对地标，即机体无滚转和俯仰（后来我会细讲，为了推导方便，暂时让它存在）。推到直接上图了，公式打起来比较烦，请见谅。之前由于假设（3）的限制，我们的推导只能在相机平面与地标平面平行的时候成立，这是不科学的，那么如何去掉这个假设呢？其实很简单，做一个仿射变换就可以了，公式如下：其中，theta和phi分别为机体的俯仰角和滚转角。经过仿射变换的面积依旧是与Zc成正相关的，这样我们就成功地拿掉了假设（3）。然后，只要将求得的Zc直接代入相机模型公式就可以算出来Xc，Yc。1.2 绝对坐标系下的估计
要将估计结果真正用于无人机导航，就要将相机坐标系下的估计结果转换到绝对坐标系。这边提一句，我把绝对坐标系的原点选为地标中心，方向是东北天（与mavros的导航坐标系一致）。如上文所说，如果相机平面与地标平面平行（即无俯仰滚转时），我们很容易就能实现坐标系的转换。但是，当出现滚转与俯仰的情况，我们需要做一些几何处理。以下举一种例子。这种推导是我自己推的，所以当中有不严谨的地方，还请各位博友指正。还有一种Oc与Ow在异侧的情况，我就不展开啦。好吧，将了这么多上代码吧。// td是一个结构体，用来储存之前识别的信息，比如中心的坐标，地标的面积
//current_att 表示了机体姿态
void EstimatePose( flag_detection_t *td, Euler_t current_att)
double detect_begin = clock();
double gama = acos(cos(current_att.pitch) * cos(current_att.roll));
float delta_l, L, H, X, Y;
if(gama / PI * 180 & 1)
td-&img_area = ComputeArea(td-&corners[td-&corners.size()-1]);
td-&Pc.z = sqrt(td-&square_area * K.at&float&(0,0) * K.at&float&(1,1) / (td-&img_area * cos(current_att.pitch) * cos(current_att.roll)));
td-&Pc.x = (td-&center.x - K.at&float&(0,2)) * td-&Pc.z / K.at&float&(0,0);
td-&Pc.y = (td-&center.y - K.at&float&(1,2)) * td-&Pc.z / K.at&float&(1,1);
Oc.x = K.at&float&(0,2) + K.at&float&(0,0) * tan(current_att.roll);
Oc.y = K.at&float&(1,2) - K.at&float&(1,1) * tan(current_att.pitch);
delta_l = sqrt(td-&Pc.x*td-&Pc.x + td-&Pc.y*td-&Pc.y) * tan(fabs(gama));
if( ((Oc.x-K.at&float&(0,2)) * (td-&center.x-K.at&float&(0,2)) + (Oc.y-K.at&float&(1,2)) * (td-&center.y-K.at&float&(1,2))) & 0 )
L = td-&Pc.z - delta_l;
direction = 1;
L = td-&Pc.z + delta_l;
direction = 0;
H = L * cos(gama);
if(direction == 0)
Y = H * tan( current_att.pitch + atan((td-&center.y - K.at&float&(1,2)) /
K.at&float&(1,1)) );
X = H * tan( current_att.roll - atan((td-&center.x - K.at&float&(0,2)) /
K.at&float&(0,0)) );
Y = H * tan( current_att.pitch + atan((td-&center.y - K.at&float&(1,2)) /
K.at&float&(1,1)) );
X = H * tan( current_att.roll - atan((td-&center.x - K.at&float&(0,2)) /
K.at&float&(0,0)) );
td-&Pw.x = X;
td-&Pw.y = Y;
td-&Pw.z = H;
double duration = ( clock() - detect_begin ) / CLOCKS_PER_SEC * 1000;
cout && "Estimate Time is " && duration &&
}2 控制算法
控制算法这种东西的话，是仁者见仁智者见智的，每个人都有不同的控制思路。我是用了速度控制，视觉伺服的方式，不断向目标点飞行，最后降落。我就直接贴代码吧。 while(ros::ok()){
ros::Time t0 = ros::Time::now();
ros::spinOnce();
att_stable = Att_stable(current_position);
arrive_flag = ArriveFlag(td-&Pw);
//控制策略
//td-&result为1表示识别到地标
if(td-&result == 0)
if(td-&prev_result == 0)
state = HOVER;
else if( arrive_flag == 1 )
state = LAND;
state = FLY;
else if( arrive_flag == 1 )
state = LAND;
state = FLY;
//控制具体执行
switch(state)
//计算velocity，其实就是一个PID
velocity = ComputeLinearVelocity(td-&Pw);
twist.twist.linear.x = velocity.x;
twist.twist.linear.y = velocity.y;
twist.twist.linear.z = 0;
twist.twist.angular.x = 0;
twist.twist.angular.y = 0;
twist.twist.angular.z = 0;
local_vel_pub.publish(twist);
case HOVER:
twist.twist.linear.x = 0;
twist.twist.linear.y = 0;
twist.twist.linear.z = 0;
local_vel_pub.publish(twist);
case LAND:
if( td-&Pw.z & 150 )
twist.twist.linear.x = 0;
twist.twist.linear.y = 0;
twist.twist.linear.z = -1;
local_vel_pub.publish(twist);
auto_land = 1;
ROS_INFO("Unexpected Error!");
if(state == LAND && auto_land == 1){
if( current_state.mode != "AUTO.LAND" &&
(ros::Time::now() - last_request & ros::Duration(5.0))){
if( set_mode_client.call(offb_set_mode1) &&
offb_set_mode1.response.mode_sent){
ROS_INFO("autoland enabled");
last_request = ros::Time::now();
ros::Time t1 = ros::Time::now();
ros::Duration d(t1 - t0);
rate.sleep();
讲到这里，定点降落的整个理论部分我已经粗略地讲完啦。中间有许多细节没有展开，我的考虑也有很多不全面的地方，各位博友可以通过评论、私聊等形式与我交流想法。下一章，我会讲一讲定点降落的实验部分，讲一下实验的过程，展示一下实验的结果。
作者：zhengyuxin0507 发表于
https://blog.csdn.net/zhengyuxin0507/article/details/
https://blog.csdn.net/zhengyuxin0507/article/details/
https://blog.csdn.net/zhengyuxin0507/article/details/
zhengyuxin0507
前面讲了三讲的环境搭建，今天终于要开始讲算法啦。先来描述一下定点降落的过程，四旋翼上搭载的摄像头识别地面上的标志，然后以地面标志作为引导，降落到标志所在处。所以，我们今天先来讲讲地标的设计与识别算法。首先是地标设计，地标设计有一个最重要的原则就是特征明显。优秀的地标能够简化识别算法，提高降落效率与鲁棒性。我的地标是这个样子滴~~~这样设计的好处在于特征明显，黑白相间的正方形非常容易识别。而且，正方形由小到大逐级嵌套，起到分级识别的作用。比如，飞机降落到接近地标的时候，我们很有可能看不到最大的那个矩形了，但是我们依然可以识别到稍小的矩形。当然，这个地标并非是我原创，借鉴于一篇硕士论文《基于视觉的小型无人直升机自主降落导航系统的设计和研究》，有兴趣的同学可以在CSDN上搜索一下。下面，我就来讲讲最重要的地标识别算法吧。分为几个步骤：动态阈值二值化、轮廓提取与矩形检测、矩形聚类与识别。1 动态阈值二值化为了提高识别算法的鲁棒性，我决定采用大津动态阈值的方法将图像二值化。具体理论就不多讲了，毕竟不是什么新的东西，我们的重点也不在这块。直接上代码吧~~~（这里啰嗦一句，这段代码我是抄CSDN上一个哥们的，但是忘了出处，没有标明，实在抱歉）//输入图像，输出阈值，很简单吧
int otsuThreshold(IplImage* img)
int T = 0;
int height = img-&
= img-&widthS
int channels
uchar* data
= (uchar*)img-&imageD
double gSum0;
double gSum1;
double N0 = 0;
double N1 = 0;
double u0 = 0;
double u1 = 0;
double w0 = 0;
double w1 = 0;
double u = 0;
double tempg = -1;
double g = -1;
double Histogram[256]={0};// = new double[256];
double N = width*
for(int i=0;i&i++)
for(int j=0;j&j++)
double temp =data[i*step + j * 3] * 0.114 + data[i*step + j * 3+1] * 0.587 + data[i*step + j * 3+2] * 0.299;
temp = temp&0? 0:
temp = temp&255? 255:
Histogram[(int)temp]++;
for (int i = 0;i&256;i++)
gSum0 = 0;
gSum1 = 0;
N0 += Histogram[i];
N1 = N-N0;
w0 = N0/N;
w1 = 1-w0;
for (int j = 0;j&=i;j++)
gSum0 += j*Histogram[j];
u0 = gSum0/N0;
for(int k = i+1;k&256;k++)
gSum1 += k*Histogram[k];
u1 = gSum1/N1;
//u = w0*u0 + w1*u1;
g = w0*w1*(u0-u1)*(u0-u1);
if (tempg&g)
}可以对比一下原图和二值化后的图像：2 轮廓提取与矩形检测轮廓提取用了opencv现成的函数——cvFindContours。具体用法直接百度就行，不再赘述。主要是矩形检测，我们分为两步：多边形近似、四边形提取。首先用cvApproxPoly函数对所有提取出的轮廓多边形近似，再寻找具有四条边且面积较大的（过滤较小的图形）的轮廓，我们暂且将其判定为矩形。代码如下：void FindSquares( IplImage* src, CvSeq* squares, CvMemStorage* storage, vector&Point& &squares_centers, vector& vector&Point& & &squares_v, Point pt )
CvSeq* cv_
// the result of detecting squares
// the pointer to read data of "result"
CvPoint corner[4];
vector&Point& corner_v;
cvFindContours( src, storage, &cv_contours, sizeof(CvContour),CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE, pt ); //find contours
while(cv_contours)
if( fabs(cvContourArea(cv_contours)) & MIN_SQUARE_AREA )
//neglect the small contours
//findout the squares
result = cvApproxPoly( cv_contours, sizeof(CvContour), storage, CV_POLY_APPROX_DP, cvContourPerimeter(cv_contours)*0.02, 0 );
if( result-&total == 4
cvCheckContourConvexity(result) )
cvStartReadSeq( result, &reader, 0 );
for( int i = 0; i & 4; i++ )
cvSeqPush( squares,(CvPoint*)cvGetSeqElem( result, i ));
memcpy( corner + i, reader.ptr, result-&elem_size );
CV_NEXT_SEQ_ELEM( result-&elem_size, reader );
for(int i =0; i & 4; i++)
//save the corner points to corner_v, it will help us process the data
temp = corner[i];
corner_v.push_back(temp);
center.x = (corner[0].x + corner[1].x + corner[2].x + corner[3].x) / 4;
center.y = (corner[0].y + corner[1].y + corner[2].y + corner[3].y) / 4;
squares_centers.push_back(center);
squares_v.push_back(corner_v);
corner_v.clear();
cv_contours = cv_contours-&h_
}执行上述代码，我们发现图像中的矩形都被检测出来了（绿色比较淡，仔细看看~~~）。3 矩形聚类与识别为什么要做矩形聚类？因为我们要把所有矩形分类，将中心点近似重合的矩形归为一类。最终，矩形数量最多的那一类我们判定为正确目标（因为实际生活中，很难有这么多层的矩形嵌套）。话不多说，上代码。//输入矩形中心，输出聚类后的结果。
//比如输入（1,1）（2,2）（3,3）（1.01,1.01）
//输出为第一类为0，3，第二类为1，第三类为2。（数字表示第几个矩形中心点）
void CenterClassification( vector&Point& &squares_centers, vector& vector&int& & &result)
vector&int& centers_
vector&int& result_
int index_i;
int index_j;
for(int i = 0; i & squares_centers.size(); i++)
centers_index.push_back(i);
//save the index of squares centers
for(int i = 0; i & centers_index.size(); i++)
result_temp.push_back(centers_index[i]);
for(int j = i + 1; j & centers_index.size(); j++)
index_i = centers_index[i];
index_j = centers_index[j];
if( ComputeDistance( squares_centers[index_i], squares_centers[index_j] ) & MIN_CENTER_DIS )
result_temp.push_back(centers_index[j]);
centers_index.erase(centers_index.begin() + j - 1);
result.push_back(result_temp);
result_temp.clear();
}可以看一下最终的识别结果。可见，第二歩中检测到的许多矩形都被我们过滤掉了。经过测试，这种识别方法的实时性和鲁棒性都不错。作者的电脑是i3-6100的CPU，但是检测算法跑到了30Hz以上，满足无人机实时性的要求。另外，在弱光，强光环境下，该算法的鲁棒性都很好。
作者：zhengyuxin0507 发表于
https://blog.csdn.net/zhengyuxin0507/article/details/
阅读：61 评论：3
https://blog.csdn.net/zhengyuxin0507/article/details/
https://blog.csdn.net/zhengyuxin0507/article/details/
zhengyuxin0507
搭建仿真环境是相当重要的，因为我们的代码如果直接放到飞机上去跑，那么很容易炸机。通过仿真环境，我们至少可以保证代码逻辑的正确性。这篇文章还是要感谢我的队友舒仔仔的帮助，话不多说，上正文。1 安装gazebo一般安装的ROS就已经自己安装了gazebo，如果已经安装了ROS，可在terminal中输入gazebo来验证，若出现下图，证明已经装好了，就不用再安装了。若没有安装gazebo，则按下面方法安装即可sudo sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" & /etc/apt/sources.list.d/gazebo-stable.list'
wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add -
# 首先更新源
sudo apt-get update
# 安装Gazebo7
sudo apt-get install gazebo7
# 开发者需要安装额外的包
sudo apt-get install libgazebo7-dev
装好之后再像上面验证一下就好了。2 源码编译gazebo仿真这里我再啰嗦一下为什么要源码编译。因为我们的飞控用的是pixhawk，它的开源代码是自带仿真的，所以我们可以在其仿真环境的基础上修改，以满足我们的需求。不用Hector_Quadrotor作为仿真环境的原因是控制接口不兼容。这么说吧，用pixhawk自带的仿真环境，我们调通了上位机（odroid）上的代码后，不需要大的修改，直接放到飞机上飞了。# 源码下载
mkdir -p ~/src
git clone https://github.com/PX4/Firmware.git
cd Firmware
git submodule update --init --recursive
# 安装交叉编译器
arm-none-eabi-gcc --version
sudo apt install gcc-arm-none-eabi
# 编译源码
cd Firmware
make px4fmu-v2_default
# 配置gazebo
sudo apt-get install libprotobuf-dev libprotoc-dev protobuf-compiler libeigen3-dev
## 在源码gazebo仿真文件夹中创建build文件夹
cd ~/src/Firmware/Tools/sitl_gazebo
mkdir Build
## 添加路径
### 设置插件的路径以便 Gazebo 能找到模型文件
export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:$HOME/src/Firmware/Tools/sitl_gazebo/Build
### 设置模型路径以便 Gazebo 能找到机型
export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:$HOME/src/Firmware/Tools/sitl_gazebo/models
### 禁用在线模型查找
export GAZEBO_MODEL_DATABASE_URI=""
### 设置 sitl_gazebo 文件夹的路径
export SITL_GAZEBO_PATH=$HOME/src/Firmware/Tools/sitl_gazebo
### 编译gazebo仿真
make#############################华丽丽的分割线#############################至此环境就算搭完啦～～～#############################华丽丽的分割线#############################3 运行gazebo打开一个terminal，依次执行以下语句：cd src/Firmware
make posix_sitl_default
source ~/catkin_ws/devel/setup.bash
// 只有mavros是用源码编译的才需要
source Tools/setup_gazebo.bash $(pwd) $(pwd)/build_posix_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo
roslaunch px4 mavros_posix_sitl.launch
出现如下界面说明你成功啦：4 通过mavros控制飞机（详见上一章）在一个新的terminal中运行上一节中建立好的offboard_node.cppcd catkin_ws
catkin_make
source devel/setup.bash
//每新开一个terminal都要有这个，否则就会出错
rosrun offboard offboard_node
在运行过后，就会看到如下效果：这一节的内容至此讲完了，下一节我们会讲地标的设计与如何将自主设计的地标添加到仿真环境中。参考资料：
作者：zhengyuxin0507 发表于
https://blog.csdn.net/zhengyuxin0507/article/details/
阅读：130 评论：1
https://blog.csdn.net/zhengyuxin0507/article/details/
https://blog.csdn.net/zhengyuxin0507/article/details/
zhengyuxin0507
一直拖了这么久没有更新，怪我太懒啦~~~ 今天要特别感谢舒仔仔同学，我的好队友帮我写了这篇博文。这篇文章主要讲了如何通过mavros来控制pixhawk。硬件逻辑是这样子，我们先搞一个odroid（土豪上TX2），再用usb线将odroid与pixhawk连接。所以，odroid发送控制指令来控制pixhawk飞行。pixhawk与外界的通信协议是mavlink。ROS下面有个包叫mavros，说到底就是mavlink的封装，用起来非常简单。话不多说，上正文。mavros安装：使用apt-get安装即可：sudo apt-get install ros-kinetic-mavros ros-kinetic-mavros-extras ros‐kinetic‐control‐ oolbox我所安装的ros版本是kinetic。其安装过程非常简单，在网上查询使用方法却说得很笼统，大多是按照官方教程，但实际上并没有详细的讲如何使用。1.
官方例程：参考：https://dev.px4.io/zh/ros/mavros_offboard.html在ROS包中创建offb_node.cpp文件（具体怎么建立ROS包，可参考ROS的Wiki教程，写得很详细），并粘贴下面内容：/**
* @file offb_node.cpp
* @brief offboard example node, written with mavros version 0.14.2, px4 flight
* stack and tested in Gazebo SITL
#include &ros/ros.h&
#include &geometry_msgs/PoseStamped.h&
#include &mavros_msgs/CommandBool.h&
#include &mavros_msgs/SetMode.h&
#include &mavros_msgs/State.h&
mavros_msgs::State current_
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *
int main(int argc, char **argv)
ros::init(argc, argv, "offb_node");
ros::NodeH
ros::Subscriber state_sub = nh.subscribe&mavros_msgs::State&
("mavros/state", 10, state_cb);
ros::Publisher local_pos_pub = nh.advertise&geometry_msgs::PoseStamped&
("mavros/setpoint_position/local", 10);
ros::ServiceClient arming_client = nh.serviceClient&mavros_msgs::CommandBool&
("mavros/cmd/arming");
ros::ServiceClient set_mode_client = nh.serviceClient&mavros_msgs::SetMode&
("mavros/set_mode");
//the setpoint publishing rate MUST be faster than 2Hz
ros::Rate rate(20.0);
// wait for FCU connection
while(ros::ok() && current_state.connected){
ros::spinOnce();
rate.sleep();
geometry_msgs::PoseS
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;
//send a few setpoints before starting
for(int i = 100; ros::ok() && i & 0; --i){
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
mavros_msgs::SetMode offb_set_
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_
arm_cmd.request.value =
ros::Time last_request = ros::Time::now();
while(ros::ok()){
if( current_state.mode != "OFFBOARD" &&
(ros::Time::now() - last_request & ros::Duration(5.0))){
if( set_mode_client.call(offb_set_mode) &&
offb_set_mode.response.mode_sent){
ROS_INFO("Offboard enabled");
last_request = ros::Time::now();
if( !current_state.armed &&
(ros::Time::now() - last_request & ros::Duration(5.0))){
if( arming_client.call(arm_cmd) &&
arm_cmd.response.success){
ROS_INFO("Vehicle armed");
last_request = ros::Time::now();
local_pos_pub.publish(pose);
ros::spinOnce();
rate.sleep();
该程序的主要功能是：使无人机缓慢飞到2米的高度。这是一段复制上去就可以直接用的程序，但想要理解深刻，需要对ROS有一定的了解，在了解后，才能看懂下面的内容。下面将对代码的每一部分进行分析，使其能够实现自己想要实现的功能。需要注意的是：对于用于发送接收的mavros的结构体变量的使用，最好还是查看mavros包中的变量定义，这有助于灵活的对代码进行调整。2.
消息的订阅和发布首先讲述一下以上程序所实现的功能。首先使飞机进入offboard模式，再进行解锁，最后发布指令使飞机飞到2米的高度。这一系列功能的实现就用到了ROS强大的发布订阅机制。可以参照这两个网页：https://404warehouse.net//autopilot-offboard-control-using-mavros-package-on-ros/这两个网站上都详细说明了mavros可发布和订阅的消息以及变量类型，举例说明：2.1 消息订阅（注意：所订阅的消息需要有回调函数去接收它）：可以看到截图中清楚地写明了订阅消息所需要地信息，那为什么这里是发布呢？这是因为这是站在mavros的角度看的呀~//回调函数
mavros_msgs::State current_
void state_cb(const mavros_msgs::State::ConstPtr& msg){
current_state = *
//消息订阅
ros::Subscriber state_sub = nh.subscribe &mavros_msgs::State& ("mavros/state", 10, state_cb); 2.2 消息发布：ros::Publisher local_pos_pub = nh.advertise&geometry_msgs::PoseStamped&
("mavros/setpoint_position/local", 10);
geometry_msgs::PoseS
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 2;
//消息发布
local_pos_pub.publish(pose);
同理，在下面截图中也有非常清楚的说明：上面的代码意思是将坐标从初始点飞到2米的高度，同时该结构体中还有另外4个给定姿态的量，就是四元数，x，y，z，w。从下面这张图可以看出：使用方法嘛~~~pose.pose.quaternion.x = 0;
pose.pose.quaternion.y = 0;
pose.pose.quaternion.z = 0;
pose.pose.quaternion.w = 0;想好你的姿态，换算成四元数复制进去就好啦~所以！mavros的维基是入手mavros控制的最好的教程，不然看其它的都没有用，当然首先还是要掌握ROS的发布订阅机制才行。发布消息的时候有一点需要注意：local_pos_pub.publish(pose);都看得出来，这一句是发布消息的。那是不是有人问，怎么一段代码里发布了这么多遍？这在上面的链接里也写得很清楚，我们所使用的pixhawk需要以至少2Hz的频率接收消息，否则将会从offboard模式切回RTL模式。开始的一段，是为了告诉pixhawk有消息要传过来啦~中间在切换状态和arm的时候，是为了保持其pixhawk接收寄存器里有值，通俗点嘛，就是既然我要用offboard模式了，你总得先告诉我要怎么搞吧，不然刚切过去那会怎么办呢？随意嘛？哈哈~那当其正常运行的时候，当然要通过发布消息来尽情的控制你的小飞机啦~3 怎么和pixhawk连接？Emm，很简单，直接用USB就可以哈哈~新打开一个终端，运行：roslaunch mavros px4.launch fcu_url:=/dev/ttyACM0;再把上面offboard的节点运行起来就可以啦~ 当然，要想搞实物，需要先仿真，下面一节我们就来讲讲仿真怎么搞~参考文献：1.
作者：zhengyuxin0507 发表于
https://blog.csdn.net/zhengyuxin0507/article/details/
阅读：165 评论：1
https://blog.csdn.net/zhengyuxin0507/article/details/
https://blog.csdn.net/zhengyuxin0507/article/details/
zhengyuxin0507
从去年开了这个博客起就一直没有写过，好在今天实验有了一些小进展，所以想分享给大家。首先讲讲我做了些什么吧。我想做一个基于单目视觉的四旋翼定点降落，能够让四旋翼从任意位置起飞之后，完全自主地降落到地面标志处。国内外有许多实验室都已经完成了这个功能，在IEEE、中国知网上也有不少论文，但基本没有人系统地写过相关中文博客。所以，我想把自己的体验分享出来，供大家交流与参考。废话不说，先上图。
以pixhawk作为飞控，odroid XU4作为图像处理上位机，摄像头用了全局快门的MT9V034（屌丝买不起贵的），地标用Apriltag也可以。但我想自己体验一下整个过程，就自主设计了一个回形地标，效果还行。由于整个系统内容太多，一篇两篇文章也讲不清楚，而且我也没有整块的时间来写（导师疯狂push项目），所以打算写个系列，慢慢写。初步拟了一个框架，以后慢慢更：
（一）概述（就是这篇）
（二）如何通过mavros控制pixhawk
（三）如何搭建基于gazebo的pixhawk仿真环境（不先仿真，肯定炸鸡一百次啊）
（四）地标的设计与识别算法
（五）位姿估算与控制算法
测试视频今天就不传上来了，一来还没剪辑好（博主不想露脸，哈哈哈），二来也是留个悬念，最后一章会传上去。希望大家能够持续关注，有问题也可以随时和博主交流。
作者：zhengyuxin0507 发表于
https://blog.csdn.net/zhengyuxin0507/article/details/
阅读：224 评论：6
https://blog.csdn.net/zhengyuxin0507/article/details/
https://blog.csdn.net/zhengyuxin0507/article/details/
zhengyuxin0507
在如今AI浪潮的时代，总觉得自己能够从事该领域的研究是一种幸运。毕竟五年前，在我选择自己专业的时候，还未料到如今的AI能够如此之火，也未料到自己能够如此热爱自己的选择。
目前，我主要在从事一些计算机视觉方面的研究，目的是想将计算机视觉中的一些理论成果应用于四旋翼无人机中。在我自己刚开始做研究的过程中，就读过不少CSDN上的博客，也因此受益匪浅。但是，随着研究内容的深入，我发现中文资料非常有限，为我这种英语底子较差的人带来了不少困难。所以，我萌生了自己写博客的想法，希望将自己的理论成果、工程经验和新奇想法都写到博客里去，也希望广大博友能够讨论交流、共同进步。
作者：zhengyuxin0507 发表于
https://blog.csdn.net/zhengyuxin0507/article/details/
阅读：33 评论：3