PX4仿真中的角度控制

书中藏着宇宙

已于 2024-04-07 21:53:38 修改

阅读量185

点赞数 1

分类专栏：无人机设计开发文章标签： c++ PX4 gazebo

于 2024-04-07 21:10:53 首次发布

本文链接：https://blog.csdn.net/m0_49586319/article/details/137478051

版权

无人机设计开发专栏收录该内容

14 篇文章 3 订阅

订阅专栏

在项目开发过程中，为了避免炸机，在无人机GAZEBO平台先进行了仿真验证，验证中发现想对无人机的姿态角控制，MAVROS某些话题不能使用，给出命令之后，无人机没有任何反应，结果查询资料后发现，确实是某些主题无人机不会进行响应：
（1） ros::Publisher local_att_pub = nh.advertise<geometry_msgs::PoseStamped> ("mavros/setpoint_attitude/attitude", 10);//设置无人机飞行角度的话题，我们开发者需要发布这个话题给无人机

这个主题根本在姿态角控制的时候就不会起作用。
(2)ros::Publisher vel_pub = nh.advertise<geometry_msgs::Twist>("/mavros/setpoint_velocity/cmd_vel_unstamped", 10);//控制无人机速度的话题，发布无人机的速度
这个在无人机YAW角度的时候起作用，发布YAM角速度，无人机确实会绕着Z轴无人机旋转，但是Pitch和Roll就不会起响应。

解决办法
使用最原始的角度控制主题：

ros::Publisher raw_pub_att = nh.advertise<mavros_msgs::AttitudeTarget> ("mavros/setpoint_raw/attitude",100);
使用这个就可以完成对三个角度的控制。

需要我们注意的就是坐标系的转换，我们控制模型使用的NED坐标系，MAVROS里面使用的是ENU坐标系，我们需要对坐标系进行一个转换。

//ned to enu
geometry_msgs::Point ned2enu_angle(geometry_msgs::Point ned_angle)
{
    geometry_msgs::Point enu_angle;
    enu_angle.x = ned_angle.y;
    enu_angle.y = ned_angle.x;
    enu_angle.z = -ned_angle.z;
    return enu_angle;
}

其次人最容易接受的就是姿态角，无人机最好传递是四元数，这样子能够避免方向锁问题。

//eluer angular  to quad
geometry_msgs::Quaternion euler_to_quat(geometry_msgs::Point euler_angle_enu)
{
    tfScalar yaw,pitch,roll;
    tf::Quaternion q;
    geometry_msgs::Quaternion quat;

    roll = euler_angle_enu.x;
    pitch = euler_angle_enu.y;
    yaw = euler_angle_enu.z;

    q.setEulerZYX(yaw,pitch,roll);
    quat.x = q.x();
    quat.y = q.y();
    quat.z = q.z();
    quat.w = q.w();
    return quat;
}

另外，发送这种类型的消息是需要给出推力值的（百分制，即0-1之间）

//calc thrust_scale，推力归一化
float thrust_scale(float thrust)
{
	float thrust_scale = 0.0;
	if (thrust>=THRUST_FULL)
	{
		
		thrust= THRUST_FULL;
	}
	 thrust_scale = thrust/ THRUST_FULL;
}

最后将信息发布出去即可完成无人机的姿态调整：

        	ned_euler.x = 0;//control_input_z;//以z轴的控制输出为参数
        	ned_euler.y = 0;
        	ned_euler.z = 0;
        	enu_euler = ned2enu_angle(ned_euler);
		    thr = thrust_scale(71.12);
		     q = euler_to_quat(enu_euler);
			raw_att.thrust=thr;
        	raw_att.orientation.x=q.x;
        	raw_att.orientation.y=q.y;
        	raw_att.orientation.z=q.z;
        	raw_att.orientation.w=q.w;
  			raw_pub_att.publish(raw_att);//发送角度消息