@1257261547 89793abb-de57-486a-836b-49d3d0d37589-图片.png

code_text main.cpp #include <Arduino.h> #include <Esp32PcntEncoder.h> #include <Esp32McpwmMotor.h> #include <PidController.h> #include <Kinematics.h> #include <Warning.h> #include <ImuDriver.h> #include <WiFi.h> #include <micro_ros_platformio.h> #include <rcl/rcl.h> #include <rclc/rclc.h> #include <rclc/executor.h> #include <geometry_msgs/msg/twist.h> #include <nav_msgs/msg/odometry.h> #include <micro_ros_utilities/string_utilities.h> #include <sensor_msgs/msg/imu.h> rcl_allocator_t allocator; rclc_support_t support; rclc_executor_t executor; rcl_node_t node; rcl_subscription_t sub_cmd_vel; geometry_msgs__msg__Twist msg_cmd_vel; rcl_publisher_t pub_odom; nav_msgs__msg__Odometry msg_odom; rcl_timer_t timer; rcl_publisher_t pub_imu; sensor_msgs__msg__Imu msg_imu; Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器 Esp32McpwmMotor motor; // 创建一个名为motor的对象，用于控制电机 PidController pid_controller[2]; Kinematics kinematics; MPU6050 mpu(Wire); // 初始化MPU6050对象 ImuDriver imu(mpu); // 初始化Imu对象 imu_t imu_data; // IMU 数据对象 float target_linear_speed = 20.0; // 单位 毫米每秒 float target_angular_speed = 0.1; // 单位 弧度每秒 float out_left_speed = 0.0; // 输出的左右轮速度，不是反馈的左右轮速度 float out_right_speed = 0.0; void start() { pinMode(TRIG, OUTPUT); pinMode(ECHO, INPUT); pinMode(WARN, OUTPUT); } void timer_callback(rcl_timer_t* timer,int64_t last_call_time) { odom_t odom = kinematics.get_odom(); int64_t stamp = rmw_uros_epoch_millis(); msg_odom.header.stamp.sec = static_cast<int32_t>(stamp/1000); msg_odom.header.stamp.nanosec = static_cast<int32_t>((stamp%1000)*1e6); msg_odom.pose.pose.position.x = odom.x; msg_odom.pose.pose.position.y = odom.y; msg_odom.pose.pose.orientation.w = cos(odom.angle*0.5); msg_odom.pose.pose.orientation.x = 0; msg_odom.pose.pose.orientation.y = 0; msg_odom.pose.pose.orientation.z = sin(odom.angle*0.5); msg_odom.twist.twist.linear.x = odom.linear_speed; msg_odom.twist.twist.angular.z = odom.angular_speed; imu.getImuDriverData(imu_data); msg_imu.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 秒部分 msg_imu.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒部分 msg_imu.angular_velocity.x = imu_data.angular_velocity.x; msg_imu.angular_velocity.y = imu_data.angular_velocity.y; msg_imu.angular_velocity.z = imu_data.angular_velocity.z; msg_imu.linear_acceleration.x = imu_data.linear_acceleration.x; msg_imu.linear_acceleration.y = imu_data.linear_acceleration.y; msg_imu.linear_acceleration.z = imu_data.linear_acceleration.z; msg_imu.orientation.x = imu_data.orientation.x; msg_imu.orientation.y = imu_data.orientation.y; msg_imu.orientation.z = imu_data.orientation.z; msg_imu.orientation.w = imu_data.orientation.w; if(rcl_publish(&pub_odom,&msg_odom,NULL)!=RCL_RET_OK) { Serial.printf("error: odom pub failed!"); } if(rcl_publish(&pub_imu, &msg_imu, NULL)!=RCL_RET_OK) { Serial.printf("error: imu pub failed!"); } } void twist_callback(const void * msg_in) { const geometry_msgs__msg__Twist* msg = (const geometry_msgs__msg__Twist*)msg_in; target_linear_speed = msg->linear.x*1000; target_angular_speed = msg->angular.z; kinematics.kinematics_inverse(target_linear_speed, target_angular_speed, &out_left_speed, &out_right_speed); Serial.printf("OUT:left_speed=%f,right_speed=%f\n", out_left_speed, out_right_speed); pid_controller[0].update_target(out_left_speed); pid_controller[1].update_target(out_right_speed); } void microros_task(void* args) { IPAddress agent_ip; agent_ip.fromString("192.168.188.93"); set_microros_wifi_transports("xbw_mi","Xbw159357",agent_ip,8888); delay(2000); allocator = rcl_get_default_allocator(); rclc_support_init(&support,0,NULL,&allocator); rclc_node_init_default(&node,"example_xbwbot_motion_control","",&support); unsigned int num_handles = 3; rclc_executor_init(&executor,&support.context,num_handles,&allocator); rclc_subscription_init_best_effort(&sub_cmd_vel,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs,msg,Twist),"/cmd_vel"); rclc_executor_add_subscription(&executor,&sub_cmd_vel,&msg_cmd_vel,&twist_callback,ON_NEW_DATA); msg_odom.header.frame_id = micro_ros_string_utilities_set(msg_odom.header.frame_id,"odom"); msg_odom.child_frame_id = micro_ros_string_utilities_set(msg_odom.child_frame_id,"base_footprint"); msg_imu.header.frame_id = micro_ros_string_utilities_set(msg_imu.header.frame_id,"imu"); rclc_publisher_init_best_effort(&pub_odom,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs,msg,Odometry),"/odom"); rclc_publisher_init_best_effort(&pub_imu,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu),"/imu"); rclc_timer_init_default(&timer,&support,RCL_MS_TO_NS(50),timer_callback); rclc_executor_add_timer(&executor,&timer); while(!rmw_uros_epoch_synchronized()) { rmw_uros_sync_session(1000); delay(10); } rclc_executor_spin(&executor); } // v=wr r = v/w void setup() { // 初始化串口 Serial.begin(115200); // 初始化串口通信，设置通信速率为115200 // 初始化电机驱动器2 encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接 encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接 // 初始化PID控制器的参数 pid_controller[0].update_pid(0.625, 0.125, 0.0); pid_controller[1].update_pid(0.625, 0.125, 0.0); pid_controller[0].out_limit(-100, 100); pid_controller[1].out_limit(-100, 100); // 初始化运动学参数 kinematics.set_wheel_distance(175); // mm kinematics.set_motor_param(0, 0.103657); kinematics.set_motor_param(1, 0.103657); // 测试下运动学逆解 xTaskCreate(microros_task,"microros_task",10240,NULL,1,NULL); start(); imu.begin(18, 19); } void loop() { digitalWrite(TRIG, HIGH); delayMicroseconds(10); digitalWrite(TRIG, LOW); double delta_time = pulseIn(ECHO, HIGH); float detect_distance = delta_time * 0.0343 / 2; if(detect_distance<50) { digitalWrite(WARN,HIGH); } else { digitalWrite(WARN,LOW); } delayMicroseconds(9990); kinematics.update_motor_speed(millis(), encoders[0].getTicks(), encoders[1].getTicks()); motor.updateMotorSpeed(0, pid_controller[0].update(kinematics.get_motor_speed(0))); motor.updateMotorSpeed(1, pid_controller[1].update(kinematics.get_motor_speed(1))); // Serial.printf("speed1=%d,speed2=%d\n",kinematics.get_motor_speed(0),kinematics.get_motor_speed(1)); // Serial.printf("x,y,yaw=%f,%f,%f\n",kinematics.get_odom().x,kinematics.get_odom().y,kinematics.get_odom().angle); imu.update(); }

我使用的main.cpp，PidController.h和PidController.h与第九章的代码相同，ImuDriver.h和.cpp是从https://github.dev/fishros/fishbot_motion_control_microros复制过来的。

小车旋转时，base_footprint与实际旋转角度偏差过大，使用EKF（扩展卡尔曼滤波）融合odom（里程计）和IMU（惯性测量单元）的数据时，通过rviz观察tf坐标发现手柄控制小车旋转90度而rviz里面的坐标实际旋转了120度左右，这种差距可能由以下多种原因引起：

一、传感器数据误差

IMU误差：

IMU提供的角速度和加速度数据存在噪声和漂移，这可能导致积分后的角度数据存在误差。 IMU的校准不准确或安装位置不当也可能导致数据偏差。

里程计误差：

里程计数据可能受到轮子打滑、地面不平整等因素的影响，导致数据不准确。 里程计的校准和参数设置（如轮子直径、轮距等）也可能影响数据的准确性。 二、EKF融合算法问题

算法参数设置不当：

EKF算法的参数（如噪声协方差矩阵、状态转移矩阵等）设置不准确，可能导致融合后的数据存在偏差。

算法实现问题：

EKF算法的实现可能存在bug或优化不足，导致数据融合效果不佳。 三、tf坐标变换问题

tf树结构问题：

tf树结构可能存在问题，如缺少必要的坐标变换节点或坐标变换关系设置错误。

时间戳不匹配：

不同传感器数据的时间戳不匹配，可能导致tf变换时的数据不一致。 四、硬件和软件问题

硬件延迟：

传感器和处理器之间的硬件延迟可能导致数据不同步，从而影响角度计算的准确性。

软件问题：

控制系统或数据处理软件的bug可能导致数据处理错误或延迟。 五、其他因素

外部环境干扰：

外部磁场干扰、温度变化等因素可能影响IMU的性能，导致数据偏差。

控制手柄精度：

控制手柄的精度不足或存在漂移，也可能导致输入指令与实际旋转角度存在偏差。

为了解决这个问题，可以采取以下措施：

优化传感器校准和安装：确保IMU和里程计等传感器经过准确校准，并安装在合适的位置以减少误差。 调整EKF算法参数：根据实际应用场景调整EKF算法的参数，以提高数据融合的准确性。 检查tf树结构：确保tf树结构正确，所有必要的坐标变换节点和关系都已正确设置。 同步传感器数据：确保不同传感器数据的时间戳匹配，以减少数据不一致的影响。 优化硬件和软件：减少硬件延迟，修复控制系统或数据处理软件中的bug。 考虑外部环境因素：在可能的情况下，减少外部环境对传感器性能的干扰。 提高控制手柄精度：选择高精度、低漂移的控制手柄，以确保输入指令的准确性。

综上所述，小车旋转时base_footprint与实际旋转角度偏差过大的问题可能由多种因素引起，需要综合考虑传感器数据误差、EKF融合算法问题、tf坐标变换问题、硬件和软件问题以及其他外部因素，并采取相应的措施进行解决。

以上为生成回答，仅供参考～

@chenxz 乌龟的屁股——规定

在部署cartographer与vanjee_WLR720_16 3DSLAM系统时，遇到坐标系无法对齐和雷达位置漂移的问题，通常涉及多个方面的配置和调试。以下是一些可能的解决步骤和检查点，供您参考：

1. 检查硬件连接与驱动 确保雷达与IMU正确连接：检查雷达和IMU的物理连接是否牢固，数据传输线是否无损坏。 验证驱动安装：确保vanjee_WLR720_16的ROS驱动已正确安装，并且能够在ROS环境中正常启动和运行。 2. 坐标系配置 检查TF树：在ROS中，使用rosrun tf view_frames命令生成TF树的可视化文件，检查各坐标系（如雷达坐标系、IMU坐标系、机器人基坐标系等）之间的转换关系是否正确。 配置cartographer的坐标系参数：在cartographer的配置文件中（通常是.lua文件），确保map_frame、tracking_frame、published_frame和odom_frame等参数设置正确。特别是tracking_frame应设置为IMU的坐标系，published_frame设置为机器人的基坐标系。 3. IMU数据校准与融合 IMU校准：确保IMU已经过校准，以减少因传感器误差导致的定位漂移。 检查IMU与雷达数据的融合：在cartographer中，IMU数据用于辅助雷达数据进行定位和建图。检查IMU数据是否被正确读取并与雷达数据融合。 4. 雷达参数调整 调整雷达扫描频率：如果雷达的扫描频率过高或过低，都可能导致数据处理不及时或数据冗余，从而影响定位精度。尝试调整雷达的扫描频率以找到最优设置。 检查雷达的IP和端口设置：确保雷达的IP和端口设置正确，以便ROS驱动能够正确连接到雷达并接收数据。 5. 调试与测试 启动RVIZ：使用RVIZ查看雷达点云数据和IMU数据，观察是否存在明显的数据抖动或错位现象。 移动测试：在安全的环境中移动机器人，观察雷达位置在RVIZ中的表现是否稳定。 记录日志：在调试过程中，记录ROS的日志文件，以便在出现问题时回溯和分析。 6. 查阅文档与社区资源 官方文档：仔细阅读cartographer和vanjee_WLR720_16的官方文档，了解配置细节和常见问题。 社区论坛：在ROS社区、GitHub等平台上搜索类似问题，查看其他用户是如何解决的。 7. 软件与依赖 检查ROS版本：确保您使用的ROS版本与cartographer和vanjee_WLR720_16的ROS驱动兼容。 依赖库：检查所有必要的依赖库是否已正确安装，并且版本兼容。

通过以上步骤，您应该能够定位并解决坐标系无法对齐和雷达位置漂移的问题。如果问题仍然存在，可能需要更深入地分析数据或寻求专业帮助。

以上为生成回答，仅供参考～

顶贴,学到了。٩(ↁωↁ❀)

@2429148505 在 四驱板使用fishbot四驱代码，无法正常发送odom和imu话题 中说：

@2429148505 在 四驱板使用fishbot四驱代码，无法正常发送odom和imu话题 中说：

@小鱼 最近测试四驱板，发现我的odom方向与实际方向相反。(我说之前导航怎么老是出问题，我还以为是打滑的原因(虽然还是走不直，以后打算加一个imu纠偏))使用的是fishbot四驱板的代码，以下是现象:当cmd_vel只给x速度时，小车会向前走，而且四个轮子输出都是正的，可以排除是安装错误的问题，但是轮子向前转动时，输出的脉冲为负值，而且返回的速度也都为负值。而且不是编码器读数应该没问题，因为把编码器的引脚改反，会发生错误，轮子会因为pid原地振动。感觉是因为正解算时解算反了，但是我找不到代码错误。不知道你有没有出现这个现象。

好像不是这个原因。我记得单独给pid控制器目标值的时候，给正值也是向前转(与脉冲方向相反)，应该不是解算问题，也不是安装问题，是pid控制器的问题。而且还有一个现象，就是给yaw角速度时，旋转的方向也是相反的。

好像就是我的编码器ab弄反了，导致一直加速。至于抖动现象，不知道是什么原因。

@839343323 写个脚本读取播放然后录制试试

@siviechen 会被直接覆盖掉

@王涛 将IMU的位置提供给robot_localization

@小鱼 在 FishBot里程计和IMU传感器数据融合 中说：

发布静态TF变换
在开始之前，我们需要发布一个静态TF变换，将base_link和IMU之间的坐标系关系告知robot_localization节点。我们可以使用以下命令行发布该静态TF变换：
ros2 run tf2_ros static_transform_publisher 0.08 0.0 0.03 0 0 0 1 base_link imu

其中，0.08、0.0和0.03分别表示x、y和z轴上的平移量。请根据实际情况进行调整。