单目避障实战（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(f"yaw = {yaw:.3f} rad, {yaw_deg:.1f} deg") def main(args=None): rclpy.init(args=args) 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( f"target yaw set: {math.degrees(self.target_yaw):.1f} deg" ) error = self.normalize_angle(self.target_yaw - current_yaw) self.get_logger().info( f"current={math.degrees(current_yaw):.1f}, " f"target={math.degrees(self.target_yaw):.1f}, " f"error={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): twist=Twist() 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( f"yaw = {current_yaw:.3f} rad, {math.degrees(current_yaw):.1f} deg，{error:.1f}error" ) def main(args=None): rclpy.init(args=args) 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.kp=0.5 self.max_speed_a=0.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): twist=Twist() 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_speed=self.kp*error angle_speed_1=self.clamp(angle_speed,-self.max_speed_a,self.max_speed_a) if abs(error)>math.radians(5): twist.linear.x=0.1 twist.angular.z=-angle_speed_1 else: twist.linear.x=0.0 twist.angular.z=0.0 self.publisher.publish(twist) self.get_logger().info( f"yaw = {current_yaw:.3f} rad, {math.degrees(current_yaw):.1f} deg" ) def main(args=None): rclpy.init(args=args) node = YawCorrect() rclpy.spin(node) node.destroy_node() rclpy.shutdown() if __name__ == "__main__": main()这样就可以实现一辆小车的自动回正功能了，这也是我们自动避障的第一步。恭喜你！！！！我也会尽快更新接下来的文章。