陆空无人机实现地面行走模式

最近教研室在做陆空无人机,飞行模式已经测试完成了,硬件使用了cuav雷讯家的飞行控制器nora+,机载计算机使用的是英伟达NXGPU开发板,无人机使用的自主研发的六涵道无人机(加装了行走的轮子),无人机的位置是通过动捕系统mocap给的。下面记录了调试过程。(我主要从两种控制模式调试:一是,位置控制模式。二是:速度控制模式)
记录使用offboard控制陆飞无人机

无人机车模式下的测试

  1. 打开mavros
roslaunch mavros px4.launch 
  1. QGC端参数配置EKF2:使用视觉

    EKF2_AID_MASK 设置为24(即勾选vision position fusion and vision yaw fusion)
    EKF2_HGT_MODE 设置为VISION (默认为气压计)
    
  2. 检查mavros/vision_pose/pose出的位置信息是否正确

rostopic echo /mavros/vision_pose/pose
  1. 切换无人机为小车模式(1300为小车模式,1700为飞行模式)

    rosrun mavros mavcmd int 183 1 1300 0 0 0 0 0  
    
  2. 发送速度控制信息 (把下面的指令复制到终端,然后TAB键两下,就会把命令补全,可以设置线速度x的值1.0 其他值全为0)

    rostopic pub -r 20 /mavros/setpoint_velocity/cmd_vel_unstamped geometry_msgs/Twist "linear:
    

    我发送的完整命令是

    slam@arm:~$ rostopic pub -r 20 /mavros/setpoint_velocity/cmd_vel_unstamped geometry_msgs/Twist "linear:
      x: 1.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0" 
    
    1. 第五条可以更改成:/mavros/setpoint_raw/local可以实现位置、速度与加速度控制(加速度控制方法PX4实现不是很好)

      rostopic pub -r 20 /mavros/setpoint_raw/local mavros_msgs/PositionTarget "header:
      

      参数配置:

        seq: 0
        stamp: {secs: 0, nsecs: 0}
        frame_id: ''
      coordinate_frame: 8    #选用机体坐标系
      type_mask: 455  #禁用位置控制 与禁用加速度控制   启动了速度控制
      position: {x: 0.0, y: 0.0, z: 0.0}
      velocity: {x: 0.2, y: 0.0, z: 0.0}
      acceleration_or_force: {x: 0.0, y: 0.0, z: 0.0}
      yaw: 0.0
      yaw_rate: 0.0" 
      
    2. 发送速度控制信息代码方式(还没测试,暂时跳过)

      ros::Publisher velocity_pub = nh.advertise<geometry_msgs::TwistStamped>("mavros/setpoint_velocity/cmd_vel", 10);
       
      geometry_msgs::Twist twist;
      geometry_msgs::Vector3 linear;
      linear.x=1;
      linear.y=0;
      linear.z=0;
      geometry_msgs::Vector3 angular;
      angular.x=0;
      angular.y=0;
      angular.z=0;
         
      twist.linear=linear;
      twist.angular=angular;
       
      geometry_msgs::TwistStamped speed;
      speed.twist = twist;
      velocity_pub.publish(speed);
      

    为/opt/ros/melodic/share/mavros/launch下的px4_config.yaml文件,将setpoint_velocity的mav_frame修改为BODY_NED,如下所示

    # setpoint_velocity
    setpoint_velocity:
      mav_frame: BODY_NED
    
    1. 打印出无人机的状态:可以看到,刚发送了速度命令,无人机就会进入offboard模式

      rostopic echo /mavros/state
      
  3. 如果无人机没有进入offboard模式,可以使用下面的命令进入,当timeout 可以再次尝试进

    rosrun mavros mavsys mode -c OFFBOARD
    

8.解锁:就可以看见小车开始按照预定方向行走了

rosrun mavros mavsafety arm
  • 0
    点赞
  • 2
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值