返回
Cooja无线传感器网络(WSN)定位仿真：原理与实现
java
2025-03-14 21:32:39
Cooja 中 WSN 定位：实现未知节点定位
最近在折腾 Contiki-NG，想弄个无线传感器网络 (WSN) 定位的仿真。简单说，就是有一个服务器节点（锚节点）广播自己的坐标，然后让一些不知道自己在哪儿的节点（未知节点）收到坐标后，想办法算出自己的位置。琢磨了半天 Cooja 仿真，踩了不少坑，这里把过程和代码都理一理，分享给大家。
一、问题出在哪？
起初，我想着直接用 Cooja 的脚本编辑器，写个脚本让锚节点发送坐标，未知节点接收，再写个算法计算位置。结果发现，这方法在 Cooja 里不太好使，尤其是在处理无线通信和数据包解析的时候，脚本处理起来比较麻烦。
根本原因在于，Cooja 的脚本主要用来控制仿真流程、记录数据，而节点的具体行为，比如数据包的发送、接收、处理，还是得靠 C 代码来实现。所以，直接用脚本搞不定节点之间的通信和定位算法。
二、 解决之道：C 代码构建节点行为
弄清楚了原因，就好办了。 咱们直接用 C 代码，分别编写锚节点（anchor.c）和未知节点（unknown.c）的行为。
1. 锚节点 (anchor.c)
原理： 锚节点周期性地广播自己的位置信息。把位置信息（x, y 坐标）封装到一个自定义的数据包里，通过 Cooja 提供的无线通信接口发送出去。
代码示例：
#include "contiki.h"
#include "net/netstack.h"
#include "net/nullnet/nullnet.h"
#include <stdio.h>
#include <stdlib.h>
// 定义锚节点坐标
#define ANCHOR_X 100
#define ANCHOR_Y 50
// 定义广播周期（秒）
#define BROADCAST_INTERVAL (5 * CLOCK_SECOND)
// 定义数据包结构
typedef struct {
uint16_t x;
uint16_t y;
} position_data_t;
//广播的回调函数什么也不做，应为空
void nullnet_recv_from(const void *data, uint16_t len,
const linkaddr_t *src, const linkaddr_t *dest){
return;
}
/*---------------------------------------------------------------------------*/
PROCESS(anchor_node_process, "Anchor Node Process");
AUTOSTART_PROCESSES(&anchor_node_process);
/*---------------------------------------------------------------------------*/
PROCESS_THREAD(anchor_node_process, ev, data)
{
static struct etimer et;
static position_data_t pos_data;
PROCESS_BEGIN();
nullnet_set_input_callback(nullnet_recv_from);
// 初始化 NullNet
nullnet_buf = (uint8_t *)&pos_data;
nullnet_len = sizeof(pos_data);
printf("Anchor ID: %d , x: %u, y: %u\n",linkaddr_node_addr.u8[0], ANCHOR_X, ANCHOR_Y);
while(1) {
// 设置定时器
etimer_set(&et, BROADCAST_INTERVAL);
PROCESS_WAIT_EVENT_UNTIL(etimer_expired(&et));
// 填充位置数据
pos_data.x = ANCHOR_X;
pos_data.y = ANCHOR_Y;
// 广播数据
NETSTACK_NETWORK.output(NULL);
}
PROCESS_END();
}
/*---------------------------------------------------------------------------*/
安全提示：
- 示例中未考虑通信安全。实际应用中，可能需要对广播数据进行加密或认证，防止恶意节点篡改或伪造位置信息。
进阶技巧:
- 可以使用CoAP 或 MQTT 协议进行通讯，让数据更结构化。
- 可以模拟信号衰减, 调整
RADIO.TX_POWER来模拟更真实的传播模型。
2. 未知节点 (unknown.c)
原理： 未知节点接收来自锚节点的广播数据包，从中提取出锚节点的坐标信息。收集到至少三个不同锚节点的坐标后，利用三边定位算法（或类似的算法）计算自己的位置。
代码示例：
#include "contiki.h"
#include "net/netstack.h"
#include "net/nullnet/nullnet.h"
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
// 定义数据包结构（与锚节点一致）
typedef struct {
uint16_t x;
uint16_t y;
} position_data_t;
// 定义存储锚节点信息的结构
typedef struct {
linkaddr_t addr;
uint16_t x;
uint16_t y;
int16_t rssi; // 接收信号强度
} anchor_info_t;
#define MAX_ANCHORS 4 // 最多存储的锚节点数量,多于三个用于平均和优化结果。
static anchor_info_t anchors[MAX_ANCHORS];
static int anchor_count = 0;
//定位计算的辅助函数
// 计算两点间距离
float calculate_distance(uint16_t x1, uint16_t y1, uint16_t x2, uint16_t y2) {
return sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2));
}
//三边定位法，使用传入的参数的坐标和距离，来确定未知节点的坐标，把结果保存在传入的后两参数里。
void trilaterate(float x1, float y1, float r1,
float x2, float y2, float r2,
float x3, float y3, float r3,
float *result_x, float *result_y) {
float a = 2 * (x2 - x1);
float b = 2 * (y2 - y1);
float c = pow(r1, 2) - pow(r2, 2) - pow(x1, 2) + pow(x2, 2) - pow(y1, 2) + pow(y2, 2);
float d = 2 * (x3 - x2);
float e = 2 * (y3 - y2);
float f = pow(r2, 2) - pow(r3, 2) - pow(x2, 2) + pow(x3, 2) - pow(y2, 2) + pow(y3, 2);
*result_x = (c * e - f * b) / (e * a - b * d);
*result_y = (c * d - a * f) / (b * d - a * e);
}
// 接收到数据的回调函数
void recv_packet(const void *data, uint16_t len,
const linkaddr_t *src, const linkaddr_t *dest)
{
if(len == sizeof(position_data_t)) {
position_data_t *pos_data = (position_data_t *)data;
// 检查是否已存在相同地址的锚节点
int i;
for(i = 0; i < anchor_count; i++) {
if(linkaddr_cmp(src, &anchors[i].addr)) {
// 更新已有锚节点的信息
anchors[i].x = pos_data->x;
anchors[i].y = pos_data->y;
anchors[i].rssi = packetbuf_attr(PACKETBUF_ATTR_RSSI);;
printf("Updated info from anchor: %d , x: %u, y: %u, RSSI: %d\n", src->u8[0],pos_data->x, pos_data->y, anchors[i].rssi);
return; //更新就不进行后续操作。
}
}
// 如果锚节点数量未达上限，添加新的锚节点
if(anchor_count < MAX_ANCHORS) {
linkaddr_copy(&anchors[anchor_count].addr, src);
anchors[anchor_count].x = pos_data->x;
anchors[anchor_count].y = pos_data->y;
anchors[anchor_count].rssi = packetbuf_attr(PACKETBUF_ATTR_RSSI);
printf("Received from anchor: %d , x: %u, y: %u , RSSI: %d\n", src->u8[0],pos_data->x, pos_data->y, anchors[anchor_count].rssi);
anchor_count++;
//接收三个就计算一次位置
if(anchor_count >= 3)
{
//使用所有锚节点多次执行三边测量来获取平均值。
float sum_x=0;
float sum_y =0;
int16_t distances[MAX_ANCHORS]; //用信号强度估算距离，仅为例子，不准确
//简单的用rssi计算出距离
for(int i =0;i<anchor_count;i++){
//简易公式，不准确
distances[i]= (int16_t)10*pow((float)anchors[i].rssi/-50,2) ;
}
int count =0;//有效计算的次数。
for (size_t i = 0; i < anchor_count - 2; i++)
{
for (size_t j = i + 1; j < anchor_count - 1; j++)
{
for (size_t k = j + 1; k < anchor_count; k++)
{
float result_x, result_y; //临时的结果变量
//使用不同组的三个点和算出来的距离来算结果。
trilaterate(anchors[i].x, anchors[i].y, distances[i],
anchors[j].x, anchors[j].y, distances[j],
anchors[k].x, anchors[k].y, distances[k],
&result_x, &result_y);
// 剔除明显错误的结果,结果正常的留下用于计算平均值。
if (!isnan(result_x) && !isnan(result_y)) {
sum_x += result_x;
sum_y += result_y;
count++;
}
}
}
}
if(count >0){ //避免分母为零。
float estimated_x = sum_x / count;
float estimated_y = sum_y / count;
//算结果
printf("Estimated position: x: %f, y: %f\n", (float)estimated_x, (float)estimated_y);
}
}
}
}
else { //打印非预期包的大小方便debug
printf("Received packet with unexpected size: %u\n", len);
}
}
/*---------------------------------------------------------------------------*/
PROCESS(unknown_node_process, "Unknown Node Process");
AUTOSTART_PROCESSES(&unknown_node_process);
/*---------------------------------------------------------------------------*/
PROCESS_THREAD(unknown_node_process, ev, data)
{
PROCESS_BEGIN();
// 设置 NullNet 的接收回调函数
nullnet_set_input_callback(recv_packet);
printf("Unknown Node started\n");
while(1) {
PROCESS_YIELD(); //持续等待事件, 不用设置定时器，来什么处理什么
}
PROCESS_END();
}
/*---------------------------------------------------------------------------*/
代码解释：
anchor_info_t结构体，存了每个锚节点的地址、坐标，还有接收信号强度（RSSI）。recv_packet是回调函数,只要收到包了就进行处理,从中取出锚点信息。trilaterate是三边测量法。- 用了个简单的办法估算距离 (根据RSSI估算),不准,但是可以说明原理。
- 为了增加精度，用了所有收到的包的信息进行计算,并取平均值。
- 加了个过滤机制，去除掉算出来明显不靠谱的坐标。
安全提示：
- RSSI 容易受到环境干扰，根据 RSSI 估算距离精度有限。实际应用中，可能需要更精确的测距方法，例如到达时间差 (TDoA) 或到达角度 (AoA)。
- 防范“女巫攻击”（Sybil Attack）。在女巫攻击中，一个恶意节点可以声称自己是多个节点，从而影响定位结果的准确性。在设计系统时，可以考虑加入节点身份验证机制，确保接收到的位置信息来自可信的锚节点。
进阶技巧:
- 可以加卡尔曼滤波 (Kalman filter), 处理测量中的各种噪声。
- 试试多跳 (multi-hop) 定位,让节点之间也互相传信息。
三、Cooja 仿真步骤
- 创建仿真： 打开 Cooja，创建一个新的仿真。
- 添加节点：
- 添加一个 "Z1 Mote" 作为锚节点，编译并加载 anchor.c。
- 添加多个 "Z1 Mote" 作为未知节点，编译并加载 unknown.c。
- 设置无线参数：
- 在 "Mote Types" 窗口，选择 "Radio Environment"。
- 根据需要调整传输范围、干扰模型等参数。
- 运行仿真： 点击 "Start" 按钮运行仿真。
- 查看结果： 在 "Simulation Visualizer" 窗口中，可以看到节点的位置和通信情况。未知节点的串口输出 (通过
printf) 会显示其估算的自身位置。
通过 Cooja 仿真，就能清楚地看到，未知节点如何利用锚节点广播的信息，逐步计算出自己的位置。
