RM调车笔记

调一整台车(以四号步兵为例)

调车前提:确保目前车上代码与主仓一致,包括rm_bringup、rm_control、rm_controller、rm_manual。.bashrc配置无误。

  1. 检查硬件连接及映射

  • can初始化是否成功、是否有数据收发(USB2xxx上外侧灯闪烁表示NUC在接收数据,内侧灯闪烁表示NUC在发送数据)。

  • 保证rm_hw中can的id和电机/电调一致(6020的id调电机上的拨码,3508的id调C620电调,2006的id调C610电调)。

  • 保证裁判系统串口连接正常,如果没有串口,自启服务开始后就不能进行自启校准,不过可以手动校准代替。

  1. 检查URDF文件

  • 将pitch的上下软限位调至无穷大和无穷小(1e10和-1e10)。

  • 调offset:

将yaw和pitch的offset置零(在transmission里)。

运行rm_hw.launch和load_controller.launch。

打开rqt,打开joint_state_controller,校准pitch(如果pitch关节用的是6020就不用校准)。

将枪管调整到正对前方且水平,查看pitch和yaw此时的joint_state,取负后作为offset。

  • 调TF:

打开joint_state_controller、robot_state_controller、chassis_controller、gimbal_controller,通过rviz看模型是否正确、TF方向是否正确。

重点关注gimbal_imu,要与imu电路板上的丝印一致。软板朝下安装时,Z轴朝下,Y轴朝前。

如果发现错了,修改standard5.urdf.xacro中的rpy(欧拉角)。

<xacro:IMU connected_to="$(arg camera_optical_frame)" imu_name="gimbal_imu"
             xyz="0.00246956 0.04105199 -0.07160856"
             rpy="-1.5487827 3.1332114 0"/>

有时候校准方向反了,取反传动比的符号。

  • 调云台PID:

位置环Kp控制(可以有Ki消除静差),速度环Kp、Kd控制。

增大Kp可以让输出以更快的速度到达预期值(响应更快,反馈力更大),增大Kd可以增大阻尼力,消除大幅抖动,但同时也有放大噪声的特点(微微抖动的一般是被放大的噪声)。

先把PID都置零,给d一个值。

打开plotjuggler,开启想调PID的对应的控制器,让关节有力。用手扭动关节到别的位置后再松开,期间查看图像,防止超调,调整d直到关节回正时并不发生摇晃、抖动。

调好Kd后调Kp。调Kp到关节具有合适的响应速度。期间Kd可能也要进行调整。

  • 调软限位:

打开joint_state_controller,校准后,将枪管的pitch分别转动至最上、最下硬限位,记下joint_state后,作为软限位的limit。

适当调整threshold。

打开robot_state_controller、chassis_controller、gimbal_controller。云台有力后,看软限位是否生效。

有时候软限位不能正常生效,很可能是k_position、k_velocity取值过小导致力矩不够的问题。

  1. 解决零飘

  • 把车架起来,运行start.launch,开遥控器。疯车说明上述步骤出了问题。若不疯车就检查自启是否有问题。

  • 求angular_vel_offset:

由于一些车没有相机触发,导致我们不能看到imu的解算信息。可以加载imu_sensor_controller。

install好imu_sensor_controller后,在运行rm_hw.launch的情况下mon launch imu_sensor_controller imu_sensor_controller。

在plotjuggler中拖出x、y、z,在函数处理中选择平均值,取样点拉到最大。等到曲线较为稳定,读曲线中线,取反后作为angular_vel_offset。

  • 如果搞完angular_vel_offset后还是飘,查看dbus话题,可能是遥控器需要校准。

奇奇怪怪

  1. 有时候pitch校准不成功,却会影响到cover。是代码逻辑的原因。

  1. 偶尔用遥控器操纵车时,车会突然不受遥控器控制,电机卸力,重新开启遥控器后又恢复正常。可能是因为进入了IDLE状态。

  • 1
    点赞
  • 5
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值