任务目标：通过风机叶片，识别风机正面
条件：无人机对准风机柱正面，正在直线上升，无人机不会撞到叶片。
解决：雷达前方数据见图1,。
原理：
在理想情况下：无人机垂直上升，风机柱子为圆柱，那黑色区域应为固定值。
上升：
但实际中：并不存在，柱子为圆锥，下粗上窄，在一定范围内波动。
使用方差衡量黑色区域。方差数学定义自己搜索。我的方差经不断调整最短距离测试在（0.32~0.55）之间。
//上升到雷达前方出现叶片
//1：到达预订点
//0：没到预订点
//首次调用Fly_To_Point_CrossTrack_XY这个函数时往偶尔会使方差很大，使init_high与当前距离之差删除该点
bool get_stop_points(float init_high)
{
bool get_average=get_lanser_average();//得到均差
bool get_variance=QuadrotorControl::get_lanser_variance();//得到方差
if(!(get_average&&get_variance))
{
return 0;
}
if((Stanard_Deviation>0.8)&&((UAV_Pose_.position.z-init_high)>0.1))
{
return 1;
}
return 0;
}
//获取有效雷达数据均值
//返回值1,更新了均值
//放回值0,没有更新均值
bool get_lanser_average()
{
int i=0;
float data_num=0;//数据长度之和
Data_len=0;//数据长度清零
int lanser_len = laser_scan_range_data.size();//雷达数据长度
for (i=0;i<lanser_len;i++)
{
if(!(std::isinf(laser_scan_range_data[i])|(std::isinf(laser_scan_range_data[i]))))//删除inf和nan
{
data_num+=laser_scan_range_data[i];
Data_len++;
}
}
if(Data_len==0)//没有有效值
{
Lanser_Average=0.0;
return 0;
}
Lanser_Average=float(data_num/Data_len);
return 1;
}
//获取雷达有效数据方差，更新雷达方差
//返回值1,更新了方差
//放回值0,没有更新方差
bool get_lanser_variance()
{
float variance_num=0;//数据方差之和
int lanser_len = laser_scan_range_data.size();//雷达数据长度
float average_average=Lanser_Average*Lanser_Average; //平均值的平方
if(!Lanser_Average)//没有有效值
{
Stanard_Deviation=0.0;//方差维0
return 0;
}
for (int i=0;i<lanser_len;i++)
{
if(!(std::isinf(laser_scan_range_data[i])|(std::isnan(laser_scan_range_data[i]))))//删除inf和nan
{
variance_num+=laser_scan_range_data[i]*laser_scan_range_data[i]-average_average;
}
}
Stanard_Deviation=sqrtf(variance_num/Data_len);//计算标准差
return 1;
}
上升到了预期位置见下图：
解决办法：继续上升
其他：获取可以试试左右旋转解决
ranges: [inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf,
21.022348403930664, 20.734376907348633, 20.594545364379883, 20.45680809020996, 20.329580307006836, 20.26622200012207, 20.20348358154297, 20.141340255737305, 20.07979965209961, 20.01884651184082, 19.95847511291504, 19.929994583129883, 19.903257369995117, 19.87681007385254, 19.85064125061035, 19.824756622314453, 19.799150466918945, 19.77382469177246, 19.748802185058594, 19.74776268005371, 19.746932983398438, 19.746322631835938, 19.745927810668945, 19.74574089050293, 19.745763778686523, 19.74600601196289, 19.7464542388916, 19.75636863708496, 19.78097915649414, 19.80586051940918, 19.831018447875977, 19.856454849243164, 19.882179260253906, 19.908174514770508, 19.934457778930664, 19.974472045898438, 20.0341854095459, 20.094472885131836, 20.155344009399414, 20.216796875, 20.278852462768555, 20.355878829956055, 20.489744186401367, 20.6256103515625, 20.76351547241211, 21.179107666015625,
inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf,
17.929794311523438, 17.701831817626953, 17.51659393310547, 17.435190200805664, 17.35926055908203, 17.34989356994629, 17.340688705444336, 17.331701278686523
]
情况2：扫到了两边叶片，但不包含全部数据
解决办法：继续上升
其他：暂无
[18.282087326049805, 18.268369674682617, 18.254873275756836, 18.27286148071289, 18.301313400268555, 18.330053329467773, 18.359081268310547, 18.36533546447754, 18.366357803344727, 18.36757469177246, 18.374107360839844, 18.467899322509766, 18.56285858154297, 18.62345314025879, 18.647375106811523, 18.671545028686523, 18.70071792602539, 18.734691619873047, 18.816043853759766,
inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf,
21.132476806640625, 20.75991439819336, 20.619718551635742, 20.48162269592285, 20.3455810546875, 20.28126335144043, 20.218408584594727, 20.156160354614258, 20.09450340270996, 20.0334415435791, 19.972963333129883, 19.941129684448242, 19.91433334350586, 19.887826919555664, 19.86160659790039, 19.835668563842773, 19.810009002685547, 19.784629821777344, 19.7595272064209, 19.757049560546875, 19.756195068359375, 19.755550384521484, 19.75511360168457, 19.754894256591797, 19.7548828125, 19.755088806152344, 19.755504608154297, 19.76498031616211, 19.78955841064453, 19.814411163330078, 19.83954429626465, 19.864952087402344, 19.890642166137695, 19.916610717773438, 19.942861557006836, 19.983510971069336, 20.04318618774414, 20.103435516357422, 20.16426658630371, 20.225685119628906, 20.28769874572754, 20.367919921875, 20.50168228149414, 20.637449264526367, 20.775239944458008, 21.219213485717773,
inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf,
17.43203353881836, 17.259769439697266, 17.03044891357422, 16.987470626831055, 16.944913864135742, 16.891971588134766, 16.820775985717773, 16.806800842285156, 16.79304313659668, 16.77947425842285, 16.766117095947266, 16.752887725830078, 16.739839553833008, 16.72701072692871, 16.71436882019043, 16.701936721801758, 16.692359924316406, 16.696548461914062, 16.70092010498047, 16.705476760864258, 16.710216522216797, 16.71513557434082, 16.720237731933594, 16.725181579589844, 16.729660034179688, 16.73432159423828, 16.739171981811523, 16.755773544311523, 16.776721954345703, 16.797903060913086, 16.819320678710938, 16.840972900390625, 16.862863540649414, 16.884992599487305, 16.90736198425293, 16.929975509643555
]
情况3：雷达扫到了全部数据
ranges: [inf, inf, inf, inf, inf,
18.407909393310547, 18.27585792541504, 18.14565658569336, 18.01958656311035, 17.95978546142578, 17.9000244140625, 17.83827781677246, 17.77714729309082, 17.71662712097168, 17.656709671020508, 17.610198974609375, 17.578540802001953, 17.544618606567383, 17.50863265991211, 17.472984313964844, 17.437664031982422, 17.402679443359375, 17.36802101135254, 17.33367919921875, 17.299665451049805, 17.29265022277832, 17.271665573120117, 17.250911712646484, 17.230396270751953, 17.21011734008789, 17.1900691986084, 17.170249938964844, 17.150663375854492, 17.13213539123535, 17.116241455078125, 17.10019302368164, 17.09554100036621, 17.09316062927246, 17.090972900390625, 17.08896827697754, 17.087148666381836, 17.08551025390625, 17.08386993408203, 17.081729888916016, 17.079774856567383, 17.0780029296875, 17.079696655273438, 17.094219207763672, 17.108945846557617, 17.12388801574707, 17.138132095336914, 17.152528762817383, 17.16713523864746, 17.18195152282715, 17.196975708007812, 17.21312141418457, 17.25506019592285, 17.296844482421875, 17.338998794555664, 17.381559371948242, 17.424509048461914, 17.46753692626953, 17.57590103149414, 17.713043212890625,
inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf,
20.841196060180664, 20.701568603515625, 20.564023971557617, 20.428508758544922, 20.331628799438477, 20.269006729125977, 20.206989288330078, 20.14556884765625, 20.084732055664062, 20.02448081970215, 19.9785099029541, 19.9519100189209, 19.925594329833984, 19.899559020996094, 19.873809814453125, 19.84833526611328, 19.82314109802246, 19.798227310180664, 19.786945343017578, 19.786291122436523, 19.785846710205078, 19.785615921020508, 19.785600662231445, 19.785797119140625, 19.786205291748047, 19.78683090209961, 19.78941535949707, 19.814260482788086, 19.839378356933594, 19.864774703979492, 19.89045524597168, 19.916410446166992, 19.942655563354492, 19.969181060791016, 20.0031795501709, 20.063291549682617, 20.123981475830078, 20.185258865356445, 20.247129440307617, 20.309598922729492, 20.38094139099121, 20.515846252441406, 20.65277671813965, 20.791770935058594, 21.207950592041016,
inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf,
16.90172576904297, 16.575122833251953, 16.517133712768555, 16.46098518371582, 16.405424118041992, 16.350364685058594, 16.295867919921875, 16.248231887817383, 16.226200103759766, 16.20450210571289, 16.183006286621094, 16.161773681640625, 16.143199920654297, 16.125093460083008, 16.107213973999023, 16.089540481567383, 16.0714111328125, 16.052976608276367, 16.045772552490234, 16.042098999023438, 16.038597106933594, 16.035274505615234, 16.032119750976562, 16.029146194458008, 16.026296615600586, 16.02313804626465, 16.02016258239746, 16.017356872558594, 16.01472282409668, 16.012266159057617, 16.00998306274414, 16.00947380065918, 16.017786026000977, 16.026283264160156, 16.034957885742188, 16.043821334838867, 16.0532169342041, 16.062801361083984, 16.072566986083984, 16.0825252532959, 16.09266471862793, 16.102989196777344, 16.11350440979004, 16.124208450317383, 16.13610076904297, 16.17719268798828, 16.202373504638672, 16.22780990600586, 16.253498077392578, 16.27944564819336, 16.305654525756836, 16.332120895385742, 16.35885238647461, 16.385845184326172, 16.413103103637695, 16.444324493408203, 16.480642318725586, 16.52571678161621, 16.57122039794922, 16.61715316772461, 16.66352081298828, 16.7103271484375, 16.895341873168945, 16.991004943847656, 17.087955474853516,
inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf, inf]
代码如下：针对情况3
实现雷达的数据分割（标记有效数据的起始与终止标签）
//删除inf，保留有效数据，分成三组，分别代表左边叶片，灯柱，右边叶片，利用这三组数据求取雷达的正方向
//分割数据，0：前方没有物体
//1：雷达中含有两个叶片数据和风机柱
//2：一个叶片和风机柱，
//3：识别到了1个物体，风机柱、左边叶片、右边叶片
//4：识别物体过多
int QuadrotorControl::Split_laser_data()
{
int start,end;
int len = laser_scan_range_data.size();//求向量的长度
mean_fengji_1=0;
mean_fengji_2=0;
mean_fengji_3=0;
yaw_fengji_1=0;
yaw_fengji_2=0;
yaw_fengji_2=0;
fengji__laser_zhu.clear();//存储有效数据的起始与终止标签
for(int i=0;i<len;i++)//切割数组
{
if(fengji__laser_zhu.size()%2==0)//起始点
{
if(!std::isinf(laser_scan_range_data[i]))
{fengji__laser_zhu.push_back(i);
}
}
if(fengji__laser_zhu.size()%2==1)//终止点
{
if((i+1)==len)
{
fengji__laser_zhu.push_back(i);
break;
}
if(std::isinf(laser_scan_range_data[i]))
{fengji__laser_zhu.push_back(i-1);
}
}
}
if(fengji__laser_zhu.size()<3)
{
ROS_INFO("laser only distinguish wick or less");
return 3;
}
if(fengji__laser_zhu.size()<6)
{
return 2;
}
if(fengji__laser_zhu.size()>7)
{
ROS_INFO("there my be something in range of laser");
return 4;
}
}
ROS_INFO("get all data");
//Get_vert();
return 1;
}
//判断读取的雷达数据是否满足要求，
求取风机正方向角度与无人机的夹角（小于90度）
bool QuadrotorControl::Control_uav_get_vert()
{
float long_distance_1=laser_scan_range_data[fengji__laser_zhu[1]];
float long_distance_2=laser_scan_range_data[fengji__laser_zhu[4]];
ROS_INFO("FLAG_1:%d;FLAG_2:%d,laser_scan_range_data[fengji__laser_zhu[1]]:%f,laser_scan_range_data[fengji__laser_zhu[4]]:%f",
fengji__laser_zhu[1],fengji__laser_zhu[4],laser_scan_range_data[fengji__laser_zhu[1]],laser_scan_range_data[fengji__laser_zhu[4]]);
yaw_fengji_1=-fengji__laser_zhu[1]*abs(laser_scan_angle_increment)+abs(laser_scan_range_min);//求得风机左边叶片的角度
yaw_fengji_2=-fengji__laser_zhu[4]*abs(laser_scan_angle_increment)+abs(laser_scan_range_min);;//求得风机右边叶片角度
ROS_INFO("FLAG_1:%f;FLAG_2:%f",yaw_fengji_1,yaw_fengji_2);
D_angle=Get_vert(long_distance_1,yaw_fengji_1,long_distance_2,yaw_fengji_2);
ROS_INFO("UAV_yaw_:%f",UAV_yaw_);
return 1;
}
//求得两个风机叶片构成直线的斜率
//在求的该直线的垂直角度，这个垂直角度就是无人机与风机正面的夹角
//取左边叶片最右边一点，与右边叶片最左边一点
//返回值：无人机yaw角相对于机体坐标系
float QuadrotorControl::Get_vert(float r1,float yaw1,float r2,float yaw2)
{
float _k,_y,_x;//_k 斜率 _y y轴方向差值 _x x轴方向差值
float target_lanser_angle=0;//该角度相对于雷达
float target_angle=0;//相对于大地坐标系
_x=r1*cos(yaw1)-r2*cos(yaw2);
_y=r1*sin(yaw1)-r2*sin(yaw2);
_k=_y/_x;//求斜率
ROS_INFO("_k:%f",_k);
target_lanser_angle=atan(-1/_k)/DEG2RAD;//算出垂直方向角度
//ROS_INFO("target_lanser_angle:%f",target_lanser_angle);
target_angle=target_lanser_angle+abs(laser_scan_angle_max);
D_angle=target_angle;
//ROS_INFO("r1:%f,yaw1:%f,r2:%f,yaw2:%f",r1,yaw1,r2,yaw2);
return target_angle;
}
利用求得的夹角，计算飞机正方向的位置，并让飞机飞到目标位置,转到目标角度，多次循环直到夹角满足条件或者雷达数据不满足条件。
//radius 飞机距离机柱距离 yaw 计算出的当前yaw与目标的yaw角之差
bool QuadrotorControl::run_target_position(float radius,float yaw)
{
const int run_init=0;//函数功能初始化
const int run_calculation_center_of_circle_position = 1 ;//计算 圆点位置
const int run_calculation_target_position = 2 ; // 计算目标 的位置
const int run_reach_target_position =3 ; // 控制无人机走向的位置
const int run_turn_target_poster =4 ; // 旋转
const int run_finish = 5 ; // 函数结束部分 恢复各初始变量
static int function_run_state_1 = run_init ; //函数状态控制
static float uav_yaw_get_point;//标记计算出风机正方向时的yaw角
static float uav_yaw_high;//风机下降的高度
float increase_speed;//选转角度的增加速度
float act_error;// 目标角度与当前角度之差
if(std::isinf(radius))
{
uav_stop();
ROS_INFO("the radius is not empty");
return 1;
}
//函数主状态机
switch( function_run_state_1 )
{
case run_init://进行初始化
{
// cricle_yaw_controller_.Set_PID_Gain(1,0,0);
PID_YAW.Init(0.001,0.2,200,0.05,0.02);
uav_yaw_get_point=UAV_yaw_*DEG2RAD;
uav_yaw_high=UAV_Pose_.position.z*0.5;//下降目标高度
function_run_state_1 = run_calculation_target_position;//设置状态机
ROS_INFO("init");
break;
}
case run_calculation_target_position://计算目标位置
{
init_target_postion.position.y = UAV_Pose_.position.y-radius+radius*cos(yaw);
init_target_postion.position.x = UAV_Pose_.position.x+radius*sin(yaw);
if(yaw>0)//yaw 范围在-90 到 90度
{init_target_postion.position.x = UAV_Pose_.position.x-radius*sin(yaw);
init_target_postion.position.y = UAV_Pose_.position.y+radius-radius*cos(yaw);
};
init_target_postion.position.z = UAV_Pose_.position.z ;
function_run_state_1 = run_reach_target_position;//设置状态机
ROS_INFO("x1:%f,y1:%f,z1:%f",UAV_Pose_.position.x,UAV_Pose_.position.y,UAV_Pose_.position.z);
ROS_INFO("x1:%f,y1:%f,z1:%f",init_target_postion.position.x,init_target_postion.position.y,init_target_postion.position.z);
break;
}
case run_reach_target_position://走向目标位置
{
ROS_INFO("running to target fengji front_points:function_run_state:%d",function_run_state_1);
if(Fly_To_Point_CrossTrack_XY( UAV_Pose_.position.x , UAV_Pose_.position.y,UAV_Pose_.position.z ,
init_target_postion.position.x ,init_target_postion.position.y ,UAV_Pose_.position.z ))
{
function_run_state_1=run_turn_target_poster;//结束
ROS_INFO("has run to target position");
}
break;
}
case run_turn_target_poster:
{
ROS_INFO("turnning to target angle:%f,now:%f",uav_yaw_get_point/DEG2RAD-yaw/DEG2RAD,UAV_yaw_);
act_error=uav_yaw_get_point/DEG2RAD-yaw/DEG2RAD-UAV_yaw_;
increase_speed=PID_YAW.Update(0,act_error);
uav_turn_yaw(-5*increase_speed);
function_run_state_1=run_init;
if(abs(act_error)<4)
{
ROS_INFO("has to target angle");
uav_stop();
function_run_state_1=run_finish;
}
break;
}
case run_finish:
{
if(Split_laser_data()==1)//判断是否满足两个叶片在雷达范围内，并读取到正前方数据
{
Control_uav_get_vert();
if(abs(D_angle)<4)
{
ROS_INFO("D_angle:%f",D_angle);
function_run_state_1=run_init;
yaw=D_angle;//更新yaw值
return 1;
}
ROS_INFO("D_angle:%f",D_angle);
function_run_state_1=run_init;
yaw=D_angle;//更新yaw值
}
else
{
ROS_INFO("cann't get closer");
function_run_state_1=run_init;
return 0;
}
break;
}
}
}
问题与解决方法
1：运用雷达的分割时，默认是识别得到，两个叶片和风机柱的，其他条件没考率，包括识别两个物体时，一个物体，多余三个物体这些部分非理想条件均为实现。
2：计算风机的正方向的角度，各取两个叶片最靠近风机柱的点，这点肯定会有误差，解决方法在移动之后，不断计算风机正方向与风机夹角，不断调整，但会往往遇到雷达数据识别物体数量过少，该问题本质上又变成了问题1
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
