单目避障实战（1）：自动回正功能实现

前言欢迎大家来到这篇文章其实这篇文章也是按之前一样的。这是我在备赛期间对技术的积累和记录之所以想要分享出来是因为自己确实在备赛期间的迷茫和不解希望能够给这样的同学一些帮助本人在技术上肯定有改进空间所以如果本文有遗漏之处欢迎与我讨论。问题分析其实所谓的避障在我看来就是在原有轨道上识别到障碍物然后躲避障碍之后在回到轨道上。那么第一步我们首先需要实现的小功能如何把车头回正其实这更多是一个闭环控制过程具体的理论支撑后续可以给大家补充PID部分的知识但是在这就不再赘述。如何知道自己的朝向在ros中呢其实本身就有里程计记录机器自身的速度和方向的如/odom,/odom_combined.所以我们可以使用这样的话题对原有的方向进行记录和计算误差。下图是我echo了该话题的图片下面我们更直观一点打印出来下面我将会使用更为精确的/odom_combined/odom_combined 里面有机器人姿态姿态是四元数我们把四元数转成 yaw打印 yaw看车头方向变化你可以先在终端确认话题类型ros2 topic info /odom_combined我们在这可以看到/odom_combined的包名和消息类型这也是我们待会要导入的。大家可以自行查阅或者使用AI看看odom_callback中的数学原理会让大家明白四元数到朝向的关系。不知道也没关系可以只是看成一个转化而且是固定的。import math import rclpy from rclpy.node import Node from nav_msgs.msg import Odometry class YawReader(Node): def __init__(self): super().__init__(yaw_reader) self.sub self.create_subscription( Odometry, /odom_combined, self.odom_callback, 10, ) def odom_callback(self, msg): q msg.pose.pose.orientation siny_cosp 2.0 * (q.w * q.z q.x * q.y) cosy_cosp 1.0 - 2.0 * (q.y * q.y q.z * q.z) yaw math.atan2(siny_cosp, cosy_cosp) yaw_deg math.degrees(yaw) self.get_logger().info(fyaw {yaw:.3f} rad, {yaw_deg:.1f} deg) def main(argsNone): rclpy.init(argsargs) node YawReader() rclpy.spin(node) node.destroy_node() rclpy.shutdown() if __name__ __main__: main()然后再理解一下Odometry结构nav_msgs/msg/Odometry header child_frame_id pose pose position x y z orientation x y z w twist twist linear angular所以msg.pose.pose.position.x msg.pose.pose.position.y msg.pose.pose.orientation分别是融合后的x/y/方向。计算偏差针对于回正操作什么是偏差呢其实就是目标角度和当前角度的差值(current_angle-target_angle).那我们现在就开始实现这一个小步骤角度不能直接减因为有pi和-pi跳变问题所以要归一化def normalize_angle(a): return math.atan2(math.sin(a), math.cos(a))完整一点就是self.target_yaw None def odom_callback(self, msg): current_yaw self.quaternion_to_yaw(msg.pose.pose.orientation) if self.target_yaw is None: self.target_yaw current_yaw self.get_logger().info( ftarget yaw set: {math.degrees(self.target_yaw):.1f} deg ) error self.normalize_angle(self.target_yaw - current_yaw) self.get_logger().info( fcurrent{math.degrees(current_yaw):.1f}, ftarget{math.degrees(self.target_yaw):.1f}, ferror{math.degrees(error):.1f} deg )辅助函数def quaternion_to_yaw(self, q): siny_cosp 2.0 * (q.w * q.z q.x * q.y) cosy_cosp 1.0 - 2.0 * (q.y * q.y q.z * q.z) return math.atan2(siny_cosp, cosy_cosp)#这个就是前面odom_callback中的转换 def normalize_angle(self, a): return math.atan2(math.sin(a), math.cos(a))于是我们的文件变成了之后再次运行本文件并转动机器就可以看到error啦import math import rclpy from rclpy.node import Node from nav_msgs.msg import Odometry from geometry_msgs.msg import Twist class YawCorrect(Node): def __init__(self): super().__init__(yaw_reader) self.sub self.create_subscription( Odometry, /odom_combined, self.odom_callback, 10, ) self.target_yaw None def quaternion_to_yaw(self, q): siny_cosp 2.0 * (q.w * q.z q.x * q.y) cosy_cosp 1.0 - 2.0 * (q.y * q.y q.z * q.z) return math.atan2(siny_cosp, cosy_cosp) def normalize_angle(self, a): return math.atan2(math.sin(a), math.cos(a)) def odom_callback(self, msg): twistTwist() current_yaw self.quaternion_to_yaw(msg.pose.pose.orientation) if self.target_yaw is None: self.target_yaw current_yaw error self.normalize_angle(current_yaw - self.target_yaw) self.get_logger().info( fyaw {current_yaw:.3f} rad, {math.degrees(current_yaw):.1f} deg{error:.1f}error ) def main(argsNone): rclpy.init(argsargs) node YawCorrect() rclpy.spin(node) node.destroy_node() rclpy.shutdown() if __name__ __main__: main()控制器编写下一步就是把error变成转向控制也就是最基础的P 控制。公式很简单angular_z kp * error含义是偏差越大转得越快 偏差越小转得越慢 偏差接近 0就不转但是要限制最大角速度防止车突然猛打方向def clamp(value, low, high): return max(low, min(high, value))回正控制可以这样理解error normalize_angle(target_yaw - current_yaw) angular_z kp * error angular_z clamp(angular_z, -max_angular_speed, max_angular_speed)比如参数kp 1.5 max_angular_speed 0.8 yaw_tolerance math.radians(5)逻辑if abs(error) yaw_tolerance: twist.linear.x 0.05 twist.angular.z angular_z else: twist.linear.x 0.0 twist.angular.z 0.0 print(回正完成)这里有个非常重要的方向问题如果你发现小车越修越偏说明符号反了把它改成angular_z -kp * error这很正常因为不同底盘对angular.z正方向的定义可能和你的 yaw 正方向不一致。所以学习时一定要先低速测试linear.x 0.0 或 0.03 max_angular_speed 0.3最后就变成了import math import rclpy from rclpy.node import Node from nav_msgs.msg import Odometry from geometry_msgs.msg import Twist class YawCorrect(Node): def __init__(self): super().__init__(yaw_reader) self.sub self.create_subscription( Odometry, /odom_combined, self.odom_callback, 10, ) self.publisher self.create_publisher(Twist, /cmd_vel, 10) self.target_yaw None self.kp0.5 self.max_speed_a0.8 def quaternion_to_yaw(self, q): siny_cosp 2.0 * (q.w * q.z q.x * q.y) cosy_cosp 1.0 - 2.0 * (q.y * q.y q.z * q.z) return math.atan2(siny_cosp, cosy_cosp) def normalize_angle(self, a): return math.atan2(math.sin(a), math.cos(a)) def clamp(self,speed_r,min_speed,max_speed): return max(min_speed,min(speed_r,max_speed)) def odom_callback(self, msg): twistTwist() current_yaw self.quaternion_to_yaw(msg.pose.pose.orientation) if self.target_yaw is None: self.target_yaw current_yaw error self.normalize_angle(current_yaw - self.target_yaw) angle_speedself.kp*error angle_speed_1self.clamp(angle_speed,-self.max_speed_a,self.max_speed_a) if abs(error)math.radians(5): twist.linear.x0.1 twist.angular.z-angle_speed_1 else: twist.linear.x0.0 twist.angular.z0.0 self.publisher.publish(twist) self.get_logger().info( fyaw {current_yaw:.3f} rad, {math.degrees(current_yaw):.1f} deg ) def main(argsNone): rclpy.init(argsargs) node YawCorrect() rclpy.spin(node) node.destroy_node() rclpy.shutdown() if __name__ __main__: main()这样就可以实现一辆小车的自动回正功能了这也是我们自动避障的第一步。恭喜你我也会尽快更新接下来的文章。