返回

Cooja无线传感器网络(WSN)定位仿真:原理与实现

java

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 仿真步骤

  1. 创建仿真: 打开 Cooja,创建一个新的仿真。
  2. 添加节点:
    • 添加一个 "Z1 Mote" 作为锚节点,编译并加载 anchor.c。
    • 添加多个 "Z1 Mote" 作为未知节点,编译并加载 unknown.c。
  3. 设置无线参数:
    • 在 "Mote Types" 窗口,选择 "Radio Environment"。
    • 根据需要调整传输范围、干扰模型等参数。
  4. 运行仿真: 点击 "Start" 按钮运行仿真。
  5. 查看结果: 在 "Simulation Visualizer" 窗口中,可以看到节点的位置和通信情况。未知节点的串口输出 (通过 printf) 会显示其估算的自身位置。

通过 Cooja 仿真,就能清楚地看到,未知节点如何利用锚节点广播的信息,逐步计算出自己的位置。