论文《基于现实迷宫地形的电脑鼠设计》深度分析（二）——超声波环境感知算法
论文概述
《基于现实迷宫地形的电脑鼠设计》是由吴润强、庹忠曜、刘文杰、项璟晨、孙科学等人于2023年发表的一篇优秀期刊论文。其针对现阶段电脑鼠计算量庞大且不适用于现实迷宫地形的问题，特基于超声波测距与传统迷宫算法原理，设计出一款可在现实迷宫地形下自动寻找出口的电脑鼠。该电脑鼠适用于岔路数量与道路宽度不定、多死路弯道并且相对较大的迷宫地形，具有适应性强、计算量小、兼容性和可塑性强等优点，对于现实迷宫地形下的自动应用具有一定研究价值。
关键词 电脑鼠；超声波测距；迷宫算法；自动应用
该论文内容相对较多，特对其进行拆开分析，本文特围绕超声波环境感知算法进行展开分析。
一、测距与判断原理
假设超声波的传播速度为v0,超声波探头的位置可近似代表电脑鼠所在的位置，则根据超声波测距的原理，可由超声波释放与接受的间隔时间t0,得到电脑鼠与障碍物之间的距离l
(1)
为读取电脑鼠与周围障碍物的距离，需测量多组l值，而当同时使用多组超声波探头时，彼此产生的余波可能会干扰距离的测量结果,同时考虑到性价性与普适性等现实因素，特采用转向舵机带动超声波探头进行测距。
在进行探测时，以小车的前进方向作为基准线，根据现实环境中可能存在的路口宽度，以固定的旋转间隔角度ɑ分别将舵机向左向右旋转90度，得到每一个角度所对应的障碍物到电脑鼠的距离，进而根据其旋转角度计算其对应的横向距离与纵向距离，距离计算方案见图8。
图8 测距计算方案
障碍物距离电脑鼠的横向距离为l与旋转角的正弦值之积，纵向距离为l与旋转角的余弦值之积。同时当路口的宽度不小于相邻两个探测点的纵向距离之差时，超声波即可探测到该路口内的障碍物情况，此时可认为该路口在超声波的探测范围之内。
当舵机旋转角度ɑ越小，其可探测到的路口宽度越小，测量也越精准。而当ɑ过小时，其计算的数量量较大，复杂度较高；当ɑ过大时，其精确度较低，可能漏过宽度较小的路口。
在实际情况下，可转弯路口的实际宽度与当前所行驶的道路宽度可大致认为相差不大，且所分段的道路长度对于二者而言相对较小，故而可认为路口的道路宽度大于分段的道路长度，则ɑ取30度是一个较为合理的值。
不妨以30度作为旋转间隔角度进行研究，在基准线的探测距离记为l0，在基准线上以ɑ为间隔向左进行三次探测，其探测距离记为lli,i=1,2,3，在基准线上以ɑ为间隔向右进行三次探测，其探测距离记为lri,i=1,2,3。则根据以上的研究有以下公式：
二、中线距离求解
三、转弯路口判断
四、死路路口判断
五、开源代码分析
下面是一个简单的示例代码，用于展示如何使用Arduino平台和HC-SR04超声波传感器来实现智能车的环境感知。
- VCC 连接到 Arduino 的 5V
- GND 连接到 Arduino 的 GND
- Trig 连接到 Arduino 的数字引脚 12
- Echo 连接到 Arduino 的数字引脚 11
#include <Servo.h>
// 定义超声波传感器引脚
const int trigPin = 12; // 触发引脚
const int echoPin = 11; // 回声引脚
// 定义电机控制引脚
const int motorPin1 = 9; // 电机1控制引脚
const int motorPin2 = 10; // 电机2控制引脚
// 定义PWM频率
const int pwmFreq = 1000;
// 定义超声波传感器的变量
long duration; // 超声波传播的时间（微秒）
int distance; // 距离（厘米）
void setup() {
// 初始化串口通信
Serial.begin(9600);
// 设置触发引脚为输出模式
pinMode(trigPin, OUTPUT);
// 设置回声引脚为输入模式
pinMode(echoPin, INPUT);
// 设置电机引脚为输出模式
pinMode(motorPin1, OUTPUT);
pinMode(motorPin2, OUTPUT);
// 设置PWM频率
analogWriteFrequency(motorPin1, pwmFreq);
analogWriteFrequency(motorPin2, pwmFreq);
}
void loop() {
// 发送超声波脉冲
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// 读取回声引脚的高电平时间
duration = pulseIn(echoPin, HIGH);
// 计算距离（厘米）
distance = duration * 0.034 / 2;
// 输出距离到串口
Serial.print("Distance: ");
Serial.print(distance);
Serial.println(" cm");
// 根据距离控制电机速度
if (distance < 10) {
// 障碍物太近，减速
analogWrite(motorPin1, 100);
analogWrite(motorPin2, 100);
} else if (distance < 20) {
// 障碍物较近，中速
analogWrite(motorPin1, 150);
analogWrite(motorPin2, 150);
} else {
// 障碍物较远，全速
analogWrite(motorPin1, 255);
analogWrite(motorPin2, 255);
}
// 每秒读取一次
delay(1000);
}我们可以增加一些算法来提高智能车的环境感知能力和行为决策。下面是一个更复杂的示例，包括PID控制算法来更精确地控制车速，并且使用卡尔曼滤波器来平滑距离测量值。
#include <Servo.h>
// 定义超声波传感器引脚
const int trigPin = 12; // 触发引脚
const int echoPin = 11; // 回声引脚
// 定义电机控制引脚
const int motorPin1 = 9; // 电机1控制引脚
const int motorPin2 = 10; // 电机2控制引脚
// 定义PWM频率
const int pwmFreq = 1000;
// 定义超声波传感器的变量
long duration; // 超声波传播的时间（微秒）
int distance; // 距离（厘米）
// PID 控制器参数
float setPoint = 30; // 目标距离
float Kp = 0.1; // 比例系数
float Ki = 0.01; // 积分系数
float Kd = 0.01; // 微分系数
float integral = 0; // 积分项
float derivative = 0; // 微分项
float lastError = 0; // 上一次的误差
float output = 0; // PID 输出
// 卡尔曼滤波器参数
float Q = 0.001; // 过程噪声
float R = 0.1; // 测量噪声
float x = 0; // 状态
float P = 1; // 不确定性
void setup() {
// 初始化串口通信
Serial.begin(9600);
// 设置触发引脚为输出模式
pinMode(trigPin, OUTPUT);
// 设置回声引脚为输入模式
pinMode(echoPin, INPUT);
// 设置电机引脚为输出模式
pinMode(motorPin1, OUTPUT);
pinMode(motorPin2, OUTPUT);
// 设置PWM频率
analogWriteFrequency(motorPin1, pwmFreq);
analogWriteFrequency(motorPin2, pwmFreq);
}
void loop() {
// 发送超声波脉冲
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// 读取回声引脚的高电平时间
duration = pulseIn(echoPin, HIGH);
// 计算距离（厘米）
distance = duration * 0.034 / 2;
// 输出距离到串口
Serial.print("Distance: ");
Serial.print(distance);
Serial.println(" cm");
// 卡尔曼滤波器
x = estimate(x, distance);
// PID 控制器
float error = setPoint - x;
integral += error;
derivative = error - lastError;
output = Kp * error + Ki * integral + Kd * derivative;
lastError = error;
// 限制输出值
if (output > 255) output = 255;
if (output < 0) output = 0;
// 控制电机速度
analogWrite(motorPin1, output);
analogWrite(motorPin2, output);
// 每秒读取一次
delay(1000);
}
// 卡尔曼滤波器函数
float estimate(float x, float z) {
float y = z - x;
P = P + Q;
float K = P / (P + R);
x = x + K * y;
P = (1 - K) * P;
return x;
}原文地址：https://blog.csdn.net/qq_63129682/article/details/143723762
免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！