基于 Nao 机器人的摄像头和声呐结合寻路方式

文章目录

一些碎碎念
思路
解法
- [1. 如何获取Sonar和Camera的内容](#1. 如何获取Sonar和Camera的内容)
- - [获取 Sonar 的内容](#获取 Sonar 的内容)
  - [获取 Camera 的内容](#获取 Camera 的内容)
- [2. 如何前进？如何后退？如何转弯？](#2. 如何前进？如何后退？如何转弯？)
- - 前进
  - 后退
  - 转弯
- [3. 走多快？退多快？转弯幅度多大？](#3. 走多快？退多快？转弯幅度多大？)
- [4. 什么时候前进？什么时候转弯？什么时候后退？什么时候停下？](#4. 什么时候前进？什么时候转弯？什么时候后退？什么时候停下？)
- [6. 后退之后，怎么找新的路线？](#6. 后退之后，怎么找新的路线？)
- [5. 如何解决在后退、前进的时候机器人摔倒的问题？](#5. 如何解决在后退、前进的时候机器人摔倒的问题？)
- [7. 如何看机器人目前的状态](#7. 如何看机器人目前的状态)
- 最终代码

一些碎碎念

我已经 3 个月没更新 CSDN 博文了，因为我那 3 个月在准备CET-6考试，没有空闲时间。今天，也就从在Nao机器人里面实现摄像头和声呐结合的寻路方式开始更吧。

由于一些事情，我把我的小组给出卖了------原本可以用图形化界面编程的，我选择使用Python。使用Python也就算了，但是用的却是Python 2.7。这不是在故意搞事吗？

要知道Python 2.7弄Jupyter Notebook那简直是折磨人，基本上是很麻烦的，你可以看我写的这个代码。

python 复制代码

# -*- coding: utf-8 -*-
import os
import subprocess
import sys

CACHE_DIR = r'C:\Users\{}\AppData\Local\pip\Cache'.format(
    os.environ.get('USERNAME') or os.environ.get('USER') or os.getlogin())

PY2 = sys.version_info[0] == 2

DEFAULT_PKGS = ['notebook', 'openai', 'toml', 'numpy', 'pandas', 'matplotlib', 'pillow', 'requests',
                'SpeechRecognition', 'pipwin', 'pyaudio', 'opencv-python']


def ensure_pip_cache():
    env_name = 'PIP_CACHE_DIR'
    if not os.environ.get(env_name):
        os.environ[env_name] = CACHE_DIR
    d = os.environ[env_name]
    if not os.path.isdir(d):
        try:
            os.makedirs(d)
        except OSError:
            pass


def run(cmd):
    print('> ' + ' '.join(cmd))
    subprocess.check_call(cmd)


def base_tools():
    if PY2:
        # 最后支持 Python 2.7 的版本
        run([sys.executable, '-m', 'pip', 'install',
             'pip==20.3.4', 'setuptools==44.1.1', 'wheel==0.34.2'])
    else:
        run([sys.executable, '-m', 'pip', 'install', '--upgrade', 'pip', 'setuptools', 'wheel'])


def resolve_versions(pkgs):
    resolved = []
    for p in pkgs:
        name = p.strip()
        if not name:
            continue
        if '==' in name or '>=' in name or '<=' in name:
            resolved.append(name)
            continue
        low = name.lower()
        if PY2:
            if low == 'notebook':
                resolved.append('notebook==5.7.16')
            elif low == 'toml':
                resolved.append('toml==0.10.2')
            elif low == 'openai':
                print('跳过 openai（不支持 Python 2.7）')
            else:
                resolved.append(name)
        else:
            resolved.append(name)
    # Python 2 下补齐 notebook 依赖避免解析新版
    if PY2 and any(x.startswith('notebook==5.7.16') for x in resolved):
        pinned = [
            'pywinpty==0.5.7',
            'traitlets==4.3.3',
            'jupyter-client==5.3.5',
            'jupyter-core==4.6.3'
        ]
        for x in pinned:
            if x not in resolved:
                resolved.insert(0, x)
    return resolved


def parse_args():
    if '--packages' in sys.argv:
        idx = sys.argv.index('--packages')
        return sys.argv[idx + 1:]
    # 交互输入（可直接回车使用默认）
    try:
        raw = input('输入要安装的包(逗号分隔，留空使用默认: notebook,openai,toml): ').strip()
    except Exception:
        raw = ''
    if not raw:
        return DEFAULT_PKGS
    return [x.strip() for x in raw.split(',') if x.strip()]


def install(pkgs):
    if not pkgs:
        print('无可安装包')
        return
    run([sys.executable, '-m', 'pip', 'install'] + pkgs)


def main():
    ensure_pip_cache()
    base_tools()
    user_pkgs = parse_args()
    final_pkgs = resolve_versions(user_pkgs)
    print('计划安装: {0}'.format(final_pkgs))
    install(final_pkgs)
    print('完成')


if __name__ == '__main__':
    try:
        main()
    except Exception as e:
        print('失败: {0}'.format(e))
        sys.exit(1)

这套代码在我电脑上跑完之后，Jupyter Notebook能用了。

好了，吐槽的话就到这里了，至于为什么选择Jupyter Notebook，那是因为它得天独厚的优势，逐个Code Cell运行。

思路

此时，就有这几个问题要解决：

如何获取Sonar和Camera的内容
如何前进？如何后退？如何转弯？
走多快？退多快？转弯幅度多大？
什么时候前进？什么时候转弯？什么时候后退？
后退之后，怎么找新的路线？
如何解决在后退、前进的时候机器人摔倒的问题？
如何看机器人目前的状态

解法

1. 如何获取Sonar和Camera的内容

获取 Sonar 的内容

python 复制代码

from naoqi import ALProxy

sonar = ALProxy("ALSonar", ip, port)
memory = ALProxy("ALMemory", ip, port)

sonar.subscribe("NaoMapper")

while True:
	l = memory.getData("Device/SubDeviceList/US/Left/Sensor/Value") or 0.0
    r = memory.getData("Device/SubDeviceList/US/Right/Sensor/Value") or 0.0
    min_sonar = min(l, r)
    
sonar.unsubscribe("NaoMapper")

获取 Camera 的内容

python 复制代码

import cv2
from naoqi import ALProxy

# 摄像头参数常量
kTopCamera = 0  # 0: 顶摄像头, 1: 下摄像头
kqvga = 1  # 分辨率: 0=160x120, 1=320x240, 2=640x480
kBGRColorSpace = 13  # 13: BGR (适合OpenCV), 11: RGB

cam_proxy = ALProxy("ALVideoDevice", ROBOT_IP, PORT)
subscriber_id = cam_proxy.subscribeCamera("python_stream", kTopCamera, kqvga, kBGRColorSpace, 30)

cv2.namedWindow("NAO Camera", cv2.WINDOW_AUTOSIZE)

while True:
	nao_image = cam_proxy.getImageRemote(subscriber_id)
	
	if nao_image is None:
   		print "No image received."
   		continue
   		
	# nao_image[0]: 宽, nao_image[1]: 高, nao_image[6]: 二进制像素数据
	img_width = nao_image[0]
	img_height = nao_image[1]
	array = nao_image[6]
	
	img_data = np.fromstring(array, dtype=np.uint8)
	
	final_img = img_data.reshape(img_height, img_width, 3)
	
	cv2.imshow("NAO Camera", final_img)
	if cv2.waitKey(1) & 0xFF == ord('q'):
		break

cam_proxy.unsubscribe(subscriber_id)
cv2.destroyAllWindows()

2. 如何前进？如何后退？如何转弯？

你调用ALProxy中的ALMotion的move方法。

前进

向前移动0.2米每秒，不转弯。

python 复制代码

from naoqi import ALProxy

ALMotion = ALProxy("ALMotion", IP, PORT)
ALMotion.move(0.2, 0.0, 0.0)

后退

向后移动0.2米每秒，不转弯。

python 复制代码

from naoqi import ALProxy

ALMotion = ALProxy("ALMotion", IP, PORT)
ALMotion.move(-0.2, 0.0, 0.0)

转弯

向左移动0.1米每秒，同时顺时针旋转0.5弧度每秒。

python 复制代码

from naoqi import ALProxy

ALMotion = ALProxy("ALMotion", IP, PORT)
ALMotion.move(0.0, 0.1, -0.3)

3. 走多快？退多快？转弯幅度多大？

不需要考虑速度调整，我们可以用一个简单的办法实现的，就是直接使用指定的速度前进、后退和转弯。

常量部分

python 复制代码

from naoqi import ALProxy

motion= ALProxy("ALMotion", IP, PORT)

# 运动参数 (低步频模式)
WALK_FREQUENCY = 0.6  # 步频
SPEED_FORWARD = 0.07  # 前进速度
SPEED_CRAWL = 0.02  # 蠕动速度
SPEED_BACKWARD = -0.06  # 倒车速度
TURN_SPEED = 0.20  # 转向限制（可根据需要调低到 0.15 以更稳）

move_config = [["Frequency", WALK_FREQUENCY]]

执行代码
1. 前进
  python 复制代码
```
motion.move(SPEED_FORWARD, 0, 0, move_config)
```
2. 后退
  python 复制代码
```
motion.move(SPEED_BACKWARD, 0, 0, move_config)
```
3. 转弯（按照程序设计，基本上都是选择前进转弯，无论是不是后退之后执行）
  python 复制代码
```
motion.move(SPEED_FORWARD, 0, TURN_SPEED, move_config)
```

4. 什么时候前进？什么时候转弯？什么时候后退？什么时候停下？

这个问题的答案很简单，就是"前面有路我就走，没路我就转弯找路，实在找不到就回溯（后退），要改变状态的时候就停下"

那怎么判定前面有没有路呢？

我们不是有 Sonar 和 Camera 嘛，Sonar 在胸前，Camera 在头上，刚刚好。

真的吗？

才怪呢！

你有没有想过，这两东西的关系。串行（一次只能执行一项任务，并且必须按照顺序依次执行，后面的任务必须等到前面的任务完成后才能开始）？还是并行（在同一时刻，任务可以同时开始进行）？

是并行。

因为在你检测的时候，move 函数是在运行的，并不是停止的，即 Nao 机器人是在行走的。此时，如果遇到障碍物，就需要 Sonar / Camera 两者其中一个发现，然后使其停下来。

既然是并行，那我怎样将这两者的结果统一起来呢？

这一点涉及到多线程和共享变量的知识了。

先说多线程

谁是副线，谁就是继承 threading.Thread。

显然，Sonar 比 Camera 好，Camera 是副线，Sonar 是主线。

先说说 Camera 做什么吧

以下是伪代码

复制代码

// 对二值红色掩码做形态学净化  
对掩码做一次腐蚀（erode）  
再做两次膨胀（dilate）

// 初始化输出变量  
vis_force = 0.0  
detected_obj = None

查找外部轮廓（findContours）

如果找到轮廓列表不为空：  
	选择面积最大的轮廓 c  
	
	计算该轮廓的面积 area  
	
	如果 area > 面积阈值（例如 300）：  
		计算轮廓矩 M  
		
		如果 M["m00"] 不为 0：  
			计算质心横坐标 cx = M["m10"] / M["m00"]  
			
			// 将 cx 相对于图像中心的水平偏移归一化为 \-1..1 的值
			vis_force = - ( (cx - img_width/2) / (img_width/2) )  （负号用于匹配控制约定）  
			
			计算目标的边界框 rect = boundingRect(c)  
			
			设置 detected_obj 为包含标签、颜色和 rect 的信息（用于显示）
			
返回或写回结果给主循环：vis_force 与 detected_obj

Sonar 啊？！那就很简单了。

复制代码

读取左声纳值 l，若为 None 则设为 0.0。  
读取右声纳值 r，若为 None 则设为 0.0。  
取 min_sonar = min(l, r)。  
将 sonar_strength 初始化为 0.0。  
如果 min_sonar 小于 SAFE_DIST：  
	计算 strength = (SAFE_DIST - min_sonar) / SAFE_DIST（归一化到 0..1，距离越近值越大）。  
	若 l >= r，则将 sonar_strength 设为 +strength；否则设为 -strength（符号表示障碍偏向哪侧）。  
// 用指数移动平均更新声纳力
sonar_force_ema = SONAR_AVG_ALPHA * sonar_strength + (1 - SONAR_AVG_ALPHA) * sonar_force_ema。

最后就是合并计算了嘛

复制代码

1. 读取平滑后的视觉力 vis_force 和声纳力 sonar_force
2. 计算未限幅的目标角速度原始值：
   raw_vw = vis_force * WEIGHT_VISION + sonar_force * WEIGHT_SONAR

3. 视觉方向滞回：
   - cur_vis_dir = 0
   - 如果 abs(vis_force) > VIS_HYSTERESIS：
       cur_vis_dir = -1 当 vis_force < 0 否则 1
   - 检测是否与上一次方向相反（sign_flip）：
       sign_flip = (self.last_vis_dir != 0 且 cur_vis_dir != 0 且 cur_vis_dir != self.last_vis_dir)
   - 更新 self.last_vis_dir：当 cur_vis_dir != 0 时替换，否则保持原值

4. 陷入检测（基于最小声纳距离 min_sonar）：
   - 如果 min_sonar < CRITICAL_DIST：self.stuck_counter += 1
   - 否则：self.stuck_counter = max(0, self.stuck_counter - 1)
   - 如果 self.stuck_counter > STUCK_LIMIT：
       - 标记将要后退：设置 switch_pause_timer、pending_backup，并清零 stuck_counter
       - 跳到结束（不继续常规速度计算）

5. 常规决策（未陷入时）：
   - 冲突检测：
       - 如果 (vis_force * sonar_force < -0.01) 且 (abs(vis_force) > 0.2)：
           - 将 robot_status 设为 "Conflict!"
           - 设置犹豫计时器 hesitation_timer = HESITATION_FRAMES
           - 不计算正常目标速度（进入犹豫等待）
       - 否则：
           - 将 target_vw 限幅到 [-MAX_TURN_SPEED, MAX_TURN_SPEED]：target_vw = clamp(raw_vw)
           - 将 target_vx = SPEED_FORWARD（默认前进速度）

           - 如果 min_sonar < CRITICAL_DIST（非常近）：
               - target_vx = 0.0
               - 标记为原地转向：is_turning_in_place = True
               - robot_status = "Turning in Place"
               - 如果 abs(target_vw) < 0.2：
                   - 强制把 target_vw 调整为有最小转速：取符号后的 MAX_TURN_SPEED

           - 否则如果 abs(target_vw) > 0.2：
               - 把前进速度降为慢速爬行：target_vx = SPEED_CRAWL

           - 如果检测到视觉方向快速翻转（sign_flip 为真）：
               - 衰减目标角速度：target_vw *= 0.55

6. 输出结果：使用计算得到的 target_vx, target_vw，并更新相应状态变量

sonar_force_ema？vis_force_ema？这俩是什么？

其实是 EMA。

什么是 EMA?

EMA（指数移动平均, Exponential Moving Average）是一种递归的平滑滤波方法，用于将噪声或高频抖动的信号平滑成更稳定的值。

s t = α ∗ x t + ( 1 − α ) ∗ s t − 1 s_t = α * x_t + (1 - α) * s_{t-1} st=α∗xt+(1−α)∗st−1,其中 x t x_t xt 是当前观测值， s t s_t st 是平滑后值， α （ 0..1 ） α（0..1） α（0..1）是平滑系数（代码中的 VIS_AVG_ALPHA）。

α α α 越大：对新数据响应越快，但噪声抑制能力越弱（更不平滑）。
α α α 越小：平滑效果越强，但对变化响应更慢（更滞后）。

这常用于传感器数据、控制环节的抖动抑制、实时信号低通滤波（如视觉力或声纳力的平滑）。

最后就是实现前面有路我就走，没路我就转弯找路，实在找不到就回溯（后退），要改变状态的时候就停下了，怎么实现呢？

用状态机就搞好了嘛。

下面是状态机的操作

编号	状态	进入条件	行为	退出 / 后续
1	`SWITCH_PAUSE`（切换缓冲）	`switch_pause_timer > 0`（切换到原地转向或检测到卡住后设置）	每循环 `switch_pause_timer -= 1`，停止移动（`target_vx=0, target_vw=0`）	计时器归零且 `pending_backup` 则设置 `backup_timer=BACKUP_DURATION` 并清 `pending_backup`，进入备份流程
2	`RECOVERING`（倒车/备份）	`backup_timer > 0`	每循环 `backup_timer -= 1`，`target_vx=SPEED_BACKWARD`，`stuck_counter=0`	计时器归零后设置 `post_backup_pause_timer=POST_BACKUP_WAIT`
3	`WAIT`（后备暂停）	`post_backup_pause_timer > 0`	每循环 `post_backup_pause_timer -= 1`，停止移动	计时器归零后 `random_turn_timer=RANDOM_TURN_DURATION` 并设置 `turn_direction`
4	`RANDOM_TURN`（随机转向）	`random_turn_timer > 0`	每循环 `random_turn_timer -= 1`，`target_vx=0`，`target_vw = turn_direction * MAX_TURN_SPEED * rand_ratio`	结束后 `recovery_pause_timer=RECOVERY_PAUSE`
5	`RESTING`（恢复/休息）	`recovery_pause_timer > 0`	每循环 `recovery_pause_timer -= 1`，停止移动，重置历史速度 `last_vx=0, last_vw=0`	计时器归零后返回正常导航
6	`HESITATING`（犹豫）	`hesitation_timer > 0`（视觉与声纳冲突触发）	每循环 `hesitation_timer -= 1`，保持不动等待感知明确	计时器归零后回到正常导航
7	`NORMAL_NAVIGATION`（巡航/感知融合）	无以上定时器且非特殊动作	计算 `raw_vw = vis_forceWEIGHT_VISION + sonar_forceWEIGHT_SONAR`，限幅到 `[-MAX_TURN_SPEED,MAX_TURN_SPEED]`，默认 `target_vx=SPEED_FORWARD`；若 `abs(target_vw)>0.2` 则 `target_vx=SPEED_CRAWL`；声纳影响减速/转向	若 `min_sonar < CRITICAL_DIST` 则进入 `TURNING_IN_PLACE`（见下）

最后，就是对应的执行部分了

整理如下表格：

场景 / 条件	执行动作	更新的计时器 / 变量	最终输出 / 执行
从前进切换到原地转向	若 `not self.was_turning_in_place and is_turning_in_place and self.last_vx > 0.01`，打印日志并先停顿	`self.switch_pause_timer = ACTION_SWITCH_PAUSE`，将 `target_vx, target_vw` 置 `0.0`	在 `switch_pause_timer>0` 期间强制 `final_vx=final_vw=0`
从原地转向切回前进	若 `self.was_turning_in_place and not is_turning_in_place and target_vx > 0`，进入恢复休息	`self.recovery_pause_timer = POST_TURN_PAUSE`，将 `target_vx, target_vw` 置 `0.0`	在 `recovery_pause_timer>0` 期间强制静止
更新转向标志	每次循环末设置	`self.was_turning_in_place = is_turning_in_place`	为下次循环提供状态依据
优先零速度保护（暂停优先）	若任一暂停计时器 > 0	---	直接令 `final_vx = final_vw = 0.0`（跳过平滑计算）
紧急模式平滑系数	若 `is_emergency = (self.backup_timer>0 or self.random_turn_timer>0)`	计算 `alpha_vx = 0.5`、`alpha_vw = 0.5`，否则使用 `SMOOTH_FACTOR_VX/SMOOTH_FACTOR_VW`	紧急动作响应更快（更大权重）
线速度平滑（EMA-like）	使用上一线速度与目标线速度做加权平均	`final_vx = self.last_vx(1-alpha_vx) + target_vxalpha_vx`	平滑线速度，避免突变
角速度单步限幅与平滑	先计算 `desired_delta_vw = target_vw - self.last_vw`，限幅 `±MAX_DELTA_VW`，再平滑	`pre_vw = self.last_vw + desired_delta_vw`，`final_vw = self.last_vw(1-alpha_vw) + pre_vwalpha_vw`	限制单步角速变化并做 EMA 平滑，减少抖动
死区与微值清理	应用角速度死区与线速度小值阈值	若 `abs(final_vw) < TURN_DEADBAND` 则 `final_vw = 0`；若 `abs(final_vx) < 0.01` 则 `final_vx = 0`；若 `abs(final_vw) < 0.05` 则 `final_vw = 0`	清除微抖与无效运动命令
更新历史	将平滑后的速度保存为历史	`self.last_vx, self.last_vw = final_vx, final_vw`	供下一次循环平滑与限幅使用

至于共同变量

你可以这样做

在 __init__ 函数里面加入 mapper_instance 传参，如下

python 复制代码

def __init__(self, mapper_instance):
	threading.Thread.__init__(self)
		self.mapper = mapper_instance

然后就可以实现了。

6. 后退之后，怎么找新的路线？

我们都知道，机器人的行走就像数据结构与算法里面的 DFS（深度优先搜索）算法，如果回溯之后，没有去寻找其他的道路，那回溯就没有意义了。

你要使 TURN_SPEED（转向速度）是一个可动态的情况，但是，TURN_SPEED 不是那个"转向速度"，而是"转向的角度"。

这就很麻烦了，我们要机器自己回溯，然后找路，但是如果角度固定了，那就有可能出现被困住了。

那就设置一个最大偏转角度 MAX_TURN_SPEED、最小转向强度 RANDOM_TURN_MIN_RATIO和最大转向强度 RANDOM_TURN_MAX_RATIO，然后随机。

在这里，不妨假定转向限制 MAX_TURN_SPEED 是 0.20，最小转向强度 RANDOM_TURN_MIN_RATIO 是 0.6，最大转向强度 RANDOM_TURN_MAX_RATIO 是 1.3。

python 复制代码

MAX_TURN_SPEED = 0.20  # 转向限制（可根据需要调低到 0.15 以更稳）
RANDOM_TURN_MIN_RATIO = 0.6  # 最小转向强度（占 MAX_TURN_SPEED 的比例）
RANDOM_TURN_MAX_RATIO = 1.3  # 最大转向强度（可略超 MAX_TURN_SPEED）

然后，摇骰子

random.choice([-1, 1, -1, 1, -1]) 会均匀地从列表中随机选取一个元素。控制物体随机向左（-1）的可能性略大于向右（1）转向
random.uniform(a, b) 是 Python 的 random 模块中的一个函数，用于生成一个在指定区间 [a, b] 内的随机浮点数。

python 复制代码

turn_direction = random.choice([-1, 1, -1, 1, -1])
rand_ratio = random.uniform(RANDOM_TURN_MIN_RATIO, RANDOM_TURN_MAX_RATIO)
target_vw = turn_direction * MAX_TURN_SPEED * rand_ratio

5. 如何解决在后退、前进的时候机器人摔倒的问题？

由于我们学校的 Nao 机器人已经是"上个世纪"的古董了，稍微一个不小心就可以实现广东话里面的"扑街"了。

那如何解决在前进、后退的时候机器人摔倒的问题呢？

根据第一性原理（从系统中最基本的命题或原理出发进行逻辑推理的方法）的解释，我们可以知道，前进、后退的时候机器人摔倒的问题发生在其过渡的向前向后倾。

过渡的向前向后倾。

为什么会过渡的向前向后倾，或者说向前向后倾发生在什么时候呢？

实验过程中，我们发现当机器人突然改变运动方向（如前进转后退或后退转前进），或转弯幅度过大时，这种情况发生的概率显著增加。

为什么？

初中无论可以告诉我们，那是因为惯性。惯性不是一种力，是物体运动遵循牛顿第一定律的性质。

那如何解决呢？

我们可以先让其停下来再去做不就好了嘛。

就好比，你开车的时候急刹车会导致人往前倾，但是，某场考试里面要求你，不能让车里的人因为惯性往前倾，那怎么办？

将速度有 v v v降到 0 0 0呀，这不就可以了嘛，就是时间要长一些，加速度要小一些（ 0 = v − a t 0 = v - at 0=v−at）罢了。

那怎样才能实现时间要长一些，加速度要小一些的效果呢？

好吧，其实方法很粗暴

python 复制代码

# 伪代码（中文描述）

1. 检测从前进切换到原地转向：
   - 条件：之前不是原地转向 && 当前要原地转向 && 上一帧为前进（self.last_vx > 0.01）
   - 动作：
     - 打印提示
     - 设置切换缓冲计时器：self.switch_pause_timer = ACTION_SWITCH_PAUSE
     - 将目标速度清零：target_vx = 0.0, target_vw = 0.0

2. 检测从原地转向切换回前进：
   - 条件：之前是原地转向 && 当前不再原地转向 && 目标前进速度大于0（target_vx > 0）
   - 动作：
     - 设置恢复停顿计时器：self.recovery_pause_timer = POST_TURN_PAUSE
     - 将目标速度清零：target_vx = 0.0, target_vw = 0.0

3. 执行移动（优先考虑各类停顿）：
   - 如果任一停顿计时器仍在倒计时（recovery_pause_timer > 0 或 post_backup_pause_timer > 0 或 switch_pause_timer > 0）：
     - final_vx = 0.0, final_vw = 0.0
   - 否则：
     - 识别是否处于紧急动作（is_emergency = backup_timer > 0 或 random_turn_timer > 0）
     - 根据是否紧急选择平滑系数：
       - alpha_vx = 0.5 如果 is_emergency 否则 SMOOTH_FACTOR_VX
       - alpha_vw = 0.5 如果 is_emergency 否则 SMOOTH_FACTOR_VW

     - 线速度平滑（类似 EMA）：
       - final_vx = last_vx * (1 - alpha_vx) + target_vx * alpha_vx

     - 角速度先做单步变化限制，再平滑：
       - desired_delta_vw = target_vw - last_vw
       - 如果 desired_delta_vw > MAX_DELTA_VW，则 desired_delta_vw = MAX_DELTA_VW
       - 如果 desired_delta_vw < -MAX_DELTA_VW，则 desired_delta_vw = -MAX_DELTA_VW
       - pre_vw = last_vw + desired_delta_vw
       - final_vw = last_vw * (1 - alpha_vw) + pre_vw * alpha_vw

4.（后续可选）对 very small 值做死区/清理（在代码中另处处理）
   - 例如：若 abs(final_vw) < TURN_DEADBAND 则 final_vw = 0；若 abs(final_vx) 很小则 final_vx = 0.0

7. 如何看机器人目前的状态

在1. 如何获取Sonar和Camera的内容当中的"获取 Camera 的内容"就提到了如何获得 Camera 的内容。

这里可以考虑使用opencv的putText函数、line函数和rectangle函数了，详细代码看下面

python 复制代码

# 4. 画面绘制 (放大显示)
big_frame = cv2.resize(frame, (0, 0), fx=WINDOW_SCALE, fy=WINDOW_SCALE)

cx_big = int(img_w * WINDOW_SCALE / 2)
cy_big = int(img_h * WINDOW_SCALE / 2)
cv2.line(big_frame, (cx_big, 0), (cx_big, int(img_h * WINDOW_SCALE)), (100, 100, 100), 1)
cv2.line(big_frame, (cx_big - 20, cy_big), (cx_big + 20, cy_big), (100, 100, 100), 1)

python 复制代码

# 这部分是目标检测的部分的
# if detected_obj:
#    text, color, rect = detected_obj
#    x, y, w, h = rect
#    bx, by, bw, bh = int(x * WINDOW_SCALE), int(y * WINDOW_SCALE), int(w * WINDOW_SCALE), int(
#        h * WINDOW_SCALE)
#    cv2.rectangle(big_frame, (bx, by), (bx + bw, by + bh), color, 2)
#    cv2.putText(big_frame, text, (bx, by - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.8, color, 2)

python 复制代码

# 机器人运动状态
status = self.mapper.robot_status
if "RECOVERING" in status:
    cv2.putText(big_frame, "!!! BACKING UP !!!", (50, 150), cv2.FONT_HERSHEY_SIMPLEX, 1.2, (0, 0, 255), 3)
elif "SWITCH" in status:
    cv2.putText(big_frame, "| PAUSE |", (50, 150), cv2.FONT_HERSHEY_SIMPLEX, 1.5, (255, 255, 255), 3)
elif "WAIT" in status:
    cv2.putText(big_frame, "... WAITING ...", (50, 150), cv2.FONT_HERSHEY_SIMPLEX, 1.2, (255, 100, 0), 3)
elif "Random Turn" in status:
    cv2.putText(big_frame, "<?> RANDOM TURN <?>", (50, 150), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255),
                2)
elif "Resting" in status:
    cv2.putText(big_frame, "... Stabilizing ...", (50, 150), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (255, 255, 0),
                2)
cv2.putText(big_frame, "Status: %s" % status, (20, 40), cv2.FONT_HERSHEY_SIMPLEX, 0.8, (0, 255, 0), 2)

python 复制代码

# 帧率
cv2.putText(big_frame, "FPS: %.1f" % self.fps, (int(img_w * WINDOW_SCALE) - 120, 30),
            cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1)

python 复制代码

cv2.putText(big_frame, "ColorOrder: %s" % used_color_order, (20, 70), cv2.FONT_HERSHEY_SIMPLEX, 0.6,
            (200, 200, 200), 1)

python 复制代码

# 手臂的状态
if self.mapper.arms_tight_mode:
    cv2.putText(big_frame, "ARMS: TIGHT (Safe)", (20, int(img_h * WINDOW_SCALE) - 20),
                cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 165, 255), 2)
else:
    cv2.putText(big_frame, "ARMS: SWING", (20, int(img_h * WINDOW_SCALE) - 20), cv2.FONT_HERSHEY_SIMPLEX,
                0.7, (200, 200, 200), 1)

最终代码

这就不展示最终代码了，以上的思想可以参考的。